Panda3D
 All Classes Functions Variables Enumerations
boundingSphere.cxx
00001 // Filename: boundingSphere.cxx
00002 // Created by:  drose (01Oct99)
00003 //
00004 ////////////////////////////////////////////////////////////////////
00005 //
00006 // PANDA 3D SOFTWARE
00007 // Copyright (c) Carnegie Mellon University.  All rights reserved.
00008 //
00009 // All use of this software is subject to the terms of the revised BSD
00010 // license.  You should have received a copy of this license along
00011 // with this source code in a file named "LICENSE."
00012 //
00013 ////////////////////////////////////////////////////////////////////
00014 
00015 #include "boundingSphere.h"
00016 #include "boundingBox.h"
00017 #include "boundingHexahedron.h"
00018 #include "boundingLine.h"
00019 #include "boundingPlane.h"
00020 #include "config_mathutil.h"
00021 #include "dcast.h"
00022 #include "cmath.h"
00023 
00024 #include <algorithm>
00025 
00026 TypeHandle BoundingSphere::_type_handle;
00027 
00028 ////////////////////////////////////////////////////////////////////
00029 //     Function: BoundingSphere::make_copy
00030 //       Access: Public, Virtual
00031 //  Description: 
00032 ////////////////////////////////////////////////////////////////////
00033 BoundingVolume *BoundingSphere::
00034 make_copy() const {
00035   return new BoundingSphere(*this);
00036 }
00037 
00038 ////////////////////////////////////////////////////////////////////
00039 //     Function: BoundingSphere::get_min
00040 //       Access: Public, Virtual
00041 //  Description: 
00042 ////////////////////////////////////////////////////////////////////
00043 LPoint3 BoundingSphere::
00044 get_min() const {
00045   nassertr(!is_empty(), LPoint3::zero());
00046   nassertr(!is_infinite(), LPoint3::zero());
00047   return LPoint3(_center[0] - _radius,
00048                   _center[1] - _radius,
00049                   _center[2] - _radius);
00050 }
00051 
00052 ////////////////////////////////////////////////////////////////////
00053 //     Function: BoundingSphere::get_max
00054 //       Access: Public, Virtual
00055 //  Description: 
00056 ////////////////////////////////////////////////////////////////////
00057 LPoint3 BoundingSphere::
00058 get_max() const {
00059   nassertr(!is_empty(), LPoint3::zero());
00060   nassertr(!is_infinite(), LPoint3::zero());
00061   return LPoint3(_center[0] + _radius,
00062                   _center[1] + _radius,
00063                   _center[2] + _radius);
00064 }
00065 
00066 ////////////////////////////////////////////////////////////////////
00067 //     Function: BoundingSphere::get_volume
00068 //       Access: Public, Virtual
00069 //  Description: 
00070 ////////////////////////////////////////////////////////////////////
00071 PN_stdfloat BoundingSphere::
00072 get_volume() const {
00073   nassertr(!is_infinite(), 0.0f);
00074   if (is_empty()) {
00075     return 0.0f;
00076   }
00077 
00078   // Volume of a sphere: four-thirds pi r cubed.
00079   return 4.0f / 3.0f * MathNumbers::pi_f * _radius * _radius * _radius;
00080 }
00081 
00082 ////////////////////////////////////////////////////////////////////
00083 //     Function: BoundingSphere::get_approx_center
00084 //       Access: Public, Virtual
00085 //  Description: 
00086 ////////////////////////////////////////////////////////////////////
00087 LPoint3 BoundingSphere::
00088 get_approx_center() const {
00089   nassertr(!is_empty(), LPoint3::zero());
00090   nassertr(!is_infinite(), LPoint3::zero());
00091   return get_center();
00092 }
00093 
00094 ////////////////////////////////////////////////////////////////////
00095 //     Function: BoundingSphere::xform
00096 //       Access: Public, Virtual
00097 //  Description: 
00098 ////////////////////////////////////////////////////////////////////
00099 void BoundingSphere::
00100 xform(const LMatrix4 &mat) {
00101   nassertv(!mat.is_nan());
00102 
00103   if (!is_empty() && !is_infinite()) {
00104     // First, determine the longest axis of the matrix, in case it
00105     // contains a non-uniform scale.
00106 
00107     LVecBase3 x, y, z;
00108     mat.get_row3(x, 0);
00109     mat.get_row3(y, 1);
00110     mat.get_row3(z, 2);
00111 
00112     PN_stdfloat xd = dot(x, x);
00113     PN_stdfloat yd = dot(y, y);
00114     PN_stdfloat zd = dot(z, z);
00115 
00116     PN_stdfloat scale = max(xd, yd);
00117     scale = max(scale, zd);
00118     scale = csqrt(scale);
00119 
00120     // Transform the radius
00121     _radius *= scale;
00122 
00123     // Transform the center
00124     _center = _center * mat;
00125   }
00126 }
00127 
00128 ////////////////////////////////////////////////////////////////////
00129 //     Function: BoundingSphere::output
00130 //       Access: Public, Virtual
00131 //  Description: 
00132 ////////////////////////////////////////////////////////////////////
00133 void BoundingSphere::
00134 output(ostream &out) const {
00135   if (is_empty()) {
00136     out << "bsphere, empty";
00137   } else if (is_infinite()) {
00138     out << "bsphere, infinite";
00139   } else {
00140     out << "bsphere, c (" << _center << "), r " << _radius;
00141   }
00142 }
00143 
00144 ////////////////////////////////////////////////////////////////////
00145 //     Function: BoundingSphere::as_bounding_sphere
00146 //       Access: Public, Virtual
00147 //  Description: Virtual downcast method.  Returns this object as a
00148 //               pointer of the indicated type, if it is in fact that
00149 //               type.  Returns NULL if it is not that type.
00150 ////////////////////////////////////////////////////////////////////
00151 const BoundingSphere *BoundingSphere::
00152 as_bounding_sphere() const {
00153   return this;
00154 }
00155 
00156 ////////////////////////////////////////////////////////////////////
00157 //     Function: BoundingSphere::extend_other
00158 //       Access: Protected, Virtual
00159 //  Description: 
00160 ////////////////////////////////////////////////////////////////////
00161 bool BoundingSphere::
00162 extend_other(BoundingVolume *other) const {
00163   return other->extend_by_sphere(this);
00164 }
00165 
00166 ////////////////////////////////////////////////////////////////////
00167 //     Function: BoundingSphere::around_other
00168 //       Access: Protected, Virtual
00169 //  Description: 
00170 ////////////////////////////////////////////////////////////////////
00171 bool BoundingSphere::
00172 around_other(BoundingVolume *other,
00173              const BoundingVolume **first,
00174              const BoundingVolume **last) const {
00175   return other->around_spheres(first, last);
00176 }
00177 
00178 ////////////////////////////////////////////////////////////////////
00179 //     Function: BoundingSphere::contains_other
00180 //       Access: Protected, Virtual
00181 //  Description: 
00182 ////////////////////////////////////////////////////////////////////
00183 int BoundingSphere::
00184 contains_other(const BoundingVolume *other) const {
00185   return other->contains_sphere(this);
00186 }
00187 
00188 
00189 ////////////////////////////////////////////////////////////////////
00190 //     Function: BoundingSphere::extend_by_point
00191 //       Access: Protected, Virtual
00192 //  Description: 
00193 ////////////////////////////////////////////////////////////////////
00194 bool BoundingSphere::
00195 extend_by_point(const LPoint3 &point) {
00196   nassertr(!point.is_nan(), false);
00197 
00198   if (is_empty()) {
00199     _center = point;
00200     _radius = 0.0f;
00201     _flags = 0;
00202   } else if (!is_infinite()) {
00203     LVector3 v = point - _center;
00204     PN_stdfloat dist2 = dot(v, v);
00205     if (dist2 > _radius * _radius) {
00206       _radius = csqrt(dist2);
00207     }
00208   }
00209   return true;
00210 }
00211 
00212 ////////////////////////////////////////////////////////////////////
00213 //     Function: BoundingSphere::extend_by_sphere
00214 //       Access: Protected, Virtual
00215 //  Description: 
00216 ////////////////////////////////////////////////////////////////////
00217 bool BoundingSphere::
00218 extend_by_sphere(const BoundingSphere *sphere) {
00219   nassertr(!sphere->is_empty() && !sphere->is_infinite(), false);
00220   nassertr(!is_infinite(), false);
00221 
00222   if (is_empty()) {
00223     _center = sphere->_center;
00224     _radius = sphere->_radius;
00225     _flags = 0;
00226   } else {
00227     PN_stdfloat dist = length(sphere->_center - _center);
00228 
00229     _radius = max(_radius, dist + sphere->_radius);
00230   }
00231   return true;
00232 }
00233 
00234 ////////////////////////////////////////////////////////////////////
00235 //     Function: BoundingSphere::extend_by_box
00236 //       Access: Protected, Virtual
00237 //  Description: 
00238 ////////////////////////////////////////////////////////////////////
00239 bool BoundingSphere::
00240 extend_by_box(const BoundingBox *box) {
00241   const LVector3 &min1 = box->get_minq();
00242   const LVector3 &max1 = box->get_maxq();
00243 
00244   if (is_empty()) {
00245     _center = (min1 + max1) * 0.5f;
00246     _radius = length(LVector3(max1 - _center));
00247     _flags = 0;
00248 
00249   } else {
00250     // Find the minimum radius necessary to reach the corner.
00251     PN_stdfloat max_dist2 = -1.0;
00252     for (int i = 0; i < 8; ++i) {
00253       PN_stdfloat dist2 = (box->get_point(i) - _center).length_squared();
00254       if (dist2 > max_dist2) {
00255         max_dist2 = dist2;
00256       }
00257     }
00258     if (max_dist2 > _radius * _radius) {
00259       _radius = csqrt(max_dist2);
00260     }
00261   }
00262 
00263   return true;
00264 }
00265 
00266 ////////////////////////////////////////////////////////////////////
00267 //     Function: BoundingSphere::extend_by_hexahedron
00268 //       Access: Protected, Virtual
00269 //  Description: 
00270 ////////////////////////////////////////////////////////////////////
00271 bool BoundingSphere::
00272 extend_by_hexahedron(const BoundingHexahedron *hexahedron) {
00273   nassertr(!hexahedron->is_empty(), false);
00274 
00275   BoundingBox box(hexahedron->get_min(), hexahedron->get_max());
00276   box.local_object();
00277   return extend_by_box(&box);
00278 }
00279 
00280 ////////////////////////////////////////////////////////////////////
00281 //     Function: BoundingSphere::extend_by_finite
00282 //       Access: Protected
00283 //  Description: 
00284 ////////////////////////////////////////////////////////////////////
00285 bool BoundingSphere::
00286 extend_by_finite(const FiniteBoundingVolume *volume) {
00287   nassertr(!volume->is_empty(), false);
00288 
00289   BoundingBox box(volume->get_min(), volume->get_max());
00290   box.local_object();
00291   return extend_by_box(&box);
00292 }
00293 
00294 ////////////////////////////////////////////////////////////////////
00295 //     Function: BoundingSphere::around_points
00296 //       Access: Protected, Virtual
00297 //  Description: 
00298 ////////////////////////////////////////////////////////////////////
00299 bool BoundingSphere::
00300 around_points(const LPoint3 *first, const LPoint3 *last) {
00301   nassertr(first != last, false);
00302 
00303   // First, get the box of all the points to construct a bounding
00304   // box.
00305   const LPoint3 *p = first;
00306 
00307 #ifndef NDEBUG
00308   // Skip any NaN points.
00309   int skipped_nan = 0;
00310   while (p != last && (*p).is_nan()) {
00311     ++p;
00312     ++skipped_nan;
00313   }
00314   if (p == last) {
00315     mathutil_cat.warning()
00316       << "BoundingSphere around NaN\n";
00317     return false;
00318   }
00319 #endif
00320 
00321   LPoint3 min_box(*p);
00322   LPoint3 max_box(*p);
00323   ++p;
00324 
00325 #ifndef NDEBUG
00326   // Skip more NaN points.
00327   while (p != last && (*p).is_nan()) {
00328     ++p;
00329     ++skipped_nan;
00330   }
00331 #endif
00332 
00333   if (p == last) {
00334     // Only one point; we have a radius of zero.  This is not the same
00335     // thing as an empty sphere, because our volume contains one
00336     // point; an empty sphere contains no points.
00337     _center = min_box;
00338     _radius = 0.0f;
00339 
00340   } else {
00341     // More than one point; we have a nonzero radius.
00342     while (p != last) {
00343 #ifndef NDEBUG
00344       // Skip more NaN points.
00345       if ((*p).is_nan()) {
00346         ++skipped_nan;
00347       } else
00348 #endif
00349         {
00350           min_box.set(min(min_box[0], (*p)[0]),
00351                       min(min_box[1], (*p)[1]),
00352                       min(min_box[2], (*p)[2]));
00353           max_box.set(max(max_box[0], (*p)[0]),
00354                       max(max_box[1], (*p)[1]),
00355                       max(max_box[2], (*p)[2]));
00356         }
00357       ++p;
00358     }
00359 
00360     // Now take the center of the bounding box as the center of the sphere.
00361     _center = (min_box + max_box) * 0.5f;
00362 
00363     // Now walk back through to get the max distance from center.
00364     PN_stdfloat max_dist2 = 0.0f;
00365     for (p = first; p != last; ++p) {
00366       LVector3 v = (*p) - _center;
00367       PN_stdfloat dist2 = dot(v, v);
00368       max_dist2 = max(max_dist2, dist2);
00369     }
00370 
00371     _radius = csqrt(max_dist2);
00372   }
00373 
00374 #ifndef NDEBUG
00375   if (skipped_nan != 0) {
00376     mathutil_cat.warning()
00377       << "BoundingSphere ignored " << skipped_nan << " NaN points of "
00378       << (last - first) << " total.\n";
00379   }
00380 #endif
00381 
00382   _flags = 0;
00383 
00384   return true;
00385 }
00386 
00387 ////////////////////////////////////////////////////////////////////
00388 //     Function: BoundingSphere::around_finite
00389 //       Access: Protected
00390 //  Description: 
00391 ////////////////////////////////////////////////////////////////////
00392 bool BoundingSphere::
00393 around_finite(const BoundingVolume **first,
00394               const BoundingVolume **last) {
00395   nassertr(first != last, false);
00396 
00397   // We're given a set of bounding volumes, all of which are finite,
00398   // and at least the first one of which is guaranteed to be nonempty.
00399   // Some others may not be.
00400 
00401   // First, get the box of all the points to construct a bounding
00402   // box.
00403   const BoundingVolume **p = first;
00404   nassertr(!(*p)->is_empty() && !(*p)->is_infinite(), false);
00405   const FiniteBoundingVolume *vol = (*p)->as_finite_bounding_volume();
00406   nassertr(vol != (FiniteBoundingVolume *)NULL, false);
00407   LPoint3 min_box = vol->get_min();
00408   LPoint3 max_box = vol->get_max();
00409 
00410   bool any_spheres = (vol->as_bounding_sphere() != NULL);
00411 
00412   for (++p; p != last; ++p) {
00413     nassertr(!(*p)->is_infinite(), false);
00414     if (!(*p)->is_empty()) {
00415       vol = (*p)->as_finite_bounding_volume();
00416       if (vol == (FiniteBoundingVolume *)NULL) {
00417         set_infinite();
00418         return true;
00419       }
00420       LPoint3 min1 = vol->get_min();
00421       LPoint3 max1 = vol->get_max();
00422       min_box.set(min(min_box[0], min1[0]),
00423                   min(min_box[1], min1[1]),
00424                   min(min_box[2], min1[2]));
00425       max_box.set(max(max_box[0], max1[0]),
00426                   max(max_box[1], max1[1]),
00427                   max(max_box[2], max1[2]));
00428 
00429       if (vol->as_bounding_sphere() != NULL) {
00430         any_spheres = true;
00431       }
00432     }
00433   }
00434 
00435   // Now take the center of the bounding box as the center of the sphere.
00436   _center = (min_box + max_box) * 0.5f;
00437 
00438   if (!any_spheres) {
00439     // Since there are no spheres in the list, we have to make this
00440     // sphere fully enclose all of the bounding boxes.
00441     _radius = length(max_box - _center);
00442 
00443   } else {
00444     // We might be able to go tighter, by lopping off the corners of
00445     // the spheres.
00446     _radius = 0.0f;
00447     for (p = first; p != last; ++p) {
00448       if (!(*p)->is_empty()) {
00449         const BoundingSphere *sphere = (*p)->as_bounding_sphere();
00450         if (sphere != (BoundingSphere *)NULL) {
00451           // This is a sphere; consider its corner.
00452           PN_stdfloat dist = length(sphere->_center - _center);
00453           _radius = max(_radius, dist + sphere->_radius);
00454           
00455         } else {
00456           // This is a nonsphere.  We fit around it.
00457           const FiniteBoundingVolume *vol = (*p)->as_finite_bounding_volume();
00458           nassertr(vol != (FiniteBoundingVolume *)NULL, false);
00459 
00460           BoundingBox box(vol->get_min(), vol->get_max());
00461           box.local_object();
00462           
00463           // Find the minimum radius necessary to reach the corner.
00464           PN_stdfloat max_dist2 = -1.0;
00465           for (int i = 0; i < 8; ++i) {
00466             PN_stdfloat dist2 = (box.get_point(i) - _center).length_squared();
00467             if (dist2 > max_dist2) {
00468               max_dist2 = dist2;
00469             }
00470           }
00471           _radius = max(_radius, csqrt(max_dist2));
00472         }
00473       }
00474     }
00475   }
00476 
00477   _flags = 0;
00478   return true;
00479 }
00480 
00481 ////////////////////////////////////////////////////////////////////
00482 //     Function: BoundingSphere::contains_point
00483 //       Access: Protected, Virtual
00484 //  Description: 
00485 ////////////////////////////////////////////////////////////////////
00486 int BoundingSphere::
00487 contains_point(const LPoint3 &point) const {
00488   nassertr(!point.is_nan(), IF_no_intersection);
00489 
00490   if (is_empty()) {
00491     return IF_no_intersection;
00492 
00493   } else if (is_infinite()) {
00494     return IF_possible | IF_some | IF_all;
00495 
00496   } else {
00497     LVector3 v = point - _center;
00498     PN_stdfloat dist2 = dot(v, v);
00499     return (dist2 <= _radius * _radius) ?
00500       IF_possible | IF_some | IF_all : IF_no_intersection;
00501   }
00502 }
00503 
00504 ////////////////////////////////////////////////////////////////////
00505 //     Function: BoundingSphere::contains_lineseg
00506 //       Access: Protected, Virtual
00507 //  Description: 
00508 ////////////////////////////////////////////////////////////////////
00509 int BoundingSphere::
00510 contains_lineseg(const LPoint3 &a, const LPoint3 &b) const {
00511   nassertr(!a.is_nan() && !b.is_nan(), IF_no_intersection);
00512 
00513   if (a == b) {
00514     return contains_point(a);
00515   }
00516   if (is_empty()) {
00517     return IF_no_intersection;
00518 
00519   } else if (is_infinite()) {
00520     return IF_possible | IF_some | IF_all;
00521 
00522   } else {
00523     LPoint3 from = a;
00524     LVector3 delta = b - a;
00525     PN_stdfloat t1, t2;
00526 
00527     // Solve the equation for the intersection of a line with a sphere
00528     // using the quadratic equation.
00529     PN_stdfloat A = dot(delta, delta);
00530 
00531     nassertr(A != 0.0f, 0);    // Trivial line segment.
00532 
00533     LVector3 fc = from - _center;
00534     PN_stdfloat B = 2.0f * dot(delta, fc);
00535     PN_stdfloat C = dot(fc, fc) - _radius * _radius;
00536 
00537     PN_stdfloat radical = B*B - 4.0f*A*C;
00538 
00539     if (IS_NEARLY_ZERO(radical)) {
00540       // Tangent.
00541       t1 = t2 = -B / (2.0f*A);
00542       return (t1 >= 0.0f && t1 <= 1.0f) ?
00543                  IF_possible | IF_some : IF_no_intersection;
00544     }
00545 
00546     if (radical < 0.0f) {
00547       // No real roots: no intersection with the line.
00548       return IF_no_intersection;
00549     }
00550 
00551     PN_stdfloat reciprocal_2A = 1.0f/(2.0f*A);
00552     PN_stdfloat sqrt_radical = csqrt(radical);
00553 
00554     t1 = ( -B - sqrt_radical ) * reciprocal_2A;
00555     t2 = ( -B + sqrt_radical ) * reciprocal_2A;
00556 
00557     if (t1 >= 0.0f && t2 <= 1.0f) {
00558       return IF_possible | IF_some | IF_all;
00559     } else if (t1 <= 1.0f && t2 >= 0.0f) {
00560       return IF_possible | IF_some;
00561     } else {
00562       return IF_no_intersection;
00563     }
00564   }
00565 }
00566 
00567 ////////////////////////////////////////////////////////////////////
00568 //     Function: BoundingSphere::contains_sphere
00569 //       Access: Protected, Virtual
00570 //  Description: Double-dispatch support: called by contains_other()
00571 //               when the type we're testing for intersection is known
00572 //               to be a sphere.
00573 ////////////////////////////////////////////////////////////////////
00574 int BoundingSphere::
00575 contains_sphere(const BoundingSphere *sphere) const {
00576   nassertr(!is_empty() && !is_infinite(), 0);
00577   nassertr(!sphere->is_empty() && !sphere->is_infinite(), 0);
00578 
00579   LVector3 v = sphere->_center - _center;
00580   PN_stdfloat dist2 = dot(v, v);
00581 
00582   if (_radius >= sphere->_radius &&
00583       dist2 <= (_radius - sphere->_radius) * (_radius - sphere->_radius)) {
00584     // The other sphere is completely within this sphere.
00585     return IF_possible | IF_some | IF_all;
00586 
00587   } else if (dist2 > (_radius + sphere->_radius) * (_radius + sphere->_radius)) {
00588     // The other sphere is completely outside this sphere.
00589     return IF_no_intersection;
00590 
00591   } else {
00592     // The other sphere is partially within this sphere.
00593     return IF_possible | IF_some;
00594   }
00595 }
00596 
00597 ////////////////////////////////////////////////////////////////////
00598 //     Function: BoundingSphere::contains_box
00599 //       Access: Protected, Virtual
00600 //  Description: Double-dispatch support: called by contains_other()
00601 //               when the type we're testing for intersection is known
00602 //               to be a box.
00603 ////////////////////////////////////////////////////////////////////
00604 int BoundingSphere::
00605 contains_box(const BoundingBox *box) const {
00606   return box->contains_sphere(this) & ~IF_all;
00607 }
00608 
00609 ////////////////////////////////////////////////////////////////////
00610 //     Function: BoundingSphere::contains_hexahedron
00611 //       Access: Protected, Virtual
00612 //  Description: Double-dispatch support: called by contains_other()
00613 //               when the type we're testing for intersection is known
00614 //               to be a hexahedron.
00615 ////////////////////////////////////////////////////////////////////
00616 int BoundingSphere::
00617 contains_hexahedron(const BoundingHexahedron *hexahedron) const {
00618   return hexahedron->contains_sphere(this) & ~IF_all;
00619 }
00620 
00621 ////////////////////////////////////////////////////////////////////
00622 //     Function: BoundingSphere::contains_line
00623 //       Access: Protected, Virtual
00624 //  Description: Double-dispatch support: called by contains_other()
00625 //               when the type we're testing for intersection is known
00626 //               to be a line.
00627 ////////////////////////////////////////////////////////////////////
00628 int BoundingSphere::
00629 contains_line(const BoundingLine *line) const {
00630   return line->contains_sphere(this) & ~IF_all;
00631 }
00632 
00633 ////////////////////////////////////////////////////////////////////
00634 //     Function: BoundingSphere::contains_plane
00635 //       Access: Protected, Virtual
00636 //  Description: Double-dispatch support: called by contains_other()
00637 //               when the type we're testing for intersection is known
00638 //               to be a plane.
00639 ////////////////////////////////////////////////////////////////////
00640 int BoundingSphere::
00641 contains_plane(const BoundingPlane *plane) const {
00642   return plane->contains_sphere(this) & ~IF_all;
00643 }
 All Classes Functions Variables Enumerations