Extend ContactDispatcher to support automatically flipping objects

src/pipeline/narrow_phase/contact_generator/contact_manifold_generator.rs

@@ -33,15 +33,84 @@ 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
+    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: &na::Isometry3<N>,
+        a: &dyn Shape<N>,
+        proc1: Option<&dyn ContactPreprocessor<N>>,
+        mb: &na::Isometry3<N>,
+        b: &dyn Shape<N>,
+        proc2: Option<&dyn ContactPreprocessor<N>>,
+        prediction: &ContactPrediction<N>,
+        manifold: &mut ContactManifold<N>,
+    ) -> bool {
+        let dispatcher = FlippedContactDispatcher(d);
+        self.0.generate_contacts(
+            &dispatcher,
+            mb,
+            b,
+            proc2,
+            ma,
+            a,
+            proc1,
+            prediction,
+            manifold,
+        )
+    }
 }
 
 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,
+                HeightFieldShapeManifoldGenerator::<N>::new(b_is_heightfield),
+            ));
         } 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(false)))
         } else if a_is_ball && b_is_plane {
-            Some(Box::new(PlaneBallManifoldGenerator::<N>::new(true)))
+            Some(wrap(flip, PlaneBallManifoldGenerator::<N>::new(true)))
         } 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/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;

src/pipeline/narrow_phase/mod.rs

@@ -9,7 +9,7 @@ pub use self::contact_generator::{
     CompositeShapeCompositeShapeManifoldGenerator, CompositeShapeShapeManifoldGenerator,
     ContactAlgorithm, ContactDispatcher, ContactManifoldGenerator,
     ConvexPolyhedronConvexPolyhedronManifoldGenerator, DefaultContactDispatcher,
-    HeightFieldShapeManifoldGenerator, PlaneBallManifoldGenerator,
+    FlippedContactManifoldGenerator, HeightFieldShapeManifoldGenerator, PlaneBallManifoldGenerator,
     PlaneConvexPolyhedronManifoldGenerator,
 };
 pub use self::events::{ContactEvent, ContactEvents, EventPool, ProximityEvent, ProximityEvents};