From 31c2a2489392991c9870f21092f86df6f8bd9c9c Mon Sep 17 00:00:00 2001 From: Peter Eastman Date: Sat, 14 Jan 2023 18:39:23 -0800 Subject: Analytic collision normals --- servers/physics_3d/gjk_epa.cpp | 6 ++- servers/physics_3d/godot_body_pair_3d.cpp | 14 ++--- servers/physics_3d/godot_body_pair_3d.h | 8 +-- servers/physics_3d/godot_collision_solver_3d.cpp | 18 ++++--- servers/physics_3d/godot_collision_solver_3d.h | 4 +- .../physics_3d/godot_collision_solver_3d_sat.cpp | 59 +++++++++++++--------- servers/physics_3d/godot_physics_server_3d.cpp | 2 +- servers/physics_3d/godot_physics_server_3d.h | 2 +- servers/physics_3d/godot_space_3d.cpp | 6 +-- 9 files changed, 67 insertions(+), 52 deletions(-) (limited to 'servers') 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(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(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 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 *separator = (SeparatorAxisTest *)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 @@ -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 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(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; -- cgit v1.2.3