summaryrefslogtreecommitdiff
path: root/servers
diff options
context:
space:
mode:
Diffstat (limited to 'servers')
-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.cpp59
-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_space_3d.cpp6
9 files changed, 67 insertions, 52 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..66d1811abb 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
}
}
@@ -859,8 +868,8 @@ static void _collision_sphere_box(const GodotShape3D *p_a, const Transform3D &p_
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_b = (withMargin ? nearest - p_margin_b * axis : nearest);
+ p_collector->call(point_a, point_b, axis);
}
template <bool withMargin>
@@ -926,8 +935,8 @@ static void analytic_sphere_cylinder_collision(real_t p_radius_a, real_t p_radiu
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_b = (withMargin ? nearest - p_margin_b * axis : nearest);
+ p_collector->call(point_a, point_b, axis);
}
template <bool withMargin>
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_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-05 23:28:54 +0000