Fix rigid body ray cast CCD in 2D and 3D Godot Physics

For 2D:
Raycast CCD now works the same as in 3D, it changes the body's velocity
to place it at the impact position instead of generating a contact point
that causes a wrong push back.

For both 2D and 3D:
The raycast CCD process reads and modifies body velocities, so it needs
to be moved to pre_solve() instead of setup() to be processed linearly
on the main thread, otherwise multithreading can cause some CCD results
to be randomly lost when multiple collisions occur.

4 files changed, 94 insertions, 40 deletions

diff --git a/servers/physics_2d/godot_body_pair_2d.cpp b/servers/physics_2d/godot_body_pair_2d.cpp
index f8ec0b6512..ca67277d1c 100644
--- a/servers/physics_2d/godot_body_pair_2d.cpp
+++ b/servers/physics_2d/godot_body_pair_2d.cpp
@@ -160,7 +160,7 @@ void GodotBodyPair2D::_validate_contacts() {
 	}
 }
 
-bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, const Transform2D &p_xform_A, GodotBody2D *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result) {
+bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, const Transform2D &p_xform_A, GodotBody2D *p_B, int p_shape_B, const Transform2D &p_xform_B) {
 	Vector2 motion = p_A->get_linear_velocity() * p_step;
 	real_t mlen = motion.length();
 	if (mlen < CMP_EPSILON) {
@@ -171,14 +171,18 @@ bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A,
 
 	real_t min, max;
 	p_A->get_shape(p_shape_A)->project_rangev(mnormal, p_xform_A, min, max);
-	bool fast_object = mlen > (max - min) * 0.3; //going too fast in that direction
 
-	if (!fast_object) { //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
+	// 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.
+	bool fast_object = mlen > (max - min) * 0.3;
+	if (!fast_object) {
 		return false;
 	}
 
-	//cast a segment from support in motion normal, in the same direction of motion by motion length
-	//support is the worst case collision point, so real collision happened before
+	// Going too fast in that direction.
+
+	// Cast a segment from support in motion normal, in the same direction of motion by motion length.
+	// Support is the worst case collision point, so real collision happened before.
 	int a;
 	Vector2 s[2];
 	p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform(mnormal).normalized(), s, a);
@@ -187,7 +191,8 @@ bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A,
 
 	Transform2D from_inv = p_xform_B.affine_inverse();
 
-	Vector2 local_from = from_inv.xform(from - mnormal * mlen * 0.1); //start from a little inside the bounding box
+	// Start from a little inside the bounding box.
+	Vector2 local_from = from_inv.xform(from - mnormal * mlen * 0.1);
 	Vector2 local_to = from_inv.xform(to);
 
 	Vector2 rpos, rnorm;
@@ -195,20 +200,22 @@ bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A,
 		return false;
 	}
 
-	//ray hit something
+	// Check one-way collision based on motion direction.
+	if (p_A->get_shape(p_shape_A)->allows_one_way_collision() && p_B->is_shape_set_as_one_way_collision(p_shape_B)) {
+		Vector2 direction = p_xform_B.get_axis(1).normalized();
+		if (direction.dot(mnormal) < CMP_EPSILON) {
+			collided = false;
+			oneway_disabled = true;
+			return false;
+		}
+	}
 
+	// Shorten the linear velocity so it does not hit, but gets close enough,
+	// next frame will hit softly or soft enough.
 	Vector2 hitpos = p_xform_B.xform(rpos);
 
-	Vector2 contact_A = to;
-	Vector2 contact_B = hitpos;
-
-	//create a contact
-
-	if (p_swap_result) {
-		_contact_added_callback(contact_B, contact_A);
-	} else {
-		_contact_added_callback(contact_A, contact_B);
-	}
+	real_t newlen = hitpos.distance_to(from) - (max - min) * 0.01;
+	p_A->set_linear_velocity(mnormal * (newlen / p_step));
 
 	return true;
 }
@@ -222,6 +229,8 @@ real_t combine_friction(GodotBody2D *A, GodotBody2D *B) {
 }
 
 bool GodotBodyPair2D::setup(real_t p_step) {
+	check_ccd = false;
+
 	if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) {
 		collided = false;
 		return false;
@@ -269,24 +278,19 @@ bool GodotBodyPair2D::setup(real_t p_step) {
 
 	collided = GodotCollisionSolver2D::solve(shape_A_ptr, xform_A, motion_A, shape_B_ptr, xform_B, motion_B, _add_contact, this, &sep_axis);
 	if (!collided) {
-		//test ccd (currently just a raycast)
+		oneway_disabled = false;
 
 		if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_A) {
-			if (_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B)) {
-				collided = true;
-			}
+			check_ccd = true;
+			return true;
 		}
 
 		if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_B) {
-			if (_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A, true)) {
-				collided = true;
-			}
+			check_ccd = true;
+			return true;
 		}
 
-		if (!collided) {
-			oneway_disabled = false;
-			return false;
-		}
+		return false;
 	}
 
 	if (oneway_disabled) {
@@ -335,7 +339,29 @@ bool GodotBodyPair2D::setup(real_t p_step) {
 }
 
 bool GodotBodyPair2D::pre_solve(real_t p_step) {
-	if (!collided || oneway_disabled) {
+	if (oneway_disabled) {
+		return false;
+	}
+
+	if (!collided) {
+		if (check_ccd) {
+			const Vector2 &offset_A = A->get_transform().get_origin();
+			Transform2D xform_Au = A->get_transform().untranslated();
+			Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A);
+
+			Transform2D xform_Bu = B->get_transform();
+			xform_Bu.elements[2] -= offset_A;
+			Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B);
+
+			if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_A) {
+				_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B);
+			}
+
+			if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_B) {
+				_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A);
+			}
+		}
+
 		return false;
 	}
 
