diff --git a/panda/src/collide/collisionPolygon.cxx b/panda/src/collide/collisionPolygon.cxx index cc3eef4810..2a1dae80da 100644 --- a/panda/src/collide/collisionPolygon.cxx +++ b/panda/src/collide/collisionPolygon.cxx @@ -484,6 +484,7 @@ test_intersection_from_sphere(const CollisionEntry &entry) const { } LPoint2 p = to_2d(from_center - dist * get_normal()); + LPoint2 edge_p; PN_stdfloat edge_dist = 0.0f; const ClipPlaneAttrib *cpa = entry.get_into_clip_planes(); @@ -492,7 +493,7 @@ test_intersection_from_sphere(const CollisionEntry &entry) const { Points new_points; 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. - edge_dist = dist_to_polygon(p, _points); + edge_dist = dist_to_polygon(p, edge_p, _points); } else if (new_points.empty()) { // The polygon is completely clipped. @@ -500,12 +501,12 @@ test_intersection_from_sphere(const CollisionEntry &entry) const { } else { // Test against the clipped polygon. - edge_dist = dist_to_polygon(p, new_points); + edge_dist = dist_to_polygon(p, edge_p, new_points); } } else { // No clip plane is in effect. Do the default test. - edge_dist = dist_to_polygon(p, _points); + edge_dist = dist_to_polygon(p, edge_p, _points); } // Now we have edge_dist, which is the distance from the sphere center to @@ -548,9 +549,21 @@ test_intersection_from_sphere(const CollisionEntry &entry) const { into_depth = max_dist - orig_dist; } + if (edge_dist >= 0.0f) { + // If colliding with an edge, we take the point on the edge. + LMatrix4 to_3d_mat; + rederive_to_3d_mat(to_3d_mat); + + LPoint3 surface_point = to_3d(edge_p, to_3d_mat); + new_entry->set_surface_point(surface_point); + new_entry->set_interior_point(surface_point - normal * into_depth); + } else { + // Otherwise, we use the projection of the center onto the polygon. + new_entry->set_surface_point(from_center - normal * dist); + new_entry->set_interior_point(from_center - normal * (dist + into_depth)); + } + new_entry->set_surface_normal(normal); - new_entry->set_surface_point(from_center - normal * dist); - new_entry->set_interior_point(from_center - normal * (dist + into_depth)); new_entry->set_contact_pos(contact_point); new_entry->set_contact_normal(get_normal()); new_entry->set_t(actual_t); @@ -1330,9 +1343,12 @@ point_is_inside(const LPoint2 &p, const CollisionPolygon::Points &points) const * 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. + * + * If the point is not within the polygon, the closest point to the edge is + * returned in the edge_p argument. */ PN_stdfloat CollisionPolygon:: -dist_to_polygon(const LPoint2 &p, const CollisionPolygon::Points &points) const { +dist_to_polygon(const LPoint2 &p, LPoint2 &edge_p, const CollisionPolygon::Points &points) const { // We know that that the polygon is convex and is defined with the points in // counterclockwise order. Therefore, we simply compare the signed distance @@ -1344,6 +1360,7 @@ dist_to_polygon(const LPoint2 &p, const CollisionPolygon::Points &points) const bool got_dist = false; PN_stdfloat best_dist = -1.0f; + size_t best_i; size_t num_points = points.size(); for (size_t i = 0; i < num_points - 1; ++i) { @@ -1353,6 +1370,7 @@ dist_to_polygon(const LPoint2 &p, const CollisionPolygon::Points &points) const if (!got_dist || d < best_dist) { best_dist = d; got_dist = true; + best_i = i; } } } @@ -1363,6 +1381,24 @@ dist_to_polygon(const LPoint2 &p, const CollisionPolygon::Points &points) const if (!got_dist || d < best_dist) { best_dist = d; got_dist = true; + best_i = num_points - 1; + } + } + + if (got_dist) { + // Project the point onto the best line, so that we can confine it to the + // line segment. + LPoint2 best_p = points[best_i]._p; + LPoint2 next_p = points[(best_i + 1) % points.size()]._p; + LVector2 segment = next_p - best_p; + PN_stdfloat t = (p - best_p).dot(segment) / segment.length_squared(); + if (t <= 0.0f) { + edge_p = best_p; + } else if (t >= 1.0f) { + edge_p = next_p; + } else { + LVector2 v(points[best_i]._v[1], -points[best_i]._v[0]); + edge_p = p - v * best_dist; } } diff --git a/panda/src/collide/collisionPolygon.h b/panda/src/collide/collisionPolygon.h index 045bcc7183..e657b3d623 100644 --- a/panda/src/collide/collisionPolygon.h +++ b/panda/src/collide/collisionPolygon.h @@ -124,7 +124,7 @@ private: const Points &points) const; bool point_is_inside(const LPoint2 &p, const Points &points) const; - PN_stdfloat dist_to_polygon(const LPoint2 &p, const Points &points) const; + PN_stdfloat dist_to_polygon(const LPoint2 &p, LPoint2 &edge_p, const Points &points) const; void project(const LVector3 &axis, PN_stdfloat ¢er, PN_stdfloat &extent) const; void setup_points(const LPoint3 *begin, const LPoint3 *end); diff --git a/tests/collide/test_into_poly.py b/tests/collide/test_into_poly.py index ccfd15fcd1..600b9d667d 100644 --- a/tests/collide/test_into_poly.py +++ b/tests/collide/test_into_poly.py @@ -31,7 +31,7 @@ def test_sphere_into_poly(): # Colliding just on the edge entry, np_from, np_into = make_collision(CollisionSphere(0, 0, 3, 2), poly) - assert entry.get_surface_point(np_from) == Point3(0, 0, 3) + assert entry.get_surface_point(np_from) == Point3(0, 0, 1) assert entry.get_surface_normal(np_into) == Vec3(-1, 0, 0) # Testing surface normal # No collision