From abb5660925a1e4109f2696effcba0e9f2b702cbb Mon Sep 17 00:00:00 2001 From: PouleyKetchoupp Date: Tue, 16 Mar 2021 09:48:20 -0700 Subject: [PATCH] Fix Cylinder shape collision with margins when using GJK-EPA In the case of falling back to GJK-EPA algorithm to generate cylinder contact points, margins were never taken into account. This fixes the depenetration phase for kinematic bodies and allows consistent floor detection for cylinder shapes. --- servers/physics/collision_solver_sat.cpp | 6 +- servers/physics/gjk_epa.cpp | 95 ++++++++++++++++-------- servers/physics/gjk_epa.h | 2 +- 3 files changed, 69 insertions(+), 34 deletions(-) diff --git a/servers/physics/collision_solver_sat.cpp b/servers/physics/collision_solver_sat.cpp index cb74b208157..28286ae0552 100644 --- a/servers/physics/collision_solver_sat.cpp +++ b/servers/physics/collision_solver_sat.cpp @@ -1635,7 +1635,7 @@ static void _collision_capsule_cylinder(const ShapeSW *p_a, const Transform &p_t CollisionSolverSW::CallbackResult callback = SeparatorAxisTest::test_contact_points; // Fallback to generic algorithm to find the best separating axis. - if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator)) { + if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) { return; } @@ -1809,7 +1809,7 @@ static void _collision_cylinder_cylinder(const ShapeSW *p_a, const Transform &p_ CollisionSolverSW::CallbackResult callback = SeparatorAxisTest::test_contact_points; // Fallback to generic algorithm to find the best separating axis. - if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator)) { + if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) { return; } @@ -1826,7 +1826,7 @@ static void _collision_cylinder_convex_polygon(const ShapeSW *p_a, const Transfo CollisionSolverSW::CallbackResult callback = SeparatorAxisTest::test_contact_points; // Fallback to generic algorithm to find the best separating axis. - if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator)) { + if (!fallback_collision_solver(p_a, p_transform_a, p_b, p_transform_b, callback, &separator, false, p_margin_a, p_margin_b)) { return; } diff --git a/servers/physics/gjk_epa.cpp b/servers/physics/gjk_epa.cpp index 8d0c2416a74..7617563f4da 100644 --- a/servers/physics/gjk_epa.cpp +++ b/servers/physics/gjk_epa.cpp @@ -111,25 +111,60 @@ struct MinkowskiDiff { Transform transform_A; Transform transform_B; + real_t margin_A = 0.0; + real_t margin_B = 0.0; + + Vector3 (*get_support)(const ShapeSW*, const Vector3&, real_t); + + void Initialize(const ShapeSW* shape0, const Transform& wtrs0, const real_t margin0, + const ShapeSW* shape1, const Transform& wtrs1, const real_t margin1) { + m_shapes[0] = shape0; + m_shapes[1] = shape1; + transform_A = wtrs0; + transform_B = wtrs1; + margin_A = margin0; + margin_B = margin1; + + if ((margin0 > 0.0) || (margin1 > 0.0)) { + get_support = get_support_with_margin; + } else { + get_support = get_support_without_margin; + } + } + + static Vector3 get_support_without_margin(const ShapeSW* p_shape, const Vector3& p_dir, real_t p_margin) { + return p_shape->get_support(p_dir.normalized()); + } + + static Vector3 get_support_with_margin(const ShapeSW* p_shape, const Vector3& p_dir, real_t p_margin) { + Vector3 local_dir_norm = p_dir; + if (local_dir_norm.length_squared() < CMP_EPSILON2) { + local_dir_norm = Vector3(-1.0, -1.0, -1.0); + } + local_dir_norm.normalize(); + + return p_shape->get_support(local_dir_norm) + p_margin * local_dir_norm; + } + // i wonder how this could be sped up... if it can - _FORCE_INLINE_ Vector3 Support0 ( const Vector3& d ) const { - return transform_A.xform( m_shapes[0]->get_support( transform_A.basis.xform_inv(d).normalized() ) ); + _FORCE_INLINE_ Vector3 Support0(const Vector3& d) const { + return transform_A.xform(get_support(m_shapes[0], transform_A.basis.xform_inv(d), margin_A)); } - _FORCE_INLINE_ Vector3 Support1 ( const Vector3& d ) const { - return transform_B.xform( m_shapes[1]->get_support( transform_B.basis.xform_inv(d).normalized() ) ); + _FORCE_INLINE_ Vector3 Support1(const Vector3& d) const { + return transform_B.xform(get_support(m_shapes[1], transform_B.basis.xform_inv(d), margin_B)); } - _FORCE_INLINE_ Vector3 Support ( const Vector3& d ) const { - return ( Support0 ( d )-Support1 ( -d ) ); + _FORCE_INLINE_ Vector3 Support (const Vector3& d) const { + return (Support0(d) - Support1(-d)); } - _FORCE_INLINE_ Vector3 Support ( const Vector3& d,U index ) const - { - if ( index ) - return ( Support1 ( d ) ); - else - return ( Support0 ( d ) ); + _FORCE_INLINE_ Vector3 Support(const Vector3& d, U index) const { + if (index) { + return Support1(d); + } else { + return Support0(d); + } } }; @@ -822,21 +857,17 @@ struct GJK }; // - static void Initialize( const ShapeSW* shape0,const Transform& wtrs0, - const ShapeSW* shape1,const Transform& wtrs1, + static void Initialize( const ShapeSW* shape0, const Transform& wtrs0, real_t margin0, + const ShapeSW* shape1, const Transform& wtrs1, real_t margin1, sResults& results, - tShape& shape, - bool withmargins) + tShape& shape) { /* Results */ - results.witnesses[0] = - results.witnesses[1] = Vector3(0,0,0); + results.witnesses[0] = Vector3(0,0,0); + results.witnesses[1] = Vector3(0,0,0); results.status = sResults::Separated; /* Shape */ - shape.m_shapes[0] = shape0; - shape.m_shapes[1] = shape1; - shape.transform_A = wtrs0; - shape.transform_B = wtrs1; + shape.Initialize(shape0, wtrs0, margin0, shape1, wtrs1, margin1); } @@ -851,13 +882,15 @@ struct GJK // bool Distance( const ShapeSW* shape0, const Transform& wtrs0, - const ShapeSW* shape1, + real_t margin0, + const ShapeSW* shape1, const Transform& wtrs1, + real_t margin1, const Vector3& guess, sResults& results) { tShape shape; - Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false); + Initialize(shape0, wtrs0, margin0, shape1, wtrs1, margin1, results, shape); GJK gjk; GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess); if(gjk_status==GJK::eStatus::Valid) @@ -889,14 +922,16 @@ bool Distance( const ShapeSW* shape0, // bool Penetration( const ShapeSW* shape0, const Transform& wtrs0, - const ShapeSW* shape1, + real_t margin0, + const ShapeSW* shape1, const Transform& wtrs1, - const Vector3& guess, + real_t margin1, + const Vector3& guess, sResults& results ) { tShape shape; - Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false); + Initialize(shape0, wtrs0, margin0, shape1, wtrs1, margin1, results, shape); GJK gjk; GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess); switch(gjk_status) @@ -957,7 +992,7 @@ bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform &p_tra GjkEpa2::sResults res; - if (GjkEpa2::Distance(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_transform_B.origin - p_transform_A.origin, res)) { + if (GjkEpa2::Distance(p_shape_A, p_transform_A, 0.0, p_shape_B, p_transform_B, 0.0, p_transform_B.origin - p_transform_A.origin, res)) { r_result_A = res.witnesses[0]; r_result_B = res.witnesses[1]; @@ -967,11 +1002,11 @@ bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform &p_tra return false; } -bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap) { +bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap, real_t p_margin_A, real_t p_margin_B) { GjkEpa2::sResults res; - if (GjkEpa2::Penetration(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_transform_B.origin - p_transform_A.origin, res)) { + if (GjkEpa2::Penetration(p_shape_A, p_transform_A, p_margin_A, p_shape_B, p_transform_B, p_margin_B, p_transform_B.origin - p_transform_A.origin, res)) { if (p_result_callback) { if (p_swap) p_result_callback(res.witnesses[1], res.witnesses[0], p_userdata); diff --git a/servers/physics/gjk_epa.h b/servers/physics/gjk_epa.h index 679dab54c3e..4c672d78f07 100644 --- a/servers/physics/gjk_epa.h +++ b/servers/physics/gjk_epa.h @@ -34,7 +34,7 @@ #include "collision_solver_sw.h" #include "shape_sw.h" -bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false); +bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false, real_t p_margin_A = 0.0, real_t p_margin_B = 0.0); bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B); #endif