summaryrefslogtreecommitdiff
path: root/servers/physics_3d
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics_3d')
-rw-r--r--servers/physics_3d/gjk_epa.cpp6
-rw-r--r--servers/physics_3d/godot_body_pair_3d.cpp14
-rw-r--r--servers/physics_3d/godot_body_pair_3d.h8
-rw-r--r--servers/physics_3d/godot_collision_solver_3d.cpp18
-rw-r--r--servers/physics_3d/godot_collision_solver_3d.h4
-rw-r--r--servers/physics_3d/godot_collision_solver_3d_sat.cpp103
-rw-r--r--servers/physics_3d/godot_physics_server_3d.cpp2
-rw-r--r--servers/physics_3d/godot_physics_server_3d.h2
-rw-r--r--servers/physics_3d/godot_shape_3d.cpp33
-rw-r--r--servers/physics_3d/godot_soft_body_3d.cpp93
-rw-r--r--servers/physics_3d/godot_space_3d.cpp6
11 files changed, 130 insertions, 159 deletions
diff --git a/servers/physics_3d/gjk_epa.cpp b/servers/physics_3d/gjk_epa.cpp
index 88f2040d17..e5678914fe 100644
--- a/servers/physics_3d/gjk_epa.cpp
+++ b/servers/physics_3d/gjk_epa.cpp
@@ -1011,9 +1011,11 @@ bool gjk_epa_calculate_penetration(const GodotShape3D *p_shape_A, const Transfor
if (GjkEpa2::Penetration(p_shape_A, p_transform_A, p_margin_A, p_shape_B, p_transform_B, p_margin_B, p_transform_B.origin - p_transform_A.origin, res)) {
if (p_result_callback) {
if (p_swap) {
- p_result_callback(res.witnesses[1], 0, res.witnesses[0], 0, p_userdata);
+ Vector3 normal = (res.witnesses[1] - res.witnesses[0]).normalized();
+ p_result_callback(res.witnesses[1], 0, res.witnesses[0], 0, normal, p_userdata);
} else {
- p_result_callback(res.witnesses[0], 0, res.witnesses[1], 0, p_userdata);
+ Vector3 normal = (res.witnesses[0] - res.witnesses[1]).normalized();
+ p_result_callback(res.witnesses[0], 0, res.witnesses[1], 0, normal, p_userdata);
}
}
return true;
diff --git a/servers/physics_3d/godot_body_pair_3d.cpp b/servers/physics_3d/godot_body_pair_3d.cpp
index ce3da390cb..619e6c00be 100644
--- a/servers/physics_3d/godot_body_pair_3d.cpp
+++ b/servers/physics_3d/godot_body_pair_3d.cpp
@@ -38,12 +38,12 @@
#define MIN_VELOCITY 0.0001
#define MAX_BIAS_ROTATION (Math_PI / 8)
-void GodotBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
+void GodotBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata) {
GodotBodyPair3D *pair = static_cast<GodotBodyPair3D *>(p_userdata);
- pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B);
+ pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B, normal);
}
-void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B) {
+void GodotBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal) {
Vector3 local_A = A->get_inv_transform().basis.xform(p_point_A);
Vector3 local_B = B->get_inv_transform().basis.xform(p_point_B - offset_B);
@@ -577,12 +577,12 @@ GodotBodyPair3D::~GodotBodyPair3D() {
B->remove_constraint(this);
}
-void GodotBodySoftBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
+void GodotBodySoftBodyPair3D::_contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata) {
GodotBodySoftBodyPair3D *pair = static_cast<GodotBodySoftBodyPair3D *>(p_userdata);
- pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B);
+ pair->contact_added_callback(p_point_A, p_index_A, p_point_B, p_index_B, normal);
}
-void GodotBodySoftBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B) {
+void GodotBodySoftBodyPair3D::contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal) {
Vector3 local_A = body->get_inv_transform().xform(p_point_A);
Vector3 local_B = p_point_B - soft_body->get_node_position(p_index_B);
@@ -591,7 +591,7 @@ void GodotBodySoftBodyPair3D::contact_added_callback(const Vector3 &p_point_A, i
contact.index_B = p_index_B;
contact.local_A = local_A;
contact.local_B = local_B;
- contact.normal = (p_point_A - p_point_B).normalized();
+ contact.normal = (normal.dot((p_point_A - p_point_B)) < 0 ? -normal : normal);
contact.used = true;
// Attempt to determine if the contact will be reused.
diff --git a/servers/physics_3d/godot_body_pair_3d.h b/servers/physics_3d/godot_body_pair_3d.h
index c3165c7fcf..a8f5180dd5 100644
--- a/servers/physics_3d/godot_body_pair_3d.h
+++ b/servers/physics_3d/godot_body_pair_3d.h
@@ -97,9 +97,9 @@ class GodotBodyPair3D : public GodotBodyContact3D {
Contact contacts[MAX_CONTACTS];
int contact_count = 0;
- static void _contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata);
+ static void _contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata);
- void contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B);
+ void contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal);
void validate_contacts();
bool _test_ccd(real_t p_step, GodotBody3D *p_A, int p_shape_A, const Transform3D &p_xform_A, GodotBody3D *p_B, int p_shape_B, const Transform3D &p_xform_B);
@@ -126,9 +126,9 @@ class GodotBodySoftBodyPair3D : public GodotBodyContact3D {
LocalVector<Contact> contacts;
- static void _contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata);
+ static void _contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata);
- void contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B);
+ void contact_added_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal);
void validate_contacts();
diff --git a/servers/physics_3d/godot_collision_solver_3d.cpp b/servers/physics_3d/godot_collision_solver_3d.cpp
index 0b92b745fe..fb5a67c008 100644
--- a/servers/physics_3d/godot_collision_solver_3d.cpp
+++ b/servers/physics_3d/godot_collision_solver_3d.cpp
@@ -81,9 +81,11 @@ bool GodotCollisionSolver3D::solve_static_world_boundary(const GodotShape3D *p_s
if (p_result_callback) {
if (p_swap_result) {
- p_result_callback(supports[i], 0, support_A, 0, p_userdata);
+ Vector3 normal = (support_A - supports[i]).normalized();
+ p_result_callback(supports[i], 0, support_A, 0, normal, p_userdata);
} else {
- p_result_callback(support_A, 0, supports[i], 0, p_userdata);
+ Vector3 normal = (supports[i] - support_A).normalized();
+ p_result_callback(support_A, 0, supports[i], 0, normal, p_userdata);
}
}
}
@@ -126,9 +128,11 @@ bool GodotCollisionSolver3D::solve_separation_ray(const GodotShape3D *p_shape_A,
if (p_result_callback) {
if (p_swap_result) {
- p_result_callback(support_B, 0, support_A, 0, p_userdata);
+ Vector3 normal = (support_B - support_A).normalized();
+ p_result_callback(support_B, 0, support_A, 0, normal, p_userdata);
} else {
- p_result_callback(support_A, 0, support_B, 0, p_userdata);
+ Vector3 normal = (support_A - support_B).normalized();
+ p_result_callback(support_A, 0, support_B, 0, normal, p_userdata);
}
}
return true;
@@ -142,7 +146,7 @@ struct _SoftBodyContactCollisionInfo {
int contact_count = 0;
};
-void GodotCollisionSolver3D::soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
+void GodotCollisionSolver3D::soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata) {
_SoftBodyContactCollisionInfo &cinfo = *(static_cast<_SoftBodyContactCollisionInfo *>(p_userdata));
++cinfo.contact_count;
@@ -152,9 +156,9 @@ void GodotCollisionSolver3D::soft_body_contact_callback(const Vector3 &p_point_A
}
if (cinfo.swap_result) {
- cinfo.result_callback(p_point_B, cinfo.node_index, p_point_A, p_index_A, cinfo.userdata);
+ cinfo.result_callback(p_point_B, cinfo.node_index, p_point_A, p_index_A, -normal, cinfo.userdata);
} else {
- cinfo.result_callback(p_point_A, p_index_A, p_point_B, cinfo.node_index, cinfo.userdata);
+ cinfo.result_callback(p_point_A, p_index_A, p_point_B, cinfo.node_index, normal, cinfo.userdata);
}
}
diff --git a/servers/physics_3d/godot_collision_solver_3d.h b/servers/physics_3d/godot_collision_solver_3d.h
index 7ef0dc97ac..36ea79576e 100644
--- a/servers/physics_3d/godot_collision_solver_3d.h
+++ b/servers/physics_3d/godot_collision_solver_3d.h
@@ -35,11 +35,11 @@
class GodotCollisionSolver3D {
public:
- typedef void (*CallbackResult)(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata);
+ typedef void (*CallbackResult)(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata);
private:
static bool soft_body_query_callback(uint32_t p_node_index, void *p_userdata);
- static void soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata);
+ static void soft_body_contact_callback(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata);
static bool soft_body_concave_callback(void *p_userdata, GodotShape3D *p_convex);
static bool concave_callback(void *p_userdata, GodotShape3D *p_convex);
static bool solve_static_world_boundary(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, real_t p_margin = 0);
diff --git a/servers/physics_3d/godot_collision_solver_3d_sat.cpp b/servers/physics_3d/godot_collision_solver_3d_sat.cpp
index d13f4ee801..2cb29b3dd0 100644
--- a/servers/physics_3d/godot_collision_solver_3d_sat.cpp
+++ b/servers/physics_3d/godot_collision_solver_3d_sat.cpp
@@ -75,11 +75,13 @@ struct _CollectorCallback {
Vector3 normal;
Vector3 *prev_axis = nullptr;
- _FORCE_INLINE_ void call(const Vector3 &p_point_A, const Vector3 &p_point_B) {
+ _FORCE_INLINE_ void call(const Vector3 &p_point_A, const Vector3 &p_point_B, Vector3 p_normal) {
+ if (p_normal.dot(p_point_B - p_point_A) < 0)
+ p_normal = -p_normal;
if (swap) {
- callback(p_point_B, 0, p_point_A, 0, userdata);
+ callback(p_point_B, 0, p_point_A, 0, -p_normal, userdata);
} else {
- callback(p_point_A, 0, p_point_B, 0, userdata);
+ callback(p_point_A, 0, p_point_B, 0, p_normal, userdata);
}
}
};
@@ -92,7 +94,7 @@ static void _generate_contacts_point_point(const Vector3 *p_points_A, int p_poin
ERR_FAIL_COND(p_point_count_B != 1);
#endif
- p_callback->call(*p_points_A, *p_points_B);
+ p_callback->call(*p_points_A, *p_points_B, p_callback->normal);
}
static void _generate_contacts_point_edge(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
@@ -102,7 +104,7 @@ static void _generate_contacts_point_edge(const Vector3 *p_points_A, int p_point
#endif
Vector3 closest_B = Geometry3D::get_closest_point_to_segment_uncapped(*p_points_A, p_points_B);
- p_callback->call(*p_points_A, closest_B);
+ p_callback->call(*p_points_A, closest_B, p_callback->normal);
}
static void _generate_contacts_point_face(const Vector3 *p_points_A, int p_point_count_A, const Vector3 *p_points_B, int p_point_count_B, _CollectorCallback *p_callback) {
@@ -111,9 +113,9 @@ static void _generate_contacts_point_face(const Vector3 *p_points_A, int p_point
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);
+ Plane plane(p_points_B[0], p_points_B[1], p_points_B[2]);
+ Vector3 closest_B = plane.project(*p_points_A);
+ p_callback->call(*p_points_A, closest_B, plane.get_normal());
}
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) {
@@ -122,9 +124,9 @@ static void _generate_contacts_point_circle(const Vector3 *p_points_A, int p_poi
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);
+ Plane plane(p_points_B[0], p_points_B[1], p_points_B[2]);
+ Vector3 closest_B = plane.project(*p_points_A);
+ p_callback->call(*p_points_A, closest_B, plane.get_normal());
}
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) {
@@ -154,8 +156,8 @@ static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_
sa.sort(dvec, 4);
//use the middle ones as contacts
- p_callback->call(base_A + axis * dvec[1], base_B + axis * dvec[1]);
- p_callback->call(base_A + axis * dvec[2], base_B + axis * dvec[2]);
+ p_callback->call(base_A + axis * dvec[1], base_B + axis * dvec[1], p_callback->normal);
+ p_callback->call(base_A + axis * dvec[2], base_B + axis * dvec[2], p_callback->normal);
return;
}
@@ -170,7 +172,14 @@ static void _generate_contacts_edge_edge(const Vector3 *p_points_A, int p_point_
Vector3 closest_A = p_points_A[0] + rel_A * d;
Vector3 closest_B = Geometry3D::get_closest_point_to_segment_uncapped(closest_A, p_points_B);
- p_callback->call(closest_A, closest_B);
+ // The normal should be perpendicular to both edges.
+ Vector3 normal = rel_A.cross(rel_B);
+ real_t normal_len = normal.length();
+ if (normal_len > 1e-3)
+ normal /= normal_len;
+ else
+ normal = p_callback->normal;
+ p_callback->call(closest_A, closest_B, normal);
}
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) {
@@ -267,7 +276,7 @@ static void _generate_contacts_edge_circle(const Vector3 *p_points_A, int p_poin
continue;
}
- p_callback->call(contact_point_A, closest_B);
+ p_callback->call(contact_point_A, closest_B, circle_plane.get_normal());
}
}
@@ -352,7 +361,7 @@ static void _generate_contacts_face_face(const Vector3 *p_points_A, int p_point_
continue;
}
- p_callback->call(clipbuf_src[i], closest_B);
+ p_callback->call(clipbuf_src[i], closest_B, plane_B.get_normal());
}
}
@@ -431,7 +440,7 @@ static void _generate_contacts_face_circle(const Vector3 *p_points_A, int p_poin
continue;
}
- p_callback->call(contact_point_A, closest_B);
+ p_callback->call(contact_point_A, closest_B, circle_plane.get_normal());
}
}
@@ -534,7 +543,7 @@ static void _generate_contacts_circle_circle(const Vector3 *p_points_A, int p_po
continue;
}
- p_callback->call(contact_point_A, closest_B);
+ p_callback->call(contact_point_A, closest_B, circle_B_plane.get_normal());
}
}
@@ -678,7 +687,7 @@ public:
return true;
}
- static _FORCE_INLINE_ void test_contact_points(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
+ static _FORCE_INLINE_ void test_contact_points(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, 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();
@@ -802,11 +811,11 @@ static void analytic_sphere_collision(const Vector3 &p_origin_a, real_t p_radius
if (p_radius_a < p_radius_b) {
Vector3 point_a = p_origin_a - b_to_a * p_radius_a;
Vector3 point_b = point_a + b_to_a * overlap;
- p_collector->call(point_a, point_b); // Consider adding b_to_a vector
+ p_collector->call(point_a, point_b, b_to_a); // Consider adding b_to_a vector
} else {
Vector3 point_b = p_origin_b + b_to_a * p_radius_b;
Vector3 point_a = point_b - b_to_a * overlap;
- p_collector->call(point_a, point_b); // Consider adding b_to_a vector
+ p_collector->call(point_a, point_b, b_to_a); // Consider adding b_to_a vector
}
}
@@ -818,9 +827,9 @@ static void _collision_sphere_sphere(const GodotShape3D *p_a, const Transform3D
// Perform an analytic sphere collision between the two spheres
analytic_sphere_collision<withMargin>(
p_transform_a.origin,
- sphere_A->get_radius(),
+ sphere_A->get_radius() * p_transform_a.basis[0].length(),
p_transform_b.origin,
- sphere_B->get_radius(),
+ sphere_B->get_radius() * p_transform_b.basis[0].length(),
p_collector,
p_margin_a,
p_margin_b);
@@ -833,7 +842,7 @@ static void _collision_sphere_box(const GodotShape3D *p_a, const Transform3D &p_
// Find the point on the box nearest to the center of the sphere.
- Vector3 center = p_transform_b.xform_inv(p_transform_a.origin);
+ Vector3 center = p_transform_b.affine_inverse().xform(p_transform_a.origin);
Vector3 extents = box_B->get_half_extents();
Vector3 nearest(MIN(MAX(center.x, -extents.x), extents.x),
MIN(MAX(center.y, -extents.y), extents.y),
@@ -844,7 +853,8 @@ static void _collision_sphere_box(const GodotShape3D *p_a, const Transform3D &p_
Vector3 delta = nearest - p_transform_a.origin;
real_t length = delta.length();
- if (length > sphere_A->get_radius() + p_margin_a + p_margin_b) {
+ real_t radius = sphere_A->get_radius() * p_transform_a.basis[0].length();
+ if (length > radius + p_margin_a + p_margin_b) {
return;
}
p_collector->collided = true;
@@ -858,9 +868,9 @@ static void _collision_sphere_box(const GodotShape3D *p_a, const Transform3D &p_
} else {
axis = delta / length;
}
- Vector3 point_a = p_transform_a.origin + (sphere_A->get_radius() + p_margin_a) * axis;
- Vector3 point_b = (withMargin ? nearest + p_margin_b * axis : nearest);
- p_collector->call(point_a, point_b);
+ Vector3 point_a = p_transform_a.origin + (radius + p_margin_a) * axis;
+ Vector3 point_b = (withMargin ? nearest - p_margin_b * axis : nearest);
+ p_collector->call(point_a, point_b, axis);
}
template <bool withMargin>
@@ -868,11 +878,12 @@ static void _collision_sphere_capsule(const GodotShape3D *p_a, const Transform3D
const GodotSphereShape3D *sphere_A = static_cast<const GodotSphereShape3D *>(p_a);
const GodotCapsuleShape3D *capsule_B = static_cast<const GodotCapsuleShape3D *>(p_b);
- real_t capsule_B_radius = capsule_B->get_radius();
+ real_t scale_A = p_transform_a.basis[0].length();
+ real_t scale_B = p_transform_b.basis[0].length();
// Construct the capsule segment (ball-center to ball-center)
Vector3 capsule_segment[2];
- Vector3 capsule_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B_radius);
+ Vector3 capsule_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius());
capsule_segment[0] = p_transform_b.origin + capsule_axis;
capsule_segment[1] = p_transform_b.origin - capsule_axis;
@@ -882,9 +893,9 @@ static void _collision_sphere_capsule(const GodotShape3D *p_a, const Transform3D
// Perform an analytic sphere collision between the sphere and the sphere-collider in the capsule
analytic_sphere_collision<withMargin>(
p_transform_a.origin,
- sphere_A->get_radius(),
+ sphere_A->get_radius() * scale_A,
capsule_closest,
- capsule_B_radius,
+ capsule_B->get_radius() * scale_B,
p_collector,
p_margin_a,
p_margin_b);
@@ -894,12 +905,12 @@ template <bool withMargin>
static void analytic_sphere_cylinder_collision(real_t p_radius_a, real_t p_radius_b, real_t p_height_b, const Transform3D &p_transform_a, const Transform3D &p_transform_b, _CollectorCallback *p_collector, real_t p_margin_a, real_t p_margin_b) {
// Find the point on the cylinder nearest to the center of the sphere.
- Vector3 center = p_transform_b.xform_inv(p_transform_a.origin);
+ Vector3 center = p_transform_b.affine_inverse().xform(p_transform_a.origin);
Vector3 nearest = center;
- real_t radius = p_radius_b;
+ real_t scale_A = p_transform_a.basis[0].length();
real_t r = Math::sqrt(center.x * center.x + center.z * center.z);
- if (r > radius) {
- real_t scale = radius / r;
+ if (r > p_radius_b) {
+ real_t scale = p_radius_b / r;
nearest.x *= scale;
nearest.z *= scale;
}
@@ -911,7 +922,7 @@ static void analytic_sphere_cylinder_collision(real_t p_radius_a, real_t p_radiu
Vector3 delta = nearest - p_transform_a.origin;
real_t length = delta.length();
- if (length > p_radius_a + p_margin_a + p_margin_b) {
+ if (length > p_radius_a * scale_A + p_margin_a + p_margin_b) {
return;
}
p_collector->collided = true;
@@ -925,9 +936,9 @@ static void analytic_sphere_cylinder_collision(real_t p_radius_a, real_t p_radiu
} else {
axis = delta / length;
}
- Vector3 point_a = p_transform_a.origin + (p_radius_a + p_margin_a) * axis;
- Vector3 point_b = (withMargin ? nearest + p_margin_b * axis : nearest);
- p_collector->call(point_a, point_b);
+ Vector3 point_a = p_transform_a.origin + (p_radius_a * scale_A + p_margin_a) * axis;
+ Vector3 point_b = (withMargin ? nearest - p_margin_b * axis : nearest);
+ p_collector->call(point_a, point_b, axis);
}
template <bool withMargin>
@@ -1623,14 +1634,14 @@ static void _collision_capsule_capsule(const GodotShape3D *p_a, const Transform3
const GodotCapsuleShape3D *capsule_A = static_cast<const GodotCapsuleShape3D *>(p_a);
const GodotCapsuleShape3D *capsule_B = static_cast<const GodotCapsuleShape3D *>(p_b);
- real_t capsule_A_radius = capsule_A->get_radius();
- real_t capsule_B_radius = capsule_B->get_radius();
+ real_t scale_A = p_transform_a.basis[0].length();
+ real_t scale_B = p_transform_b.basis[0].length();
// Get the closest points between the capsule segments
Vector3 capsule_A_closest;
Vector3 capsule_B_closest;
- Vector3 capsule_A_axis = p_transform_a.basis.get_column(1) * (capsule_A->get_height() * 0.5 - capsule_A_radius);
- Vector3 capsule_B_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B_radius);
+ Vector3 capsule_A_axis = p_transform_a.basis.get_column(1) * (capsule_A->get_height() * 0.5 - capsule_A->get_radius());
+ Vector3 capsule_B_axis = p_transform_b.basis.get_column(1) * (capsule_B->get_height() * 0.5 - capsule_B->get_radius());
Geometry3D::get_closest_points_between_segments(
p_transform_a.origin + capsule_A_axis,
p_transform_a.origin - capsule_A_axis,
@@ -1642,9 +1653,9 @@ static void _collision_capsule_capsule(const GodotShape3D *p_a, const Transform3
// Perform the analytic collision between the two closest capsule spheres
analytic_sphere_collision<withMargin>(
capsule_A_closest,
- capsule_A_radius,
+ capsule_A->get_radius() * scale_A,
capsule_B_closest,
- capsule_B_radius,
+ capsule_B->get_radius() * scale_B,
p_collector,
p_margin_a,
p_margin_b);
diff --git a/servers/physics_3d/godot_physics_server_3d.cpp b/servers/physics_3d/godot_physics_server_3d.cpp
index e8250acb45..b6d8acfbf3 100644
--- a/servers/physics_3d/godot_physics_server_3d.cpp
+++ b/servers/physics_3d/godot_physics_server_3d.cpp
@@ -1737,7 +1737,7 @@ void GodotPhysicsServer3D::_update_shapes() {
}
}
-void GodotPhysicsServer3D::_shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
+void GodotPhysicsServer3D::_shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata) {
CollCbkData *cbk = static_cast<CollCbkData *>(p_userdata);
if (cbk->max == 0) {
diff --git a/servers/physics_3d/godot_physics_server_3d.h b/servers/physics_3d/godot_physics_server_3d.h
index 3da0c6debe..040e673dcd 100644
--- a/servers/physics_3d/godot_physics_server_3d.h
+++ b/servers/physics_3d/godot_physics_server_3d.h
@@ -77,7 +77,7 @@ public:
Vector3 *ptr = nullptr;
};
- static void _shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata);
+ static void _shape_col_cbk(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata);
virtual RID world_boundary_shape_create() override;
virtual RID separation_ray_shape_create() override;
diff --git a/servers/physics_3d/godot_shape_3d.cpp b/servers/physics_3d/godot_shape_3d.cpp
index 8aacfa929f..300dca4e08 100644
--- a/servers/physics_3d/godot_shape_3d.cpp
+++ b/servers/physics_3d/godot_shape_3d.cpp
@@ -852,17 +852,12 @@ Vector3 GodotConvexPolygonShape3D::get_support(const Vector3 &p_normal) const {
// Get the array of vertices
const Vector3 *const vertices_array = mesh.vertices.ptr();
- // Get the array of extreme vertices
- const int *const extreme_array = extreme_vertices.ptr();
- const uint32_t extreme_size = extreme_vertices.size();
-
- // Start with an initial assumption of the first extreme vertex
- int best_vertex = extreme_array[0];
+ // Start with an initial assumption of the first extreme vertex.
+ int best_vertex = extreme_vertices[0];
real_t max_support = p_normal.dot(vertices_array[best_vertex]);
- // Check the remaining extreme vertices for a better vertex
- for (uint32_t i = 0; i < extreme_size; ++i) {
- int vert = extreme_array[i];
+ // Check the remaining extreme vertices for a better vertex.
+ for (const int &vert : extreme_vertices) {
real_t s = p_normal.dot(vertices_array[vert]);
if (s > max_support) {
best_vertex = vert;
@@ -870,27 +865,18 @@ Vector3 GodotConvexPolygonShape3D::get_support(const Vector3 &p_normal) const {
}
}
- // If we checked all vertices in the mesh then we're done
- if (extreme_size == mesh.vertices.size()) {
+ // If we checked all vertices in the mesh then we're done.
+ if (extreme_vertices.size() == mesh.vertices.size()) {
return vertices_array[best_vertex];
}
- // Get the array of neighbor arrays for each vertex
- const LocalVector<int> *const vertex_neighbors_array = vertex_neighbors.ptr();
-
// Move along the surface until we reach the true support vertex.
int last_vertex = -1;
while (true) {
int next_vertex = -1;
- // Get the array of neighbors around the best vertex
- const LocalVector<int> &neighbors = vertex_neighbors_array[best_vertex];
- const int *const neighbors_array = neighbors.ptr();
- const uint32_t neighbors_size = neighbors.size();
-
- // Iterate over all the neighbors checking for a better vertex
- for (uint32_t i = 0; i < neighbors_size; ++i) {
- int vert = neighbors_array[i];
+ // Iterate over all the neighbors checking for a better vertex.
+ for (const int &vert : vertex_neighbors[best_vertex]) {
if (vert != last_vertex) {
real_t s = p_normal.dot(vertices_array[vert]);
if (s > max_support) {
@@ -1149,8 +1135,7 @@ void GodotConvexPolygonShape3D::_setup(const Vector<Vector3> &p_vertices) {
if (extreme_vertices.size() < mesh.vertices.size()) {
vertex_neighbors.resize(mesh.vertices.size());
- for (uint32_t i = 0; i < mesh.edges.size(); i++) {
- Geometry3D::MeshData::Edge &edge = mesh.edges[i];
+ for (Geometry3D::MeshData::Edge &edge : mesh.edges) {
vertex_neighbors[edge.vertex_a].push_back(edge.vertex_b);
vertex_neighbors[edge.vertex_b].push_back(edge.vertex_a);
}
diff --git a/servers/physics_3d/godot_soft_body_3d.cpp b/servers/physics_3d/godot_soft_body_3d.cpp
index 871017a5e7..1820a29553 100644
--- a/servers/physics_3d/godot_soft_body_3d.cpp
+++ b/servers/physics_3d/godot_soft_body_3d.cpp
@@ -167,14 +167,11 @@ void GodotSoftBody3D::update_rendering_server(PhysicsServer3DRenderingServerHand
}
void GodotSoftBody3D::update_normals_and_centroids() {
- uint32_t i, ni;
-
- for (i = 0, ni = nodes.size(); i < ni; ++i) {
- nodes[i].n = Vector3();
+ for (Node &node : nodes) {
+ node.n = Vector3();
}
- for (i = 0, ni = faces.size(); i < ni; ++i) {
- Face &face = faces[i];
+ for (Face &face : faces) {
const Vector3 n = vec3_cross(face.n[0]->x - face.n[2]->x, face.n[0]->x - face.n[1]->x);
face.n[0]->n += n;
face.n[1]->n += n;
@@ -184,8 +181,7 @@ void GodotSoftBody3D::update_normals_and_centroids() {
face.centroid = 0.33333333333 * (face.n[0]->x + face.n[1]->x + face.n[2]->x);
}
- for (i = 0, ni = nodes.size(); i < ni; ++i) {
- Node &node = nodes[i];
+ for (Node &node : nodes) {
real_t len = node.n.length();
if (len > CMP_EPSILON) {
node.n /= len;
@@ -235,9 +231,7 @@ void GodotSoftBody3D::update_area() {
int i, ni;
// Face area.
- for (i = 0, ni = faces.size(); i < ni; ++i) {
- Face &face = faces[i];
-
+ for (Face &face : faces) {
const Vector3 &x0 = face.n[0]->x;
const Vector3 &x1 = face.n[1]->x;
const Vector3 &x2 = face.n[2]->x;
@@ -255,12 +249,11 @@ void GodotSoftBody3D::update_area() {
memset(counts.ptr(), 0, counts.size() * sizeof(int));
}
- for (i = 0, ni = nodes.size(); i < ni; ++i) {
- nodes[i].area = 0.0;
+ for (Node &node : nodes) {
+ node.area = 0.0;
}
- for (i = 0, ni = faces.size(); i < ni; ++i) {
- const Face &face = faces[i];
+ for (const Face &face : faces) {
for (int j = 0; j < 3; ++j) {
const int index = (int)(face.n[j] - &nodes[0]);
counts[index]++;
@@ -278,8 +271,7 @@ void GodotSoftBody3D::update_area() {
}
void GodotSoftBody3D::reset_link_rest_lengths() {
- for (uint32_t i = 0, ni = links.size(); i < ni; ++i) {
- Link &link = links[i];
+ for (Link &link : links) {
link.rl = (link.n[0]->x - link.n[1]->x).length();
link.c1 = link.rl * link.rl;
}
@@ -287,8 +279,7 @@ void GodotSoftBody3D::reset_link_rest_lengths() {
void GodotSoftBody3D::update_link_constants() {
real_t inv_linear_stiffness = 1.0 / linear_stiffness;
- for (uint32_t i = 0, ni = links.size(); i < ni; ++i) {
- Link &link = links[i];
+ for (Link &link : links) {
link.c0 = (link.n[0]->im + link.n[1]->im) * inv_linear_stiffness;
}
}
@@ -619,9 +610,9 @@ void GodotSoftBody3D::generate_bending_constraints(int p_distance) {
}
}
}
- for (i = 0; i < links.size(); ++i) {
- const int ia = (int)(links[i].n[0] - &nodes[0]);
- const int ib = (int)(links[i].n[1] - &nodes[0]);
+ for (Link &link : links) {
+ const int ia = (int)(link.n[0] - &nodes[0]);
+ const int ib = (int)(link.n[1] - &nodes[0]);
int idx = ib * n + ia;
int idx_inv = ia * n + ib;
adj[idx] = 1;
@@ -635,9 +626,9 @@ void GodotSoftBody3D::generate_bending_constraints(int p_distance) {
// Build node links.
node_links.resize(nodes.size());
- for (i = 0; i < links.size(); ++i) {
- const int ia = (int)(links[i].n[0] - &nodes[0]);
- const int ib = (int)(links[i].n[1] - &nodes[0]);
+ for (Link &link : links) {
+ const int ia = (int)(link.n[0] - &nodes[0]);
+ const int ib = (int)(link.n[1] - &nodes[0]);
if (node_links[ia].find(ib) == -1) {
node_links[ia].push_back(ib);
}
@@ -649,8 +640,7 @@ void GodotSoftBody3D::generate_bending_constraints(int p_distance) {
for (uint32_t ii = 0; ii < node_links.size(); ii++) {
for (uint32_t jj = 0; jj < node_links[ii].size(); jj++) {
int k = node_links[ii][jj];
- for (uint32_t kk = 0; kk < node_links[k].size(); kk++) {
- int l = node_links[k][kk];
+ for (const int &l : node_links[k]) {
if ((int)ii != l) {
int idx_ik = k * n + ii;
int idx_kj = l * n + k;
@@ -916,8 +906,7 @@ void GodotSoftBody3D::set_drag_coefficient(real_t p_val) {
}
void GodotSoftBody3D::add_velocity(const Vector3 &p_velocity) {
- for (uint32_t i = 0, ni = nodes.size(); i < ni; ++i) {
- Node &node = nodes[i];
+ for (Node &node : nodes) {
if (node.im > 0) {
node.v += p_velocity;
}
@@ -929,26 +918,22 @@ void GodotSoftBody3D::apply_forces(const LocalVector<GodotArea3D *> &p_wind_area
return;
}
- uint32_t i, ni;
int32_t j;
real_t volume = 0.0;
const Vector3 &org = nodes[0].x;
// Iterate over faces (try not to iterate elsewhere if possible).
- for (i = 0, ni = faces.size(); i < ni; ++i) {
- const Face &face = faces[i];
-
+ for (const Face &face : faces) {
Vector3 wind_force(0, 0, 0);
// Compute volume.
volume += vec3_dot(face.n[0]->x - org, vec3_cross(face.n[1]->x - org, face.n[2]->x - org));
// Compute nodal forces from area winds.
- int wind_area_count = p_wind_areas.size();
- if (wind_area_count > 0) {
- for (j = 0; j < wind_area_count; j++) {
- wind_force += _compute_area_windforce(p_wind_areas[j], &face);
+ if (!p_wind_areas.is_empty()) {
+ for (const GodotArea3D *area : p_wind_areas) {
+ wind_force += _compute_area_windforce(area, &face);
}
for (j = 0; j < 3; j++) {
@@ -962,8 +947,7 @@ void GodotSoftBody3D::apply_forces(const LocalVector<GodotArea3D *> &p_wind_area
// Apply nodal pressure forces.
if (pressure_coefficient > CMP_EPSILON) {
real_t ivolumetp = 1.0 / Math::abs(volume) * pressure_coefficient;
- for (i = 0, ni = nodes.size(); i < ni; ++i) {
- Node &node = nodes[i];
+ for (Node &node : nodes) {
if (node.im > 0) {
node.f += node.n * (node.area * ivolumetp);
}
@@ -1048,9 +1032,7 @@ void GodotSoftBody3D::predict_motion(real_t p_delta) {
real_t clamp_delta_v = max_displacement * inv_delta;
// Integrate.
- uint32_t i, ni;
- for (i = 0, ni = nodes.size(); i < ni; ++i) {
- Node &node = nodes[i];
+ for (Node &node : nodes) {
node.q = node.x;
Vector3 delta_v = node.f * node.im * p_delta;
for (int c = 0; c < 3; c++) {
@@ -1065,9 +1047,7 @@ void GodotSoftBody3D::predict_motion(real_t p_delta) {
update_bounds();
// Node tree update.
- for (i = 0, ni = nodes.size(); i < ni; ++i) {
- const Node &node = nodes[i];
-
+ for (const Node &node : nodes) {
AABB node_aabb(node.x, Vector3());
node_aabb.expand_to(node.x + node.v * p_delta);
node_aabb.grow_by(collision_margin);
@@ -1088,17 +1068,13 @@ void GodotSoftBody3D::predict_motion(real_t p_delta) {
void GodotSoftBody3D::solve_constraints(real_t p_delta) {
const real_t inv_delta = 1.0 / p_delta;
- uint32_t i, ni;
-
- for (i = 0, ni = links.size(); i < ni; ++i) {
- Link &link = links[i];
+ for (Link &link : links) {
link.c3 = link.n[1]->q - link.n[0]->q;
link.c2 = 1 / (link.c3.length_squared() * link.c0);
}
// Solve velocities.
- for (i = 0, ni = nodes.size(); i < ni; ++i) {
- Node &node = nodes[i];
+ for (Node &node : nodes) {
node.x = node.q + node.v * p_delta;
}
@@ -1108,9 +1084,7 @@ void GodotSoftBody3D::solve_constraints(real_t p_delta) {
solve_links(1.0, ti);
}
const real_t vc = (1.0 - damping_coefficient) * inv_delta;
- for (i = 0, ni = nodes.size(); i < ni; ++i) {
- Node &node = nodes[i];
-
+ for (Node &node : nodes) {
node.x += node.bv * p_delta;
node.bv = Vector3();
@@ -1123,8 +1097,7 @@ void GodotSoftBody3D::solve_constraints(real_t p_delta) {
}
void GodotSoftBody3D::solve_links(real_t kst, real_t ti) {
- for (uint32_t i = 0, ni = links.size(); i < ni; ++i) {
- Link &link = links[i];
+ for (Link &link : links) {
if (link.c0 > 0) {
Node &node_a = *link.n[0];
Node &node_b = *link.n[1];
@@ -1183,9 +1156,7 @@ void GodotSoftBody3D::query_ray(const Vector3 &p_from, const Vector3 &p_to, Godo
void GodotSoftBody3D::initialize_face_tree() {
face_tree.clear();
- for (uint32_t i = 0; i < faces.size(); ++i) {
- Face &face = faces[i];
-
+ for (Face &face : faces) {
AABB face_aabb;
face_aabb.position = face.n[0]->x;
@@ -1199,9 +1170,7 @@ void GodotSoftBody3D::initialize_face_tree() {
}
void GodotSoftBody3D::update_face_tree(real_t p_delta) {
- for (uint32_t i = 0; i < faces.size(); ++i) {
- const Face &face = faces[i];
-
+ for (const Face &face : faces) {
AABB face_aabb;
const Node *node0 = face.n[0];
diff --git a/servers/physics_3d/godot_space_3d.cpp b/servers/physics_3d/godot_space_3d.cpp
index c3aad22932..93572965d2 100644
--- a/servers/physics_3d/godot_space_3d.cpp
+++ b/servers/physics_3d/godot_space_3d.cpp
@@ -445,7 +445,7 @@ struct _RestCallbackData {
_RestResultData *other_results = nullptr;
};
-static void _rest_cbk_result(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, void *p_userdata) {
+static void _rest_cbk_result(const Vector3 &p_point_A, int p_index_A, const Vector3 &p_point_B, int p_index_B, const Vector3 &normal, void *p_userdata) {
_RestCallbackData *rd = static_cast<_RestCallbackData *>(p_userdata);
Vector3 contact_rel = p_point_B - p_point_A;
@@ -480,7 +480,7 @@ static void _rest_cbk_result(const Vector3 &p_point_A, int p_index_A, const Vect
// Keep this result as separate result.
result.len = len;
result.contact = p_point_B;
- result.normal = contact_rel / len;
+ result.normal = normal;
result.object = rd->object;
result.shape = rd->shape;
result.local_shape = rd->local_shape;
@@ -499,7 +499,7 @@ static void _rest_cbk_result(const Vector3 &p_point_A, int p_index_A, const Vect
rd->best_result.len = len;
rd->best_result.contact = p_point_B;
- rd->best_result.normal = contact_rel / len;
+ rd->best_result.normal = normal;
rd->best_result.object = rd->object;
rd->best_result.shape = rd->shape;
rd->best_result.local_shape = rd->local_shape;
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-24 21:23:54 +0000