Panda3D
|
00001 // Filename: boundingHexahedron.cxx 00002 // Created by: drose (03Oct99) 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 "boundingHexahedron.h" 00016 #include "boundingSphere.h" 00017 #include "boundingBox.h" 00018 #include "config_mathutil.h" 00019 00020 #include <math.h> 00021 #include <algorithm> 00022 00023 TypeHandle BoundingHexahedron::_type_handle; 00024 00025 //////////////////////////////////////////////////////////////////// 00026 // Function: BoundingHexahedron::Constructor 00027 // Access: Published 00028 // Description: 00029 //////////////////////////////////////////////////////////////////// 00030 BoundingHexahedron:: 00031 BoundingHexahedron(const LFrustum &frustum, bool is_ortho, 00032 CoordinateSystem cs) { 00033 if (cs == CS_default) { 00034 cs = get_default_coordinate_system(); 00035 } 00036 00037 PN_stdfloat fs = 1.0f; 00038 if (!is_ortho) { 00039 fs = frustum._ffar / frustum._fnear; 00040 } 00041 00042 // We build the points based on a Z-up right-handed frustum. If the 00043 // requested coordinate system is otherwise, we'll convert it in a 00044 // second pass. 00045 _points[0].set(frustum._l * fs, frustum._ffar, frustum._b * fs); 00046 _points[1].set(frustum._r * fs, frustum._ffar, frustum._b * fs); 00047 _points[2].set(frustum._r * fs, frustum._ffar, frustum._t * fs); 00048 _points[3].set(frustum._l * fs, frustum._ffar, frustum._t * fs); 00049 _points[4].set(frustum._l, frustum._fnear, frustum._b); 00050 _points[5].set(frustum._r, frustum._fnear, frustum._b); 00051 _points[6].set(frustum._r, frustum._fnear, frustum._t); 00052 _points[7].set(frustum._l, frustum._fnear, frustum._t); 00053 00054 _flags = 0; 00055 00056 // Now fix the coordinate system, if necessary. 00057 if (cs == CS_zup_right) { 00058 set_centroid(); 00059 set_planes(); 00060 } else { 00061 xform(LMatrix4::convert_mat(CS_zup_right, cs)); 00062 } 00063 } 00064 00065 //////////////////////////////////////////////////////////////////// 00066 // Function: BoundingHexahedron::Constructor 00067 // Access: Published 00068 // Description: 00069 //////////////////////////////////////////////////////////////////// 00070 BoundingHexahedron:: 00071 BoundingHexahedron(const LPoint3 &fll, const LPoint3 &flr, 00072 const LPoint3 &fur, const LPoint3 &ful, 00073 const LPoint3 &nll, const LPoint3 &nlr, 00074 const LPoint3 &nur, const LPoint3 &nul) { 00075 _points[0] = fll; 00076 _points[1] = flr; 00077 _points[2] = fur; 00078 _points[3] = ful; 00079 _points[4] = nll; 00080 _points[5] = nlr; 00081 _points[6] = nur; 00082 _points[7] = nul; 00083 00084 _flags = 0; 00085 set_centroid(); 00086 set_planes(); 00087 } 00088 00089 //////////////////////////////////////////////////////////////////// 00090 // Function: BoundingHexahedron::make_copy 00091 // Access: Public, Virtual 00092 // Description: 00093 //////////////////////////////////////////////////////////////////// 00094 BoundingVolume *BoundingHexahedron:: 00095 make_copy() const { 00096 return new BoundingHexahedron(*this); 00097 } 00098 00099 //////////////////////////////////////////////////////////////////// 00100 // Function: BoundingHexahedron::get_min 00101 // Access: Public, Virtual 00102 // Description: 00103 //////////////////////////////////////////////////////////////////// 00104 LPoint3 BoundingHexahedron:: 00105 get_min() const { 00106 nassertr(!is_empty(), LPoint3(0.0f, 0.0f, 0.0f)); 00107 nassertr(!is_infinite(), LPoint3(0.0f, 0.0f, 0.0f)); 00108 int i; 00109 LPoint3 m = _points[0]; 00110 for (i = 1; i < num_points; i++) { 00111 m.set(min(m[0], _points[i][0]), 00112 min(m[1], _points[i][1]), 00113 min(m[2], _points[i][2])); 00114 } 00115 return m; 00116 } 00117 00118 //////////////////////////////////////////////////////////////////// 00119 // Function: BoundingHexahedron::get_max 00120 // Access: Public, Virtual 00121 // Description: 00122 //////////////////////////////////////////////////////////////////// 00123 LPoint3 BoundingHexahedron:: 00124 get_max() const { 00125 nassertr(!is_empty(), LPoint3(0.0f, 0.0f, 0.0f)); 00126 nassertr(!is_infinite(), LPoint3(0.0f, 0.0f, 0.0f)); 00127 int i; 00128 LPoint3 m = _points[0]; 00129 for (i = 1; i < num_points; i++) { 00130 m.set(max(m[0], _points[i][0]), 00131 max(m[1], _points[i][1]), 00132 max(m[2], _points[i][2])); 00133 } 00134 return m; 00135 } 00136 00137 //////////////////////////////////////////////////////////////////// 00138 // Function: BoundingHexahedron::get_approx_center 00139 // Access: Public, Virtual 00140 // Description: 00141 //////////////////////////////////////////////////////////////////// 00142 LPoint3 BoundingHexahedron:: 00143 get_approx_center() const { 00144 nassertr(!is_empty(), LPoint3(0.0f, 0.0f, 0.0f)); 00145 nassertr(!is_infinite(), LPoint3(0.0f, 0.0f, 0.0f)); 00146 return _centroid; 00147 } 00148 00149 //////////////////////////////////////////////////////////////////// 00150 // Function: BoundingHexahedron::xform 00151 // Access: Public, Virtual 00152 // Description: 00153 //////////////////////////////////////////////////////////////////// 00154 void BoundingHexahedron:: 00155 xform(const LMatrix4 &mat) { 00156 if (!is_empty() && !is_infinite()) { 00157 for (int i = 0; i < num_points; i++) { 00158 _points[i] = _points[i] * mat; 00159 } 00160 set_centroid(); 00161 set_planes(); 00162 } 00163 } 00164 00165 //////////////////////////////////////////////////////////////////// 00166 // Function: BoundingHexahedron::output 00167 // Access: Public, Virtual 00168 // Description: 00169 //////////////////////////////////////////////////////////////////// 00170 void BoundingHexahedron:: 00171 output(ostream &out) const { 00172 if (is_empty()) { 00173 out << "bhexahedron, empty"; 00174 } else if (is_infinite()) { 00175 out << "bhexahedron, infinite"; 00176 } else { 00177 out << "bhexahedron, min " << get_min() << " max " << get_max(); 00178 } 00179 } 00180 00181 //////////////////////////////////////////////////////////////////// 00182 // Function: BoundingHexahedron::write 00183 // Access: Public, Virtual 00184 // Description: 00185 //////////////////////////////////////////////////////////////////// 00186 void BoundingHexahedron:: 00187 write(ostream &out, int indent_level) const { 00188 if (is_empty()) { 00189 indent(out, indent_level) << "bhexahedron, empty\n"; 00190 } else if (is_infinite()) { 00191 out << "bhexahedron, infinite\n"; 00192 } else { 00193 indent(out, indent_level) 00194 << "bhexahedron, min " << get_min() << " max " << get_max() << ":\n"; 00195 int i; 00196 for (i = 0; i < num_points; i++) { 00197 indent(out, indent_level + 2) << _points[i] << "\n"; 00198 } 00199 indent(out, indent_level + 2) << "centroid is " << _centroid << "\n"; 00200 } 00201 } 00202 00203 //////////////////////////////////////////////////////////////////// 00204 // Function: BoundingHexahedron::as_bounding_hexahedron 00205 // Access: Public, Virtual 00206 // Description: Virtual downcast method. Returns this object as a 00207 // pointer of the indicated type, if it is in fact that 00208 // type. Returns NULL if it is not that type. 00209 //////////////////////////////////////////////////////////////////// 00210 const BoundingHexahedron *BoundingHexahedron:: 00211 as_bounding_hexahedron() const { 00212 return this; 00213 } 00214 00215 //////////////////////////////////////////////////////////////////// 00216 // Function: BoundingHexahedron::extend_other 00217 // Access: Protected, Virtual 00218 // Description: 00219 //////////////////////////////////////////////////////////////////// 00220 bool BoundingHexahedron:: 00221 extend_other(BoundingVolume *other) const { 00222 return other->extend_by_hexahedron(this); 00223 } 00224 00225 //////////////////////////////////////////////////////////////////// 00226 // Function: BoundingHexahedron::around_other 00227 // Access: Protected, Virtual 00228 // Description: 00229 //////////////////////////////////////////////////////////////////// 00230 bool BoundingHexahedron:: 00231 around_other(BoundingVolume *other, 00232 const BoundingVolume **first, 00233 const BoundingVolume **last) const { 00234 return other->around_hexahedrons(first, last); 00235 } 00236 00237 //////////////////////////////////////////////////////////////////// 00238 // Function: BoundingHexahedron::contains_other 00239 // Access: Protected, Virtual 00240 // Description: 00241 //////////////////////////////////////////////////////////////////// 00242 int BoundingHexahedron:: 00243 contains_other(const BoundingVolume *other) const { 00244 return other->contains_hexahedron(this); 00245 } 00246 00247 //////////////////////////////////////////////////////////////////// 00248 // Function: BoundingHexahedron::contains_point 00249 // Access: Protected, Virtual 00250 // Description: 00251 //////////////////////////////////////////////////////////////////// 00252 int BoundingHexahedron:: 00253 contains_point(const LPoint3 &point) const { 00254 if (is_empty()) { 00255 return IF_no_intersection; 00256 00257 } else if (is_infinite()) { 00258 return IF_possible | IF_some | IF_all; 00259 00260 } else { 00261 // The hexahedron contains the point iff the point is behind all of 00262 // the planes. 00263 for (int i = 0; i < num_planes; i++) { 00264 const LPlane &p = _planes[i]; 00265 if (p.dist_to_plane(point) > 0.0f) { 00266 return IF_no_intersection; 00267 } 00268 } 00269 return IF_possible | IF_some | IF_all; 00270 } 00271 } 00272 00273 //////////////////////////////////////////////////////////////////// 00274 // Function: BoundingHexahedron::contains_lineseg 00275 // Access: Protected, Virtual 00276 // Description: 00277 //////////////////////////////////////////////////////////////////// 00278 int BoundingHexahedron:: 00279 contains_lineseg(const LPoint3 &a, const LPoint3 &b) const { 00280 if (is_empty()) { 00281 return IF_no_intersection; 00282 00283 } else if (is_infinite()) { 00284 return IF_possible | IF_some | IF_all; 00285 00286 } else { 00287 // The hexahedron does not contains the line segment if both points 00288 // are in front of any one plane. 00289 for (int i = 0; i < num_planes; i++) { 00290 const LPlane &p = _planes[i]; 00291 if (p.dist_to_plane(a) > 0.0f || 00292 p.dist_to_plane(b) > 0.0f) { 00293 return IF_no_intersection; 00294 } 00295 } 00296 00297 // If there is no plane that both points are in front of, the 00298 // hexahedron may or may not contain the line segment. For the 00299 // moment, we won't bother to check that more thoroughly, though. 00300 return IF_possible; 00301 } 00302 } 00303 00304 //////////////////////////////////////////////////////////////////// 00305 // Function: BoundingHexahedron::contains_sphere 00306 // Access: Protected, Virtual 00307 // Description: 00308 //////////////////////////////////////////////////////////////////// 00309 int BoundingHexahedron:: 00310 contains_sphere(const BoundingSphere *sphere) const { 00311 nassertr(!is_empty(), 0); 00312 00313 // The hexahedron contains the sphere iff the sphere is at least 00314 // partly behind all of the planes. 00315 const LPoint3 ¢er = sphere->get_center(); 00316 PN_stdfloat radius = sphere->get_radius(); 00317 00318 int result = IF_possible | IF_some | IF_all; 00319 00320 for (int i = 0; i < num_planes; i++) { 00321 const LPlane &p = _planes[i]; 00322 PN_stdfloat dist = p.dist_to_plane(center); 00323 00324 if (dist > radius) { 00325 // The sphere is completely in front of this plane; it's thus 00326 // completely outside of the hexahedron. 00327 return IF_no_intersection; 00328 00329 } else if (dist > -radius) { 00330 // The sphere is not completely behind this plane, but some of 00331 // it is. 00332 result &= ~IF_all; 00333 } 00334 } 00335 00336 return result; 00337 } 00338 00339 //////////////////////////////////////////////////////////////////// 00340 // Function: BoundingHexahedron::contains_box 00341 // Access: Protected, Virtual 00342 // Description: 00343 //////////////////////////////////////////////////////////////////// 00344 int BoundingHexahedron:: 00345 contains_box(const BoundingBox *box) const { 00346 nassertr(!is_empty(), 0); 00347 nassertr(!box->is_empty(), 0); 00348 00349 // Put the box inside a sphere for the purpose of this test. 00350 const LPoint3 &min = box->get_minq(); 00351 const LPoint3 &max = box->get_maxq(); 00352 LPoint3 center = (min + max) * 0.5f; 00353 PN_stdfloat radius2 = (max - center).length_squared(); 00354 00355 int result = IF_possible | IF_some | IF_all; 00356 00357 for (int i = 0; i < num_planes; i++) { 00358 const LPlane &p = _planes[i]; 00359 PN_stdfloat dist = p.dist_to_plane(center); 00360 PN_stdfloat dist2 = dist * dist; 00361 00362 if (dist2 <= radius2) { 00363 // The sphere is not completely behind this plane, but some of 00364 // it is. 00365 00366 // Look a little closer. 00367 bool all_in = true; 00368 bool all_out = true; 00369 for (int i = 0; i < 8 && (all_in || all_out) ; ++i) { 00370 if (p.dist_to_plane(box->get_point(i)) < 0.0f) { 00371 // This point is inside the plane. 00372 all_out = false; 00373 } else { 00374 // This point is outside the plane. 00375 all_in = false; 00376 } 00377 } 00378 00379 if (all_out) { 00380 return IF_no_intersection; 00381 } else if (!all_in) { 00382 result &= ~IF_all; 00383 } 00384 00385 } else if (dist >= 0.0f) { 00386 // The sphere is completely in front of this plane. 00387 return IF_no_intersection; 00388 } 00389 } 00390 00391 return result; 00392 } 00393 00394 //////////////////////////////////////////////////////////////////// 00395 // Function: BoundingHexahedron::contains_hexahedron 00396 // Access: Protected, Virtual 00397 // Description: 00398 //////////////////////////////////////////////////////////////////// 00399 int BoundingHexahedron:: 00400 contains_hexahedron(const BoundingHexahedron *hexahedron) const { 00401 nassertr(!is_empty(), 0); 00402 nassertr(!hexahedron->is_empty(), 0); 00403 00404 // Put the hexahedron inside a sphere for the purposes of this test. 00405 LPoint3 min = hexahedron->get_min(); 00406 LPoint3 max = hexahedron->get_max(); 00407 LPoint3 center = (min + max) * 0.5f; 00408 PN_stdfloat radius2 = (max - center).length_squared(); 00409 00410 int result = IF_possible | IF_some | IF_all; 00411 00412 for (int i = 0; i < num_planes; i++) { 00413 const LPlane &p = _planes[i]; 00414 PN_stdfloat dist = p.dist_to_plane(center); 00415 PN_stdfloat dist2 = dist * dist; 00416 00417 if (dist >= 0.0f && dist2 > radius2) { 00418 // The sphere is completely in front of this plane; it's thus 00419 // completely outside of the hexahedron. 00420 return IF_no_intersection; 00421 00422 } else {/*if (dist < 0.0f && dist2 < radius2) {*/ 00423 // The sphere is not completely behind this plane, but some of 00424 // it is. 00425 00426 // Look a little closer. 00427 unsigned points_out = 0; 00428 for (int i = 0; i < 8; ++i) { 00429 if (p.dist_to_plane(hexahedron->get_point(i)) > 0.0f) { 00430 // This point is outside the plane. 00431 ++points_out; 00432 } 00433 } 00434 00435 if (points_out != 0) { 00436 if (points_out == 8) { 00437 return IF_no_intersection; 00438 } 00439 result &= ~IF_all; 00440 } 00441 } 00442 } 00443 00444 return result; 00445 } 00446 00447 //////////////////////////////////////////////////////////////////// 00448 // Function: BoundingHexahedron::set_planes 00449 // Access: Private 00450 // Description: 00451 //////////////////////////////////////////////////////////////////// 00452 void BoundingHexahedron:: 00453 set_planes() { 00454 _planes[0] = LPlane(_points[0], _points[3], _points[2]); 00455 00456 // Test to see if we have accidentally inverted our frustum by 00457 // transforming it with a -1 matrix. We do this by ensuring that 00458 // the centroid is in front of all of the planes (actually, we only 00459 // need to test the first plane). 00460 if (_planes[0].dist_to_plane(_centroid) > 0) { 00461 // Oops! We're flipped! Rebuild the planes in the opposite 00462 // direction. 00463 _planes[0] = LPlane(_points[0], _points[2], _points[3]); 00464 _planes[1] = LPlane(_points[0], _points[5], _points[1]); 00465 _planes[2] = LPlane(_points[1], _points[6], _points[2]); 00466 _planes[3] = LPlane(_points[2], _points[7], _points[3]); 00467 _planes[4] = LPlane(_points[3], _points[4], _points[0]); 00468 _planes[5] = LPlane(_points[4], _points[7], _points[6]); 00469 00470 } else { 00471 // No, a perfectly sane universe. 00472 _planes[1] = LPlane(_points[0], _points[1], _points[5]); 00473 _planes[2] = LPlane(_points[1], _points[2], _points[6]); 00474 _planes[3] = LPlane(_points[2], _points[3], _points[7]); 00475 _planes[4] = LPlane(_points[3], _points[0], _points[4]); 00476 _planes[5] = LPlane(_points[4], _points[6], _points[7]); 00477 } 00478 00479 // Still not entirely sure why some code keeps triggering these, but 00480 // I'm taking them out of the normal build for now. 00481 #ifdef _DEBUG 00482 nassertv(_planes[0].dist_to_plane(_centroid) <= 0.001); 00483 nassertv(_planes[1].dist_to_plane(_centroid) <= 0.001); 00484 nassertv(_planes[2].dist_to_plane(_centroid) <= 0.001); 00485 nassertv(_planes[3].dist_to_plane(_centroid) <= 0.001); 00486 nassertv(_planes[4].dist_to_plane(_centroid) <= 0.001); 00487 nassertv(_planes[5].dist_to_plane(_centroid) <= 0.001); 00488 #endif 00489 } 00490 00491 //////////////////////////////////////////////////////////////////// 00492 // Function: BoundingHexahedron::set_centroid 00493 // Access: Private 00494 // Description: 00495 //////////////////////////////////////////////////////////////////// 00496 void BoundingHexahedron:: 00497 set_centroid() { 00498 LPoint3 net = _points[0]; 00499 for (int i = 1; i < num_points; i++) { 00500 net += _points[i]; 00501 } 00502 _centroid = net / (PN_stdfloat)num_points; 00503 }