diff options
Diffstat (limited to 'servers')
| -rw-r--r-- | servers/physics_3d/godot_body_pair_3d.cpp | 80 |
1 files changed, 57 insertions, 23 deletions
diff --git a/servers/physics_3d/godot_body_pair_3d.cpp b/servers/physics_3d/godot_body_pair_3d.cpp index 619e6c00be..3a91e5e480 100644 --- a/servers/physics_3d/godot_body_pair_3d.cpp +++ b/servers/physics_3d/godot_body_pair_3d.cpp @@ -167,6 +167,9 @@ void GodotBodyPair3D::validate_contacts() { // 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) { + GodotShape3D *shape_A_ptr = p_A->get_shape(p_shape_A); + GodotShape3D *shape_B_ptr = p_B->get_shape(p_shape_B); + Vector3 motion = p_A->get_linear_velocity() * p_step; real_t mlen = motion.length(); if (mlen < CMP_EPSILON) { @@ -176,7 +179,7 @@ bool GodotBodyPair3D::_test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A, Vector3 mnormal = motion / mlen; real_t min = 0.0, max = 0.0; - p_A->get_shape(p_shape_A)->project_range(mnormal, p_xform_A, min, max); + shape_A_ptr->project_range(mnormal, p_xform_A, min, max); // 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. @@ -187,33 +190,64 @@ 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. - // 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. - - // 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); - Vector3 to = from + motion; - - Transform3D from_inv = p_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. - Vector3 local_from = from_inv.xform(from - motion * 0.1); - Vector3 local_to = from_inv.xform(to); + // 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; + Vector3 supports_A[max_supports]; + int support_count_A; + GodotShape3D::FeatureType support_type_A; + // Convert mnormal into body A's local xform because get_supports requires (and returns) local coordinates. + shape_A_ptr->get_supports(p_xform_A.basis.xform_inv(mnormal).normalized(), max_supports, supports_A, support_count_A, support_type_A); + + // Cast a segment from each support point of A in the motion direction. + int segment_support_idx = -1; + float segment_hit_length = FLT_MAX; + Vector3 segment_hit_local; + for (int i = 0; i < support_count_A; i++) { + supports_A[i] = p_xform_A.xform(supports_A[i]); + + Vector3 from = supports_A[i]; + Vector3 to = from + motion; + + Transform3D from_inv = p_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. + Vector3 local_from = from_inv.xform(from - motion * 0.1); + Vector3 local_to = from_inv.xform(to); + + Vector3 rpos, rnorm; + if (shape_B_ptr->intersect_segment(local_from, local_to, rpos, rnorm, true)) { + float hit_length = local_from.distance_to(rpos); + if (hit_length < segment_hit_length) { + segment_support_idx = i; + segment_hit_length = hit_length; + segment_hit_local = rpos; + } + } + } - 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 + if (segment_support_idx == -1) { + // 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 will collide next frame. - Vector3 hitpos = p_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. + Vector3 hitpos = p_xform_B.xform(segment_hit_local); + + real_t newlen = hitpos.distance_to(supports_A[segment_support_idx]); + if (shape_B_ptr->is_concave()) { + // Subtracting 5% of body length from the distance between collision and support point + // should cause body A's support point to arrive just before a face of B next frame. + newlen = MAX(newlen - (max - min) * 0.05, 0.0); + // NOTE: This may stop body A completely, without a proper collision response. + // We consider this preferable to tunneling. + } else { + // 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. + newlen += (max - min) * 0.01; + } p_A->set_linear_velocity((mnormal * newlen) / p_step); |