Merge pull request #79 from dimforge/split_geom

Move most of the geometric code to another crate.

57 files changed, 853 insertions, 8273 deletions

diff --git a/src/geometry/ball.rs b/src/geometry/ball.rs
deleted file mode 100644
index 7f4ad03..0000000
--- a/src/geometry/ball.rs
+++ /dev/null
@@ -1,16 +0,0 @@
-#[cfg(feature = "simd-is-enabled")]
-use crate::math::{Point, SimdFloat};
-
-#[cfg(feature = "simd-is-enabled")]
-#[derive(Copy, Clone, Debug)]
-pub(crate) struct WBall {
-    pub center: Point<SimdFloat>,
-    pub radius: SimdFloat,
-}
-
-#[cfg(feature = "simd-is-enabled")]
-impl WBall {
-    pub fn new(center: Point<SimdFloat>, radius: SimdFloat) -> Self {
-        WBall { center, radius }
-    }
-}
diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs
index 863990d..b0a274d 100644
--- a/src/geometry/broad_phase_multi_sap.rs
+++ b/src/geometry/broad_phase_multi_sap.rs
@@ -1,17 +1,17 @@
-use crate::data::hashmap::HashMap;
 use crate::data::pubsub::Subscription;
 use crate::dynamics::RigidBodySet;
 use crate::geometry::{ColliderHandle, ColliderSet, RemovedCollider};
-use crate::math::{Point, Vector, DIM};
+use crate::math::{Point, Real, Vector, DIM};
 use bit_vec::BitVec;
-use ncollide::bounding_volume::{BoundingVolume, AABB};
+use parry::bounding_volume::{BoundingVolume, AABB};
+use parry::utils::hashmap::HashMap;
 use std::cmp::Ordering;
 use std::ops::{Index, IndexMut};
 
 const NUM_SENTINELS: usize = 1;
 const NEXT_FREE_SENTINEL: u32 = u32::MAX;
-const SENTINEL_VALUE: f32 = f32::MAX;
-const CELL_WIDTH: f32 = 20.0;
+const SENTINEL_VALUE: Real = Real::MAX;
+const CELL_WIDTH: Real = 20.0;
 
 #[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -63,12 +63,12 @@ fn sort2(a: u32, b: u32) -> (u32, u32) {
     }
 }
 
