Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix Physics3D and Physics2D CCD sometimes adjusting velocity too much #69934

Merged
merged 1 commit into from
Dec 14, 2022
Merged

Fix Physics3D and Physics2D CCD sometimes adjusting velocity too much #69934

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
23 changes: 18 additions & 5 deletions servers/physics_2d/godot_body_pair_2d.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -161,6 +161,11 @@ void GodotBodyPair2D::_validate_contacts() {
}
}

// _test_ccd prevents tunneling by slowing down a high velocity body that is about to collide so that next frame it will be at an appropriate location to collide (i.e. slight overlap)
// Warning: the way velocity is adjusted down to cause a collision means the momentum will be weaker than it should for a bounce!
// Process: only proceed if body A's motion is high relative to its size.
// 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 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();
Expand All @@ -180,24 +185,32 @@ bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A,
return false;
}

// Going too fast in that direction.
// 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 is the worst case collision point, so real collision happened before.
// 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.
int a;
Vector2 s[2];
p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform(mnormal).normalized(), s, a);
p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform_inv(mnormal).normalized(), s, a);
Vector2 from = p_xform_A.xform(s[0]);
// Back up 10% of the per-frame motion behind the support point and use that as the beginning of our cast.
// This should ensure the calculated new velocity will really cause a bit of overlap instead of just getting us very close.
from -= motion * 0.1;
Vector2 to = from + motion;

Transform2D from_inv = p_xform_B.affine_inverse();

// Start from a little inside the bounding box.
Vector2 local_from = from_inv.xform(from - mnormal * mlen * 0.1);
Vector2 local_from = from_inv.xform(from);
Vector2 local_to = from_inv.xform(to);

Vector2 rpos, rnorm;
if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm)) {
// 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;
}

Expand All @@ -215,7 +228,7 @@ bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A,
// next frame will hit softly or soft enough.
Vector2 hitpos = p_xform_B.xform(rpos);

real_t newlen = hitpos.distance_to(from) - (max - min) * 0.01;
real_t newlen = hitpos.distance_to(from);
p_A->set_linear_velocity(mnormal * (newlen / p_step));

return true;
Expand Down
29 changes: 20 additions & 9 deletions servers/physics_3d/godot_body_pair_3d.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -161,6 +161,11 @@ void GodotBodyPair3D::validate_contacts() {
}
}

// _test_ccd prevents tunneling by slowing down a high velocity body that is about to collide so that next frame it will be at an appropriate location to collide (i.e. slight overlap)
// Warning: the way velocity is adjusted down to cause a collision means the momentum will be weaker than it should for a bounce!
// Process: only proceed if body A's motion is high relative to its size.
// 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) {
Vector3 motion = p_A->get_linear_velocity() * p_step;
real_t mlen = motion.length();
Expand All @@ -177,33 +182,39 @@ bool GodotBodyPair3D::_test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A,
// 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;
return false; // moving slow enough that there's no chance of tunneling.
}

// Going too fast in that direction.
// 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 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());
// 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);
// Back up 10% of the per-frame motion behind the support point and use that as the beginning of our cast.
// This should ensure the calculated new velocity will really cause a bit of overlap instead of just getting us very close.
from -= motion * 0.1;
Vector3 to = from + motion;

Transform3D from_inv = p_xform_B.affine_inverse();

// Start from a little inside the bounding box.
Vector3 local_from = from_inv.xform(from - mnormal * mlen * 0.1);
Vector3 local_from = from_inv.xform(from);
Vector3 local_to = from_inv.xform(to);

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
// 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 does not hit, but gets close enough,
// next frame will hit softly or soft enough.
// 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;
real_t newlen = hitpos.distance_to(from); // this length (speed) should cause the point we chose slightly behind A's support point to arrive right at B's collider next frame.
p_A->set_linear_velocity((mnormal * newlen) / p_step);

return true;
Expand Down