Cylinder support in Godot Physics 3D

Backport of cylinder support from Master.
godotengine · Feb 18, 2021 · c6fbd55 · c6fbd55
1 parent 9a89782
commit c6fbd55
Show file tree

Hide file tree

Showing 9 changed files with 1,062 additions and 68 deletions.
diff --git a/COPYRIGHT.txt b/COPYRIGHT.txt
@@ -88,6 +88,11 @@ Copyright: 1997-2017, Sam Lantinga
  2014-2021, Godot Engine contributors.
 License: Expat and Zlib
 
+Files: ./servers/physics/collision_solver_sat.cpp
+Comment: Open Dynamics Engine
+Copyright: 2001-2003, Russell L. Smith, Alen Ladavac, Nguyen Binh
+License: BSD-3-clause
+
 Files: ./servers/physics/gjk_epa.cpp
  ./servers/physics/joints/generic_6dof_joint_sw.cpp
  ./servers/physics/joints/generic_6dof_joint_sw.h

diff --git a/core/math/geometry.h b/core/math/geometry.h
@@ -295,27 +295,34 @@ class Geometry {
 		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 = 0, Vector3 *r_norm = 0) {
+	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 = 0, Vector3 *r_norm = 0, 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.
@@ -326,10 +333,10 @@ class Geometry {
 			return false; // Avoid numerical error.
 		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;
 
@@ -375,10 +382,12 @@ class Geometry {
 		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/scene/3d/collision_shape.cpp b/scene/3d/collision_shape.cpp
@@ -30,6 +30,9 @@
 
 #include "collision_shape.h"
 
+#include "core/math/quick_hull.h"
+#include "mesh_instance.h"
+#include "physics_body.h"
 #include "scene/resources/box_shape.h"
 #include "scene/resources/capsule_shape.h"
 #include "scene/resources/concave_polygon_shape.h"
@@ -38,10 +41,6 @@
 #include "scene/resources/ray_shape.h"
 #include "scene/resources/sphere_shape.h"
 #include "servers/visual_server.h"
-//TODO: Implement CylinderShape and HeightMapShape?
-#include "core/math/quick_hull.h"
-#include "mesh_instance.h"
-#include "physics_body.h"
 
 void CollisionShape::make_convex_from_brothers() {