Add get_contact_impulse method to PhysicsDirectBodyState2D

This makes it consistent with 3D.

11 files changed, 48 insertions, 20 deletions

diff --git a/doc/classes/PhysicsDirectBodyState2D.xml b/doc/classes/PhysicsDirectBodyState2D.xml
index eca6a1cbc7..a46de4c189 100644
--- a/doc/classes/PhysicsDirectBodyState2D.xml
+++ b/doc/classes/PhysicsDirectBodyState2D.xml
@@ -151,6 +151,13 @@
 				[b]Note:[/b] By default, this returns 0 unless bodies are configured to monitor contacts. See [member RigidBody2D.contact_monitor].
 			</description>
 		</method>
+		<method name="get_contact_impulse" qualifiers="const">
+			<return type="Vector2" />
+			<param index="0" name="contact_idx" type="int" />
+			<description>
+				Returns the impulse created by the contact.
+			</description>
+		</method>
 		<method name="get_contact_local_normal" qualifiers="const">
 			<return type="Vector2" />
 			<param index="0" name="contact_idx" type="int" />
diff --git a/doc/classes/PhysicsDirectBodyState2DExtension.xml b/doc/classes/PhysicsDirectBodyState2DExtension.xml
index 8fd34c1243..496cbf9136 100644
--- a/doc/classes/PhysicsDirectBodyState2DExtension.xml
+++ b/doc/classes/PhysicsDirectBodyState2DExtension.xml
@@ -130,6 +130,12 @@
 			<description>
 			</description>
 		</method>
+		<method name="_get_contact_impulse" qualifiers="virtual const">
+			<return type="Vector2" />
+			<param index="0" name="contact_idx" type="int" />
+			<description>
+			</description>
+		</method>
 		<method name="_get_contact_local_normal" qualifiers="virtual const">
 			<return type="Vector2" />
 			<param index="0" name="contact_idx" type="int" />
diff --git a/servers/extensions/physics_server_2d_extension.cpp b/servers/extensions/physics_server_2d_extension.cpp
index a0c082ee44..a174ceadac 100644
--- a/servers/extensions/physics_server_2d_extension.cpp
+++ b/servers/extensions/physics_server_2d_extension.cpp
@@ -101,6 +101,7 @@ void PhysicsDirectBodyState2DExtension::_bind_methods() {
 	GDVIRTUAL_BIND(_get_contact_collider_object, "contact_idx");
 	GDVIRTUAL_BIND(_get_contact_collider_shape, "contact_idx");
 	GDVIRTUAL_BIND(_get_contact_collider_velocity_at_position, "contact_idx");
+	GDVIRTUAL_BIND(_get_contact_impulse, "contact_idx");
 
 	GDVIRTUAL_BIND(_get_step);
 	GDVIRTUAL_BIND(_integrate_forces);
diff --git a/servers/extensions/physics_server_2d_extension.h b/servers/extensions/physics_server_2d_extension.h
index c4970f6398..0008653f66 100644
--- a/servers/extensions/physics_server_2d_extension.h
+++ b/servers/extensions/physics_server_2d_extension.h
@@ -100,6 +100,7 @@ public:
 	EXBIND1RC(Object *, get_contact_collider_object, int)
 	EXBIND1RC(int, get_contact_collider_shape, int)
 	EXBIND1RC(Vector2, get_contact_collider_velocity_at_position, int)
+	EXBIND1RC(Vector2, get_contact_impulse, int)
 
 	EXBIND0RC(real_t, get_step)
 	EXBIND0(integrate_forces)
diff --git a/servers/physics_2d/godot_body_2d.h b/servers/physics_2d/godot_body_2d.h
index 34da64dac9..71dc826604 100644
--- a/servers/physics_2d/godot_body_2d.h
+++ b/servers/physics_2d/godot_body_2d.h
@@ -132,6 +132,7 @@ class GodotBody2D : public GodotCollisionObject2D {
 		ObjectID collider_instance_id;
 		RID collider;
 		Vector2 collider_velocity_at_pos;
+		Vector2 impulse;
 	};
 
 	Vector<Contact> contacts; //no contacts by default
@@ -190,7 +191,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 Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos);
+	_FORCE_INLINE_ void add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos, const Vector2 &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); }
@@ -340,7 +341,7 @@ public:
 
 //add contact inline
 
