Cylinder support in Godot Physics 3D

Cylinder collision detection uses a mix of SAT and GJKEPA.
GJKEPA is used to find the best separation axis in cases where finding
it analytically is too complex.

Changes in SAT solver:
Added support for generating separation axes for cylinder shape.
Added support for generating contact points with circle feature.

Changes in GJKEPA solver:
Updated from latest Bullet version which includes EPA fixes in some
scenarios.
Setting a lower EPA_ACCURACY to fix accuracy problems with cylinder vs.
cylinder in some cases.

1 files changed, 206 insertions, 8 deletions

diff --git a/servers/physics_3d/shape_3d_sw.cpp b/servers/physics_3d/shape_3d_sw.cpp
index 9c37060bea..5bac4f19b9 100644
--- a/servers/physics_3d/shape_3d_sw.cpp
+++ b/servers/physics_3d/shape_3d_sw.cpp
@@ -34,10 +34,12 @@
 #include "core/math/quick_hull.h"
 #include "core/templates/sort_array.h"
 
-#define _POINT_SNAP 0.001953125
 #define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.0002
 #define _FACE_IS_VALID_SUPPORT_THRESHOLD 0.9998
 
+#define _CYLINDER_EDGE_IS_VALID_SUPPORT_THRESHOLD 0.002
+#define _CYLINDER_FACE_IS_VALID_SUPPORT_THRESHOLD 0.999
+
 void Shape3DSW::configure(const AABB &p_aabb) {
 	aabb = p_aabb;
 	configured = true;
@@ -50,7 +52,8 @@ void Shape3DSW::configure(const AABB &p_aabb) {
 Vector3 Shape3DSW::get_support(const Vector3 &p_normal) const {
 	Vector3 res;
 	int amnt;
-	get_supports(p_normal, 1, &res, amnt);
+	FeatureType type;
+	get_supports(p_normal, 1, &res, amnt, type);
 	return res;
 }
 
@@ -167,16 +170,19 @@ Vector3 RayShape3DSW::get_support(const Vector3 &p_normal) const {
 	}
 }
 
-void RayShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
+void RayShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
 	if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) {
 		r_amount = 2;
+		r_type = FEATURE_EDGE;
 		r_supports[0] = Vector3(0, 0, 0);
 		r_supports[1] = Vector3(0, 0, length);
 	} else if (p_normal.z > 0) {
 		r_amount = 1;
+		r_type = FEATURE_POINT;
 		*r_supports = Vector3(0, 0, length);
 	} else {
 		r_amount = 1;
+		r_type = FEATURE_POINT;
 		*r_supports = Vector3(0, 0, 0);
 	}
 }
@@ -246,9 +252,10 @@ Vector3 SphereShape3DSW::get_support(const Vector3 &p_normal) const {
 	return p_normal * radius;
 }
 
-void SphereShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
+void SphereShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
 	*r_supports = p_normal * radius;
 	r_amount = 1;
+	r_type = FEATURE_POINT;
 }
 
 bool SphereShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
@@ -312,7 +319,7 @@ Vector3 BoxShape3DSW::get_support(const Vector3 &p_normal) const {
 	return point;
 }
 
-void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
+void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
 	static const int next[3] = { 1, 2, 0 };
 	static const int next2[3] = { 2, 0, 1 };
 
@@ -325,6 +332,7 @@ void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_s
 
 			bool neg = dot < 0;
 			r_amount = 4;
+			r_type = FEATURE_FACE;
 
 			Vector3 point;
 			point[i] = half_extents[i];
@@ -362,6 +370,7 @@ void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_s
 
 		if (Math::abs(p_normal.dot(axis)) < _EDGE_IS_VALID_SUPPORT_THRESHOLD) {
 			r_amount = 2;
+			r_type = FEATURE_EDGE;
 
 			int i_n = next[i];
 			int i_n2 = next2[i];
@@ -389,6 +398,7 @@ void BoxShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_s
 			(p_normal.z < 0) ? -half_extents.z : half_extents.z);
 
 	r_amount = 1;
+	r_type = FEATURE_POINT;
 	r_supports[0] = point;
 }
 
@@ -500,7 +510,7 @@ Vector3 CapsuleShape3DSW::get_support(const Vector3 &p_normal) const {
 	return n;
 }
 
-void CapsuleShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
+void CapsuleShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
 	Vector3 n = p_normal;
 
 	real_t d = n.z;
@@ -512,6 +522,7 @@ void CapsuleShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3
 		n *= radius;
 
 		r_amount = 2;
+		r_type = FEATURE_EDGE;
 		r_supports[0] = n;
 		r_supports[0].z += height * 0.5;
 		r_supports[1] = n;
