Panda3D
 All Classes Functions Variables Enumerations
collisionPolygon.cxx
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 &copy) :
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 &params) {
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 
 All Classes Functions Variables Enumerations