Merge pull request #45933 from nekomatata/cylinder-support-3.2
[3.2] Cylinder support in Godot Physics 3D
This commit is contained in:
Rémi Verschelde
GitHub
commit
7af8da32b1
9 changed files with 1062 additions and 68 deletions
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -295,27 +295,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 = 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 @@ public:
|
|||
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 @@ 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();
|
||||
|
|
|
|||
|
|
@ -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() {
|
||||
|
||||
|
|
|
|||
|
|
@ -31,7 +31,38 @@
|
|||
#include "collision_solver_sat.h"
|
||||
#include "core/math/geometry.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 and circle-face contact points for cylinder-trimesh and cylinder-box collision 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 {
|
||||
|
||||
|
|
@ -86,6 +117,18 @@ 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
|
||||
|
||||
Vector3 closest_B = Plane(p_points_B[0], p_points_B[1], p_points_B[2]).project(*p_points_A);
|
||||
|
||||
p_callback->call(*p_points_A, closest_B);
|
||||
}
|
||||
|
||||
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
|
||||
|
|
@ -133,6 +176,105 @@ static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_
|
|||
p_callback->call(closest_A, closest_B);
|
||||
}
|
||||
|
||||
static void _generate_contacts_edge_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);
|
||||
|
||||
static const int max_clip = 2;
|
||||
Vector3 contact_points[max_clip];
|
||||
int num_points = 0;
|
||||
|
||||
// Project edge point in circle plane.
|
||||
const Vector3 &edge_A_1 = p_points_A[0];
|
||||
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) {
|
||||
contact_points[num_points] = edge_A_1;
|
||||
++num_points;
|
||||
}
|
||||
|
||||
const Vector3 &edge_A_2 = p_points_A[1];
|
||||
Vector3 proj_point_2 = circle_plane.project(edge_A_2);
|
||||
|
||||
Vector3 dist_vec_2 = proj_point_2 - circle_B_pos;
|
||||
real_t dist_sq_2 = dist_vec_2.length_squared();
|
||||
|
||||
// Point 2 is inside disk, add as contact point.
|
||||
if (dist_sq_2 <= circle_B_radius * circle_B_radius) {
|
||||
contact_points[num_points] = edge_A_2;
|
||||
++num_points;
|
||||
}
|
||||
|
||||
if (num_points < 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) {
|
||||
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 face_point_1 = edge_A_1 + fraction_1 * edge_dir;
|
||||
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 face_point_2 = edge_A_1 + fraction_2 * edge_dir;
|
||||
ERR_FAIL_COND(num_points >= max_clip);
|
||||
contact_points[num_points] = face_point_2;
|
||||
++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_face_face(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
|
||||
|
|
@ -227,37 +369,232 @@ 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 < 3);
|
||||
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;
|
||||
|
||||
// Clip face with circle segments.
|
||||
static const int circle_segments = 8;
|
||||
Vector3 circle_points[circle_segments];
|
||||
|
||||
real_t angle_delta = 2.0 * Math_PI / circle_segments;
|
||||
|
||||
for (int i = 0; i < circle_segments; ++i) {
|
||||
Vector3 point_pos = circle_B_pos;
|
||||
point_pos += circle_B_line_1 * Math::cos(i * angle_delta);
|
||||
point_pos += circle_B_line_2 * Math::sin(i * angle_delta);
|
||||
circle_points[i] = point_pos;
|
||||
}
|
||||
|
||||
_generate_contacts_face_face(p_points_A, p_point_count_A, circle_points, circle_segments, p_callback);
|
||||
|
||||
// Clip face with circle plane.
|
||||
Vector3 circle_B_normal = circle_B_line_1.cross(circle_B_line_2).normalized();
|
||||
|
||||
Plane circle_plane(circle_B_pos, circle_B_normal);
|
||||
|
||||
static const int max_clip = 32;
|
||||
Vector3 contact_points[max_clip];
|
||||
int num_points = 0;
|
||||
|
||||
for (int i = 0; i < p_point_count_A; i++) {
|
||||
int i_n = (i + 1) % p_point_count_A;
|
||||
|
||||
const Vector3 &edge0_A = p_points_A[i];
|
||||
const Vector3 &edge1_A = p_points_A[i_n];
|
||||
|
||||
real_t dist0 = circle_plane.distance_to(edge0_A);
|
||||
real_t dist1 = circle_plane.distance_to(edge1_A);
|
||||
|
||||
// First point in front of plane, generate contact point.
|
||||
if (dist0 * circle_plane.d >= 0) {
|
||||
ERR_FAIL_COND(num_points >= max_clip);
|
||||
contact_points[num_points] = edge0_A;
|
||||
++num_points;
|
||||
}
|
||||
|
||||
// Points on different sides, generate contact point.
|
||||
if (dist0 * dist1 < 0) {
|
||||
// calculate intersection
|
||||
Vector3 rel = edge1_A - edge0_A;
|
||||
real_t den = circle_plane.normal.dot(rel);
|
||||
real_t dist = -(circle_plane.normal.dot(edge0_A) - circle_plane.d) / den;
|
||||
Vector3 inters = edge0_A + rel * dist;
|
||||
|
||||
ERR_FAIL_COND(num_points >= max_clip);
|
||||
contact_points[num_points] = inters;
|
||||
++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, ShapeSW::FeatureType p_feature_type_A, const Vector3 *p_points_B, int p_point_count_B, ShapeSW::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,
|
||||
},
|
||||
{
|
||||
0,
|
||||
_generate_contacts_edge_edge,
|
||||
_generate_contacts_face_face,
|
||||
_generate_contacts_edge_circle,
|
||||
},
|
||||
{
|
||||
0,
|
||||
0,
|
||||
_generate_contacts_face_face,
|
||||
}
|
||||
_generate_contacts_face_circle,
|
||||
},
|
||||
{
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
_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;
|
||||
|
|
@ -266,17 +603,18 @@ 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);
|
||||
|
|
@ -360,6 +698,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
|
||||
|
|
@ -378,7 +727,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);
|
||||
ShapeSW::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]);
|
||||
}
|
||||
|
|
@ -392,7 +742,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);
|
||||
ShapeSW::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]);
|
||||
}
|
||||
|
|
@ -407,7 +758,7 @@ public:
|
|||
callback->normal = best_axis;
|
||||
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;
|
||||
}
|
||||
|
|
@ -538,6 +889,60 @@ static void _collision_sphere_capsule(const ShapeSW *p_a, const Transform &p_tra
|
|||
|
||||
template <bool withMargin>
|
||||
static void _collision_sphere_cylinder(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
|
||||
const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW *>(p_a);
|
||||
const CylinderShapeSW *cylinder_B = static_cast<const CylinderShapeSW *>(p_b);
|
||||
|
||||
SeparatorAxisTest<SphereShapeSW, CylinderShapeSW, 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>
|
||||
|
|
@ -750,7 +1155,7 @@ static void _collision_box_capsule(const ShapeSW *p_a, const Transform &p_transf
|
|||
// 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;
|
||||
|
|
@ -833,6 +1238,115 @@ static void _collision_box_capsule(const ShapeSW *p_a, const Transform &p_transf
|
|||
|
||||
template <bool withMargin>
|
||||
static void _collision_box_cylinder(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
|
||||
const BoxShapeSW *box_A = static_cast<const BoxShapeSW *>(p_a);
|
||||
const CylinderShapeSW *cylinder_B = static_cast<const CylinderShapeSW *>(p_b);
|
||||
|
||||
SeparatorAxisTest<BoxShapeSW, CylinderShapeSW, 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>
|
||||
|
|
@ -1113,6 +1627,19 @@ static void _collision_capsule_capsule(const ShapeSW *p_a, const Transform &p_tr
|
|||
|
||||
template <bool withMargin>
|
||||
static void _collision_capsule_cylinder(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
|
||||
const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW *>(p_a);
|
||||
const CylinderShapeSW *cylinder_B = static_cast<const CylinderShapeSW *>(p_b);
|
||||
|
||||
SeparatorAxisTest<CapsuleShapeSW, CylinderShapeSW, withMargin> separator(capsule_A, p_transform_a, cylinder_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
|
||||
|
||||
CollisionSolverSW::CallbackResult callback = SeparatorAxisTest<CapsuleShapeSW, CylinderShapeSW, 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>
|
||||
|
|
@ -1237,14 +1764,165 @@ static void _collision_capsule_face(const ShapeSW *p_a, const Transform &p_trans
|
|||
|
||||
template <bool withMargin>
|
||||
static void _collision_cylinder_cylinder(const ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
|
||||
const CylinderShapeSW *cylinder_A = static_cast<const CylinderShapeSW *>(p_a);
|
||||
const CylinderShapeSW *cylinder_B = static_cast<const CylinderShapeSW *>(p_b);
|
||||
|
||||
SeparatorAxisTest<CylinderShapeSW, CylinderShapeSW, 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;
|
||||
}
|
||||
|
||||
CollisionSolverSW::CallbackResult callback = SeparatorAxisTest<CylinderShapeSW, CylinderShapeSW, 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 ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
|
||||
const CylinderShapeSW *cylinder_A = static_cast<const CylinderShapeSW *>(p_a);
|
||||
const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW *>(p_b);
|
||||
|
||||
SeparatorAxisTest<CylinderShapeSW, ConvexPolygonShapeSW, withMargin> separator(cylinder_A, p_transform_a, convex_polygon_B, p_transform_b, p_collector, p_margin_a, p_margin_b);
|
||||
|
||||
CollisionSolverSW::CallbackResult callback = SeparatorAxisTest<CylinderShapeSW, ConvexPolygonShapeSW, 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 ShapeSW *p_a, const Transform &p_transform_a, const ShapeSW *p_b, const Transform &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
|
||||
const CylinderShapeSW *cylinder_A = static_cast<const CylinderShapeSW *>(p_a);
|
||||
const FaceShapeSW *face_B = static_cast<const FaceShapeSW *>(p_b);
|
||||
|
||||
SeparatorAxisTest<CylinderShapeSW, FaceShapeSW, 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>
|
||||
|
|
|
|||
|
|
@ -46,8 +46,25 @@ bool CollisionSolverSW::solve_static_plane(const ShapeSW *p_shape_A, const Trans
|
|||
static const int max_supports = 16;
|
||||
Vector3 supports[max_supports];
|
||||
int support_count;
|
||||
ShapeSW::FeatureType support_type;
|
||||
|
||||
p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count);
|
||||
p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type);
|
||||
|
||||
if (support_type == ShapeSW::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;
|
||||
|
||||
|
|
@ -267,8 +284,25 @@ bool CollisionSolverSW::solve_distance_plane(const ShapeSW *p_shape_A, const Tra
|
|||
static const int max_supports = 16;
|
||||
Vector3 supports[max_supports];
|
||||
int support_count;
|
||||
ShapeSW::FeatureType support_type;
|
||||
|
||||
p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count);
|
||||
p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(), max_supports, supports, support_count, support_type);
|
||||
|
||||
if (support_type == ShapeSW::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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
@ -458,7 +461,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)
|
||||
{
|
||||
|
|
@ -504,7 +507,6 @@ struct GJK
|
|||
{
|
||||
Vector3 n;
|
||||
real_t d;
|
||||
real_t p;
|
||||
sSV* c[3];
|
||||
sFace* f[3];
|
||||
sFace* l[2];
|
||||
|
|
@ -650,7 +652,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; }
|
||||
|
|
@ -689,6 +691,44 @@ struct GJK
|
|||
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)
|
||||
|
|
@ -703,15 +743,16 @@ 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 (!(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))
|
||||
{
|
||||
|
|
@ -732,15 +773,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);
|
||||
|
|
|
|||
|
|
@ -72,7 +72,7 @@ RID PhysicsServerSW::shape_create(ShapeType p_shape) {
|
|||
} break;
|
||||
case SHAPE_CYLINDER: {
|
||||
|
||||
ERR_FAIL_V_MSG(RID(), "CylinderShape is not supported in GodotPhysics. Please switch to Bullet in the Project Settings.");
|
||||
shape = memnew(CylinderShapeSW);
|
||||
} break;
|
||||
case SHAPE_CONVEX_POLYGON: {
|
||||
|
||||
|
|
|
|||
|
|
@ -34,10 +34,12 @@
|
|||
#include "core/math/quick_hull.h"
|
||||
#include "core/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 ShapeSW::configure(const AABB &p_aabb) {
|
||||
aabb = p_aabb;
|
||||
configured = true;
|
||||
|
|
@ -51,7 +53,8 @@ Vector3 ShapeSW::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;
|
||||
}
|
||||
|
||||
|
|
@ -184,18 +187,21 @@ Vector3 RayShapeSW::get_support(const Vector3 &p_normal) const {
|
|||
return Vector3(0, 0, 0);
|
||||
}
|
||||
|
||||
void RayShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
|
||||
void RayShapeSW::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);
|
||||
}
|
||||
}
|
||||
|
|
@ -276,10 +282,11 @@ Vector3 SphereShapeSW::get_support(const Vector3 &p_normal) const {
|
|||
return p_normal * radius;
|
||||
}
|
||||
|
||||
void SphereShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
|
||||
void SphereShapeSW::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 SphereShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
|
||||
|
|
@ -352,7 +359,7 @@ Vector3 BoxShapeSW::get_support(const Vector3 &p_normal) const {
|
|||
return point;
|
||||
}
|
||||
|
||||
void BoxShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
|
||||
void BoxShapeSW::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 };
|
||||
|
|
@ -368,6 +375,7 @@ void BoxShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_sup
|
|||
|
||||
bool neg = dot < 0;
|
||||
r_amount = 4;
|
||||
r_type = FEATURE_FACE;
|
||||
|
||||
Vector3 point;
|
||||
point[i] = half_extents[i];
|
||||
|
|
@ -409,6 +417,7 @@ void BoxShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_sup
|
|||
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];
|
||||
|
|
@ -436,6 +445,7 @@ void BoxShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_sup
|
|||
(p_normal.z < 0) ? -half_extents.z : half_extents.z);
|
||||
|
||||
r_amount = 1;
|
||||
r_type = FEATURE_POINT;
|
||||
r_supports[0] = point;
|
||||
}
|
||||
|
||||
|
|
@ -556,7 +566,7 @@ Vector3 CapsuleShapeSW::get_support(const Vector3 &p_normal) const {
|
|||
return n;
|
||||
}
|
||||
|
||||
void CapsuleShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
|
||||
void CapsuleShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
|
||||
|
||||
Vector3 n = p_normal;
|
||||
|
||||
|
|
@ -570,6 +580,7 @@ void CapsuleShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r
|
|||
n *= radius;
|
||||
|
||||
r_amount = 2;
|
||||
r_type = FEATURE_EDGE;
|
||||
r_supports[0] = n;
|
||||
r_supports[0].z += height * 0.5;
|
||||
r_supports[1] = n;
|
||||
|
|
@ -582,6 +593,7 @@ void CapsuleShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r
|
|||
n *= radius;
|
||||
n.z += h * 0.5;
|
||||
r_amount = 1;
|
||||
r_type = FEATURE_POINT;
|
||||
*r_supports = n;
|
||||
}
|
||||
}
|
||||
|
|
@ -709,6 +721,186 @@ CapsuleShapeSW::CapsuleShapeSW() {
|
|||
height = radius = 0;
|
||||
}
|
||||
|
||||
/********** CYLINDER *************/
|
||||
|
||||
void CylinderShapeSW::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 CylinderShapeSW::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 CylinderShapeSW::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 CylinderShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
|
||||
return Geometry::segment_intersects_cylinder(p_begin, p_end, height, radius, &r_result, &r_normal, 1);
|
||||
}
|
||||
|
||||
bool CylinderShapeSW::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 CylinderShapeSW::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 = Geometry::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 CylinderShapeSW::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 CylinderShapeSW::_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 CylinderShapeSW::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 CylinderShapeSW::get_data() const {
|
||||
Dictionary d;
|
||||
d["radius"] = radius;
|
||||
d["height"] = height;
|
||||
return d;
|
||||
}
|
||||
|
||||
CylinderShapeSW::CylinderShapeSW() {
|
||||
height = radius = 0;
|
||||
}
|
||||
|
||||
/********** CONVEX POLYGON *************/
|
||||
|
||||
void ConvexPolygonShapeSW::project_range(const Vector3 &p_normal, const Transform &p_transform, real_t &r_min, real_t &r_max) const {
|
||||
|
|
@ -756,7 +948,7 @@ Vector3 ConvexPolygonShapeSW::get_support(const Vector3 &p_normal) const {
|
|||
return vrts[vert_support_idx];
|
||||
}
|
||||
|
||||
void ConvexPolygonShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
|
||||
void ConvexPolygonShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
|
||||
|
||||
const Geometry::MeshData::Face *faces = mesh.faces.ptr();
|
||||
int fc = mesh.faces.size();
|
||||
|
|
@ -805,6 +997,7 @@ void ConvexPolygonShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vect
|
|||
r_supports[j] = vertices[ind[j]];
|
||||
}
|
||||
r_amount = m;
|
||||
r_type = FEATURE_FACE;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
|
@ -816,6 +1009,7 @@ void ConvexPolygonShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vect
|
|||
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;
|
||||
|
|
@ -824,6 +1018,7 @@ void ConvexPolygonShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vect
|
|||
|
||||
r_supports[0] = vertices[vtx];
|
||||
r_amount = 1;
|
||||
r_type = FEATURE_POINT;
|
||||
}
|
||||
|
||||
bool ConvexPolygonShapeSW::intersect_segment(const Vector3 &p_begin, const Vector3 &p_end, Vector3 &r_result, Vector3 &r_normal) const {
|
||||
|
|
@ -1019,7 +1214,7 @@ Vector3 FaceShapeSW::get_support(const Vector3 &p_normal) const {
|
|||
return vertex[vert_support_idx];
|
||||
}
|
||||
|
||||
void FaceShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount) const {
|
||||
void FaceShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_supports, int &r_amount, FeatureType &r_type) const {
|
||||
|
||||
Vector3 n = p_normal;
|
||||
|
||||
|
|
@ -1027,6 +1222,7 @@ void FaceShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_su
|
|||
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];
|
||||
|
|
@ -1063,6 +1259,7 @@ void FaceShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_su
|
|||
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;
|
||||
|
|
@ -1070,6 +1267,7 @@ void FaceShapeSW::get_supports(const Vector3 &p_normal, int p_max, Vector3 *r_su
|
|||
}
|
||||
|
||||
r_amount = 1;
|
||||
r_type = FEATURE_POINT;
|
||||
r_supports[0] = vertex[vert_support_idx];
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -69,8 +69,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(); }
|
||||
|
|
@ -87,7 +90,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;
|
||||
|
|
@ -113,7 +116,7 @@ class ConcaveShapeSW : public ShapeSW {
|
|||
public:
|
||||
virtual bool is_concave() const { return true; }
|
||||
typedef void (*Callback)(void *p_userdata, ShapeSW *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;
|
||||
|
||||
|
|
@ -133,7 +136,7 @@ public:
|
|||
virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::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;
|
||||
|
|
@ -161,7 +164,7 @@ public:
|
|||
virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::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;
|
||||
|
|
@ -190,7 +193,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;
|
||||
|
|
@ -216,7 +219,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;
|
||||
|
|
@ -246,7 +249,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;
|
||||
|
|
@ -259,6 +262,35 @@ public:
|
|||
CapsuleShapeSW();
|
||||
};
|
||||
|
||||
class CylinderShapeSW : public ShapeSW {
|
||||
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 PhysicsServer::ShapeType get_type() const { return PhysicsServer::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;
|
||||
|
||||
CylinderShapeSW();
|
||||
};
|
||||
|
||||
struct ConvexPolygonShapeSW : public ShapeSW {
|
||||
|
||||
Geometry::MeshData mesh;
|
||||
|
|
@ -272,7 +304,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;
|
||||
|
|
@ -414,7 +446,7 @@ struct FaceShapeSW : public ShapeSW {
|
|||
|
||||
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;
|
||||
|
|
@ -455,7 +487,7 @@ struct MotionShapeSW : public ShapeSW {
|
|||
}
|
||||
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; }
|
||||
|
|
|
|||
Loading…
Reference in a new issue