-void GodotBody2D::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos) {
+void GodotBody2D::add_contact(const Vector2 &p_local_pos, const Vector2 &p_local_normal, real_t p_depth, int p_local_shape, const Vector2 &p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID &p_collider, const Vector2 &p_collider_velocity_at_pos, const Vector2 &p_impulse) {
 	int c_max = contacts.size();
 
 	if (c_max == 0) {
@@ -380,6 +381,7 @@ void GodotBody2D::add_contact(const Vector2 &p_local_pos, const Vector2 &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_2D_H
diff --git a/servers/physics_2d/godot_body_direct_state_2d.cpp b/servers/physics_2d/godot_body_direct_state_2d.cpp
index 35092b631a..2fa933ce17 100644
--- a/servers/physics_2d/godot_body_direct_state_2d.cpp
+++ b/servers/physics_2d/godot_body_direct_state_2d.cpp
@@ -210,6 +210,11 @@ Vector2 GodotPhysicsDirectBodyState2D::get_contact_collider_velocity_at_position
 	return body->contacts[p_contact_idx].collider_velocity_at_pos;
 }
 
+Vector2 GodotPhysicsDirectBodyState2D::get_contact_impulse(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector2());
+	return body->contacts[p_contact_idx].impulse;
+}
+
 PhysicsDirectSpaceState2D *GodotPhysicsDirectBodyState2D::get_space_state() {
 	return body->get_space()->get_direct_state();
 }
diff --git a/servers/physics_2d/godot_body_direct_state_2d.h b/servers/physics_2d/godot_body_direct_state_2d.h
index c01ee62920..545d52ad23 100644
--- a/servers/physics_2d/godot_body_direct_state_2d.h
+++ b/servers/physics_2d/godot_body_direct_state_2d.h
@@ -92,8 +92,8 @@ public:
 	virtual Vector2 get_contact_collider_position(int p_contact_idx) const override;
 	virtual ObjectID get_contact_collider_id(int p_contact_idx) const override;
 	virtual int get_contact_collider_shape(int p_contact_idx) const override;
-
 	virtual Vector2 get_contact_collider_velocity_at_position(int p_contact_idx) const override;
+	virtual Vector2 get_contact_impulse(int p_contact_idx) const override;
 
 	virtual PhysicsDirectSpaceState2D *get_space_state() override;
 
diff --git a/servers/physics_2d/godot_body_pair_2d.cpp b/servers/physics_2d/godot_body_pair_2d.cpp
index 474367def0..40dbb4fcf4 100644
--- a/servers/physics_2d/godot_body_pair_2d.cpp
+++ b/servers/physics_2d/godot_body_pair_2d.cpp
@@ -434,21 +434,6 @@ bool GodotBodyPair2D::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;
 
-		if (A->can_report_contacts()) {
-			Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x);
-			A->add_contact(global_A + offset_A, -c.normal, depth, shape_A, global_B + offset_A, shape_B, B->get_instance_id(), B->get_self(), crB + B->get_linear_velocity());
-		}
-
-		if (B->can_report_contacts()) {
-			Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x);
-			B->add_contact(global_B + offset_A, c.normal, depth, shape_B, global_A + offset_A, shape_A, A->get_instance_id(), A->get_self(), crA + A->get_linear_velocity());
-		}
-
-		if (report_contacts_only) {
-			collided = false;
-			continue;
-		}
-
 		// Precompute normal mass, tangent mass, and bias.
 		real_t rnA = c.rA.dot(c.normal);
 		real_t rnB = c.rB.dot(c.normal);
@@ -466,11 +451,28 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) {
 		c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration);
 		c.depth = depth;
 
