diff options
Diffstat (limited to 'src/geometry')
| -rw-r--r-- | src/geometry/contact.rs | 24 | ||||
| -rw-r--r-- | src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs | 38 |
2 files changed, 33 insertions, 29 deletions
diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs index 0beec0a..2cda3e0 100644 --- a/src/geometry/contact.rs +++ b/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) { diff --git a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs index 0d382ac..d879a22 100644 --- a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs +++ b/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; |