Constraint solver: properly take non-zero center of masses into account.

1 files changed, 5 insertions, 2 deletions

diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs
index ec9e186..af2e1e9 100644
--- a/src/dynamics/solver/velocity_ground_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs
@@ -89,6 +89,9 @@ impl WVelocityGroundConstraint {
         let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
         let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
 
+        let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
+        let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
+
         let force_dir1 = position1
             * -Vector::from(
                 array![|ii| if flipped[ii] { manifolds[ii].local_n2 } else { manifolds[ii].local_n1 }; SIMD_WIDTH],
@@ -130,8 +133,8 @@ impl WVelocityGroundConstraint {
 
                 let impulse =
                     SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]);
-                let dp1 = p1.coords - position1.translation.vector;
-                let dp2 = p2.coords - position2.translation.vector;
+                let dp1 = p1 - world_com1;
+                let dp2 = p2 - world_com2;
 
                 let vel1 = linvel1 + angvel1.gcross(dp1);
                 let vel2 = linvel2 + angvel2.gcross(dp2);