parry3d/utils/
point_in_poly2d.rs

1use crate::math::Real;
2use na::Point2;
3use num::Zero;
4
5/// Tests if the given point is inside a convex polygon with arbitrary orientation.
6///
7/// The polygon is assumed to be closed, i.e., first and last point of the polygon are implicitly
8/// assumed to be connected by an edge.
9pub fn point_in_convex_poly2d(pt: &Point2<Real>, poly: &[Point2<Real>]) -> bool {
10    if poly.is_empty() {
11        false
12    } else {
13        let mut sign = 0.0;
14
15        for i1 in 0..poly.len() {
16            let i2 = (i1 + 1) % poly.len();
17            let seg_dir = poly[i2] - poly[i1];
18            let dpt = pt - poly[i1];
19            let perp = dpt.perp(&seg_dir);
20
21            if sign.is_zero() {
22                sign = perp;
23            } else if sign * perp < 0.0 {
24                return false;
25            }
26        }
27
28        true
29    }
30}
31
32/// Tests if the given point is inside an arbitrary closed polygon with arbitrary orientation,
33/// using a counting winding strategy.
34///
35/// The polygon is assumed to be closed, i.e., first and last point of the polygon are implicitly
36/// assumed to be connected by an edge.
37///
38/// This handles concave polygons. For a function dedicated to convex polygons, see [`point_in_convex_poly2d`].
39pub fn point_in_poly2d(pt: &Point2<Real>, poly: &[Point2<Real>]) -> bool {
40    if poly.is_empty() {
41        return false;
42    }
43
44    let mut winding = 0i32;
45
46    for (i, a) in poly.iter().enumerate() {
47        let b = poly[(i + 1) % poly.len()];
48        let seg_dir = b - a;
49        let dpt = pt - a;
50        let perp = dpt.perp(&seg_dir);
51        winding += match (dpt.y >= 0.0, b.y > pt.y) {
52            (true, true) if perp < 0.0 => 1,
53            (false, false) if perp > 0.0 => 1,
54            _ => 0,
55        };
56    }
57
58    winding % 2 == 1
59}
60
61#[cfg(test)]
62mod tests {
63    use super::*;
64
65    #[test]
66    fn point_in_poly2d_self_intersecting() {
67        let poly = [
68            [-1.0, -1.0],
69            [0.0, -1.0],
70            [0.0, 1.0],
71            [-2.0, 1.0],
72            [-2.0, -2.0],
73            [1.0, -2.0],
74            [1.0, 2.0],
75            [-1.0, 2.0],
76        ]
77        .map(Point2::from);
78        assert!(!point_in_poly2d(&[-0.5, -0.5].into(), &poly));
79        assert!(point_in_poly2d(&[0.5, -0.5].into(), &poly));
80    }
81
82    #[test]
83    fn point_in_poly2d_concave() {
84        let poly = [
85            [615.4741821289063, 279.4120788574219],
86            [617.95947265625, 281.8973693847656],
87            [624.1727294921875, 288.73193359375],
88            [626.6580200195313, 292.4598693847656],
89            [634.7352294921875, 302.40106201171875],
90            [637.8418579101563, 306.7503356933594],
91            [642.8124389648438, 312.96356201171875],
92            [652.7536010742188, 330.98193359375],
93            [654.6176147460938, 334.7098693847656],
94            [661.4521484375, 349.0003356933594],
95            [666.4227294921875, 360.18414306640625],
96            [670.1506958007813, 367.6400451660156],
97            [675.1212768554688, 381.30914306640625],
98            [678.2279052734375, 391.2503356933594],
99            [681.33447265625, 402.43414306640625],
100            [683.81982421875, 414.23931884765625],
101            [685.0624389648438, 422.3165283203125],
102            [685.6837768554688, 431.0150146484375],
103            [686.3051147460938, 442.8201904296875],
104            [685.6837768554688, 454.0040283203125],
105            [683.81982421875, 460.83856201171875],
106            [679.4705200195313, 470.77972412109375],
107            [674.4999389648438, 480.720947265625],
108            [670.1506958007813, 486.93414306640625],
109            [662.073486328125, 497.49664306640625],
110            [659.5881958007813, 499.36065673828125],
111            [653.3749389648438, 503.70989990234375],
112            [647.7830200195313, 506.1951904296875],
113            [642.8124389648438, 507.43780517578125],
114            [631.6286010742188, 508.05914306640625],
115            [621.0661010742188, 508.05914306640625],
116            [605.5330200195313, 508.05914306640625],
117            [596.2131958007813, 508.05914306640625],
118            [586.893310546875, 508.05914306640625],
119            [578.8161010742188, 508.05914306640625],
120            [571.3602294921875, 506.1951904296875],
121            [559.5551147460938, 499.36065673828125],
122            [557.0697631835938, 497.49664306640625],
123            [542.1580200195313, 484.4488525390625],
124            [534.7021484375, 476.37164306640625],
125            [532.8381958007813, 473.8863525390625],
126            [527.2462768554688, 466.43048095703125],
127            [522.2756958007813, 450.89739990234375],
128            [521.6543579101563, 444.06280517578125],
129            [521.0330200195313, 431.6363525390625],
130            [521.6543579101563, 422.93780517578125],
131            [523.518310546875, 409.26873779296875],
132            [527.2462768554688, 397.46356201171875],
133            [532.8381958007813, 385.6584167480469],
134            [540.9154052734375, 373.23193359375],
135            [547.1286010742188, 365.77606201171875],
136            [559.5551147460938, 354.59222412109375],
137            [573.2241821289063, 342.165771484375],
138            [575.70947265625, 339.68048095703125],
139            [584.4080200195313, 331.603271484375],
140            [597.455810546875, 317.3128356933594],
141            [601.8051147460938, 311.7209167480469],
142            [607.39697265625, 303.6437072753906],
143            [611.7462768554688, 296.1878356933594],
144            [614.2315673828125, 288.1106262207031],
145            [615.4741821289063, 280.65472412109375],
146            [615.4741821289063, 279.4120788574219],
147        ]
148        .map(Point2::from);
149        let pt = Point2::from([596.0181884765625, 427.9162902832031]);
150        assert!(point_in_poly2d(&pt, &poly));
151    }
152
153    #[test]
154    #[cfg(all(feature = "dim2", feature = "alloc"))]
155    fn point_in_poly2d_concave_exact_vertex_bug() {
156        let poly = crate::shape::Ball::new(1.0).to_polyline(10);
157        assert!(point_in_poly2d(&Point2::origin(), &poly));
158        assert!(point_in_poly2d(&Point2::new(-0.25, 0.0), &poly));
159        assert!(point_in_poly2d(&Point2::new(0.25, 0.0), &poly));
160        assert!(point_in_poly2d(&Point2::new(0.0, -0.25), &poly));
161        assert!(point_in_poly2d(&Point2::new(0.0, 0.25), &poly));
162        assert!(!point_in_poly2d(&Point2::new(-2.0, 0.0), &poly));
163        assert!(!point_in_poly2d(&Point2::new(2.0, 0.0), &poly));
164        assert!(!point_in_poly2d(&Point2::new(0.0, -2.0), &poly));
165        assert!(!point_in_poly2d(&Point2::new(0.0, 2.0), &poly));
166    }
167}
parry3d/utils/point_in_poly2d.rs

parry3d/utils/
point_in_poly2d.rs
