diff --git a/servers/physics_2d/godot_body_pair_2d.cpp b/servers/physics_2d/godot_body_pair_2d.cpp index f8ec0b6512e..ca67277d1c9 100644 --- a/servers/physics_2d/godot_body_pair_2d.cpp +++ b/servers/physics_2d/godot_body_pair_2d.cpp @@ -160,7 +160,7 @@ void GodotBodyPair2D::_validate_contacts() { } } -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, bool p_swap_result) { +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(); if (mlen < CMP_EPSILON) { @@ -171,14 +171,18 @@ bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, real_t min, max; p_A->get_shape(p_shape_A)->project_rangev(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. int a; Vector2 s[2]; p_A->get_shape(p_shape_A)->get_supports(p_xform_A.basis_xform(mnormal).normalized(), s, a); @@ -187,7 +191,8 @@ bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, Transform2D from_inv = p_xform_B.affine_inverse(); - Vector2 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. + Vector2 local_from = from_inv.xform(from - mnormal * mlen * 0.1); Vector2 local_to = from_inv.xform(to); Vector2 rpos, rnorm; @@ -195,20 +200,22 @@ bool GodotBodyPair2D::_test_ccd(real_t p_step, GodotBody2D *p_A, int p_shape_A, return false; } - //ray hit something + // 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.get_axis(1).normalized(); + if (direction.dot(mnormal) < CMP_EPSILON) { + collided = false; + oneway_disabled = true; + return false; + } + } + // 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 contact_A = to; - Vector2 contact_B = hitpos; - - //create a contact - - if (p_swap_result) { - _contact_added_callback(contact_B, contact_A); - } else { - _contact_added_callback(contact_A, contact_B); - } + real_t newlen = hitpos.distance_to(from) - (max - min) * 0.01; + p_A->set_linear_velocity(mnormal * (newlen / p_step)); return true; } @@ -222,6 +229,8 @@ real_t combine_friction(GodotBody2D *A, GodotBody2D *B) { } bool GodotBodyPair2D::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; @@ -269,24 +278,19 @@ bool GodotBodyPair2D::setup(real_t p_step) { collided = GodotCollisionSolver2D::solve(shape_A_ptr, xform_A, motion_A, shape_B_ptr, xform_B, motion_B, _add_contact, this, &sep_axis); if (!collided) { - //test ccd (currently just a raycast) + oneway_disabled = false; if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_A) { - if (_test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B)) { - collided = true; - } + check_ccd = true; + return true; } if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_B) { - if (_test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A, true)) { - collided = true; - } + check_ccd = true; + return true; } - if (!collided) { - oneway_disabled = false; - return false; - } + return false; } if (oneway_disabled) { @@ -335,7 +339,29 @@ bool GodotBodyPair2D::setup(real_t p_step) { } bool GodotBodyPair2D::pre_solve(real_t p_step) { - if (!collided || oneway_disabled) { + if (oneway_disabled) { + return false; + } + + if (!collided) { + if (check_ccd) { + const Vector2 &offset_A = A->get_transform().get_origin(); + Transform2D xform_Au = A->get_transform().untranslated(); + Transform2D xform_A = xform_Au * A->get_shape_transform(shape_A); + + Transform2D xform_Bu = B->get_transform(); + xform_Bu.elements[2] -= offset_A; + Transform2D xform_B = xform_Bu * B->get_shape_transform(shape_B); + + if (A->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_A) { + _test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B); + } + + if (B->get_continuous_collision_detection_mode() == PhysicsServer2D::CCD_MODE_CAST_RAY && collide_B) { + _test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A); + } + } + return false; } diff --git a/servers/physics_2d/godot_body_pair_2d.h b/servers/physics_2d/godot_body_pair_2d.h index aa1b5b7886f..0a086a78b1d 100644 --- a/servers/physics_2d/godot_body_pair_2d.h +++ b/servers/physics_2d/godot_body_pair_2d.h @@ -79,10 +79,11 @@ class GodotBodyPair2D : public GodotConstraint2D { Contact contacts[MAX_CONTACTS]; int contact_count = 0; bool collided = false; + bool check_ccd = false; bool oneway_disabled = false; bool report_contacts_only = false; - bool _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, bool p_swap_result = false); + bool _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); void _validate_contacts(); static void _add_contact(const Vector2 &p_point_A, const Vector2 &p_point_B, void *p_self); _FORCE_INLINE_ void _contact_added_callback(const Vector2 &p_point_A, const Vector2 &p_point_B); diff --git a/servers/physics_3d/godot_body_pair_3d.cpp b/servers/physics_3d/godot_body_pair_3d.cpp index 5c25ba9537a..dd82a020591 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 7c2c31704be..8a9664e7e14 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;