From 333f184734edc640b9a6f0f053124d8d05504c86 Mon Sep 17 00:00:00 2001 From: PouleyKetchoupp Date: Tue, 9 Feb 2021 11:25:29 -0700 Subject: 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. --- core/math/geometry_3d.h | 33 +++++++++++++++++++++------------ 1 file changed, 21 insertions(+), 12 deletions(-) (limited to 'core') 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(); -- cgit v1.2.3