Start adding cylinders.

10 files changed, 282 insertions, 9 deletions

diff --git a/src/dynamics/mass_properties_capsule.rs b/src/dynamics/mass_properties_capsule.rs
index 5f08958..77ba96d 100644
--- a/src/dynamics/mass_properties_capsule.rs
+++ b/src/dynamics/mass_properties_capsule.rs
@@ -1,7 +1,7 @@
 use crate::dynamics::MassProperties;
 #[cfg(feature = "dim3")]
 use crate::geometry::Capsule;
-use crate::math::{Point, PrincipalAngularInertia, Vector};
+use crate::math::{Point, PrincipalAngularInertia, Rotation, Vector};
 
 impl MassProperties {
     fn cylinder_y_volume_unit_inertia(
@@ -57,4 +57,16 @@ impl MassProperties {
             )
         }
     }
+
+    #[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);
+
+        Self::with_principal_inertia_frame(
+            Point::origin(),
+            cyl_vol * density,
+            cyl_unit_i * density,
+            Rotation::identity(),
+        )
+    }
 }
diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index 7c293b6..fe42bd7 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -1,7 +1,9 @@
 use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet};
+#[cfg(feature = "dim3")]
+use crate::geometry::PolygonalFeatureMap;
 use crate::geometry::{
-    Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon,
-    Proximity, Ray, RayIntersection, Triangle, Trimesh,
+    Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, Cylinder, HeightField, InteractionGraph,
+    Polygon, Proximity, Ray, RayIntersection, Triangle, Trimesh,
 };
 use crate::math::{AngVector, Isometry, Point, Rotation, Vector};
 use na::Point3;
@@ -27,6 +29,9 @@ pub enum Shape {
     Trimesh(Trimesh),
     /// A heightfield shape.
     HeightField(HeightField),
+    #[cfg(feature = "dim3")]
+    /// A cylindrical shape.
+    Cylinder(Cylinder),
 }
 
 impl Shape {
@@ -86,6 +91,25 @@ impl Shape {
         }
     }
 
+    /// Gets a reference to the underlying cylindrical shape, if `self` is one.
+    pub fn as_cylinder(&self) -> Option<&Cylinder> {
+        match self {
+            Shape::Cylinder(c) => Some(c),
+            _ => None,
+        }
+    }
+
+    /// gets a reference to this shape seen as a PolygonalFeatureMap.
+    #[cfg(feature = "dim3")]
+    pub fn as_polygonal_feature_map(&self) -> Option<&dyn PolygonalFeatureMap> {
+        match self {
+            Shape::Triangle(t) => Some(t),
+            Shape::Cuboid(c) => Some(c),
+            Shape::Cylinder(c) => Some(c),
+            _ => None,
+        }
+    }
+
     /// Computes the axis-aligned bounding box of this shape.
     pub fn compute_aabb(&self, position: &Isometry<f32>) -> AABB<f32> {
         match self {
@@ -96,6 +120,7 @@ impl Shape {
             Shape::Triangle(triangle) => triangle.bounding_volume(position),
             Shape::Trimesh(trimesh) => trimesh.aabb(position),
             Shape::HeightField(heightfield) => heightfield.bounding_volume(position),
+            Shape::Cylinder(cylinder) => cylinder.bounding_volume(position),
         }
     }
 
@@ -139,6 +164,10 @@ impl Shape {
             Shape::HeightField(heightfield) => {
                 heightfield.toi_and_normal_with_ray(position, ray, max_toi, true)
             }
+            #[cfg(feature = "dim3")]
+            Shape::Cylinder(cylinder) => {
+                cylinder.toi_and_normal_with_ray(position, ray, max_toi, true)
+            }
         }
     }
 }
@@ -242,9 +271,12 @@ impl Collider {
             Shape::Capsule(caps) => {
                 MassProperties::from_capsule(self.density, caps.a, caps.b, caps.radius)
             }
-            Shape::Triangle(_) => MassProperties::zero(),
-            Shape::Trimesh(_) => MassProperties::zero(),
-            Shape::HeightField(_) => MassProperties::zero(),
+            Shape::Triangle(_) | Shape::Trimesh(_) | Shape::HeightField(_) => {
+                MassProperties::zero()
+            }
+            Shape::Cylinder(c) => {
+                MassProperties::from_cylinder(self.density, c.half_height, c.radius)
+            }
         }
     }
 }
