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