diff --git a/src/pipeline/narrow_phase/contact_generator/default_contact_dispatcher.rs b/src/pipeline/narrow_phase/contact_generator/default_contact_dispatcher.rs index d041e5d35..43545c3fc 100644 --- a/src/pipeline/narrow_phase/contact_generator/default_contact_dispatcher.rs +++ b/src/pipeline/narrow_phase/contact_generator/default_contact_dispatcher.rs @@ -51,8 +51,8 @@ impl ContactDispatcher for DefaultContactDispatcher { if a_is_heightfield || b_is_heightfield { return Some(wrap( - flip, - HeightFieldShapeManifoldGenerator::::new(b_is_heightfield), + flip ^ b_is_heightfield, + HeightFieldShapeManifoldGenerator::::new(), )); } else if a_is_capsule && b_is_capsule { Some(wrap(flip, CapsuleCapsuleManifoldGenerator::::new())) diff --git a/src/pipeline/narrow_phase/contact_generator/heightfield_shape_manifold_generator.rs b/src/pipeline/narrow_phase/contact_generator/heightfield_shape_manifold_generator.rs index c1cbbd940..13c6fdd7b 100644 --- a/src/pipeline/narrow_phase/contact_generator/heightfield_shape_manifold_generator.rs +++ b/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 { sub_detectors: HashMap, usize), DeterministicState>, - flip: bool, timestamp: usize, } impl HeightFieldShapeManifoldGenerator { /// Creates a new collision detector between an heightfield and another shape. - pub fn new(flip: bool) -> HeightFieldShapeManifoldGenerator { + pub fn new() -> HeightFieldShapeManifoldGenerator { HeightFieldShapeManifoldGenerator { sub_detectors: HashMap::with_hasher(DeterministicState), - flip, timestamp: 0, } } @@ -47,20 +45,27 @@ impl HeightFieldShapeManifoldGenerator { .entry(i) { Entry::Occupied(mut entry) => { - let ok = if self.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, @@ -70,46 +75,7 @@ impl HeightFieldShapeManifoldGenerator { proc2, prediction, manifold, - ) - }; - - if ok { - entry.get_mut().1 = self.timestamp; - } - } - Entry::Vacant(entry) => { - let new_detector = if self.flip { - dispatcher.get_contact_algorithm(g2, elt1) - } else { - dispatcher.get_contact_algorithm(elt1, g2) - }; - - if let Some(mut new_detector) = new_detector { - if self.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)); } } @@ -135,16 +101,9 @@ impl ContactManifoldGenerator for HeightFieldShapeManifoldGener prediction: &ContactPrediction, manifold: &mut ContactManifold, ) -> bool { - if !self.flip { - if let Some(hf) = a.as_shape::>() { - self.do_update(d, ma, hf, proc1, mb, b, proc2, prediction, manifold); - return true; - } - } else { - if let Some(hf) = b.as_shape::>() { - self.do_update(d, mb, hf, proc2, ma, a, proc1, prediction, manifold); - return true; - } + if let Some(hf) = a.as_shape::>() { + self.do_update(d, ma, hf, proc1, mb, b, proc2, prediction, manifold); + return true; } return false;