PinJoint2D: fix inertia tensor, taking center of mass into account

1 files changed, 12 insertions, 8 deletions

diff --git a/servers/physics_2d/godot_joints_2d.cpp b/servers/physics_2d/godot_joints_2d.cpp
index 0876184d8c..0c21b08ea9 100644
--- a/servers/physics_2d/godot_joints_2d.cpp
+++ b/servers/physics_2d/godot_joints_2d.cpp
@@ -118,22 +118,26 @@ bool GodotPinJoint2D::setup(real_t p_step) {
 	K1[0].y = 0.0f;
 	K1[1].y = A->get_inv_mass() + B_inv_mass;
 
+	Vector2 r1 = rA - A->get_center_of_mass();
+
 	Transform2D K2;
-	K2[0].x = A->get_inv_inertia() * rA.y * rA.y;
-	K2[1].x = -A->get_inv_inertia() * rA.x * rA.y;
-	K2[0].y = -A->get_inv_inertia() * rA.x * rA.y;
-	K2[1].y = A->get_inv_inertia() * rA.x * rA.x;
+	K2[0].x = A->get_inv_inertia() * r1.y * r1.y;
+	K2[1].x = -A->get_inv_inertia() * r1.x * r1.y;
+	K2[0].y = -A->get_inv_inertia() * r1.x * r1.y;
+	K2[1].y = A->get_inv_inertia() * r1.x * r1.x;
 
 	Transform2D K;
 	K[0] = K1[0] + K2[0];
 	K[1] = K1[1] + K2[1];
 
 	if (B) {
+		Vector2 r2 = rB - B->get_center_of_mass();
+
 		Transform2D K3;
-		K3[0].x = B->get_inv_inertia() * rB.y * rB.y;
-		K3[1].x = -B->get_inv_inertia() * rB.x * rB.y;
-		K3[0].y = -B->get_inv_inertia() * rB.x * rB.y;
-		K3[1].y = B->get_inv_inertia() * rB.x * rB.x;
+		K3[0].x = B->get_inv_inertia() * r2.y * r2.y;
+		K3[1].x = -B->get_inv_inertia() * r2.x * r2.y;
+		K3[0].y = -B->get_inv_inertia() * r2.x * r2.y;
+		K3[1].y = B->get_inv_inertia() * r2.x * r2.x;
 
 		K[0] += K3[0];
 		K[1] += K3[1];