From fa18d6a5980fd81558e67314751846063fcd36f4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= <sebcrozet@dimforge.com>
Date: Wed, 4 Dec 2024 11:24:23 +0100
Subject: [PATCH] Fix point_in_poly2d for self-intersecting polygons

---
 crates/parry2d/examples/point_in_poly2d.rs | 37 ++++++++++++++++++----
 src/utils/point_in_poly2d.rs               | 21 ++++++++++--
 2 files changed, 49 insertions(+), 9 deletions(-)

diff --git a/crates/parry2d/examples/point_in_poly2d.rs b/crates/parry2d/examples/point_in_poly2d.rs
index 99f28770..6abc37a8 100644
--- a/crates/parry2d/examples/point_in_poly2d.rs
+++ b/crates/parry2d/examples/point_in_poly2d.rs
@@ -10,27 +10,35 @@ const RENDER_SCALE: f32 = 30.0;
 #[macroquad::main("parry2d::utils::point_in_poly2d")]
 async fn main() {
     let mut spikes = spikes_polygon();
+    let mut squares = squares_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);
+    let polygon_render_pos = Point2::new(screen_width() / 2.0, screen_height() / 2.0);
+
+    for i in 0.. {
+        let polygon = if (i / 350) % 2 == 0 {
+            &mut spikes
+        } else {
+            &mut squares
+        };
 
-    loop {
         clear_background(BLACK);
 
-        spikes
+        polygon
             .iter_mut()
             .for_each(|pt| *pt = animation_rotation * *pt);
-        draw_polygon(&spikes, RENDER_SCALE, spikes_render_pos, BLUE);
+
+        draw_polygon(&polygon, RENDER_SCALE, polygon_render_pos, BLUE);
 
         /*
          * Compute polygon intersections.
          */
         for point in &test_points {
-            if point_in_poly2d(point, &spikes) {
-                draw_point(*point, RENDER_SCALE, spikes_render_pos, RED);
+            if point_in_poly2d(point, &polygon) {
+                draw_point(*point, RENDER_SCALE, polygon_render_pos, RED);
             } else {
-                draw_point(*point, RENDER_SCALE, spikes_render_pos, GREEN);
+                draw_point(*point, RENDER_SCALE, polygon_render_pos, GREEN);
             }
         }
 
@@ -60,6 +68,21 @@ fn spikes_polygon() -> Vec<Point2<f32>> {
     polygon
 }
 
+fn squares_polygon() -> Vec<Point2<f32>> {
+    let scale = 3.0;
+    [
+        Point2::new(-1.0, -1.0) * scale,
+        Point2::new(0.0, -1.0) * scale,
+        Point2::new(0.0, 1.0) * scale,
+        Point2::new(-2.0, 1.0) * scale,
+        Point2::new(-2.0, -2.0) * scale,
+        Point2::new(1.0, -2.0) * scale,
+        Point2::new(1.0, 2.0) * scale,
+        Point2::new(-1.0, 2.0) * scale,
+    ]
+    .to_vec()
+}
+
 fn grid_points() -> Vec<Point2<f32>> {
     let count = 40;
     let spacing = 0.6;
diff --git a/src/utils/point_in_poly2d.rs b/src/utils/point_in_poly2d.rs
index c57f77e4..97e70bb4 100644
--- a/src/utils/point_in_poly2d.rs
+++ b/src/utils/point_in_poly2d.rs
@@ -50,18 +50,35 @@ pub fn point_in_poly2d(pt: &Point2<Real>, poly: &[Point2<Real>]) -> bool {
         let perp = dpt.perp(&seg_dir);
         winding += match (dpt.y >= 0.0, b.y > pt.y) {
             (true, true) if perp < 0.0 => 1,
-            (false, false) if perp > 0.0 => -1,
+            (false, false) if perp > 0.0 => 1,
             _ => 0,
         };
     }
 
-    winding != 0
+    winding % 2 == 1
 }
 
 #[cfg(test)]
 mod tests {
     use super::*;
 
+    #[test]
+    fn point_in_poly2d_self_intersecting() {
+        let poly = [
+            [-1.0, -1.0],
+            [0.0, -1.0],
+            [0.0, 1.0],
+            [-2.0, 1.0],
+            [-2.0, -2.0],
+            [1.0, -2.0],
+            [1.0, 2.0],
+            [-1.0, 2.0],
+        ]
+        .map(Point2::from);
+        assert!(!point_in_poly2d(&[-0.5, -0.5].into(), &poly));
+        assert!(point_in_poly2d(&[0.5, -0.5].into(), &poly));
+    }
+
     #[test]
     fn point_in_poly2d_concave() {
         let poly = [