First public release of Rapier.v0.1.0

1 files changed, 485 insertions, 0 deletions

diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs
new file mode 100644
index 0000000..0beec0a
--- /dev/null
+++ b/src/geometry/contact.rs
@@ -0,0 +1,485 @@
+use crate::dynamics::BodyPair;
+use crate::geometry::contact_generator::ContactPhase;
+use crate::geometry::{Collider, ColliderPair, ColliderSet};
+use crate::math::{Isometry, Point, Vector};
+use std::any::Any;
+#[cfg(feature = "simd-is-enabled")]
+use {
+    crate::math::{SimdFloat, SIMD_WIDTH},
+    simba::simd::SimdValue,
+};
+
+#[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))]
+/// 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>,
+    #[cfg_attr(feature = "serde-serialize", serde(skip))]
+    pub(crate) generator_workspace: Option<Box<dyn Any + Send + Sync>>,
+}
+
+impl ContactPair {
+    pub(crate) fn new(
+        pair: ColliderPair,
+        generator: ContactPhase,
+        generator_workspace: Option<Box<dyn Any + Send + Sync>>,
+    ) -> 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,
+    ) -> (
+        &'b Collider,
+        &'b Collider,
+        &'a mut ContactManifold,
+        Option<&'a mut (dyn Any + Send + Sync)>,
+    ) {
+        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);
+            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),
+            )
+        } else {
+            (
+                coll2,
+                coll1,
+                manifold,
+                self.generator_workspace.as_mut().map(|w| &mut **w),
+            )
+        }
+    }
+}
+
+#[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,
+    // We put the friction and restitution here because
+    // this 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,
+}
+
+impl ContactManifold {
+    pub(crate) fn new(
+        pair: ColliderPair,
+        subshapes: (usize, usize),
+        body_pair: BodyPair,
+        friction: f32,
+        restitution: f32,
+    ) -> 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,
+            constraint_index: 0,
+            position_constraint_index: 0,
+        }
+    }
+
+    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,
+            constraint_index: self.constraint_index,
+            position_constraint_index: self.position_constraint_index,
+        }
+    }
+
+    pub(crate) fn from_colliders(pair: ColliderPair, coll1: &Collider, coll2: &Collider) -> Self {
+        Self::with_subshape_indices(pair, coll1, coll2, 0, 0)
+    }
+
+    pub(crate) fn with_subshape_indices(
+        pair: ColliderPair,
+        coll1: &Collider,
+        coll2: &Collider,
+        subshape1: usize,
+        subshape2: usize,
+    ) -> Self {
+        Self::new(
+            pair,
+            (subshape1, subshape2),
+            BodyPair::new(coll1.parent, coll2.parent),
+            (coll1.friction + coll2.friction) * 0.5,
+            (coll1.restitution + coll2.restitution) * 0.5,
+        )
+    }
+
+    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);
+    }
+
+    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 {
+        if self.points.len() == 0 {
+            return false;
+        }
+
+        //        const DOT_THRESHOLD: f32 = 0.crate::COS_10_DEGREES;
+        const DOT_THRESHOLD: f32 = crate::utils::COS_5_DEGREES;
+
+        let local_n2 = pos12 * self.local_n2;
+
+        if -self.local_n1.dot(&local_n2) < 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;
+
+            let dist_threshold = 0.001; // FIXME: this should not be hard-coded.
+            if na::distance_squared(&pt.local_p1, &new_local_p1) > dist_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;
+                    }
+                }
+            }
+        }
+    }
+}