@@ -523,6 +534,7 @@ void CapsuleShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3
 		n *= radius;
 		n.z += h * 0.5;
 		r_amount = 1;
+		r_type = FEATURE_POINT;
 		*r_supports = n;
 	}
 }
@@ -642,6 +654,186 @@ CapsuleShape3DSW::CapsuleShape3DSW() {
 	height = radius = 0;
 }
 
+/********** CYLINDER *************/
+
+void CylinderShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
+	Vector3 cylinder_axis = p_transform.basis.get_axis(1).normalized();
+	real_t axis_dot = cylinder_axis.dot(p_normal);
+
+	Vector3 local_normal = p_transform.basis.xform_inv(p_normal);
+	real_t scale = local_normal.length();
+	real_t scaled_radius = radius * scale;
+	real_t scaled_height = height * scale;
+
+	real_t length;
+	if (Math::abs(axis_dot) > 1.0) {
+		length = scaled_height * 0.5;
+	} else {
+		length = Math::abs(axis_dot * scaled_height * 0.5) + scaled_radius * Math::sqrt(1.0 - axis_dot * axis_dot);
+	}
+
+	real_t distance = p_normal.dot(p_transform.origin);
+
+	r_min = distance - length;
+	r_max = distance + length;
+}
+
+Vector3 CylinderShape3DSW::get_support(const Vector3 &p_normal) const {
+	Vector3 n = p_normal;
+	real_t h = (n.y > 0) ? height : -height;
+	real_t s = Math::sqrt(n.x * n.x + n.z * n.z);
+	if (Math::is_zero_approx(s)) {
+		n.x = radius;
+		n.y = h * 0.5;
+		n.z = 0.0;
+	} else {
+		real_t d = radius / s;
+		n.x = n.x * d;
+		n.y = h * 0.5;
+		n.z = n.z * d;
+	}
+
+	return n;
+}
+
+void CylinderShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
+	real_t d = p_normal.y;
+	if (Math::abs(d) > _CYLINDER_FACE_IS_VALID_SUPPORT_THRESHOLD) {
+		real_t h = (d > 0) ? height : -height;
+
+		Vector3 n = p_normal;
+		n.x = 0.0;
+		n.z = 0.0;
+		n.y = h * 0.5;
+
+		r_amount = 3;
+		r_type = FEATURE_CIRCLE;
+		r_supports[0] = n;
+		r_supports[1] = n;
+		r_supports[1].x += radius;
+		r_supports[2] = n;
+		r_supports[2].z += radius;
+	} else if (Math::abs(d) < _CYLINDER_EDGE_IS_VALID_SUPPORT_THRESHOLD) {
+		// make it flat
+		Vector3 n = p_normal;
+		n.y = 0.0;
+		n.normalize();
+		n *= radius;
+
+		r_amount = 2;
+		r_type = FEATURE_EDGE;
+		r_supports[0] = n;
+		r_supports[0].y += height * 0.5;
+		r_supports[1] = n;
+		r_supports[1].y -= height * 0.5;
+	} else {
+		r_amount = 1;
+		r_type = FEATURE_POINT;
+		r_supports[0] = get_support(p_normal);
+		return;
+
+		Vector3 n = p_normal;
+		real_t h = n.y * Math::sqrt(0.25 * height * height + radius * radius);
+		if (Math::abs(h) > 1.0) {
+			// Top or bottom surface.
+			n.y = (n.y > 0.0) ? height * 0.5 : -height * 0.5;
+		} else {
+			// Lateral surface.
+			n.y = height * 0.5 * h;
+		}
+
+		real_t s = Math::sqrt(n.x * n.x + n.z * n.z);
+		if (Math::is_zero_approx(s)) {
+			n.x = 0.0;
+			n.z = 0.0;
+		} else {
+			real_t scaled_radius = radius / s;
+			n.x = n.x * scaled_radius;
+			n.z = n.z * scaled_radius;
+		}
+
+		r_supports[0] = n;
+	}
+}
+
+bool CylinderShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
+	return Geometry3D::segment_intersects_cylinder(p_begin, p_end, height, radius, &r_result, &r_normal, 1);
+}
+
+bool CylinderShape3DSW::intersect_point(const Vector3 &p_point) const {
+	if (Math::abs(p_point.y) < height * 0.5) {
+		return Vector3(p_point.x, 0, p_point.z).length() < radius;
+	}
+	return false;
+}
+
+Vector3 CylinderShape3DSW::get_closest_point_to(const Vector3 &p_point) const {
+	if (Math::absf(p_point.y) > height * 0.5) {
+		// Project point to top disk.
+		real_t dir = p_point.y > 0.0 ? 1.0 : -1.0;
+		Vector3 circle_pos(0.0, dir * height * 0.5, 0.0);
+		Plane circle_plane(circle_pos, Vector3(0.0, dir, 0.0));
+		Vector3 proj_point = circle_plane.project(p_point);
+
+		// Clip position.
+		Vector3 delta_point_1 = proj_point - circle_pos;
+		real_t dist_point_1 = delta_point_1.length_squared();
+		if (!Math::is_zero_approx(dist_point_1)) {
+			dist_point_1 = Math::sqrt(dist_point_1);
+			proj_point = circle_pos + delta_point_1 * MIN(dist_point_1, radius) / dist_point_1;
+		}
+
+		return proj_point;
+	} else {
+		Vector3 s[2] = {
+			Vector3(0, -height * 0.5, 0),
+			Vector3(0, height * 0.5, 0),
+		};
+
+		Vector3 p = Geometry3D::get_closest_point_to_segment(p_point, s);
+
+		if (p.distance_to(p_point) < radius) {
+			return p_point;
+		}
+
+		return p + (p_point - p).normalized() * radius;
+	}
+}
+
+Vector3 CylinderShape3DSW::get_moment_of_inertia(real_t p_mass) const {
+	// use bad AABB approximation
+	Vector3 extents = get_aabb().size * 0.5;
+
+	return Vector3(
+			(p_mass / 3.0) * (extents.y * extents.y + extents.z * extents.z),
+			(p_mass / 3.0) * (extents.x * extents.x + extents.z * extents.z),
+			(p_mass / 3.0) * (extents.y * extents.y + extents.y * extents.y));
+}
+
+void CylinderShape3DSW::_setup(real_t p_height, real_t p_radius) {
+	height = p_height;
+	radius = p_radius;
+	configure(AABB(Vector3(-radius, -height * 0.5, -radius), Vector3(radius * 2.0, height, radius * 2.0)));
+}
+
+void CylinderShape3DSW::set_data(const Variant &p_data) {
+	Dictionary d = p_data;
+	ERR_FAIL_COND(!d.has("radius"));
+	ERR_FAIL_COND(!d.has("height"));
+	_setup(d["height"], d["radius"]);
+}
+
+Variant CylinderShape3DSW::get_data() const {
+	Dictionary d;
+	d["radius"] = radius;
+	d["height"] = height;
+	return d;
+}
+
+CylinderShape3DSW::CylinderShape3DSW() {
+	height = radius = 0;
+}
+
 /********** CONVEX POLYGON *************/
 
 void ConvexPolygonShape3DSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
