Add a function to easily create a concave polygon from a concave hull

build/ncollide2d/Cargo.toml

@@ -39,7 +39,9 @@ simba           = "0.1"
 nalgebra        = "0.21"
 approx          = { version = "0.3", default-features = false }
 serde           = { version = "1.0", optional = true, features = ["derive"]}
+spade           = "1.8.2"
 
 [dev-dependencies]
 rand  = { version = "0.7", default-features = false }
-simba = { version = "0.1", features = [ "partial_fixed_point_support" ] }
+simba = { version = "0.1", features = [ "partial_fixed_point_support" ] }
+gnuplot         = "0.0.37"

build/ncollide2d/examples/concave.rs

@@ -0,0 +1,128 @@
+use gnuplot::*;
+use ncollide2d::math::Point;
+use ncollide2d::nalgebra::geometry::Isometry;
+use ncollide2d::query::PointQuery;
+use ncollide2d::shape::{Compound, ConvexPolygon};
+
+fn main() {
+    let hull = [
+        (5., 1.),
+        (4., 0.),
+        (4.5, 1.),
+        (3.5, 1.5),
+        (3., 0.),
+        (2., 1.),
+        (1., 0.),
+        (0., 1.),
+        (0.5, 1.5),
+        (0., 2.),
+        (1., 3.),
+        (2., 3.),
+        (4.25, 3.25),
+        (4.25, 2.25),
+        (4., 3.),
+        (4., 2.),
+        (5., 2.),
+        (5., 1.),
+    ];
+
+    let polygon = Compound::new_concave_polygon(Isometry::identity(), &hull);
+
+    let mut fg = Figure::new();
+    let mut axe = fg.axes2d();
+
+    display_polygon(&mut axe, &polygon);
+    display_hull(&mut axe, &hull);
+
+    // Points inside the concave hull
+    must_be_inside(&mut axe, &polygon, &[(2., 2.)]);
+
+    // Points at the edge of the hull
+    must_be_inside(&mut axe, &polygon, &[(5., 1.), (2., 1.)]);
+
+    // Points outside the convex hull
+    must_be_outside(&mut axe, &polygon, &[(-1., -1.), (3., 3.2), (4.5, 3.)]);
+
+    // Points outside the concave hull, and the convex hull
+    must_be_outside(
+        &mut axe,
+        &polygon,
+        &[
+            (0.25, 0.25),
+            (0.25, 1.5),
+            (2., 0.5),
+            (3.5, 0.5),
+            (3.5, 1.),
+            (3.5, 1.25),
+            (4.25, 0.75),
+            (4.5, 2.5),
+            (4.25, 2.1),
+            (4.1, 2.3),
+        ],
+    );
+
+    // Points inside the concave hull
+    must_be_inside(
+        &mut axe,
+        &polygon,
+        &[
+            (4.5, 0.75),
+            (2.75, 0.75),
+            (1., 0.75),
+            (4.75, 1.75),
+            (4.1, 2.75),
+            (4.1, 3.1),
+            (3.5, 3.1),
+        ],
+    );
+
+    fg.show().unwrap();
+}
+
+/// Asserts that all points are inside the polygon, and displays them in green
+fn must_be_inside(axe: &mut gnuplot::Axes2D, polygon: &Compound<f64>, points: &[(f64, f64)]) {
+    axe.points(
+        points.iter().map(|(x, _y)| *x),
+        points.iter().map(|(_x, y)| *y),
+        &[Color("green")],
+    );
+    for &(x, y) in points {
+        assert!(polygon.contains_point(&Isometry::identity(), &Point::new(x, y)));
+    }
+}
+
+/// Asserts that all points are inside the polygon, and displays them in red
+fn must_be_outside(axe: &mut gnuplot::Axes2D, polygon: &Compound<f64>, points: &[(f64, f64)]) {
+    axe.points(
+        points.iter().map(|(x, _y)| *x),
+        points.iter().map(|(_x, y)| *y),
+        &[Color("red")],
+    );
+    for &(x, y) in points {
+        assert!(!polygon.contains_point(&Isometry::identity(), &Point::new(x, y)));
+    }
+}
+
+/// Displays all the triangles contained inside the concave polygon in orange
+fn display_polygon(axe: &mut gnuplot::Axes2D, polygon: &Compound<f64>) {
+    for (_isometry, triangle) in polygon.shapes() {
+        let triangle = triangle.as_shape::<ConvexPolygon<f64>>().unwrap();
+        let p = triangle.points();
+        assert!(p.len() == 3);
+        let points = [p[0], p[1], p[2], p[0]];
+        axe.lines(
+            points.iter().map(|point| point.iter().nth(0).unwrap()),
+            points.iter().map(|point| point.iter().nth(1).unwrap()),
+            &[Color("orange"), LineWidth(5.)],
+        );
+    }
+}
+
+/// Displays the hull in black
+fn display_hull(axe: &mut gnuplot::Axes2D, hull: &[(f64, f64)]) {
+    axe.lines(
+        hull.iter().map(|(x, _y)| *x),
+        hull.iter().map(|(_x, y)| *y),
+        &[Color("black")],
+    );
+}

