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.

8 files changed, 1078 insertions, 83 deletions

diff --git a/COPYRIGHT.txt b/COPYRIGHT.txt
index b9b15abf5d..ff24d18e6f 100644
--- a/COPYRIGHT.txt
+++ b/COPYRIGHT.txt
@@ -72,6 +72,11 @@ Copyright: 2008-2016, The Android Open Source Project
  2002, Google, Inc.
 License: Apache-2.0
 
+Files: ./servers/physics_3d/collision_solver_3d_sat.cpp
+Comment: Open Dynamics Engine
+Copyright: 2001-2003, Russell L. Smith, Alen Ladavac, Nguyen Binh
+License: BSD-3-clause
+
 Files: ./servers/physics_3d/gjk_epa.cpp
  ./servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp
  ./servers/physics_3d/joints/generic_6dof_joint_3d_sw.h
diff --git a/core/math/geometry_3d.h b/core/math/geometry_3d.h
index 825817af5e..4ef9b4dbe6 100644
--- a/core/math/geometry_3d.h
+++ b/core/math/geometry_3d.h
@@ -252,27 +252,34 @@ public:
 		return true;
 	}
 
-	static inline bool segment_intersects_cylinder(const Vector3 &p_from, const Vector3 &p_to, real_t p_height, real_t p_radius, Vector3 *r_res = nullptr, Vector3 *r_norm = nullptr) {
+	static inline bool segment_intersects_cylinder(const Vector3 &p_from, const Vector3 &p_to, real_t p_height, real_t p_radius, Vector3 *r_res = nullptr, Vector3 *r_norm = nullptr, int p_cylinder_axis = 2) {
 		Vector3 rel = (p_to - p_from);
 		real_t rel_l = rel.length();
 		if (rel_l < CMP_EPSILON) {
 			return false; // Both points are the same.
 		}
 
+		ERR_FAIL_COND_V(p_cylinder_axis < 0, false);
+		ERR_FAIL_COND_V(p_cylinder_axis > 2, false);
+		Vector3 cylinder_axis;
+		cylinder_axis[p_cylinder_axis] = 1.0;
+
 		// First check if they are parallel.
 		Vector3 normal = (rel / rel_l);
-		Vector3 crs = normal.cross(Vector3(0, 0, 1));
+		Vector3 crs = normal.cross(cylinder_axis);
 		real_t crs_l = crs.length();
 
-		Vector3 z_dir;
+		Vector3 axis_dir;
 
 		if (crs_l < CMP_EPSILON) {
-			z_dir = Vector3(1, 0, 0); // Any x/y vector OK.
+			Vector3 side_axis;
+			side_axis[(p_cylinder_axis + 1) % 3] = 1.0; // Any side axis OK.
+			axis_dir = side_axis;
 		} else {
-			z_dir = crs / crs_l;
+			axis_dir = crs / crs_l;
 		}
 
-		real_t dist = z_dir.dot(p_from);
+		real_t dist = axis_dir.dot(p_from);
 
 		if (dist >= p_radius) {
 			return false; // Too far away.
@@ -285,10 +292,10 @@ public:
 		}
 		Size2 size(Math::sqrt(w2), p_height * 0.5);
 
-		Vector3 x_dir = z_dir.cross(Vector3(0, 0, 1)).normalized();
+		Vector3 side_dir = axis_dir.cross(cylinder_axis).normalized();
 
-		Vector2 from2D(x_dir.dot(p_from), p_from.z);
-		Vector2 to2D(x_dir.dot(p_to), p_to.z);
+		Vector2 from2D(side_dir.dot(p_from), p_from[p_cylinder_axis]);
+		Vector2 to2D(side_dir.dot(p_to), p_to[p_cylinder_axis]);
 
 		real_t min = 0, max = 1;
 
@@ -335,10 +342,12 @@ public:
 		Vector3 res_normal = result;
 
 		if (axis == 0) {
-			res_normal.z = 0;
+			res_normal[p_cylinder_axis] = 0;
 		} else {
-			res_normal.x = 0;
-			res_normal.y = 0;
+			int axis_side = (p_cylinder_axis + 1) % 3;
+			res_normal[axis_side] = 0;
+			axis_side = (axis_side + 1) % 3;
+			res_normal[axis_side] = 0;
 		}
 
 		res_normal.normalize();
diff --git a/servers/physics_3d/collision_solver_3d_sat.cpp b/servers/physics_3d/collision_solver_3d_sat.cpp
index b8e056f1f4..2a31cf1c22 100644
--- a/servers/physics_3d/collision_solver_3d_sat.cpp
+++ b/servers/physics_3d/collision_solver_3d_sat.cpp
@@ -31,7 +31,38 @@
 #include "collision_solver_3d_sat.h"
 #include "core/math/geometry_3d.h"
 
-#define _EDGE_IS_VALID_SUPPORT_THRESHOLD 0.02
+#include "gjk_epa.h"
+
+#define fallback_collision_solver gjk_epa_calculate_penetration
+
+// Cylinder SAT analytic methods for Cylinder-trimesh and Cylinder-box are based on ODE colliders.
+
+/*
+ *	Cylinder-trimesh and Cylinder-box colliders by Alen Ladavac
+ *   Ported to ODE by Nguyen Binh
+ */
+
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith.       *
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
+ *                                                                       *
+ * This library is free software; you can redistribute it and/or         *
+ * modify it under the terms of EITHER:                                  *
+ *   (1) The GNU Lesser General Public License as published by the Free  *
+ *       Software Foundation; either version 2.1 of the License, or (at  *
+ *       your option) any later version. The text of the GNU Lesser      *
+ *       General Public License is included with this library in the     *
+ *       file LICENSE.TXT.                                               *
+ *   (2) The BSD-style license that is included with this library in     *
+ *       the file LICENSE-BSD.TXT.                                       *
+ *                                                                       *
+ * This library is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
+ *                                                                       *
+ *************************************************************************/
 
 struct _CollectorCallback {
 	CollisionSolver3DSW::CallbackResult callback;
@@ -82,6 +113,36 @@ static void _generate_contacts_point_face(const Vector3 *p_points_A, int p_point
 	p_callback->call(*p_points_A, closest_B);
 }
 
+static void _generate_contacts_point_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
+#ifdef DEBUG_ENABLED
+	ERR_FAIL_COND(p_point_count_A != 1);
+	ERR_FAIL_COND(p_point_count_B != 3);
+#endif
+
+	const Vector3 &point_A = p_points_A[0];
+
+	const Vector3 &circle_B_pos = p_points_B[0];
+	Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos;
+	Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos;
+
+	real_t circle_B_radius = circle_B_line_1.length();
+	Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized();
+
+	// Project point onto Circle B plane.
+	Plane circle_plane(circle_B_pos, circle_B_normal);
+	Vector3 proj_point_A = circle_plane.project(point_A);
+
+	// Clip point.
+	Vector3 delta_point_1 = proj_point_A - circle_B_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_A = circle_B_pos + delta_point_1 * MIN(dist_point_1, circle_B_radius) / dist_point_1;
+	}
+
+	p_callback->call(point_A, proj_point_A);
+}
+
 static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
 #ifdef DEBUG_ENABLED
 	ERR_FAIL_COND(p_point_count_A != 2);
@@ -217,36 +278,319 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_
 	}
 }
 
