Simplify flipping in contact manifold generators

src/pipeline/narrow_phase/contact_generator/contact_manifold_generator.rs

@@ -33,15 +33,91 @@ pub trait ContactManifoldGenerator<N: RealField>: Any + Send + Sync {
     fn init_manifold(&self) -> ContactManifold<N> {
         ContactManifold::new()
     }
+
+    /// Construct a version of `Self` that flips the arguments corresponding to each object.
+    ///
+    /// Allows implementations to assume a particular argument order (e.g. placing a shape of a
+    /// particular type first) for convenience, while still allowing a `ContactDispatcher` to
+    /// support either argument ordering.
+    fn flip(self) -> FlippedContactManifoldGenerator<Self>
+    where
+        Self: Sized,
+    {
+        FlippedContactManifoldGenerator(self)
+    }
+}
+
+pub struct FlippedContactManifoldGenerator<T>(T);
+
+impl<N: RealField, T: ContactManifoldGenerator<N>> ContactManifoldGenerator<N>
+    for FlippedContactManifoldGenerator<T>
+{
+    fn generate_contacts(
+        &mut self,
+        d: &dyn ContactDispatcher<N>,
+        ma: &Isometry<N>,
+        a: &dyn Shape<N>,
+        proc1: Option<&dyn ContactPreprocessor<N>>,
+        mb: &Isometry<N>,
+        b: &dyn Shape<N>,
+        proc2: Option<&dyn ContactPreprocessor<N>>,
+        prediction: &ContactPrediction<N>,
+        manifold: &mut ContactManifold<N>,
+    ) -> bool {
+        let dispatcher = FlippedContactDispatcher(d);
+        manifold.flip_new_contacts();
+        let result = self.0.generate_contacts(
+            &dispatcher,
+            ma,
+            a,
+            proc1,
+            mb,
+            b,
+            proc2,
+            prediction,
+            manifold,
+        );
+        manifold.flip_new_contacts();
+        result
+    }
 }
 
 pub type ContactAlgorithm<N> = Box<dyn ContactManifoldGenerator<N>>;
 
-pub trait ContactDispatcher<N>: Any + Send + Sync {
+pub trait ContactDispatcher<N>: Send + Sync {
     /// Allocate a collision algorithm corresponding to a pair of objects with the given shapes.
+    ///
+    /// Shorthand for `self.get_flipped_contact_algorithm(false, a, b)`.
     fn get_contact_algorithm(
         &self,
         a: &dyn Shape<N>,
         b: &dyn Shape<N>,
+    ) -> Option<ContactAlgorithm<N>> {
+        self.get_flipped_contact_algorithm(false, a, b)
+    }
+
+    /// Allocate a collision algorithm corresponding to a pair of objects with the given shapes.
+    ///
+    /// If `flip` is true, `a` and `b` are reversed, as are the corresponding arguments to any
+    /// returned `ContactAlgorithm`.
+    fn get_flipped_contact_algorithm(
+        &self,
+        flip: bool,
+        a: &dyn Shape<N>,
+        b: &dyn Shape<N>,
     ) -> Option<ContactAlgorithm<N>>;
 }
+
+/// A view of a `ContactDispatcher` which inverts the value of `flip` in `get_flipped_contact_algorithm`
+struct FlippedContactDispatcher<'a, N>(&'a dyn ContactDispatcher<N>);
+
+impl<N> ContactDispatcher<N> for FlippedContactDispatcher<'_, N> {
+    fn get_flipped_contact_algorithm(
+        &self,
+        flip: bool,
+        a: &dyn Shape<N>,
+        b: &dyn Shape<N>,
+    ) -> Option<ContactAlgorithm<N>> {
+        self.0.get_flipped_contact_algorithm(!flip, a, b)
+    }
+}

src/pipeline/narrow_phase/contact_generator/default_contact_dispatcher.rs

@@ -4,9 +4,9 @@ use crate::pipeline::{
     BallBallManifoldGenerator, BallConvexPolyhedronManifoldGenerator,
     CapsuleCapsuleManifoldGenerator, CapsuleShapeManifoldGenerator,
     CompositeShapeCompositeShapeManifoldGenerator, CompositeShapeShapeManifoldGenerator,
-    ContactAlgorithm, ContactDispatcher, ConvexPolyhedronConvexPolyhedronManifoldGenerator,
-    HeightFieldShapeManifoldGenerator, PlaneBallManifoldGenerator,
-    PlaneConvexPolyhedronManifoldGenerator,
+    ContactAlgorithm, ContactDispatcher, ContactManifoldGenerator,
+    ConvexPolyhedronConvexPolyhedronManifoldGenerator, HeightFieldShapeManifoldGenerator,
+    PlaneBallManifoldGenerator, PlaneConvexPolyhedronManifoldGenerator,
 };
 #[cfg(feature = "dim3")]
 use crate::shape::TriMesh;
