From 35f5cd7d67c8d7d85adaaca2a9b2a4dff9fc996d Mon Sep 17 00:00:00 2001 From: David Rose Date: Wed, 3 Dec 2003 01:59:43 +0000 Subject: [PATCH] robustify polygons to detect sphere collisions with the polygon edge --- panda/src/collide/collisionPolygon.I | 75 ++++ panda/src/collide/collisionPolygon.cxx | 475 +++++++++++-------------- panda/src/collide/collisionPolygon.h | 32 +- panda/src/collide/collisionSolid.I | 2 +- panda/src/collide/collisionSolid.cxx | 2 +- panda/src/collide/collisionSolid.h | 1 + panda/src/collide/config_collide.cxx | 6 + panda/src/collide/config_collide.h | 1 + panda/src/putil/bam.h | 3 +- 9 files changed, 309 insertions(+), 288 deletions(-) diff --git a/panda/src/collide/collisionPolygon.I b/panda/src/collide/collisionPolygon.I index fff6e9f673..1e39e5d14a 100644 --- a/panda/src/collide/collisionPolygon.I +++ b/panda/src/collide/collisionPolygon.I @@ -102,3 +102,78 @@ verify_points(const LPoint3f &a, const LPoint3f &b, array[3] = d; return verify_points(array, array + 4); } + +//////////////////////////////////////////////////////////////////// +// Function: CollisionPolygon::to_2d +// Access: Private +// Description: Assuming the indicated point in 3-d space lies within +// the polygon's plane, returns the corresponding point +// in the polygon's 2-d definition space. +//////////////////////////////////////////////////////////////////// +INLINE LPoint2f CollisionPolygon:: +to_2d(const LVecBase3f &point3d) const { + LPoint3f point = LPoint3f(point3d) * _to_2d_mat; + return LPoint2f(point[0], point[2]); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionPolygon::calc_to_3d_mat +// Access: Private +// Description: Fills the indicated matrix with the appropriate +// rotation transform to move points from the 2-d plane +// into the 3-d (X, 0, Z) plane. +//////////////////////////////////////////////////////////////////// +INLINE void CollisionPolygon:: +calc_to_3d_mat(LMatrix4f &to_3d_mat) const { + look_at(to_3d_mat, get_plane().get_normal()); + to_3d_mat.set_row(3, get_plane().get_point()); +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionPolygon::to_3d +// Access: Private, Static +// Description: Extrude the indicated point in the polygon's 2-d +// definition space back into 3-d coordinates. +//////////////////////////////////////////////////////////////////// +INLINE LPoint3f CollisionPolygon:: +to_3d(const LVecBase2f &point2d, const LMatrix4f &to_3d_mat) { + return LPoint3f(point2d[0], 0.0f, point2d[1]) * to_3d_mat; +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionPolygon::PointDef::Constructor +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE CollisionPolygon::PointDef:: +PointDef(const LPoint2f &p, const LVector2f &v) : _p(p), _v(v) { +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionPolygon::PointDef::Constructor +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE CollisionPolygon::PointDef:: +PointDef(float x, float y) : _p(x, y) { +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionPolygon::PointDef::Copy Constructor +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE CollisionPolygon::PointDef:: +PointDef(const CollisionPolygon::PointDef ©) : _p(copy._p), _v(copy._v) { +} + +//////////////////////////////////////////////////////////////////// +// Function: CollisionPolygon::PointDef::Copy Assignment Operator +// Access: Public +// Description: +//////////////////////////////////////////////////////////////////// +INLINE void CollisionPolygon::PointDef:: +operator = (const CollisionPolygon::PointDef ©) { + _p = copy._p; + _v = copy._v; +} diff --git a/panda/src/collide/collisionPolygon.cxx b/panda/src/collide/collisionPolygon.cxx index 217f291e1f..c8416a5f9c 100644 --- a/panda/src/collide/collisionPolygon.cxx +++ b/panda/src/collide/collisionPolygon.cxx @@ -53,6 +53,20 @@ is_right(const LVector2f &v1, const LVector2f &v2) { return (-v1[0] * v2[1] + v1[1] * v2[0]) > 0; } +//////////////////////////////////////////////////////////////////// +// Function: dist_to_line +// Description: Returns the linear distance of p to the line defined +// by f and f+v, where v is a normalized vector. The +// result is negative if p is left of the line, positive +// if it is right of the line. +//////////////////////////////////////////////////////////////////// +INLINE float +dist_to_line(const LPoint2f &p, + const LPoint2f &f, const LVector2f &v) { + LVector2f v1 = (p - f); + return (-v1[0] * v[1] + v1[1] * v[0]); +} + //////////////////////////////////////////////////////////////////// // Function: CollisionPolygon::Copy Constructor @@ -63,9 +77,7 @@ CollisionPolygon:: CollisionPolygon(const CollisionPolygon ©) : CollisionPlane(copy), _points(copy._points), - _median(copy._median), - _axis(copy._axis), - _reversed(copy._reversed) + _to_2d_mat(copy._to_2d_mat) { } @@ -161,12 +173,12 @@ is_concave() const { return true; } - LPoint2f p0 = _points[0]; - LPoint2f p1 = _points[1]; + LPoint2f p0 = _points[0]._p; + LPoint2f p1 = _points[1]._p; float dx1 = p1[0] - p0[0]; float dy1 = p1[1] - p0[1]; p0 = p1; - p1 = _points[2]; + p1 = _points[2]._p; float dx2 = p1[0] - p0[0]; float dy2 = p1[1] - p0[1]; @@ -174,7 +186,7 @@ is_concave() const { for (size_t i = 0; i < _points.size() - 1; i++) { p0 = p1; - p1 = _points[(i+3) % _points.size()]; + p1 = _points[(i+3) % _points.size()]._p; dx1 = dx2; dy1 = dy2; @@ -214,13 +226,13 @@ xform(const LMatrix4f &mat) { } if (!_points.empty()) { + LMatrix4f to_3d_mat; + calc_to_3d_mat(to_3d_mat); + pvector verts; Points::const_iterator pi; for (pi = _points.begin(); pi != _points.end(); ++pi) { - verts.push_back(to_3d(*pi) * mat); - } - if (_reversed) { - reverse(verts.begin(), verts.end()); + verts.push_back(to_3d((*pi)._p, to_3d_mat) * mat); } const LPoint3f *verts_begin = &verts[0]; @@ -241,7 +253,16 @@ xform(const LMatrix4f &mat) { //////////////////////////////////////////////////////////////////// LPoint3f CollisionPolygon:: get_collision_origin() const { - return to_3d(_median); + LMatrix4f to_3d_mat; + calc_to_3d_mat(to_3d_mat); + + LPoint2f median = _points[0]._p; + for (int n = 1; n < (int)_points.size(); n++) { + median += _points[n]._p; + } + median /= _points.size(); + + return to_3d(median, to_3d_mat); } //////////////////////////////////////////////////////////////////// @@ -301,25 +322,7 @@ get_viz(const CullTraverserData &data) const { void CollisionPolygon:: output(ostream &out) const { out << "cpolygon, (" << get_plane() - << "), "; - switch (_axis) { - case AT_x: - out << "x-aligned, "; - break; - - case AT_y: - out << "y-aligned, "; - break; - - case AT_z: - out << "z-aligned, "; - break; - } - - out << _points.size() << " vertices"; - if (_reversed){ - out << " (reversed)"; - } + << "), " << _points.size() << " vertices"; } //////////////////////////////////////////////////////////////////// @@ -332,17 +335,17 @@ write(ostream &out, int indent_level) const { indent(out, indent_level) << (*this) << "\n"; Points::const_iterator pi; for (pi = _points.begin(); pi != _points.end(); ++pi) { - indent(out, indent_level + 2) << (*pi) << "\n"; + indent(out, indent_level + 2) << (*pi)._p << "\n"; } + LMatrix4f to_3d_mat; + calc_to_3d_mat(to_3d_mat); out << "In 3-d space:\n"; PTA_Vertexf verts; for (pi = _points.begin(); pi != _points.end(); ++pi) { - verts.push_back(to_3d(*pi)); - } - if (_reversed) { - reverse(verts.begin(), verts.end()); + verts.push_back(to_3d((*pi)._p, to_3d_mat)); } + PTA_Vertexf::const_iterator vi; for (vi = verts.begin(); vi != verts.end(); ++vi) { indent(out, indent_level + 2) << (*vi) << "\n"; @@ -364,10 +367,12 @@ recompute_bound() { // Now actually compute the bounding volume by putting it around all // of our vertices. + LMatrix4f to_3d_mat; + calc_to_3d_mat(to_3d_mat); pvector vertices; Points::const_iterator pi; for (pi = _points.begin(); pi != _points.end(); ++pi) { - vertices.push_back(to_3d(*pi)); + vertices.push_back(to_3d((*pi)._p, to_3d_mat)); } const LPoint3f *vertices_begin = &vertices[0]; @@ -443,7 +448,8 @@ test_intersection_from_sphere(const CollisionEntry &entry) const { LVector3f from_radius_v = LVector3f(sphere->get_radius(), 0.0f, 0.0f) * wrt_mat; - float from_radius = length(from_radius_v); + float from_radius_2 = from_radius_v.length_squared(); + float from_radius = csqrt(from_radius_2); LVector3f normal = has_effective_normal() ? get_effective_normal() : get_normal(); #ifndef NDEBUG @@ -457,9 +463,9 @@ test_intersection_from_sphere(const CollisionEntry &entry) const { #endif // The nearest point within the plane to our center is the - // intersection of the line (center, center+normal) with the plane. + // intersection of the line (center, center - normal) with the plane. float dist; - if (!get_plane().intersects_line(dist, from_center, -normal)) { + if (!get_plane().intersects_line(dist, from_center, -get_normal())) { // No intersection with plane? This means the plane's effective // normal was within the plane itself. A useless polygon. return NULL; @@ -470,7 +476,8 @@ test_intersection_from_sphere(const CollisionEntry &entry) const { return NULL; } - LPoint2f p = to_2d(from_center + dist * normal); + LPoint2f p = to_2d(from_center - dist * get_normal()); + float edge_dist; const ClipPlaneAttrib *cpa = entry.get_into_clip_planes(); if (cpa != (ClipPlaneAttrib *)NULL) { @@ -479,9 +486,7 @@ test_intersection_from_sphere(const CollisionEntry &entry) const { if (apply_clip_plane(new_points, cpa, entry.get_into_node_path().get_net_transform())) { // All points are behind the clip plane; just do the default // test. - if (!circle_is_inside(p, from_radius, _points, _median)) { - return NULL; - } + edge_dist = dist_to_polygon(p, _points); } else if (new_points.empty()) { // The polygon is completely clipped. @@ -489,21 +494,38 @@ test_intersection_from_sphere(const CollisionEntry &entry) const { } else { // Test against the clipped polygon. - LPoint2f new_median = new_points[0]; - for (int n = 1; n < (int)new_points.size(); n++) { - new_median += new_points[n]; - } - new_median /= new_points.size(); - if (!circle_is_inside(p, from_radius, new_points, new_median)) { - return NULL; - } + edge_dist = dist_to_polygon(p, new_points); } } else { - // No clip plane is in effect. Do the default test. - if (!circle_is_inside(p, from_radius, _points, _median)) { - return NULL; - } + // No clip plane is in effect. Do the default test. + edge_dist = dist_to_polygon(p, _points); + } + + // Now we have edge_dist, which is the distance from the sphere + // center to the nearest edge of the polygon, within the polygon's + // plane. + + if (edge_dist > from_radius) { + // No intersection; the circle is outside the polygon. + return NULL; + } + + // The sphere appears to intersect the polygon. If the edge is less + // than from_radius away, the sphere may be resting on an edge of + // the polygon. Determine how far the center of the sphere must + // remain from the plane, based on its distance from the nearest + // edge. + + float max_dist = from_radius; + if (edge_dist >= 0.0f) { + float max_dist_2 = max(from_radius_2 - edge_dist * edge_dist, 0.0f); + max_dist = csqrt(max_dist_2); + } + + if (dist > max_dist) { + // There's no intersection: the sphere is hanging off the edge. + return NULL; } if (collide_cat.is_debug()) { @@ -513,14 +535,14 @@ test_intersection_from_sphere(const CollisionEntry &entry) const { } PT(CollisionEntry) new_entry = new CollisionEntry(entry); - float into_depth = from_radius - dist; + float into_depth = max_dist - dist; if (moved_from_center) { // We have to base the depth of intersection on the sphere's final // resting point, not the point from which we tested the // intersection. float orig_dist; get_plane().intersects_line(orig_dist, orig_center, -normal); - into_depth = from_radius - orig_dist; + into_depth = max_dist - orig_dist; } new_entry->set_surface_normal(normal); @@ -681,6 +703,22 @@ fill_viz_geom() { draw_polygon(_viz_geom, _points); } +//////////////////////////////////////////////////////////////////// +// Function: CollisionPolygon::compute_vectors +// Access: Private, Static +// Description: Now that the _p members of the given points array +// have been computed, go back and compute all of the _v +// members. +//////////////////////////////////////////////////////////////////// +void CollisionPolygon:: +compute_vectors(Points &points) { + size_t num_points = points.size(); + for (size_t i = 0; i < num_points; i++) { + points[i]._v = points[(i + 1) % num_points]._p - points[i]._p; + points[i]._v.normalize(); + } +} + //////////////////////////////////////////////////////////////////// // Function: CollisionPolygon::draw_polygon // Access: Private @@ -698,13 +736,12 @@ draw_polygon(GeomNode *geom_node, const CollisionPolygon::Points &points) const return; } + LMatrix4f to_3d_mat; + calc_to_3d_mat(to_3d_mat); PTA_Vertexf verts; Points::const_iterator pi; for (pi = points.begin(); pi != points.end(); ++pi) { - verts.push_back(to_3d(*pi)); - } - if (_reversed) { - reverse(verts.begin(), verts.end()); + verts.push_back(to_3d((*pi)._p, to_3d_mat)); } PTA_int lengths; @@ -734,12 +771,12 @@ point_is_inside(const LPoint2f &p, const CollisionPolygon::Points &points) const // order, a point is interior to the polygon iff the point is not right of // each of the edges. for (int i = 0; i < (int)points.size() - 1; i++) { - if (is_right(p - points[i], points[i+1] - points[i])) { + if (is_right(p - points[i]._p, points[i+1]._p - points[i]._p)) { return false; } } - if (is_right(p - points[points.size() - 1], - points[0] - points[points.size() - 1])) { + if (is_right(p - points[points.size() - 1]._p, + points[0]._p - points[points.size() - 1]._p)) { return false; } @@ -747,70 +784,30 @@ point_is_inside(const LPoint2f &p, const CollisionPolygon::Points &points) const } //////////////////////////////////////////////////////////////////// -// Function: CollisionPolygon::circle_is_inside +// Function: CollisionPolygon::dist_to_polygon // Access: Private -// Description: Returns true if the circle with the indicated center -// and radius intersects the polygon in its 2-d space, -// false otherwise. +// Description: Returns the linear distance from the 2-d point to the +// nearest part of the polygon defined by the points +// vector. The result is negative if the point is +// within the polygon. //////////////////////////////////////////////////////////////////// -bool CollisionPolygon:: -circle_is_inside(const LPoint2f ¢er, float radius, - const CollisionPolygon::Points &points, - const LPoint2f &median) const { - // Now we have found a point on the polygon's plane that corresponds - // to the point tangent to our collision sphere where it first - // touches the plane. We want to decide whether the sphere itself - // will intersect the polygon. We can approximate this by testing - // whether a circle of the given radius centered around this tangent - // point, in the plane of the polygon, would intersect. +float CollisionPolygon:: +dist_to_polygon(const LPoint2f &p, const CollisionPolygon::Points &points) const { - // But even this approximate test is too difficult. To approximate - // the approximation, we'll test two points: (1) the center itself. - // If this is inside the polygon, then certainly the circle - // intersects the polygon, and the sphere collides. (2) a point on - // the outside of the circle, nearest to the center of the polygon. - // If _this_ point is inside the polygon, then again the circle, and - // hence the sphere, intersects. If neither point is inside the - // polygon, chances are reasonably good the sphere doesn't intersect - // the polygon after all. + // We know that that the polygon is convex and is defined with the + // points in counterclockwise order. Therefore, we simply compare + // the signed distance to each line; the answer is the maximum of + // these. (This doesn't quite get the right answer if the closest + // part of the polygon is one of the vertices, but it's close enough + // for these purposes.) - if (point_is_inside(center, points)) { - // The circle's center is inside the polygon; we have a collision! + float max_dist = dist_to_line(p, points.front()._p, points.front()._v); - } else { - - if (radius > 0.0f) { - // Now find the point on the rim of the circle nearest the - // polygon's center. - - // First, get a vector from the center of the circle to the center - // of the polygon. - LVector2f rim = median - center; - float rim_length = length(rim); - - if (rim_length <= radius) { - // Here's a surprise: the center of the polygon is within the - // circle! Since the center is guaranteed to be interior to the - // polygon (the polygon is convex), it follows that the circle - // intersects the polygon. - - } else { - // Now scale this vector to length radius, and get the new point. - rim = (rim * radius / rim_length) + center; - - // Is the new point within the polygon? - if (point_is_inside(rim, points)) { - // It sure is! The circle intersects! - - } else { - // No intersection. - return false; - } - } - } + for (size_t i = 1; i < points.size(); i++) { + max_dist = max(max_dist, dist_to_line(p, points[i]._p, points[i]._v)); } - - return true; + + return max_dist; } //////////////////////////////////////////////////////////////////// @@ -875,54 +872,21 @@ setup_points(const LPoint3f *begin, const LPoint3f *end) { set_plane(Planef(normal, begin[0])); - // Now determine the largest of |normal[0]|, |normal[1]|, and - // |normal[2]|. This will tell us which axis-aligned plane the - // polygon is most nearly aligned with, and therefore which plane we - // should project onto for determining interiorness of the - // intersection point. + // Construct a matrix that rotates the points from the (X,0,Z) plane + // into the 3-d plane. + LMatrix4f to_3d_mat; + calc_to_3d_mat(to_3d_mat); - if (fabs(normal[0]) >= fabs(normal[1])) { - if (fabs(normal[0]) >= fabs(normal[2])) { - _reversed = (normal[0] > 0.0f); - _axis = AT_x; - } else { - _reversed = (normal[2] > 0.0f); - _axis = AT_z; - } - } else { - if (fabs(normal[1]) >= fabs(normal[2])) { - _reversed = (normal[1] < 0.0f); - _axis = AT_y; - } else { - _reversed = (normal[2] > 0.0f); - _axis = AT_z; - } - } + // And the inverse matrix rotates points from 3-d space into the 2-d + // plane. + _to_2d_mat.invert_from(to_3d_mat); // Now project all of the points onto the 2-d plane. const LPoint3f *pi; - switch (_axis) { - case AT_x: - for (pi = begin; pi != end; ++pi) { - const LPoint3f &point = (*pi); - _points.push_back(LPoint2f(point[1], point[2])); - } - break; - - case AT_y: - for (pi = begin; pi != end; ++pi) { - const LPoint3f &point = (*pi); - _points.push_back(LPoint2f(point[0], point[2])); - } - break; - - case AT_z: - for (pi = begin; pi != end; ++pi) { - const LPoint3f &point = (*pi); - _points.push_back(LPoint2f(point[0], point[1])); - } - break; + for (pi = begin; pi != end; ++pi) { + LPoint3f point = (*pi) * _to_2d_mat; + _points.push_back(PointDef(point[0], point[2])); } nassertv(_points.size() >= 3); @@ -943,73 +907,18 @@ setup_points(const LPoint3f *begin, const LPoint3f *end) { */ #endif - // Now average up all the points to get the median. This is the - // geometric center of the polygon, and (since the polygon must be - // convex) is also a point within the polygon. - _median = _points[0]; - for (int n = 1; n < (int)_points.size(); n++) { - _median += _points[n]; - } - _median /= _points.size(); - - // One final complication: In projecting the polygon onto the plane, - // we might have lost its counterclockwise-vertex orientation. If - // this is the case, we must reverse the order of the vertices. - if (_reversed) { - reverse(_points.begin(), _points.end()); - } - - /* -#ifndef NDEBUG - bool still_reversed = - is_right(_points[2] - _points[0], _points[1] - _points[0]); - if (still_reversed) { - collide_cat.warning() - << "Invalid poly:\n"; - write(collide_cat.warning(false), 2); - } - nassertv(!still_reversed); -#endif - */ + compute_vectors(_points); } //////////////////////////////////////////////////////////////////// -// Function: CollisionPolygon::to_2d +// Function: CollisionPolygon::legacy_to_3d // Access: Private -// Description: Assuming the indicated point in 3-d space lies within -// the polygon's plane, returns the corresponding point -// in the polygon's 2-d definition space. -//////////////////////////////////////////////////////////////////// -LPoint2f CollisionPolygon:: -to_2d(const LVecBase3f &point3d) const { - nassertr(!point3d.is_nan(), LPoint2f(0.0f, 0.0f)); - - // Project the point of intersection with the plane onto the - // axis-aligned plane we projected the polygon onto, and see if the - // point is interior to the polygon. - switch (_axis) { - case AT_x: - return LPoint2f(point3d[1], point3d[2]); - - case AT_y: - return LPoint2f(point3d[0], point3d[2]); - - case AT_z: - return LPoint2f(point3d[0], point3d[1]); - } - - nassertr(false, LPoint2f(0.0f, 0.0f)); - return LPoint2f(0.0f, 0.0f); -} - -//////////////////////////////////////////////////////////////////// -// Function: CollisionPolygon::to_3d -// Access: Private -// Description: Extrude the indicated point in the polygon's 2-d -// definition space back into 3-d coordinates. +// Description: Converts the indicated point to 3-d space according +// to the way CollisionPolygons used to be stored in bam +// files prior to 4.9. //////////////////////////////////////////////////////////////////// LPoint3f CollisionPolygon:: -to_3d(const LVecBase2f &point2d) const { +legacy_to_3d(const LVecBase2f &point2d, int axis) const { nassertr(!point2d.is_nan(), LPoint3f(0.0f, 0.0f, 0.0f)); LVector3f normal = get_normal(); @@ -1018,17 +927,15 @@ to_3d(const LVecBase2f &point2d) const { nassertr(!normal.is_nan(), LPoint3f(0.0f, 0.0f, 0.0f)); nassertr(!cnan(D), LPoint3f(0.0f, 0.0f, 0.0f)); - switch (_axis) { - case AT_x: - return LPoint3f(-(normal[1]*point2d[0] + normal[2]*point2d[1] + D)/normal[0], - point2d[0], point2d[1]); + switch (axis) { + case 0: // AT_x: + return LPoint3f(-(normal[1]*point2d[0] + normal[2]*point2d[1] + D)/normal[0], point2d[0], point2d[1]); - case AT_y: + case 1: // AT_y: return LPoint3f(point2d[0], - -(normal[0]*point2d[0] + normal[2]*point2d[1] + D)/normal[1], - point2d[1]); + -(normal[0]*point2d[0] + normal[2]*point2d[1] + D)/normal[1], point2d[1]); - case AT_z: + case 2: // AT_z: return LPoint3f(point2d[0], point2d[1], -(normal[0]*point2d[0] + normal[1]*point2d[1] + D)/normal[2]); } @@ -1042,7 +949,8 @@ to_3d(const LVecBase2f &point2d) const { // Access: Private // Description: Clips the source_points of the polygon by the // indicated clipping plane, and modifies new_points to -// reflect the new set of clipped points. +// reflect the new set of clipped points (but does not +// compute the vectors in new_points). // // The return value is true if the set of points is // unmodified (all points are behind the clip plane), or @@ -1075,9 +983,6 @@ clip_polygon(CollisionPolygon::Points &new_points, // a 2-d clipping line. LPoint2f from2d = to_2d(from3d); LVector2f delta2d = to_2d(delta3d); - if (_reversed) { - delta2d = -delta2d; - } float a = -delta2d[1]; float b = delta2d[0]; @@ -1092,12 +997,12 @@ clip_polygon(CollisionPolygon::Points &new_points, // number of vertices, or keep them the same number.) new_points.reserve(source_points.size() + 1); - LPoint2f last_point = source_points.back(); + LPoint2f last_point = source_points.back()._p; bool last_is_in = !is_right(last_point - from2d, delta2d); bool all_in = last_is_in; Points::const_iterator pi; for (pi = source_points.begin(); pi != source_points.end(); ++pi) { - const LPoint2f &this_point = (*pi); + const LPoint2f &this_point = (*pi)._p; bool this_is_in = !is_right(this_point - from2d, delta2d); if (this_is_in != last_is_in) { @@ -1107,13 +1012,13 @@ clip_polygon(CollisionPolygon::Points &new_points, float t = -(a * last_point[0] + b * last_point[1] + c) / (a * d[0] + b * d[1]); LPoint2f p = last_point + t * d; - new_points.push_back(p); + new_points.push_back(PointDef(p[0], p[1])); last_is_in = this_is_in; } if (this_is_in) { // We are behind the clipping line. Keep the point. - new_points.push_back(this_point); + new_points.push_back(PointDef(this_point[0], this_point[1])); } else { all_in = false; } @@ -1168,6 +1073,10 @@ apply_clip_plane(CollisionPolygon::Points &new_points, } } + if (!all_in) { + compute_vectors(new_points); + } + return all_in; } @@ -1178,19 +1087,14 @@ apply_clip_plane(CollisionPolygon::Points &new_points, // the particular object to a Datagram //////////////////////////////////////////////////////////////////// void CollisionPolygon:: -write_datagram(BamWriter *manager, Datagram &me) -{ - int i; - +write_datagram(BamWriter *manager, Datagram &me) { CollisionPlane::write_datagram(manager, me); me.add_uint16(_points.size()); - for(i = 0; i < (int)_points.size(); i++) - { - _points[i].write_datagram(me); + for (size_t i = 0; i < _points.size(); i++) { + _points[i]._p.write_datagram(me); + _points[i]._v.write_datagram(me); } - _median.write_datagram(me); - me.add_uint8(_axis); - me.add_uint8(_reversed); + _to_2d_mat.write_datagram(me); } //////////////////////////////////////////////////////////////////// @@ -1202,33 +1106,61 @@ write_datagram(BamWriter *manager, Datagram &me) // place //////////////////////////////////////////////////////////////////// void CollisionPolygon:: -fillin(DatagramIterator& scan, BamReader* manager) -{ - int i; - LPoint2f temp; +fillin(DatagramIterator &scan, BamReader *manager) { CollisionPlane::fillin(scan, manager); - int size = scan.get_uint16(); - for(i = 0; i < size; i++) - { - temp.read_datagram(scan); - _points.push_back(temp); - } - _median.read_datagram(scan); - _axis = (enum AxisType)scan.get_uint8(); + if (manager->get_file_minor_ver() < 9) { + // Decode old-style collision polygon. - // It seems that Windows wants this expression to prevent a - // 'performance warning'. Whatever. - _reversed = (scan.get_uint8() != 0); + Points points; + size_t size = scan.get_uint16(); + for (size_t i = 0; i < size; i++) { + LPoint2f p; + p.read_datagram(scan); + + points.push_back(PointDef(p[0], p[1])); + } + LPoint2f median; + median.read_datagram(scan); + + int axis = scan.get_uint8(); + bool reversed = (scan.get_uint8() != 0); + + // Now convert the above points into 3-d by the old-style rules. + pvector verts; + Points::const_iterator pi; + for (pi = points.begin(); pi != points.end(); ++pi) { + verts.push_back(legacy_to_3d((*pi)._p, axis)); + } + if (reversed) { + reverse(verts.begin(), verts.end()); + } + + const LPoint3f *verts_begin = &verts[0]; + const LPoint3f *verts_end = verts_begin + verts.size(); + setup_points(verts_begin, verts_end); + + } else { + // Load new-style collision polygon. + size_t size = scan.get_uint16(); + for (size_t i = 0; i < size; i++) { + LPoint2f p; + LVector2f v; + p.read_datagram(scan); + v.read_datagram(scan); + _points.push_back(PointDef(p, v)); + } + _to_2d_mat.read_datagram(scan); + } } + //////////////////////////////////////////////////////////////////// // Function: CollisionPolygon::make_CollisionPolygon // Access: Protected // Description: Factory method to generate a CollisionPolygon object //////////////////////////////////////////////////////////////////// TypedWritable* CollisionPolygon:: -make_CollisionPolygon(const FactoryParams ¶ms) -{ +make_CollisionPolygon(const FactoryParams ¶ms) { CollisionPolygon *me = new CollisionPolygon; DatagramIterator scan; BamReader *manager; @@ -1244,8 +1176,7 @@ make_CollisionPolygon(const FactoryParams ¶ms) // Description: Factory method to generate a CollisionPolygon object //////////////////////////////////////////////////////////////////// void CollisionPolygon:: -register_with_read_factory(void) -{ +register_with_read_factory() { BamReader::get_factory()->register_factory(get_class_type(), make_CollisionPolygon); } diff --git a/panda/src/collide/collisionPolygon.h b/panda/src/collide/collisionPolygon.h index 391ba7956e..ca698d64f0 100644 --- a/panda/src/collide/collisionPolygon.h +++ b/panda/src/collide/collisionPolygon.h @@ -23,6 +23,7 @@ #include "collisionPlane.h" #include "clipPlaneAttrib.h" +#include "look_at.h" #include "vector_LPoint2f.h" @@ -78,18 +79,29 @@ protected: virtual void fill_viz_geom(); private: - typedef vector_LPoint2f Points; + class PointDef { + public: + INLINE PointDef(const LPoint2f &p, const LVector2f &v); + INLINE PointDef(float x, float y); + INLINE PointDef(const PointDef ©); + INLINE void operator = (const PointDef ©); + LPoint2f _p; // the point in 2-d space + LVector2f _v; // the normalized vector to the next point + }; + typedef pvector Points; + + static void compute_vectors(Points &points); void draw_polygon(GeomNode *geom_node, const Points &points) const; bool point_is_inside(const LPoint2f &p, const Points &points) const; - bool circle_is_inside(const LPoint2f ¢er, float radius, - const CollisionPolygon::Points &points, - const LPoint2f &median) const; + float dist_to_polygon(const LPoint2f &p, const Points &points) const; void setup_points(const LPoint3f *begin, const LPoint3f *end); - LPoint2f to_2d(const LVecBase3f &point3d) const; - LPoint3f to_3d(const LVecBase2f &point2d) const; + INLINE LPoint2f to_2d(const LVecBase3f &point3d) const; + INLINE void calc_to_3d_mat(LMatrix4f &to_3d_mat) const; + INLINE static LPoint3f to_3d(const LVecBase2f &point2d, const LMatrix4f &to_3d_mat); + LPoint3f legacy_to_3d(const LVecBase2f &point2d, int axis) const; bool clip_polygon(Points &new_points, const Points &source_points, const Planef &plane) const; @@ -98,13 +110,7 @@ private: private: Points _points; - LPoint2f _median; - - enum AxisType { - AT_x, AT_y, AT_z - }; - AxisType _axis; - bool _reversed; + LMatrix4f _to_2d_mat; public: static void register_with_read_factory(void); diff --git a/panda/src/collide/collisionSolid.I b/panda/src/collide/collisionSolid.I index 0e8bf830b0..23ae96e9dc 100644 --- a/panda/src/collide/collisionSolid.I +++ b/panda/src/collide/collisionSolid.I @@ -88,7 +88,7 @@ clear_effective_normal() { //////////////////////////////////////////////////////////////////// INLINE bool CollisionSolid:: has_effective_normal() const { - return (_flags & F_effective_normal) != 0; + return respect_effective_normal && (_flags & F_effective_normal) != 0; } //////////////////////////////////////////////////////////////////// diff --git a/panda/src/collide/collisionSolid.cxx b/panda/src/collide/collisionSolid.cxx index a7c4e1b0ab..8a1e911f92 100644 --- a/panda/src/collide/collisionSolid.cxx +++ b/panda/src/collide/collisionSolid.cxx @@ -89,7 +89,7 @@ test_intersection(const CollisionEntry &) const { //////////////////////////////////////////////////////////////////// void CollisionSolid:: xform(const LMatrix4f &mat) { - if (has_effective_normal()) { + if ((_flags & F_effective_normal) != 0) { _effective_normal = _effective_normal * mat; _effective_normal.normalize(); } diff --git a/panda/src/collide/collisionSolid.h b/panda/src/collide/collisionSolid.h index 0e4b951e16..e56ee348a0 100644 --- a/panda/src/collide/collisionSolid.h +++ b/panda/src/collide/collisionSolid.h @@ -21,6 +21,7 @@ #include "pandabase.h" +#include "config_collide.h" #include "typedWritableReferenceCount.h" #include "boundedObject.h" #include "luse.h" diff --git a/panda/src/collide/config_collide.cxx b/panda/src/collide/config_collide.cxx index 9b7f436baa..d009e79417 100644 --- a/panda/src/collide/config_collide.cxx +++ b/panda/src/collide/config_collide.cxx @@ -51,6 +51,12 @@ ConfigureFn(config_collide) { // case-by-case basis whether they really need this feature. const bool respect_prev_transform = config_collide.GetBool("respect-prev-transform", false); +// This should be true to support the effective_normal interface of +// polygons. Set it false to disable this feature, so that all +// collision solids (including polygons and planes) use their actual +// normal for intersection and physics tests. +const bool respect_effective_normal = config_collide.GetBool("respect-effective-normal", true); + //////////////////////////////////////////////////////////////////// // Function: init_libcollide // Description: Initializes the library. This must be called at diff --git a/panda/src/collide/config_collide.h b/panda/src/collide/config_collide.h index 9ee35683a7..5a533ae698 100644 --- a/panda/src/collide/config_collide.h +++ b/panda/src/collide/config_collide.h @@ -25,6 +25,7 @@ NotifyCategoryDecl(collide, EXPCL_PANDA, EXPTP_PANDA); extern const bool respect_prev_transform; +extern const bool respect_effective_normal; extern EXPCL_PANDA void init_libcollide(); diff --git a/panda/src/putil/bam.h b/panda/src/putil/bam.h index 642ca54f5b..da982b315f 100644 --- a/panda/src/putil/bam.h +++ b/panda/src/putil/bam.h @@ -34,7 +34,7 @@ static const unsigned short _bam_major_ver = 4; // Bumped to major version 3 on 12/8/00 to change float64's to float32's. // Bumped to major version 4 on 4/10/02 to store new scene graph. -static const unsigned short _bam_minor_ver = 8; +static const unsigned short _bam_minor_ver = 9; // Bumped to minor version 1 on 4/10/03 to add CullFaceAttrib::reverse. // Bumped to minor version 2 on 4/12/03 to add num_components to texture. // Bumped to minor version 3 on 4/15/03 to add ImageBuffer::_alpha_file_channel @@ -43,6 +43,7 @@ static const unsigned short _bam_minor_ver = 8; // Bumped to minor version 6 on 7/22/03 to add shear to scene graph and animation data. // Bumped to minor version 7 on 11/10/03 to add CollisionSolid::_effective_normal // Bumped to minor version 8 on 11/12/03 to add FFTCompressor::reject_compression +// Bumped to minor version 9 on 12/02/03 to change CollisionPolygon internals. #endif