Fix regression on 2D contact kinematic determination.
@@ -100,23 +100,31 @@ where
match f1 {
FeatureId::Face { .. } => kinematic.set_plane1(f1, local1, n1.generators()[0]),
FeatureId::Edge { .. } => {
+ if na::dimension::<P::Vector>() == 2 {
+ kinematic.set_plane1(f1, local1, n1.generators()[0])
+ } else {
let e1 = self.manifold1.edge(f1).expect("Invalid edge id.");
let dir1 = m1.inverse_transform_unit_vector(&e1.direction().unwrap());
kinematic.set_line1(f1, local1, dir1, n1)
}
+ }
FeatureId::Vertex { .. } => kinematic.set_point1(f1, local1, n1),
- FeatureId::Unknown => unreachable!()
+ FeatureId::Unknown => unreachable!(),
}
match f2 {
FeatureId::Face { .. } => kinematic.set_plane2(f2, local2, n2.generators()[0]),
FeatureId::Edge { .. } => {
+ if na::dimension::<P::Vector>() == 2 {
+ kinematic.set_plane2(f2, local2, n2.generators()[0])
+ } else {
let e2 = self.manifold2.edge(f2).expect("Invalid edge id.");
let dir2 = m2.inverse_transform_unit_vector(&e2.direction().unwrap());
kinematic.set_line2(f2, local2, dir2, n2)
}
+ }
FeatureId::Vertex { .. } => kinematic.set_point2(f2, local2, n2),
- FeatureId::Unknown => unreachable!()
+ FeatureId::Unknown => unreachable!(),
}
if self.contact_manifold.push(c, kinematic, ids) {
@@ -127,8 +135,10 @@ where
fn reduce_manifolds_to_deepest_contact(&mut self, m1: &M, m2: &M) {
if let Some(deepest) = self.contact_manifold.deepest_contact() {
- self.manifold1.reduce_to_feature(deepest.kinematic.feature1());
- self.manifold2.reduce_to_feature(deepest.kinematic.feature2());
+ self.manifold1
+ .reduce_to_feature(deepest.kinematic.feature1());
+ self.manifold2
+ .reduce_to_feature(deepest.kinematic.feature2());
self.manifold1.transform_by(&m1.inverse());
self.manifold2.transform_by(&m2.inverse());
} else {
@@ -525,9 +535,6 @@ where
self.last_gjk_dir = Some(dir.unwrap());
if contact.depth > na::zero() {
- sma.support_face_toward(ma, &contact.normal, &mut self.manifold1);
- smb.support_face_toward(mb, &-contact.normal, &mut self.manifold2);
-
sma.support_face_toward(ma, &contact.normal, &mut self.manifold1);
smb.support_face_toward(mb, &-contact.normal, &mut self.manifold2);
self.clip_polyfaces(contact.normal);
