Skip to content

Commit

Permalink
Merge pull request #76138 from rburing/ccd_against_moving
Browse files Browse the repository at this point in the history
Improve rigid body CCD against moving bodies
  • Loading branch information
akien-mga committed Jun 19, 2023
2 parents cce100a + 47c5b8b commit 7ff0a30
Show file tree
Hide file tree
Showing 2 changed files with 11 additions and 5 deletions.
9 changes: 6 additions & 3 deletions servers/physics_2d/godot_body_pair_2d.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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.
Expand All @@ -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;
Expand All @@ -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));
Expand Down
7 changes: 5 additions & 2 deletions servers/physics_3d/godot_body_pair_3d.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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.
Expand All @@ -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
Expand Down

0 comments on commit 7ff0a30

Please sign in to comment.