Add convex polygons support.

8 files changed, 208 insertions, 21 deletions

diff --git a/benchmarks2d/all_benchmarks2.rs b/benchmarks2d/all_benchmarks2.rs
index 9a7a607..e5f2ff1 100644
--- a/benchmarks2d/all_benchmarks2.rs
+++ b/benchmarks2d/all_benchmarks2.rs
@@ -13,6 +13,7 @@ use std::cmp::Ordering;
 mod balls2;
 mod boxes2;
 mod capsules2;
+mod convex_polygons2;
 mod heightfield2;
 mod joint_ball2;
 mod joint_fixed2;
@@ -55,6 +56,7 @@ pub fn main() {
         ("Balls", balls2::init_world),
         ("Boxes", boxes2::init_world),
         ("Capsules", capsules2::init_world),
+        ("Convex polygons", convex_polygons2::init_world),
         ("Heightfield", heightfield2::init_world),
         ("Pyramid", pyramid2::init_world),
         ("(Stress test) joint ball", joint_ball2::init_world),
diff --git a/benchmarks2d/convex_polygons2.rs b/benchmarks2d/convex_polygons2.rs
new file mode 100644
index 0000000..f5a6da9
--- /dev/null
+++ b/benchmarks2d/convex_polygons2.rs
@@ -0,0 +1,86 @@
+use na::Point2;
+use rand::distributions::{Distribution, Standard};
+use rand::{rngs::StdRng, SeedableRng};
+use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
+use rapier2d::geometry::{ColliderBuilder, ColliderSet};
+use rapier_testbed2d::Testbed;
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let joints = JointSet::new();
+
+    /*
+     * Ground
+     */
+    let ground_size = 30.0;
+
+    let rigid_body = RigidBodyBuilder::new_static().build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size, 1.2).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .rotation(std::f32::consts::FRAC_PI_2)
+        .translation(ground_size, ground_size * 2.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size * 2.0, 1.2).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    let rigid_body = RigidBodyBuilder::new_static()
+        .rotation(std::f32::consts::FRAC_PI_2)
+        .translation(-ground_size, ground_size * 2.0)
+        .build();
+    let handle = bodies.insert(rigid_body);
+    let collider = ColliderBuilder::cuboid(ground_size * 2.0, 1.2).build();
+    colliders.insert(collider, handle, &mut bodies);
+
+    /*
+     * Create the convex polygons
+     */
+    let num = 26;
+    let scale = 2.0;
+    let border_rad = 0.0;
+
+    let shift = border_rad * 2.0 + scale;
+    let centerx = shift * (num as f32) / 2.0;
+    let centery = shift / 2.0;
+
+    let mut rng = StdRng::seed_from_u64(0);
+    let distribution = Standard;
+
+    for i in 0..num {
+        for j in 0usize..num * 5 {
+            let x = i as f32 * shift - centerx;
+            let y = j as f32 * shift * 2.0 + centery + 2.0;
+
+            let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build();
+            let handle = bodies.insert(rigid_body);
+
+            let mut points = Vec::new();
+
+            for _ in 0..10 {
+                let pt: Point2<f32> = distribution.sample(&mut rng);
+                points.push(pt * scale);
+            }
+
+            let collider = ColliderBuilder::convex_hull(&points).unwrap().build();
+            colliders.insert(collider, handle, &mut bodies);
+        }
+    }
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, joints);
+    testbed.look_at(Point2::new(0.0, 50.0), 10.0);
+}
+
+fn main() {
+    let testbed = Testbed::from_builders(0, vec![("Balls", init_world)]);
+    testbed.run()
+}
diff --git a/examples2d/sensor2.rs b/examples2d/sensor2.rs
index 959ecbf..6649a43 100644
--- a/examples2d/sensor2.rs
+++ b/examples2d/sensor2.rs
@@ -1,6 +1,6 @@
 use na::{Point2, Point3};
 use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet};
