Replace the delta-position by the collider positions in the contact manifolds.

12 files changed, 62 insertions, 60 deletions

diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index 5128b6f..e9bd7bb 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -381,6 +381,7 @@ impl RigidBody {
     }
 
     /// The world-space position of this rigid-body.
+    #[inline]
     pub fn position(&self) -> &Isometry<f32> {
         &self.position
     }
diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs
index 69fcf57..b358435 100644
--- a/src/dynamics/solver/position_constraint.rs
+++ b/src/dynamics/solver/position_constraint.rs
@@ -94,6 +94,8 @@ impl PositionConstraint {
         let shift2 = manifold.local_n2 * -manifold.kinematics.radius2;
         let radius =
             manifold.kinematics.radius1 + manifold.kinematics.radius2 /*- params.allowed_linear_error*/;
+        let delta1 = rb1.position.inverse() * manifold.position1;
+        let delta2 = rb2.position.inverse() * manifold.position2;
 
         for (l, manifold_points) in manifold
             .active_contacts()
@@ -104,8 +106,8 @@ impl PositionConstraint {
             let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS];
 
             for l in 0..manifold_points.len() {
-                local_p1[l] = manifold.delta1 * (manifold_points[l].local_p1 + shift1);
-                local_p2[l] = manifold.delta2 * (manifold_points[l].local_p2 + shift2);
+                local_p1[l] = delta1 * (manifold_points[l].local_p1 + shift1);
+                local_p2[l] = delta2 * (manifold_points[l].local_p2 + shift2);
             }
 
             let constraint = PositionConstraint {
diff --git a/src/dynamics/solver/position_constraint_wide.rs b/src/dynamics/solver/position_constraint_wide.rs
index 0633926..df5ba65 100644
--- a/src/dynamics/solver/position_constraint_wide.rs
+++ b/src/dynamics/solver/position_constraint_wide.rs
@@ -41,9 +41,12 @@ impl WPositionConstraint {
         let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
         let sqrt_ii1: AngularInertia<SimdFloat> =
             AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
+        let rb_pos1 = Isometry::from(array![|ii| *rbs1[ii].position(); SIMD_WIDTH]);
+
         let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
         let sqrt_ii2: AngularInertia<SimdFloat> =
             AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
+        let rb_pos2 = Isometry::from(array![|ii| *rbs2[ii].position(); SIMD_WIDTH]);
 
         let local_n1 = Vector::from(array![|ii| manifolds[ii].local_n1; SIMD_WIDTH]);
         let local_n2 = Vector::from(array![|ii| manifolds[ii].local_n2; SIMD_WIDTH]);
@@ -51,8 +54,11 @@ impl WPositionConstraint {
         let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
         let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]);
 
-        let delta1 = Isometry::from(array![|ii| manifolds[ii].delta1; SIMD_WIDTH]);
-        let delta2 = Isometry::from(array![|ii| manifolds[ii].delta2; SIMD_WIDTH]);
+        let coll_pos1 = Isometry::from(array![|ii| manifolds[ii].position1; SIMD_WIDTH]);
+        let coll_pos2 = Isometry::from(array![|ii| manifolds[ii].position2; SIMD_WIDTH]);
+
+        let delta1 = rb_pos1.inverse() * coll_pos1;
+        let delta2 = rb_pos2.inverse() * coll_pos2;
 
         let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
         let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
diff --git a/src/dynamics/solver/position_ground_constraint.rs b/src/dynamics/solver/position_ground_constraint.rs
index dcd2d64..541c649 100644
--- a/src/dynamics/solver/position_ground_constraint.rs
+++ b/src/dynamics/solver/position_ground_constraint.rs
@@ -34,23 +34,22 @@ impl PositionGroundConstraint {
 
         let local_n1;
         let local_n2;
-        let delta1;
+        let coll_pos1;
         let delta2;
 
         if flip {
             std::mem::swap(&mut rb1, &mut rb2);
             local_n1 = manifold.local_n2;
             local_n2 = manifold.local_n1;
-            delta1 = &manifold.delta2;
-            delta2 = &manifold.delta1;
+            coll_pos1 = &manifold.position2;
+            delta2 = rb1.position() * manifold.position1;
         } else {
             local_n1 = manifold.local_n1;
             local_n2 = manifold.local_n2;
-            delta1 = &manifold.delta1;
-            delta2 = &manifold.delta2;
+            coll_pos1 = &manifold.position1;
+            delta2 = rb2.position().inverse() * manifold.position2;
         };
 
-        let coll_pos1 = rb1.position * delta1;
         let shift1 = local_n1 * -manifold.kinematics.radius1;
         let shift2 = local_n2 * -manifold.kinematics.radius2;
         let n1 = coll_pos1 * local_n1;
diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs
index e423c0a..42686ad 100644
--- a/src/dynamics/solver/position_ground_constraint_wide.rs
+++ b/src/dynamics/solver/position_ground_constraint_wide.rs
@@ -54,18 +54,18 @@ impl WPositionGroundConstraint {
             array![|ii| if flipped[ii] { manifolds[ii].local_n1 } else { manifolds[ii].local_n2 }; SIMD_WIDTH],
         );
 
-        let delta1 = Isometry::from(
-            array![|ii| if flipped[ii] { manifolds[ii].delta2 } else { manifolds[ii].delta1 }; SIMD_WIDTH],
+        let coll_pos1 = Isometry::from(
+            array![|ii| if flipped[ii] { manifolds[ii].position2 } else { manifolds[ii].position1 }; SIMD_WIDTH],
         );
-        let delta2 = Isometry::from(
-            array![|ii| if flipped[ii] { manifolds[ii].delta1 } else { manifolds[ii].delta2 }; SIMD_WIDTH],
+        let coll_pos2 = Isometry::from(
+            array![|ii| if flipped[ii] { manifolds[ii].position1 } else { manifolds[ii].position2 }; SIMD_WIDTH],
         );
 
         let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]);
         let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]);
 
-        let coll_pos1 =
-            delta1 * Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]);
+        let delta2 = Isometry::from(array![|ii| rbs2[ii].predicted_position; SIMD_WIDTH]).inverse()
+            * coll_pos2;
 
         let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH];
 
diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs
index 948081d..f08439c 100644
--- a/src/dynamics/solver/velocity_constraint.rs
+++ b/src/dynamics/solver/velocity_constraint.rs
@@ -148,9 +148,7 @@ impl VelocityConstraint {
         let rb2 = &bodies[manifold.body_pair.body2];
         let mj_lambda1 = rb1.active_set_offset;
         let mj_lambda2 = rb2.active_set_offset;
-        let pos_coll1 = rb1.position * manifold.delta1;
-        let pos_coll2 = rb2.position * manifold.delta2;
-        let force_dir1 = pos_coll1 * (-manifold.local_n1);
+        let force_dir1 = manifold.position1 * (-manifold.local_n1);
         let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff;
 
         for (l, manifold_points) in manifold
@@ -217,8 +215,8 @@ impl VelocityConstraint {
 
             for k in 0..manifold_points.len() {
                 let manifold_point = &manifold_points[k];
-                let dp1 = (pos_coll1 * manifold_point.local_p1) - rb1.world_com;
-                let dp2 = (pos_coll2 * manifold_point.local_p2) - rb2.world_com;
+                let dp1 = (manifold.position1 * manifold_point.local_p1) - rb1.world_com;
+                let dp2 = (manifold.position2 * manifold_point.local_p2) - rb2.world_com;
 
                 let vel1 = rb1.linvel + rb1.angvel.gcross(dp1);
                 let vel2 = rb2.linvel + rb2.angvel.gcross(dp2);
diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs
index 5d8078a..a74a91d 100644
--- a/src/dynamics/solver/velocity_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_constraint_wide.rs
@@ -72,17 +72,12 @@ impl WVelocityConstraint {
         let rbs1 = array![|ii| &bodies[manifolds[ii].body_pair.body1]; SIMD_WIDTH];
         let rbs2 = array![|ii| &bodies[manifolds[ii].body_pair.body2]; SIMD_WIDTH];
 
-        let delta1 = Isometry::from(array![|ii| manifolds[ii].delta1; SIMD_WIDTH]);
-        let delta2 = Isometry::from(array![|ii| manifolds[ii].delta2; SIMD_WIDTH]);
-
         let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]);
         let ii1: AngularInertia<SimdFloat> =
             AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]);
 
         let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]);
         let angvel1 = AngVector::<SimdFloat>::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]);
