Panda3D
|
00001 // Filename: collisionPolygon.cxx 00002 // Created by: drose (25Apr00) 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 "collisionPolygon.h" 00016 #include "collisionHandler.h" 00017 #include "collisionEntry.h" 00018 #include "collisionSphere.h" 00019 #include "collisionLine.h" 00020 #include "collisionRay.h" 00021 #include "collisionSegment.h" 00022 #include "config_collide.h" 00023 #include "cullTraverserData.h" 00024 #include "boundingBox.h" 00025 #include "pointerToArray.h" 00026 #include "geomNode.h" 00027 #include "geom.h" 00028 #include "datagram.h" 00029 #include "datagramIterator.h" 00030 #include "bamReader.h" 00031 #include "bamWriter.h" 00032 #include "transformState.h" 00033 #include "clipPlaneAttrib.h" 00034 #include "nearly_zero.h" 00035 #include "geom.h" 00036 #include "geomTrifans.h" 00037 #include "geomLinestrips.h" 00038 #include "geomVertexWriter.h" 00039 #include "renderState.h" 00040 00041 #include <algorithm> 00042 00043 PStatCollector CollisionPolygon::_volume_pcollector("Collision Volumes:CollisionPolygon"); 00044 PStatCollector CollisionPolygon::_test_pcollector("Collision Tests:CollisionPolygon"); 00045 TypeHandle CollisionPolygon::_type_handle; 00046 00047 //////////////////////////////////////////////////////////////////// 00048 // Function: CollisionPolygon::Copy Constructor 00049 // Access: Public 00050 // Description: 00051 //////////////////////////////////////////////////////////////////// 00052 CollisionPolygon:: 00053 CollisionPolygon(const CollisionPolygon ©) : 00054 CollisionPlane(copy), 00055 _points(copy._points), 00056 _to_2d_mat(copy._to_2d_mat) 00057 { 00058 } 00059 00060 //////////////////////////////////////////////////////////////////// 00061 // Function: CollisionPolygon::make_copy 00062 // Access: Public, Virtual 00063 // Description: 00064 //////////////////////////////////////////////////////////////////// 00065 CollisionSolid *CollisionPolygon:: 00066 make_copy() { 00067 return new CollisionPolygon(*this); 00068 } 00069 00070 //////////////////////////////////////////////////////////////////// 00071 // Function: CollisionPolygon::verify_points 00072 // Access: Public, Static 00073 // Description: Verifies that the indicated set of points will define 00074 // a valid CollisionPolygon: that is, at least three 00075 // non-collinear points, with no points repeated. 00076 // 00077 // This does not check that the polygon defined is 00078 // convex; that check is made later, once we have 00079 // projected the points to 2-d space where the decision 00080 // is easier. 00081 //////////////////////////////////////////////////////////////////// 00082 bool CollisionPolygon:: 00083 verify_points(const LPoint3 *begin, const LPoint3 *end) { 00084 int num_points = end - begin; 00085 if (num_points < 3) { 00086 return false; 00087 } 00088 00089 bool all_ok = true; 00090 00091 // First, check for repeated or invalid points. 00092 const LPoint3 *pi; 00093 for (pi = begin; pi != end && all_ok; ++pi) { 00094 if ((*pi).is_nan()) { 00095 all_ok = false; 00096 } else { 00097 // Make sure no points are repeated. 00098 const LPoint3 *pj; 00099 for (pj = begin; pj != pi && all_ok; ++pj) { 00100 if ((*pj) == (*pi)) { 00101 all_ok = false; 00102 } 00103 } 00104 } 00105 } 00106 00107 if (all_ok) { 00108 // Create a plane to determine the planarity of the first three 00109 // points (or the first two points and the nth point thereafter, in 00110 // case the first three points happen to be collinear). 00111 bool got_normal = false; 00112 for (int i = 2; i < num_points && !got_normal; i++) { 00113 LPlane plane(begin[0], begin[1], begin[i]); 00114 LVector3 normal = plane.get_normal(); 00115 PN_stdfloat normal_length = normal.length(); 00116 got_normal = IS_THRESHOLD_EQUAL(normal_length, 1.0f, 0.001f); 00117 } 00118 00119 if (!got_normal) { 00120 all_ok = false; 00121 } 00122 } 00123 00124 return all_ok; 00125 } 00126 00127 //////////////////////////////////////////////////////////////////// 00128 // Function: CollisionPolygon::is_valid 00129 // Access: Public 00130 // Description: Returns true if the CollisionPolygon is valid 00131 // (that is, it has at least three vertices), or false 00132 // otherwise. 00133 //////////////////////////////////////////////////////////////////// 00134 bool CollisionPolygon:: 00135 is_valid() const { 00136 return (_points.size() >= 3); 00137 } 00138 00139 //////////////////////////////////////////////////////////////////// 00140 // Function: CollisionPolygon::is_concave 00141 // Access: Public 00142 // Description: Returns true if the CollisionPolygon appears to be 00143 // concave, or false if it is safely convex. 00144 //////////////////////////////////////////////////////////////////// 00145 bool CollisionPolygon:: 00146 is_concave() const { 00147 if (_points.size() < 3) { 00148 // It's not even a valid polygon. 00149 return true; 00150 } 00151 00152 LPoint2 p0 = _points[0]._p; 00153 LPoint2 p1 = _points[1]._p; 00154 PN_stdfloat dx1 = p1[0] - p0[0]; 00155 PN_stdfloat dy1 = p1[1] - p0[1]; 00156 p0 = p1; 00157 p1 = _points[2]._p; 00158 00159 PN_stdfloat dx2 = p1[0] - p0[0]; 00160 PN_stdfloat dy2 = p1[1] - p0[1]; 00161 int asum = ((dx1 * dy2 - dx2 * dy1 >= 0.0f) ? 1 : 0); 00162 00163 for (size_t i = 0; i < _points.size() - 1; i++) { 00164 p0 = p1; 00165 p1 = _points[(i+3) % _points.size()]._p; 00166 00167 dx1 = dx2; 00168 dy1 = dy2; 00169 dx2 = p1[0] - p0[0]; 00170 dy2 = p1[1] - p0[1]; 00171 int csum = ((dx1 * dy2 - dx2 * dy1 >= 0.0f) ? 1 : 0); 00172 00173 if (csum ^ asum) { 00174 // Oops, the polygon is concave. 00175 return true; 00176 } 00177 } 00178 00179 // The polygon is safely convex. 00180 return false; 00181 } 00182 00183 //////////////////////////////////////////////////////////////////// 00184 // Function: CollisionPolygon::xform 00185 // Access: Public, Virtual 00186 // Description: Transforms the solid by the indicated matrix. 00187 //////////////////////////////////////////////////////////////////// 00188 void CollisionPolygon:: 00189 xform(const LMatrix4 &mat) { 00190 // We need to convert all the vertices to 3-d for this operation, 00191 // and then convert them back. Hopefully we won't lose too much 00192 // precision during all of this. 00193 00194 if (collide_cat.is_spam()) { 00195 collide_cat.spam() 00196 << "CollisionPolygon transformed by:\n"; 00197 mat.write(collide_cat.spam(false), 2); 00198 if (_points.empty()) { 00199 collide_cat.spam(false) 00200 << " (no points)\n"; 00201 } 00202 } 00203 00204 if (!_points.empty()) { 00205 LMatrix4 to_3d_mat; 00206 rederive_to_3d_mat(to_3d_mat); 00207 00208 epvector<LPoint3> verts; 00209 verts.reserve(_points.size()); 00210 Points::const_iterator pi; 00211 for (pi = _points.begin(); pi != _points.end(); ++pi) { 00212 verts.push_back(to_3d((*pi)._p, to_3d_mat) * mat); 00213 } 00214 00215 const LPoint3 *verts_begin = &verts[0]; 00216 const LPoint3 *verts_end = verts_begin + verts.size(); 00217 setup_points(verts_begin, verts_end); 00218 } 00219 00220 CollisionSolid::xform(mat); 00221 } 00222 00223 //////////////////////////////////////////////////////////////////// 00224 // Function: CollisionPolygon::get_collision_origin 00225 // Access: Public, Virtual 00226 // Description: Returns the point in space deemed to be the "origin" 00227 // of the solid for collision purposes. The closest 00228 // intersection point to this origin point is considered 00229 // to be the most significant. 00230 //////////////////////////////////////////////////////////////////// 00231 LPoint3 CollisionPolygon:: 00232 get_collision_origin() const { 00233 LMatrix4 to_3d_mat; 00234 rederive_to_3d_mat(to_3d_mat); 00235 00236 LPoint2 median = _points[0]._p; 00237 for (int n = 1; n < (int)_points.size(); n++) { 00238 median += _points[n]._p; 00239 } 00240 median /= _points.size(); 00241 00242 return to_3d(median, to_3d_mat); 00243 } 00244 00245 //////////////////////////////////////////////////////////////////// 00246 // Function: CollisionPolygon::get_viz 00247 // Access: Public, Virtual 00248 // Description: Returns a GeomNode that may be rendered to visualize 00249 // the CollisionSolid. This is used during the cull 00250 // traversal to render the CollisionNodes that have been 00251 // made visible. 00252 //////////////////////////////////////////////////////////////////// 00253 PT(PandaNode) CollisionPolygon:: 00254 get_viz(const CullTraverser *trav, const CullTraverserData &data, 00255 bool bounds_only) const { 00256 const ClipPlaneAttrib *cpa = DCAST(ClipPlaneAttrib, data._state->get_attrib(ClipPlaneAttrib::get_class_slot())); 00257 if (cpa == (const ClipPlaneAttrib *)NULL) { 00258 // Fortunately, the polygon is not clipped. This is the normal, 00259 // easy case. 00260 return CollisionSolid::get_viz(trav, data, bounds_only); 00261 } 00262 00263 if (collide_cat.is_debug()) { 00264 collide_cat.debug() 00265 << "drawing polygon with clip plane " << *cpa << "\n"; 00266 } 00267 00268 // The polygon is clipped. We need to render it clipped. We could 00269 // just turn on the ClipPlaneAttrib state and render the full 00270 // polygon, letting the hardware do the clipping, but we get fancy 00271 // and clip it by hand instead, just to prove that our clipping 00272 // algorithm works properly. This does require some more dynamic 00273 // work. 00274 Points new_points; 00275 if (apply_clip_plane(new_points, cpa, data.get_net_transform(trav))) { 00276 // All points are behind the clip plane; just draw the original 00277 // polygon. 00278 return CollisionSolid::get_viz(trav, data, bounds_only); 00279 } 00280 00281 if (new_points.empty()) { 00282 // All points are in front of the clip plane; draw nothing. 00283 return NULL; 00284 } 00285 00286 // Draw the clipped polygon. 00287 PT(GeomNode) viz_geom_node = new GeomNode("viz"); 00288 PT(GeomNode) bounds_viz_geom_node = new GeomNode("bounds_viz"); 00289 draw_polygon(viz_geom_node, bounds_viz_geom_node, new_points); 00290 00291 if (bounds_only) { 00292 return bounds_viz_geom_node.p(); 00293 } else { 00294 return viz_geom_node.p(); 00295 } 00296 } 00297 00298 //////////////////////////////////////////////////////////////////// 00299 // Function: CollisionPolygon::get_volume_pcollector 00300 // Access: Public, Virtual 00301 // Description: Returns a PStatCollector that is used to count the 00302 // number of bounding volume tests made against a solid 00303 // of this type in a given frame. 00304 //////////////////////////////////////////////////////////////////// 00305 PStatCollector &CollisionPolygon:: 00306 get_volume_pcollector() { 00307 return _volume_pcollector; 00308 } 00309 00310 //////////////////////////////////////////////////////////////////// 00311 // Function: CollisionPolygon::get_test_pcollector 00312 // Access: Public, Virtual 00313 // Description: Returns a PStatCollector that is used to count the 00314 // number of intersection tests made against a solid 00315 // of this type in a given frame. 00316 //////////////////////////////////////////////////////////////////// 00317 PStatCollector &CollisionPolygon:: 00318 get_test_pcollector() { 00319 return _test_pcollector; 00320 } 00321 00322 //////////////////////////////////////////////////////////////////// 00323 // Function: CollisionPolygon::output 00324 // Access: Public, Virtual 00325 // Description: 00326 //////////////////////////////////////////////////////////////////// 00327 void CollisionPolygon:: 00328 output(ostream &out) const { 00329 out << "cpolygon, (" << get_plane() 00330 << "), " << _points.size() << " vertices"; 00331 } 00332 00333 //////////////////////////////////////////////////////////////////// 00334 // Function: CollisionPolygon::write 00335 // Access: Public, Virtual 00336 // Description: 00337 //////////////////////////////////////////////////////////////////// 00338 void CollisionPolygon:: 00339 write(ostream &out, int indent_level) const { 00340 indent(out, indent_level) << (*this) << "\n"; 00341 Points::const_iterator pi; 00342 for (pi = _points.begin(); pi != _points.end(); ++pi) { 00343 indent(out, indent_level + 2) << (*pi)._p << "\n"; 00344 } 00345 00346 LMatrix4 to_3d_mat; 00347 rederive_to_3d_mat(to_3d_mat); 00348 out << "In 3-d space:\n"; 00349 for (pi = _points.begin(); pi != _points.end(); ++pi) { 00350 LVertex vert = to_3d((*pi)._p, to_3d_mat); 00351 indent(out, indent_level + 2) << vert << "\n"; 00352 } 00353 } 00354 00355 //////////////////////////////////////////////////////////////////// 00356 // Function: CollisionPolygon::compute_internal_bounds 00357 // Access: Protected, Virtual 00358 // Description: 00359 //////////////////////////////////////////////////////////////////// 00360 PT(BoundingVolume) CollisionPolygon:: 00361 compute_internal_bounds() const { 00362 if (_points.empty()) { 00363 return new BoundingBox; 00364 } 00365 00366 LMatrix4 to_3d_mat; 00367 rederive_to_3d_mat(to_3d_mat); 00368 00369 Points::const_iterator pi = _points.begin(); 00370 LPoint3 p = to_3d((*pi)._p, to_3d_mat); 00371 00372 LPoint3 x = p; 00373 LPoint3 n = p; 00374 00375 for (++pi; pi != _points.end(); ++pi) { 00376 p = to_3d((*pi)._p, to_3d_mat); 00377 00378 n.set(min(n[0], p[0]), 00379 min(n[1], p[1]), 00380 min(n[2], p[2])); 00381 x.set(max(x[0], p[0]), 00382 max(x[1], p[1]), 00383 max(x[2], p[2])); 00384 } 00385 00386 return new BoundingBox(n, x); 00387 } 00388 00389 //////////////////////////////////////////////////////////////////// 00390 // Function: CollisionPolygon::test_intersection_from_sphere 00391 // Access: Protected, Virtual 00392 // Description: This is part of the double-dispatch implementation of 00393 // test_intersection(). It is called when the "from" 00394 // object is a sphere. 00395 //////////////////////////////////////////////////////////////////// 00396 PT(CollisionEntry) CollisionPolygon:: 00397 test_intersection_from_sphere(const CollisionEntry &entry) const { 00398 if (_points.size() < 3) { 00399 return NULL; 00400 } 00401 00402 const CollisionSphere *sphere; 00403 DCAST_INTO_R(sphere, entry.get_from(), 0); 00404 00405 CPT(TransformState) wrt_space = entry.get_wrt_space(); 00406 CPT(TransformState) wrt_prev_space = entry.get_wrt_prev_space(); 00407 00408 const LMatrix4 &wrt_mat = wrt_space->get_mat(); 00409 00410 LPoint3 orig_center = sphere->get_center() * wrt_mat; 00411 LPoint3 from_center = orig_center; 00412 bool moved_from_center = false; 00413 PN_stdfloat t = 1.0f; 00414 LPoint3 contact_point(from_center); 00415 PN_stdfloat actual_t = 1.0f; 00416 00417 LVector3 from_radius_v = 00418 LVector3(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat; 00419 PN_stdfloat from_radius_2 = from_radius_v.length_squared(); 00420 PN_stdfloat from_radius = csqrt(from_radius_2); 00421 00422 if (wrt_prev_space != wrt_space) { 00423 // If we have a delta between the previous position and the 00424 // current position, we use that to determine some more properties 00425 // of the collision. 00426 LPoint3 b = from_center; 00427 LPoint3 a = sphere->get_center() * wrt_prev_space->get_mat(); 00428 LVector3 delta = b - a; 00429 00430 // First, there is no collision if the "from" object is definitely 00431 // moving in the same direction as the plane's normal. 00432 PN_stdfloat dot = delta.dot(get_normal()); 00433 if (dot > 0.1f) { 00434 return NULL; 00435 } 00436 00437 if (IS_NEARLY_ZERO(dot)) { 00438 // If we're moving parallel to the plane, the sphere is tested 00439 // at its final point. Leave it as it is. 00440 00441 } else { 00442 // Otherwise, we're moving into the plane; the sphere is tested 00443 // at the point along its path that is closest to intersecting 00444 // the plane. This may be the actual intersection point, or it 00445 // may be the starting point or the final point. 00446 // dot is equal to the (negative) magnitude of 'delta' along the 00447 // direction of the plane normal 00448 // t = ratio of (distance from start pos to plane) to (distance 00449 // from start pos to end pos), along axis of plane normal 00450 PN_stdfloat dist_to_p = dist_to_plane(a); 00451 t = (dist_to_p / -dot); 00452 00453 // also compute the actual contact point and time of contact 00454 // for handlers that need it 00455 actual_t = ((dist_to_p - from_radius) / -dot); 00456 actual_t = min((PN_stdfloat)1.0, max((PN_stdfloat)0.0, actual_t)); 00457 contact_point = a + (actual_t * delta); 00458 00459 if (t >= 1.0f) { 00460 // Leave it where it is. 00461 00462 } else if (t < 0.0f) { 00463 from_center = a; 00464 moved_from_center = true; 00465 00466 } else { 00467 from_center = a + t * delta; 00468 moved_from_center = true; 00469 } 00470 } 00471 } 00472 00473 LVector3 normal = (has_effective_normal() && sphere->get_respect_effective_normal()) ? get_effective_normal() : get_normal(); 00474 #ifndef NDEBUG 00475 if (!IS_THRESHOLD_EQUAL(normal.length_squared(), 1.0f, 0.001), NULL) { 00476 collide_cat.info() 00477 << "polygon within " << entry.get_into_node_path() 00478 << " has normal " << normal << " of length " << normal.length() 00479 << "\n"; 00480 normal.normalize(); 00481 } 00482 #endif 00483 00484 // The nearest point within the plane to our center is the 00485 // intersection of the line (center, center - normal) with the plane. 00486 PN_stdfloat dist; 00487 if (!get_plane().intersects_line(dist, from_center, -get_normal())) { 00488 // No intersection with plane? This means the plane's effective 00489 // normal was within the plane itself. A useless polygon. 00490 return NULL; 00491 } 00492 00493 if (dist > from_radius || dist < -from_radius) { 00494 // No intersection with the plane. 00495 return NULL; 00496 } 00497 00498 LPoint2 p = to_2d(from_center - dist * get_normal()); 00499 PN_stdfloat edge_dist = 0.0f; 00500 00501 const ClipPlaneAttrib *cpa = entry.get_into_clip_planes(); 00502 if (cpa != (ClipPlaneAttrib *)NULL) { 00503 // We have a clip plane; apply it. 00504 Points new_points; 00505 if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform())) { 00506 // All points are behind the clip plane; just do the default 00507 // test. 00508 edge_dist = dist_to_polygon(p, _points); 00509 00510 } else if (new_points.empty()) { 00511 // The polygon is completely clipped. 00512 return NULL; 00513 00514 } else { 00515 // Test against the clipped polygon. 00516 edge_dist = dist_to_polygon(p, new_points); 00517 } 00518 00519 } else { 00520 // No clip plane is in effect. Do the default test. 00521 edge_dist = dist_to_polygon(p, _points); 00522 } 00523 00524 // Now we have edge_dist, which is the distance from the sphere 00525 // center to the nearest edge of the polygon, within the polygon's 00526 // plane. 00527 00528 if (edge_dist > from_radius) { 00529 // No intersection; the circle is outside the polygon. 00530 return NULL; 00531 } 00532 00533 // The sphere appears to intersect the polygon. If the edge is less 00534 // than from_radius away, the sphere may be resting on an edge of 00535 // the polygon. Determine how far the center of the sphere must 00536 // remain from the plane, based on its distance from the nearest 00537 // edge. 00538 00539 PN_stdfloat max_dist = from_radius; 00540 if (edge_dist >= 0.0f) { 00541 PN_stdfloat max_dist_2 = max(from_radius_2 - edge_dist * edge_dist, (PN_stdfloat)0.0); 00542 max_dist = csqrt(max_dist_2); 00543 } 00544 00545 if (dist > max_dist) { 00546 // There's no intersection: the sphere is hanging off the edge. 00547 return NULL; 00548 } 00549 00550 if (collide_cat.is_debug()) { 00551 collide_cat.debug() 00552 << "intersection detected from " << entry.get_from_node_path() 00553 << " into " << entry.get_into_node_path() << "\n"; 00554 } 00555 PT(CollisionEntry) new_entry = new CollisionEntry(entry); 00556 00557 PN_stdfloat into_depth = max_dist - dist; 00558 if (moved_from_center) { 00559 // We have to base the depth of intersection on the sphere's final 00560 // resting point, not the point from which we tested the 00561 // intersection. 00562 PN_stdfloat orig_dist; 00563 get_plane().intersects_line(orig_dist, orig_center, -normal); 00564 into_depth = max_dist - orig_dist; 00565 } 00566 00567 new_entry->set_surface_normal(normal); 00568 new_entry->set_surface_point(from_center - normal * dist); 00569 new_entry->set_interior_point(from_center - normal * (dist + into_depth)); 00570 new_entry->set_contact_pos(contact_point); 00571 new_entry->set_contact_normal(get_normal()); 00572 new_entry->set_t(actual_t); 00573 00574 return new_entry; 00575 } 00576 00577 //////////////////////////////////////////////////////////////////// 00578 // Function: CollisionPolygon::test_intersection_from_line 00579 // Access: Protected, Virtual 00580 // Description: This is part of the double-dispatch implementation of 00581 // test_intersection(). It is called when the "from" 00582 // object is a line. 00583 //////////////////////////////////////////////////////////////////// 00584 PT(CollisionEntry) CollisionPolygon:: 00585 test_intersection_from_line(const CollisionEntry &entry) const { 00586 if (_points.size() < 3) { 00587 return NULL; 00588 } 00589 00590 const CollisionLine *line; 00591 DCAST_INTO_R(line, entry.get_from(), 0); 00592 00593 const LMatrix4 &wrt_mat = entry.get_wrt_mat(); 00594 00595 LPoint3 from_origin = line->get_origin() * wrt_mat; 00596 LVector3 from_direction = line->get_direction() * wrt_mat; 00597 00598 PN_stdfloat t; 00599 if (!get_plane().intersects_line(t, from_origin, from_direction)) { 00600 // No intersection. 00601 return NULL; 00602 } 00603 00604 LPoint3 plane_point = from_origin + t * from_direction; 00605 LPoint2 p = to_2d(plane_point); 00606 00607 const ClipPlaneAttrib *cpa = entry.get_into_clip_planes(); 00608 if (cpa != (ClipPlaneAttrib *)NULL) { 00609 // We have a clip plane; apply it. 00610 Points new_points; 00611 if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform())) { 00612 // All points are behind the clip plane. 00613 if (!point_is_inside(p, _points)) { 00614 return NULL; 00615 } 00616 00617 } else { 00618 if (new_points.size() < 3) { 00619 return NULL; 00620 } 00621 if (!point_is_inside(p, new_points)) { 00622 return NULL; 00623 } 00624 } 00625 00626 } else { 00627 // No clip plane is in effect. Do the default test. 00628 if (!point_is_inside(p, _points)) { 00629 return NULL; 00630 } 00631 } 00632 00633 if (collide_cat.is_debug()) { 00634 collide_cat.debug() 00635 << "intersection detected from " << entry.get_from_node_path() 00636 << " into " << entry.get_into_node_path() << "\n"; 00637 } 00638 PT(CollisionEntry) new_entry = new CollisionEntry(entry); 00639 00640 LVector3 normal = (has_effective_normal() && line->get_respect_effective_normal()) ? get_effective_normal() : get_normal(); 00641 00642 new_entry->set_surface_normal(normal); 00643 new_entry->set_surface_point(plane_point); 00644 00645 return new_entry; 00646 } 00647 00648 //////////////////////////////////////////////////////////////////// 00649 // Function: CollisionPolygon::test_intersection_from_ray 00650 // Access: Protected, Virtual 00651 // Description: This is part of the double-dispatch implementation of 00652 // test_intersection(). It is called when the "from" 00653 // object is a ray. 00654 //////////////////////////////////////////////////////////////////// 00655 PT(CollisionEntry) CollisionPolygon:: 00656 test_intersection_from_ray(const CollisionEntry &entry) const { 00657 if (_points.size() < 3) { 00658 return NULL; 00659 } 00660 00661 const CollisionRay *ray; 00662 DCAST_INTO_R(ray, entry.get_from(), 0); 00663 00664 const LMatrix4 &wrt_mat = entry.get_wrt_mat(); 00665 00666 LPoint3 from_origin = ray->get_origin() * wrt_mat; 00667 LVector3 from_direction = ray->get_direction() * wrt_mat; 00668 00669 PN_stdfloat t; 00670 if (!get_plane().intersects_line(t, from_origin, from_direction)) { 00671 // No intersection. 00672 return NULL; 00673 } 00674 00675 if (t < 0.0f) { 00676 // The intersection point is before the start of the ray. 00677 return NULL; 00678 } 00679 00680 LPoint3 plane_point = from_origin + t * from_direction; 00681 LPoint2 p = to_2d(plane_point); 00682 00683 const ClipPlaneAttrib *cpa = entry.get_into_clip_planes(); 00684 if (cpa != (ClipPlaneAttrib *)NULL) { 00685 // We have a clip plane; apply it. 00686 Points new_points; 00687 if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform())) { 00688 // All points are behind the clip plane. 00689 if (!point_is_inside(p, _points)) { 00690 return NULL; 00691 } 00692 00693 } else { 00694 if (new_points.size() < 3) { 00695 return NULL; 00696 } 00697 if (!point_is_inside(p, new_points)) { 00698 return NULL; 00699 } 00700 } 00701 00702 } else { 00703 // No clip plane is in effect. Do the default test. 00704 if (!point_is_inside(p, _points)) { 00705 return NULL; 00706 } 00707 } 00708 00709 if (collide_cat.is_debug()) { 00710 collide_cat.debug() 00711 << "intersection detected from " << entry.get_from_node_path() 00712 << " into " << entry.get_into_node_path() << "\n"; 00713 } 00714 PT(CollisionEntry) new_entry = new CollisionEntry(entry); 00715 00716 LVector3 normal = (has_effective_normal() && ray->get_respect_effective_normal()) ? get_effective_normal() : get_normal(); 00717 00718 new_entry->set_surface_normal(normal); 00719 new_entry->set_surface_point(plane_point); 00720 00721 return new_entry; 00722 } 00723 00724 //////////////////////////////////////////////////////////////////// 00725 // Function: CollisionPolygon::test_intersection_from_segment 00726 // Access: Public, Virtual 00727 // Description: This is part of the double-dispatch implementation of 00728 // test_intersection(). It is called when the "from" 00729 // object is a segment. 00730 //////////////////////////////////////////////////////////////////// 00731 PT(CollisionEntry) CollisionPolygon:: 00732 test_intersection_from_segment(const CollisionEntry &entry) const { 00733 if (_points.size() < 3) { 00734 return NULL; 00735 } 00736 00737 const CollisionSegment *segment; 00738 DCAST_INTO_R(segment, entry.get_from(), 0); 00739 00740 const LMatrix4 &wrt_mat = entry.get_wrt_mat(); 00741 00742 LPoint3 from_a = segment->get_point_a() * wrt_mat; 00743 LPoint3 from_b = segment->get_point_b() * wrt_mat; 00744 LPoint3 from_direction = from_b - from_a; 00745 00746 PN_stdfloat t; 00747 if (!get_plane().intersects_line(t, from_a, from_direction)) { 00748 // No intersection. 00749 return NULL; 00750 } 00751 00752 if (t < 0.0f || t > 1.0f) { 00753 // The intersection point is before the start of the segment or 00754 // after the end of the segment. 00755 return NULL; 00756 } 00757 00758 LPoint3 plane_point = from_a + t * from_direction; 00759 LPoint2 p = to_2d(plane_point); 00760 00761 const ClipPlaneAttrib *cpa = entry.get_into_clip_planes(); 00762 if (cpa != (ClipPlaneAttrib *)NULL) { 00763 // We have a clip plane; apply it. 00764 Points new_points; 00765 if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform())) { 00766 // All points are behind the clip plane. 00767 if (!point_is_inside(p, _points)) { 00768 return NULL; 00769 } 00770 00771 } else { 00772 if (new_points.size() < 3) { 00773 return NULL; 00774 } 00775 if (!point_is_inside(p, new_points)) { 00776 return NULL; 00777 } 00778 } 00779 00780 } else { 00781 // No clip plane is in effect. Do the default test. 00782 if (!point_is_inside(p, _points)) { 00783 return NULL; 00784 } 00785 } 00786 00787 if (collide_cat.is_debug()) { 00788 collide_cat.debug() 00789 << "intersection detected from " << entry.get_from_node_path() 00790 << " into " << entry.get_into_node_path() << "\n"; 00791 } 00792 PT(CollisionEntry) new_entry = new CollisionEntry(entry); 00793 00794 LVector3 normal = (has_effective_normal() && segment->get_respect_effective_normal()) ? get_effective_normal() : get_normal(); 00795 00796 new_entry->set_surface_normal(normal); 00797 new_entry->set_surface_point(plane_point); 00798 00799 return new_entry; 00800 } 00801 00802 //////////////////////////////////////////////////////////////////// 00803 // Function: CollisionPolygon::test_intersection_from_parabola 00804 // Access: Public, Virtual 00805 // Description: This is part of the double-dispatch implementation of 00806 // test_intersection(). It is called when the "from" 00807 // object is a parabola. 00808 //////////////////////////////////////////////////////////////////// 00809 PT(CollisionEntry) CollisionPolygon:: 00810 test_intersection_from_parabola(const CollisionEntry &entry) const { 00811 if (_points.size() < 3) { 00812 return NULL; 00813 } 00814 00815 const CollisionParabola *parabola; 00816 DCAST_INTO_R(parabola, entry.get_from(), 0); 00817 00818 const LMatrix4 &wrt_mat = entry.get_wrt_mat(); 00819 00820 // Convert the parabola into local coordinate space. 00821 LParabola local_p(parabola->get_parabola()); 00822 local_p.xform(wrt_mat); 00823 00824 PN_stdfloat t1, t2; 00825 if (!get_plane().intersects_parabola(t1, t2, local_p)) { 00826 // No intersection. 00827 return NULL; 00828 } 00829 00830 PN_stdfloat t; 00831 if (t1 >= parabola->get_t1() && t1 <= parabola->get_t2()) { 00832 if (t2 >= parabola->get_t1() && t2 <= parabola->get_t2()) { 00833 // Both intersection points are within our segment of the 00834 // parabola. Choose the first of the two. 00835 t = min(t1, t2); 00836 } else { 00837 // Only t1 is within our segment. 00838 t = t1; 00839 } 00840 00841 } else if (t2 >= parabola->get_t1() && t2 <= parabola->get_t2()) { 00842 // Only t2 is within our segment. 00843 t = t2; 00844 00845 } else { 00846 // Neither intersection point is within our segment. 00847 return NULL; 00848 } 00849 00850 LPoint3 plane_point = local_p.calc_point(t); 00851 LPoint2 p = to_2d(plane_point); 00852 00853 const ClipPlaneAttrib *cpa = entry.get_into_clip_planes(); 00854 if (cpa != (ClipPlaneAttrib *)NULL) { 00855 // We have a clip plane; apply it. 00856 Points new_points; 00857 if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform())) { 00858 // All points are behind the clip plane. 00859 if (!point_is_inside(p, _points)) { 00860 return NULL; 00861 } 00862 00863 } else { 00864 if (new_points.size() < 3) { 00865 return NULL; 00866 } 00867 if (!point_is_inside(p, new_points)) { 00868 return NULL; 00869 } 00870 } 00871 00872 } else { 00873 // No clip plane is in effect. Do the default test. 00874 if (!point_is_inside(p, _points)) { 00875 return NULL; 00876 } 00877 } 00878 00879 if (collide_cat.is_debug()) { 00880 collide_cat.debug() 00881 << "intersection detected from " << entry.get_from_node_path() 00882 << " into " << entry.get_into_node_path() << "\n"; 00883 } 00884 PT(CollisionEntry) new_entry = new CollisionEntry(entry); 00885 00886 LVector3 normal = (has_effective_normal() && parabola->get_respect_effective_normal()) ? get_effective_normal() : get_normal(); 00887 00888 new_entry->set_surface_normal(normal); 00889 new_entry->set_surface_point(plane_point); 00890 00891 return new_entry; 00892 } 00893 00894 //////////////////////////////////////////////////////////////////// 00895 // Function: CollisionPolygon::fill_viz_geom 00896 // Access: Protected, Virtual 00897 // Description: Fills the _viz_geom GeomNode up with Geoms suitable 00898 // for rendering this solid. 00899 //////////////////////////////////////////////////////////////////// 00900 void CollisionPolygon:: 00901 fill_viz_geom() { 00902 if (collide_cat.is_debug()) { 00903 collide_cat.debug() 00904 << "Recomputing viz for " << *this << "\n"; 00905 } 00906 nassertv(_viz_geom != (GeomNode *)NULL && _bounds_viz_geom != (GeomNode *)NULL); 00907 draw_polygon(_viz_geom, _bounds_viz_geom, _points); 00908 } 00909 00910 //////////////////////////////////////////////////////////////////// 00911 // Function: CollisionPolygon::dist_to_line_segment 00912 // Access: Private, Static 00913 // Description: Returns the linear distance of p to the line segment 00914 // defined by f and t, where v = (t - f).normalize(). 00915 // The result is negative if p is left of the line, 00916 // positive if it is right of the line. If the result 00917 // is positive, it is constrained by endpoints of the 00918 // line segment (i.e. the result might be larger than it 00919 // would be for a straight distance-to-line test). If 00920 // the result is negative, we don't bother. 00921 //////////////////////////////////////////////////////////////////// 00922 PN_stdfloat CollisionPolygon:: 00923 dist_to_line_segment(const LPoint2 &p, 00924 const LPoint2 &f, const LPoint2 &t, 00925 const LVector2 &v) { 00926 LVector2 v1 = (p - f); 00927 PN_stdfloat d = (v1[0] * v[1] - v1[1] * v[0]); 00928 if (d < 0.0f) { 00929 return d; 00930 } 00931 00932 // Compute the nearest point on the line. 00933 LPoint2 q = p + LVector2(-v[1], v[0]) * d; 00934 00935 // Now constrain that point to the line segment. 00936 if (v[0] > 0.0f) { 00937 // X+ 00938 if (v[1] > 0.0f) { 00939 // Y+ 00940 if (v[0] > v[1]) { 00941 // X-dominant. 00942 if (q[0] < f[0]) { 00943 return (p - f).length(); 00944 } if (q[0] > t[0]) { 00945 return (p - t).length(); 00946 } else { 00947 return d; 00948 } 00949 } else { 00950 // Y-dominant. 00951 if (q[1] < f[1]) { 00952 return (p - f).length(); 00953 } if (q[1] > t[1]) { 00954 return (p - t).length(); 00955 } else { 00956 return d; 00957 } 00958 } 00959 } else { 00960 // Y- 00961 if (v[0] > -v[1]) { 00962 // X-dominant. 00963 if (q[0] < f[0]) { 00964 return (p - f).length(); 00965 } if (q[0] > t[0]) { 00966 return (p - t).length(); 00967 } else { 00968 return d; 00969 } 00970 } else { 00971 // Y-dominant. 00972 if (q[1] > f[1]) { 00973 return (p - f).length(); 00974 } if (q[1] < t[1]) { 00975 return (p - t).length(); 00976 } else { 00977 return d; 00978 } 00979 } 00980 } 00981 } else { 00982 // X- 00983 if (v[1] > 0.0f) { 00984 // Y+ 00985 if (-v[0] > v[1]) { 00986 // X-dominant. 00987 if (q[0] > f[0]) { 00988 return (p - f).length(); 00989 } if (q[0] < t[0]) { 00990 return (p - t).length(); 00991 } else { 00992 return d; 00993 } 00994 } else { 00995 // Y-dominant. 00996 if (q[1] < f[1]) { 00997 return (p - f).length(); 00998 } if (q[1] > t[1]) { 00999 return (p - t).length(); 01000 } else { 01001 return d; 01002 } 01003 } 01004 } else { 01005 // Y- 01006 if (-v[0] > -v[1]) { 01007 // X-dominant. 01008 if (q[0] > f[0]) { 01009 return (p - f).length(); 01010 } if (q[0] < t[0]) { 01011 return (p - t).length(); 01012 } else { 01013 return d; 01014 } 01015 } else { 01016 // Y-dominant. 01017 if (q[1] > f[1]) { 01018 return (p - f).length(); 01019 } if (q[1] < t[1]) { 01020 return (p - t).length(); 01021 } else { 01022 return d; 01023 } 01024 } 01025 } 01026 } 01027 } 01028 01029 01030 //////////////////////////////////////////////////////////////////// 01031 // Function: CollisionPolygon::compute_vectors 01032 // Access: Private, Static 01033 // Description: Now that the _p members of the given points array 01034 // have been computed, go back and compute all of the _v 01035 // members. 01036 //////////////////////////////////////////////////////////////////// 01037 void CollisionPolygon:: 01038 compute_vectors(Points &points) { 01039 size_t num_points = points.size(); 01040 for (size_t i = 0; i < num_points; i++) { 01041 points[i]._v = points[(i + 1) % num_points]._p - points[i]._p; 01042 points[i]._v.normalize(); 01043 } 01044 } 01045 01046 //////////////////////////////////////////////////////////////////// 01047 // Function: CollisionPolygon::draw_polygon 01048 // Access: Private 01049 // Description: Fills up the indicated GeomNode with the Geoms to 01050 // draw the polygon indicated with the given set of 2-d 01051 // points. 01052 //////////////////////////////////////////////////////////////////// 01053 void CollisionPolygon:: 01054 draw_polygon(GeomNode *viz_geom_node, GeomNode *bounds_viz_geom_node, 01055 const CollisionPolygon::Points &points) const { 01056 if (points.size() < 3) { 01057 if (collide_cat.is_debug()) { 01058 collide_cat.debug() 01059 << "(Degenerate poly, ignoring.)\n"; 01060 } 01061 return; 01062 } 01063 01064 LMatrix4 to_3d_mat; 01065 rederive_to_3d_mat(to_3d_mat); 01066 01067 PT(GeomVertexData) vdata = new GeomVertexData 01068 ("collision", GeomVertexFormat::get_v3(), 01069 Geom::UH_static); 01070 GeomVertexWriter vertex(vdata, InternalName::get_vertex()); 01071 01072 Points::const_iterator pi; 01073 for (pi = points.begin(); pi != points.end(); ++pi) { 01074 vertex.add_data3(to_3d((*pi)._p, to_3d_mat)); 01075 } 01076 01077 PT(GeomTrifans) body = new GeomTrifans(Geom::UH_static); 01078 body->add_consecutive_vertices(0, points.size()); 01079 body->close_primitive(); 01080 01081 PT(GeomLinestrips) border = new GeomLinestrips(Geom::UH_static); 01082 border->add_consecutive_vertices(0, points.size()); 01083 border->add_vertex(0); 01084 border->close_primitive(); 01085 01086 PT(Geom) geom1 = new Geom(vdata); 01087 geom1->add_primitive(body); 01088 01089 PT(Geom) geom2 = new Geom(vdata); 01090 geom2->add_primitive(border); 01091 01092 viz_geom_node->add_geom(geom1, ((CollisionPolygon *)this)->get_solid_viz_state()); 01093 viz_geom_node->add_geom(geom2, ((CollisionPolygon *)this)->get_wireframe_viz_state()); 01094 01095 bounds_viz_geom_node->add_geom(geom1, ((CollisionPolygon *)this)->get_solid_bounds_viz_state()); 01096 bounds_viz_geom_node->add_geom(geom2, ((CollisionPolygon *)this)->get_wireframe_bounds_viz_state()); 01097 } 01098 01099 01100 //////////////////////////////////////////////////////////////////// 01101 // Function: CollisionPolygon::point_is_inside 01102 // Access: Private 01103 // Description: Returns true if the indicated point is within the 01104 // polygon's 2-d space, false otherwise. 01105 //////////////////////////////////////////////////////////////////// 01106 bool CollisionPolygon:: 01107 point_is_inside(const LPoint2 &p, const CollisionPolygon::Points &points) const { 01108 // We insist that the polygon be convex. This makes things a bit simpler. 01109 01110 // In the case of a convex polygon, defined with points in counterclockwise 01111 // order, a point is interior to the polygon iff the point is not right of 01112 // each of the edges. 01113 for (int i = 0; i < (int)points.size() - 1; i++) { 01114 if (is_right(p - points[i]._p, points[i+1]._p - points[i]._p)) { 01115 return false; 01116 } 01117 } 01118 if (is_right(p - points[points.size() - 1]._p, 01119 points[0]._p - points[points.size() - 1]._p)) { 01120 return false; 01121 } 01122 01123 return true; 01124 } 01125 01126 //////////////////////////////////////////////////////////////////// 01127 // Function: CollisionPolygon::dist_to_polygon 01128 // Access: Private 01129 // Description: Returns the linear distance from the 2-d point to the 01130 // nearest part of the polygon defined by the points 01131 // vector. The result is negative if the point is 01132 // within the polygon. 01133 //////////////////////////////////////////////////////////////////// 01134 PN_stdfloat CollisionPolygon:: 01135 dist_to_polygon(const LPoint2 &p, const CollisionPolygon::Points &points) const { 01136 01137 // We know that that the polygon is convex and is defined with the 01138 // points in counterclockwise order. Therefore, we simply compare 01139 // the signed distance to each line segment; we ignore any negative 01140 // values, and take the minimum of all the positive values. 01141 01142 // If all values are negative, the point is within the polygon; we 01143 // therefore return an arbitrary negative result. 01144 01145 bool got_dist = false; 01146 PN_stdfloat best_dist = -1.0f; 01147 01148 size_t num_points = points.size(); 01149 for (size_t i = 0; i < num_points - 1; ++i) { 01150 PN_stdfloat d = dist_to_line_segment(p, points[i]._p, points[i + 1]._p, 01151 points[i]._v); 01152 if (d >= 0.0f) { 01153 if (!got_dist || d < best_dist) { 01154 best_dist = d; 01155 got_dist = true; 01156 } 01157 } 01158 } 01159 01160 PN_stdfloat d = dist_to_line_segment(p, points[num_points - 1]._p, points[0]._p, 01161 points[num_points - 1]._v); 01162 if (d >= 0.0f) { 01163 if (!got_dist || d < best_dist) { 01164 best_dist = d; 01165 got_dist = true; 01166 } 01167 } 01168 01169 return best_dist; 01170 } 01171 01172 //////////////////////////////////////////////////////////////////// 01173 // Function: CollisionPolygon::setup_points 01174 // Access: Private 01175 // Description: 01176 //////////////////////////////////////////////////////////////////// 01177 void CollisionPolygon:: 01178 setup_points(const LPoint3 *begin, const LPoint3 *end) { 01179 int num_points = end - begin; 01180 nassertv(num_points >= 3); 01181 01182 _points.clear(); 01183 01184 // Tell the base CollisionPlane class what its plane will be. To do 01185 // this, we must first compute the polygon normal. 01186 LVector3 normal = LVector3::zero(); 01187 01188 // Project the polygon into each of the three major planes and 01189 // calculate the area of each 2-d projection. This becomes the 01190 // polygon normal. This works because the ratio between these 01191 // different areas corresponds to the angle at which the polygon is 01192 // tilted toward each plane. 01193 for (int i = 0; i < num_points; i++) { 01194 const LPoint3 &p0 = begin[i]; 01195 const LPoint3 &p1 = begin[(i + 1) % num_points]; 01196 normal[0] += p0[1] * p1[2] - p0[2] * p1[1]; 01197 normal[1] += p0[2] * p1[0] - p0[0] * p1[2]; 01198 normal[2] += p0[0] * p1[1] - p0[1] * p1[0]; 01199 } 01200 01201 if (normal.length_squared() == 0.0f) { 01202 // The polygon has no area. 01203 return; 01204 } 01205 01206 #ifndef NDEBUG 01207 // Make sure all the source points are good. 01208 { 01209 if (!verify_points(begin, end)) { 01210 collide_cat.error() << "Invalid points in CollisionPolygon:\n"; 01211 const LPoint3 *pi; 01212 for (pi = begin; pi != end; ++pi) { 01213 collide_cat.error(false) << " " << (*pi) << "\n"; 01214 } 01215 collide_cat.error(false) 01216 << " normal " << normal << " with length " << normal.length() << "\n"; 01217 01218 return; 01219 } 01220 } 01221 01222 if (collide_cat.is_spam()) { 01223 collide_cat.spam() 01224 << "CollisionPolygon defined with " << num_points << " vertices:\n"; 01225 const LPoint3 *pi; 01226 for (pi = begin; pi != end; ++pi) { 01227 collide_cat.spam(false) << " " << (*pi) << "\n"; 01228 } 01229 } 01230 #endif 01231 01232 set_plane(LPlane(normal, begin[0])); 01233 01234 // Construct a matrix that rotates the points from the (X,0,Z) plane 01235 // into the 3-d plane. 01236 LMatrix4 to_3d_mat; 01237 calc_to_3d_mat(to_3d_mat); 01238 01239 // And the inverse matrix rotates points from 3-d space into the 2-d 01240 // plane. 01241 _to_2d_mat.invert_from(to_3d_mat); 01242 01243 // Now project all of the points onto the 2-d plane. 01244 01245 const LPoint3 *pi; 01246 for (pi = begin; pi != end; ++pi) { 01247 LPoint3 point = (*pi) * _to_2d_mat; 01248 _points.push_back(PointDef(point[0], point[2])); 01249 } 01250 01251 nassertv(_points.size() >= 3); 01252 01253 #ifndef NDEBUG 01254 /* 01255 // Now make sure the points define a convex polygon. 01256 if (is_concave()) { 01257 collide_cat.error() << "Invalid concave CollisionPolygon defined:\n"; 01258 const LPoint3 *pi; 01259 for (pi = begin; pi != end; ++pi) { 01260 collide_cat.error(false) << " " << (*pi) << "\n"; 01261 } 01262 collide_cat.error(false) 01263 << " normal " << normal << " with length " << normal.length() << "\n"; 01264 _points.clear(); 01265 } 01266 */ 01267 #endif 01268 01269 compute_vectors(_points); 01270 } 01271 01272 //////////////////////////////////////////////////////////////////// 01273 // Function: CollisionPolygon::legacy_to_3d 01274 // Access: Private 01275 // Description: Converts the indicated point to 3-d space according 01276 // to the way CollisionPolygons used to be stored in bam 01277 // files prior to 4.9. 01278 //////////////////////////////////////////////////////////////////// 01279 LPoint3 CollisionPolygon:: 01280 legacy_to_3d(const LVecBase2 &point2d, int axis) const { 01281 nassertr(!point2d.is_nan(), LPoint3(0.0f, 0.0f, 0.0f)); 01282 01283 LVector3 normal = get_normal(); 01284 PN_stdfloat D = get_plane()[3]; 01285 01286 nassertr(!normal.is_nan(), LPoint3(0.0f, 0.0f, 0.0f)); 01287 nassertr(!cnan(D), LPoint3(0.0f, 0.0f, 0.0f)); 01288 01289 switch (axis) { 01290 case 0: // AT_x: 01291 return LPoint3(-(normal[1]*point2d[0] + normal[2]*point2d[1] + D)/normal[0], point2d[0], point2d[1]); 01292 01293 case 1: // AT_y: 01294 return LPoint3(point2d[0], 01295 -(normal[0]*point2d[0] + normal[2]*point2d[1] + D)/normal[1], point2d[1]); 01296 01297 case 2: // AT_z: 01298 return LPoint3(point2d[0], point2d[1], 01299 -(normal[0]*point2d[0] + normal[1]*point2d[1] + D)/normal[2]); 01300 } 01301 01302 nassertr(false, LPoint3(0.0f, 0.0f, 0.0f)); 01303 return LPoint3(0.0f, 0.0f, 0.0f); 01304 } 01305 01306 //////////////////////////////////////////////////////////////////// 01307 // Function: CollisionPolygon::clip_polygon 01308 // Access: Private 01309 // Description: Clips the source_points of the polygon by the 01310 // indicated clipping plane, and modifies new_points to 01311 // reflect the new set of clipped points (but does not 01312 // compute the vectors in new_points). 01313 // 01314 // The return value is true if the set of points is 01315 // unmodified (all points are behind the clip plane), or 01316 // false otherwise. 01317 //////////////////////////////////////////////////////////////////// 01318 bool CollisionPolygon:: 01319 clip_polygon(CollisionPolygon::Points &new_points, 01320 const CollisionPolygon::Points &source_points, 01321 const LPlane &plane) const { 01322 new_points.clear(); 01323 if (source_points.empty()) { 01324 return true; 01325 } 01326 01327 LPoint3 from3d; 01328 LVector3 delta3d; 01329 if (!plane.intersects_plane(from3d, delta3d, get_plane())) { 01330 // The clipping plane is parallel to the polygon. The polygon is 01331 // either all in or all out. 01332 if (plane.dist_to_plane(get_plane().get_point()) < 0.0) { 01333 // A point within the polygon is behind the clipping plane: the 01334 // polygon is all in. 01335 new_points = source_points; 01336 return true; 01337 } 01338 return false; 01339 } 01340 01341 // Project the line of intersection into the 2-d plane. Now we have 01342 // a 2-d clipping line. 01343 LPoint2 from2d = to_2d(from3d); 01344 LVector2 delta2d = to_2d(delta3d); 01345 01346 PN_stdfloat a = -delta2d[1]; 01347 PN_stdfloat b = delta2d[0]; 01348 PN_stdfloat c = from2d[0] * delta2d[1] - from2d[1] * delta2d[0]; 01349 01350 // Now walk through the points. Any point on the left of our line 01351 // gets removed, and the line segment clipped at the point of 01352 // intersection. 01353 01354 // We might increase the number of vertices by as many as 1, if the 01355 // plane clips off exactly one corner. (We might also decrease the 01356 // number of vertices, or keep them the same number.) 01357 new_points.reserve(source_points.size() + 1); 01358 01359 LPoint2 last_point = source_points.back()._p; 01360 bool last_is_in = !is_right(last_point - from2d, delta2d); 01361 bool all_in = last_is_in; 01362 Points::const_iterator pi; 01363 for (pi = source_points.begin(); pi != source_points.end(); ++pi) { 01364 const LPoint2 &this_point = (*pi)._p; 01365 bool this_is_in = !is_right(this_point - from2d, delta2d); 01366 01367 // There appears to be a compiler bug in gcc 4.0: we need to 01368 // extract this comparison outside of the if statement. 01369 bool crossed_over = (this_is_in != last_is_in); 01370 if (crossed_over) { 01371 // We have just crossed over the clipping line. Find the point 01372 // of intersection. 01373 LVector2 d = this_point - last_point; 01374 PN_stdfloat denom = (a * d[0] + b * d[1]); 01375 if (denom != 0.0) { 01376 PN_stdfloat t = -(a * last_point[0] + b * last_point[1] + c) / denom; 01377 LPoint2 p = last_point + t * d; 01378 01379 new_points.push_back(PointDef(p[0], p[1])); 01380 last_is_in = this_is_in; 01381 } 01382 } 01383 01384 if (this_is_in) { 01385 // We are behind the clipping line. Keep the point. 01386 new_points.push_back(PointDef(this_point[0], this_point[1])); 01387 } else { 01388 all_in = false; 01389 } 01390 01391 last_point = this_point; 01392 } 01393 01394 return all_in; 01395 } 01396 01397 //////////////////////////////////////////////////////////////////// 01398 // Function: CollisionPolygon::apply_clip_plane 01399 // Access: Private 01400 // Description: Clips the polygon by all of the clip planes named in 01401 // the clip plane attribute and fills new_points up with 01402 // the resulting points. 01403 // 01404 // The return value is true if the set of points is 01405 // unmodified (all points are behind all the clip 01406 // planes), or false otherwise. 01407 //////////////////////////////////////////////////////////////////// 01408 bool CollisionPolygon:: 01409 apply_clip_plane(CollisionPolygon::Points &new_points, 01410 const ClipPlaneAttrib *cpa, 01411 const TransformState *net_transform) const { 01412 bool all_in = true; 01413 01414 int num_planes = cpa->get_num_on_planes(); 01415 bool first_plane = true; 01416 01417 for (int i = 0; i < num_planes; i++) { 01418 NodePath plane_path = cpa->get_on_plane(i); 01419 PlaneNode *plane_node = DCAST(PlaneNode, plane_path.node()); 01420 if ((plane_node->get_clip_effect() & PlaneNode::CE_collision) != 0) { 01421 CPT(TransformState) new_transform = 01422 net_transform->invert_compose(plane_path.get_net_transform()); 01423 01424 LPlane plane = plane_node->get_plane() * new_transform->get_mat(); 01425 if (first_plane) { 01426 first_plane = false; 01427 if (!clip_polygon(new_points, _points, plane)) { 01428 all_in = false; 01429 } 01430 } else { 01431 Points last_points; 01432 last_points.swap(new_points); 01433 if (!clip_polygon(new_points, last_points, plane)) { 01434 all_in = false; 01435 } 01436 } 01437 } 01438 } 01439 01440 if (!all_in) { 01441 compute_vectors(new_points); 01442 } 01443 01444 return all_in; 01445 } 01446 01447 //////////////////////////////////////////////////////////////////// 01448 // Function: CollisionPolygon::write_datagram 01449 // Access: Public 01450 // Description: Function to write the important information in 01451 // the particular object to a Datagram 01452 //////////////////////////////////////////////////////////////////// 01453 void CollisionPolygon:: 01454 write_datagram(BamWriter *manager, Datagram &me) { 01455 CollisionPlane::write_datagram(manager, me); 01456 me.add_uint16(_points.size()); 01457 for (size_t i = 0; i < _points.size(); i++) { 01458 _points[i]._p.write_datagram(me); 01459 _points[i]._v.write_datagram(me); 01460 } 01461 _to_2d_mat.write_datagram(me); 01462 } 01463 01464 //////////////////////////////////////////////////////////////////// 01465 // Function: CollisionPolygon::fillin 01466 // Access: Protected 01467 // Description: Function that reads out of the datagram (or asks 01468 // manager to read) all of the data that is needed to 01469 // re-create this object and stores it in the appropiate 01470 // place 01471 //////////////////////////////////////////////////////////////////// 01472 void CollisionPolygon:: 01473 fillin(DatagramIterator &scan, BamReader *manager) { 01474 CollisionPlane::fillin(scan, manager); 01475 01476 size_t size = scan.get_uint16(); 01477 for (size_t i = 0; i < size; i++) { 01478 LPoint2 p; 01479 LVector2 v; 01480 p.read_datagram(scan); 01481 v.read_datagram(scan); 01482 _points.push_back(PointDef(p, v)); 01483 } 01484 _to_2d_mat.read_datagram(scan); 01485 01486 if (manager->get_file_minor_ver() < 13) { 01487 // Before bam version 6.13, we were inadvertently storing 01488 // CollisionPolygon vertices clockwise, instead of 01489 // counter-clockwise. Correct that by re-projecting. 01490 if (_points.size() >= 3) { 01491 LMatrix4 to_3d_mat; 01492 rederive_to_3d_mat(to_3d_mat); 01493 01494 epvector<LPoint3> verts; 01495 verts.reserve(_points.size()); 01496 Points::const_iterator pi; 01497 for (pi = _points.begin(); pi != _points.end(); ++pi) { 01498 verts.push_back(to_3d((*pi)._p, to_3d_mat)); 01499 } 01500 01501 const LPoint3 *verts_begin = &verts[0]; 01502 const LPoint3 *verts_end = verts_begin + verts.size(); 01503 setup_points(verts_begin, verts_end); 01504 } 01505 } 01506 } 01507 01508 01509 //////////////////////////////////////////////////////////////////// 01510 // Function: CollisionPolygon::make_CollisionPolygon 01511 // Access: Protected 01512 // Description: Factory method to generate a CollisionPolygon object 01513 //////////////////////////////////////////////////////////////////// 01514 TypedWritable* CollisionPolygon:: 01515 make_CollisionPolygon(const FactoryParams ¶ms) { 01516 CollisionPolygon *me = new CollisionPolygon; 01517 DatagramIterator scan; 01518 BamReader *manager; 01519 01520 parse_params(params, scan, manager); 01521 me->fillin(scan, manager); 01522 return me; 01523 } 01524 01525 //////////////////////////////////////////////////////////////////// 01526 // Function: CollisionPolygon::register_with_factory 01527 // Access: Public, Static 01528 // Description: Factory method to generate a CollisionPolygon object 01529 //////////////////////////////////////////////////////////////////// 01530 void CollisionPolygon:: 01531 register_with_read_factory() { 01532 BamReader::get_factory()->register_factory(get_class_type(), make_CollisionPolygon); 01533 } 01534 01535