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