collision/primitive/
polygon.rs

1//! Convex polygon primitive
2
3use cgmath::{BaseFloat, Point2, Vector2};
4use cgmath::prelude::*;
5
6use crate::{Aabb2, Line2, Ray2};
7use crate::prelude::*;
8use crate::primitive::util::{get_bound, get_max_point};
9
10/// Convex polygon primitive.
11///
12/// Can contain any number of vertices, but a high number of vertices will
13/// affect performance of course. Vertices need to be in CCW order.
14#[derive(Debug, Clone, PartialEq)]
15#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
16pub struct ConvexPolygon<S> {
17    /// Vertices of the convex polygon.
18    pub vertices: Vec<Point2<S>>,
19}
20
21impl<S> ConvexPolygon<S> {
22    /// Create a new convex polygon from the given vertices. Vertices need to be in CCW order.
23    pub fn new(vertices: Vec<Point2<S>>) -> Self {
24        Self { vertices }
25    }
26}
27
28impl<S> Primitive for ConvexPolygon<S>
29where
30    S: BaseFloat,
31{
32    type Point = Point2<S>;
33
34    fn support_point<T>(&self, direction: &Vector2<S>, transform: &T) -> Point2<S>
35    where
36        T: Transform<Point2<S>>,
37    {
38        if self.vertices.len() < 10 {
39            get_max_point(self.vertices.iter(), direction, transform)
40        } else {
41            support_point(&self.vertices, direction, transform)
42        }
43    }
44}
45
46impl<S> ComputeBound<Aabb2<S>> for ConvexPolygon<S>
47where
48    S: BaseFloat,
49{
50    fn compute_bound(&self) -> Aabb2<S> {
51        get_bound(self.vertices.iter())
52    }
53}
54
55impl<S> Discrete<Ray2<S>> for ConvexPolygon<S>
56where
57    S: BaseFloat,
58{
59    /// Ray must be in object space
60    fn intersects(&self, ray: &Ray2<S>) -> bool {
61        for j in 0..self.vertices.len() - 1 {
62            let i = if j == 0 {
63                self.vertices.len() - 1
64            } else {
65                j - 1
66            };
67            let normal = Vector2::new(
68                self.vertices[j].y - self.vertices[i].y,
69                self.vertices[i].x - self.vertices[j].x,
70            );
71            // check if edge normal points toward the ray origin
72            if ray.direction.dot(normal) < S::zero()
73                // check line ray intersection
74                && ray.intersection(&Line2::new(self.vertices[i], self.vertices[j]))
75                    .is_some()
76            {
77                return true;
78            }
79        }
80
81        false
82    }
83}
84
85impl<S> Continuous<Ray2<S>> for ConvexPolygon<S>
86where
87    S: BaseFloat,
88{
89    type Result = Point2<S>;
90
91    /// Ray must be in object space
92    fn intersection(&self, ray: &Ray2<S>) -> Option<Point2<S>> {
93        for j in 0..self.vertices.len() - 1 {
94            let i = if j == 0 {
95                self.vertices.len() - 1
96            } else {
97                j - 1
98            };
99            let normal = Vector2::new(
100                self.vertices[j].y - self.vertices[i].y,
101                self.vertices[i].x - self.vertices[j].x,
102            );
103            // check if edge normal points toward the ray origin
104            if ray.direction.dot(normal) < S::zero() {
105                // check line ray intersection
106                if let point @ Some(_) =
107                    ray.intersection(&Line2::new(self.vertices[i], self.vertices[j]))
108                {
109                    return point;
110                }
111            }
112        }
113
114        None
115    }
116}
117
118fn support_point<P, T>(vertices: &[P], direction: &P::Diff, transform: &T) -> P
119where
120    P: EuclideanSpace,
121    P::Scalar: BaseFloat,
122    T: Transform<P>,
123{
124    let direction = transform.inverse_transform_vector(*direction).unwrap();
125
126    // figure out where to start, if the direction is negative for the first vertex,
127    // go halfway around the polygon
128    let mut start_index: i32 = 0;
129    let mut max_dot = vertices[0].dot(direction);
130    if max_dot < P::Scalar::zero() {
131        start_index = vertices.len() as i32 / 2;
132        max_dot = dot_index(vertices, start_index, &direction);
133    }
134
135    let left_dot = dot_index(vertices, start_index - 1, &direction);
136    let right_dot = dot_index(vertices, start_index + 1, &direction);
137
138    // check if start is highest
139    let p = if start_index == 0 && max_dot > left_dot && max_dot > right_dot {
140        vertices[0]
141    } else {
142        // figure out iteration direction
143        let mut add: i32 = 1;
144        let mut previous_dot = left_dot;
145        if left_dot > max_dot && left_dot > right_dot {
146            add = -1;
147            previous_dot = right_dot;
148        }
149
150        // iterate
151        let mut index = start_index + add;
152        let mut current_dot = max_dot;
153        if index == vertices.len() as i32 {
154            index = 0;
155        }
156        if index == -1 {
157            index = vertices.len() as i32 - 1;
158        }
159        while index != start_index {
160            let next_dot = dot_index(vertices, index + add, &direction);
161            if current_dot > previous_dot && current_dot > next_dot {
162                break;
163            }
164            previous_dot = current_dot;
165            current_dot = next_dot;
166            index += add;
167            if index == vertices.len() as i32 {
168                index = 0;
169            }
170            if index == -1 {
171                index = vertices.len() as i32 - 1;
172            }
173        }
174        vertices[index as usize]
175    };
176
177    transform.transform_point(p)
178}
179
180#[inline]
181fn dot_index<P>(vertices: &[P], index: i32, direction: &P::Diff) -> P::Scalar
182where
183    P: EuclideanSpace,
184    P::Scalar: BaseFloat,
185{
186    let index_u = index as usize;
187    if index_u == vertices.len() {
188        vertices[0]
189    } else if index == -1 {
190        vertices[vertices.len() - 1]
191    } else {
192        vertices[index_u]
193    }.dot(*direction)
194}
195
196#[cfg(test)]
197mod tests {
198    use cgmath::{Basis2, Decomposed, Point2, Rad, Vector2};
199    use approx::assert_ulps_eq;
200
201    use super::*;
202    use {Aabb2, Ray2};
203
204    #[test]
205    fn test_support_point() {
206        let vertices = vec![
207            Point2::new(5., 5.),
208            Point2::new(4., 6.),
209            Point2::new(3., 7.),
210            Point2::new(2., 6.),
211            Point2::new(1., 6.),
212            Point2::new(0., 5.),
213            Point2::new(-1., 4.),
214            Point2::new(-3., 3.),
215            Point2::new(-6., 1.),
216            Point2::new(-5., 0.),
217            Point2::new(-4., -1.),
218            Point2::new(-2., -3.),
219            Point2::new(0., -7.),
220            Point2::new(1., -8.),
221            Point2::new(2., -5.),
222            Point2::new(3., 0.),
223            Point2::new(4., 3.),
224        ];
225        let t = transform(0., 0., 0.);
226        let point = support_point(&vertices, &Vector2::new(-1., 0.), &t);
227        assert_eq!(Point2::new(-5., 0.), point);
228
229        let point = support_point(&vertices, &Vector2::new(0., -1.), &t);
230        assert_eq!(Point2::new(1., -8.), point);
231
232        let point = support_point(&vertices, &Vector2::new(0., 1.), &t);
233        assert_eq!(Point2::new(3., 7.), point);
234
235        let point = support_point(&vertices, &Vector2::new(1., 0.), &t);
236        assert_eq!(Point2::new(5., 5.), point);
237    }
238
239    #[test]
240    fn test_bound() {
241        let vertices = vec![
242            Point2::new(5., 5.),
243            Point2::new(4., 6.),
244            Point2::new(3., 7.),
245            Point2::new(2., 6.),
246            Point2::new(1., 6.),
247            Point2::new(0., 5.),
248            Point2::new(-1., 4.),
249            Point2::new(-3., 3.),
250            Point2::new(-6., 1.),
251            Point2::new(-5., 0.),
252            Point2::new(-4., -1.),
253            Point2::new(-2., -3.),
254            Point2::new(0., -7.),
255            Point2::new(1., -8.),
256            Point2::new(2., -5.),
257            Point2::new(3., 0.),
258            Point2::new(4., 3.),
259        ];
260        let polygon = ConvexPolygon::new(vertices);
261        assert_eq!(
262            Aabb2::new(Point2::new(-6., -8.), Point2::new(5., 7.)),
263            polygon.compute_bound()
264        );
265    }
266
267    #[test]
268    fn test_ray_discrete() {
269        let vertices = vec![
270            Point2::new(5., 5.),
271            Point2::new(4., 6.),
272            Point2::new(3., 7.),
273            Point2::new(2., 6.),
274            Point2::new(1., 6.),
275            Point2::new(0., 5.),
276            Point2::new(-1., 4.),
277            Point2::new(-3., 3.),
278            Point2::new(-6., 1.),
279            Point2::new(-5., 0.),
280            Point2::new(-4., -1.),
281            Point2::new(-2., -3.),
282            Point2::new(0., -7.),
283            Point2::new(1., -8.),
284            Point2::new(2., -5.),
285            Point2::new(3., 0.),
286            Point2::new(4., 3.),
287        ];
288        let polygon = ConvexPolygon::new(vertices);
289        let ray = Ray2::new(Point2::new(0., 10.), Vector2::new(0., -1.));
290        assert!(polygon.intersects(&ray));
291        let ray = Ray2::new(Point2::new(0., 10.), Vector2::new(0., 1.));
292        assert!(!polygon.intersects(&ray));
293        let ray = Ray2::new(Point2::new(0., 7.2), Vector2::new(1., 0.));
294        assert!(!polygon.intersects(&ray));
295        let ray = Ray2::new(Point2::new(0., 6.8), Vector2::new(1., 0.));
296        assert!(polygon.intersects(&ray));
297    }
298
299    #[test]
300    fn test_ray_discrete_transformed() {
301        let vertices = vec![
302            Point2::new(5., 5.),
303            Point2::new(4., 6.),
304            Point2::new(3., 7.),
305            Point2::new(2., 6.),
306            Point2::new(1., 6.),
307            Point2::new(0., 5.),
308            Point2::new(-1., 4.),
309            Point2::new(-3., 3.),
310            Point2::new(-6., 1.),
311            Point2::new(-5., 0.),
312            Point2::new(-4., -1.),
313            Point2::new(-2., -3.),
314            Point2::new(0., -7.),
315            Point2::new(1., -8.),
316            Point2::new(2., -5.),
317            Point2::new(3., 0.),
318            Point2::new(4., 3.),
319        ];
320        let polygon = ConvexPolygon::new(vertices);
321        let t = transform(0., 0., 0.);
322        let ray = Ray2::new(Point2::new(0., 10.), Vector2::new(0., -1.));
323        assert!(polygon.intersects_transformed(&ray, &t));
324        let ray = Ray2::new(Point2::new(0., 10.), Vector2::new(0., 1.));
325        assert!(!polygon.intersects_transformed(&ray, &t));
326        let ray = Ray2::new(Point2::new(0., 7.2), Vector2::new(1., 0.));
327        assert!(!polygon.intersects_transformed(&ray, &t));
328        let ray = Ray2::new(Point2::new(0., 6.8), Vector2::new(1., 0.));
329        assert!(polygon.intersects_transformed(&ray, &t));
330        let t = transform(0., -2., 0.);
331        assert!(!polygon.intersects_transformed(&ray, &t));
332        let t = transform(0., 0., 0.3);
333        assert!(polygon.intersects_transformed(&ray, &t));
334    }
335
336    #[test]
337    fn test_ray_continuous() {
338        let vertices = vec![
339            Point2::new(5., 5.),
340            Point2::new(4., 6.),
341            Point2::new(3., 7.),
342            Point2::new(2., 6.),
343            Point2::new(1., 6.),
344            Point2::new(0., 5.),
345            Point2::new(-1., 4.),
346            Point2::new(-3., 3.),
347            Point2::new(-6., 1.),
348            Point2::new(-5., 0.),
349            Point2::new(-4., -1.),
350            Point2::new(-2., -3.),
351            Point2::new(0., -7.),
352            Point2::new(1., -8.),
353            Point2::new(2., -5.),
354            Point2::new(3., 0.),
355            Point2::new(4., 3.),
356        ];
357        let polygon = ConvexPolygon::new(vertices);
358        let ray = Ray2::new(Point2::new(0., 10.), Vector2::new(0., -1.));
359        assert_eq!(Some(Point2::new(0., 5.)), polygon.intersection(&ray));
360        let ray = Ray2::new(Point2::new(0., 10.), Vector2::new(0., 1.));
361        assert_eq!(None, polygon.intersection(&ray));
362        let ray = Ray2::new(Point2::new(0., 7.2), Vector2::new(1., 0.));
363        assert_eq!(None, polygon.intersection(&ray));
364        let ray = Ray2::new(Point2::new(0., 6.8), Vector2::new(1., 0.));
365        let p = polygon.intersection(&ray).unwrap();
366        assert_ulps_eq!(2.8, p.x);
367        assert_ulps_eq!(6.8, p.y);
368    }
369
370    #[test]
371    fn test_ray_continuous_transformed() {
372        let vertices = vec![
373            Point2::new(5., 5.),
374            Point2::new(4., 6.),
375            Point2::new(3., 7.),
376            Point2::new(2., 6.),
377            Point2::new(1., 6.),
378            Point2::new(0., 5.),
379            Point2::new(-1., 4.),
380            Point2::new(-3., 3.),
381            Point2::new(-6., 1.),
382            Point2::new(-5., 0.),
383            Point2::new(-4., -1.),
384            Point2::new(-2., -3.),
385            Point2::new(0., -7.),
386            Point2::new(1., -8.),
387            Point2::new(2., -5.),
388            Point2::new(3., 0.),
389            Point2::new(4., 3.),
390        ];
391        let t = transform(0., 0., 0.);
392        let polygon = ConvexPolygon::new(vertices);
393        let ray = Ray2::new(Point2::new(0., 10.), Vector2::new(0., -1.));
394        assert_eq!(
395            Some(Point2::new(0., 5.)),
396            polygon.intersection_transformed(&ray, &t)
397        );
398        let ray = Ray2::new(Point2::new(0., 10.), Vector2::new(0., 1.));
399        assert_eq!(None, polygon.intersection_transformed(&ray, &t));
400        let ray = Ray2::new(Point2::new(0., 7.2), Vector2::new(1., 0.));
401        assert_eq!(None, polygon.intersection_transformed(&ray, &t));
402        let ray = Ray2::new(Point2::new(0., 6.8), Vector2::new(1., 0.));
403        let p = polygon.intersection_transformed(&ray, &t).unwrap();
404        assert_ulps_eq!(2.8, p.x);
405        assert_ulps_eq!(6.8, p.y);
406        let t = transform(0., -2., 0.);
407        assert_eq!(None, polygon.intersection_transformed(&ray, &t));
408        let t = transform(0., 0., 0.3);
409        let p = polygon.intersection_transformed(&ray, &t).unwrap();
410        assert_ulps_eq!(0.38913357, p.x);
411        assert_ulps_eq!(6.8, p.y);
412    }
413
414    fn transform(dx: f32, dy: f32, rot: f32) -> Decomposed<Vector2<f32>, Basis2<f32>> {
415        Decomposed {
416            scale: 1.,
417            rot: Rotation2::from_angle(Rad(rot)),
418            disp: Vector2::new(dx, dy),
419        }
420    }
421}
collision/primitive/polygon.rs

collision/primitive/
polygon.rs
