feat: implement concave polylines intersection (#210)

* feat: implement non-convex polygons intersection

* fix: handle case where a polyline is fully contained for polyline_intersection

* feat: add example for point_in_poly2d

* chore: warning/clippy fixes

* fix no-std build

crates/parry2d-f64/Cargo.toml

@@ -19,7 +19,7 @@ maintenance = { status = "actively-developed" }
 [features]
 default = ["required-features", "std"]
 required-features = ["dim2", "f64"]
-std = ["nalgebra/std", "slab", "rustc-hash", "simba/std", "arrayvec/std", "spade"]
+std = ["nalgebra/std", "slab", "rustc-hash", "simba/std", "arrayvec/std", "spade", "thiserror"]
 dim2 = []
 f64 = []
 serde-serialize = ["serde", "nalgebra/serde-serialize", "arrayvec/serde", "bitflags/serde"]
@@ -60,6 +60,8 @@ spade = { version = "2", optional = true } # Make this optional?
 rayon = { version = "1", optional = true }
 bytemuck = { version = "1", features = ["derive"], optional = true }
 log = "0.4"
+ordered-float = { version = "4", default-features = false }
+thiserror = { version = "1", optional = true }
 
 [dev-dependencies]
 simba = { version = "0.8", default-features = false, features = ["partial_fixed_point_support"] }

crates/parry2d/Cargo.toml

@@ -19,7 +19,7 @@ maintenance = { status = "actively-developed" }
 [features]
 default = ["required-features", "std"]
 required-features = ["dim2", "f32"]
-std = ["nalgebra/std", "slab", "rustc-hash", "simba/std", "arrayvec/std", "spade"]
+std = ["nalgebra/std", "slab", "rustc-hash", "simba/std", "arrayvec/std", "spade", "thiserror"]
 dim2 = []
 f32 = []
 serde-serialize = ["serde", "nalgebra/serde-serialize", "arrayvec/serde", "bitflags/serde"]
@@ -59,10 +59,13 @@ cust_core = { version = "0.1", optional = true }
 spade = { version = "2", optional = true }
 rayon = { version = "1", optional = true }
 bytemuck = { version = "1", features = ["derive"], optional = true }
+ordered-float = { version = "4", default-features = false }
 log = "0.4"
+thiserror = { version = "1", optional = true }
 
 [dev-dependencies]
 simba = { version = "0.8", default-features = false, features = ["partial_fixed_point_support"] }
 oorandom = "11"
 ptree = "0.4.0"
 rand = { version = "0.8" }
+macroquad = "0.4"

crates/parry2d/examples/point_in_poly2d.rs

@@ -0,0 +1,108 @@
+use macroquad::prelude::*;
+use nalgebra::{Point2, UnitComplex, Vector2};
+use parry2d::utils::point_in_poly2d;
+
+const RENDER_SCALE: f32 = 30.0;
+
+#[macroquad::main("parry2d::utils::point_in_poly2d")]
+async fn main() {
+    let mut spikes = spikes_polygon();
+    let test_points = grid_points();
+
+    let animation_rotation = UnitComplex::new(0.02);
+    let spikes_render_pos = Point2::new(screen_width() / 2.0, screen_height() / 2.0);
+
+    loop {
+        clear_background(BLACK);
+
+        /*
+         * Compute polygon intersections.
+         */
+        spikes
+            .iter_mut()
+            .for_each(|pt| *pt = animation_rotation * *pt);
+        draw_polygon(&spikes, RENDER_SCALE, spikes_render_pos, BLUE);
+
+        for point in &test_points {
+            if point_in_poly2d(point, &spikes) {
+                draw_point(*point, RENDER_SCALE, spikes_render_pos, RED);
+            } else {
+                draw_point(*point, RENDER_SCALE, spikes_render_pos, GREEN);
+            }
+        }
+
+        next_frame().await
+    }
+}
+
+fn spikes_polygon() -> Vec<Point2<f32>> {
+    let teeths = 3;
+    let width = 15.0;
+    let height = 7.5;
+    let tooth_width = width / (teeths as f32);
+    let center = Vector2::new(width / 2.0, height / 2.0);
+
+    let mut polygon = vec![
+        Point2::new(width, 0.0) - center,
+        Point2::new(width, height) - center,
+        Point2::new(0.0, height) - center,
+    ];
+
+    for i in 0..teeths {
+        let x = i as f32 * tooth_width;
+        polygon.push(Point2::new(x, 0.0) - center);
+        polygon.push(Point2::new(x + tooth_width / 2.0, height * 1.5) - center);
+    }
+
+    polygon
+}
+
+fn grid_points() -> Vec<Point2<f32>> {
+    let count = 40;
+    let spacing = 0.6;
+    let mut pts = vec![];
+    for i in 0..count {
+        for j in 0..count {
+            pts.push(Point2::new(
+                (i as f32 - count as f32 / 2.0) * spacing,
+                (j as f32 - count as f32 / 2.0) * spacing,
+            ));
+        }
+    }
+    pts
+}
+
+fn draw_polygon(polygon: &[Point2<f32>], scale: f32, shift: Point2<f32>, color: Color) {
+    for i in 0..polygon.len() {
+        let a = polygon[i];
+        let b = polygon[(i + 1) % polygon.len()];
+        draw_line(
+            a.x * scale + shift.x,
+            a.y * scale + shift.y,
+            b.x * scale + shift.x,
+            b.y * scale + shift.y,
+            2.0,
+            color,
+        );
+    }
+}
+
+fn draw_point(point: Point2<f32>, scale: f32, shift: Point2<f32>, color: Color) {
+    let edge_len = 0.15;
+    draw_line(
+        (point.x - edge_len) * scale + shift.x,
+        point.y * scale + shift.y,
+        (point.x + edge_len) * scale + shift.x,
+        point.y * scale + shift.y,
+        2.0,
+        color,
+    );
+    draw_line(
+        point.x * scale + shift.x,
+        (point.y - edge_len) * scale + shift.y,
+        point.x * scale + shift.x,
+        (point.y + edge_len) * scale + shift.y,
+        2.0,
+        color,
+    );
+}

crates/parry2d/examples/polygons_intersection2d.rs

@@ -0,0 +1,128 @@
+use macroquad::prelude::*;
+use nalgebra::{Point2, UnitComplex, Vector2};
+use parry2d::shape::Ball;
+use parry2d::transformation::polygons_intersection_points;
+
+const RENDER_SCALE: f32 = 30.0;
+
+#[macroquad::main("parry2d::utils::polygons_intersection_points")]
+async fn main() {
+    let spikes = spikes_polygon();
+    let mut animated_spikes = spikes.clone();
+
+    let star = star_polygon();
+
+    let animation_scale = 2.0;
+    let animation_rotation = UnitComplex::new(0.008);
+
+    let spikes_render_pos = Point2::new(300.0, 300.0);
+    let star_render_pos = Point2::new(600.0, 300.0);
+
+    for i in 0.. {
+        clear_background(BLACK);
+
+        /*
+         *
+         * Compute the rotated/scaled polygons, and compute their intersection with the original
+         * polygon.
+         *
+         */
+        animated_spikes
+            .iter_mut()
+            .for_each(|pt| *pt = animation_rotation * *pt);
+        let spikes_intersections = polygons_intersection_points(&spikes, &animated_spikes);
+
+        let animated_star: Vec<_> = star
+            .iter()
+            .map(|pt| {
+                animation_rotation.powf(i as f32)
+                    * *pt
+                    * ((i as f32 / 100.0).sin().abs() * animation_scale)
+            })
+            .collect();
+
+        let star_intersections = polygons_intersection_points(&star, &animated_star);
+
+        /*
+         *
+         * Render the polygons and their intersections.
+         *
+         */
+        draw_polygon(&spikes, RENDER_SCALE, spikes_render_pos, BLUE);
+        draw_polygon(&animated_spikes, RENDER_SCALE, spikes_render_pos, GREEN);
+
+        draw_polygon(&star, RENDER_SCALE, star_render_pos, BLUE);
+        draw_polygon(&animated_star, RENDER_SCALE, star_render_pos, GREEN);
+
+        if let Ok(intersections) = spikes_intersections {
+            draw_text(
+                &format!("# spikes intersections: {}", intersections.len()),
+                0.0,
+                15.0,
+                20.0,
+                WHITE,
+            );
+            for intersection in intersections {
+                draw_polygon(&intersection, RENDER_SCALE, spikes_render_pos, RED);
+            }
+        }
+
+        if let Ok(intersections) = star_intersections {
+            draw_text(
+                &format!("# star intersections: {}", intersections.len()),
+                0.0,
+                30.0,
+                20.0,
+                WHITE,
+            );
+            for intersection in intersections {
+                draw_polygon(&intersection, RENDER_SCALE, star_render_pos, RED);
+            }
+        }
+
+        next_frame().await
+    }
+}
+
+fn star_polygon() -> Vec<Point2<f32>> {
+    let mut star = Ball::new(1.5).to_polyline(10);
+    star.iter_mut().step_by(2).for_each(|pt| *pt = *pt * 0.6);
+    star
+}
+
+fn spikes_polygon() -> Vec<Point2<f32>> {
+    let teeths = 5;
+    let width = 10.0;
+    let height = 5.0;
+    let tooth_width = width / (teeths as f32);
+    let center = Vector2::new(width / 2.0, height / 2.0);
+
+    let mut polygon = vec![
+        Point2::new(width, 0.0) - center,
+        Point2::new(width, height) - center,
+        Point2::new(0.0, height) - center,
+    ];
+
+    for i in 0..teeths {
+        let x = i as f32 * tooth_width;
+        polygon.push(Point2::new(x, 0.0) - center);
+        polygon.push(Point2::new(x + tooth_width / 2.0, height * 0.8) - center);
+    }
+
+    polygon
+}
+
+fn draw_polygon(polygon: &[Point2<f32>], scale: f32, shift: Point2<f32>, color: Color) {
+    for i in 0..polygon.len() {
+        let a = polygon[i];
+        let b = polygon[(i + 1) % polygon.len()];
+        draw_line(
+            a.x * scale + shift.x,
+            a.y * scale + shift.y,
+            b.x * scale + shift.x,
+            b.y * scale + shift.y,
+            2.0,
+            color,
+        );
+    }
+}

crates/parry3d-f64/Cargo.toml

@@ -19,7 +19,7 @@ maintenance = { status = "actively-developed" }
 [features]
 default = ["required-features", "std"]
 required-features = ["dim3", "f64"]
-std = ["nalgebra/std", "slab", "rustc-hash", "simba/std", "arrayvec/std", "spade"]
+std = ["nalgebra/std", "slab", "rustc-hash", "simba/std", "arrayvec/std", "spade", "thiserror"]
 dim3 = []
 f64 = []
 serde-serialize = ["serde", "nalgebra/serde-serialize", "bitflags/serde"]
@@ -53,13 +53,15 @@ approx = { version = "0.5", default-features = false }
 serde = { version = "1.0", optional = true, features = ["derive", "rc"] }
 rkyv = { version = "0.7.41", optional = true }
 num-derive = "0.4"
-indexmap = { version = "2", features = [ "serde" ], optional = true }
+indexmap = { version = "2", features = ["serde"], optional = true }
 rustc-hash = { version = "1", optional = true }
 cust_core = { version = "0.1", optional = true }
 spade = { version = "2", optional = true } # Make this optional?
 rayon = { version = "1", optional = true }
 bytemuck = { version = "1", features = ["derive"], optional = true }
 log = "0.4"
+ordered-float = { version = "4", default-features = false }
+thiserror = { version = "1", optional = true }
 
 [dev-dependencies]
 oorandom = "11"

crates/parry3d/Cargo.toml

@@ -19,7 +19,7 @@ maintenance = { status = "actively-developed" }
 [features]
 default = ["required-features", "std"]
 required-features = ["dim3", "f32"]
-std = ["nalgebra/std", "slab", "rustc-hash", "simba/std", "arrayvec/std", "spade"]
+std = ["nalgebra/std", "slab", "rustc-hash", "simba/std", "arrayvec/std", "spade", "thiserror"]
 dim3 = []
 f32 = []
 serde-serialize = ["serde", "nalgebra/serde-serialize", "bitflags/serde"]
@@ -54,13 +54,15 @@ approx = { version = "0.5", default-features = false }
 serde = { version = "1.0", optional = true, features = ["derive", "rc"] }
 rkyv = { version = "0.7.41", optional = true }
 num-derive = "0.4"
-indexmap = { version = "2", features = [ "serde" ], optional = true }
+indexmap = { version = "2", features = ["serde"], optional = true }
 rustc-hash = { version = "1", optional = true }
 cust_core = { version = "0.1", optional = true }
 spade = { version = "2", optional = true } # Make this optional?
 rayon = { version = "1", optional = true }
 bytemuck = { version = "1", features = ["derive"], optional = true }
 log = "0.4"
+ordered-float = { version = "4", default-features = false }
+thiserror = { version = "1", optional = true }
 
 [dev-dependencies]
 oorandom = "11"

crates/parry3d/benches/query/ray.rs

@@ -13,7 +13,7 @@ use test::Bencher;
 #[macro_use]
 mod macros;
 
-// FIXME: will the randomness of `solid` and `max_time_of_impact` affect too much the benchmark?
+// TODO: will the randomness of `solid` and `max_time_of_impact` affect too much the benchmark?
 bench_method!(
     bench_ray_against_ball,
     cast_ray,

src/bounding_volume/aabb_utils.rs

@@ -16,7 +16,7 @@ where
     let mut basis = na::zero::<Vector<Real>>();
 
     for d in 0..DIM {
-        // FIXME: this could be further improved iterating on `m`'s columns, and passing
+        // TODO: this could be further improved iterating on `m`'s columns, and passing
         // Id as the transformation matrix.
         basis[d] = 1.0;
         max[d] = i.support_point(m, &basis)[d];
@@ -40,7 +40,7 @@ where
     let mut basis = na::zero::<Vector<Real>>();
 
     for d in 0..DIM {
-        // FIXME: this could be further improved iterating on `m`'s columns, and passing
+        // TODO: this could be further improved iterating on `m`'s columns, and passing
         // Id as the transformation matrix.
         basis[d] = 1.0;
         max[d] = i.local_support_point(&basis)[d];

src/bounding_volume/bounding_sphere.rs

@@ -56,7 +56,7 @@ impl BoundingVolume for BoundingSphere {
 
     #[inline]
     fn intersects(&self, other: &BoundingSphere) -> bool {
-        // FIXME: refactor that with the code from narrow_phase::ball_ball::collide(...) ?
+        // TODO: refactor that with the code from narrow_phase::ball_ball::collide(...) ?
         let delta_pos = other.center - self.center;
         let distance_squared = delta_pos.norm_squared();
         let sum_radius = self.radius + other.radius;

src/bounding_volume/bounding_sphere_utils.rs

@@ -3,7 +3,7 @@ use crate::utils;
 use na::{self, ComplexField};
 
 /// Computes the bounding sphere of a set of point, given its center.
-// FIXME: return a bounding sphere?
+// TODO: return a bounding sphere?
 #[inline]
 pub fn point_cloud_bounding_sphere_with_center(
     pts: &[Point<Real>],
@@ -23,7 +23,7 @@ pub fn point_cloud_bounding_sphere_with_center(
 }
 
 /// Computes a bounding sphere of the specified set of point.
-// FIXME: return a bounding sphere?
+// TODO: return a bounding sphere?
 #[inline]
 pub fn point_cloud_bounding_sphere(pts: &[Point<Real>]) -> (Point<Real>, Real) {
     point_cloud_bounding_sphere_with_center(pts, utils::center(pts))

src/bounding_volume/bounding_volume.rs

@@ -6,7 +6,7 @@ use crate::math::{Point, Real};
 /// intersection, inclusion test. Two bounding volume must also be mergeable into a bigger bounding
 /// volume.
 pub trait BoundingVolume {
-    // FIXME: keep that ? What about non-spacial bounding volumes (e.g. bounding cones, curvature
+    // TODO: keep that ? What about non-spacial bounding volumes (e.g. bounding cones, curvature
     // bounds, etc.) ?
     /// Returns a point inside of this bounding volume. This is ideally its center.
     fn center(&self) -> Point<Real>;