From c286f44c4e2aa1692781e9c4c7c1ce2bacdd2415 Mon Sep 17 00:00:00 2001
From: Sébastien Crozet <developer@crozet.re>
Date: Mon, 31 Aug 2020 19:02:37 +0200
Subject: Constraint solver: properly take non-zero center of masses into
 account.

---
 src/dynamics/solver/velocity_ground_constraint.rs | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

(limited to 'src/dynamics/solver/velocity_ground_constraint.rs')

diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index 457c3b3..65a61bd 100644
--- a/src/dynamics/solver/velocity_ground_constraint.rs
+++ b/src/dynamics/solver/velocity_ground_constraint.rs
@@ -155,8 +155,8 @@ impl VelocityGroundConstraint {
                         rb2.position * manifold_point.local_p2,
                     )
                 };
-                let dp2 = p2.coords - rb2.position.translation.vector;
-                let dp1 = p1.coords - rb1.position.translation.vector;
+                let dp2 = p2 - rb2.world_com;
+                let dp1 = p1 - rb1.world_com;
                 let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
                 let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
 
-- 
cgit