@@ -291,6 +323,12 @@ impl ColliderBuilder {
         Self::new(Shape::Ball(Ball::new(radius)))
     }
 
+    /// Initialize a new collider builder with a cylindrical shape defined by its half-height
+    /// (along along the y axis) and its radius.
+    pub fn cylinder(half_height: f32, radius: f32) -> Self {
+        Self::new(Shape::Cylinder(Cylinder::new(half_height, radius)))
+    }
+
     /// Initialize a new collider builder with a cuboid shape defined by its half-extents.
     #[cfg(feature = "dim2")]
     pub fn cuboid(hx: f32, hy: f32) -> Self {
diff --git a/src/geometry/contact_generator/ball_convex_contact_generator.rs b/src/geometry/contact_generator/ball_convex_contact_generator.rs
index a187832..0856029 100644
--- a/src/geometry/contact_generator/ball_convex_contact_generator.rs
+++ b/src/geometry/contact_generator/ball_convex_contact_generator.rs
@@ -12,6 +12,7 @@ pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContex
             Shape::Triangle(tri2) => do_generate_contacts(tri2, ball1, ctxt, true),
             Shape::Cuboid(cube2) => do_generate_contacts(cube2, ball1, ctxt, true),
             Shape::Capsule(capsule2) => do_generate_contacts(capsule2, ball1, ctxt, true),
+            Shape::Cylinder(cylinder2) => do_generate_contacts(cylinder2, ball1, ctxt, true),
             _ => unimplemented!(),
         }
     } else if let Shape::Ball(ball2) = ctxt.shape2 {
@@ -19,6 +20,7 @@ pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContex
             Shape::Triangle(tri1) => do_generate_contacts(tri1, ball2, ctxt, false),
             Shape::Cuboid(cube1) => do_generate_contacts(cube1, ball2, ctxt, false),
             Shape::Capsule(capsule1) => do_generate_contacts(capsule1, ball2, ctxt, false),
+            Shape::Cylinder(cylinder1) => do_generate_contacts(cylinder1, ball2, ctxt, false),
             _ => unimplemented!(),
         }
     }
diff --git a/src/geometry/contact_generator/contact_dispatcher.rs b/src/geometry/contact_generator/contact_dispatcher.rs
index 8c846e0..e925fd5 100644
--- a/src/geometry/contact_generator/contact_dispatcher.rs
+++ b/src/geometry/contact_generator/contact_dispatcher.rs
@@ -1,6 +1,7 @@
 use crate::geometry::contact_generator::{
     ContactGenerator, ContactPhase, HeightFieldShapeContactGeneratorWorkspace,
-    PrimitiveContactGenerator, TrimeshShapeContactGeneratorWorkspace,
+    PfmPfmContactManifoldGeneratorWorkspace, PrimitiveContactGenerator,
+    TrimeshShapeContactGeneratorWorkspace,
 };
 use crate::geometry::Shape;
 use std::any::Any;
