diff options
Diffstat (limited to 'src/geometry/contact.rs')
| -rw-r--r-- | src/geometry/contact.rs | 531 |
1 files changed, 0 insertions, 531 deletions
diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs deleted file mode 100644 index a4a176e..0000000 --- a/src/geometry/contact.rs +++ /dev/null @@ -1,531 +0,0 @@ -use crate::data::MaybeSerializableData; -use crate::dynamics::BodyPair; -use crate::geometry::contact_generator::{ContactGeneratorWorkspace, ContactPhase}; -use crate::geometry::{Collider, ColliderPair, ColliderSet}; -use crate::math::{Isometry, Point, Vector}; -#[cfg(feature = "simd-is-enabled")] -use { - crate::math::{SimdFloat, SIMD_WIDTH}, - simba::simd::SimdValue, -}; - -bitflags::bitflags! { - #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] - /// Flags affecting the behavior of the constraints solver for a given contact manifold. - pub struct SolverFlags: u32 { - /// The constraint solver will take this contact manifold into - /// account for force computation. - const COMPUTE_IMPULSES = 0b01; - } -} - -#[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>, - pub local_p2: Point<SimdFloat>, - pub local_n1: Vector<SimdFloat>, - pub local_n2: Vector<SimdFloat>, - pub dist: SimdFloat, - pub fid1: [u8; SIMD_WIDTH], - pub fid2: [u8; SIMD_WIDTH], -} - -#[cfg(feature = "simd-is-enabled")] -impl WContact { - pub fn extract(&self, i: usize) -> (Contact, Vector<f32>, Vector<f32>) { - let c = Contact { - 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], - }; - - (c, self.local_n1.extract(i), self.local_n2.extract(i)) - } -} - -#[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>, - /// 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`. - pub impulse: f32, - /// The friction impulse along the vector orthonormal to the contact normal, applied to the first - /// collider's rigid-body. - #[cfg(feature = "dim2")] - pub tangent_impulse: f32, - /// The friction impulses along the basis orthonormal to the contact normal, applied to the first - /// 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, - } - } - - #[cfg(feature = "dim2")] - pub(crate) fn zero_tangent_impulse() -> f32 { - 0.0 - } - - #[cfg(feature = "dim3")] - 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; - } - - // 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))] -#[derive(Clone)] -/// The description of all the contacts between a pair of colliders. -pub struct ContactPair { - /// The pair of colliders involved. - pub pair: ColliderPair, - /// The set of contact manifolds between the two colliders. - /// - /// All contact manifold contain themselves contact points between the colliders. - pub manifolds: Vec<ContactManifold>, - #[cfg_attr(feature = "serde-serialize", serde(skip))] - pub(crate) generator: Option<ContactPhase>, - pub(crate) generator_workspace: Option<ContactGeneratorWorkspace>, -} - -impl ContactPair { - pub(crate) fn new( - pair: ColliderPair, - generator: ContactPhase, - generator_workspace: Option<ContactGeneratorWorkspace>, - ) -> Self { - Self { - pair, - manifolds: Vec::new(), - generator: Some(generator), - generator_workspace, - } - } - - /// Does this contact pair have any active contact? - /// - /// An active contact is a contact that may result in a non-zero contact force. - pub fn has_any_active_contact(&self) -> bool { - for manifold in &self.manifolds { - if manifold.num_active_contacts != 0 { - return true; - } - } - - false - } - - pub(crate) fn single_manifold<'a, 'b>( - &'a mut self, - colliders: &'b ColliderSet, - flags: SolverFlags, - ) -> ( - &'b Collider, - &'b Collider, - &'a mut ContactManifold, - Option<&'a mut (dyn MaybeSerializableData)>, - ) { - let coll1 = &colliders[self.pair.collider1]; - 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); - } - - // 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 { - ( - coll1, - coll2, - manifold, - self.generator_workspace.as_mut().map(|w| &mut *w.0), - ) - } else { - ( - coll2, - coll1, - manifold, - self.generator_workspace.as_mut().map(|w| &mut *w.0), - ) - } - } -} - -#[derive(Clone, Debug)] -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -/// A contact manifold between two colliders. -/// -/// 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, - // 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), - pub(crate) warmstart_multiplier: f32, - // 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 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>, - /// Flags used to control some aspects of the constraints solver for this contact manifold. - pub solver_flags: SolverFlags, -} - -impl ContactManifold { - 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 { - 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, - delta1, - delta2, - constraint_index: 0, - position_constraint_index: 0, - solver_flags, - } - } - - 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) - } - - 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(), - (coll1.friction + coll2.friction) * 0.5, - (coll1.restitution + coll2.restitution) * 0.5, - solver_flags, - ) - } - - pub(crate) fn min_warmstart_multiplier() -> f32 { - // Multiplier used to reduce the amount of warm-starting. - // This coefficient increases exponentially over time, until it reaches 1.0. - // This will reduce significant overshoot at the timesteps that - // follow a timestep involving high-velocity impacts. - 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) { - // 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; - return; - } - - for pt in &self.points { - if pt.impulse != 0.0 { - self.warmstart_multiplier = (self.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; - } - } - } - } - } -} |