Panda3D
|
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 }