00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
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
00030
00031
00032
00033 BoundingVolume *BoundingSphere::
00034 make_copy() const {
00035 return new BoundingSphere(*this);
00036 }
00037
00038
00039
00040
00041
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
00054
00055
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
00068
00069
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
00079 return 4.0f / 3.0f * MathNumbers::pi_f * _radius * _radius * _radius;
00080 }
00081
00082
00083
00084
00085
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
00096
00097
00098
00099 void BoundingSphere::
00100 xform(const LMatrix4 &mat) {
00101 nassertv(!mat.is_nan());
00102
00103 if (!is_empty() && !is_infinite()) {
00104
00105
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
00121 _radius *= scale;
00122
00123
00124 _center = _center * mat;
00125 }
00126 }
00127
00128
00129
00130
00131
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
00146
00147
00148
00149
00150
00151 const BoundingSphere *BoundingSphere::
00152 as_bounding_sphere() const {
00153 return this;
00154 }
00155
00156
00157
00158
00159
00160
00161 bool BoundingSphere::
00162 extend_other(BoundingVolume *other) const {
00163 return other->extend_by_sphere(this);
00164 }
00165
00166
00167
00168
00169
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
00180
00181
00182
00183 int BoundingSphere::
00184 contains_other(const BoundingVolume *other) const {
00185 return other->contains_sphere(this);
00186 }
00187
00188
00189
00190
00191
00192
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
00214
00215
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
00236
00237
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
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
00268
00269
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
00282
00283
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
00296
00297
00298
00299 bool BoundingSphere::
00300 around_points(const LPoint3 *first, const LPoint3 *last) {
00301 nassertr(first != last, false);
00302
00303
00304
00305 const LPoint3 *p = first;
00306
00307 #ifndef NDEBUG
00308
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
00327 while (p != last && (*p).is_nan()) {
00328 ++p;
00329 ++skipped_nan;
00330 }
00331 #endif
00332
00333 if (p == last) {
00334
00335
00336
00337 _center = min_box;
00338 _radius = 0.0f;
00339
00340 } else {
00341
00342 while (p != last) {
00343 #ifndef NDEBUG
00344
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
00361 _center = (min_box + max_box) * 0.5f;
00362
00363
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
00389
00390
00391
00392 bool BoundingSphere::
00393 around_finite(const BoundingVolume **first,
00394 const BoundingVolume **last) {
00395 nassertr(first != last, false);
00396
00397
00398
00399
00400
00401
00402
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
00436 _center = (min_box + max_box) * 0.5f;
00437
00438 if (!any_spheres) {
00439
00440
00441 _radius = length(max_box - _center);
00442
00443 } else {
00444
00445
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
00452 PN_stdfloat dist = length(sphere->_center - _center);
00453 _radius = max(_radius, dist + sphere->_radius);
00454
00455 } else {
00456
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
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
00483
00484
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
00506
00507
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
00528
00529 PN_stdfloat A = dot(delta, delta);
00530
00531 nassertr(A != 0.0f, 0);
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
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
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
00569
00570
00571
00572
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
00585 return IF_possible | IF_some | IF_all;
00586
00587 } else if (dist2 > (_radius + sphere->_radius) * (_radius + sphere->_radius)) {
00588
00589 return IF_no_intersection;
00590
00591 } else {
00592
00593 return IF_possible | IF_some;
00594 }
00595 }
00596
00597
00598
00599
00600
00601
00602
00603
00604 int BoundingSphere::
00605 contains_box(const BoundingBox *box) const {
00606 return box->contains_sphere(this) & ~IF_all;
00607 }
00608
00609
00610
00611
00612
00613
00614
00615
00616 int BoundingSphere::
00617 contains_hexahedron(const BoundingHexahedron *hexahedron) const {
00618 return hexahedron->contains_sphere(this) & ~IF_all;
00619 }
00620
00621
00622
00623
00624
00625
00626
00627
00628 int BoundingSphere::
00629 contains_line(const BoundingLine *line) const {
00630 return line->contains_sphere(this) & ~IF_all;
00631 }
00632
00633
00634
00635
00636
00637
00638
00639
00640 int BoundingSphere::
00641 contains_plane(const BoundingPlane *plane) const {
00642 return plane->contains_sphere(this) & ~IF_all;
00643 }