From 47c5b8bafceb203a5105d4e99789f238b068a90f Mon Sep 17 00:00:00 2001 From: Ricardo Buring Date: Sun, 16 Apr 2023 19:46:33 +0200 Subject: [PATCH] Improve rigid body CCD against moving bodies --- servers/physics_2d/godot_body_pair_2d.cpp | 9 ++++++--- servers/physics_3d/godot_body_pair_3d.cpp | 7 +++++-- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/servers/physics_2d/godot_body_pair_2d.cpp b/servers/physics_2d/godot_body_pair_2d.cpp index 40dbb4fcf420..4a90a7bd764c 100644 --- a/servers/physics_2d/godot_body_pair_2d.cpp +++ b/servers/physics_2d/godot_body_pair_2d.cpp @@ -187,6 +187,9 @@ bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, // A is moving fast enough that tunneling might occur. See if it's really about to collide. + // Roughly predict body B's position in the next frame (ignoring collisions). + Transform2D predicted_xform_B = p_xform_B.translated(p_B->get_linear_velocity() * p_step); + // 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. @@ -200,7 +203,7 @@ bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, // This should ensure the calculated new velocity will really cause a bit of overlap instead of just getting us very close. Vector2 to = from + motion; - Transform2D from_inv = p_xform_B.affine_inverse(); + Transform2D from_inv = predicted_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. @@ -216,7 +219,7 @@ bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, // 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.columns[1].normalized(); + Vector2 direction = predicted_xform_B.columns[1].normalized(); if (direction.dot(mnormal) < CMP_EPSILON) { collided = false; oneway_disabled = true; @@ -226,7 +229,7 @@ bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, // 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 hitpos = predicted_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. p_A->set_linear_velocity(mnormal * (newlen / p_step)); diff --git a/servers/physics_3d/godot_body_pair_3d.cpp b/servers/physics_3d/godot_body_pair_3d.cpp index c467a583ba0d..f710e68ea068 100644 --- a/servers/physics_3d/godot_body_pair_3d.cpp +++ b/servers/physics_3d/godot_body_pair_3d.cpp @@ -190,6 +190,9 @@ 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. + // Roughly predict body B's position in the next frame (ignoring collisions). + Transform3D predicted_xform_B = p_xform_B.translated(p_B->get_linear_velocity() * p_step); + // 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; @@ -209,7 +212,7 @@ bool GodotBodyPair3D::_test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A, Vector3 from = supports_A[i]; Vector3 to = from + motion; - Transform3D from_inv = p_xform_B.affine_inverse(); + Transform3D from_inv = predicted_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. @@ -234,7 +237,7 @@ bool GodotBodyPair3D::_test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A, return false; } - Vector3 hitpos = p_xform_B.xform(segment_hit_local); + Vector3 hitpos = predicted_xform_B.xform(segment_hit_local); real_t newlen = hitpos.distance_to(supports_A[segment_support_idx]); // Adding 1% of body length to the distance between collision and support point