From d630269593d485ff8c9010d7ad6cca7c32cf93a6 Mon Sep 17 00:00:00 2001 From: PouleyKetchoupp Date: Wed, 17 Nov 2021 17:57:02 -0700 Subject: [PATCH] Fix contact generation with backface collision disabled Replaced the previous implementation for backface collision handling (in test_axis function from SAT algorithm) with much simpler logic in the collision generation phase with face shapes, in order to get rid of wrong contacts when backface collision is disabled. Now it just ignores the generated collision if the contact normal is against the face normal, with a threshold to keep edge contacts. Added a special case for soft bodies to invert the collision instead of ignoring it, because for now it's the best solution to avoid soft bodies to go through concave shapes (they use small spheres). This might be replaced with a better algorithm for soft bodies later. --- .../physics_3d/godot_collision_solver_3d.cpp | 6 +- .../godot_collision_solver_3d_sat.cpp | 79 ++++++++++++++++--- servers/physics_3d/godot_shape_3d.cpp | 8 +- servers/physics_3d/godot_shape_3d.h | 7 +- 4 files changed, 78 insertions(+), 22 deletions(-) diff --git a/servers/physics_3d/godot_collision_solver_3d.cpp b/servers/physics_3d/godot_collision_solver_3d.cpp index 686f3e4d594..540b16c6e3d 100644 --- a/servers/physics_3d/godot_collision_solver_3d.cpp +++ b/servers/physics_3d/godot_collision_solver_3d.cpp @@ -264,7 +264,7 @@ bool GodotCollisionSolver3D::solve_soft_body(const GodotShape3D *p_shape_A, cons local_aabb.size[i] = smax - smin; } - concave_shape_A->cull(local_aabb, soft_body_concave_callback, &query_cinfo); + concave_shape_A->cull(local_aabb, soft_body_concave_callback, &query_cinfo, true); } else { AABB shape_aabb = p_transform_A.xform(p_shape_A->get_aabb()); shape_aabb.grow_by(collision_margin); @@ -346,7 +346,7 @@ bool GodotCollisionSolver3D::solve_concave(const GodotShape3D *p_shape_A, const local_aabb.size[i] = smax - smin; } - concave_B->cull(local_aabb, concave_callback, &cinfo); + concave_B->cull(local_aabb, concave_callback, &cinfo, false); return cinfo.collided; } @@ -559,7 +559,7 @@ bool GodotCollisionSolver3D::solve_distance(const GodotShape3D *p_shape_A, const local_aabb.size[i] = smax - smin; } - concave_B->cull(local_aabb, concave_distance_callback, &cinfo); + concave_B->cull(local_aabb, concave_distance_callback, &cinfo, false); if (!cinfo.collided) { r_point_A = cinfo.close_A; r_point_B = cinfo.close_B; diff --git a/servers/physics_3d/godot_collision_solver_3d_sat.cpp b/servers/physics_3d/godot_collision_solver_3d_sat.cpp index 0790333f65c..4faa07b6c9b 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(); } diff --git a/servers/physics_3d/godot_shape_3d.cpp b/servers/physics_3d/godot_shape_3d.cpp index d1e919ab6a5..5364a9833d5 100644 --- a/servers/physics_3d/godot_shape_3d.cpp +++ b/servers/physics_3d/godot_shape_3d.cpp @@ -1401,7 +1401,7 @@ bool GodotConcavePolygonShape3D::_cull(int p_idx, _CullParams *p_params) const { return false; } -void GodotConcavePolygonShape3D::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const { +void GodotConcavePolygonShape3D::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const { // make matrix local to concave if (faces.size() == 0) { return; @@ -1416,6 +1416,7 @@ void GodotConcavePolygonShape3D::cull(const AABB &p_local_aabb, QueryCallback p_ GodotFaceShape3D face; // use this to send in the callback face.backface_collision = backface_collision; + face.invert_backface_collision = p_invert_backface_collision; _CullParams params; params.aabb = local_aabb; @@ -1961,7 +1962,7 @@ void GodotHeightMapShape3D::_get_cell(const Vector3 &p_point, int &r_x, int &r_y r_z = (clamped_point.z < 0.0) ? (clamped_point.z - 0.5) : (clamped_point.z + 0.5); } -void GodotHeightMapShape3D::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const { +void GodotHeightMapShape3D::cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const { if (heights.is_empty()) { return; } @@ -1988,7 +1989,8 @@ void GodotHeightMapShape3D::cull(const AABB &p_local_aabb, QueryCallback p_callb int end_z = MIN(depth - 1, aabb_max[2]); GodotFaceShape3D face; - face.backface_collision = true; + face.backface_collision = !p_invert_backface_collision; + face.invert_backface_collision = p_invert_backface_collision; for (int z = start_z; z < end_z; z++) { for (int x = start_x; x < end_x; x++) { diff --git a/servers/physics_3d/godot_shape_3d.h b/servers/physics_3d/godot_shape_3d.h index 7a32b69166e..43319510d41 100644 --- a/servers/physics_3d/godot_shape_3d.h +++ b/servers/physics_3d/godot_shape_3d.h @@ -107,7 +107,7 @@ public: // Returns true to stop the query. typedef bool (*QueryCallback)(void *p_userdata, GodotShape3D *p_convex); - virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const = 0; + virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const = 0; GodotConcaveShape3D() {} }; @@ -370,7 +370,7 @@ public: virtual bool intersect_point(const Vector3 &p_point) const override; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override; + virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const override; virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; @@ -433,7 +433,7 @@ public: virtual bool intersect_point(const Vector3 &p_point) const override; virtual Vector3 get_closest_point_to(const Vector3 &p_point) const override; - virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata) const override; + virtual void cull(const AABB &p_local_aabb, QueryCallback p_callback, void *p_userdata, bool p_invert_backface_collision) const override; virtual Vector3 get_moment_of_inertia(real_t p_mass) const override; @@ -448,6 +448,7 @@ struct GodotFaceShape3D : public GodotShape3D { Vector3 normal; //cache Vector3 vertex[3]; bool backface_collision = false; + bool invert_backface_collision = false; virtual PhysicsServer3D::ShapeType get_type() const override { return PhysicsServer3D::SHAPE_CONCAVE_POLYGON; }