+		Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent;
+
+		c.acc_impulse -= P;
+
+		if (A->can_report_contacts()) {
+			Vector2 crB(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x);
+			A->add_contact(global_A + offset_A, -c.normal, depth, shape_A, global_B + offset_A, shape_B, B->get_instance_id(), B->get_self(), crB + B->get_linear_velocity(), c.acc_impulse);
+		}
+
+		if (B->can_report_contacts()) {
+			Vector2 crA(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x);
+			B->add_contact(global_B + offset_A, c.normal, depth, shape_B, global_A + offset_A, shape_A, A->get_instance_id(), A->get_self(), crA + A->get_linear_velocity(), c.acc_impulse);
+		}
+
+		if (report_contacts_only) {
+			collided = false;
+			continue;
+		}
+
 #ifdef ACCUMULATE_IMPULSES
 		{
 			// Apply normal + friction impulse
-			Vector2 P = c.acc_normal_impulse * c.normal + c.acc_tangent_impulse * tangent;
-
 			if (collide_A) {
 				A->apply_impulse(-P, c.rA + A->get_center_of_mass());
 			}
@@ -581,6 +583,7 @@ void GodotBodyPair2D::solve(real_t p_step) {
 		if (collide_B) {
 			B->apply_impulse(j, c.rB + B->get_center_of_mass());
 		}
+		c.acc_impulse -= j;
 	}
 }
 
diff --git a/servers/physics_2d/godot_body_pair_2d.h b/servers/physics_2d/godot_body_pair_2d.h
index 7e7a9839c1..4e9bfa6022 100644
--- a/servers/physics_2d/godot_body_pair_2d.h
+++ b/servers/physics_2d/godot_body_pair_2d.h
@@ -59,6 +59,7 @@ class GodotBodyPair2D : public GodotConstraint2D {
 		Vector2 position;
 		Vector2 normal;
 		Vector2 local_A, local_B;
+		Vector2 acc_impulse; // accumulated impulse
 		real_t acc_normal_impulse = 0.0; // accumulated normal impulse (Pn)
 		real_t acc_tangent_impulse = 0.0; // accumulated tangent impulse (Pt)
 		real_t acc_bias_impulse = 0.0; // accumulated normal impulse for position bias (Pnb)
diff --git a/servers/physics_server_2d.cpp b/servers/physics_server_2d.cpp
index 4973b9970a..214de27b35 100644
--- a/servers/physics_server_2d.cpp
+++ b/servers/physics_server_2d.cpp
@@ -126,6 +126,7 @@ void PhysicsDirectBodyState2D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("get_contact_collider_object", "contact_idx"), &PhysicsDirectBodyState2D::get_contact_collider_object);
 	ClassDB::bind_method(D_METHOD("get_contact_collider_shape", "contact_idx"), &PhysicsDirectBodyState2D::get_contact_collider_shape);
 	ClassDB::bind_method(D_METHOD("get_contact_collider_velocity_at_position", "contact_idx"), &PhysicsDirectBodyState2D::get_contact_collider_velocity_at_position);
+	ClassDB::bind_method(D_METHOD("get_contact_impulse", "contact_idx"), &PhysicsDirectBodyState2D::get_contact_impulse);
 	ClassDB::bind_method(D_METHOD("get_step"), &PhysicsDirectBodyState2D::get_step);
 	ClassDB::bind_method(D_METHOD("integrate_forces"), &PhysicsDirectBodyState2D::integrate_forces);
 	ClassDB::bind_method(D_METHOD("get_space_state"), &PhysicsDirectBodyState2D::get_space_state);
diff --git a/servers/physics_server_2d.h b/servers/physics_server_2d.h
index aac4d9d69e..836ab5bd76 100644
--- a/servers/physics_server_2d.h
+++ b/servers/physics_server_2d.h
@@ -99,6 +99,7 @@ public:
 	virtual Object *get_contact_collider_object(int p_contact_idx) const;
 	virtual int get_contact_collider_shape(int p_contact_idx) const = 0;
 	virtual Vector2 get_contact_collider_velocity_at_position(int p_contact_idx) const = 0;
+	virtual Vector2 get_contact_impulse(int p_contact_idx) const = 0;
 
 	virtual real_t get_step() const = 0;
 	virtual void integrate_forces();