diff options
Diffstat (limited to 'servers/physics_3d/godot_collision_solver_3d_sat.cpp')
| -rw-r--r-- | servers/physics_3d/godot_collision_solver_3d_sat.cpp | 79 |
1 files changed, 66 insertions, 13 deletions
diff --git a/servers/physics_3d/godot_collision_solver_3d_sat.cpp b/servers/physics_3d/godot_collision_solver_3d_sat.cpp index 0790333f65..4faa07b6c9 100644 --- a/servers/physics_3d/godot_collision_solver_3d_sat.cpp +++ b/servers/physics_3d/godot_collision_solver_3d_sat.cpp @@ -36,6 +36,8 @@ #define fallback_collision_solver gjk_epa_calculate_penetration +#define _BACKFACE_NORMAL_THRESHOLD -0.0002 + // Cylinder SAT analytic methods and face-circle contact points for cylinder-trimesh and cylinder-box collision are based on ODE colliders. /* @@ -612,13 +614,14 @@ class SeparatorAxisTest { const Transform3D *transform_A = nullptr; const Transform3D *transform_B = nullptr; real_t best_depth = 1e15; - Vector3 best_axis; _CollectorCallback *callback = nullptr; real_t margin_A = 0.0; real_t margin_B = 0.0; Vector3 separator_axis; public: + Vector3 best_axis; + _FORCE_INLINE_ bool test_previous_axis() { if (callback && callback->prev_axis && *callback->prev_axis != Vector3()) { return test_axis(*callback->prev_axis); @@ -627,7 +630,7 @@ public: } } - _FORCE_INLINE_ bool test_axis(const Vector3 &p_axis, bool p_directional = false) { + _FORCE_INLINE_ bool test_axis(const Vector3 &p_axis) { Vector3 axis = p_axis; if (axis.is_equal_approx(Vector3())) { @@ -661,12 +664,7 @@ public: //use the smallest depth if (min_B < 0.0) { // could be +0.0, we don't want it to become -0.0 - if (p_directional) { - min_B = max_B; - axis = -axis; - } else { - min_B = -min_B; - } + min_B = -min_B; } if (max_B < min_B) { @@ -1014,7 +1012,7 @@ static void _collision_sphere_face(const GodotShape3D *p_a, const Transform3D &p Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); - if (!separator.test_axis(normal, !face_B->backface_collision)) { + if (!separator.test_axis(normal)) { return; } @@ -1041,6 +1039,17 @@ static void _collision_sphere_face(const GodotShape3D *p_a, const Transform3D &p } } + if (!face_B->backface_collision) { + if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) { + if (face_B->invert_backface_collision) { + separator.best_axis = separator.best_axis.bounce(normal); + } else { + // Just ignore backface collision. + return; + } + } + } + separator.generate_contacts(); } @@ -1486,7 +1495,7 @@ static void _collision_box_face(const GodotShape3D *p_a, const Transform3D &p_tr Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); - if (!separator.test_axis(normal, !face_B->backface_collision)) { + if (!separator.test_axis(normal)) { return; } @@ -1591,6 +1600,17 @@ static void _collision_box_face(const GodotShape3D *p_a, const Transform3D &p_tr } } + if (!face_B->backface_collision) { + if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) { + if (face_B->invert_backface_collision) { + separator.best_axis = separator.best_axis.bounce(normal); + } else { + // Just ignore backface collision. + return; + } + } + } + separator.generate_contacts(); } @@ -1802,7 +1822,7 @@ static void _collision_capsule_face(const GodotShape3D *p_a, const Transform3D & Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); - if (!separator.test_axis(normal, !face_B->backface_collision)) { + if (!separator.test_axis(normal)) { return; } @@ -1858,6 +1878,17 @@ static void _collision_capsule_face(const GodotShape3D *p_a, const Transform3D & } } + if (!face_B->backface_collision) { + if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) { + if (face_B->invert_backface_collision) { + separator.best_axis = separator.best_axis.bounce(normal); + } else { + // Just ignore backface collision. + return; + } + } + } + separator.generate_contacts(); } @@ -1952,7 +1983,7 @@ static void _collision_cylinder_face(const GodotShape3D *p_a, const Transform3D Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); // Face B normal. - if (!separator.test_axis(normal, !face_B->backface_collision)) { + if (!separator.test_axis(normal)) { return; } @@ -2034,6 +2065,17 @@ static void _collision_cylinder_face(const GodotShape3D *p_a, const Transform3D } } + if (!face_B->backface_collision) { + if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) { + if (face_B->invert_backface_collision) { + separator.best_axis = separator.best_axis.bounce(normal); + } else { + // Just ignore backface collision. + return; + } + } + } + separator.generate_contacts(); } @@ -2174,7 +2216,7 @@ static void _collision_convex_polygon_face(const GodotShape3D *p_a, const Transf Vector3 normal = (vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized(); - if (!separator.test_axis(normal, !face_B->backface_collision)) { + if (!separator.test_axis(normal)) { return; } @@ -2266,6 +2308,17 @@ static void _collision_convex_polygon_face(const GodotShape3D *p_a, const Transf } } + if (!face_B->backface_collision) { + if (separator.best_axis.dot(normal) < _BACKFACE_NORMAL_THRESHOLD) { + if (face_B->invert_backface_collision) { + separator.best_axis = separator.best_axis.bounce(normal); + } else { + // Just ignore backface collision. + return; + } + } + } + separator.generate_contacts(); } |