-static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
+static void _generate_contacts_face_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
+#ifdef DEBUG_ENABLED
+	ERR_FAIL_COND(p_point_count_A < 2);
+	ERR_FAIL_COND(p_point_count_B != 3);
+#endif
+
+	const Vector3 &circle_B_pos = p_points_B[0];
+	Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos;
+	Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos;
+
+	real_t circle_B_radius = circle_B_line_1.length();
+	Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized();
+
+	Plane circle_plane(circle_B_pos, circle_B_normal);
+
+	bool edge = (p_point_count_A == 2);
+
+	static const int max_clip = 32;
+	Vector3 contact_points[max_clip];
+	int num_points = 0;
+
+	// Clip edges with circle.
+	for (int i = 0; i < p_point_count_A; i++) {
+		int i_n = (i + 1) % p_point_count_A;
+
+		// Project edge point in circle plane.
+		const Vector3 &edge_A_1 = p_points_A[i];
+		Vector3 proj_point_1 = circle_plane.project(edge_A_1);
+
+		Vector3 dist_vec = proj_point_1 - circle_B_pos;
+		real_t dist_sq = dist_vec.length_squared();
+
+		// Point 1 is inside disk, add as contact point.
+		if (dist_sq <= circle_B_radius * circle_B_radius) {
+			//p_callback->call(edge_A_1, proj_point_1);
+			ERR_FAIL_COND(num_points >= max_clip);
+			contact_points[num_points] = edge_A_1;
+			++num_points;
+		}
+		// No need to test point 2 now, as it will be part of the next edge.
+
+		if (edge && i > 0) {
+			// Done with testing the only two points.
+			break;
+		}
+
+		// Project edge point in circle plane.
+		const Vector3 &edge_A_2 = p_points_A[i_n];
+		Vector3 proj_point_2 = circle_plane.project(edge_A_2);
+
+		Vector3 line_vec = proj_point_2 - proj_point_1;
+		real_t line_length_sq = line_vec.length_squared();
+
+		// Create a quadratic formula of the form ax^2 + bx + c = 0
+		real_t a, b, c;
+
+		a = line_length_sq;
+		b = 2.0 * dist_vec.dot(line_vec);
+		c = dist_sq - circle_B_radius * circle_B_radius;
+
+		// Solve for t.
+		real_t sqrtterm = b * b - 4.0 * a * c;
+
+		// If the term we intend to square root is less than 0 then the answer won't be real,
+		// so the line doesn't intersect.
+		if (sqrtterm < 0) {
+			continue;
+		}
+
+		sqrtterm = Math::sqrt(sqrtterm);
+
+		Vector3 edge_dir = edge_A_2 - edge_A_1;
+
+		real_t fraction_1 = (-b - sqrtterm) / (2.0 * a);
+		if ((fraction_1 > 0.0) && (fraction_1 < 1.0)) {
+			//Vector3 intersection_1 = proj_point_1 + fraction_1 * line_vec;
+			Vector3 face_point_1 = edge_A_1 + fraction_1 * edge_dir;
+			//p_callback->call(face_point_1, intersection_1);
+			ERR_FAIL_COND(num_points >= max_clip);
+			contact_points[num_points] = face_point_1;
+			++num_points;
+		}
+
+		real_t fraction_2 = (-b + sqrtterm) / (2.0 * a);
+		if ((fraction_2 > 0.0) && (fraction_2 < 1.0) && !Math::is_equal_approx(fraction_1, fraction_2)) {
+			//Vector3 intersection_2 = proj_point_1 + fraction_2 * line_vec;
+			Vector3 face_point_2 = edge_A_1 + fraction_2 * edge_dir;
+			//p_callback->call(face_point_2, intersection_2);
+			ERR_FAIL_COND(num_points >= max_clip);
+			contact_points[num_points] = face_point_2;
+			++num_points;
+		}
+	}
+
+	// In case of a face, add extra contact points for proper support.
+	if (!edge) {
+		Plane plane_A(p_points_A[0], p_points_A[1], p_points_A[2]);
+
+		if (num_points < 3) {
+			if (num_points == 0) {
+				// Use 3 arbitrary equidistant points from the circle.
+				for (int i = 0; i < 3; ++i) {
+					Vector3 circle_point = circle_B_pos;
+					circle_point += circle_B_line_1 * Math::cos(2.0 * Math_PI * i / 3.0);
+					circle_point += circle_B_line_2 * Math::sin(2.0 * Math_PI * i / 3.0);
+
+					Vector3 face_point = plane_A.project(circle_point);
+
+					contact_points[num_points] = face_point;
+					++num_points;
+				}
+			} else if (num_points == 1) {
+				Vector3 line_center = circle_B_pos - contact_points[0];
+				Vector3 line_tangent = line_center.cross(plane_A.normal);
+
+				Vector3 dir = line_tangent.cross(plane_A.normal).normalized();
+				if (line_center.dot(dir) > 0.0) {
+					// Use 2 equidistant points on the circle inside the face.
+					line_center.normalize();
+					line_tangent.normalize();
+					for (int i = 0; i < 2; ++i) {
+						Vector3 circle_point = circle_B_pos;
+						circle_point -= line_center * circle_B_radius * Math::cos(2.0 * Math_PI * (i + 1) / 3.0);
+						circle_point += line_tangent * circle_B_radius * Math::sin(2.0 * Math_PI * (i + 1) / 3.0);
+
+						Vector3 face_point = plane_A.project(circle_point);
+
+						contact_points[num_points] = face_point;
+						++num_points;
+					}
+				}
+				// Otherwise the circle touches an edge from the outside, no extra contact point.
+			} else { // if (num_points == 2)
+				// Use equidistant 3rd point on the circle inside the face.
+				Vector3 contacts_line = contact_points[1] - contact_points[0];
+				Vector3 dir = contacts_line.cross(plane_A.normal).normalized();
+
+				Vector3 circle_point = contact_points[0] + 0.5 * contacts_line;
+				Vector3 line_center = (circle_B_pos - circle_point);
+
+				if (line_center.dot(dir) > 0.0) {
+					circle_point += dir * (line_center.length() + circle_B_radius);
+				} else {
+					circle_point += dir * (circle_B_radius - line_center.length());
+				}
+
+				Vector3 face_point = plane_A.project(circle_point);
+
+				contact_points[num_points] = face_point;
+				++num_points;
+			}
+		}
+	}
+
+	// Generate contact points.
+	for (int i = 0; i < num_points; i++) {
+		const Vector3 &contact_point_A = contact_points[i];
+
+		real_t d = circle_plane.distance_to(contact_point_A);
+		Vector3 closest_B = contact_point_A - circle_plane.normal * d;
+
+		if (p_callback->normal.dot(contact_point_A) >= p_callback->normal.dot(closest_B)) {
+			continue;
+		}
+
+		p_callback->call(contact_point_A, closest_B);
+	}
+}
+
+static void _generate_contacts_circle_circle(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
+#ifdef DEBUG_ENABLED
+	ERR_FAIL_COND(p_point_count_A != 3);
+	ERR_FAIL_COND(p_point_count_B != 3);
+#endif
+
+	const Vector3 &circle_A_pos = p_points_A[0];
+	Vector3 circle_A_line_1 = p_points_A[1] - circle_A_pos;
+	Vector3 circle_A_line_2 = p_points_A[2] - circle_A_pos;
+
+	real_t circle_A_radius = circle_A_line_1.length();
+	Vector3 circle_A_normal = circle_A_line_1.cross(circle_A_line_2).normalized();
+
+	const Vector3 &circle_B_pos = p_points_B[0];
+	Vector3 circle_B_line_1 = p_points_B[1] - circle_B_pos;
+	Vector3 circle_B_line_2 = p_points_B[2] - circle_B_pos;
+
+	real_t circle_B_radius = circle_B_line_1.length();
+	Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized();
+
+	static const int max_clip = 4;
+	Vector3 contact_points[max_clip];
+	int num_points = 0;
+
+	Vector3 centers_diff = circle_B_pos - circle_A_pos;
+	Vector3 norm_proj = circle_A_normal.dot(centers_diff) * circle_A_normal;
+	Vector3 comp_proj = centers_diff - norm_proj;
+	real_t proj_dist = comp_proj.length();
+	if (!Math::is_zero_approx(proj_dist)) {
+		comp_proj /= proj_dist;
+		if ((proj_dist > circle_A_radius - circle_B_radius) && (proj_dist > circle_B_radius - circle_A_radius)) {
+			// Circles are overlapping, use the 2 points of intersection as contacts.
+			real_t radius_a_sqr = circle_A_radius * circle_A_radius;
+			real_t radius_b_sqr = circle_B_radius * circle_B_radius;
+			real_t d_sqr = proj_dist * proj_dist;
+			real_t s = (1.0 + (radius_a_sqr - radius_b_sqr) / d_sqr) * 0.5;
+			real_t h = Math::sqrt(MAX(radius_a_sqr - d_sqr * s * s, 0.0));
+			Vector3 midpoint = circle_A_pos + s * comp_proj * proj_dist;
+			Vector3 h_vec = h * circle_A_normal.cross(comp_proj);
+
+			Vector3 point_A = midpoint + h_vec;
+			contact_points[num_points] = point_A;
+			++num_points;
+
+			point_A = midpoint - h_vec;
+			contact_points[num_points] = point_A;
+			++num_points;
+
+			// Add 2 points from circle A and B along the line between the centers.
+			point_A = circle_A_pos + comp_proj * circle_A_radius;
+			contact_points[num_points] = point_A;
+			++num_points;
+
+			point_A = circle_B_pos - comp_proj * circle_B_radius - norm_proj;
+			contact_points[num_points] = point_A;
+			++num_points;
+		} // Otherwise one circle is inside the other one, use 3 arbitrary equidistant points.
+	} // Otherwise circles are concentric, use 3 arbitrary equidistant points.
+
+	if (num_points == 0) {
+		// Generate equidistant points.
+		if (circle_A_radius < circle_B_radius) {
+			// Circle A inside circle B.
+			for (int i = 0; i < 3; ++i) {
+				Vector3 circle_A_point = circle_A_pos;
+				circle_A_point += circle_A_line_1 * Math::cos(2.0 * Math_PI * i / 3.0);
+				circle_A_point += circle_A_line_2 * Math::sin(2.0 * Math_PI * i / 3.0);
+
+				contact_points[num_points] = circle_A_point;
+				++num_points;
+			}
+		} else {
+			// Circle B inside circle A.
+			for (int i = 0; i < 3; ++i) {
+				Vector3 circle_B_point = circle_B_pos;
+				circle_B_point += circle_B_line_1 * Math::cos(2.0 * Math_PI * i / 3.0);
+				circle_B_point += circle_B_line_2 * Math::sin(2.0 * Math_PI * i / 3.0);
+
+				Vector3 circle_A_point = circle_B_point - norm_proj;
+
+				contact_points[num_points] = circle_A_point;
+				++num_points;
+			}
+		}
+	}
+
+	Plane circle_B_plane(circle_B_pos, circle_B_normal);
+
+	// Generate contact points.
+	for (int i = 0; i < num_points; i++) {
+		const Vector3 &contact_point_A = contact_points[i];
+
+		real_t d = circle_B_plane.distance_to(contact_point_A);
+		Vector3 closest_B = contact_point_A - circle_B_plane.normal * d;
+
+		if (p_callback->normal.dot(contact_point_A) >= p_callback->normal.dot(closest_B)) {
+			continue;
+		}
+
+		p_callback->call(contact_point_A, closest_B);
+	}
+}
+
+static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_point_count_A, Shape3DSW::FeatureType p_feature_type_A, const Vector3 *p_points_B, int p_point_count_B, Shape3DSW::FeatureType p_feature_type_B, _CollectorCallback *p_callback) {
 #ifdef DEBUG_ENABLED
 	ERR_FAIL_COND(p_point_count_A < 1);
 	ERR_FAIL_COND(p_point_count_B < 1);
 #endif
 
-	static const GenerateContactsFunc generate_contacts_func_table[3][3] = {
+	static const GenerateContactsFunc generate_contacts_func_table[4][4] = {
 		{
 				_generate_contacts_point_point,
 				_generate_contacts_point_edge,
 				_generate_contacts_point_face,
+				_generate_contacts_point_circle,
 		},
 		{
 				nullptr,
 				_generate_contacts_edge_edge,
 				_generate_contacts_face_face,
+				_generate_contacts_face_circle,
 		},
 		{
 				nullptr,
 				nullptr,
 				_generate_contacts_face_face,
-		}
+				_generate_contacts_face_circle,
+		},
+		{
+				nullptr,
+				nullptr,
+				nullptr,
+				_generate_contacts_circle_circle,
+		},
 	};
 
 	int pointcount_B;
 	int pointcount_A;
 	const Vector3 *points_A;
 	const Vector3 *points_B;
+	int version_A;
+	int version_B;
 
-	if (p_point_count_A > p_point_count_B) {
+	if (p_feature_type_A > p_feature_type_B) {
 		//swap
 		p_callback->swap = !p_callback->swap;
 		p_callback->normal = -p_callback->normal;
@@ -255,16 +599,17 @@ static void _generate_contacts_from_supports(const Vector3 *p_points_A, int p_po
 		pointcount_A = p_point_count_B;
 		points_A = p_points_B;
 		points_B = p_points_A;
+		version_A = p_feature_type_B;
+		version_B = p_feature_type_A;
 	} else {
 		pointcount_B = p_point_count_B;
 		pointcount_A = p_point_count_A;
 		points_A = p_points_A;
 		points_B = p_points_B;
+		version_A = p_feature_type_A;
+		version_B = p_feature_type_B;
 	}
 
-	int version_A = (pointcount_A > 3 ? 3 : pointcount_A) - 1;
-	int version_B = (pointcount_B > 3 ? 3 : pointcount_B) - 1;
-
 	GenerateContactsFunc contacts_func = generate_contacts_func_table[version_A][version_B];
 	ERR_FAIL_COND(!contacts_func);
 	contacts_func(points_A, pointcount_A, points_B, pointcount_B, p_callback);
@@ -346,6 +691,17 @@ public:
 		return true;
 	}
 
+	static _FORCE_INLINE_ void test_contact_points(const Vector3 &p_point_A, const Vector3 &p_point_B, void *p_userdata) {
+		SeparatorAxisTest<ShapeA, ShapeB, withMargin> *separator = (SeparatorAxisTest<ShapeA, ShapeB, withMargin> *)p_userdata;
+		Vector3 axis = (p_point_B - p_point_A);
+		real_t depth = axis.length();
+
+		// Filter out bogus directions with a treshold and re-testing axis.
+		if (separator->best_depth - depth > 0.001) {
+			separator->test_axis(axis / depth);
+		}
+	}
+
 	_FORCE_INLINE_ void generate_contacts() {
 		// nothing to do, don't generate
 		if (best_axis == Vector3(0.0, 0.0, 0.0)) {
@@ -365,7 +721,8 @@ public:
 
 		Vector3 supports_A[max_supports];
 		int support_count_A;
-		shape_A->get_supports(transform_A->basis.xform_inv(-best_axis).normalized(), max_supports, supports_A, support_count_A);
+		Shape3DSW::FeatureType support_type_A;
+		shape_A->get_supports(transform_A->basis.xform_inv(-best_axis).normalized(), max_supports, supports_A, support_count_A, support_type_A);
 		for (int i = 0; i < support_count_A; i++) {
 			supports_A[i] = transform_A->xform(supports_A[i]);
 		}
@@ -378,7 +735,8 @@ public:
 
 		Vector3 supports_B[max_supports];
 		int support_count_B;
-		shape_B->get_supports(transform_B->basis.xform_inv(best_axis).normalized(), max_supports, supports_B, support_count_B);
+		Shape3DSW::FeatureType support_type_B;
+		shape_B->get_supports(transform_B->basis.xform_inv(best_axis).normalized(), max_supports, supports_B, support_count_B, support_type_B);
 		for (int i = 0; i < support_count_B; i++) {
 			supports_B[i] = transform_B->xform(supports_B[i]);
 		}
@@ -393,7 +751,7 @@ public:
 		if (callback->prev_axis) {
 			*callback->prev_axis = best_axis;
 		}
-		_generate_contacts_from_supports(supports_A, support_count_A, supports_B, support_count_B, callback);
+		_generate_contacts_from_supports(supports_A, support_count_A, support_type_A, supports_B, support_count_B, support_type_B, callback);
 
 		callback->collided = true;
 	}
@@ -529,6 +887,61 @@ static void _collision_sphere_capsule(const Shape3DSW *p_a, const Transform &p_t
 
 template <bool withMargin>
 static void _collision_sphere_cylinder(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+	const SphereShape3DSW *sphere_A = static_cast<const SphereShape3DSW *>(p_a);
+	const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b);
+
+	SeparatorAxisTest<SphereShape3DSW, CylinderShape3DSW, withMargin> separator(sphere_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
+
+	if (!separator.test_previous_axis()) {
+		return;
+	}
+
+	// Cylinder B end caps.
+	Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1).normalized();
+	if (!separator.test_axis(cylinder_B_axis)) {
+		return;
+	}
+
+	Vector3 cylinder_diff = p_transform_b.origin - p_transform_a.origin;
+
+	// Cylinder B lateral surface.
+	if (!separator.test_axis(cylinder_B_axis.cross(cylinder_diff).cross(cylinder_B_axis).normalized())) {
+		return;
+	}
+
+	// Closest point to cylinder caps.
+	const Vector3 &sphere_center = p_transform_a.origin;
+	Vector3 cyl_axis = p_transform_b.basis.get_axis(1);
+	Vector3 cap_axis = p_transform_b.basis.get_axis(0);
+	real_t height_scale = cyl_axis.length();
+	real_t cap_dist = cylinder_B->get_height() * 0.5 * height_scale;
+	cyl_axis /= height_scale;
+	real_t radius_scale = cap_axis.length();
+	real_t cap_radius = cylinder_B->get_radius() * radius_scale;
+
+	for (int i = 0; i < 2; i++) {
+		Vector3 cap_dir = ((i == 0) ? cyl_axis : -cyl_axis);
+		Vector3 cap_pos = p_transform_b.origin + cap_dir * cap_dist;
+
+		Vector3 closest_point;
+
+		Vector3 diff = sphere_center - cap_pos;
+		Vector3 proj = diff - cap_dir.dot(diff) * cap_dir;
+
+		real_t proj_len = proj.length();
+		if (Math::is_zero_approx(proj_len)) {
+			// Point is equidistant to all circle points.
+			continue;
+		}
+
+		closest_point = cap_pos + (cap_radius / proj_len) * proj;
+
+		if (!separator.test_axis((closest_point - sphere_center).normalized())) {
+			return;
+		}
+	}
+
+	separator.generate_contacts();
 }
 
 template <bool withMargin>
@@ -739,7 +1152,7 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran
 
 	// faces of A
 	for (int i = 0; i < 3; i++) {
-		Vector3 axis = p_transform_a.basis.get_axis(i);
+		Vector3 axis = p_transform_a.basis.get_axis(i).normalized();
 
 		if (!separator.test_axis(axis)) {
 			return;
@@ -826,6 +1239,115 @@ static void _collision_box_capsule(const Shape3DSW *p_a, const Transform &p_tran
 
 template <bool withMargin>
 static void _collision_box_cylinder(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+	const BoxShape3DSW *box_A = static_cast<const BoxShape3DSW *>(p_a);
+	const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b);
+
+	SeparatorAxisTest<BoxShape3DSW, CylinderShape3DSW, withMargin> separator(box_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
+
+	if (!separator.test_previous_axis()) {
+		return;
+	}
+
+	// Faces of A.
+	for (int i = 0; i < 3; i++) {
+		Vector3 axis = p_transform_a.basis.get_axis(i).normalized();
+
+		if (!separator.test_axis(axis)) {
+			return;
+		}
+	}
+
+	Vector3 cyl_axis = p_transform_b.basis.get_axis(1).normalized();
+
+	// Cylinder end caps.
+	{
+		if (!separator.test_axis(cyl_axis)) {
+			return;
+		}
+	}
+
+	// Edges of A, cylinder lateral surface.
+	for (int i = 0; i < 3; i++) {
+		Vector3 box_axis = p_transform_a.basis.get_axis(i);
+		Vector3 axis = box_axis.cross(cyl_axis);
+		if (Math::is_zero_approx(axis.length_squared())) {
+			continue;
+		}
+
+		if (!separator.test_axis(axis.normalized())) {
+			return;
+		}
+	}
+
+	// Gather points of A.
+	Vector3 vertices_A[8];
+	Vector3 box_extent = box_A->get_half_extents();
+	for (int i = 0; i < 2; i++) {
+		for (int j = 0; j < 2; j++) {
+			for (int k = 0; k < 2; k++) {
+				Vector3 extent = box_extent;
+				extent.x *= (i * 2 - 1);
+				extent.y *= (j * 2 - 1);
+				extent.z *= (k * 2 - 1);
+				Vector3 &point = vertices_A[i * 2 * 2 + j * 2 + k];
+				point = p_transform_a.origin;
+				for (int l = 0; l < 3; l++) {
+					point += p_transform_a.basis.get_axis(l) * extent[l];
+				}
+			}
+		}
+	}
+
+	// Points of A, cylinder lateral surface.
+	for (int i = 0; i < 8; i++) {
+		const Vector3 &point = vertices_A[i];
+		Vector3 axis = Plane(cyl_axis, 0).project(point).normalized();
+
+		if (!separator.test_axis(axis)) {
+			return;
+		}
+	}
+
+	// Edges of A, cylinder end caps rim.
+	int edges_start_A[12] = { 0, 2, 4, 6, 0, 1, 4, 5, 0, 1, 2, 3 };
+	int edges_end_A[12] = { 1, 3, 5, 7, 2, 3, 6, 7, 4, 5, 6, 7 };
+
+	Vector3 cap_axis = cyl_axis * (cylinder_B->get_height() * 0.5);
+
+	for (int i = 0; i < 2; i++) {
+		Vector3 cap_pos = p_transform_b.origin + ((i == 0) ? cap_axis : -cap_axis);
+
+		for (int e = 0; e < 12; e++) {
+			const Vector3 &edge_start = vertices_A[edges_start_A[e]];
+			const Vector3 &edge_end = vertices_A[edges_end_A[e]];
+
+			Vector3 edge_dir = (edge_end - edge_start);
+			edge_dir.normalize();
+
+			real_t edge_dot = edge_dir.dot(cyl_axis);
+			if (Math::is_zero_approx(edge_dot)) {
+				// Edge is perpendicular to cylinder axis.
+				continue;
+			}
+
+			// Calculate intersection between edge and circle plane.
+			Vector3 edge_diff = cap_pos - edge_start;
+			real_t diff_dot = edge_diff.dot(cyl_axis);
+			Vector3 intersection = edge_start + edge_dir * diff_dot / edge_dot;
+
+			// Calculate tangent that touches intersection.
+			Vector3 tangent = (cap_pos - intersection).cross(cyl_axis);
+
+			// Axis is orthogonal both to tangent and edge direction.
+			Vector3 axis = tangent.cross(edge_dir);
+
+			if (!separator.test_axis(axis.normalized())) {
+				return;
+			}
+		}
+	}
+
+	separator.generate_contacts();
 }
 
 template <bool withMargin>
@@ -1111,6 +1633,19 @@ static void _collision_capsule_capsule(const Shape3DSW *p_a, const Transform &p_
 
 template <bool withMargin>
 static void _collision_capsule_cylinder(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+	const CapsuleShape3DSW *capsule_A = static_cast<const CapsuleShape3DSW *>(p_a);
+	const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b);
+
+	SeparatorAxisTest<CapsuleShape3DSW, CylinderShape3DSW, withMargin> separator(capsule_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
+
+	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)) {
+		return;
+	}
+
+	separator.generate_contacts();
 }
 
 template <bool withMargin>
@@ -1236,14 +1771,165 @@ static void _collision_capsule_face(const Shape3DSW *p_a, const Transform &p_tra
 
 template <bool withMargin>
 static void _collision_cylinder_cylinder(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+	const CylinderShape3DSW *cylinder_A = static_cast<const CylinderShape3DSW *>(p_a);
+	const CylinderShape3DSW *cylinder_B = static_cast<const CylinderShape3DSW *>(p_b);
+
+	SeparatorAxisTest<CylinderShape3DSW, CylinderShape3DSW, withMargin> separator(cylinder_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
+
+	Vector3 cylinder_A_axis = p_transform_a.basis.get_axis(1);
+	Vector3 cylinder_B_axis = p_transform_b.basis.get_axis(1);
+
+	if (!separator.test_previous_axis()) {
+		return;
+	}
+
+	// Cylinder A end caps.
+	if (!separator.test_axis(cylinder_A_axis.normalized())) {
+		return;
+	}
+
+	// Cylinder B end caps.
+	if (!separator.test_axis(cylinder_A_axis.normalized())) {
+		return;
+	}
+
+	Vector3 cylinder_diff = p_transform_b.origin - p_transform_a.origin;
+
+	// Cylinder A lateral surface.
+	if (!separator.test_axis(cylinder_A_axis.cross(cylinder_diff).cross(cylinder_A_axis).normalized())) {
+		return;
+	}
+
+	// Cylinder B lateral surface.
+	if (!separator.test_axis(cylinder_B_axis.cross(cylinder_diff).cross(cylinder_B_axis).normalized())) {
+		return;
+	}
+
+	real_t proj = cylinder_A_axis.cross(cylinder_B_axis).cross(cylinder_B_axis).dot(cylinder_A_axis);
+	if (Math::is_zero_approx(proj)) {
+		// Parallel cylinders, handle with specific axes only.
+		// Note: GJKEPA with no margin can lead to degenerate cases in this situation.
+		separator.generate_contacts();
+		return;
+	}
+
+	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)) {
+		return;
+	}
+
+	separator.generate_contacts();
 }
 
 template <bool withMargin>
 static void _collision_cylinder_convex_polygon(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+	const CylinderShape3DSW *cylinder_A = static_cast<const CylinderShape3DSW *>(p_a);
+	const ConvexPolygonShape3DSW *convex_polygon_B = static_cast<const ConvexPolygonShape3DSW *>(p_b);
+
+	SeparatorAxisTest<CylinderShape3DSW, ConvexPolygonShape3DSW, withMargin> separator(cylinder_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
+
+	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)) {
+		return;
+	}
+
+	separator.generate_contacts();
 }
 
 template <bool withMargin>
 static void _collision_cylinder_face(const Shape3DSW *p_a, const Transform &p_transform_a, const Shape3DSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
+	const CylinderShape3DSW *cylinder_A = static_cast<const CylinderShape3DSW *>(p_a);
+	const FaceShape3DSW *face_B = static_cast<const FaceShape3DSW *>(p_b);
+
+	SeparatorAxisTest<CylinderShape3DSW, FaceShape3DSW, withMargin> separator(cylinder_A, p_transform_a, face_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
+
+	if (!separator.test_previous_axis()) {
+		return;
+	}
+
+	Vector3 vertex[3] = {
+		p_transform_b.xform(face_B->vertex[0]),
+		p_transform_b.xform(face_B->vertex[1]),
+		p_transform_b.xform(face_B->vertex[2]),
+	};
+
+	// Face B normal.
+	if (!separator.test_axis((vertex[0] - vertex[2]).cross(vertex[0] - vertex[1]).normalized())) {
+		return;
+	}
+
+	Vector3 cyl_axis = p_transform_a.basis.get_axis(1).normalized();
+
+	// Cylinder end caps.
+	{
+		if (!separator.test_axis(cyl_axis)) {
+			return;
+		}
+	}
+
+	// Edges of B, cylinder lateral surface.
+	for (int i = 0; i < 3; i++) {
+		Vector3 edge_axis = vertex[i] - vertex[(i + 1) % 3];
+		Vector3 axis = edge_axis.cross(cyl_axis);
+		if (Math::is_zero_approx(axis.length_squared())) {
+			continue;
+		}
+
+		if (!separator.test_axis(axis.normalized())) {
+			return;
+		}
+	}
+
+	// Points of B, cylinder lateral surface.
+	for (int i = 0; i < 3; i++) {
+		const Vector3 &point = vertex[i];
+		Vector3 axis = Plane(cyl_axis, 0).project(point).normalized();
+
+		if (!separator.test_axis(axis)) {
+			return;
+		}
+	}
+
+	// Edges of B, cylinder end caps rim.
+	Vector3 cap_axis = cyl_axis * (cylinder_A->get_height() * 0.5);
+
+	for (int i = 0; i < 2; i++) {
+		Vector3 cap_pos = p_transform_a.origin + ((i == 0) ? cap_axis : -cap_axis);
+
+		for (int j = 0; j < 3; j++) {
+			const Vector3 &edge_start = vertex[j];
+			const Vector3 &edge_end = vertex[(j + 1) % 3];
+			Vector3 edge_dir = edge_end - edge_start;
+			edge_dir.normalize();
+
+			real_t edge_dot = edge_dir.dot(cyl_axis);
+			if (Math::is_zero_approx(edge_dot)) {
+				// Edge is perpendicular to cylinder axis.
+				continue;
+			}
+
+			// Calculate intersection between edge and circle plane.
+			Vector3 edge_diff = cap_pos - edge_start;
+			real_t diff_dot = edge_diff.dot(cyl_axis);
+			Vector3 intersection = edge_start + edge_dir * diff_dot / edge_dot;
+
+			// Calculate tangent that touches intersection.
+			Vector3 tangent = (cap_pos - intersection).cross(cyl_axis);
+
+			// Axis is orthogonal both to tangent and edge direction.
+			Vector3 axis = tangent.cross(edge_dir);
+
+			if (!separator.test_axis(axis.normalized())) {
+				return;
+			}
+		}
+	}
+
+	separator.generate_contacts();
 }
 
 template <bool withMargin>
diff --git a/servers/physics_3d/collision_solver_3d_sw.cpp b/servers/physics_3d/collision_solver_3d_sw.cpp
index 1150696b84..fd9ea00d92 100644
--- a/servers/physics_3d/collision_solver_3d_sw.cpp
+++ b/servers/physics_3d/collision_solver_3d_sw.cpp
@@ -46,8 +46,24 @@ bool CollisionSolver3DSW::solve_static_plane(const Shape3DSW *p_shape_A, const T
 	static const int max_supports = 16;
 	Vector3 supports[max_supports];
 	int support_count;
-
-	p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count);
+	Shape3DSW::FeatureType support_type;
+	p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type);
+
+	if (support_type == Shape3DSW::FEATURE_CIRCLE) {
+		ERR_FAIL_COND_V(support_count != 3, false);
+
+		Vector3 circle_pos = supports[0];
+		Vector3 circle_axis_1 = supports[1] - circle_pos;
+		Vector3 circle_axis_2 = supports[2] - circle_pos;
+
+		// Use 3 equidistant points on the circle.
+		for (int i = 0; i < 3; ++i) {
+			Vector3 vertex_pos = circle_pos;
+			vertex_pos += circle_axis_1 * Math::cos(2.0 * Math_PI * i / 3.0);
+			vertex_pos += circle_axis_2 * Math::sin(2.0 * Math_PI * i / 3.0);
+			supports[i] = vertex_pos;
+		}
+	}
 
 	bool found = false;
 
@@ -265,8 +281,25 @@ bool CollisionSolver3DSW::solve_distance_plane(const Shape3DSW *p_shape_A, const
 	static const int max_supports = 16;
 	Vector3 supports[max_supports];
 	int support_count;
+	Shape3DSW::FeatureType support_type;
+
+	p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type);
 
-	p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count);
+	if (support_type == Shape3DSW::FEATURE_CIRCLE) {
+		ERR_FAIL_COND_V(support_count != 3, false);
+
+		Vector3 circle_pos = supports[0];
+		Vector3 circle_axis_1 = supports[1] - circle_pos;
+		Vector3 circle_axis_2 = supports[2] - circle_pos;
+
+		// Use 3 equidistant points on the circle.
+		for (int i = 0; i < 3; ++i) {
+			Vector3 vertex_pos = circle_pos;
+			vertex_pos += circle_axis_1 * Math::cos(2.0 * Math_PI * i / 3.0);
+			vertex_pos += circle_axis_2 * Math::sin(2.0 * Math_PI * i / 3.0);
+			supports[i] = vertex_pos;
+		}
+	}
 
 	bool collided = false;
 	Vector3 closest;
diff --git a/servers/physics_3d/gjk_epa.cpp b/servers/physics_3d/gjk_epa.cpp
index dafd2feb8b..aa7c11eec5 100644
--- a/servers/physics_3d/gjk_epa.cpp
+++ b/servers/physics_3d/gjk_epa.cpp
@@ -64,7 +64,7 @@ GJK-EPA collision solver by Nathanael Presson, 2008
 
 /* GJK	*/
 #define GJK_MAX_ITERATIONS	128
-#define GJK_ACCURARY		((real_t)0.0001)
+#define GJK_ACCURACY		((real_t)0.0001)
 #define GJK_MIN_DISTANCE	((real_t)0.0001)
 #define GJK_DUPLICATED_EPS	((real_t)0.0001)
 #define GJK_SIMPLEX2_EPS	((real_t)0.0)
@@ -72,10 +72,13 @@ GJK-EPA collision solver by Nathanael Presson, 2008
 #define GJK_SIMPLEX4_EPS	((real_t)0.0)
 
 /* EPA	*/
-#define EPA_MAX_VERTICES	64
+#define EPA_MAX_VERTICES	128
 #define EPA_MAX_FACES		(EPA_MAX_VERTICES*2)
 #define EPA_MAX_ITERATIONS	255
-#define EPA_ACCURACY		((real_t)0.0001)
+// -- GODOT start --
+//#define EPA_ACCURACY		((real_t)0.0001)
+#define EPA_ACCURACY		((real_t)0.00001)
+// -- GODOT end --
 #define EPA_FALLBACK		(10*EPA_ACCURACY)
 #define EPA_PLANE_EPS		((real_t)0.00001)
 #define EPA_INSIDE_EPS		((real_t)0.01)
@@ -237,7 +240,7 @@ struct	GJK
 				/* Check for termination				*/
 				const real_t	omega=vec3_dot(m_ray,w)/rl;
 				alpha=MAX(omega,alpha);
-				if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
+				if(((rl-alpha)-(GJK_ACCURACY*rl))<=0)
 				{/* Return old simplex				*/
 					removevertice(m_simplices[m_current]);
 					break;
@@ -466,7 +469,7 @@ struct	GJK
 			if(ng&&(Math::abs(vl)>GJK_SIMPLEX4_EPS))
 			{
 				real_t	mindist=-1;
-				real_t	subw[3];
+				real_t	subw[3] = {0.f, 0.f, 0.f};
 				U		subm=0;
 				for(U i=0;i<3;++i)
 				{
@@ -512,7 +515,6 @@ struct	GJK
 		{
 			Vector3	n;
 			real_t	d;
-			real_t	p;
 			sSV*		c[3];
 			sFace*		f[3];
 			sFace*		l[2];
@@ -661,8 +663,7 @@ struct	GJK
 										remove(m_hull,best);
 										append(m_stock,best);
 										best=findbest();
-										if(best->p>=outer.p) { outer=*best;
-}
+										outer=*best;
 									} else { m_status=eStatus::InvalidHull;break; }
 								} else { m_status=eStatus::AccuraryReached;break; }
 							} else { m_status=eStatus::OutOfVertices;break; }
@@ -688,24 +689,54 @@ struct	GJK
 					}
 				}
 				/* Fallback		*/
-				m_status	=	eStatus::FallBack;
-				m_normal	=	-guess;
-				const real_t	nl=m_normal.length();
-				if(nl>0) {
-					m_normal	=	m_normal/nl;
+				m_status = eStatus::FallBack;
+				m_normal = -guess;
+				const real_t nl = m_normal.length();
+				if (nl > 0) {
+					m_normal = m_normal/nl;
 				} else {
-					m_normal	=	Vector3(1,0,0);
-}
+					m_normal = Vector3(1,0,0);
+				}
 				m_depth	=	0;
 				m_result.rank=1;
 				m_result.c[0]=simplex.c[0];
 				m_result.p[0]=1;
 				return(m_status);
 			}
+
+			bool getedgedist(sFace* face, sSV* a, sSV* b, real_t& dist)
+			{
+				const Vector3 ba = b->w - a->w;
+				const Vector3 n_ab = vec3_cross(ba, face->n);   // Outward facing edge normal direction, on triangle plane
+				const real_t a_dot_nab = vec3_dot(a->w, n_ab);  // Only care about the sign to determine inside/outside, so not normalization required
+
+				if (a_dot_nab < 0) {
+					// Outside of edge a->b
+					const real_t ba_l2 = ba.length_squared();
+					const real_t a_dot_ba = vec3_dot(a->w, ba);
+					const real_t b_dot_ba = vec3_dot(b->w, ba);
+
+					if (a_dot_ba > 0) {
+						// Pick distance vertex a
+						dist = a->w.length();
+					} else if (b_dot_ba < 0) {
+						// Pick distance vertex b
+						dist = b->w.length();
+					} else {
+						// Pick distance to edge a->b
+						const real_t a_dot_b = vec3_dot(a->w, b->w);
+						dist = Math::sqrt(MAX((a->w.length_squared() * b->w.length_squared() - a_dot_b * a_dot_b) / ba_l2, 0.0));
+					}
+
+					return true;
+				}
+
+				return false;
+			}
+
 			sFace*				newface(sSV* a,sSV* b,sSV* c,bool forced)
 			{
-				if(m_stock.root)
-				{
+				if (m_stock.root) {
 					sFace*	face=m_stock.root;
 					remove(m_stock,face);
 					append(m_hull,face);
@@ -716,23 +747,23 @@ struct	GJK
 					face->n		=	vec3_cross(b->w-a->w,c->w-a->w);
 					const real_t	l=face->n.length();
 					const bool		v=l>EPA_ACCURACY;
-					face->p		=	MIN(MIN(
-						vec3_dot(a->w,vec3_cross(face->n,a->w-b->w)),
-						vec3_dot(b->w,vec3_cross(face->n,b->w-c->w))),
-						vec3_dot(c->w,vec3_cross(face->n,c->w-a->w)))	/
-						(v?l:1);
-					face->p		=	face->p>=-EPA_INSIDE_EPS?0:face->p;
-					if(v)
-					{
-						face->d		=	vec3_dot(a->w,face->n)/l;
+					if (v) {
+						if (!(getedgedist(face, a, b, face->d) ||
+							  getedgedist(face, b, c, face->d) ||
+							  getedgedist(face, c, a, face->d))) {
+							// Origin projects to the interior of the triangle
+							// Use distance to triangle plane
+							face->d = vec3_dot(a->w, face->n) / l;
+						}
 						face->n		/=	l;
-						if(forced||(face->d>=-EPA_PLANE_EPS))
-						{
+						if (forced||(face->d>=-EPA_PLANE_EPS)) {
 							return(face);
-						} else { m_status=eStatus::NonConvex;
-}
-					} else { m_status=eStatus::Degenerated;
-}
+						} else {
+							m_status=eStatus::NonConvex;
+						}
+					} else {
+						m_status=eStatus::Degenerated;
+					}
 					remove(m_hull,face);
 					append(m_stock,face);
 					return(nullptr);
@@ -747,15 +778,13 @@ struct	GJK
 			{
 				sFace*		minf=m_hull.root;
 				real_t	mind=minf->d*minf->d;
-				real_t	maxp=minf->p;
 				for(sFace* f=minf->l[1];f;f=f->l[1])
 				{
 					const real_t	sqd=f->d*f->d;
-					if((f->p>=maxp)&&(sqd<mind))
+					if(sqd<mind)
 					{
 						minf=f;
 						mind=sqd;
-						maxp=f->p;
 					}
 				}
 				return(minf);
diff --git a/servers/physics_3d/physics_server_3d_sw.cpp b/servers/physics_3d/physics_server_3d_sw.cpp
index d5d40c181f..735e9094d2 100644
--- a/servers/physics_3d/physics_server_3d_sw.cpp
+++ b/servers/physics_3d/physics_server_3d_sw.cpp
@@ -74,7 +74,10 @@ RID PhysicsServer3DSW::capsule_shape_create() {
 	return rid;
 }
 RID PhysicsServer3DSW::cylinder_shape_create() {
-	ERR_FAIL_V(RID());
+	Shape3DSW *shape = memnew(CylinderShape3DSW);
+	RID rid = shape_owner.make_rid(shape);
+	shape->set_self(rid);
+	return rid;
 }
 RID PhysicsServer3DSW::convex_polygon_shape_create() {
 	Shape3DSW *shape = memnew(ConvexPolygonShape3DSW);
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];
 }
 
diff --git a/servers/physics_3d/shape_3d_sw.h b/servers/physics_3d/shape_3d_sw.h
index 851c0d9443..cafe978abb 100644
--- a/servers/physics_3d/shape_3d_sw.h
+++ b/servers/physics_3d/shape_3d_sw.h
@@ -67,8 +67,11 @@ protected:
 	void configure(const AABB &p_aabb);
 
 public:
-	enum {
-		MAX_SUPPORTS = 8
+	enum FeatureType {
+		FEATURE_POINT,
+		FEATURE_EDGE,
+		FEATURE_FACE,
+		FEATURE_CIRCLE,
 	};
 
 	virtual real_t get_area() const { return aabb.get_area(); }
@@ -85,7 +88,7 @@ public:
 
 	virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const = 0;
 	virtual Vector3 get_support(const Vector3 &p_normal) const;
-	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const = 0;
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const = 0;
 	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const = 0;
 	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_point, Vector3 &r_normal) const = 0;
 	virtual bool intersect_point(const Vector3 &p_point) const = 0;
@@ -110,7 +113,7 @@ class ConcaveShape3DSW : public Shape3DSW {
 public:
 	virtual bool is_concave() const { return true; }
 	typedef void (*Callback)(void *p_userdata, Shape3DSW *p_convex);
-	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; }
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; }
 
 	virtual void cull(const AABB &p_local_aabb, Callback p_callback, void *p_userdata) const = 0;
 
@@ -129,7 +132,7 @@ public:
 	virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_PLANE; }
 	virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const;
 	virtual Vector3 get_support(const Vector3 &p_normal) const;
-	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; }
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; }
 
 	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
 	virtual bool intersect_point(const Vector3 &p_point) const;