-
-        let pos1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
         let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]);
 
         let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]);
@@ -91,13 +86,10 @@ impl WVelocityConstraint {
 
         let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
         let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
-
-        let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
         let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]);
 
-        let coll_pos1 = pos1 * delta1;
-        let coll_pos2 = pos2 * delta2;
-
+        let coll_pos1 = Isometry::from(array![|ii| manifolds[ii].position1; SIMD_WIDTH]);
+        let coll_pos2 = Isometry::from(array![|ii| manifolds[ii].position2; SIMD_WIDTH]);
         let force_dir1 = coll_pos1 * -Vector::from(array![|ii| manifolds[ii].local_n1; SIMD_WIDTH]);
 
         let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH];
diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs
index d8ef8be..5f7df97 100644
--- a/src/dynamics/solver/velocity_ground_constraint.rs
+++ b/src/dynamics/solver/velocity_ground_constraint.rs
@@ -71,13 +71,13 @@ impl VelocityGroundConstraint {
         let coll_pos2;
 
         if flipped {
-            coll_pos1 = rb2.position * manifold.delta2;
-            coll_pos2 = rb1.position * manifold.delta1;
+            coll_pos1 = manifold.position2;
+            coll_pos2 = manifold.position1;
             force_dir1 = coll_pos1 * (-manifold.local_n2);
             std::mem::swap(&mut rb1, &mut rb2);
         } else {
-            coll_pos1 = rb1.position * manifold.delta1;
-            coll_pos2 = rb2.position * manifold.delta2;
+            coll_pos1 = manifold.position1;
+            coll_pos2 = manifold.position2;
             force_dir1 = coll_pos1 * (-manifold.local_n1);
         }
 
diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs
index be01944..2c85f0f 100644
--- a/src/dynamics/solver/velocity_ground_constraint_wide.rs
+++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs
@@ -86,19 +86,13 @@ impl WVelocityGroundConstraint {
         let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]);
         let angvel2 = AngVector::<SimdFloat>::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]);
 
-        let pos1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]);
-        let pos2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]);
-
-        let delta1 = Isometry::from(
-            array![|ii| if flipped[ii] { manifolds[ii].delta2 } else { manifolds[ii].delta1 }; SIMD_WIDTH],
+        let coll_pos1 = Isometry::from(
+            array![|ii| if flipped[ii] { manifolds[ii].position2 } else { manifolds[ii].position1 }; SIMD_WIDTH],
         );
-        let delta2 = Isometry::from(
-            array![|ii| if flipped[ii] { manifolds[ii].delta1 } else { manifolds[ii].delta2 }; SIMD_WIDTH],
+        let coll_pos2 = Isometry::from(
+            array![|ii| if flipped[ii] { manifolds[ii].position1 } else { manifolds[ii].position2 }; SIMD_WIDTH],
         );
 
-        let coll_pos1 = pos1 * delta1;
-        let coll_pos2 = pos2 * delta2;
-
         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]);
 
diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs
index a4a176e..bf976b5 100644
--- a/src/geometry/contact.rs
+++ b/src/geometry/contact.rs
@@ -236,6 +236,8 @@ impl ContactPair {
         // (This order can be modified by the contact determination algorithm).
         let manifold = &mut self.manifolds[0];
         if manifold.pair.collider1 == self.pair.collider1 {
+            manifold.position1 = *coll1.position();
+            manifold.position2 = *coll2.position();
             (
                 coll1,
                 coll2,
@@ -243,6 +245,8 @@ impl ContactPair {
                 self.generator_workspace.as_mut().map(|w| &mut *w.0),
             )
         } else {
+            manifold.position1 = *coll2.position();
+            manifold.position2 = *coll1.position();
             (
                 coll2,
                 coll1,
@@ -293,12 +297,10 @@ pub struct ContactManifold {
     pub friction: f32,
     /// The restitution coefficient for all the contacts on this contact manifold.
     pub restitution: f32,
-    /// The relative position between the first collider and its parent at the time the
-    /// contact points were generated.
-    pub delta1: Isometry<f32>,
-    /// The relative position between the second collider and its parent at the time the
-    /// contact points were generated.
-    pub delta2: Isometry<f32>,
+    /// The world-space position of the first collider at the time the contact points were generated.
+    pub position1: Isometry<f32>,
+    /// The world-space position of the second collider at the time the contact points were generated.
+    pub position2: Isometry<f32>,
     /// Flags used to control some aspects of the constraints solver for this contact manifold.
     pub solver_flags: SolverFlags,
 }
@@ -308,8 +310,8 @@ impl ContactManifold {
         pair: ColliderPair,
         subshapes: (usize, usize),
         body_pair: BodyPair,
-        delta1: Isometry<f32>,
-        delta2: Isometry<f32>,
+        position1: Isometry<f32>,
+        position2: Isometry<f32>,
         friction: f32,
         restitution: f32,
         solver_flags: SolverFlags,
@@ -329,8 +331,8 @@ impl ContactManifold {
             warmstart_multiplier: Self::min_warmstart_multiplier(),
             friction,
             restitution,
-            delta1,
-            delta2,
+            position1,
+            position2,
             constraint_index: 0,
             position_constraint_index: 0,
             solver_flags,
@@ -353,8 +355,8 @@ impl ContactManifold {
             warmstart_multiplier: self.warmstart_multiplier,
             friction: self.friction,
             restitution: self.restitution,
-            delta1: self.delta1,
-            delta2: self.delta2,
+            position1: self.position1,
+            position2: self.position2,
             constraint_index: self.constraint_index,
             position_constraint_index: self.position_constraint_index,
             solver_flags: self.solver_flags,
@@ -382,8 +384,8 @@ impl ContactManifold {
             pair,
             (subshape1, subshape2),
             BodyPair::new(coll1.parent, coll2.parent),
-            *coll1.position_wrt_parent(),
-            *coll2.position_wrt_parent(),
+            *coll1.position(),
+            *coll2.position(),
             (coll1.friction + coll2.friction) * 0.5,
             (coll1.restitution + coll2.restitution) * 0.5,
             solver_flags,
@@ -427,7 +429,7 @@ impl ContactManifold {
         self.pair = self.pair.swap();
         self.body_pair = self.body_pair.swap();
         self.subshape_index_pair = (self.subshape_index_pair.1, self.subshape_index_pair.0);
-        std::mem::swap(&mut self.delta1, &mut self.delta2);
+        std::mem::swap(&mut self.position1, &mut self.position2);
     }
 
     pub(crate) fn update_warmstart_multiplier(&mut self) {
diff --git a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs
index 70360c9..f3790a1 100644
--- a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs
+++ b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs
@@ -143,6 +143,8 @@ fn do_generate_contacts(
         let manifold = &mut manifolds[sub_detector.manifold_id];
 
         let mut ctxt2 = if coll_pair.collider1 != manifold.pair.collider1 {
+            manifold.position1 = collider2.position;
+            manifold.position2 = collider1.position;
             PrimitiveContactGenerationContext {
                 prediction_distance,
                 collider1: collider2,
@@ -155,6 +157,8 @@ fn do_generate_contacts(
                 workspace: sub_detector.workspace.as_mut().map(|w| &mut *w.0),
             }
         } else {
+            manifold.position1 = collider1.position;
+            manifold.position2 = collider2.position;
             PrimitiveContactGenerationContext {
                 prediction_distance,
                 collider1,
diff --git a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs
index 3f7d7cd..238c84d 100644
--- a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs
+++ b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs
@@ -177,6 +177,8 @@ fn do_generate_contacts(
             .dispatch_primitives(ShapeType::Triangle, shape_type2);
 
         let mut ctxt2 = if ctxt_pair_pair.collider1 != manifold.pair.collider1 {
+            manifold.position1 = *collider2.position();
+            manifold.position2 = *collider1.position();
             PrimitiveContactGenerationContext {
                 prediction_distance: ctxt.prediction_distance,
                 collider1: collider2,
@@ -189,6 +191,8 @@ fn do_generate_contacts(
                 workspace: workspace2.as_mut().map(|w| &mut *w.0),
             }
         } else {
+            manifold.position1 = *collider1.position();
+            manifold.position2 = *collider2.position();
             PrimitiveContactGenerationContext {
                 prediction_distance: ctxt.prediction_distance,
                 collider1,