src/shape/compound.rs

@@ -3,11 +3,12 @@
 //!
 
 use crate::bounding_volume::{BoundingVolume, AABB};
-use crate::math::Isometry;
+use crate::math::{Isometry, Point};
 use crate::partitioning::{BVHImpl, BVT};
 use crate::query::{Contact, ContactKinematic, ContactPrediction, ContactPreprocessor};
-use crate::shape::{CompositeShape, FeatureId, Shape, ShapeHandle};
+use crate::shape::{CompositeShape, ConvexPolygon, FeatureId, Shape, ShapeHandle};
 use na::{self, RealField};
+use spade::delaunay::FloatDelaunayTriangulation;
 use std::mem;
 
 /// A compound shape with an aabb bounding volume.
@@ -51,6 +52,58 @@ impl<N: RealField> Compound<N> {
             nbits,
         }
     }
+
+    /// Creates a new 2D concave polygon from a set of points assumed to describe a
+    /// counter-clockwise convex polyline.
+    pub fn new_concave_polygon(isometry: Isometry<N>, hull: &[(N, N)]) -> Compound<N>
+    where
+        N: spade::SpadeFloat,
+    {
+        assert!(hull.len() >= 3, "A polygon must have at least 3 vertex");
+        assert!(
+            hull.first() == hull.last(),
+            "The hull must be closed (the first and last vertex be the same)"
+        );
+
+        let mut delaunay = FloatDelaunayTriangulation::with_walk_locate();
+
+        // Add each vertex of the hull one by one. An index (accessible with the `fix()` method is
+        // associated with each vertex. This index is strictly increasing.
+        for (x, y) in hull {
+            let _ = delaunay.insert([*x, *y]);
+        }
+
+        Compound::new(
+            delaunay
+                .triangles()
+                .filter_map(|face| {
+                    let indexes = {
+                        let vertex_handle = face.as_triangle();
+                        let mut indexes = [0; 3];
+                        for i in 0..3 {
+                            indexes[i] = vertex_handle[i].fix();
+                        }
+                        indexes
+                    };
+
+                    // The index of triangle are clockwise, and since the hull is counter clockwise
+                    // vertices inside the hull will have their index in decreasing order.
+                    let [a, b, c] = indexes;
+                    if (a > b && b > c) || (c > a && a > b) || (b > c && c > a) {
+                        let points: Vec<_> = indexes
+                            .iter()
+                            .map(|i| hull[*i])
+                            .map(|(x, y)| Point::new(x, y))
+                            .collect();
+                        Some(ConvexPolygon::try_from_points(&points).unwrap())
+                    } else {
+                        None
+                    }
+                })
+                .map(|triangle| (isometry, ShapeHandle::new(triangle)))
+                .collect(),
+        )
+    }
 }
 
 impl<N: RealField> Compound<N> {