Merge branch 'master' into infinite_fall_memory

Fix merge conflict resulting from "axii" spelling correction

47 files changed, 2011 insertions, 634 deletions

diff --git a/src/dynamics/mass_properties.rs b/src/dynamics/mass_properties.rs
index 22e7da5..d64839c 100644
--- a/src/dynamics/mass_properties.rs
+++ b/src/dynamics/mass_properties.rs
@@ -91,7 +91,7 @@ impl MassProperties {
     }
 
     #[cfg(feature = "dim3")]
-    /// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axii.
+    /// Reconstructs the inverse angular inertia tensor of the rigid body from its principal inertia values and axes.
     pub fn reconstruct_inverse_inertia_matrix(&self) -> Matrix3<f32> {
         let inv_principal_inertia = self.inv_principal_inertia_sqrt.map(|e| e * e);
         self.principal_inertia_local_frame.to_rotation_matrix()
@@ -103,7 +103,7 @@ impl MassProperties {
     }
 
     #[cfg(feature = "dim3")]
-    /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axii.
+    /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axes.
     pub fn reconstruct_inertia_matrix(&self) -> Matrix3<f32> {
         let principal_inertia = self.inv_principal_inertia_sqrt.map(|e| utils::inv(e * e));
         self.principal_inertia_local_frame.to_rotation_matrix()
diff --git a/src/dynamics/mass_properties_capsule.rs b/src/dynamics/mass_properties_capsule.rs
index 5f08958..3b1b214 100644
--- a/src/dynamics/mass_properties_capsule.rs
+++ b/src/dynamics/mass_properties_capsule.rs
@@ -1,30 +1,9 @@
 use crate::dynamics::MassProperties;
 #[cfg(feature = "dim3")]
 use crate::geometry::Capsule;
-use crate::math::{Point, PrincipalAngularInertia, Vector};
+use crate::math::Point;
 
 impl MassProperties {
-    fn cylinder_y_volume_unit_inertia(
-        half_height: f32,
-        radius: f32,
-    ) -> (f32, PrincipalAngularInertia<f32>) {
-        #[cfg(feature = "dim2")]
-        {
-            Self::cuboid_volume_unit_inertia(Vector::new(radius, half_height))
-        }
-
-        #[cfg(feature = "dim3")]
-        {
-            let volume = half_height * radius * radius * std::f32::consts::PI * 2.0;
-            let sq_radius = radius * radius;
-            let sq_height = half_height * half_height * 4.0;
-            let off_principal = (sq_radius * 3.0 + sq_height) / 12.0;
-
-            let inertia = Vector::new(off_principal, sq_radius / 2.0, off_principal);
-            (volume, inertia)
-        }
-    }
-
     pub(crate) fn from_capsule(density: f32, a: Point<f32>, b: Point<f32>, radius: f32) -> Self {
         let half_height = (b - a).norm() / 2.0;
         let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius);
diff --git a/src/dynamics/mass_properties_cone.rs b/src/dynamics/mass_properties_cone.rs
new file mode 100644
index 0000000..12f831f
--- /dev/null
+++ b/src/dynamics/mass_properties_cone.rs
@@ -0,0 +1,29 @@
+use crate::dynamics::MassProperties;
+use crate::math::{Point, PrincipalAngularInertia, Rotation, Vector};
+
+impl MassProperties {
+    pub(crate) fn cone_y_volume_unit_inertia(
+        half_height: f32,
+        radius: f32,
+    ) -> (f32, PrincipalAngularInertia<f32>) {
+        let volume = radius * radius * std::f32::consts::PI * half_height * 2.0 / 3.0;
+        let sq_radius = radius * radius;
+        let sq_height = half_height * half_height * 4.0;
+        let off_principal = sq_radius * 3.0 / 20.0 + sq_height * 3.0 / 5.0;
+        let principal = sq_radius * 3.0 / 10.0;
+
+        (volume, Vector::new(off_principal, principal, off_principal))
+    }
+
+    pub(crate) fn from_cone(density: f32, half_height: f32, radius: f32) -> Self {
+        let (cyl_vol, cyl_unit_i) = Self::cone_y_volume_unit_inertia(half_height, radius);
+        let cyl_mass = cyl_vol * density;
+
+        Self::with_principal_inertia_frame(
+            Point::new(0.0, -half_height / 2.0, 0.0),
+            cyl_mass,
+            cyl_unit_i * cyl_mass,
+            Rotation::identity(),
+        )
+    }
+}
diff --git a/src/dynamics/mass_properties_cylinder.rs b/src/dynamics/mass_properties_cylinder.rs
new file mode 100644
index 0000000..7c8054a
--- /dev/null
+++ b/src/dynamics/mass_properties_cylinder.rs
@@ -0,0 +1,40 @@
+use crate::dynamics::MassProperties;
+#[cfg(feature = "dim3")]
+use crate::math::{Point, Rotation};
+use crate::math::{PrincipalAngularInertia, Vector};
+
+impl MassProperties {
+    pub(crate) fn cylinder_y_volume_unit_inertia(
+        half_height: f32,
+        radius: f32,
+    ) -> (f32, PrincipalAngularInertia<f32>) {
+        #[cfg(feature = "dim2")]
+        {
+            Self::cuboid_volume_unit_inertia(Vector::new(radius, half_height))
+        }
+
+        #[cfg(feature = "dim3")]
+        {
+            let volume = half_height * radius * radius * std::f32::consts::PI * 2.0;
+            let sq_radius = radius * radius;
+            let sq_height = half_height * half_height * 4.0;
+            let off_principal = (sq_radius * 3.0 + sq_height) / 12.0;
+
+            let inertia = Vector::new(off_principal, sq_radius / 2.0, off_principal);
+            (volume, inertia)
+        }
+    }
+
+    #[cfg(feature = "dim3")]
+    pub(crate) fn from_cylinder(density: f32, half_height: f32, radius: f32) -> Self {
+        let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius);
+        let cyl_mass = cyl_vol * density;
+
+        Self::with_principal_inertia_frame(
+            Point::origin(),
+            cyl_mass,
+            cyl_unit_i * cyl_mass,
+            Rotation::identity(),
+        )
+    }
+}
diff --git a/src/dynamics/mass_properties_polygon.rs b/src/dynamics/mass_properties_polygon.rs
index c87e888..8b0b811 100644
--- a/src/dynamics/mass_properties_polygon.rs
+++ b/src/dynamics/mass_properties_polygon.rs
@@ -1,3 +1,5 @@
+#![allow(dead_code)] // TODO: remove this
+
 use crate::dynamics::MassProperties;
 use crate::math::Point;
 
diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs
index 4499d95..10cdd29 100644
--- a/src/dynamics/mod.rs
+++ b/src/dynamics/mod.rs
@@ -22,7 +22,10 @@ mod joint;
 mod mass_properties;
 mod mass_properties_ball;
 mod mass_properties_capsule;
+#[cfg(feature = "dim3")]
+mod mass_properties_cone;
 mod mass_properties_cuboid;
+mod mass_properties_cylinder;
 #[cfg(feature = "dim2")]
 mod mass_properties_polygon;
 mod rigid_body;
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index af1fb4a..417ce34 100644
--- a/src/dynamics/rigid_body.rs
+++ b/src/dynamics/rigid_body.rs
@@ -54,6 +54,8 @@ pub struct RigidBody {
     pub(crate) active_set_timestamp: u32,
     /// The status of the body, governing how it is affected by external forces.
     pub body_status: BodyStatus,
+    /// User-defined data associated to this rigid-body.
+    pub user_data: u128,
 }
 
 impl Clone for RigidBody {
@@ -90,6 +92,7 @@ impl RigidBody {
             active_set_offset: 0,
             active_set_timestamp: 0,
             body_status: BodyStatus::Dynamic,
+            user_data: 0,
         }
     }
 
@@ -218,6 +221,7 @@ impl RigidBody {
         let shift = Translation::from(com.coords);
         shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse()
     }
+
     pub(crate) fn integrate(&mut self, dt: f32) {
         self.position = self.integrate_velocity(dt) * self.position;
     }
@@ -334,13 +338,14 @@ impl RigidBody {
     }
 }
 
-/// A builder for rigid-bodies.
+/// A builder for rigid-bodies.
 pub struct RigidBodyBuilder {
     position: Isometry<f32>,
     linvel: Vector<f32>,
     angvel: AngVector<f32>,
     body_status: BodyStatus,
     can_sleep: bool,
+    user_data: u128,
 }
 
 impl RigidBodyBuilder {
@@ -352,6 +357,7 @@ impl RigidBodyBuilder {
             angvel: na::zero(),
             body_status,
             can_sleep: true,
+            user_data: 0,
         }
     }
 
@@ -399,6 +405,12 @@ impl RigidBodyBuilder {
         self
     }
 
+    /// An arbitrary user-defined 128-bit integer associated to the rigid-bodies built by this builder.
+    pub fn user_data(mut self, data: u128) -> Self {
+        self.user_data = data;
+        self
+    }
+
     /// Sets the initial linear velocity of the rigid-body to be created.
     #[cfg(feature = "dim2")]
     pub fn linvel(mut self, x: f32, y: f32) -> Self {
@@ -433,6 +445,7 @@ impl RigidBodyBuilder {
         rb.linvel = self.linvel;
         rb.angvel = self.angvel;
         rb.body_status = self.body_status;
+        rb.user_data = self.user_data;
 
         if !self.can_sleep {
             rb.activation.threshold = -1.0;
diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs
index db83fa3..d8a46ee 100644
--- a/src/geometry/broad_phase_multi_sap.rs
+++ b/src/geometry/broad_phase_multi_sap.rs
@@ -337,7 +337,7 @@ impl SAPAxis {
 
 #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
 struct SAPRegion {
-    axii: [SAPAxis; DIM],
+    axes: [SAPAxis; DIM],
     existing_proxies: BitVec,
     #[cfg_attr(feature = "serde-serialize", serde(skip))]
     to_insert: Vec<usize>, // Workspace
@@ -347,14 +347,14 @@ struct SAPRegion {
 
 impl SAPRegion {
     pub fn new(bounds: AABB<f32>) -> Self {
-        let axii = [
+        let axes = [
             SAPAxis::new(bounds.mins.x, bounds.maxs.x),
             SAPAxis::new(bounds.mins.y, bounds.maxs.y),
             #[cfg(feature = "dim3")]
             SAPAxis::new(bounds.mins.z, bounds.maxs.z),
         ];
         SAPRegion {
-            axii,
+            axes,
             existing_proxies: BitVec::new(),
             to_insert: Vec::new(),
             update_count: 0,
@@ -394,14 +394,14 @@ impl SAPRegion {
             // Update endpoints.
             let mut deleted = 0;
             for dim in 0..DIM {
-                self.axii[dim].update_endpoints(dim, proxies, reporting);
-                deleted += self.axii[dim].delete_out_of_bounds_proxies(&mut self.existing_proxies);
+                self.axes[dim].update_endpoints(dim, proxies, reporting);
+                deleted += self.axes[dim].delete_out_of_bounds_proxies(&mut self.existing_proxies);
             }
 
             if deleted > 0 {
                 self.proxy_count -= deleted;
                 for dim in 0..DIM {
-                    self.axii[dim].delete_out_of_bounds_endpoints(&self.existing_proxies);
+                    self.axes[dim].delete_out_of_bounds_endpoints(&self.existing_proxies);
                 }
             }
 
@@ -411,9 +411,9 @@ impl SAPRegion {
         if !self.to_insert.is_empty() {
             // Insert new proxies.
             for dim in 1..DIM {
-                self.axii[dim].batch_insert(dim, &self.to_insert, proxies, None);
+                self.axes[dim].batch_insert(dim, &self.to_insert, proxies, None);
             }
-            self.axii[0].batch_insert(0, &self.to_insert, proxies, Some(reporting));
+            self.axes[0].batch_insert(0, &self.to_insert, proxies, Some(reporting));
             self.to_insert.clear();
 
             // In the rare event that all proxies leave this region in the next step, we need an
diff --git a/src/geometry/capsule.rs b/src/geometry/capsule.rs
index 0d754af..54736cc 100644
--- a/src/geometry/capsule.rs
+++ b/src/geometry/capsule.rs
@@ -1,18 +1,16 @@
-use crate::geometry::AABB;
+use crate::geometry::{Ray, RayIntersection, AABB};
 use crate::math::{Isometry, Point, Rotation, Vector};
 use approx::AbsDiffEq;
 use na::Unit;
-use ncollide::query::{PointProjection, PointQuery};
-use ncollide::shape::{FeatureId, Segment};
+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 segment with a radius.
+/// A capsule shape defined as a round segment.
 pub struct Capsule {
-    /// The first endpoint of the capsule.
-    pub a: Point<f32>,
-    /// The second enpdoint of the capsule.
-    pub b: Point<f32>,
+    /// The axis and endpoint of the capsule.
+    pub segment: Segment<f32>,
     /// The radius of the capsule.
     pub radius: f32,
 }
@@ -39,13 +37,14 @@ impl Capsule {
 
     /// 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 {
-        Self { a, b, radius }
+        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.a;
-        let b = pos * self.b;
+        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())
@@ -53,7 +52,7 @@ impl Capsule {
 
     /// The height of this capsule.
     pub fn height(&self) -> f32 {
-        (self.b - self.a).norm()
+        (self.segment.b - self.segment.a).norm()
     }
 
     /// The half-height of this capsule.
@@ -63,17 +62,17 @@ impl Capsule {
 
     /// The center of this capsule.
     pub fn center(&self) -> Point<f32> {
-        na::center(&self.a, &self.b)
+        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.a, pos * self.b, self.radius)
+        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.b - self.a;
+        let mut dir = self.segment.b - self.segment.a;
         if dir.y < 0.0 {
             dir = -dir;
         }
@@ -96,24 +95,49 @@ impl Capsule {
     }
 }
 
-// impl SupportMap<f32> for Capsule {
-//     fn local_support_point(&self, dir: &Vector) -> Point {
-//         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>) -> Point {
-//         if dir.dot(&self.a.coords) > dir.dot(&self.b.coords) {
-//             self.a + **dir * self.radius
-//         } else {
-//             self.b + **dir * self.radius
-//         }
-//     }
-// }
+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.
-// Wa should find a way to avoid this code duplication.
+// We should find a way to avoid this code duplication.
 impl PointQuery<f32> for Capsule {
     #[inline]
     fn project_point(
@@ -122,7 +146,7 @@ impl PointQuery<f32> for Capsule {
         pt: &Point<f32>,
         solid: bool,
     ) -> PointProjection<f32> {
-        let seg = Segment::new(self.a, self.b);
+        let seg = Segment::new(self.segment.a, self.segment.b);
         let proj = seg.project_point(m, pt, solid);
         let dproj = *pt - proj.point;
 
diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index 7c293b6..f53d75a 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -1,145 +1,189 @@
 use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
 use crate::geometry::{
-    Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon,
-    Proximity, Ray, RayIntersection, Triangle, Trimesh,
+    Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph,
+    InteractionGroups, Proximity, 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::{HasBoundingVolume, AABB};
-use ncollide::query::RayCast;
-use num::Zero;
+use ncollide::bounding_volume::AABB;
+use std::ops::Deref;
+use std::sync::Arc;
 
+/// The shape of a collider.
 #[derive(Clone)]
-#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
-/// An enum grouping all the possible shape of a collider.
-pub enum Shape {
-    /// A ball shape.
-    Ball(Ball),
-    /// A convex polygon shape.
-    Polygon(Polygon),
-    /// A cuboid shape.
-    Cuboid(Cuboid),
-    /// A capsule shape.
-    Capsule(Capsule),
-    /// A triangle shape.
-    Triangle(Triangle),
-    /// A triangle mesh shape.
-    Trimesh(Trimesh),
-    /// A heightfield shape.
-    HeightField(HeightField),
+pub struct ColliderShape(pub Arc<dyn Shape>);
+
+impl Deref for ColliderShape {
+    type Target = dyn Shape;
+    fn deref(&self) -> &dyn Shape {
+        &*self.0
+    }
 }
 
-impl Shape {
-    /// Gets a reference to the underlying ball shape, if `self` is one.
-    pub fn as_ball(&self) -> Option<&Ball> {
-        match self {
-            Shape::Ball(b) => Some(b),
-            _ => None,
-        }
+impl ColliderShape {
+    /// Initialize a ball shape defined by its radius.
+    pub fn ball(radius: f32) -> Self {
+        ColliderShape(Arc::new(Ball::new(radius)))
     }
 
-    /// Gets a reference to the underlying polygon shape, if `self` is one.
-    pub fn as_polygon(&self) -> Option<&Polygon> {
-        match self {
-            Shape::Polygon(p) => Some(p),
-            _ => None,
-        }
+    /// 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)))
     }
 
-    /// Gets a reference to the underlying cuboid shape, if `self` is one.
-    pub fn as_cuboid(&self) -> Option<&Cuboid> {
-        match self {
-            Shape::Cuboid(c) => Some(c),
-            _ => None,
-        }
+    /// 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,
+        )))
     }
 
-    /// Gets a reference to the underlying capsule shape, if `self` is one.
-    pub fn as_capsule(&self) -> Option<&Capsule> {
-        match self {
-            Shape::Capsule(c) => Some(c),
-            _ => None,
-        }
+    /// 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)))
     }
 
-    /// Gets a reference to the underlying triangle mesh shape, if `self` is one.
-    pub fn as_trimesh(&self) -> Option<&Trimesh> {
-        match self {
-            Shape::Trimesh(c) => Some(c),
-            _ => None,
-        }
+    /// Initialize a cuboid shape defined by its half-extents.
+    pub fn cuboid(half_extents: Vector<f32>) -> Self {
+        ColliderShape(Arc::new(Cuboid::new(half_extents)))
     }
 
-    /// Gets a reference to the underlying heightfield shape, if `self` is one.
-    pub fn as_heightfield(&self) -> Option<&HeightField> {
-        match self {
-            Shape::HeightField(h) => Some(h),
-            _ => None,
-        }
+    /// 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)))
     }
 
-    /// Gets a reference to the underlying triangle shape, if `self` is one.
-    pub fn as_triangle(&self) -> Option<&Triangle> {
-        match self {
-            Shape::Triangle(c) => Some(c),
-            _ => None,
-        }
+    /// Initialize a segment shape from its endpoints.
+    pub fn segment(a: Point<f32>, b: Point<f32>) -> Self {
+        ColliderShape(Arc::new(Segment::new(a, b)))
+    }
+
+    /// 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)))
     }