diff --git a/build/ncollide2d/Cargo.toml b/build/ncollide2d/Cargo.toml index 096bb6593..a81965987 100644 --- a/build/ncollide2d/Cargo.toml +++ b/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" ] } \ No newline at end of file +simba = { version = "0.1", features = [ "partial_fixed_point_support" ] } +gnuplot = "0.0.37" diff --git a/build/ncollide2d/examples/concave.rs b/build/ncollide2d/examples/concave.rs new file mode 100644 index 000000000..4cee38b18 --- /dev/null +++ b/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, 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, 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) { + for (_isometry, triangle) in polygon.shapes() { + let triangle = triangle.as_shape::>().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")], + ); +} diff --git a/src/shape/compound.rs b/src/shape/compound.rs index 7994d55c7..a5a7b94d3 100644 --- a/src/shape/compound.rs +++ b/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 Compound { 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, hull: &[(N, N)]) -> Compound + 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 Compound {