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.

3 files changed, 68 insertions, 35 deletions

diff --git a/servers/physics_3d/collision_solver_3d_sat.cpp b/servers/physics_3d/collision_solver_3d_sat.cpp
index 651961433c..5c77fa2847 100644
--- a/servers/physics_3d/collision_solver_3d_sat.cpp
+++ b/servers/physics_3d/collision_solver_3d_sat.cpp
@@ -1630,7 +1630,7 @@ static void _collision_capsule_cylinder(const Shape3DSW *p_a, const Transform &p
 	CollisionSolver3DSW::CallbackResult callback = SeparatorAxisTest<CapsuleShape3DSW, CylinderShape3DSW, withMargin>::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;
 	}
 
@@ -1805,7 +1805,7 @@ static void _collision_cylinder_cylinder(const Shape3DSW *p_a, const Transform &
 	CollisionSolver3DSW::CallbackResult callback = SeparatorAxisTest<CylinderShape3DSW, CylinderShape3DSW, withMargin>::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;
 	}
 
@@ -1822,7 +1822,7 @@ static void _collision_cylinder_convex_polygon(const Shape3DSW *p_a, const Trans
 	CollisionSolver3DSW::CallbackResult callback = SeparatorAxisTest<CylinderShape3DSW, ConvexPolygonShape3DSW, withMargin>::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_3d/gjk_epa.cpp b/servers/physics_3d/gjk_epa.cpp
index aa7c11eec5..e44c92da79 100644
--- a/servers/physics_3d/gjk_epa.cpp
+++ b/servers/physics_3d/gjk_epa.cpp
@@ -110,26 +110,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 Shape3DSW*, const Vector3&, real_t);
+
+	void Initialize(const Shape3DSW* shape0, const Transform& wtrs0, const real_t margin0,
+		const Shape3DSW* 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 Shape3DSW* 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 Shape3DSW* 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 ) );
+	_FORCE_INLINE_ Vector3 Support(const Vector3& d, U index) const {
+		if (index) {
+			return Support1(d);
 		} else {
-			return ( Support0 ( d ) );
-}
+			return Support0(d);
+		}
 	}
 };
 
@@ -828,22 +862,17 @@ struct	GJK
 	};
 
 	//
-	static void	Initialize(	const Shape3DSW* shape0,const Transform& wtrs0,
-		const Shape3DSW* shape1,const Transform& wtrs1,
+	static void	Initialize(	const Shape3DSW* shape0, const Transform& wtrs0, real_t margin0,
+		const Shape3DSW* 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);
 	}
 
 
@@ -857,13 +886,15 @@ struct	GJK
 //
 bool Distance(	const Shape3DSW*	shape0,
 									  const Transform&		wtrs0,
-									  const Shape3DSW*	shape1,
+									  real_t				margin0,
+									  const Shape3DSW*		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)
@@ -896,14 +927,16 @@ bool Distance(	const Shape3DSW*	shape0,
 //
 bool Penetration(	const Shape3DSW*	shape0,
 									 const Transform&		wtrs0,
-									 const Shape3DSW*	shape1,
+									 real_t					margin0,
+									 const Shape3DSW*		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)
@@ -963,7 +996,7 @@ bool Penetration(	const Shape3DSW*	shape0,
 bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B) {
 	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];
 		return true;
@@ -972,10 +1005,10 @@ bool gjk_epa_calculate_distance(const Shape3DSW *p_shape_A, const Transform &p_t
 	return false;
 }
 
-bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap) {
+bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CollisionSolver3DSW::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_3d/gjk_epa.h b/servers/physics_3d/gjk_epa.h
index be3ba4e664..a7e2e1719e 100644
--- a/servers/physics_3d/gjk_epa.h
+++ b/servers/physics_3d/gjk_epa.h
@@ -34,7 +34,7 @@
 #include "collision_solver_3d_sw.h"
 #include "shape_3d_sw.h"
 
-bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CollisionSolver3DSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap = false);
+bool gjk_epa_calculate_penetration(const Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, CollisionSolver3DSW::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 Shape3DSW *p_shape_A, const Transform &p_transform_A, const Shape3DSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B);
 
 #endif