diff --git a/servers/physics_2d/godot_body_pair_2d.h b/servers/physics_2d/godot_body_pair_2d.h
index aa1b5b7886..0a086a78b1 100644
--- a/servers/physics_2d/godot_body_pair_2d.h
+++ b/servers/physics_2d/godot_body_pair_2d.h
@@ -79,10 +79,11 @@ class GodotBodyPair2D : public GodotConstraint2D {
 	Contact contacts[MAX_CONTACTS];
 	int contact_count = 0;
 	bool collided = false;
+	bool check_ccd = false;
 	bool oneway_disabled = false;
 	bool report_contacts_only = false;
 
-	bool _test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, const Transform2D &p_xform_A, GodotBody2D *p_B, int p_shape_B, const Transform2D &p_xform_B, bool p_swap_result = false);
+	bool _test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, const Transform2D &p_xform_A, GodotBody2D *p_B, int p_shape_B, const Transform2D &p_xform_B);
 	void _validate_contacts();
 	static void _add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self);
 	_FORCE_INLINE_ void _contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B);
diff --git a/servers/physics_3d/godot_body_pair_3d.cpp b/servers/physics_3d/godot_body_pair_3d.cpp
index 5c25ba9537..dd82a02059 100644
--- a/servers/physics_3d/godot_body_pair_3d.cpp
+++ b/servers/physics_3d/godot_body_pair_3d.cpp
@@ -172,21 +172,26 @@ bool GodotBodyPair3D::_test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A,
 
 	real_t min, max;
 	p_A->get_shape(p_shape_A)->project_range(mnormal, p_xform_A, min, max);
-	bool fast_object = mlen > (max - min) * 0.3; //going too fast in that direction
 
-	if (!fast_object) { //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
+	// 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.
+	bool fast_object = mlen > (max - min) * 0.3;
+	if (!fast_object) {
 		return false;
 	}
 
-	//cast a segment from support in motion normal, in the same direction of motion by motion length
-	//support is the worst case collision point, so real collision happened before
+	// Going too fast in that direction.
+
+	// Cast a segment from support in motion normal, in the same direction of motion by motion length.
+	// Support is the worst case collision point, so real collision happened before.
 	Vector3 s = p_A->get_shape(p_shape_A)->get_support(p_xform_A.basis.xform(mnormal).normalized());
 	Vector3 from = p_xform_A.xform(s);
 	Vector3 to = from + motion;
 
 	Transform3D from_inv = p_xform_B.affine_inverse();
 
-	Vector3 local_from = from_inv.xform(from - mnormal * mlen * 0.1); //start from a little inside the bounding box
+	// Start from a little inside the bounding box.
+	Vector3 local_from = from_inv.xform(from - mnormal * mlen * 0.1);
 	Vector3 local_to = from_inv.xform(to);
 
 	Vector3 rpos, rnorm;
@@ -194,7 +199,8 @@ bool GodotBodyPair3D::_test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A,
 		return false;
 	}
 
-	//shorten the linear velocity so it does not hit, but gets close enough, next frame will hit softly or soft enough
+	// Shorten the linear velocity so it does not hit, but gets close enough,
+	// next frame will hit softly or soft enough.
 	Vector3 hitpos = p_xform_B.xform(rpos);
 
 	real_t newlen = hitpos.distance_to(from) - (max - min) * 0.01;
@@ -212,6 +218,8 @@ real_t combine_friction(GodotBody3D *A, GodotBody3D *B) {
 }
 
 bool GodotBodyPair3D::setup(real_t p_step) {
+	check_ccd = false;
+
 	if (!A->interacts_with(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self())) {
 		collided = false;
 		return false;
@@ -248,14 +256,14 @@ bool GodotBodyPair3D::setup(real_t p_step) {
 	collided = GodotCollisionSolver3D::solve_static(shape_A_ptr, xform_A, shape_B_ptr, xform_B, _contact_added_callback, this, &sep_axis);
 
 	if (!collided) {
-		//test ccd (currently just a raycast)
-
 		if (A->is_continuous_collision_detection_enabled() && collide_A) {
-			_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B);
+			check_ccd = true;
+			return true;
 		}
 
 		if (B->is_continuous_collision_detection_enabled() && collide_B) {
-			_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A);
+			check_ccd = true;
+			return true;
 		}
 
 		return false;
@@ -266,6 +274,24 @@ bool GodotBodyPair3D::setup(real_t p_step) {
 
 bool GodotBodyPair3D::pre_solve(real_t p_step) {
 	if (!collided) {
+		if (check_ccd) {
+			const Vector3 &offset_A = A->get_transform().get_origin();
+			Transform3D xform_Au = Transform3D(A->get_transform().basis, Vector3());
+			Transform3D xform_A = xform_Au * A->get_shape_transform(shape_A);
+
+			Transform3D xform_Bu = B->get_transform();
+			xform_Bu.origin -= offset_A;
+			Transform3D xform_B = xform_Bu * B->get_shape_transform(shape_B);
+
+			if (A->is_continuous_collision_detection_enabled() && collide_A) {
+				_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B);
+			}
+
+			if (B->is_continuous_collision_detection_enabled() && collide_B) {
+				_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A);
+			}
+		}
+
 		return false;
 	}
 
diff --git a/servers/physics_3d/godot_body_pair_3d.h b/servers/physics_3d/godot_body_pair_3d.h
index 7c2c31704b..8a9664e7e1 100644
--- a/servers/physics_3d/godot_body_pair_3d.h
+++ b/servers/physics_3d/godot_body_pair_3d.h
@@ -60,6 +60,7 @@ protected:
 
 	Vector3 sep_axis;
 	bool collided = false;
+	bool check_ccd = false;
 
 	GodotSpace3D *space = nullptr;