@@ -689,7 +881,7 @@ Vector3 ConvexPolygonShape3DSW::get_support(const Vector3 &p_normal) const {
 	return vrts[vert_support_idx];
 }
 
-void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
+void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
 	const Geometry3D::MeshData::Face *faces = mesh.faces.ptr();
 	int fc = mesh.faces.size();
 
@@ -734,6 +926,7 @@ void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Ve
 				r_supports[j] = vertices[ind[j]];
 			}
 			r_amount = m;
+			r_type = FEATURE_FACE;
 			return;
 		}
 	}
@@ -743,6 +936,7 @@ void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Ve
 		dot = ABS(dot);
 		if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD && (edges[i].a == vtx || edges[i].b == vtx)) {
 			r_amount = 2;
+			r_type = FEATURE_EDGE;
 			r_supports[0] = vertices[edges[i].a];
 			r_supports[1] = vertices[edges[i].b];
 			return;
@@ -751,6 +945,7 @@ void ConvexPolygonShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Ve
 
 	r_supports[0] = vertices[vtx];
 	r_amount = 1;
+	r_type = FEATURE_POINT;
 }
 
 bool ConvexPolygonShape3DSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
@@ -935,12 +1130,13 @@ Vector3 FaceShape3DSW::get_support(const Vector3 &p_normal) const {
 	return vertex[vert_support_idx];
 }
 
-void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
+void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
 	Vector3 n = p_normal;
 
 	/** TEST FACE AS SUPPORT **/
 	if (normal.dot(n) > _FACE_IS_VALID_SUPPORT_THRESHOLD) {
 		r_amount = 3;
+		r_type = FEATURE_FACE;
 		for (int i = 0; i < 3; i++) {
 			r_supports[i] = vertex[i];
 		}
@@ -974,6 +1170,7 @@ void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_
 		dot = ABS(dot);
 		if (dot < _EDGE_IS_VALID_SUPPORT_THRESHOLD) {
 			r_amount = 2;
+			r_type = FEATURE_EDGE;
 			r_supports[0] = vertex[i];
 			r_supports[1] = vertex[nx];
 			return;
@@ -981,6 +1178,7 @@ void FaceShape3DSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_
 	}
 
 	r_amount = 1;
+	r_type = FEATURE_POINT;
 	r_supports[0] = vertex[vert_support_idx];
 }