Panda3D
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 
25 using std::max;
26 using std::min;
27 
28 TypeHandle BoundingSphere::_type_handle;
29 
30 /**
31  *
32  */
33 BoundingVolume *BoundingSphere::
34 make_copy() const {
35  return new BoundingSphere(*this);
36 }
37 
38 /**
39  *
40  */
41 LPoint3 BoundingSphere::
42 get_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  */
53 LPoint3 BoundingSphere::
54 get_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  */
65 PN_stdfloat BoundingSphere::
66 get_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  */
79 LPoint3 BoundingSphere::
80 get_approx_center() const {
81  nassertr(!is_empty(), LPoint3::zero());
82  nassertr(!is_infinite(), LPoint3::zero());
83  return get_center();
84 }
85 
86 /**
87  *
88  */
89 void BoundingSphere::
90 xform(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  */
125 void BoundingSphere::
126 output(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  */
141 as_bounding_sphere() const {
142  return this;
143 }
144 
145 /**
146  *
147  */
148 bool BoundingSphere::
149 extend_other(BoundingVolume *other) const {
150  return other->extend_by_sphere(this);
151 }
152 
153 /**
154  *
155  */
156 bool BoundingSphere::
157 around_other(BoundingVolume *other,
158  const BoundingVolume **first,
159  const BoundingVolume **last) const {
160  return other->around_spheres(first, last);
161 }
162 
163 /**
164  *
165  */
166 int BoundingSphere::
167 contains_other(const BoundingVolume *other) const {
168  return other->contains_sphere(this);
169 }
170 
171 
172 /**
173  *
174  */
175 bool BoundingSphere::
176 extend_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  */
196 bool BoundingSphere::
197 extend_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  */
216 bool BoundingSphere::
217 extend_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  */
246 bool BoundingSphere::
247 extend_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  */
258 bool BoundingSphere::
259 extend_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  */
270 bool BoundingSphere::
271 around_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  */
360 bool BoundingSphere::
361 around_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);
372  const FiniteBoundingVolume *vol = (*p)->as_finite_bounding_volume();
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.
424  const FiniteBoundingVolume *vol = (*p)->as_finite_bounding_volume();
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  */
451 int BoundingSphere::
452 contains_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  */
472 int BoundingSphere::
473 contains_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  */
534 int BoundingSphere::
535 contains_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  */
561 int BoundingSphere::
562 contains_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  */
570 int BoundingSphere::
571 contains_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  */
579 int BoundingSphere::
580 contains_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  */
588 int BoundingSphere::
589 contains_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...
Definition: boundingLine.h:29
This funny bounding volume is an infinite plane that divides space into two regions: the part behind ...
Definition: boundingPlane.h:28
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.