@@ -73,7 +74,9 @@ impl ContactDispatcher for DefaultContactDispatcher {
             | (Shape::Triangle(_), Shape::Ball(_))
             | (Shape::Ball(_), Shape::Triangle(_))
             | (Shape::Capsule(_), Shape::Ball(_))
-            | (Shape::Ball(_), Shape::Capsule(_)) => (
+            | (Shape::Ball(_), Shape::Capsule(_))
+            | (Shape::Cylinder(_), Shape::Ball(_))
+            | (Shape::Ball(_), Shape::Cylinder(_)) => (
                 PrimitiveContactGenerator {
                     generate_contacts: super::generate_contacts_ball_convex,
                     ..PrimitiveContactGenerator::default()
@@ -94,6 +97,13 @@ impl ContactDispatcher for DefaultContactDispatcher {
                 },
                 None,
             ),
+            (Shape::Cylinder(_), _) | (_, Shape::Cylinder(_)) => (
+                PrimitiveContactGenerator {
+                    generate_contacts: super::generate_contacts_pfm_pfm,
+                    ..PrimitiveContactGenerator::default()
+                },
+                Some(Box::new(PfmPfmContactManifoldGeneratorWorkspace::default())),
+            ),
             _ => (PrimitiveContactGenerator::default(), None),
         }
     }
diff --git a/src/geometry/contact_generator/mod.rs b/src/geometry/contact_generator/mod.rs
index ecd2540..a6bad05 100644
--- a/src/geometry/contact_generator/mod.rs
+++ b/src/geometry/contact_generator/mod.rs
@@ -18,6 +18,10 @@ pub use self::cuboid_triangle_contact_generator::generate_contacts_cuboid_triang
 pub use self::heightfield_shape_contact_generator::{
     generate_contacts_heightfield_shape, HeightFieldShapeContactGeneratorWorkspace,
 };
+#[cfg(feature = "dim3")]
+pub use self::pfm_pfm_contact_generator::{
+    generate_contacts_pfm_pfm, PfmPfmContactManifoldGeneratorWorkspace,
+};
 pub use self::polygon_polygon_contact_generator::generate_contacts_polygon_polygon;
 pub use self::trimesh_shape_contact_generator::{
     generate_contacts_trimesh_shape, TrimeshShapeContactGeneratorWorkspace,
@@ -39,6 +43,8 @@ mod cuboid_cuboid_contact_generator;
 mod cuboid_polygon_contact_generator;
 mod cuboid_triangle_contact_generator;
 mod heightfield_shape_contact_generator;
+#[cfg(feature = "dim3")]
+mod pfm_pfm_contact_generator;
 mod polygon_polygon_contact_generator;
 mod trimesh_shape_contact_generator;
 
diff --git a/src/geometry/contact_generator/pfm_pfm_contact_generator.rs b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs
new file mode 100644
index 0000000..cfb4472
--- /dev/null
+++ b/src/geometry/contact_generator/pfm_pfm_contact_generator.rs
@@ -0,0 +1,123 @@
+use crate::geometry::contact_generator::PrimitiveContactGenerationContext;
+use crate::geometry::{Contact, KinematicsCategory, PolygonalFeatureMap, PolyhedronFace};
+use crate::math::{Isometry, Vector};
+use na::Unit;
+use ncollide::query;
+use ncollide::query::algorithms::{gjk::GJKResult, VoronoiSimplex};
+
+pub struct PfmPfmContactManifoldGeneratorWorkspace {
+    simplex: VoronoiSimplex<f32>,
+    last_gjk_dir: Option<Unit<Vector<f32>>>,
+    last_optimal_dir: Option<Unit<Vector<f32>>>,
+    feature1: PolyhedronFace,
+    feature2: PolyhedronFace,
+}
+
+impl Default for PfmPfmContactManifoldGeneratorWorkspace {
+    fn default() -> Self {
+        Self {
+            simplex: VoronoiSimplex::new(),
+            last_gjk_dir: None,
+            last_optimal_dir: None,
+            feature1: PolyhedronFace::new(),
+            feature2: PolyhedronFace::new(),
+        }
+    }
+}
+
+pub fn generate_contacts_pfm_pfm(ctxt: &mut PrimitiveContactGenerationContext) {
+    if let (Some(pfm1), Some(pfm2)) = (
+        ctxt.collider1.shape().as_polygonal_feature_map(),
+        ctxt.collider2.shape().as_polygonal_feature_map(),
+    ) {
+        do_generate_contacts(pfm1, pfm2, ctxt);
+        ctxt.manifold.update_warmstart_multiplier();
+        ctxt.manifold.sort_contacts(ctxt.prediction_distance);
+    }
+}
+
+fn do_generate_contacts(
+    pfm1: &dyn PolygonalFeatureMap,
+    pfm2: &dyn PolygonalFeatureMap,
+    ctxt: &mut PrimitiveContactGenerationContext,
+) {
+    let pos12 = ctxt.position1.inverse() * ctxt.position2;
+    let pos21 = pos12.inverse();
+
+    // if ctxt.manifold.try_update_contacts(&pos12) {
+    //     return;
+    // }
+
+    let workspace: &mut PfmPfmContactManifoldGeneratorWorkspace = ctxt
+        .workspace
+        .as_mut()
+        .expect("The PfmPfmContactManifoldGeneratorWorkspace is missing.")
+        .downcast_mut()
+        .expect("Invalid workspace type, expected a PfmPfmContactManifoldGeneratorWorkspace.");
+
+    let contact = query::contact_support_map_support_map_with_params(
+        &Isometry::identity(),
+        pfm1,
+        &pos12,
+        pfm2,
+        ctxt.prediction_distance,
+        &mut workspace.simplex,
+        workspace.last_gjk_dir,
+    );
+
+    let old_manifold_points = ctxt.manifold.points.clone();
+    ctxt.manifold.points.clear();
+
+    match contact {
+        GJKResult::ClosestPoints(local_p1, local_p2, dir) => {
+            workspace.last_gjk_dir = Some(dir);
+            let normal1 = dir;
+            let normal2 = pos21 * -dir;
+            pfm1.local_support_feature(&normal1, &mut workspace.feature1);
+            pfm2.local_support_feature(&normal2, &mut workspace.feature2);
+            workspace.feature2.transform_by(&pos12);
+
+            // PolyhedronFace::contacts(
+            //     ctxt.prediction_distance,
+            //     &workspace.feature1,
+            //     &normal1,
+            //     &workspace.feature2,
+            //     &pos21,
+            //     ctxt.manifold,
+            // );
+
+            println!(
+                "Contact patatrac: {:?}, {:?}, {}, {}",
+                ctxt.manifold.points.len(),
+                ctxt.position1 * dir,
+                workspace.feature1.num_vertices,
+                workspace.feature2.num_vertices
+            );
+
+            if ctxt.manifold.all_contacts().is_empty() {
+                // Add at least the deepest contact.
+                let dist = (local_p2 - local_p1).dot(&dir);
+                ctxt.manifold.points.push(Contact {
+                    local_p1,
+                    local_p2: pos21 * local_p2,
+                    impulse: 0.0,
+                    tangent_impulse: Contact::zero_tangent_impulse(),
+                    fid1: 0, // FIXME
+                    fid2: 0, // FIXME
+                    dist,
+                });
+            }
+
+            // Adjust points to take the radius into account.
+            ctxt.manifold.local_n1 = *normal1;
+            ctxt.manifold.local_n2 = *normal2;
+            ctxt.manifold.kinematics.category = KinematicsCategory::PlanePoint; // FIXME
+            ctxt.manifold.kinematics.radius1 = 0.0;
+            ctxt.manifold.kinematics.radius2 = 0.0;
+        }
+        GJKResult::NoIntersection(dir) => {
+            workspace.last_gjk_dir = Some(dir);
+        }
+        _ => {}
+    }
+}
diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs
index 562f962..1ccb2c8 100644
--- a/src/geometry/mod.rs
+++ b/src/geometry/mod.rs
@@ -30,6 +30,9 @@ pub type Triangle = ncollide::shape::Triangle<f32>;
 pub type Ball = ncollide::shape::Ball<f32>;
 /// A heightfield shape.
 pub type HeightField = ncollide::shape::HeightField<f32>;
+/// A cylindrical shape.
+#[cfg(feature = "dim3")]
+pub type Cylinder = ncollide::shape::Cylinder<f32>;
 /// An axis-aligned bounding box.
 pub type AABB = ncollide::bounding_volume::AABB<f32>;
 /// Event triggered when two non-sensor colliders start or stop being in contact.
@@ -51,6 +54,8 @@ pub(crate) use self::contact::WContact;
 pub(crate) use self::contact_generator::{clip_segments, clip_segments_with_normal};
 pub(crate) use self::narrow_phase::ContactManifoldIndex;
 #[cfg(feature = "dim3")]
+pub(crate) use self::polygonal_feature_map::PolygonalFeatureMap;
+#[cfg(feature = "dim3")]
 pub(crate) use self::polyhedron_feature3d::PolyhedronFace;
 pub(crate) use self::waabb::{WRay, WAABB};
 pub(crate) use self::wquadtree::WQuadtree;
@@ -81,3 +86,5 @@ mod trimesh;
 mod waabb;
 mod wquadtree;
 //mod z_order;
+#[cfg(feature = "dim3")]
+mod polygonal_feature_map;
diff --git a/src/geometry/polygonal_feature_map.rs b/src/geometry/polygonal_feature_map.rs
new file mode 100644
index 0000000..8b047fc
--- /dev/null
+++ b/src/geometry/polygonal_feature_map.rs
@@ -0,0 +1,65 @@
+use crate::geometry::PolyhedronFace;
+use crate::geometry::{cuboid, Cuboid, Cylinder, Triangle};
+use crate::math::{Point, Vector};
+use approx::AbsDiffEq;
+use na::{Unit, Vector2, Vector3};
+use ncollide::shape::Segment;
+use ncollide::shape::SupportMap;
+
+/// Trait implemented by convex shapes with features with polyhedral approximations.
+pub trait PolygonalFeatureMap: SupportMap<f32> {
+    fn local_support_feature(&self, dir: &Unit<Vector<f32>>, out_feature: &mut PolyhedronFace);
+}
+
+impl PolygonalFeatureMap for Segment<f32> {
+    fn local_support_feature(&self, _: &Unit<Vector<f32>>, out_feature: &mut PolyhedronFace) {
+        *out_feature = PolyhedronFace::from(*self);
+    }
+}
+
+impl PolygonalFeatureMap for Triangle {
+    fn local_support_feature(&self, _: &Unit<Vector<f32>>, out_feature: &mut PolyhedronFace) {
+        *out_feature = PolyhedronFace::from(*self);
+    }
+}
+
+impl PolygonalFeatureMap for Cuboid {
+    fn local_support_feature(&self, dir: &Unit<Vector<f32>>, out_feature: &mut PolyhedronFace) {
+        let face = cuboid::support_face(self, **dir);
+        *out_feature = PolyhedronFace::from(face);
+    }
+}
+
+impl PolygonalFeatureMap for Cylinder {
+    fn local_support_feature(&self, dir: &Unit<Vector<f32>>, out_features: &mut PolyhedronFace) {
+        let dir2 = Vector2::new(dir.x, dir.z)
+            .try_normalize(f32::default_epsilon())
+            .unwrap_or(Vector2::x());
+
+        if dir.y.abs() < 0.5 {
+            // We return a segment lying on the cylinder's curved part.
+            out_features.vertices[0] = Point::new(
+                dir2.x * self.radius,
+                -self.half_height,
+                dir2.y * self.radius,
+            );
+            out_features.vertices[1] =
+                Point::new(dir2.x * self.radius, self.half_height, dir2.y * self.radius);
+            out_features.eids = [0, 0, 0, 0]; // FIXME
+            out_features.fid = 1;
+            out_features.num_vertices = 2;
+            out_features.vids = [0, 1, 1, 1]; // FIXME
+        } else {
+            // We return a square approximation of the cylinder cap.
+            let y = self.half_height.copysign(dir.y);
+            out_features.vertices[0] = Point::new(dir2.x * self.radius, y, dir2.y * self.radius);
+            out_features.vertices[1] = Point::new(-dir2.y * self.radius, y, dir2.x * self.radius);
+            out_features.vertices[2] = Point::new(-dir2.x * self.radius, y, -dir2.y * self.radius);
+            out_features.vertices[3] = Point::new(dir2.y * self.radius, y, -dir2.x * self.radius);
+            out_features.eids = [0, 1, 2, 3]; // FIXME
+            out_features.fid = if dir.y < 0.0 { 0 } else { 2 };
+            out_features.num_vertices = 4;
+            out_features.vids = [0, 1, 2, 3]; // FIXME
+        }
+    }
+}
diff --git a/src/geometry/polyhedron_feature3d.rs b/src/geometry/polyhedron_feature3d.rs
index dfeee29..5655f64 100644
--- a/src/geometry/polyhedron_feature3d.rs
+++ b/src/geometry/polyhedron_feature3d.rs
@@ -50,6 +50,16 @@ impl From<Segment<f32>> for PolyhedronFace {
 }
 
 impl PolyhedronFace {
+    pub fn new() -> Self {
+        Self {
+            vertices: [Point::origin(); 4],
+            vids: [0; 4],
+            eids: [0; 4],
+            fid: 0,
+            num_vertices: 0,
+        }
+    }
+
     pub fn transform_by(&mut self, iso: &Isometry<f32>) {
         for v in &mut self.vertices[0..self.num_vertices] {
             *v = iso * *v;
diff --git a/src/utils.rs b/src/utils.rs
index ecdd4fd..04e6a3a 100644
--- a/src/utils.rs
+++ b/src/utils.rs
@@ -91,7 +91,7 @@ impl<N: Scalar + Copy + WSign<N>> WSign<Vector3<N>> for Vector3<N> {
 
 impl WSign<SimdFloat> for SimdFloat {
     fn copy_sign_to(self, to: SimdFloat) -> SimdFloat {
-        self.simd_copysign(to)
+        to.simd_copysign(self)
     }
 }