Resolved issues with script PhysicsDirectBodyState3d contacts

Resolved a problem with PhysicsDirectBodyState3D sometimes returning incorrect contact positions and added a new get_contact_local_velocity_at_position method to compliment the existing one for the collider.

10 files changed, 39 insertions, 10 deletions

diff --git a/doc/classes/PhysicsDirectBodyState3D.xml b/doc/classes/PhysicsDirectBodyState3D.xml
index b257cbc1ed..def9ca7a50 100644
--- a/doc/classes/PhysicsDirectBodyState3D.xml
+++ b/doc/classes/PhysicsDirectBodyState3D.xml
@@ -179,6 +179,13 @@
 				Returns the local shape index of the collision.
 			</description>
 		</method>
+		<method name="get_contact_local_velocity_at_position" qualifiers="const">
+			<return type="Vector3" />
+			<param index="0" name="contact_idx" type="int" />
+			<description>
+				Returns the linear velocity vector at the body's contact point.
+			</description>
+		</method>
 		<method name="get_space_state">
 			<return type="PhysicsDirectSpaceState3D" />
 			<description>
diff --git a/doc/classes/PhysicsDirectBodyState3DExtension.xml b/doc/classes/PhysicsDirectBodyState3DExtension.xml
index 6b0c50f660..3220733d23 100644
--- a/doc/classes/PhysicsDirectBodyState3DExtension.xml
+++ b/doc/classes/PhysicsDirectBodyState3DExtension.xml
@@ -154,6 +154,12 @@
 			<description>
 			</description>
 		</method>
+		<method name="_get_contact_local_velocity_at_position" qualifiers="virtual const">
+			<return type="Vector3" />
+			<param index="0" name="contact_idx" type="int" />
+			<description>
+			</description>
+		</method>
 		<method name="_get_inverse_inertia" qualifiers="virtual const">
 			<return type="Vector3" />
 			<description>
diff --git a/servers/extensions/physics_server_3d_extension.cpp b/servers/extensions/physics_server_3d_extension.cpp
index 997daaa9f2..0937021c64 100644
--- a/servers/extensions/physics_server_3d_extension.cpp
+++ b/servers/extensions/physics_server_3d_extension.cpp
@@ -102,6 +102,7 @@ void PhysicsDirectBodyState3DExtension::_bind_methods() {
 	GDVIRTUAL_BIND(_get_contact_local_normal, "contact_idx");
 	GDVIRTUAL_BIND(_get_contact_impulse, "contact_idx");
 	GDVIRTUAL_BIND(_get_contact_local_shape, "contact_idx");
+	GDVIRTUAL_BIND(_get_contact_local_velocity_at_position, "contact_idx");
 	GDVIRTUAL_BIND(_get_contact_collider, "contact_idx");
 	GDVIRTUAL_BIND(_get_contact_collider_position, "contact_idx");
 	GDVIRTUAL_BIND(_get_contact_collider_id, "contact_idx");
diff --git a/servers/extensions/physics_server_3d_extension.h b/servers/extensions/physics_server_3d_extension.h
index a9d04b1f05..eb8b627245 100644
--- a/servers/extensions/physics_server_3d_extension.h
+++ b/servers/extensions/physics_server_3d_extension.h
@@ -96,6 +96,7 @@ public:
 	EXBIND1RC(Vector3, get_contact_local_normal, int)
 	EXBIND1RC(Vector3, get_contact_impulse, int)
 	EXBIND1RC(int, get_contact_local_shape, int)
+	EXBIND1RC(Vector3, get_contact_local_velocity_at_position, int)
 	EXBIND1RC(RID, get_contact_collider, int)
 	EXBIND1RC(Vector3, get_contact_collider_position, int)
 	EXBIND1RC(ObjectID, get_contact_collider_id, int)
diff --git a/servers/physics_3d/godot_body_3d.h b/servers/physics_3d/godot_body_3d.h
index 51b360d705..81b668122a 100644
--- a/servers/physics_3d/godot_body_3d.h
+++ b/servers/physics_3d/godot_body_3d.h
@@ -119,6 +119,7 @@ class GodotBody3D : public GodotCollisionObject3D {
 	struct Contact {
 		Vector3 local_pos;
 		Vector3 local_normal;
+		Vector3 local_velocity_at_pos;
 		real_t depth = 0.0;
 		int local_shape = 0;
 		Vector3 collider_pos;
@@ -184,7 +185,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, const Vector3 &p_impulse);
+	_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_local_velocity_at_pos, 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); }
@@ -348,7 +349,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, const Vector3 &p_impulse) {
+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_local_velocity_at_pos, 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) {
@@ -381,6 +382,7 @@ void GodotBody3D::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local
 
 	c[idx].local_pos = p_local_pos;
 	c[idx].local_normal = p_local_normal;
+	c[idx].local_velocity_at_pos = p_local_velocity_at_pos;
 	c[idx].depth = p_depth;
 	c[idx].local_shape = p_local_shape;
 	c[idx].collider_pos = p_collider_pos;
diff --git a/servers/physics_3d/godot_body_direct_state_3d.cpp b/servers/physics_3d/godot_body_direct_state_3d.cpp
index 9f28f3809a..0af746c68d 100644
--- a/servers/physics_3d/godot_body_direct_state_3d.cpp
+++ b/servers/physics_3d/godot_body_direct_state_3d.cpp
@@ -193,6 +193,11 @@ Vector3 GodotPhysicsDirectBodyState3D::get_contact_impulse(int p_contact_idx) co
 	return body->contacts[p_contact_idx].impulse;
 }
 
