diff --git a/servers/physics_2d/godot_body_pair_2d.cpp b/servers/physics_2d/godot_body_pair_2d.cpp index 7b7c67bbc29..79e084e90e2 100644 --- a/servers/physics_2d/godot_body_pair_2d.cpp +++ b/servers/physics_2d/godot_body_pair_2d.cpp @@ -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(); @@ -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; } @@ -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; diff --git a/servers/physics_3d/godot_body_pair_3d.cpp b/servers/physics_3d/godot_body_pair_3d.cpp index 7e6cc6f8342..981a7c502f3 100644 --- a/servers/physics_3d/godot_body_pair_3d.cpp +++ b/servers/physics_3d/godot_body_pair_3d.cpp @@ -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(); @@ -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;