Panda3D
|
00001 // Filename: collisionBox.cxx 00002 // Created by: amith tudur (31Jul09) 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 00016 #include "collisionDSSolid.h" 00017 #include "collisionBox.h" 00018 #include "collisionLine.h" 00019 #include "collisionRay.h" 00020 #include "collisionSegment.h" 00021 #include "collisionHandler.h" 00022 #include "collisionEntry.h" 00023 #include "config_collide.h" 00024 #include "boundingBox.h" 00025 #include "datagram.h" 00026 #include "datagramIterator.h" 00027 #include "bamReader.h" 00028 #include "bamWriter.h" 00029 #include "nearly_zero.h" 00030 #include "cmath.h" 00031 #include "mathNumbers.h" 00032 #include "geom.h" 00033 #include "geomTristrips.h" 00034 #include "geomVertexWriter.h" 00035 #include "config_mathutil.h" 00036 #include "dcast.h" 00037 00038 #include <math.h> 00039 00040 PStatCollector CollisionBox::_volume_pcollector( 00041 "Collision Volumes:CollisionBox"); 00042 PStatCollector CollisionBox::_test_pcollector( 00043 "Collision Tests:CollisionBox"); 00044 TypeHandle CollisionBox::_type_handle; 00045 00046 const int CollisionBox::plane_def[6][4] = { 00047 {0, 4, 5, 1}, 00048 {4, 6, 7, 5}, 00049 {6, 2, 3, 7}, 00050 {2, 0, 1, 3}, 00051 {1, 5, 7, 3}, 00052 {2, 6, 4, 0}, 00053 }; 00054 00055 //////////////////////////////////////////////////////////////////// 00056 // Function: CollisionBox::make_copy 00057 // Access: Public, Virtual 00058 // Description: 00059 //////////////////////////////////////////////////////////////////// 00060 CollisionSolid *CollisionBox:: 00061 make_copy() { 00062 return new CollisionBox(*this); 00063 } 00064 00065 //////////////////////////////////////////////////////////////////// 00066 // Function: CollisionBox::setup_box 00067 // Access: Public, Virtual 00068 // Description: Compute parameters for each of the box's sides 00069 //////////////////////////////////////////////////////////////////// 00070 void CollisionBox:: 00071 setup_box(){ 00072 for(int plane = 0; plane < 6; plane++) { 00073 LPoint3f array[4]; 00074 array[0] = get_point(plane_def[plane][0]); 00075 array[1] = get_point(plane_def[plane][1]); 00076 array[2] = get_point(plane_def[plane][2]); 00077 array[3] = get_point(plane_def[plane][3]); 00078 setup_points(array, array+4, plane); 00079 } 00080 } 00081 00082 //////////////////////////////////////////////////////////////////// 00083 // Function: CollisionBox::setup_points 00084 // Access: Private 00085 // Description: Computes the plane and 2d projection of points that 00086 // make up this side 00087 //////////////////////////////////////////////////////////////////// 00088 void CollisionBox:: 00089 setup_points(const LPoint3f *begin, const LPoint3f *end, int plane) { 00090 int num_points = end - begin; 00091 nassertv(num_points >= 3); 00092 00093 _points[plane].clear(); 00094 00095 // Construct a matrix that rotates the points from the (X,0,Z) plane 00096 // into the 3-d plane. 00097 LMatrix4f to_3d_mat; 00098 calc_to_3d_mat(to_3d_mat, plane); 00099 00100 // And the inverse matrix rotates points from 3-d space into the 2-d 00101 // plane. 00102 _to_2d_mat[plane].invert_from(to_3d_mat); 00103 00104 // Now project all of the points onto the 2-d plane. 00105 00106 const LPoint3f *pi; 00107 for (pi = begin; pi != end; ++pi) { 00108 LPoint3f point = (*pi) * _to_2d_mat[plane]; 00109 _points[plane].push_back(PointDef(point[0], point[2])); 00110 } 00111 00112 nassertv(_points[plane].size() >= 3); 00113 00114 #ifndef NDEBUG 00115 /* 00116 // Now make sure the points define a convex polygon. 00117 if (is_concave()) { 00118 collide_cat.error() << "Invalid concave CollisionPolygon defined:\n"; 00119 const LPoint3f *pi; 00120 for (pi = begin; pi != end; ++pi) { 00121 collide_cat.error(false) << " " << (*pi) << "\n"; 00122 } 00123 collide_cat.error(false) 00124 << " normal " << normal << " with length " << normal.length() << "\n"; 00125 _points.clear(); 00126 } 00127 */ 00128 #endif 00129 00130 compute_vectors(_points[plane]); 00131 } 00132 00133 //////////////////////////////////////////////////////////////////// 00134 // Function: CollisionBox::test_intersection 00135 // Access: Public, Virtual 00136 // Description: First Dispatch point for box as a FROM object 00137 //////////////////////////////////////////////////////////////////// 00138 PT(CollisionEntry) CollisionBox:: 00139 test_intersection(const CollisionEntry &entry) const { 00140 return entry.get_into()->test_intersection_from_box(entry); 00141 } 00142 00143 //////////////////////////////////////////////////////////////////// 00144 // Function: CollisionBox::xform 00145 // Access: Public, Virtual 00146 // Description: Transforms the solid by the indicated matrix. 00147 //////////////////////////////////////////////////////////////////// 00148 void CollisionBox:: 00149 xform(const LMatrix4f &mat) { 00150 _center = _center * mat; 00151 for(int v = 0; v < 8; v++) 00152 _vertex[v] = _vertex[v] * mat; 00153 for(int p = 0; p < 6 ; p++) 00154 _planes[p] = set_plane(p); 00155 _x = _vertex[0].get_x()-_center.get_x(); 00156 _y = _vertex[0].get_y()-_center.get_y(); 00157 _z = _vertex[0].get_z()-_center.get_z(); 00158 _radius = sqrt( _x*_x + _y*_y + _z*_z ); 00159 setup_box(); 00160 mark_viz_stale(); 00161 mark_internal_bounds_stale(); 00162 } 00163 00164 //////////////////////////////////////////////////////////////////// 00165 // Function: CollisionBox::get_collision_origin 00166 // Access: Public, Virtual 00167 // Description: Returns the point in space deemed to be the "origin" 00168 // of the solid for collision purposes. The closest 00169 // intersection point to this origin point is considered 00170 // to be the most significant. 00171 //////////////////////////////////////////////////////////////////// 00172 LPoint3f CollisionBox:: 00173 get_collision_origin() const { 00174 return _center; 00175 } 00176 00177 //////////////////////////////////////////////////////////////////// 00178 // Function: CollisionBox::get_min 00179 // Access: Public, Virtual 00180 // Description: 00181 //////////////////////////////////////////////////////////////////// 00182 LPoint3f CollisionBox:: 00183 get_min() const { 00184 return _min; 00185 } 00186 00187 //////////////////////////////////////////////////////////////////// 00188 // Function: CollisionBox::get_max 00189 // Access: Public, Virtual 00190 // Description: 00191 //////////////////////////////////////////////////////////////////// 00192 LPoint3f CollisionBox:: 00193 get_max() const { 00194 return _max; 00195 } 00196 00197 //////////////////////////////////////////////////////////////////// 00198 // Function: CollisionBox::get_approx_center 00199 // Access: Public, Virtual 00200 // Description: 00201 //////////////////////////////////////////////////////////////////// 00202 LPoint3f CollisionBox:: 00203 get_approx_center() const { 00204 return (_min + _max) * 0.5f; 00205 } 00206 00207 //////////////////////////////////////////////////////////////////// 00208 // Function: CollisionBox::get_volume_pcollector 00209 // Access: Public, Virtual 00210 // Description: Returns a PStatCollector that is used to count the 00211 // number of bounding volume tests made against a solid 00212 // of this type in a given frame. 00213 //////////////////////////////////////////////////////////////////// 00214 PStatCollector &CollisionBox:: 00215 get_volume_pcollector() { 00216 return _volume_pcollector; 00217 } 00218 00219 //////////////////////////////////////////////////////////////////// 00220 // Function: CollisionBox::get_test_pcollector 00221 // Access: Public, Virtual 00222 // Description: Returns a PStatCollector that is used to count the 00223 // number of intersection tests made against a solid 00224 // of this type in a given frame. 00225 //////////////////////////////////////////////////////////////////// 00226 PStatCollector &CollisionBox:: 00227 get_test_pcollector() { 00228 return _test_pcollector; 00229 } 00230 00231 //////////////////////////////////////////////////////////////////// 00232 // Function: CollisionBox::output 00233 // Access: Public, Virtual 00234 // Description: 00235 //////////////////////////////////////////////////////////////////// 00236 void CollisionBox:: 00237 output(ostream &out) const { 00238 } 00239 00240 //////////////////////////////////////////////////////////////////// 00241 // Function: CollisionBox::compute_internal_bounds 00242 // Access: Protected, Virtual 00243 // Description: Sphere is chosen as the Bounding Volume type for 00244 // speed and efficiency 00245 //////////////////////////////////////////////////////////////////// 00246 PT(BoundingVolume) CollisionBox:: 00247 compute_internal_bounds() const { 00248 return new BoundingSphere(_center, _radius); 00249 } 00250 00251 //////////////////////////////////////////////////////////////////// 00252 // Function: CollisionBox::test_intersection_from_sphere 00253 // Access: Public, Virtual 00254 // Description: Double dispatch point for sphere as FROM object 00255 //////////////////////////////////////////////////////////////////// 00256 PT(CollisionEntry) CollisionBox:: 00257 test_intersection_from_sphere(const CollisionEntry &entry) const { 00258 00259 const CollisionSphere *sphere; 00260 DCAST_INTO_R(sphere, entry.get_from(), 0); 00261 00262 CPT(TransformState) wrt_space = entry.get_wrt_space(); 00263 CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space(); 00264 00265 const LMatrix4f &wrt_mat = wrt_space->get_mat(); 00266 00267 LPoint3f orig_center = sphere->get_center() * wrt_mat; 00268 LPoint3f from_center = orig_center; 00269 bool moved_from_center = false; 00270 float t = 1.0f; 00271 LPoint3f contact_point(from_center); 00272 float actual_t = 1.0f; 00273 00274 LVector3f from_radius_v = 00275 LVector3f(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat; 00276 float from_radius_2 = from_radius_v.length_squared(); 00277 float from_radius = csqrt(from_radius_2); 00278 00279 int ip; 00280 float max_dist,dist; 00281 bool intersect; 00282 Planef plane; 00283 LVector3f normal; 00284 00285 for(ip = 0, intersect = false; ip < 6 && !intersect; ip++) { 00286 plane = get_plane( ip ); 00287 if (_points[ip].size() < 3) { 00288 continue; 00289 } 00290 if (wrt_prev_space != wrt_space) { 00291 // If we have a delta between the previous position and the 00292 // current position, we use that to determine some more properties 00293 // of the collision. 00294 LPoint3f b = from_center; 00295 LPoint3f a = sphere->get_center() * wrt_prev_space->get_mat(); 00296 LVector3f delta = b - a; 00297 00298 // First, there is no collision if the "from" object is definitely 00299 // moving in the same direction as the plane's normal. 00300 float dot = delta.dot(plane.get_normal()); 00301 if (dot > 0.1f) { 00302 continue; // no intersection 00303 } 00304 00305 if (IS_NEARLY_ZERO(dot)) { 00306 // If we're moving parallel to the plane, the sphere is tested 00307 // at its final point. Leave it as it is. 00308 00309 } else { 00310 // Otherwise, we're moving into the plane; the sphere is tested 00311 // at the point along its path that is closest to intersecting 00312 // the plane. This may be the actual intersection point, or it 00313 // may be the starting point or the final point. 00314 // dot is equal to the (negative) magnitude of 'delta' along the 00315 // direction of the plane normal 00316 // t = ratio of (distance from start pos to plane) to (distance 00317 // from start pos to end pos), along axis of plane normal 00318 float dist_to_p = plane.dist_to_plane(a); 00319 t = (dist_to_p / -dot); 00320 00321 // also compute the actual contact point and time of contact 00322 // for handlers that need it 00323 actual_t = ((dist_to_p - from_radius) / -dot); 00324 actual_t = min(1.0f, max(0.0f, actual_t)); 00325 contact_point = a + (actual_t * delta); 00326 00327 if (t >= 1.0f) { 00328 // Leave it where it is. 00329 00330 } else if (t < 0.0f) { 00331 from_center = a; 00332 moved_from_center = true; 00333 } else { 00334 from_center = a + t * delta; 00335 moved_from_center = true; 00336 } 00337 } 00338 } 00339 00340 normal = (has_effective_normal() && sphere->get_respect_effective_normal()) ? get_effective_normal() : plane.get_normal(); 00341 00342 #ifndef NDEBUG 00343 /*if (!IS_THRESHOLD_EQUAL(normal.length_squared(), 1.0f, 0.001), NULL) { 00344 std::cout 00345 << "polygon within " << entry.get_into_node_path() 00346 << " has normal " << normal << " of length " << normal.length() 00347 << "\n"; 00348 normal.normalize(); 00349 }*/ 00350 #endif 00351 00352 // The nearest point within the plane to our center is the 00353 // intersection of the line (center, center - normal) with the plane. 00354 00355 if (!plane.intersects_line(dist, from_center, -(plane.get_normal()))) { 00356 // No intersection with plane? This means the plane's effective 00357 // normal was within the plane itself. A useless polygon. 00358 continue; 00359 } 00360 00361 if (dist > from_radius || dist < -from_radius) { 00362 // No intersection with the plane. 00363 continue; 00364 } 00365 00366 LPoint2f p = to_2d(from_center - dist * plane.get_normal(), ip); 00367 float edge_dist = 0.0f; 00368 00369 const ClipPlaneAttrib *cpa = entry.get_into_clip_planes(); 00370 if (cpa != (ClipPlaneAttrib *)NULL) { 00371 // We have a clip plane; apply it. 00372 Points new_points; 00373 if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform(),ip)) { 00374 // All points are behind the clip plane; just do the default 00375 // test. 00376 edge_dist = dist_to_polygon(p, _points[ip]); 00377 } else if (new_points.empty()) { 00378 // The polygon is completely clipped. 00379 continue; 00380 } else { 00381 // Test against the clipped polygon. 00382 edge_dist = dist_to_polygon(p, new_points); 00383 } 00384 } else { 00385 // No clip plane is in effect. Do the default test. 00386 edge_dist = dist_to_polygon(p, _points[ip]); 00387 } 00388 00389 // Now we have edge_dist, which is the distance from the sphere 00390 // center to the nearest edge of the polygon, within the polygon's 00391 // plane. edge_dist<0 means the point is within the polygon. 00392 if(edge_dist < 0) { 00393 intersect = true; 00394 continue; 00395 } 00396 00397 if((edge_dist > 0) && 00398 ((edge_dist * edge_dist + dist * dist) > from_radius_2)) { 00399 // No intersection; the circle is outside the polygon. 00400 continue; 00401 } 00402 00403 // The sphere appears to intersect the polygon. If the edge is less 00404 // than from_radius away, the sphere may be resting on an edge of 00405 // the polygon. Determine how far the center of the sphere must 00406 // remain from the plane, based on its distance from the nearest 00407 // edge. 00408 00409 max_dist = from_radius; 00410 if (edge_dist >= 0.0f) { 00411 float max_dist_2 = max(from_radius_2 - edge_dist * edge_dist, 0.0f); 00412 max_dist = csqrt(max_dist_2); 00413 } 00414 00415 if (dist > max_dist) { 00416 // There's no intersection: the sphere is hanging off the edge. 00417 continue; 00418 } 00419 intersect = true; 00420 } 00421 if( !intersect ) 00422 return NULL; 00423 00424 if (collide_cat.is_debug()) { 00425 collide_cat.debug() 00426 << "intersection detected from " << entry.get_from_node_path() 00427 << " into " << entry.get_into_node_path() << "\n"; 00428 } 00429 00430 PT(CollisionEntry) new_entry = new CollisionEntry(entry); 00431 00432 float into_depth = max_dist - dist; 00433 if (moved_from_center) { 00434 // We have to base the depth of intersection on the sphere's final 00435 // resting point, not the point from which we tested the 00436 // intersection. 00437 float orig_dist; 00438 plane.intersects_line(orig_dist, orig_center, -normal); 00439 into_depth = max_dist - orig_dist; 00440 } 00441 00442 new_entry->set_surface_normal(normal); 00443 new_entry->set_surface_point(from_center - normal * dist); 00444 new_entry->set_interior_point(from_center - normal * (dist + into_depth)); 00445 new_entry->set_contact_pos(contact_point); 00446 new_entry->set_contact_normal(plane.get_normal()); 00447 new_entry->set_t(actual_t); 00448 00449 return new_entry; 00450 } 00451 00452 00453 //////////////////////////////////////////////////////////////////// 00454 // Function: CollisionBox::test_intersection_from_ray 00455 // Access: Public, Virtual 00456 // Description: Double dispatch point for ray as a FROM object 00457 //////////////////////////////////////////////////////////////////// 00458 PT(CollisionEntry) CollisionBox:: 00459 test_intersection_from_ray(const CollisionEntry &entry) const { 00460 const CollisionRay *ray; 00461 DCAST_INTO_R(ray, entry.get_from(), 0); 00462 const LMatrix4f &wrt_mat = entry.get_wrt_mat(); 00463 00464 LPoint3f from_origin = ray->get_origin() * wrt_mat; 00465 LVector3f from_direction = ray->get_direction() * wrt_mat; 00466 00467 int i, j; 00468 float t, near_t; 00469 bool intersect; 00470 Planef plane; 00471 Planef near_plane; 00472 00473 //Returns the details about the first plane of the box that the ray 00474 //intersects. 00475 for(i = 0, intersect = false, t = 0, j = 0; i < 6 && j < 2; i++) { 00476 plane = get_plane(i); 00477 00478 if (!plane.intersects_line(t, from_origin, from_direction)) { 00479 // No intersection. The ray is parallel to the plane. 00480 continue; 00481 } 00482 00483 if (t < 0.0f) { 00484 // The intersection point is before the start of the ray, and so 00485 // the ray is entirely in front of the plane. 00486 continue; 00487 } 00488 LPoint3f plane_point = from_origin + t * from_direction; 00489 LPoint2f p = to_2d(plane_point, i); 00490 00491 if (!point_is_inside(p, _points[i])){ 00492 continue; 00493 } 00494 intersect = true; 00495 if(j) { 00496 if(t < near_t) { 00497 near_plane = plane; 00498 near_t = t; 00499 } 00500 } 00501 else { 00502 near_plane = plane; 00503 near_t = t; 00504 } 00505 ++j; 00506 } 00507 00508 00509 if(!intersect) { 00510 //No intersection with ANY of the box's planes has been detected 00511 return NULL; 00512 } 00513 00514 if (collide_cat.is_debug()) { 00515 collide_cat.debug() 00516 << "intersection detected from " << entry.get_from_node_path() 00517 << " into " << entry.get_into_node_path() << "\n"; 00518 } 00519 00520 PT(CollisionEntry) new_entry = new CollisionEntry(entry); 00521 00522 LPoint3f into_intersection_point = from_origin + near_t * from_direction; 00523 00524 LVector3f normal = 00525 (has_effective_normal() && ray->get_respect_effective_normal()) 00526 ? get_effective_normal() : near_plane.get_normal(); 00527 00528 new_entry->set_surface_normal(normal); 00529 new_entry->set_surface_point(into_intersection_point); 00530 00531 return new_entry; 00532 } 00533 00534 00535 //////////////////////////////////////////////////////////////////// 00536 // Function: CollisionBox::test_intersection_from_segment 00537 // Access: Public, Virtual 00538 // Description: Double dispatch point for segment as a FROM object 00539 //////////////////////////////////////////////////////////////////// 00540 PT(CollisionEntry) CollisionBox:: 00541 test_intersection_from_segment(const CollisionEntry &entry) const { 00542 const CollisionSegment *seg; 00543 DCAST_INTO_R(seg, entry.get_from(), 0); 00544 const LMatrix4f &wrt_mat = entry.get_wrt_mat(); 00545 00546 LPoint3f from_origin = seg->get_point_a() * wrt_mat; 00547 LPoint3f from_extent = seg->get_point_b() * wrt_mat; 00548 LVector3f from_direction = from_extent - from_origin; 00549 00550 int i, j; 00551 float t, near_t; 00552 bool intersect; 00553 Planef plane; 00554 Planef near_plane; 00555 00556 //Returns the details about the first plane of the box that the 00557 //segment intersects. 00558 for(i = 0, intersect = false, t = 0, j = 0; i < 6 && j < 2; i++) { 00559 plane = get_plane(i); 00560 00561 if (!plane.intersects_line(t, from_origin, from_direction)) { 00562 // No intersection. The segment is parallel to the plane. 00563 continue; 00564 } 00565 00566 if (t < 0.0f || t > 1.0f) { 00567 // The intersection point is before the start of the segment, 00568 // or after the end of the segment, so the segment is either 00569 // entirely in front of or behind the plane. 00570 continue; 00571 } 00572 LPoint3f plane_point = from_origin + t * from_direction; 00573 LPoint2f p = to_2d(plane_point, i); 00574 00575 if (!point_is_inside(p, _points[i])){ 00576 continue; 00577 } 00578 intersect = true; 00579 if(j) { 00580 if(t < near_t) { 00581 near_plane = plane; 00582 near_t = t; 00583 } 00584 } 00585 else { 00586 near_plane = plane; 00587 near_t = t; 00588 } 00589 ++j; 00590 } 00591 00592 00593 if(!intersect) { 00594 //No intersection with ANY of the box's planes has been detected 00595 return NULL; 00596 } 00597 00598 if (collide_cat.is_debug()) { 00599 collide_cat.debug() 00600 << "intersection detected from " << entry.get_from_node_path() 00601 << " into " << entry.get_into_node_path() << "\n"; 00602 } 00603 00604 PT(CollisionEntry) new_entry = new CollisionEntry(entry); 00605 00606 LPoint3f into_intersection_point = from_origin + near_t * from_direction; 00607 00608 LVector3f normal = 00609 (has_effective_normal() && seg->get_respect_effective_normal()) 00610 ? get_effective_normal() : near_plane.get_normal(); 00611 00612 new_entry->set_surface_normal(normal); 00613 new_entry->set_surface_point(into_intersection_point); 00614 00615 return new_entry; 00616 } 00617 00618 00619 //////////////////////////////////////////////////////////////////// 00620 // Function: CollisionBox::test_intersection_from_box 00621 // Access: Public, Virtual 00622 // Description: Double dispatch point for box as a FROM object 00623 // NOT Implemented. 00624 //////////////////////////////////////////////////////////////////// 00625 PT(CollisionEntry) CollisionBox:: 00626 test_intersection_from_box(const CollisionEntry &entry) const { 00627 return NULL; 00628 } 00629 00630 00631 //////////////////////////////////////////////////////////////////// 00632 // Function: CollisionBox::fill_viz_geom 00633 // Access: Protected, Virtual 00634 // Description: Fills the _viz_geom GeomNode up with Geoms suitable 00635 // for rendering this solid. 00636 //////////////////////////////////////////////////////////////////// 00637 void CollisionBox:: 00638 fill_viz_geom() { 00639 if (collide_cat.is_debug()) { 00640 collide_cat.debug() 00641 << "Recomputing viz for " << *this << "\n"; 00642 } 00643 00644 PT(GeomVertexData) vdata = new GeomVertexData 00645 ("collision", GeomVertexFormat::get_v3(), 00646 Geom::UH_static); 00647 GeomVertexWriter vertex(vdata, InternalName::get_vertex()); 00648 00649 for(int i = 0; i < 6; i++) { 00650 for(int j = 0; j < 4; j++) 00651 vertex.add_data3f(get_point(plane_def[i][j])); 00652 00653 PT(GeomTrifans) body = new GeomTrifans(Geom::UH_static); 00654 body->add_consecutive_vertices(i*4, 4); 00655 body->close_primitive(); 00656 00657 PT(Geom) geom = new Geom(vdata); 00658 geom->add_primitive(body); 00659 00660 _viz_geom->add_geom(geom, get_solid_viz_state()); 00661 _bounds_viz_geom->add_geom(geom, get_solid_bounds_viz_state()); 00662 } 00663 } 00664 00665 //////////////////////////////////////////////////////////////////// 00666 // Function: CollisionBox::apply_clip_plane 00667 // Access: Private 00668 // Description: Clips the polygon by all of the clip planes named in 00669 // the clip plane attribute and fills new_points up with 00670 // the resulting points. 00671 // 00672 // The return value is true if the set of points is 00673 // unmodified (all points are behind all the clip 00674 // planes), or false otherwise. 00675 //////////////////////////////////////////////////////////////////// 00676 bool CollisionBox:: 00677 apply_clip_plane(CollisionBox::Points &new_points, 00678 const ClipPlaneAttrib *cpa, 00679 const TransformState *net_transform, int plane_no) const { 00680 bool all_in = true; 00681 00682 int num_planes = cpa->get_num_on_planes(); 00683 bool first_plane = true; 00684 00685 for (int i = 0; i < num_planes; i++) { 00686 NodePath plane_path = cpa->get_on_plane(i); 00687 PlaneNode *plane_node = DCAST(PlaneNode, plane_path.node()); 00688 if ((plane_node->get_clip_effect() & PlaneNode::CE_collision) != 0) { 00689 CPT(TransformState) new_transform = 00690 net_transform->invert_compose(plane_path.get_net_transform()); 00691 00692 Planef plane = plane_node->get_plane() * new_transform->get_mat(); 00693 if (first_plane) { 00694 first_plane = false; 00695 if (!clip_polygon(new_points, _points[plane_no], plane, plane_no)) { 00696 all_in = false; 00697 } 00698 } else { 00699 Points last_points; 00700 last_points.swap(new_points); 00701 if (!clip_polygon(new_points, last_points, plane, plane_no)) { 00702 all_in = false; 00703 } 00704 } 00705 } 00706 } 00707 00708 if (!all_in) { 00709 compute_vectors(new_points); 00710 } 00711 00712 return all_in; 00713 } 00714 //////////////////////////////////////////////////////////////////// 00715 // Function: CollisionBox::clip_polygon 00716 // Access: Private 00717 // Description: Clips the source_points of the polygon by the 00718 // indicated clipping plane, and modifies new_points to 00719 // reflect the new set of clipped points (but does not 00720 // compute the vectors in new_points). 00721 // 00722 // The return value is true if the set of points is 00723 // unmodified (all points are behind the clip plane), or 00724 // false otherwise. 00725 //////////////////////////////////////////////////////////////////// 00726 bool CollisionBox:: 00727 clip_polygon(CollisionBox::Points &new_points, 00728 const CollisionBox::Points &source_points, 00729 const Planef &plane, int plane_no) const { 00730 new_points.clear(); 00731 if (source_points.empty()) { 00732 return true; 00733 } 00734 00735 LPoint3f from3d; 00736 LVector3f delta3d; 00737 if (!plane.intersects_plane(from3d, delta3d, get_plane(plane_no))) { 00738 // The clipping plane is parallel to the polygon. The polygon is 00739 // either all in or all out. 00740 if (plane.dist_to_plane(get_plane(plane_no).get_point()) < 0.0) { 00741 // A point within the polygon is behind the clipping plane: the 00742 // polygon is all in. 00743 new_points = source_points; 00744 return true; 00745 } 00746 return false; 00747 } 00748 00749 // Project the line of intersection into the 2-d plane. Now we have 00750 // a 2-d clipping line. 00751 LPoint2f from2d = to_2d(from3d,plane_no); 00752 LVector2f delta2d = to_2d(delta3d,plane_no); 00753 00754 float a = -delta2d[1]; 00755 float b = delta2d[0]; 00756 float c = from2d[0] * delta2d[1] - from2d[1] * delta2d[0]; 00757 00758 // Now walk through the points. Any point on the left of our line 00759 // gets removed, and the line segment clipped at the point of 00760 // intersection. 00761 00762 // We might increase the number of vertices by as many as 1, if the 00763 // plane clips off exactly one corner. (We might also decrease the 00764 // number of vertices, or keep them the same number.) 00765 new_points.reserve(source_points.size() + 1); 00766 00767 LPoint2f last_point = source_points.back()._p; 00768 bool last_is_in = !is_right(last_point - from2d, delta2d); 00769 bool all_in = last_is_in; 00770 Points::const_iterator pi; 00771 for (pi = source_points.begin(); pi != source_points.end(); ++pi) { 00772 const LPoint2f &this_point = (*pi)._p; 00773 bool this_is_in = !is_right(this_point - from2d, delta2d); 00774 00775 // There appears to be a compiler bug in gcc 4.0: we need to 00776 // extract this comparison outside of the if statement. 00777 bool crossed_over = (this_is_in != last_is_in); 00778 if (crossed_over) { 00779 // We have just crossed over the clipping line. Find the point 00780 // of intersection. 00781 LVector2f d = this_point - last_point; 00782 float denom = (a * d[0] + b * d[1]); 00783 if (denom != 0.0) { 00784 float t = -(a * last_point[0] + b * last_point[1] + c) / denom; 00785 LPoint2f p = last_point + t * d; 00786 00787 new_points.push_back(PointDef(p[0], p[1])); 00788 last_is_in = this_is_in; 00789 } 00790 } 00791 00792 if (this_is_in) { 00793 // We are behind the clipping line. Keep the point. 00794 new_points.push_back(PointDef(this_point[0], this_point[1])); 00795 } else { 00796 all_in = false; 00797 } 00798 00799 last_point = this_point; 00800 } 00801 00802 return all_in; 00803 } 00804 00805 00806 //////////////////////////////////////////////////////////////////// 00807 // Function: CollisionBox:: 00808 // Access: Private 00809 // Description: Returns the linear distance from the 2-d point to the 00810 // nearest part of the polygon defined by the points 00811 // vector. The result is negative if the point is 00812 // within the polygon. 00813 //////////////////////////////////////////////////////////////////// 00814 float CollisionBox:: 00815 dist_to_polygon(const LPoint2f &p, const CollisionBox::Points &points) const { 00816 00817 // We know that that the polygon is convex and is defined with the 00818 // points in counterclockwise order. Therefore, we simply compare 00819 // the signed distance to each line segment; we ignore any negative 00820 // values, and take the minimum of all the positive values. 00821 00822 // If all values are negative, the point is within the polygon; we 00823 // therefore return an arbitrary negative result. 00824 00825 bool got_dist = false; 00826 float best_dist = -1.0f; 00827 00828 size_t num_points = points.size(); 00829 for (size_t i = 0; i < num_points - 1; ++i) { 00830 float d = dist_to_line_segment(p, points[i]._p, points[i + 1]._p, 00831 points[i]._v); 00832 if (d >= 0.0f) { 00833 if (!got_dist || d < best_dist) { 00834 best_dist = d; 00835 got_dist = true; 00836 } 00837 } 00838 } 00839 00840 float d = dist_to_line_segment(p, points[num_points - 1]._p, points[0]._p, 00841 points[num_points - 1]._v); 00842 if (d >= 0.0f) { 00843 if (!got_dist || d < best_dist) { 00844 best_dist = d; 00845 got_dist = true; 00846 } 00847 } 00848 00849 return best_dist; 00850 } 00851 00852 //////////////////////////////////////////////////////////////////// 00853 // Function: CollisionBox::dist_to_line_segment 00854 // Access: Private, Static 00855 // Description: Returns the linear distance of p to the line segment 00856 // defined by f and t, where v = (t - f).normalize(). 00857 // The result is negative if p is left of the line, 00858 // positive if it is right of the line. If the result 00859 // is positive, it is constrained by endpoints of the 00860 // line segment (i.e. the result might be larger than it 00861 // would be for a straight distance-to-line test). If 00862 // the result is negative, we don't bother. 00863 //////////////////////////////////////////////////////////////////// 00864 float CollisionBox:: 00865 dist_to_line_segment(const LPoint2f &p, 00866 const LPoint2f &f, const LPoint2f &t, 00867 const LVector2f &v) { 00868 LVector2f v1 = (p - f); 00869 float d = (v1[0] * v[1] - v1[1] * v[0]); 00870 if (d < 0.0f) { 00871 return d; 00872 } 00873 00874 // Compute the nearest point on the line. 00875 LPoint2f q = p + LVector2f(-v[1], v[0]) * d; 00876 00877 // Now constrain that point to the line segment. 00878 if (v[0] > 0.0f) { 00879 // X+ 00880 if (v[1] > 0.0f) { 00881 // Y+ 00882 if (v[0] > v[1]) { 00883 // X-dominant. 00884 if (q[0] < f[0]) { 00885 return (p - f).length(); 00886 } if (q[0] > t[0]) { 00887 return (p - t).length(); 00888 } else { 00889 return d; 00890 } 00891 } else { 00892 // Y-dominant. 00893 if (q[1] < f[1]) { 00894 return (p - f).length(); 00895 } if (q[1] > t[1]) { 00896 return (p - t).length(); 00897 } else { 00898 return d; 00899 } 00900 } 00901 } else { 00902 // Y- 00903 if (v[0] > -v[1]) { 00904 // X-dominant. 00905 if (q[0] < f[0]) { 00906 return (p - f).length(); 00907 } if (q[0] > t[0]) { 00908 return (p - t).length(); 00909 } else { 00910 return d; 00911 } 00912 } else { 00913 // Y-dominant. 00914 if (q[1] > f[1]) { 00915 return (p - f).length(); 00916 } if (q[1] < t[1]) { 00917 return (p - t).length(); 00918 } else { 00919 return d; 00920 } 00921 } 00922 } 00923 } else { 00924 // X- 00925 if (v[1] > 0.0f) { 00926 // Y+ 00927 if (-v[0] > v[1]) { 00928 // X-dominant. 00929 if (q[0] > f[0]) { 00930 return (p - f).length(); 00931 } if (q[0] < t[0]) { 00932 return (p - t).length(); 00933 } else { 00934 return d; 00935 } 00936 } else { 00937 // Y-dominant. 00938 if (q[1] < f[1]) { 00939 return (p - f).length(); 00940 } if (q[1] > t[1]) { 00941 return (p - t).length(); 00942 } else { 00943 return d; 00944 } 00945 } 00946 } else { 00947 // Y- 00948 if (-v[0] > -v[1]) { 00949 // X-dominant. 00950 if (q[0] > f[0]) { 00951 return (p - f).length(); 00952 } if (q[0] < t[0]) { 00953 return (p - t).length(); 00954 } else { 00955 return d; 00956 } 00957 } else { 00958 // Y-dominant. 00959 if (q[1] > f[1]) { 00960 return (p - f).length(); 00961 } if (q[1] < t[1]) { 00962 return (p - t).length(); 00963 } else { 00964 return d; 00965 } 00966 } 00967 } 00968 } 00969 } 00970 00971 //////////////////////////////////////////////////////////////////// 00972 // Function: CollisionBox::point_is_inside 00973 // Access: Private 00974 // Description: Returns true if the indicated point is within the 00975 // polygon's 2-d space, false otherwise. 00976 //////////////////////////////////////////////////////////////////// 00977 bool CollisionBox:: 00978 point_is_inside(const LPoint2f &p, const CollisionBox::Points &points) const { 00979 // We insist that the polygon be convex. This makes things a bit simpler. 00980 // In the case of a convex polygon, defined with points in counterclockwise 00981 // order, a point is interior to the polygon iff the point is not right of 00982 // each of the edges. 00983 for (int i = 0; i < (int)points.size() - 1; i++) { 00984 if (is_right(p - points[i]._p, points[i+1]._p - points[i]._p)) { 00985 return false; 00986 } 00987 } 00988 if (is_right(p - points[points.size() - 1]._p, 00989 points[0]._p - points[points.size() - 1]._p)) { 00990 return false; 00991 } 00992 00993 return true; 00994 } 00995 00996 //////////////////////////////////////////////////////////////////// 00997 // Function: CollisionBox::compute_point 00998 // Access: Protected 00999 // Description: Returns a point on the surface of the sphere. 01000 // latitude and longitude range from 0.0 to 1.0. This 01001 // is used by fill_viz_geom() to create a visible 01002 // representation of the sphere. 01003 //////////////////////////////////////////////////////////////////// 01004 Vertexf CollisionBox:: 01005 compute_point(float latitude, float longitude) const { 01006 float s1, c1; 01007 csincos(latitude * MathNumbers::pi_f, &s1, &c1); 01008 01009 float s2, c2; 01010 csincos(longitude * 2.0f * MathNumbers::pi_f, &s2, &c2); 01011 01012 Vertexf p(s1 * c2, s1 * s2, c1); 01013 return p * get_radius() + get_center(); 01014 } 01015 01016 //////////////////////////////////////////////////////////////////// 01017 // Function: CollisionBox::compute_vectors 01018 // Access: Private, Static 01019 // Description: Now that the _p members of the given points array 01020 // have been computed, go back and compute all of the _v 01021 // members. 01022 //////////////////////////////////////////////////////////////////// 01023 void CollisionBox:: 01024 compute_vectors(Points &points) { 01025 size_t num_points = points.size(); 01026 for (size_t i = 0; i < num_points; i++) { 01027 points[i]._v = points[(i + 1) % num_points]._p - points[i]._p; 01028 points[i]._v.normalize(); 01029 } 01030 } 01031 01032 //////////////////////////////////////////////////////////////////// 01033 // Function: CollisionBox::register_with_read_factory 01034 // Access: Public, Static 01035 // Description: Factory method to generate a CollisionBox object 01036 //////////////////////////////////////////////////////////////////// 01037 void CollisionBox:: 01038 register_with_read_factory() { 01039 BamReader::get_factory()->register_factory(get_class_type(), make_CollisionBox); 01040 } 01041 01042 //////////////////////////////////////////////////////////////////// 01043 // Function: CollisionBox::write_datagram 01044 // Access: Public 01045 // Description: Function to write the important information in 01046 // the particular object to a Datagram 01047 //////////////////////////////////////////////////////////////////// 01048 void CollisionBox:: 01049 write_datagram(BamWriter *manager, Datagram &me) { 01050 CollisionSolid::write_datagram(manager, me); 01051 _center.write_datagram(me); 01052 _min.write_datagram(me); 01053 _max.write_datagram(me); 01054 for(int i=0; i < 8; i++) { 01055 _vertex[i].write_datagram(me); 01056 } 01057 me.add_float32(_radius); 01058 me.add_float32(_x); 01059 me.add_float32(_y); 01060 me.add_float32(_z); 01061 for(int i=0; i < 6; i++) { 01062 _planes[i].write_datagram(me); 01063 } 01064 for(int i=0; i < 6; i++) { 01065 _to_2d_mat[i].write_datagram(me); 01066 } 01067 for(int i=0; i < 6; i++) { 01068 me.add_uint16(_points[i].size()); 01069 for (size_t j = 0; j < _points[i].size(); j++) { 01070 _points[i][j]._p.write_datagram(me); 01071 _points[i][j]._v.write_datagram(me); 01072 } 01073 } 01074 } 01075 01076 //////////////////////////////////////////////////////////////////// 01077 // Function: CollisionBox::make_CollisionBox 01078 // Access: Protected 01079 // Description: Factory method to generate a CollisionBox object 01080 //////////////////////////////////////////////////////////////////// 01081 TypedWritable *CollisionBox:: 01082 make_CollisionBox(const FactoryParams ¶ms) { 01083 CollisionBox *me = new CollisionBox; 01084 DatagramIterator scan; 01085 BamReader *manager; 01086 01087 parse_params(params, scan, manager); 01088 me->fillin(scan, manager); 01089 return me; 01090 } 01091 01092 //////////////////////////////////////////////////////////////////// 01093 // Function: CollisionBox::fillin 01094 // Access: Protected 01095 // Description: Function that reads out of the datagram (or asks 01096 // manager to read) all of the data that is needed to 01097 // re-create this object and stores it in the appropiate 01098 // place 01099 //////////////////////////////////////////////////////////////////// 01100 void CollisionBox:: 01101 fillin(DatagramIterator& scan, BamReader* manager) { 01102 CollisionSolid::fillin(scan, manager); 01103 _center.read_datagram(scan); 01104 _min.read_datagram(scan); 01105 _max.read_datagram(scan); 01106 for(int i=0; i < 8; i++) { 01107 _vertex[i].read_datagram(scan); 01108 } 01109 _radius = scan.get_float32(); 01110 _x = scan.get_float32(); 01111 _y = scan.get_float32(); 01112 _z = scan.get_float32(); 01113 for(int i=0; i < 6; i++) { 01114 _planes[i].read_datagram(scan); 01115 } 01116 for(int i=0; i < 6; i++) { 01117 _to_2d_mat[i].read_datagram(scan); 01118 } 01119 for(int i=0; i < 6; i++) { 01120 size_t size = scan.get_uint16(); 01121 for (size_t j = 0; j < size; j++) { 01122 LPoint2f p; 01123 LVector2f v; 01124 p.read_datagram(scan); 01125 v.read_datagram(scan); 01126 _points[i].push_back(PointDef(p, v)); 01127 } 01128 } 01129 }