diff options
Diffstat (limited to 'src/geometry/contact.rs')
| -rw-r--r-- | src/geometry/contact.rs | 333 |
1 files changed, 55 insertions, 278 deletions
diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs index a4a176e..1f0a902 100644 --- a/src/geometry/contact.rs +++ b/src/geometry/contact.rs @@ -1,7 +1,8 @@ +use crate::buckler::query::TrackedData; use crate::data::MaybeSerializableData; -use crate::dynamics::BodyPair; +use crate::dynamics::{BodyPair, RigidBodyHandle, RigidBodySet}; use crate::geometry::contact_generator::{ContactGeneratorWorkspace, ContactPhase}; -use crate::geometry::{Collider, ColliderPair, ColliderSet}; +use crate::geometry::{Collider, ColliderPair, ColliderSet, Contact, ContactManifold}; use crate::math::{Isometry, Point, Vector}; #[cfg(feature = "simd-is-enabled")] use { @@ -19,39 +20,6 @@ bitflags::bitflags! { } } -#[derive(Copy, Clone, Debug, PartialEq, Eq)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// The type local linear approximation of the neighborhood of a pair contact points on two shapes -pub enum KinematicsCategory { - /// Both neighborhoods are assimilated to a single point. - PointPoint, - /// The first shape's neighborhood at the contact point is assimilated to a plane while - /// the second is assimilated to a point. - PlanePoint, -} - -#[derive(Copy, Clone, Debug, PartialEq)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// Local contact geometry at the neighborhood of a pair of contact points. -pub struct ContactKinematics { - /// The local contact geometry. - pub category: KinematicsCategory, - /// The dilation applied to the first contact geometry. - pub radius1: f32, - /// The dilation applied to the second contact geometry. - pub radius2: f32, -} - -impl Default for ContactKinematics { - fn default() -> Self { - ContactKinematics { - category: KinematicsCategory::PointPoint, - radius1: 0.0, - radius2: 0.0, - } - } -} - #[cfg(feature = "simd-is-enabled")] pub(crate) struct WContact { pub local_p1: Point<SimdFloat>, @@ -70,10 +38,9 @@ impl WContact { local_p1: self.local_p1.extract(i), local_p2: self.local_p2.extract(i), dist: self.dist.extract(i), - impulse: 0.0, - tangent_impulse: Contact::zero_tangent_impulse(), fid1: self.fid1[i], fid2: self.fid2[i], + data: ContactData::default(), }; (c, self.local_n1.extract(i), self.local_n2.extract(i)) @@ -83,11 +50,7 @@ impl WContact { #[derive(Copy, Clone, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// A single contact between two collider. -pub struct Contact { - /// The contact point in the local-space of the first collider. - pub local_p1: Point<f32>, - /// The contact point in the local-space of the second collider. - pub local_p2: Point<f32>, +pub struct ContactData { /// The impulse, along the contact normal, applied by this contact to the first collider's rigid-body. /// /// The impulse applied to the second collider's rigid-body is given by `-impulse`. @@ -100,46 +63,9 @@ pub struct Contact { /// collider's rigid-body. #[cfg(feature = "dim3")] pub tangent_impulse: [f32; 2], - /// The identifier of the subshape of the first collider involved in this contact. - /// - /// For primitive shapes like cuboid, ball, etc., this is 0. - /// For shapes like trimesh and heightfield this identifies the specific triangle - /// involved in the contact. - pub fid1: u8, - /// The identifier of the subshape of the second collider involved in this contact. - /// - /// For primitive shapes like cuboid, ball, etc., this is 0. - /// For shapes like trimesh and heightfield this identifies the specific triangle - /// involved in the contact. - pub fid2: u8, - /// The distance between the two colliders along the contact normal. - /// - /// If this is negative, the colliders are penetrating. - pub dist: f32, } -impl Contact { - pub(crate) fn new( - local_p1: Point<f32>, - local_p2: Point<f32>, - fid1: u8, - fid2: u8, - dist: f32, - ) -> Self { - Self { - local_p1, - local_p2, - impulse: 0.0, - #[cfg(feature = "dim2")] - tangent_impulse: 0.0, - #[cfg(feature = "dim3")] - tangent_impulse: [0.0; 2], - fid1, - fid2, - dist, - } - } - +impl ContactData { #[cfg(feature = "dim2")] pub(crate) fn zero_tangent_impulse() -> f32 { 0.0 @@ -149,26 +75,15 @@ impl Contact { pub(crate) fn zero_tangent_impulse() -> [f32; 2] { [0.0, 0.0] } +} - pub(crate) fn copy_geometry_from(&mut self, contact: Contact) { - self.local_p1 = contact.local_p1; - self.local_p2 = contact.local_p2; - self.fid1 = contact.fid1; - self.fid2 = contact.fid2; - self.dist = contact.dist; +impl Default for ContactData { + fn default() -> Self { + Self { + impulse: 0.0, + tangent_impulse: Self::zero_tangent_impulse(), + } } - - // pub(crate) fn swap(self) -> Self { - // Self { - // local_p1: self.local_p2, - // local_p2: self.local_p1, - // impulse: self.impulse, - // tangent_impulse: self.tangent_impulse, - // fid1: self.fid2, - // fid2: self.fid1, - // dist: self.dist, - // } - // } } #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -227,15 +142,16 @@ impl ContactPair { let coll2 = &colliders[self.pair.collider2]; if self.manifolds.len() == 0 { - let manifold = ContactManifold::from_colliders(self.pair, coll1, coll2, flags); - self.manifolds.push(manifold); + let manifold_data = ContactManifoldData::from_colliders(self.pair, coll1, coll2, flags); + self.manifolds + .push(ContactManifold::with_data((0, 0), manifold_data)); } // We have to make sure the order of the returned collider // match the order of the pair stored inside of the manifold. // (This order can be modified by the contact determination algorithm). let manifold = &mut self.manifolds[0]; - if manifold.pair.collider1 == self.pair.collider1 { + if manifold.data.pair.collider1 == self.pair.collider1 { ( coll1, coll2, @@ -259,30 +175,12 @@ impl ContactPair { /// /// A contact manifold describes a set of contacts between two colliders. All the contact /// part of the same contact manifold share the same contact normal and contact kinematics. -pub struct ContactManifold { - // NOTE: use a SmallVec instead? - // And for 2D use an ArrayVec since there will never be more than 2 contacts anyways. - #[cfg(feature = "dim2")] - pub(super) points: arrayvec::ArrayVec<[Contact; 2]>, - #[cfg(feature = "dim3")] - pub(super) points: Vec<Contact>, - /// The number of active contacts on this contact manifold. - /// - /// Active contacts are these that may result in contact forces. - pub num_active_contacts: usize, - /// The contact normal of all the contacts of this manifold, expressed in the local space of the first collider. - pub local_n1: Vector<f32>, - /// The contact normal of all the contacts of this manifold, expressed in the local space of the second collider. - pub local_n2: Vector<f32>, - /// The contact kinematics of all the contacts of this manifold. - pub kinematics: ContactKinematics, +pub struct ContactManifoldData { // The following are set by the narrow-phase. - /// The pair of body involved in this contact manifold. - pub body_pair: BodyPair, /// The pair of colliders involved in this contact manifold. pub pair: ColliderPair, - /// The pair of subshapes involved in this contact manifold. - pub subshape_index_pair: (usize, usize), + /// The pair of body involved in this contact manifold. + pub body_pair: BodyPair, pub(crate) warmstart_multiplier: f32, // The two following are set by the constraints solver. pub(crate) constraint_index: usize, @@ -303,29 +201,44 @@ pub struct ContactManifold { pub solver_flags: SolverFlags, } -impl ContactManifold { +impl Default for ContactManifoldData { + fn default() -> Self { + Self::new( + ColliderPair::new(ColliderSet::invalid_handle(), ColliderSet::invalid_handle()), + BodyPair::new( + RigidBodySet::invalid_handle(), + RigidBodySet::invalid_handle(), + ), + Isometry::identity(), + Isometry::identity(), + 0.0, + 0.0, + SolverFlags::empty(), + ) + } +} + +impl TrackedData for ContactManifoldData { + fn flip(&mut self) { + std::mem::swap(&mut self.pair.collider1, &mut self.pair.collider2); + std::mem::swap(&mut self.body_pair.body1, &mut self.body_pair.body2); + std::mem::swap(&mut self.delta1, &mut self.delta2); + } +} + +impl ContactManifoldData { pub(crate) fn new( pair: ColliderPair, - subshapes: (usize, usize), body_pair: BodyPair, delta1: Isometry<f32>, delta2: Isometry<f32>, friction: f32, restitution: f32, solver_flags: SolverFlags, - ) -> ContactManifold { + ) -> ContactManifoldData { Self { - #[cfg(feature = "dim2")] - points: arrayvec::ArrayVec::new(), - #[cfg(feature = "dim3")] - points: Vec::new(), - num_active_contacts: 0, - local_n1: Vector::zeros(), - local_n2: Vector::zeros(), pair, - subshape_index_pair: subshapes, body_pair, - kinematics: ContactKinematics::default(), warmstart_multiplier: Self::min_warmstart_multiplier(), friction, restitution, @@ -337,50 +250,23 @@ impl ContactManifold { } } - pub(crate) fn take(&mut self) -> Self { - ContactManifold { - #[cfg(feature = "dim2")] - points: self.points.clone(), - #[cfg(feature = "dim3")] - points: std::mem::replace(&mut self.points, Vec::new()), - num_active_contacts: self.num_active_contacts, - local_n1: self.local_n1, - local_n2: self.local_n2, - kinematics: self.kinematics, - body_pair: self.body_pair, - pair: self.pair, - subshape_index_pair: self.subshape_index_pair, - 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, - solver_flags: self.solver_flags, - } - } - pub(crate) fn from_colliders( pair: ColliderPair, coll1: &Collider, coll2: &Collider, flags: SolverFlags, ) -> Self { - Self::with_subshape_indices(pair, coll1, coll2, 0, 0, flags) + Self::with_subshape_indices(pair, coll1, coll2, flags) } pub(crate) fn with_subshape_indices( pair: ColliderPair, coll1: &Collider, coll2: &Collider, - subshape1: usize, - subshape2: usize, solver_flags: SolverFlags, ) -> Self { Self::new( pair, - (subshape1, subshape2), BodyPair::new(coll1.parent, coll2.parent), *coll1.position_wrt_parent(), *coll2.position_wrt_parent(), @@ -398,134 +284,25 @@ impl ContactManifold { 0.01 } - /// Number of active contacts on this contact manifold. - #[inline] - pub fn num_active_contacts(&self) -> usize { - self.num_active_contacts - } - - /// The slice of all the active contacts on this contact manifold. - /// - /// Active contacts are contacts that may end up generating contact forces. - #[inline] - pub fn active_contacts(&self) -> &[Contact] { - &self.points[..self.num_active_contacts] - } - - #[inline] - pub(crate) fn active_contacts_mut(&mut self) -> &mut [Contact] { - &mut self.points[..self.num_active_contacts] - } - - /// The slice of all the contacts, active or not, on this contact manifold. - #[inline] - pub fn all_contacts(&self) -> &[Contact] { - &self.points - } - - pub(crate) fn swap_identifiers(&mut self) { - 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) { + pub(crate) fn update_warmstart_multiplier(manifold: &mut ContactManifold) { // In 2D, tall stacks will actually suffer from this // because oscillation due to inaccuracies in 2D often // cause contacts to break, which would result in // a reset of the warmstart multiplier. if cfg!(feature = "dim2") { - self.warmstart_multiplier = 1.0; + manifold.data.warmstart_multiplier = 1.0; return; } - for pt in &self.points { - if pt.impulse != 0.0 { - self.warmstart_multiplier = (self.warmstart_multiplier * 2.0).min(1.0); + for pt in &manifold.points { + if pt.data.impulse != 0.0 { + manifold.data.warmstart_multiplier = + (manifold.data.warmstart_multiplier * 2.0).min(1.0); return; } } // Reset the multiplier. - self.warmstart_multiplier = Self::min_warmstart_multiplier() - } - - #[inline] - pub(crate) fn try_update_contacts(&mut self, pos12: &Isometry<f32>) -> bool { - // const DOT_THRESHOLD: f32 = 0.crate::COS_10_DEGREES; - const DOT_THRESHOLD: f32 = crate::utils::COS_5_DEGREES; - const DIST_SQ_THRESHOLD: f32 = 0.001; // FIXME: this should not be hard-coded. - self.try_update_contacts_eps(pos12, DOT_THRESHOLD, DIST_SQ_THRESHOLD) - } - - #[inline] - pub(crate) fn try_update_contacts_eps( - &mut self, - pos12: &Isometry<f32>, - angle_dot_threshold: f32, - dist_sq_threshold: f32, - ) -> bool { - if self.points.len() == 0 { - return false; - } - - let local_n2 = pos12 * self.local_n2; - - if -self.local_n1.dot(&local_n2) < angle_dot_threshold { - return false; - } - - for pt in &mut self.points { - let local_p2 = pos12 * pt.local_p2; - let dpt = local_p2 - pt.local_p1; - let dist = dpt.dot(&self.local_n1); - - if dist * pt.dist < 0.0 { - // We switched between penetrating/non-penetrating. - // The may result in other contacts to appear. - return false; - } - let new_local_p1 = local_p2 - self.local_n1 * dist; - - if na::distance_squared(&pt.local_p1, &new_local_p1) > dist_sq_threshold { - return false; - } - - pt.dist = dist; - pt.local_p1 = new_local_p1; - } - - true - } - - /// Sort the contacts of this contact manifold such that the active contacts are in the first - /// positions of the array. - #[inline] - pub(crate) fn sort_contacts(&mut self, prediction_distance: f32) { - let num_contacts = self.points.len(); - match num_contacts { - 0 => { - self.num_active_contacts = 0; - } - 1 => { - self.num_active_contacts = (self.points[0].dist < prediction_distance) as usize; - } - _ => { - let mut first_inactive_index = num_contacts; - - self.num_active_contacts = 0; - while self.num_active_contacts != first_inactive_index { - if self.points[self.num_active_contacts].dist >= prediction_distance { - // Swap with the last contact. - self.points - .swap(self.num_active_contacts, first_inactive_index - 1); - first_inactive_index -= 1; - } else { - self.num_active_contacts += 1; - } - } - } - } + manifold.data.warmstart_multiplier = Self::min_warmstart_multiplier() } } |