Analytic collision normals

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;