Merge pull request #6 from dimforge/collider_removal

Add collider removal + fix rigid-bodies with multiple colliders

1 files changed, 6 insertions, 6 deletions

diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs
index 9212e89..190076d 100644
--- a/src/dynamics/solver/velocity_constraint.rs
+++ b/src/dynamics/solver/velocity_constraint.rs
@@ -148,7 +148,9 @@ impl VelocityConstraint {
         let rb2 = &bodies[manifold.body_pair.body2];
         let mj_lambda1 = rb1.active_set_offset;
         let mj_lambda2 = rb2.active_set_offset;
-        let force_dir1 = rb1.position * (-manifold.local_n1);
+        let pos_coll1 = rb1.position * manifold.delta1;
+        let pos_coll2 = rb2.position * manifold.delta2;
+        let force_dir1 = pos_coll1 * (-manifold.local_n1);
         let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff;
 
         for (l, manifold_points) in manifold
@@ -215,10 +217,8 @@ impl VelocityConstraint {
 
             for k in 0..manifold_points.len() {
                 let manifold_point = &manifold_points[k];
-                let dp1 = (rb1.position * manifold_point.local_p1).coords
-                    - rb1.position.translation.vector;
-                let dp2 = (rb2.position * manifold_point.local_p2).coords
-                    - rb2.position.translation.vector;
+                let dp1 = (pos_coll1 * manifold_point.local_p1) - rb1.world_com;
+                let dp2 = (pos_coll2 * manifold_point.local_p2) - rb2.world_com;
 
                 let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
                 let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
@@ -355,7 +355,7 @@ impl VelocityConstraint {
             }
         }
 
-        // Solve penetration.
+        // Solve non-penetration.
         for i in 0..self.num_contacts as usize {
             let elt = &mut self.elements[i].normal_part;
             let dimpulse = self.dir1.dot(&mj_lambda1.linear) + elt.gcross1.gdot(mj_lambda1.angular)