Fix constraints resolution with non-identity relative collider position.

src/geometry/contact.rs

@@ -273,16 +273,21 @@ pub struct ContactManifold {
     /// The pair of subshapes involved in this contact manifold.
     pub subshape_index_pair: (usize, usize),
     pub(crate) warmstart_multiplier: f32,
-    // We put the friction and restitution here because
-    // this avoids reading the colliders inside of the
+    // The two following are set by the constraints solver.
+    pub(crate) constraint_index: usize,
+    pub(crate) position_constraint_index: usize,
+    // We put the following fields here to avoids reading the colliders inside of the
     // contact preparation method.
     /// The friction coefficient for of all the contacts on this contact manifold.
     pub friction: f32,
     /// The restitution coefficient for all the contacts on this contact manifold.
     pub restitution: f32,
-    // The following are set by the constraints solver.
-    pub(crate) constraint_index: usize,
-    pub(crate) position_constraint_index: usize,
+    /// 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>,
 }
 
 impl ContactManifold {
@@ -290,6 +295,8 @@ impl ContactManifold {
         pair: ColliderPair,
         subshapes: (usize, usize),
         body_pair: BodyPair,
+        delta1: Isometry<f32>,
+        delta2: Isometry<f32>,
         friction: f32,
         restitution: f32,
     ) -> ContactManifold {
@@ -308,6 +315,8 @@ impl ContactManifold {
             warmstart_multiplier: Self::min_warmstart_multiplier(),
             friction,
             restitution,
+            delta1,
+            delta2,
             constraint_index: 0,
             position_constraint_index: 0,
         }
@@ -329,6 +338,8 @@ impl ContactManifold {
             warmstart_multiplier: self.warmstart_multiplier,
             friction: self.friction,
             restitution: self.restitution,
+            delta1: self.delta1,
+            delta2: self.delta2,
             constraint_index: self.constraint_index,
             position_constraint_index: self.position_constraint_index,
         }
@@ -349,6 +360,8 @@ impl ContactManifold {
             pair,
             (subshape1, subshape2),
             BodyPair::new(coll1.parent, coll2.parent),
+            *coll1.delta(),
+            *coll2.delta(),
             (coll1.friction + coll2.friction) * 0.5,
             (coll1.restitution + coll2.restitution) * 0.5,
         )
@@ -391,6 +404,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);
     }
 
     pub(crate) fn update_warmstart_multiplier(&mut self) {

src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs

@@ -10,10 +10,8 @@ pub fn generate_contacts_cuboid_cuboid(ctxt: &mut PrimitiveContactGenerationCont
         generate_contacts(
             ctxt.prediction_distance,
             cube1,
-            ctxt.collider1.position_wrt_parent(),
             ctxt.position1,
             cube2,
-            ctxt.collider2.position_wrt_parent(),
             ctxt.position2,
             ctxt.manifold,
         );
@@ -28,19 +26,15 @@ pub fn generate_contacts_cuboid_cuboid(ctxt: &mut PrimitiveContactGenerationCont
 pub fn generate_contacts<'a>(
     prediction_distance: f32,
     mut cube1: &'a Cuboid<f32>,
-    mut origin1: &'a Isometry<f32>,
     mut pos1: &'a Isometry<f32>,
     mut cube2: &'a Cuboid<f32>,
-    mut origin2: &'a Isometry<f32>,
     mut pos2: &'a Isometry<f32>,
     manifold: &mut ContactManifold,
 ) {
     let mut pos12 = pos1.inverse() * pos2;
     let mut pos21 = pos12.inverse();
-    let mut orig_pos12 = origin1 * pos12 * origin2.inverse();
-    let mut orig_pos21 = orig_pos12.inverse();
 
-    if manifold.try_update_contacts(&orig_pos12) {
+    if manifold.try_update_contacts(&pos12) {
         return;
     }
 
@@ -87,9 +81,8 @@ pub fn generate_contacts<'a>(
     if sep2.0 > sep1.0 && sep2.0 > sep3.0 {
         // The reference shape will be the second shape.
         std::mem::swap(&mut cube1, &mut cube2);
+        std::mem::swap(&mut pos1, &mut pos2);
         std::mem::swap(&mut pos12, &mut pos21);
-        std::mem::swap(&mut orig_pos12, &mut orig_pos21);
-        std::mem::swap(&mut origin1, &mut origin2);
         manifold.swap_identifiers();
         best_sep = sep2;
         swapped = true;
@@ -104,49 +97,46 @@ pub fn generate_contacts<'a>(
 
     // Now the reference feature is from `cube1` and the best separation is `best_sep`.
     // Everything must be expressed in the local-space of `cube1` for contact clipping.
-    let mut feature1 = cuboid::support_feature(cube1, best_sep.1);
-    feature1.transform_by(origin1);
+    let feature1 = cuboid::support_feature(cube1, best_sep.1);
     let mut feature2 = cuboid::support_feature(cube2, pos21 * -best_sep.1);
     feature2.transform_by(&pos12);
-    feature2.transform_by(origin1);
-    let n1 = origin1 * best_sep.1;
 
     match (&feature1, &feature2) {
         (CuboidFeature::Face(f1), CuboidFeature::Vertex(v2)) => {
-            CuboidFeature::face_vertex_contacts(f1, &n1, v2, &orig_pos21, manifold)
+            CuboidFeature::face_vertex_contacts(f1, &best_sep.1, v2, &pos21, manifold)
         }
         #[cfg(feature = "dim3")]
         (CuboidFeature::Face(f1), CuboidFeature::Edge(e2)) => CuboidFeature::face_edge_contacts(
             prediction_distance,
             f1,
-            &n1,
+            &best_sep.1,
             e2,
-            &orig_pos21,
+            &pos21,
             manifold,
             false,
         ),
         (CuboidFeature::Face(f1), CuboidFeature::Face(f2)) => CuboidFeature::face_face_contacts(
             prediction_distance,
             f1,
-            &n1,
+            &best_sep.1,
             f2,
-            &orig_pos21,
+            &pos21,
             manifold,
         ),
         #[cfg(feature = "dim3")]
         (CuboidFeature::Edge(e1), CuboidFeature::Edge(e2)) => {
-            CuboidFeature::edge_edge_contacts(e1, &n1, e2, &orig_pos21, manifold)
+            CuboidFeature::edge_edge_contacts(e1, &best_sep.1, e2, &pos21, manifold)
         }
         #[cfg(feature = "dim3")]
         (CuboidFeature::Edge(e1), CuboidFeature::Face(f2)) => {
             // Since f2 is also expressed in the local-space of the first
-            // feature, the position we provide here is orig_pos21.
+            // feature, the position we provide here is pos21.
             CuboidFeature::face_edge_contacts(
                 prediction_distance,
                 f2,
-                &-n1,
+                &-best_sep.1,
                 e1,
-                &orig_pos21,
+                &pos21,
                 manifold,
                 true,
             )
@@ -154,8 +144,8 @@ pub fn generate_contacts<'a>(
         _ => unreachable!(), // The other cases are not possible.
     }
 
-    manifold.local_n1 = n1;
-    manifold.local_n2 = orig_pos21 * -n1;
+    manifold.local_n1 = best_sep.1;
+    manifold.local_n2 = pos21 * -best_sep.1;
     manifold.kinematics.category = KinematicsCategory::PlanePoint;
     manifold.kinematics.radius1 = 0.0;
     manifold.kinematics.radius2 = 0.0;