@@ -24,8 +24,9 @@ impl DefaultContactDispatcher {
 }
 
 impl<N: RealField> ContactDispatcher<N> for DefaultContactDispatcher {
-    fn get_contact_algorithm(
+    fn get_flipped_contact_algorithm(
         &self,
+        flip: bool,
         a: &dyn Shape<N>,
         b: &dyn Shape<N>,
     ) -> Option<ContactAlgorithm<N>> {
@@ -44,55 +45,68 @@ impl<N: RealField> ContactDispatcher<N> for DefaultContactDispatcher {
             let b_is_trimesh = b.is_shape::<TriMesh<N>>();
 
             if a_is_trimesh && b_is_trimesh {
-                return Some(Box::new(TriMeshTriMeshManifoldGenerator::<N>::new()));
+                return Some(wrap(flip, TriMeshTriMeshManifoldGenerator::<N>::new()));
             }
         }
 
         if a_is_heightfield || b_is_heightfield {
-            return Some(Box::new(HeightFieldShapeManifoldGenerator::<N>::new(
-                b_is_heightfield,
-            )));
+            return Some(wrap(
+                flip ^ b_is_heightfield,
+                HeightFieldShapeManifoldGenerator::<N>::new(),
+            ));
         } else if a_is_capsule && b_is_capsule {
-            Some(Box::new(CapsuleCapsuleManifoldGenerator::<N>::new()))
+            Some(wrap(flip, CapsuleCapsuleManifoldGenerator::<N>::new()))
         } else if a_is_capsule || b_is_capsule {
-            Some(Box::new(CapsuleShapeManifoldGenerator::<N>::new(
-                b_is_capsule,
-            )))
+            Some(wrap(
+                flip,
+                CapsuleShapeManifoldGenerator::<N>::new(b_is_capsule),
+            ))
         } else if a_is_ball && b_is_ball {
-            Some(Box::new(BallBallManifoldGenerator::<N>::new()))
+            Some(wrap(flip, BallBallManifoldGenerator::<N>::new()))
         } else if a_is_plane && b_is_ball {
-            Some(Box::new(PlaneBallManifoldGenerator::<N>::new(false)))
+            Some(wrap(flip, PlaneBallManifoldGenerator::<N>::new()))
         } else if a_is_ball && b_is_plane {
-            Some(Box::new(PlaneBallManifoldGenerator::<N>::new(true)))
+            Some(wrap(!flip, PlaneBallManifoldGenerator::<N>::new()))
         } else if a_is_plane && b.is_support_map() {
             let gen = PlaneConvexPolyhedronManifoldGenerator::<N>::new(false);
-            Some(Box::new(gen))
+            Some(wrap(flip, gen))
         } else if b_is_plane && a.is_support_map() {
             let gen = PlaneConvexPolyhedronManifoldGenerator::<N>::new(true);
-            Some(Box::new(gen))
+            Some(wrap(flip, gen))
         } else if a_is_ball && b.is_convex_polyhedron() {
             let gen = BallConvexPolyhedronManifoldGenerator::<N>::new(false);
-            Some(Box::new(gen))
+            Some(wrap(flip, gen))
         } else if b_is_ball && a.is_convex_polyhedron() {
             let gen = BallConvexPolyhedronManifoldGenerator::<N>::new(true);
-            Some(Box::new(gen))
+            Some(wrap(flip, gen))
         } else if a.is_convex_polyhedron() && b.is_convex_polyhedron() {
             let gen = ConvexPolyhedronConvexPolyhedronManifoldGenerator::new();
-            Some(Box::new(gen))
+            Some(wrap(flip, gen))
         } else if a.is_composite_shape() && b.is_composite_shape() {
-            Some(Box::new(
+            Some(wrap(
+                flip,
                 CompositeShapeCompositeShapeManifoldGenerator::<N>::new(),
             ))
         } else if a.is_composite_shape() {
-            Some(Box::new(CompositeShapeShapeManifoldGenerator::<N>::new(
-                false,
-            )))
+            Some(wrap(
+                flip,
+                CompositeShapeShapeManifoldGenerator::<N>::new(false),
+            ))
         } else if b.is_composite_shape() {
-            Some(Box::new(CompositeShapeShapeManifoldGenerator::<N>::new(
-                true,
-            )))
+            Some(wrap(
+                flip,
+                CompositeShapeShapeManifoldGenerator::<N>::new(true),
+            ))
         } else {
             None
         }
     }
 }
+
+fn wrap<N: RealField, T: ContactManifoldGenerator<N>>(flip: bool, x: T) -> ContactAlgorithm<N> {
+    if flip {
+        Box::new(x.flip())
+    } else {
+        Box::new(x)
+    }
+}

src/pipeline/narrow_phase/contact_generator/heightfield_shape_manifold_generator.rs

@@ -10,16 +10,14 @@ use std::collections::{hash_map::Entry, HashMap};
 /// Collision detector between an heightfield and another shape.
 pub struct HeightFieldShapeManifoldGenerator<N: RealField> {
     sub_detectors: HashMap<usize, (ContactAlgorithm<N>, usize), DeterministicState>,
-    flip: bool,
     timestamp: usize,
 }
 
 impl<N: RealField> HeightFieldShapeManifoldGenerator<N> {
     /// Creates a new collision detector between an heightfield and another shape.
-    pub fn new(flip: bool) -> HeightFieldShapeManifoldGenerator<N> {
+    pub fn new() -> HeightFieldShapeManifoldGenerator<N> {
         HeightFieldShapeManifoldGenerator {
             sub_detectors: HashMap::with_hasher(DeterministicState),
-            flip,
             timestamp: 0,
         }
     }
@@ -35,7 +33,6 @@ impl<N: RealField> HeightFieldShapeManifoldGenerator<N> {
         proc2: Option<&dyn ContactPreprocessor<N>>,
         prediction: &ContactPrediction<N>,
         manifold: &mut ContactManifold<N>,
-        flip: bool,
     ) {
         self.timestamp += 1;
 
@@ -48,20 +45,27 @@ impl<N: RealField> HeightFieldShapeManifoldGenerator<N> {
             .entry(i)
         {
             Entry::Occupied(mut entry) => {
-                let ok = if flip {
-                    entry.get_mut().0.generate_contacts(
-                        dispatcher,
-                        m2,
-                        g2,
-                        proc2,
-                        m1,
-                        elt1,
-                        Some(&(proc1, part_proc1)),
-                        prediction,
-                        manifold,
-                    )
-                } else {
-                    entry.get_mut().0.generate_contacts(
+                let ok = entry.get_mut().0.generate_contacts(
+                    dispatcher,
+                    m1,
+                    elt1,
+                    Some(&(proc1, part_proc1)),
+                    m2,
+                    g2,
+                    proc2,
+                    prediction,
+                    manifold,
+                );
+
+                if ok {
+                    entry.get_mut().1 = self.timestamp;
+                }
+            }
+            Entry::Vacant(entry) => {
+                let new_detector = dispatcher.get_contact_algorithm(elt1, g2);
+
+                if let Some(mut new_detector) = new_detector {
+                    let _ = new_detector.generate_contacts(
                         dispatcher,
                         m1,
                         elt1,
@@ -71,46 +75,7 @@ impl<N: RealField> HeightFieldShapeManifoldGenerator<N> {
                         proc2,
                         prediction,
                         manifold,
-                    )
-                };
-
-                if ok {
-                    entry.get_mut().1 = self.timestamp;
-                }
-            }
-            Entry::Vacant(entry) => {
-                let new_detector = if flip {
-                    dispatcher.get_contact_algorithm(g2, elt1)
-                } else {
-                    dispatcher.get_contact_algorithm(elt1, g2)
-                };
-
-                if let Some(mut new_detector) = new_detector {
-                    if flip {
-                        let _ = new_detector.generate_contacts(
-                            dispatcher,
-                            m2,
-                            g2,
-                            proc2,
-                            m1,
-                            elt1,
-                            Some(&(proc1, part_proc1)),
-                            prediction,
-                            manifold,
-                        );
-                    } else {
-                        let _ = new_detector.generate_contacts(
-                            dispatcher,
-                            m1,
-                            elt1,
-                            Some(&(proc1, part_proc1)),
-                            m2,
-                            g2,
-                            proc2,
-                            prediction,
-                            manifold,
-                        );
-                    }
+                    );
                     let _ = entry.insert((new_detector, self.timestamp));
                 }
             }
@@ -136,16 +101,9 @@ impl<N: RealField> ContactManifoldGenerator<N> for HeightFieldShapeManifoldGener
         prediction: &ContactPrediction<N>,
         manifold: &mut ContactManifold<N>,
     ) -> bool {
-        if !self.flip {
-            if let Some(hf) = a.as_shape::<HeightField<N>>() {
-                self.do_update(d, ma, hf, proc1, mb, b, proc2, prediction, manifold, false);
-                return true;
-            }
-        } else {
-            if let Some(hf) = b.as_shape::<HeightField<N>>() {
-                self.do_update(d, mb, hf, proc2, ma, a, proc1, prediction, manifold, true);
-                return true;
-            }
+        if let Some(hf) = a.as_shape::<HeightField<N>>() {
+            self.do_update(d, ma, hf, proc1, mb, b, proc2, prediction, manifold);
+            return true;
         }
 
         return false;

src/pipeline/narrow_phase/contact_generator/mod.rs

@@ -7,7 +7,7 @@ pub use self::composite_shape_composite_shape_manifold_generator::CompositeShape
 pub use self::composite_shape_shape_manifold_generator::CompositeShapeShapeManifoldGenerator;
 #[doc(inline)]
 pub use self::contact_manifold_generator::{
-    ContactAlgorithm, ContactDispatcher, ContactManifoldGenerator,
+    ContactAlgorithm, ContactDispatcher, ContactManifoldGenerator, FlippedContactManifoldGenerator,
 };
 pub use self::convex_polyhedron_convex_polyhedron_manifold_generator::ConvexPolyhedronConvexPolyhedronManifoldGenerator;
 pub use self::default_contact_dispatcher::DefaultContactDispatcher;