@@ -156,7 +159,7 @@ public:
 	virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_RAY; }
 	virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const;
 	virtual Vector3 get_support(const Vector3 &p_normal) const;
-	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const;
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const;
 
 	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
 	virtual bool intersect_point(const Vector3 &p_point) const;
@@ -184,7 +187,7 @@ public:
 
 	virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const;
 	virtual Vector3 get_support(const Vector3 &p_normal) const;
-	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const;
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const;
 	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
 	virtual bool intersect_point(const Vector3 &p_point) const;
 	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
@@ -209,7 +212,7 @@ public:
 
 	virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const;
 	virtual Vector3 get_support(const Vector3 &p_normal) const;
-	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const;
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const;
 	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
 	virtual bool intersect_point(const Vector3 &p_point) const;
 	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
@@ -238,7 +241,7 @@ public:
 
 	virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const;
 	virtual Vector3 get_support(const Vector3 &p_normal) const;
-	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const;
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const;
 	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
 	virtual bool intersect_point(const Vector3 &p_point) const;
 	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
@@ -251,6 +254,35 @@ public:
 	CapsuleShape3DSW();
 };
 
+class CylinderShape3DSW : public Shape3DSW {
+	real_t height;
+	real_t radius;
+
+	void _setup(real_t p_height, real_t p_radius);
+
+public:
+	_FORCE_INLINE_ real_t get_height() const { return height; }
+	_FORCE_INLINE_ real_t get_radius() const { return radius; }
+
+	virtual real_t get_area() const { return 4.0 / 3.0 * Math_PI * radius * radius * radius + height * Math_PI * radius * radius; }
+
+	virtual PhysicsServer3D::ShapeType get_type() const { return PhysicsServer3D::SHAPE_CYLINDER; }
+
+	virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const;
+	virtual Vector3 get_support(const Vector3 &p_normal) const;
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const;
+	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
+	virtual bool intersect_point(const Vector3 &p_point) const;
+	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
+
+	virtual Vector3 get_moment_of_inertia(real_t p_mass) const;
+
+	virtual void set_data(const Variant &p_data);
+	virtual Variant get_data() const;
+
+	CylinderShape3DSW();
+};
+
 struct ConvexPolygonShape3DSW : public Shape3DSW {
 	Geometry3D::MeshData mesh;
 
@@ -263,7 +295,7 @@ public:
 
 	virtual void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const;
 	virtual Vector3 get_support(const Vector3 &p_normal) const;
-	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const;
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const;
 	virtual bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
 	virtual bool intersect_point(const Vector3 &p_point) const;
 	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
@@ -399,7 +431,7 @@ struct FaceShape3DSW : public Shape3DSW {
 
 	void project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const;
 	Vector3 get_support(const Vector3 &p_normal) const;
-	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const;
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const;
 	bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const;
 	virtual bool intersect_point(const Vector3 &p_point) const;
 	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const;
@@ -437,7 +469,7 @@ struct MotionShape3DSW : public Shape3DSW {
 		}
 		return support;
 	}
-	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const { r_amount = 0; }
+	virtual void get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const { r_amount = 0; }
 	bool intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const { return false; }
 	virtual bool intersect_point(const Vector3 &p_point) const { return false; }
 	virtual Vector3 get_closest_point_to(const Vector3 &p_point) const { return p_point; }