41 LPoint3 BoundingSphere::
43 nassertr(!
is_empty(), LPoint3::zero());
45 return LPoint3(_center[0] - _radius,
47 _center[2] - _radius);
53 LPoint3 BoundingSphere::
55 nassertr(!
is_empty(), LPoint3::zero());
57 return LPoint3(_center[0] + _radius,
59 _center[2] + _radius);
65 PN_stdfloat BoundingSphere::
73 return 4.0f / 3.0f * MathNumbers::pi_f * _radius * _radius * _radius;
79 LPoint3 BoundingSphere::
80 get_approx_center()
const {
81 nassertr(!
is_empty(), LPoint3::zero());
90 xform(
const LMatrix4 &mat) {
91 nassertv(!mat.is_nan());
102 PN_stdfloat xd = dot(x, x);
103 PN_stdfloat yd = dot(y, y);
104 PN_stdfloat zd = dot(z, z);
106 PN_stdfloat scale = max(xd, yd);
107 scale = max(scale, zd);
108 scale = csqrt(scale);
114 _center = _center * mat;
121 void BoundingSphere::
122 output(std::ostream &out)
const {
124 out <<
"bsphere, empty";
126 out <<
"bsphere, infinite";
128 out <<
"bsphere, c (" << _center <<
"), r " << _radius;
144 bool BoundingSphere::
146 return other->extend_by_sphere(
this);
152 bool BoundingSphere::
156 return other->around_spheres(first, last);
164 return other->contains_sphere(
this);
171 bool BoundingSphere::
172 extend_by_point(
const LPoint3 &point) {
173 nassertr(!point.is_nan(),
false);
180 LVector3 v = point - _center;
181 PN_stdfloat dist2 = dot(v, v);
182 if (dist2 > _radius * _radius) {
183 _radius = csqrt(dist2);
192 bool BoundingSphere::
198 _center = sphere->_center;
199 _radius = sphere->_radius;
202 PN_stdfloat dist = length(sphere->_center - _center);
204 _radius = max(_radius, dist + sphere->_radius);
212 bool BoundingSphere::
214 const LVector3 &min1 = box->
get_minq();
215 const LVector3 &max1 = box->
get_maxq();
218 _center = (min1 + max1) * 0.5f;
219 _radius = length(LVector3(max1 - _center));
224 PN_stdfloat max_dist2 = -1.0;
225 for (
int i = 0; i < 8; ++i) {
226 PN_stdfloat dist2 = (box->
get_point(i) - _center).length_squared();
227 if (dist2 > max_dist2) {
231 if (max_dist2 > _radius * _radius) {
232 _radius = csqrt(max_dist2);
242 bool BoundingSphere::
244 nassertr(!hexahedron->
is_empty(),
false);
246 BoundingBox box(hexahedron->get_min(), hexahedron->get_max());
248 return extend_by_box(&box);
254 bool BoundingSphere::
256 nassertr(!volume->
is_empty(),
false);
258 BoundingBox box(volume->get_min(), volume->get_max());
260 return extend_by_box(&box);
266 bool BoundingSphere::
267 around_points(
const LPoint3 *first,
const LPoint3 *last) {
268 nassertr(first != last,
false);
271 const LPoint3 *p = first;
276 while (p != last && (*p).is_nan()) {
281 mathutil_cat.warning()
282 <<
"BoundingSphere around NaN\n";
293 while (p != last && (*p).is_nan()) {
316 min_box.set(min(min_box[0], (*p)[0]),
317 min(min_box[1], (*p)[1]),
318 min(min_box[2], (*p)[2]));
319 max_box.set(max(max_box[0], (*p)[0]),
320 max(max_box[1], (*p)[1]),
321 max(max_box[2], (*p)[2]));
327 _center = (min_box + max_box) * 0.5f;
330 PN_stdfloat max_dist2 = 0.0f;
331 for (p = first; p != last; ++p) {
332 LVector3 v = (*p) - _center;
333 PN_stdfloat dist2 = dot(v, v);
334 max_dist2 = max(max_dist2, dist2);
337 _radius = csqrt(max_dist2);
341 if (skipped_nan != 0) {
342 mathutil_cat.warning()
343 <<
"BoundingSphere ignored " << skipped_nan <<
" NaN points of "
344 << (last - first) <<
" total.\n";
356 bool BoundingSphere::
359 nassertr(first != last,
false);
367 nassertr(!(*p)->is_empty() && !(*p)->is_infinite(),
false);
369 nassertr(vol !=
nullptr,
false);
370 LPoint3 min_box = vol->get_min();
371 LPoint3 max_box = vol->get_max();
375 for (++p; p != last; ++p) {
376 nassertr(!(*p)->is_infinite(),
false);
377 if (!(*p)->is_empty()) {
379 if (vol ==
nullptr) {
383 LPoint3 min1 = vol->get_min();
384 LPoint3 max1 = vol->get_max();
385 min_box.set(min(min_box[0], min1[0]),
386 min(min_box[1], min1[1]),
387 min(min_box[2], min1[2]));
388 max_box.set(max(max_box[0], max1[0]),
389 max(max_box[1], max1[1]),
390 max(max_box[2], max1[2]));
399 _center = (min_box + max_box) * 0.5f;
404 _radius = length(max_box - _center);
410 for (p = first; p != last; ++p) {
411 if (!(*p)->is_empty()) {
413 if (sphere !=
nullptr) {
415 PN_stdfloat dist = length(sphere->_center - _center);
416 _radius = max(_radius, dist + sphere->_radius);
421 nassertr(vol !=
nullptr,
false);
427 PN_stdfloat max_dist2 = -1.0;
428 for (
int i = 0; i < 8; ++i) {
429 PN_stdfloat dist2 = (box.
get_point(i) - _center).length_squared();
430 if (dist2 > max_dist2) {
434 _radius = max(_radius, csqrt(max_dist2));
448 contains_point(
const LPoint3 &point)
const {
449 nassertr(!point.is_nan(), IF_no_intersection);
452 return IF_no_intersection;
455 return IF_possible | IF_some | IF_all;
458 LVector3 v = point - _center;
459 PN_stdfloat dist2 = dot(v, v);
460 return (dist2 <= _radius * _radius) ?
461 IF_possible | IF_some | IF_all : IF_no_intersection;
469 contains_lineseg(
const LPoint3 &a,
const LPoint3 &b)
const {
470 nassertr(!a.is_nan() && !b.is_nan(), IF_no_intersection);
473 return contains_point(a);
476 return IF_no_intersection;
479 return IF_possible | IF_some | IF_all;
483 LVector3 delta = b - a;
488 PN_stdfloat A = dot(delta, delta);
490 nassertr(A != 0.0f, 0);
492 LVector3 fc = from - _center;
493 PN_stdfloat B = 2.0f * dot(delta, fc);
494 PN_stdfloat C = dot(fc, fc) - _radius * _radius;
496 PN_stdfloat radical = B*B - 4.0f*A*C;
498 if (IS_NEARLY_ZERO(radical)) {
500 t1 = t2 = -B / (2.0f*A);
501 return (t1 >= 0.0f && t1 <= 1.0f) ?
502 IF_possible | IF_some : IF_no_intersection;
505 if (radical < 0.0f) {
507 return IF_no_intersection;
510 PN_stdfloat reciprocal_2A = 1.0f/(2.0f*A);
511 PN_stdfloat sqrt_radical = csqrt(radical);
513 t1 = ( -B - sqrt_radical ) * reciprocal_2A;
514 t2 = ( -B + sqrt_radical ) * reciprocal_2A;
516 if (t1 >= 0.0f && t2 <= 1.0f) {
517 return IF_possible | IF_some | IF_all;
518 }
else if (t1 <= 1.0f && t2 >= 0.0f) {
519 return IF_possible | IF_some;
521 return IF_no_intersection;
535 LVector3 v = sphere->_center - _center;
536 PN_stdfloat dist2 = dot(v, v);
538 if (_radius >= sphere->_radius &&
539 dist2 <= (_radius - sphere->_radius) * (_radius - sphere->_radius)) {
541 return IF_possible | IF_some | IF_all;
543 }
else if (dist2 > (_radius + sphere->_radius) * (_radius + sphere->_radius)) {
545 return IF_no_intersection;
549 return IF_possible | IF_some;
559 return box->contains_sphere(
this) & ~IF_all;
568 return hexahedron->contains_sphere(
this) & ~IF_all;
577 return line->contains_sphere(
this) & ~IF_all;
586 return plane->contains_sphere(
this) & ~IF_all;