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.

2 files changed, 37 insertions, 10 deletions

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;