+Vector3 GodotPhysicsDirectBodyState3D::get_contact_local_velocity_at_position(int p_contact_idx) const {
+	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, Vector3());
+	return body->contacts[p_contact_idx].local_velocity_at_pos;
+}
+
 int GodotPhysicsDirectBodyState3D::get_contact_local_shape(int p_contact_idx) const {
 	ERR_FAIL_INDEX_V(p_contact_idx, body->contact_count, -1);
 	return body->contacts[p_contact_idx].local_shape;
diff --git a/servers/physics_3d/godot_body_direct_state_3d.h b/servers/physics_3d/godot_body_direct_state_3d.h
index be2e851b4d..8066050c9f 100644
--- a/servers/physics_3d/godot_body_direct_state_3d.h
+++ b/servers/physics_3d/godot_body_direct_state_3d.h
@@ -91,6 +91,7 @@ public:
 	virtual Vector3 get_contact_local_normal(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 Vector3 get_contact_local_velocity_at_position(int p_contact_idx) const override;
 
 	virtual RID get_contact_collider(int p_contact_idx) const override;
 	virtual Vector3 get_contact_collider_position(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 292563b6ab..c467a583ba 100644
--- a/servers/physics_3d/godot_body_pair_3d.cpp
+++ b/servers/physics_3d/godot_body_pair_3d.cpp
@@ -407,14 +407,18 @@ bool GodotBodyPair3D::pre_solve(real_t p_step) {
 
 		// contact query reporting...
 
-		if (A->can_report_contacts()) {
+		if (A->can_report_contacts() || B->can_report_contacts()) {
+			Vector3 crB = B->get_angular_velocity().cross(c.rB) + B->get_linear_velocity();
 			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, c.acc_impulse);
-		}
+			Vector3 wlB = global_B - offset_B;
 
-		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, -c.acc_impulse);
+			if (A->can_report_contacts()) {
+				A->add_contact(global_A, -c.normal, depth, shape_A, crA, wlB, shape_B, B->get_instance_id(), B->get_self(), crB, c.acc_impulse);
+			}
+
+			if (B->can_report_contacts()) {
+				B->add_contact(wlB, c.normal, depth, shape_B, crB, global_A, shape_A, A->get_instance_id(), A->get_self(), crA, -c.acc_impulse);
+			}
 		}
 
 		if (report_contacts_only) {
@@ -797,9 +801,9 @@ bool GodotBodySoftBodyPair3D::pre_solve(real_t p_step) {
 
 		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);
+			Vector3 crB = soft_body->get_node_velocity(c.index_B);
+			body->add_contact(global_A, -c.normal, depth, body_shape, crA, global_B, 0, soft_body->get_instance_id(), soft_body->get_self(), crB, c.acc_impulse);
 		}
-
 		if (report_contacts_only) {
 			collided = false;
 			continue;
diff --git a/servers/physics_server_3d.cpp b/servers/physics_server_3d.cpp
index ee749de1c6..34c364d960 100644
--- a/servers/physics_server_3d.cpp
+++ b/servers/physics_server_3d.cpp
@@ -139,6 +139,7 @@ void PhysicsDirectBodyState3D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("get_contact_local_normal", "contact_idx"), &PhysicsDirectBodyState3D::get_contact_local_normal);
 	ClassDB::bind_method(D_METHOD("get_contact_impulse", "contact_idx"), &PhysicsDirectBodyState3D::get_contact_impulse);
 	ClassDB::bind_method(D_METHOD("get_contact_local_shape", "contact_idx"), &PhysicsDirectBodyState3D::get_contact_local_shape);
+	ClassDB::bind_method(D_METHOD("get_contact_local_velocity_at_position", "contact_idx"), &PhysicsDirectBodyState3D::get_contact_local_velocity_at_position);
 	ClassDB::bind_method(D_METHOD("get_contact_collider", "contact_idx"), &PhysicsDirectBodyState3D::get_contact_collider);
 	ClassDB::bind_method(D_METHOD("get_contact_collider_position", "contact_idx"), &PhysicsDirectBodyState3D::get_contact_collider_position);
 	ClassDB::bind_method(D_METHOD("get_contact_collider_id", "contact_idx"), &PhysicsDirectBodyState3D::get_contact_collider_id);
diff --git a/servers/physics_server_3d.h b/servers/physics_server_3d.h
index 0275aee818..33e9f8a7c9 100644
--- a/servers/physics_server_3d.h
+++ b/servers/physics_server_3d.h
@@ -97,6 +97,7 @@ public:
 	virtual Vector3 get_contact_local_normal(int p_contact_idx) const = 0;
 	virtual Vector3 get_contact_impulse(int p_contact_idx) const = 0;
 	virtual int get_contact_local_shape(int p_contact_idx) const = 0;
+	virtual Vector3 get_contact_local_velocity_at_position(int p_contact_idx) const = 0;
 
 	virtual RID get_contact_collider(int p_contact_idx) const = 0;
 	virtual Vector3 get_contact_collider_position(int p_contact_idx) const = 0;