Bring patch 81654 from Godot

migueldeicaza · Dec 11, 2023 · a1d3b50 · a1d3b50
1 parent 3acdad3
commit a1d3b50
Show file tree

Hide file tree

Showing 5 changed files with 50 additions and 11 deletions.
diff --git a/servers/physics_2d/godot_body_pair_2d.cpp b/servers/physics_2d/godot_body_pair_2d.cpp
@@ -403,7 +403,6 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) {
 
 	bool do_process = false;
 
-	const Vector2 &offset_A = A->get_transform().get_origin();
 	const Transform2D &transform_A = A->get_transform();
 	const Transform2D &transform_B = B->get_transform();
 
@@ -428,6 +427,7 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) {
 		}
 
 #ifdef DEBUG_ENABLED
+		const Vector2 &offset_A = A->get_transform().get_origin();
 		if (space->is_debugging_contacts()) {
 			space->add_debug_contact(global_A + offset_A);
 			space->add_debug_contact(global_B + offset_A);
@@ -458,16 +458,8 @@ bool GodotBodyPair2D::pre_solve(real_t p_step) {
 
 		c.acc_impulse -= P;
 
-		if (A->can_report_contacts() || B->can_report_contacts()) {
-			Vector2 crB = Vector2(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x) + B->get_linear_velocity();
-			Vector2 crA = Vector2(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x) + 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);
-			}
-			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);
-			}
-		}
+		c.initial_rB = Vector2(-B->get_angular_velocity() * c.rB.y, B->get_angular_velocity() * c.rB.x) + B->get_linear_velocity();
+		c.initial_rA = Vector2(-A->get_angular_velocity() * c.rA.y, A->get_angular_velocity() * c.rA.x) + A->get_linear_velocity();
 
 		if (report_contacts_only) {
 			collided = false;
@@ -591,6 +583,37 @@ void GodotBodyPair2D::solve(real_t p_step) {
 	}
 }
 
+void GodotBodyPair2D::post_solve(real_t p_step) {
+	for (int i = 0; i < contact_count; ++i) {
+		Contact &c = contacts[i];
+
+		if (!c.active) {
+			continue;
+		}
+
+		if (A->can_report_contacts() || B->can_report_contacts()) {
+			const Vector2 &offset_A = A->get_transform().get_origin();
+
+			const Transform2D &transform_A = A->get_transform();
+			const Transform2D &transform_B = B->get_transform();
+
+			Vector2 global_A = transform_A.basis_xform(c.local_A);
+			Vector2 global_B = transform_B.basis_xform(c.local_B) + offset_B;
+
+			Vector2 axis = global_A - global_B;
+			real_t depth = axis.dot(c.normal);
+
+			if (A->can_report_contacts()) {
+				A->add_contact(global_A + offset_A, -c.normal, depth, shape_A, c.initial_rA, global_B + offset_A, shape_B,
+						B->get_instance_id(), B->get_self(), c.initial_rB, c.acc_impulse);
+			}
+			if (B->can_report_contacts()) {
+				B->add_contact(global_B + offset_A, c.normal, depth, shape_B, c.initial_rB, global_A + offset_A, shape_A,
+						A->get_instance_id(), A->get_self(), c.initial_rA, c.acc_impulse);			}
+		}
+	}
+}
+
 GodotBodyPair2D::GodotBodyPair2D(GodotBody2D *p_A, int p_shape_A, GodotBody2D *p_B, int p_shape_B) :
 		GodotConstraint2D(_arr, 2) {
 	A = p_A;

diff --git a/servers/physics_2d/godot_body_pair_2d.h b/servers/physics_2d/godot_body_pair_2d.h
@@ -70,6 +70,7 @@ class GodotBodyPair2D : public GodotConstraint2D {
 		real_t depth = 0.0;
 		bool active = false;
 		bool used = false;
+		Vector2 initial_rA, initial_rB;
 		Vector2 rA, rB;
 		real_t bounce = 0.0;
 	};
@@ -93,6 +94,7 @@ class GodotBodyPair2D : public GodotConstraint2D {
 	virtual bool setup(real_t p_step) override;
 	virtual bool pre_solve(real_t p_step) override;
 	virtual void solve(real_t p_step) override;
+	virtual void post_solve(real_t p_step) override;
 
 	GodotBodyPair2D(GodotBody2D *p_A, int p_shape_A, GodotBody2D *p_B, int p_shape_B);
 	~GodotBodyPair2D();

diff --git a/servers/physics_2d/godot_constraint_2d.h b/servers/physics_2d/godot_constraint_2d.h
@@ -63,6 +63,7 @@ class GodotConstraint2D {
 	virtual bool setup(real_t p_step) = 0;
 	virtual bool pre_solve(real_t p_step) = 0;
 	virtual void solve(real_t p_step) = 0;
+	virtual void post_solve(real_t p_step) {}
 
 	virtual ~GodotConstraint2D() {}
 };

diff --git a/servers/physics_2d/godot_step_2d.cpp b/servers/physics_2d/godot_step_2d.cpp
@@ -101,6 +101,13 @@ void GodotStep2D::_solve_island(uint32_t p_island_index, void *p_userdata) const
 	}
 }
 
+void GodotStep2D::_post_solve_island(LocalVector<GodotConstraint2D *> &p_constraint_island) const {
+	uint32_t constraint_count = p_constraint_island.size();
+	for (uint32_t constraint_index = 0; constraint_index < constraint_count; ++constraint_index) {
+		p_constraint_island[constraint_index]->post_solve(delta);
+	}
+}
+
 void GodotStep2D::_check_suspend(LocalVector<GodotBody2D *> &p_body_island) const {
 	bool can_sleep = true;
 
@@ -269,6 +276,11 @@ void GodotStep2D::step(GodotSpace2D *p_space, real_t p_delta) {
 		profile_begtime = profile_endtime;
 	}
 
+	// Warning: This doesn't run on threads, because it involves thread-unsafe processing.
+	for (uint32_t island_index = 0; island_index < island_count; ++island_index) {
+		_post_solve_island(constraint_islands[island_index]);
+	}
+
 	/* INTEGRATE VELOCITIES */
 
 	b = body_list->first();

diff --git a/servers/physics_2d/godot_step_2d.h b/servers/physics_2d/godot_step_2d.h
@@ -49,6 +49,7 @@ class GodotStep2D {
 	void _setup_constraint(uint32_t p_constraint_index, void *p_userdata = nullptr);
 	void _pre_solve_island(LocalVector<GodotConstraint2D *> &p_constraint_island) const;
 	void _solve_island(uint32_t p_island_index, void *p_userdata = nullptr) const;
+	void _post_solve_island(LocalVector<GodotConstraint2D *> &p_constraint_island) const;
 	void _check_suspend(LocalVector<GodotBody2D *> &p_body_island) const;
 
 public: