Bring patch 73569 from Godot

migueldeicaza · Dec 11, 2023 · 05eb261 · 05eb261
1 parent a1d3b50
commit 05eb261
Show file tree

Hide file tree

Showing 3 changed files with 21 additions and 6 deletions.
diff --git a/servers/physics_3d/godot_body_3d.h b/servers/physics_3d/godot_body_3d.h
@@ -185,7 +185,12 @@ class GodotBody3D : public GodotCollisionObject3D {
 	_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_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_ int 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 update_contact_impulse(const int index, const Vector3 &p_impulse) {
+		Contact *c = contacts.ptrw();
+		c[index].impulse = 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); }
@@ -349,11 +354,11 @@ class GodotBody3D : public GodotCollisionObject3D {
 
 //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_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 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) {
-		return;
+		return -1;
 	}
 
 	Contact *c = contacts.ptrw();
@@ -376,7 +381,7 @@ void GodotBody3D::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local
 			idx = least_deep;
 		}
 		if (idx == -1) {
-			return; //none least deepe than this
+			return -1; //none least deepe than this
 		}
 	}
 
@@ -391,6 +396,7 @@ void GodotBody3D::add_contact(const Vector3 &p_local_pos, const Vector3 &p_local
 	c[idx].collider = p_collider;
 	c[idx].collider_velocity_at_pos = p_collider_velocity_at_pos;
 	c[idx].impulse = p_impulse;
+	return idx;
 }
 
 #endif // GODOT_BODY_3D_H
diff --git a/servers/physics_3d/godot_body_pair_3d.cpp b/servers/physics_3d/godot_body_pair_3d.cpp
@@ -416,11 +416,13 @@ bool GodotBodyPair3D::pre_solve(real_t p_step) {
 			Vector3 crA = A->get_angular_velocity().cross(c.rA) + A->get_linear_velocity();
 
 			if (A->can_report_contacts()) {
-				A->add_contact(global_A + offset_A, -c.normal, depth, shape_A, crA, global_B + offset_A, shape_B, B->get_instance_id(), B->get_self(), crB, c.acc_impulse);
+				int contact_index = A->add_contact(global_A + offset_A, -c.normal, depth, shape_A, crA, global_B + offset_A, shape_B, B->get_instance_id(), B->get_self(), crB, c.acc_impulse);
+				c.contact_index_A = contact_index;
 			}
 
 			if (B->can_report_contacts()) {
-				B->add_contact(global_B + offset_A, c.normal, depth, shape_B, crB, global_A + offset_A, shape_A, A->get_instance_id(), A->get_self(), crA, -c.acc_impulse);
+				int contact_index = B->add_contact(global_B + offset_A, c.normal, depth, shape_B, crB, global_A + offset_A, shape_A, A->get_instance_id(), A->get_self(), crA, -c.acc_impulse);
+				c.contact_index_B = contact_index;
 			}
 		}
 
@@ -592,6 +594,12 @@ void GodotBodyPair3D::solve(real_t p_step) {
 
 			c.active = true;
 		}
+		if (c.contact_index_A >= 0) {
+			A->update_contact_impulse(c.contact_index_A, c.acc_impulse);
+		}
+		if (c.contact_index_B >= 0) {
+			B->update_contact_impulse(c.contact_index_B, -c.acc_impulse);
+		}
 	}
 }
 

diff --git a/servers/physics_3d/godot_body_pair_3d.h b/servers/physics_3d/godot_body_pair_3d.h
@@ -43,6 +43,7 @@ class GodotBodyContact3D : public GodotConstraint3D {
 		Vector3 position;
 		Vector3 normal;
 		int index_A = 0, index_B = 0;
+		int contact_index_A = -1, contact_index_B = -1;
 		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)