Fix CCD in case of multiple supports in motion direction

For example, an axis-aligned box under the influence of gravity landing
on a horizontal triangle could previously tunnel, because only one of
the supports of the box was considered to be the candidate point that
would collide first. Now all supports are considered as candidates.

Also fix CCD always pushing the fast body into the other body, which
works well for convex shapes but not for trimesh/concave shapes.

1 files changed, 57 insertions, 23 deletions

diff --git a/servers/physics_3d/godot_body_pair_3d.cpp b/servers/physics_3d/godot_body_pair_3d.cpp
index 619e6c00be..3a91e5e480 100644
--- a/servers/physics_3d/godot_body_pair_3d.cpp
+++ b/servers/physics_3d/godot_body_pair_3d.cpp
@@ -167,6 +167,9 @@ void GodotBodyPair3D::validate_contacts() {
 // cast forward along motion vector to see if A is going to enter/pass B's collider next frame, only proceed if it does.
 // adjust the velocity of A down so that it will just slightly intersect the collider instead of blowing right past it.
 bool GodotBodyPair3D::_test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A, const Transform3D &p_xform_A, GodotBody3D *p_B, int p_shape_B, const Transform3D &p_xform_B) {
+	GodotShape3D *shape_A_ptr = p_A->get_shape(p_shape_A);
+	GodotShape3D *shape_B_ptr = p_B->get_shape(p_shape_B);
+
 	Vector3 motion = p_A->get_linear_velocity() * p_step;
 	real_t mlen = motion.length();
 	if (mlen < CMP_EPSILON) {
@@ -176,7 +179,7 @@ bool GodotBodyPair3D::_test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A,
 	Vector3 mnormal = motion / mlen;
 
 	real_t min = 0.0, max = 0.0;
-	p_A->get_shape(p_shape_A)->project_range(mnormal, p_xform_A, min, max);
+	shape_A_ptr->project_range(mnormal, p_xform_A, min, max);
 
 	// Did it move enough in this direction to even attempt raycast?
 	// Let's say it should move more than 1/3 the size of the object in that axis.
@@ -187,33 +190,64 @@ bool GodotBodyPair3D::_test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A,
 
 	// A is moving fast enough that tunneling might occur. See if it's really about to collide.
 
-	// Cast a segment from support in motion normal, in the same direction of motion by motion length.
-	// Support point will the farthest forward collision point along the movement vector.
-	// i.e. the point that should hit B first if any collision does occur.
-
-	// convert mnormal into body A's local xform because get_support requires (and returns) local coordinates.
-	Vector3 s = p_A->get_shape(p_shape_A)->get_support(p_xform_A.basis.xform_inv(mnormal).normalized());
-	Vector3 from = p_xform_A.xform(s);
-	Vector3 to = from + motion;
-
-	Transform3D from_inv = p_xform_B.affine_inverse();
-
-	// Back up 10% of the per-frame motion behind the support point and use that as the beginning of our cast.
-	// At high speeds, this may mean we're actually casting from well behind the body instead of inside it, which is odd. But it still works out.
-	Vector3 local_from = from_inv.xform(from - motion * 0.1);
-	Vector3 local_to = from_inv.xform(to);
+	// Support points are the farthest forward points on A in the direction of the motion vector.
+	// i.e. the candidate points of which one should hit B first if any collision does occur.
+	static const int max_supports = 16;
+	Vector3 supports_A[max_supports];
+	int support_count_A;
+	GodotShape3D::FeatureType support_type_A;
+	// Convert mnormal into body A's local xform because get_supports requires (and returns) local coordinates.
+	shape_A_ptr->get_supports(p_xform_A.basis.xform_inv(mnormal).normalized(), max_supports, supports_A, support_count_A, support_type_A);
+
+	// Cast a segment from each support point of A in the motion direction.
+	int segment_support_idx = -1;
+	float segment_hit_length = FLT_MAX;
+	Vector3 segment_hit_local;
+	for (int i = 0; i < support_count_A; i++) {
+		supports_A[i] = p_xform_A.xform(supports_A[i]);
+
+		Vector3 from = supports_A[i];
+		Vector3 to = from + motion;
+
+		Transform3D from_inv = p_xform_B.affine_inverse();
+
+		// Back up 10% of the per-frame motion behind the support point and use that as the beginning of our cast.
+		// At high speeds, this may mean we're actually casting from well behind the body instead of inside it, which is odd.
+		// But it still works out.
+		Vector3 local_from = from_inv.xform(from - motion * 0.1);
+		Vector3 local_to = from_inv.xform(to);
+
+		Vector3 rpos, rnorm;
+		if (shape_B_ptr->intersect_segment(local_from, local_to, rpos, rnorm, true)) {
+			float hit_length = local_from.distance_to(rpos);
+			if (hit_length < segment_hit_length) {
+				segment_support_idx = i;
+				segment_hit_length = hit_length;
+				segment_hit_local = rpos;
+			}
+		}
+	}
 
-	Vector3 rpos, rnorm;
-	if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm, true)) {
-		// there was no hit. Since the segment is the length of per-frame motion, this means the bodies will not
+	if (segment_support_idx == -1) {
+		// There was no hit. Since the segment is the length of per-frame motion, this means the bodies will not
 		// actually collide yet on next frame. We'll probably check again next frame once they're closer.
 		return false;
 	}
 
-	// Shorten the linear velocity so it will collide next frame.
-	Vector3 hitpos = p_xform_B.xform(rpos);
-
-	real_t newlen = hitpos.distance_to(from) + (max - min) * 0.01; // adding 1% of body length to the distance between collision and support point should cause body A's support point to arrive just within B's collider next frame.
+	Vector3 hitpos = p_xform_B.xform(segment_hit_local);
+
+	real_t newlen = hitpos.distance_to(supports_A[segment_support_idx]);
+	if (shape_B_ptr->is_concave()) {
+		// Subtracting 5% of body length from the distance between collision and support point
+		// should cause body A's support point to arrive just before a face of B next frame.
+		newlen = MAX(newlen - (max - min) * 0.05, 0.0);
+		// NOTE: This may stop body A completely, without a proper collision response.
+		// We consider this preferable to tunneling.
+	} else {
+		// Adding 1% of body length to the distance between collision and support point
+		// should cause body A's support point to arrive just within B's collider next frame.
+		newlen += (max - min) * 0.01;
+	}
 
 	p_A->set_linear_velocity((mnormal * newlen) / p_step);