diff options
| -rw-r--r-- | COPYRIGHT.txt | 5 | ||||
| -rw-r--r-- | core/math/geometry_3d.h | 33 | ||||
| -rw-r--r-- | servers/physics_3d/collision_solver_3d_sat.cpp | 710 | ||||
| -rw-r--r-- | servers/physics_3d/collision_solver_3d_sw.cpp | 39 | ||||
| -rw-r--r-- | servers/physics_3d/gjk_epa.cpp | 99 | ||||
| -rw-r--r-- | servers/physics_3d/physics_server_3d_sw.cpp | 5 | ||||
| -rw-r--r-- | servers/physics_3d/shape_3d_sw.cpp | 214 | ||||
| -rw-r--r-- | servers/physics_3d/shape_3d_sw.h | 56 |
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; } |