Implement collision impulse in Godot Physics 3D

5 files changed, 47 insertions, 35 deletions

diff --git a/servers/physics_3d/godot_body_3d.h b/servers/physics_3d/godot_body_3d.h
index 412cbebc7d..6fb7129c83 100644
--- a/servers/physics_3d/godot_body_3d.h
+++ b/servers/physics_3d/godot_body_3d.h
@@ -126,6 +126,7 @@ class GodotBody3D : public GodotCollisionObject3D {
 		ObjectID collider_instance_id;
 		RID collider;
 		Vector3 collider_velocity_at_pos;
+		Vector3 impulse;
 	};
 
 	Vector<Contact> contacts; //no contacts by default
@@ -183,7 +184,7 @@ public:
 	_FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); }
 
 	_FORCE_INLINE_ bool can_report_contacts() const { return !contacts.is_empty(); }
-	_FORCE_INLINE_ void add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos);
+	_FORCE_INLINE_ void add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos, const Vector3 &p_impulse);
 
 	_FORCE_INLINE_ void add_exception(const RID &p_exception) { exceptions.insert(p_exception); }
 	_FORCE_INLINE_ void remove_exception(const RID &p_exception) { exceptions.erase(p_exception); }
@@ -347,7 +348,7 @@ public:
 
 //add contact inline
 