-use rapier2d::geometry::{ColliderBuilder, ColliderSet, Proximity};
+use rapier2d::geometry::{ColliderBuilder, ColliderSet};
 use rapier_testbed2d::Testbed;
 
 pub fn init_world(testbed: &mut Testbed) {
@@ -70,10 +70,11 @@ pub fn init_world(testbed: &mut Testbed) {
 
     // Callback that will be executed on the main loop to handle proximities.
     testbed.add_callback(move |_, physics, events, graphics, _| {
-        while let Ok(prox) = events.proximity_events.try_recv() {
-            let color = match prox.new_status {
-                Proximity::WithinMargin | Proximity::Intersecting => Point3::new(1.0, 1.0, 0.0),
-                Proximity::Disjoint => Point3::new(0.5, 0.5, 1.0),
+        while let Ok(prox) = events.intersection_events.try_recv() {
+            let color = if prox.intersecting {
+                Point3::new(1.0, 1.0, 0.0)
+            } else {
+                Point3::new(0.5, 0.5, 1.0)
             };
 
             let parent_handle1 = physics.colliders.get(prox.collider1).unwrap().parent();
diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs
index 71f0676..f2ba74b 100644
--- a/src/geometry/collider.rs
+++ b/src/geometry/collider.rs
@@ -3,13 +3,15 @@ use crate::geometry::InteractionGroups;
 use crate::math::{AngVector, Isometry, Point, Rotation, Vector};
 use cdl::bounding_volume::AABB;
 use cdl::shape::{
-    Ball, Capsule, Cuboid, HalfSpace, HeightField, RoundCuboid, RoundTriangle, Segment, Shape,
-    ShapeType, ShapeWithBorder, TriMesh, Triangle,
+    Ball, Capsule, Cuboid, HalfSpace, HeightField, RoundCuboid, RoundShape, RoundTriangle, Segment,
+    Shape, ShapeType, TriMesh, Triangle,
 };
 #[cfg(feature = "dim3")]
 use cdl::shape::{
     Cone, ConvexPolyhedron, Cylinder, RoundCone, RoundConvexPolyhedron, RoundCylinder,
 };
+#[cfg(feature = "dim2")]
+use cdl::shape::{ConvexPolygon, RoundConvexPolygon};
 use na::Point3;
 use std::ops::Deref;
 use std::sync::Arc;
@@ -44,7 +46,7 @@ impl ColliderShape {
     /// 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(ShapeWithBorder {
+        ColliderShape(Arc::new(RoundShape {
             base_shape: Cylinder::new(half_height, radius),
             border_radius,
         }))
@@ -55,7 +57,7 @@ impl ColliderShape {
     /// radius of the sphere used for dilating the cylinder).
     #[cfg(feature = "dim3")]
     pub fn round_cone(half_height: f32, radius: f32, border_radius: f32) -> Self {
-        ColliderShape(Arc::new(ShapeWithBorder {
+        ColliderShape(Arc::new(RoundShape {
             base_shape: Cone::new(half_height, radius),
             border_radius,
         }))
@@ -73,6 +75,14 @@ impl ColliderShape {
         ColliderShape(Arc::new(Cuboid::new(half_extents)))
     }
 
+    /// Initialize a round cuboid shape defined by its half-extents and border radius.
+    pub fn round_cuboid(half_extents: Vector<f32>, border_radius: f32) -> Self {
+        ColliderShape(Arc::new(RoundShape {
+            base_shape: Cuboid::new(half_extents),
+            border_radius,
+        }))
+    }
+
     /// 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)))
@@ -93,9 +103,16 @@ impl ColliderShape {
         ColliderShape(Arc::new(TriMesh::new(vertices, indices)))
     }
 
-    #[cfg(feature = "dim3")]
     pub fn convex_hull(points: &[Point<f32>]) -> Option<Self> {
-        ConvexPolyhedron::from_convex_hull(points).map(|ch| ColliderShape(Arc::new(ch)))
+        #[cfg(feature = "dim2")]
+        return ConvexPolygon::from_convex_hull(points).map(|ch| ColliderShape(Arc::new(ch)));
+        #[cfg(feature = "dim3")]
+        return ConvexPolyhedron::from_convex_hull(points).map(|ch| ColliderShape(Arc::new(ch)));
+    }
+
+    #[cfg(feature = "dim2")]
+    pub fn convex_polyline(points: Vec<Point<f32>>) -> Option<Self> {
+        ConvexPolygon::from_convex_polyline(points).map(|ch| ColliderShape(Arc::new(ch)))
     }
 
     #[cfg(feature = "dim3")]
@@ -103,10 +120,27 @@ impl ColliderShape {
         ConvexPolyhedron::from_convex_mesh(points, indices).map(|ch| ColliderShape(Arc::new(ch)))
     }
 
-    #[cfg(feature = "dim3")]
     pub fn round_convex_hull(points: &[Point<f32>], border_radius: f32) -> Option<Self> {
-        ConvexPolyhedron::from_convex_hull(points).map(|ch| {
-            ColliderShape(Arc::new(ShapeWithBorder {
+        #[cfg(feature = "dim2")]
+        return ConvexPolygon::from_convex_hull(points).map(|ch| {
+            ColliderShape(Arc::new(RoundShape {
+                base_shape: ch,
+                border_radius,
+            }))
+        });
+        #[cfg(feature = "dim3")]
+        return ConvexPolyhedron::from_convex_hull(points).map(|ch| {
+            ColliderShape(Arc::new(RoundShape {
+                base_shape: ch,
+                border_radius,
+            }))
+        });
+    }
+
+    #[cfg(feature = "dim2")]
+    pub fn round_convex_polyline(points: Vec<Point<f32>>, border_radius: f32) -> Option<Self> {
+        ConvexPolygon::from_convex_polyline(points).map(|ch| {
+            ColliderShape(Arc::new(RoundShape {
                 base_shape: ch,
                 border_radius,
             }))
@@ -120,7 +154,7 @@ impl ColliderShape {
         border_radius: f32,
     ) -> Option<Self> {
         ConvexPolyhedron::from_convex_mesh(points, indices).map(|ch| {
-            ColliderShape(Arc::new(ShapeWithBorder {
+            ColliderShape(Arc::new(RoundShape {
                 base_shape: ch,
                 border_radius,
             }))
@@ -217,6 +251,12 @@ impl<'de> serde::Deserialize<'de> for ColliderShape {
                     Some(ShapeType::HalfSpace) => deser::<A, HalfSpace>(&mut seq)?,
                     Some(ShapeType::RoundCuboid) => deser::<A, RoundCuboid>(&mut seq)?,
                     Some(ShapeType::RoundTriangle) => deser::<A, RoundTriangle>(&mut seq)?,
+                    #[cfg(feature = "dim2")]
+                    Some(ShapeType::ConvexPolygon) => deser::<A, ConvexPolygon>(&mut seq)?,
+                    #[cfg(feature = "dim2")]
+                    Some(ShapeType::RoundConvexPolygon) => {
+                        deser::<A, RoundConvexPolygon>(&mut seq)?
+                    }
                     #[cfg(feature = "dim3")]
                     Some(ShapeType::Cylinder) => deser::<A, Cylinder>(&mut seq)?,
                     #[cfg(feature = "dim3")]
@@ -439,6 +479,16 @@ impl ColliderBuilder {
         Self::new(ColliderShape::cuboid(Vector::new(hx, hy)))
     }
 
+    /// Initialize a new collider builder with a round cuboid shape defined by its half-extents
+    /// and border radius.
+    #[cfg(feature = "dim2")]
+    pub fn round_cuboid(hx: f32, hy: f32, border_radius: f32) -> Self {
+        Self::new(ColliderShape::round_cuboid(
+            Vector::new(hx, hy),
+            border_radius,
+        ))
+    }
+
     /// Initialize a new collider builder with a capsule shape aligned with the `x` axis.
     pub fn capsule_x(half_height: f32, radius: f32) -> Self {
         let p = Point::from(Vector::x() * half_height);
@@ -464,6 +514,16 @@ impl ColliderBuilder {
         Self::new(ColliderShape::cuboid(Vector::new(hx, hy, hz)))
     }
 
+    /// Initialize a new collider builder with a round cuboid shape defined by its half-extents
+    /// and border radius.
+    #[cfg(feature = "dim3")]
+    pub fn round_cuboid(hx: f32, hy: f32, hz: f32, border_radius: f32) -> Self {
+        Self::new(ColliderShape::round_cuboid(
+            Vector::new(hx, hy, hz),
+            border_radius,
+        ))
+    }
+
     /// Initializes a collider builder with a segment shape.
     pub fn segment(a: Point<f32>, b: Point<f32>) -> Self {
         Self::new(ColliderShape::segment(a, b))
@@ -479,16 +539,24 @@ impl ColliderBuilder {
         Self::new(ColliderShape::trimesh(vertices, indices))
     }
 
-    #[cfg(feature = "dim3")]
     pub fn convex_hull(points: &[Point<f32>]) -> Option<Self> {
         ColliderShape::convex_hull(points).map(|cp| Self::new(cp))
     }
 
-    #[cfg(feature = "dim3")]
     pub fn round_convex_hull(points: &[Point<f32>], border_radius: f32) -> Option<Self> {
         ColliderShape::round_convex_hull(points, border_radius).map(|cp| Self::new(cp))
     }
 
+    #[cfg(feature = "dim2")]
+    pub fn convex_polyline(points: Vec<Point<f32>>) -> Option<Self> {
+        ColliderShape::convex_polyline(points).map(|cp| Self::new(cp))
+    }
+
+    #[cfg(feature = "dim2")]
+    pub fn round_convex_polyline(points: Vec<Point<f32>>, border_radius: f32) -> Option<Self> {
+        ColliderShape::round_convex_polyline(points, border_radius).map(|cp| Self::new(cp))
+    }
+
     #[cfg(feature = "dim3")]
     pub fn convex_mesh(points: Vec<Point<f32>>, indices: &[usize]) -> Option<Self> {
         ColliderShape::convex_mesh(points, indices).map(|cp| Self::new(cp))
diff --git a/src_testbed/box2d_backend.rs b/src_testbed/box2d_backend.rs
index f448a6f..2d4ef29 100644
--- a/src_testbed/box2d_backend.rs
+++ b/src_testbed/box2d_backend.rs
@@ -174,6 +174,14 @@ impl Box2dWorld {
             b2_shape.set_radius(b.radius);
             b2_shape.set_position(center);
             body.create_fixture(&b2_shape, &mut fixture_def);
+        } else if let Some(p) = shape.as_convex_polygon() {
+            let vertices: Vec<_> = p
+                .points()
+                .iter()
+                .map(|p| na_vec_to_b2_vec(p.coords))
+                .collect();
+            let b2_shape = b2::PolygonShape::new_with(&vertices);
+            body.create_fixture(&b2_shape, &mut fixture_def);
         } else if let Some(c) = shape.as_cuboid() {
             let b2_shape = b2::PolygonShape::new_box(c.half_extents.x, c.half_extents.y);
             body.create_fixture(&b2_shape, &mut fixture_def);
diff --git a/src_testbed/engine.rs b/src_testbed/engine.rs
index b1ecc8b..a1fd74e 100644
--- a/src_testbed/engine.rs
+++ b/src_testbed/engine.rs
@@ -344,6 +344,15 @@ impl GraphicsManager {
             )))
         }
 
+        #[cfg(feature = "dim2")]
+        if let Some(convex_polygon) = shape
+            .as_convex_polygon()
+            .or(shape.as_round_convex_polygon().map(|r| &r.base_shape))
+        {
+            let vertices = convex_polygon.points().to_vec();
+            out.push(Node::Convex(Convex::new(handle, vertices, color, window)))
+        }
+
         #[cfg(feature = "dim3")]
         if let Some(convex_polyhedron) = shape
             .as_convex_polyhedron()
diff --git a/src_testbed/nphysics_backend.rs b/src_testbed/nphysics_backend.rs
index 4384480..224b5d3 100644
--- a/src_testbed/nphysics_backend.rs
+++ b/src_testbed/nphysics_backend.rs
@@ -1,3 +1,5 @@
+#[cfg(feature = "dim2")]
+use ncollide::shape::ConvexPolygon;
 use ncollide::shape::{Ball, Capsule, Cuboid, HeightField, ShapeHandle};
 use nphysics::force_generator::DefaultForceGeneratorSet;
 use nphysics::joint::{
@@ -144,6 +146,7 @@ impl NPhysicsWorld {
         self.mechanical_world
             .integration_parameters
             .set_dt(params.dt());
+        self.mechanical_world.integration_parameters.warmstart_coeff = params.warmstart_coeff;
 
         counters.step_started();
         self.mechanical_world.step(
@@ -175,12 +178,15 @@ fn nphysics_collider_from_rapier_collider(
     collider: &Collider,
     is_dynamic: bool,
 ) -> Option<ColliderDesc<f32>> {
-    let margin = ColliderDesc::<f32>::default_margin();
+    let mut margin = ColliderDesc::<f32>::default_margin();
     let mut pos = *collider.position_wrt_parent();
     let shape = collider.shape();
 
     let shape = if let Some(cuboid) = shape.as_cuboid() {
         ShapeHandle::new(Cuboid::new(cuboid.half_extents.map(|e| e - margin)))
+    } else if let Some(cuboid) = shape.as_round_cuboid() {
+        margin = cuboid.border_radius;
+        ShapeHandle::new(Cuboid::new(cuboid.base_shape.half_extents))
     } else if let Some(ball) = shape.as_ball() {
         ShapeHandle::new(Ball::new(ball.radius - margin))
     } else if let Some(capsule) = shape.as_capsule() {
@@ -208,7 +214,12 @@ fn nphysics_collider_from_rapier_collider(
         }
 
         #[cfg(feature = "dim2")]
-        {
+        if let Some(polygon) = shape.as_round_convex_polygon() {
+            margin = polygon.border_radius;
+            ShapeHandle::new(ConvexPolygon::try_from_points(polygon.base_shape.points()).unwrap())
+        } else if let Some(polygon) = shape.as_convex_polygon() {
+            ShapeHandle::new(ConvexPolygon::try_from_points(polygon.points()).unwrap())
+        } else {
             return None;
         }
     };
@@ -219,6 +230,7 @@ fn nphysics_collider_from_rapier_collider(
         ColliderDesc::new(shape)
             .position(pos)
             .density(density)
-            .sensor(collider.is_sensor()),
+            .sensor(collider.is_sensor())
+            .margin(margin),
     )
 }
diff --git a/src_testbed/objects/heightfield.rs b/src_testbed/objects/heightfield.rs
index dbf14ad..fed168d 100644
--- a/src_testbed/objects/heightfield.rs
+++ b/src_testbed/objects/heightfield.rs
@@ -10,7 +10,6 @@ use rapier::math::Point;
 #[cfg(feature = "dim3")]
 use rapier::math::Vector;
 use std::cell::RefCell;
-use std::rc::Rc;
 
 pub struct HeightField {
     color: Point3<f32>,
@@ -52,6 +51,8 @@ impl HeightField {
         color: Point3<f32>,
         window: &mut Window,
     ) -> HeightField {
+        use std::rc::Rc;
+
         let (vertices, indices) = heightfield.to_trimesh();
         let indices = indices.into_iter().map(|i| na::convert(i)).collect();
         let mesh = Mesh::new(vertices, indices, None, None, false);