-fn point_key(point: Point<f32>) -> Point<i32> {
+fn point_key(point: Point<Real>) -> Point<i32> {
     (point / CELL_WIDTH).coords.map(|e| e.floor() as i32).into()
 }
 
-fn region_aabb(index: Point<i32>) -> AABB<f32> {
-    let mins = index.coords.map(|i| i as f32 * CELL_WIDTH).into();
+fn region_aabb(index: Point<i32>) -> AABB {
+    let mins = index.coords.map(|i| i as Real * CELL_WIDTH).into();
     let maxs = mins + Vector::repeat(CELL_WIDTH);
     AABB::new(mins, maxs)
 }
@@ -76,7 +76,7 @@ fn region_aabb(index: Point<i32>) -> AABB<f32> {
 #[derive(Copy, Clone, Debug, PartialEq)]
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 struct Endpoint {
-    value: f32,
+    value: Real,
     packed_flag_proxy: u32,
 }
 
@@ -86,14 +86,14 @@ const START_SENTINEL_TAG: u32 = u32::MAX;
 const END_SENTINEL_TAG: u32 = u32::MAX ^ START_FLAG_MASK;
 
 impl Endpoint {
-    pub fn start_endpoint(value: f32, proxy: u32) -> Self {
+    pub fn start_endpoint(value: Real, proxy: u32) -> Self {
         Self {
             value,
             packed_flag_proxy: proxy | START_FLAG_MASK,
         }
     }
 
-    pub fn end_endpoint(value: f32, proxy: u32) -> Self {
+    pub fn end_endpoint(value: Real, proxy: u32) -> Self {
         Self {
             value,
             packed_flag_proxy: proxy & PROXY_MASK,
@@ -134,15 +134,15 @@ impl Endpoint {
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 #[derive(Clone)]
 struct SAPAxis {
-    min_bound: f32,
-    max_bound: f32,
+    min_bound: Real,
+    max_bound: Real,
     endpoints: Vec<Endpoint>,
     #[cfg_attr(feature = "serde-serialize", serde(skip))]
     new_endpoints: Vec<(Endpoint, usize)>, // Workspace
 }
 
 impl SAPAxis {
-    fn new(min_bound: f32, max_bound: f32) -> Self {
+    fn new(min_bound: Real, max_bound: Real) -> Self {
         assert!(min_bound <= max_bound);
 
         Self {
@@ -345,7 +345,7 @@ struct SAPRegion {
 }
 
 impl SAPRegion {
-    pub fn new(bounds: AABB<f32>) -> Self {
+    pub fn new(bounds: AABB) -> Self {
         let axes = [
             SAPAxis::new(bounds.mins.x, bounds.maxs.x),
             SAPAxis::new(bounds.mins.y, bounds.maxs.y),
@@ -361,7 +361,7 @@ impl SAPRegion {
         }
     }
 
-    pub fn recycle(bounds: AABB<f32>, mut old: Self) -> Self {
+    pub fn recycle(bounds: AABB, mut old: Self) -> Self {
         // Correct the bounds
         for (axis, &bound) in old.axes.iter_mut().zip(bounds.mins.iter()) {
             axis.min_bound = bound;
@@ -381,7 +381,7 @@ impl SAPRegion {
         old
     }
 
-    pub fn recycle_or_new(bounds: AABB<f32>, pool: &mut Vec<Self>) -> Self {
+    pub fn recycle_or_new(bounds: AABB, pool: &mut Vec<Self>) -> Self {
         if let Some(old) = pool.pop() {
             Self::recycle(bounds, old)
         } else {
@@ -477,8 +477,8 @@ pub struct BroadPhase {
     #[cfg_attr(
         feature = "serde-serialize",
         serde(
-            serialize_with = "crate::data::hashmap::serialize_hashmap_capacity",
-            deserialize_with = "crate::data::hashmap::deserialize_hashmap_capacity"
+            serialize_with = "parry::utils::hashmap::serialize_hashmap_capacity",
+            deserialize_with = "parry::utils::hashmap::deserialize_hashmap_capacity"
         )
     )]
     reporting: HashMap<(u32, u32), bool>, // Workspace
@@ -488,7 +488,7 @@ pub struct BroadPhase {
 #[derive(Clone)]
 pub(crate) struct BroadPhaseProxy {
     handle: ColliderHandle,
-    aabb: AABB<f32>,
+    aabb: AABB,
     next_free: u32,
 }
 
@@ -620,7 +620,7 @@ impl BroadPhase {
 
     pub(crate) fn update_aabbs(
         &mut self,
-        prediction_distance: f32,
+        prediction_distance: Real,
         bodies: &RigidBodySet,
         colliders: &mut ColliderSet,
     ) {
@@ -762,7 +762,7 @@ impl BroadPhase {
 #[cfg(test)]
 mod test {
     use crate::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
-    use crate::geometry::{BroadPhase, ColliderBuilder, ColliderSet, NarrowPhase};
+    use crate::geometry::{BroadPhase, ColliderBuilder, ColliderSet};
 
     #[test]
     fn test_add_update_remove() {
diff --git a/src/geometry/capsule.rs b/src/geometry/capsule.rs
deleted file mode 100644
index 54736cc..0000000
--- a/src/geometry/capsule.rs
+++ /dev/null
@@ -1,192 +0,0 @@
-use crate::geometry::{Ray, RayIntersection, AABB};
-use crate::math::{Isometry, Point, Rotation, Vector};
-use approx::AbsDiffEq;
-use na::Unit;
-use ncollide::query::{algorithms::VoronoiSimplex, PointProjection, PointQuery, RayCast};
-use ncollide::shape::{FeatureId, Segment, SupportMap};
-
-#[derive(Copy, Clone, Debug)]
-#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-/// A capsule shape defined as a round segment.
-pub struct Capsule {
-    /// The axis and endpoint of the capsule.
-    pub segment: Segment<f32>,
-    /// The radius of the capsule.
-    pub radius: f32,
-}
-
-impl Capsule {
-    /// Creates a new capsule aligned with the `x` axis and with the given half-height an radius.
-    pub fn new_x(half_height: f32, radius: f32) -> Self {
-        let b = Point::from(Vector::x() * half_height);
-        Self::new(-b, b, radius)
-    }
-
-    /// Creates a new capsule aligned with the `y` axis and with the given half-height an radius.
-    pub fn new_y(half_height: f32, radius: f32) -> Self {
-        let b = Point::from(Vector::y() * half_height);
-        Self::new(-b, b, radius)
-    }
-
-    /// Creates a new capsule aligned with the `z` axis and with the given half-height an radius.
-    #[cfg(feature = "dim3")]
-    pub fn new_z(half_height: f32, radius: f32) -> Self {
-        let b = Point::from(Vector::z() * half_height);
-        Self::new(-b, b, radius)
-    }
-
-    /// Creates a new capsule defined as the segment between `a` and `b` and with the given `radius`.
-    pub fn new(a: Point<f32>, b: Point<f32>, radius: f32) -> Self {
-        let segment = Segment::new(a, b);
-        Self { segment, radius }
-    }
-
-    /// The axis-aligned bounding box of this capsule.
-    pub fn aabb(&self, pos: &Isometry<f32>) -> AABB {
-        let a = pos * self.segment.a;
-        let b = pos * self.segment.b;
-        let mins = a.coords.inf(&b.coords) - Vector::repeat(self.radius);
-        let maxs = a.coords.sup(&b.coords) + Vector::repeat(self.radius);
-        AABB::new(mins.into(), maxs.into())
-    }
-
-    /// The height of this capsule.
-    pub fn height(&self) -> f32 {
-        (self.segment.b - self.segment.a).norm()
-    }
-
-    /// The half-height of this capsule.
-    pub fn half_height(&self) -> f32 {
-        self.height() / 2.0
-    }
-
-    /// The center of this capsule.
-    pub fn center(&self) -> Point<f32> {
-        na::center(&self.segment.a, &self.segment.b)
-    }
-
-    /// Creates a new capsule equal to `self` with all its endpoints transformed by `pos`.
-    pub fn transform_by(&self, pos: &Isometry<f32>) -> Self {
-        Self::new(pos * self.segment.a, pos * self.segment.b, self.radius)
-    }
-
-    /// The rotation `r` such that `r * Y` is collinear with `b - a`.
-    pub fn rotation_wrt_y(&self) -> Rotation<f32> {
-        let mut dir = self.segment.b - self.segment.a;
-        if dir.y < 0.0 {
-            dir = -dir;
-        }
-
-        #[cfg(feature = "dim2")]
-        {
-            Rotation::rotation_between(&Vector::y(), &dir)
-        }
-
-        #[cfg(feature = "dim3")]
-        {
-            Rotation::rotation_between(&Vector::y(), &dir).unwrap_or(Rotation::identity())
-        }
-    }
-
-    /// The transform `t` such that `t * Y` is collinear with `b - a` and such that `t * origin = (b + a) / 2.0`.
-    pub fn transform_wrt_y(&self) -> Isometry<f32> {
-        let rot = self.rotation_wrt_y();
-        Isometry::from_parts(self.center().coords.into(), rot)
-    }
-}
-
-impl SupportMap<f32> for Capsule {
-    fn local_support_point(&self, dir: &Vector<f32>) -> Point<f32> {
-        let dir = Unit::try_new(*dir, 0.0).unwrap_or(Vector::y_axis());
-        self.local_support_point_toward(&dir)
-    }
-
-    fn local_support_point_toward(&self, dir: &Unit<Vector<f32>>) -> Point<f32> {
-        if dir.dot(&self.segment.a.coords) > dir.dot(&self.segment.b.coords) {
-            self.segment.a + **dir * self.radius
-        } else {
-            self.segment.b + **dir * self.radius
-        }
-    }
-}
-
-impl RayCast<f32> for Capsule {
-    fn toi_and_normal_with_ray(
-        &self,
-        m: &Isometry<f32>,
-        ray: &Ray,
-        max_toi: f32,
-        solid: bool,
-    ) -> Option<RayIntersection> {
-        let ls_ray = ray.inverse_transform_by(m);
-
-        ncollide::query::ray_intersection_with_support_map_with_params(
-            &Isometry::identity(),
-            self,
-            &mut VoronoiSimplex::new(),
-            &ls_ray,
-            max_toi,
-            solid,
-        )
-        .map(|mut res| {
-            res.normal = m * res.normal;
-            res
-        })
-    }
-}
-
-// TODO: this code has been extracted from ncollide and added here
-// so we can modify it to fit with our new definition of capsule.
-// We should find a way to avoid this code duplication.
-impl PointQuery<f32> for Capsule {
-    #[inline]
-    fn project_point(
-        &self,
-        m: &Isometry<f32>,
-        pt: &Point<f32>,
-        solid: bool,
-    ) -> PointProjection<f32> {
-        let seg = Segment::new(self.segment.a, self.segment.b);
-        let proj = seg.project_point(m, pt, solid);
-        let dproj = *pt - proj.point;
-
-        if let Some((dir, dist)) = Unit::try_new_and_get(dproj, f32::default_epsilon()) {
-            let inside = dist <= self.radius;
-            if solid && inside {
-                return PointProjection::new(true, *pt);
-            } else {
-                return PointProjection::new(inside, proj.point + dir.into_inner() * self.radius);
-            }
-        } else if solid {
-            return PointProjection::new(true, *pt);
-        }
-
-        #[cfg(feature = "dim2")]
-        if let Some(dir) = seg.normal() {
-            let dir = m * *dir;
-            PointProjection::new(true, proj.point + dir * self.radius)
-        } else {
-            // The segment has no normal, likely because it degenerates to a point.
-            PointProjection::new(true, proj.point + Vector::ith(1, self.radius))
-        }
-
-        #[cfg(feature = "dim3")]
-        if let Some(dir) = seg.direction() {
-            use crate::utils::WBasis;
-            let dir = m * dir.orthonormal_basis()[0];
-            PointProjection::new(true, proj.point + dir * self.radius)
-        } else {
-            // The segment has no normal, likely because it degenerates to a point.
-            PointProjection::new(true, proj.point + Vector::ith(1, self.radius))
-        }
-    }
-
-    #[inline]
-    fn project_point_with_feature(
-        &self,
-        m: &Isometry<f32>,
-        pt: &Point<f32>,
-    ) -> (PointProjection<f32>, FeatureId) {
-        (self.project_point(m, pt, false), FeatureId::Face(0))
-    }
-}
diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index c04be35..fa2da68 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -1,190 +1,43 @@
-use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
-use crate::geometry::{
-    Ball, Capsule, Cuboid, HeightField, InteractionGroups, Segment, Shape, ShapeType, Triangle,
-    Trimesh,
-};
-#[cfg(feature = "dim3")]
-use crate::geometry::{Cone, Cylinder, RoundCylinder};
-use crate::math::{AngVector, Isometry, Point, Rotation, Vector};
-use na::Point3;
-use ncollide::bounding_volume::AABB;
-use std::ops::Deref;
-use std::sync::Arc;
-
-// TODO: move this to its own file.
-/// The shape of a collider.
-#[derive(Clone)]
-pub struct ColliderShape(pub Arc<dyn Shape>);
-
-impl Deref for ColliderShape {
-    type Target = dyn Shape;
-    fn deref(&self) -> &dyn Shape {
-        &*self.0
+use crate::dynamics::{CoefficientCombineRule, MassProperties, RigidBodyHandle};
+use crate::geometry::{InteractionGroups, SharedShape};
+use crate::math::{AngVector, Isometry, Point, Real, Rotation, Vector, DIM};
+use crate::parry::transformation::vhacd::VHACDParameters;
+use parry::bounding_volume::AABB;
+use parry::shape::Shape;
+
+bitflags::bitflags! {
+    #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+    /// Flags affecting the behavior of the constraints solver for a given contact manifold.
+    pub(crate) struct ColliderFlags: u8 {
+        const SENSOR = 1 << 0;
+        const FRICTION_COMBINE_RULE_01 = 1 << 1;
+        const FRICTION_COMBINE_RULE_10 = 1 << 2;
+        const RESTITUTION_COMBINE_RULE_01 = 1 << 3;
+        const RESTITUTION_COMBINE_RULE_10 = 1 << 4;
     }
 }
 
-impl ColliderShape {
-    /// Initialize a ball shape defined by its radius.
-    pub fn ball(radius: f32) -> Self {
-        ColliderShape(Arc::new(Ball::new(radius)))
-    }
-
-    /// Initialize a cylindrical shape defined by its half-height
-    /// (along along the y axis) and its radius.
-    #[cfg(feature = "dim3")]
-    pub fn cylinder(half_height: f32, radius: f32) -> Self {
-        ColliderShape(Arc::new(Cylinder::new(half_height, radius)))
-    }
-
-    /// Initialize a rounded cylindrical shape defined by its half-height
-    /// (along along the y axis), its radius, and its roundedness (the
-    /// radius of the sphere used for dilating the cylinder).
-    #[cfg(feature = "dim3")]
-    pub fn round_cylinder(half_height: f32, radius: f32, border_radius: f32) -> Self {
-        ColliderShape(Arc::new(RoundCylinder::new(
-            half_height,
-            radius,
-            border_radius,
-        )))
-    }
-
-    /// Initialize a cone shape defined by its half-height
-    /// (along along the y axis) and its basis radius.
-    #[cfg(feature = "dim3")]
-    pub fn cone(half_height: f32, radius: f32) -> Self {
-        ColliderShape(Arc::new(Cone::new(half_height, radius)))
-    }
-
-    /// Initialize a cuboid shape defined by its half-extents.
-    pub fn cuboid(half_extents: Vector<f32>) -> Self {
-        ColliderShape(Arc::new(Cuboid::new(half_extents)))
-    }
-
-    /// Initialize a capsule shape from its endpoints and radius.
-    pub fn capsule(a: Point<f32>, b: Point<f32>, radius: f32) -> Self {
-        ColliderShape(Arc::new(Capsule::new(a, b, radius)))
+impl ColliderFlags {
+    pub fn is_sensor(self) -> bool {
+        self.contains(ColliderFlags::SENSOR)
     }
 
-    /// Initialize a segment shape from its endpoints.
-    pub fn segment(a: Point<f32>, b: Point<f32>) -> Self {
-        ColliderShape(Arc::new(Segment::new(a, b)))
+    pub fn friction_combine_rule_value(self) -> u8 {
+        (self.bits & 0b0000_0110) >> 1
     }
 
-    /// Initializes a triangle shape.
-    pub fn triangle(a: Point<f32>, b: Point<f32>, c: Point<f32>) -> Self {
-        ColliderShape(Arc::new(Triangle::new(a, b, c)))
+    pub fn restitution_combine_rule_value(self) -> u8 {
+        (self.bits & 0b0001_1000) >> 3
     }
 
-    /// Initializes a triangle mesh shape defined by its vertex and index buffers.
-    pub fn trimesh(vertices: Vec<Point<f32>>, indices: Vec<Point3<u32>>) -> Self {
-        ColliderShape(Arc::new(Trimesh::new(vertices, indices)))
-    }
-
-    /// Initializes an heightfield shape defined by its set of height and a scale
-    /// factor along each coordinate axis.
-    #[cfg(feature = "dim2")]
-    pub fn heightfield(heights: na::DVector<f32>, scale: Vector<f32>) -> Self {
-        ColliderShape(Arc::new(HeightField::new(heights, scale)))
-    }
-
-    /// Initializes an heightfield shape on the x-z plane defined by its set of height and a scale
-    /// factor along each coordinate axis.
-    #[cfg(feature = "dim3")]
-    pub fn heightfield(heights: na::DMatrix<f32>, scale: Vector<f32>) -> Self {
-        ColliderShape(Arc::new(HeightField::new(heights, scale)))
-    }
-}
-
-#[cfg(feature = "serde-serialize")]
-impl serde::Serialize for ColliderShape {
-    fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
-    where
-        S: serde::Serializer,
-    {
-        use crate::serde::ser::SerializeStruct;
-
-        if let Some(ser) = self.0.as_serialize() {
-            let typ = self.0.shape_type();
-            let mut state = serializer.serialize_struct("ColliderShape", 2)?;
-            state.serialize_field("tag", &(typ as i32))?;
-            state.serialize_field("inner", ser)?;
-            state.end()
-        } else {
-            Err(serde::ser::Error::custom(
-                "Found a non-serializable custom shape.",
-            ))
-        }
+    pub fn with_friction_combine_rule(mut self, rule: CoefficientCombineRule) -> Self {
+        self.bits = (self.bits & !0b0000_0110) | ((rule as u8) << 1);
+        self
     }
-}
 
-#[cfg(feature = "serde-serialize")]
-impl<'de> serde::Deserialize<'de> for ColliderShape {
-    fn deserialize<D>(deserializer: D) -> Result<Self, D::Error>
-    where
-        D: serde::Deserializer<'de>,
-    {
-        struct Visitor {};
-        impl<'de> serde::de::Visitor<'de> fo