-void GodotBody3D::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos) {
+void GodotBody3D::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local_normal, real_t p_depth, int p_local_shape, const Vector3 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector3 &p_collider_velocity_at_pos, const Vector3 &p_impulse) {
 	int c_max = contacts.size();
 
 	if (c_max == 0) {
@@ -387,6 +388,7 @@ void GodotBody3D::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local
 	c[idx].collider_instance_id = p_collider_instance_id;
 	c[idx].collider = p_collider;
 	c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos;
+	c[idx].impulse = p_impulse;
 }
 
 #endif // GODOT_BODY_3D_H
diff --git a/servers/physics_3d/godot_body_direct_state_3d.cpp b/servers/physics_3d/godot_body_direct_state_3d.cpp
index 25088d33f3..1a714ca586 100644
--- a/servers/physics_3d/godot_body_direct_state_3d.cpp
+++ b/servers/physics_3d/godot_body_direct_state_3d.cpp
@@ -188,8 +188,9 @@ Vector3 GodotPhysicsDirectBodyState3D::get_contact_local_normal(int p_contact_id
 	return body->contacts[p_contact_idx].local_normal;
 }
 
-real_t GodotPhysicsDirectBodyState3D::get_contact_impulse(int p_contact_idx) const {
-	return 0.0f; // Only implemented for bullet
+Vector3 GodotPhysicsDirectBodyState3D::get_contact_impulse(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
+	return body->contacts[p_contact_idx].impulse;
 }
 
 int GodotPhysicsDirectBodyState3D::get_contact_local_shape(int p_contact_idx) const {
diff --git a/servers/physics_3d/godot_body_direct_state_3d.h b/servers/physics_3d/godot_body_direct_state_3d.h
index 483cfb9298..6ee5171ded 100644
--- a/servers/physics_3d/godot_body_direct_state_3d.h
+++ b/servers/physics_3d/godot_body_direct_state_3d.h
@@ -89,7 +89,7 @@ public:
 
 	virtual Vector3 get_contact_local_position(int p_contact_idx) const override;
 	virtual Vector3 get_contact_local_normal(int p_contact_idx) const override;
-	virtual real_t get_contact_impulse(int p_contact_idx) const override;
+	virtual Vector3 get_contact_impulse(int p_contact_idx) const override;
 	virtual int get_contact_local_shape(int p_contact_idx) const override;
 
 	virtual RID get_contact_collider(int p_contact_idx) const override;
diff --git a/servers/physics_3d/godot_body_pair_3d.cpp b/servers/physics_3d/godot_body_pair_3d.cpp
index 00b7941292..31496860c0 100644
--- a/servers/physics_3d/godot_body_pair_3d.cpp
+++ b/servers/physics_3d/godot_body_pair_3d.cpp
@@ -364,16 +364,30 @@ bool GodotBodyPair3D::pre_solve(real_t p_step) {
 		c.rA = global_A - A->get_center_of_mass();
 		c.rB = global_B - B->get_center_of_mass() - offset_B;
 
+		// Precompute normal mass, tangent mass, and bias.
+		Vector3 inertia_A = inv_inertia_tensor_A.xform(c.rA.cross(c.normal));
+		Vector3 inertia_B = inv_inertia_tensor_B.xform(c.rB.cross(c.normal));
+		real_t kNormal = inv_mass_A + inv_mass_B;
+		kNormal += c.normal.dot(inertia_A.cross(c.rA)) + c.normal.dot(inertia_B.cross(c.rB));
+		c.mass_normal = 1.0f / kNormal;
+
+		c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration);
+		c.depth = depth;
+
+		Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse;
+
+		c.acc_impulse -= j_vec;
+
 		// contact query reporting...
 
 		if (A->can_report_contacts()) {
 			Vector3 crA = A->get_angular_velocity().cross(c.rA) + A->get_linear_velocity();
-			A->add_contact(global_A, -c.normal, depth, shape_A, global_B, shape_B, B->get_instance_id(), B->get_self(), crA);
+			A->add_contact(global_A, -c.normal, depth, shape_A, global_B, shape_B, B->get_instance_id(), B->get_self(), crA, c.acc_impulse);
 		}
 
 		if (B->can_report_contacts()) {
 			Vector3 crB = B->get_angular_velocity().cross(c.rB) + B->get_linear_velocity();
-			B->add_contact(global_B, c.normal, depth, shape_B, global_A, shape_A, A->get_instance_id(), A->get_self(), crB);
+			B->add_contact(global_B, c.normal, depth, shape_B, global_A, shape_A, A->get_instance_id(), A->get_self(), crB, -c.acc_impulse);
 		}
 
 		if (report_contacts_only) {
@@ -384,17 +398,6 @@ bool GodotBodyPair3D::pre_solve(real_t p_step) {
 		c.active = true;
 		do_process = true;
 
-		// Precompute normal mass, tangent mass, and bias.
-		Vector3 inertia_A = inv_inertia_tensor_A.xform(c.rA.cross(c.normal));
-		Vector3 inertia_B = inv_inertia_tensor_B.xform(c.rB.cross(c.normal));
-		real_t kNormal = inv_mass_A + inv_mass_B;
-		kNormal += c.normal.dot(inertia_A.cross(c.rA)) + c.normal.dot(inertia_B.cross(c.rB));
-		c.mass_normal = 1.0f / kNormal;
-
-		c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration);
-		c.depth = depth;
-
-		Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse;
 		if (collide_A) {
 			A->apply_impulse(-j_vec, c.rA + A->get_center_of_mass());
 		}
@@ -504,6 +507,7 @@ void GodotBodyPair3D::solve(real_t p_step) {
 			if (collide_B) {
 				B->apply_impulse(j, c.rB + B->get_center_of_mass());
 			}
+			c.acc_impulse -= j;
 
 			c.active = true;
 		}
@@ -550,6 +554,7 @@ void GodotBodyPair3D::solve(real_t p_step) {
 			if (collide_B) {
 				B->apply_impulse(jt, c.rB + B->get_center_of_mass());
 			}
+			c.acc_impulse -= jt;
 
 			c.active = true;
 		}
@@ -745,23 +750,6 @@ bool GodotBodySoftBodyPair3D::pre_solve(real_t p_step) {
 		c.rA = global_A - transform_A.origin - body->get_center_of_mass();
 		c.rB = global_B;
 
-		if (body->can_report_contacts()) {
-			Vector3 crA = body->get_angular_velocity().cross(c.rA) + body->get_linear_velocity();
-			body->add_contact(global_A, -c.normal, depth, body_shape, global_B, 0, soft_body->get_instance_id(), soft_body->get_self(), crA);
-		}
-
-		if (report_contacts_only) {
-			collided = false;
-			continue;
-		}
-
-		c.active = true;
-		do_process = true;
-
-		if (body_collides) {
-			body->set_active(true);
-		}
-
 		// Precompute normal mass, tangent mass, and bias.
 		Vector3 inertia_A = body_inv_inertia_tensor.xform(c.rA.cross(c.normal));
 		real_t kNormal = body_inv_mass + node_inv_mass;
@@ -778,6 +766,24 @@ bool GodotBodySoftBodyPair3D::pre_solve(real_t p_step) {
 		if (soft_body_collides) {
 			soft_body->apply_node_impulse(c.index_B, j_vec);
 		}
+		c.acc_impulse -= j_vec;
+
+		if (body->can_report_contacts()) {
+			Vector3 crA = body->get_angular_velocity().cross(c.rA) + body->get_linear_velocity();
+			body->add_contact(global_A, -c.normal, depth, body_shape, global_B, 0, soft_body->get_instance_id(), soft_body->get_self(), crA, c.acc_impulse);
+		}
+
+		if (report_contacts_only) {
+			collided = false;
+			continue;
+		}
+
+		c.active = true;
+		do_process = true;
+
+		if (body_collides) {
+			body->set_active(true);
+		}
 
 		c.bounce = body->get_bounce();
 
@@ -880,6 +886,7 @@ void GodotBodySoftBodyPair3D::solve(real_t p_step) {
 			if (soft_body_collides) {
 				soft_body->apply_node_impulse(c.index_B, j);
 			}
+			c.acc_impulse -= j;
 
 			c.active = true;
 		}
@@ -924,6 +931,7 @@ void GodotBodySoftBodyPair3D::solve(real_t p_step) {
 			if (soft_body_collides) {
 				soft_body->apply_node_impulse(c.index_B, jt);
 			}
+			c.acc_impulse -= jt;
 
 			c.active = true;
 		}
diff --git a/servers/physics_3d/godot_body_pair_3d.h b/servers/physics_3d/godot_body_pair_3d.h
index 7a7309f9d5..fd6c6b35d8 100644
--- a/servers/physics_3d/godot_body_pair_3d.h
+++ b/servers/physics_3d/godot_body_pair_3d.h
@@ -44,6 +44,7 @@ protected:
 		Vector3 normal;
 		int index_A = 0, index_B = 0;
 		Vector3 local_A, local_B;
+		Vector3 acc_impulse; // accumulated impulse - only one of the object's impulse is needed as impulse_a == -impulse_b
 		real_t acc_normal_impulse = 0.0; // accumulated normal impulse (Pn)
 		Vector3 acc_tangent_impulse; // accumulated tangent impulse (Pt)
 		real_t acc_bias_impulse = 0.0; // accumulated normal impulse for position bias (Pnb)