Panda3D
Loading...
Searching...
No Matches
boundingSphere.cxx
Go to the documentation of this file.
1/**
2 * PANDA 3D SOFTWARE
3 * Copyright (c) Carnegie Mellon University. All rights reserved.
4 *
5 * All use of this software is subject to the terms of the revised BSD
6 * license. You should have received a copy of this license along
7 * with this source code in a file named "LICENSE."
8 *
9 * @file boundingSphere.cxx
10 * @author drose
11 * @date 1999-10-01
12 */
13
14#include "boundingSphere.h"
15#include "boundingBox.h"
16#include "boundingHexahedron.h"
17#include "boundingLine.h"
18#include "boundingPlane.h"
19#include "config_mathutil.h"
20#include "dcast.h"
21#include "cmath.h"
22
23#include <algorithm>
24
25using std::max;
26using std::min;
27
28TypeHandle BoundingSphere::_type_handle;
29
30/**
31 *
32 */
33BoundingVolume *BoundingSphere::
34make_copy() const {
35 return new BoundingSphere(*this);
36}
37
38/**
39 *
40 */
41LPoint3 BoundingSphere::
42get_min() const {
43 nassertr(!is_empty(), LPoint3::zero());
44 nassertr(!is_infinite(), LPoint3::zero());
45 return LPoint3(_center[0] - _radius,
46 _center[1] - _radius,
47 _center[2] - _radius);
48}
49
50/**
51 *
52 */
53LPoint3 BoundingSphere::
54get_max() const {
55 nassertr(!is_empty(), LPoint3::zero());
56 nassertr(!is_infinite(), LPoint3::zero());
57 return LPoint3(_center[0] + _radius,
58 _center[1] + _radius,
59 _center[2] + _radius);
60}
61
62/**
63 *
64 */
65PN_stdfloat BoundingSphere::
66get_volume() const {
67 nassertr(!is_infinite(), 0.0f);
68 if (is_empty()) {
69 return 0.0f;
70 }
71
72 // Volume of a sphere: four-thirds pi r cubed.
73 return 4.0f / 3.0f * MathNumbers::pi_f * _radius * _radius * _radius;
74}
75
76/**
77 *
78 */
79LPoint3 BoundingSphere::
80get_approx_center() const {
81 nassertr(!is_empty(), LPoint3::zero());
82 nassertr(!is_infinite(), LPoint3::zero());
83 return get_center();
84}
85
86/**
87 *
88 */
89void BoundingSphere::
90xform(const LMatrix4 &mat) {
91 nassertv(!mat.is_nan());
92
93 if (!is_empty() && !is_infinite()) {
94 // First, determine the longest axis of the matrix, in case it contains a
95 // non-uniform scale.
96
97 LVecBase3 x, y, z;
98 mat.get_row3(x, 0);
99 mat.get_row3(y, 1);
100 mat.get_row3(z, 2);
101
102 PN_stdfloat xd = dot(x, x);
103 PN_stdfloat yd = dot(y, y);
104 PN_stdfloat zd = dot(z, z);
105
106 PN_stdfloat scale = max(xd, yd);
107 scale = max(scale, zd);
108 scale = csqrt(scale);
109
110 // Transform the radius
111 _radius *= scale;
112
113 // Transform the center
114 _center = _center * mat;
115
116 if (cinf(_radius)) {
117 set_infinite();
118 }
119 }
120}
121
122/**
123 *
124 */
125void BoundingSphere::
126output(std::ostream &out) const {
127 if (is_empty()) {
128 out << "bsphere, empty";
129 } else if (is_infinite()) {
130 out << "bsphere, infinite";
131 } else {
132 out << "bsphere, c (" << _center << "), r " << _radius;
133 }
134}
135
136/**
137 * Virtual downcast method. Returns this object as a pointer of the indicated
138 * type, if it is in fact that type. Returns NULL if it is not that type.
139 */
141as_bounding_sphere() const {
142 return this;
143}
144
145/**
146 *
147 */
148bool BoundingSphere::
149extend_other(BoundingVolume *other) const {
150 return other->extend_by_sphere(this);
151}
152
153/**
154 *
155 */
156bool BoundingSphere::
157around_other(BoundingVolume *other,
158 const BoundingVolume **first,
159 const BoundingVolume **last) const {
160 return other->around_spheres(first, last);
161}
162
163/**
164 *
165 */
166int BoundingSphere::
167contains_other(const BoundingVolume *other) const {
168 return other->contains_sphere(this);
169}
170
171
172/**
173 *
174 */
175bool BoundingSphere::
176extend_by_point(const LPoint3 &point) {
177 nassertr(!point.is_nan(), false);
178
179 if (is_empty()) {
180 _center = point;
181 _radius = 0.0f;
182 _flags = 0;
183 } else if (!is_infinite()) {
184 LVector3 v = point - _center;
185 PN_stdfloat dist2 = dot(v, v);
186 if (dist2 > _radius * _radius) {
187 _radius = csqrt(dist2);
188 }
189 }
190 return true;
191}
192
193/**
194 *
195 */
196bool BoundingSphere::
197extend_by_sphere(const BoundingSphere *sphere) {
198 nassertr(!sphere->is_empty() && !sphere->is_infinite(), false);
199 nassertr(!is_infinite(), false);
200
201 if (is_empty()) {
202 _center = sphere->_center;
203 _radius = sphere->_radius;
204 _flags = 0;
205 } else {
206 PN_stdfloat dist = length(sphere->_center - _center);
207
208 _radius = max(_radius, dist + sphere->_radius);
209 }
210 return true;
211}
212
213/**
214 *
215 */
216bool BoundingSphere::
217extend_by_box(const BoundingBox *box) {
218 const LVector3 &min1 = box->get_minq();
219 const LVector3 &max1 = box->get_maxq();
220
221 if (is_empty()) {
222 _center = (min1 + max1) * 0.5f;
223 _radius = length(LVector3(max1 - _center));
224 _flags = 0;
225
226 } else {
227 // Find the minimum radius necessary to reach the corner.
228 PN_stdfloat max_dist2 = -1.0;
229 for (int i = 0; i < 8; ++i) {
230 PN_stdfloat dist2 = (box->get_point(i) - _center).length_squared();
231 if (dist2 > max_dist2) {
232 max_dist2 = dist2;
233 }
234 }
235 if (max_dist2 > _radius * _radius) {
236 _radius = csqrt(max_dist2);
237 }
238 }
239
240 return true;
241}
242
243/**
244 *
245 */
246bool BoundingSphere::
247extend_by_hexahedron(const BoundingHexahedron *hexahedron) {
248 nassertr(!hexahedron->is_empty(), false);
249
250 BoundingBox box(hexahedron->get_min(), hexahedron->get_max());
251 box.local_object();
252 return extend_by_box(&box);
253}
254
255/**
256 *
257 */
258bool BoundingSphere::
259extend_by_finite(const FiniteBoundingVolume *volume) {
260 nassertr(!volume->is_empty(), false);
261
262 BoundingBox box(volume->get_min(), volume->get_max());
263 box.local_object();
264 return extend_by_box(&box);
265}
266
267/**
268 *
269 */
270bool BoundingSphere::
271around_points(const LPoint3 *first, const LPoint3 *last) {
272 nassertr(first != last, false);
273
274 // First, get the box of all the points to construct a bounding box.
275 const LPoint3 *p = first;
276
277#ifndef NDEBUG
278 // Skip any NaN points.
279 int skipped_nan = 0;
280 while (p != last && (*p).is_nan()) {
281 ++p;
282 ++skipped_nan;
283 }
284 if (p == last) {
285 mathutil_cat.warning()
286 << "BoundingSphere around NaN\n";
287 return false;
288 }
289#endif
290
291 LPoint3 min_box(*p);
292 LPoint3 max_box(*p);
293 ++p;
294
295#ifndef NDEBUG
296 // Skip more NaN points.
297 while (p != last && (*p).is_nan()) {
298 ++p;
299 ++skipped_nan;
300 }
301#endif
302
303 if (p == last) {
304 // Only one point; we have a radius of zero. This is not the same thing
305 // as an empty sphere, because our volume contains one point; an empty
306 // sphere contains no points.
307 _center = min_box;
308 _radius = 0.0f;
309
310 } else {
311 // More than one point; we have a nonzero radius.
312 while (p != last) {
313#ifndef NDEBUG
314 // Skip more NaN points.
315 if ((*p).is_nan()) {
316 ++skipped_nan;
317 } else
318#endif
319 {
320 min_box.set(min(min_box[0], (*p)[0]),
321 min(min_box[1], (*p)[1]),
322 min(min_box[2], (*p)[2]));
323 max_box.set(max(max_box[0], (*p)[0]),
324 max(max_box[1], (*p)[1]),
325 max(max_box[2], (*p)[2]));
326 }
327 ++p;
328 }
329
330 // Now take the center of the bounding box as the center of the sphere.
331 _center = (min_box + max_box) * 0.5f;
332
333 // Now walk back through to get the max distance from center.
334 PN_stdfloat max_dist2 = 0.0f;
335 for (p = first; p != last; ++p) {
336 LVector3 v = (*p) - _center;
337 PN_stdfloat dist2 = dot(v, v);
338 max_dist2 = max(max_dist2, dist2);
339 }
340
341 _radius = csqrt(max_dist2);
342 }
343
344#ifndef NDEBUG
345 if (skipped_nan != 0) {
346 mathutil_cat.warning()
347 << "BoundingSphere ignored " << skipped_nan << " NaN points of "
348 << (last - first) << " total.\n";
349 }
350#endif
351
352 _flags = 0;
353
354 return true;
355}
356
357/**
358 *
359 */
360bool BoundingSphere::
361around_finite(const BoundingVolume **first,
362 const BoundingVolume **last) {
363 nassertr(first != last, false);
364
365 // We're given a set of bounding volumes, all of which are finite, and at
366 // least the first one of which is guaranteed to be nonempty. Some others
367 // may not be.
368
369 // First, get the box of all the points to construct a bounding box.
370 const BoundingVolume **p = first;
371 nassertr(!(*p)->is_empty() && !(*p)->is_infinite(), false);
373 nassertr(vol != nullptr, false);
374 LPoint3 min_box = vol->get_min();
375 LPoint3 max_box = vol->get_max();
376
377 bool any_spheres = (vol->as_bounding_sphere() != nullptr);
378
379 for (++p; p != last; ++p) {
380 nassertr(!(*p)->is_infinite(), false);
381 if (!(*p)->is_empty()) {
382 vol = (*p)->as_finite_bounding_volume();
383 if (vol == nullptr) {
384 set_infinite();
385 return true;
386 }
387 LPoint3 min1 = vol->get_min();
388 LPoint3 max1 = vol->get_max();
389 min_box.set(min(min_box[0], min1[0]),
390 min(min_box[1], min1[1]),
391 min(min_box[2], min1[2]));
392 max_box.set(max(max_box[0], max1[0]),
393 max(max_box[1], max1[1]),
394 max(max_box[2], max1[2]));
395
396 if (vol->as_bounding_sphere() != nullptr) {
397 any_spheres = true;
398 }
399 }
400 }
401
402 // Now take the center of the bounding box as the center of the sphere.
403 _center = (min_box + max_box) * 0.5f;
404
405 if (!any_spheres) {
406 // Since there are no spheres in the list, we have to make this sphere
407 // fully enclose all of the bounding boxes.
408 _radius = length(max_box - _center);
409
410 } else {
411 // We might be able to go tighter, by lopping off the corners of the
412 // spheres.
413 _radius = 0.0f;
414 for (p = first; p != last; ++p) {
415 if (!(*p)->is_empty()) {
416 const BoundingSphere *sphere = (*p)->as_bounding_sphere();
417 if (sphere != nullptr) {
418 // This is a sphere; consider its corner.
419 PN_stdfloat dist = length(sphere->_center - _center);
420 _radius = max(_radius, dist + sphere->_radius);
421
422 } else {
423 // This is a nonsphere. We fit around it.
425 nassertr(vol != nullptr, false);
426
427 BoundingBox box(vol->get_min(), vol->get_max());
428 box.local_object();
429
430 // Find the minimum radius necessary to reach the corner.
431 PN_stdfloat max_dist2 = -1.0;
432 for (int i = 0; i < 8; ++i) {
433 PN_stdfloat dist2 = (box.get_point(i) - _center).length_squared();
434 if (dist2 > max_dist2) {
435 max_dist2 = dist2;
436 }
437 }
438 _radius = max(_radius, csqrt(max_dist2));
439 }
440 }
441 }
442 }
443
444 _flags = 0;
445 return true;
446}
447
448/**
449 *
450 */
451int BoundingSphere::
452contains_point(const LPoint3 &point) const {
453 nassertr(!point.is_nan(), IF_no_intersection);
454
455 if (is_empty()) {
456 return IF_no_intersection;
457
458 } else if (is_infinite()) {
459 return IF_possible | IF_some | IF_all;
460
461 } else {
462 LVector3 v = point - _center;
463 PN_stdfloat dist2 = dot(v, v);
464 return (dist2 <= _radius * _radius) ?
465 IF_possible | IF_some | IF_all : IF_no_intersection;
466 }
467}
468
469/**
470 *
471 */
472int BoundingSphere::
473contains_lineseg(const LPoint3 &a, const LPoint3 &b) const {
474 nassertr(!a.is_nan() && !b.is_nan(), IF_no_intersection);
475
476 if (a == b) {
477 return contains_point(a);
478 }
479 if (is_empty()) {
480 return IF_no_intersection;
481
482 } else if (is_infinite()) {
483 return IF_possible | IF_some | IF_all;
484
485 } else {
486 LPoint3 from = a;
487 LVector3 delta = b - a;
488 PN_stdfloat t1, t2;
489
490 // Solve the equation for the intersection of a line with a sphere using
491 // the quadratic equation.
492 PN_stdfloat A = dot(delta, delta);
493
494 nassertr(A != 0.0f, 0); // Trivial line segment.
495
496 LVector3 fc = from - _center;
497 PN_stdfloat B = 2.0f * dot(delta, fc);
498 PN_stdfloat C = dot(fc, fc) - _radius * _radius;
499
500 PN_stdfloat radical = B*B - 4.0f*A*C;
501
502 if (IS_NEARLY_ZERO(radical)) {
503 // Tangent.
504 t1 = t2 = -B / (2.0f*A);
505 return (t1 >= 0.0f && t1 <= 1.0f) ?
506 IF_possible | IF_some : IF_no_intersection;
507 }
508
509 if (radical < 0.0f) {
510 // No real roots: no intersection with the line.
511 return IF_no_intersection;
512 }
513
514 PN_stdfloat reciprocal_2A = 1.0f/(2.0f*A);
515 PN_stdfloat sqrt_radical = csqrt(radical);
516
517 t1 = ( -B - sqrt_radical ) * reciprocal_2A;
518 t2 = ( -B + sqrt_radical ) * reciprocal_2A;
519
520 if (t1 >= 0.0f && t2 <= 1.0f) {
521 return IF_possible | IF_some | IF_all;
522 } else if (t1 <= 1.0f && t2 >= 0.0f) {
523 return IF_possible | IF_some;
524 } else {
525 return IF_no_intersection;
526 }
527 }
528}
529
530/**
531 * Double-dispatch support: called by contains_other() when the type we're
532 * testing for intersection is known to be a sphere.
533 */
534int BoundingSphere::
535contains_sphere(const BoundingSphere *sphere) const {
536 nassertr(!is_empty() && !is_infinite(), 0);
537 nassertr(!sphere->is_empty() && !sphere->is_infinite(), 0);
538
539 LVector3 v = sphere->_center - _center;
540 PN_stdfloat dist2 = dot(v, v);
541
542 if (_radius >= sphere->_radius &&
543 dist2 <= (_radius - sphere->_radius) * (_radius - sphere->_radius)) {
544 // The other sphere is completely within this sphere.
545 return IF_possible | IF_some | IF_all;
546
547 } else if (dist2 > (_radius + sphere->_radius) * (_radius + sphere->_radius)) {
548 // The other sphere is completely outside this sphere.
549 return IF_no_intersection;
550
551 } else {
552 // The other sphere is partially within this sphere.
553 return IF_possible | IF_some;
554 }
555}
556
557/**
558 * Double-dispatch support: called by contains_other() when the type we're
559 * testing for intersection is known to be a box.
560 */
561int BoundingSphere::
562contains_box(const BoundingBox *box) const {
563 return box->contains_sphere(this) & ~IF_all;
564}
565
566/**
567 * Double-dispatch support: called by contains_other() when the type we're
568 * testing for intersection is known to be a hexahedron.
569 */
570int BoundingSphere::
571contains_hexahedron(const BoundingHexahedron *hexahedron) const {
572 return hexahedron->contains_sphere(this) & ~IF_all;
573}
574
575/**
576 * Double-dispatch support: called by contains_other() when the type we're
577 * testing for intersection is known to be a line.
578 */
579int BoundingSphere::
580contains_line(const BoundingLine *line) const {
581 return line->contains_sphere(this) & ~IF_all;
582}
583
584/**
585 * Double-dispatch support: called by contains_other() when the type we're
586 * testing for intersection is known to be a plane.
587 */
588int BoundingSphere::
589contains_plane(const BoundingPlane *plane) const {
590 return plane->contains_sphere(this) & ~IF_all;
591}
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
An axis-aligned bounding box; that is, a minimum and maximum coordinate triple.
Definition boundingBox.h:29
const LPoint3 & get_minq() const
An inline accessor for the minimum value.
Definition boundingBox.I:41
get_point
Returns the nth vertex of the rectangular solid.
Definition boundingBox.h:51
const LPoint3 & get_maxq() const
An inline accessor for the maximum value.
Definition boundingBox.I:52
This defines a bounding convex hexahedron.
This funny bounding volume is an infinite line with no thickness and extending to infinity in both di...
This funny bounding volume is an infinite plane that divides space into two regions: the part behind ...
This defines a bounding sphere, consisting of a center and a radius.
virtual const BoundingSphere * as_bounding_sphere() const
Virtual downcast method.
BoundingSphere()
Constructs an empty sphere.
This is an abstract class for any volume in any sense which can be said to define the locality of ref...
void set_infinite()
Marks the volume as infinite, even if it is normally finite.
bool is_empty() const
Any kind of volume might be empty.
virtual const BoundingSphere * as_bounding_sphere() const
Virtual downcast method.
bool is_infinite() const
The other side of the empty coin is an infinite volume.
A special kind of GeometricBoundingVolume that is known to be finite.
virtual const FiniteBoundingVolume * as_finite_bounding_volume() const
Virtual downcast method.
void local_object()
This function should be called, once, immediately after creating a new instance of some ReferenceCoun...
TypeHandle is the identifier used to differentiate C++ class types.
Definition typeHandle.h:81
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.