quadtree_cd/
geometry.rs

1use std::ops::{Add, Neg, Sub};
2
3#[derive(Clone, Copy, Debug)]
4pub struct BoundingBox<T: Copy> {
5    pub x0: T,
6    pub y0: T,
7    pub x1: T,
8    pub y1: T,
9}
10
11impl<T> BoundingBox<T>
12where
13    T: Copy + PartialOrd + Neg<Output = T>,
14{
15    pub fn new(x0: T, y0: T, x1: T, y1: T) -> Self {
16        BoundingBox { x0, y0, x1, y1 }
17    }
18}
19
20pub trait Intersection {
21    fn intersects(&self, other: &Self) -> bool;
22}
23
24#[derive(Debug, Clone, Copy)]
25pub struct RotatedRect {
26    pub x: f32,
27    pub y: f32,
28    pub w: f32,
29    pub h: f32,
30    pub a: f32,
31}
32
33#[derive(Debug, Clone, Copy)]
34struct Vector(f32, f32);
35
36impl Vector {
37    fn rotate(&self, a: f32) -> Self {
38        let cosa = a.cos();
39        let sina = a.sin();
40        Vector(
41            self.0 * cosa + self.1 * sina,
42            -self.0 * sina + self.1 * cosa,
43        )
44    }
45}
46
47impl Add<Vector> for Vector {
48    type Output = Self;
49
50    fn add(self, other: Self) -> Self {
51        Vector(self.0 + other.0, self.1 + other.1)
52    }
53}
54
55impl Neg for Vector {
56    type Output = Self;
57
58    fn neg(self) -> Self {
59        Vector(-self.0, -self.1)
60    }
61}
62
63impl Sub<Vector> for Vector {
64    type Output = Self;
65
66    fn sub(self, other: Self) -> Self {
67        Vector(self.0 - other.0, self.1 - other.1)
68    }
69}
70
71impl From<&RotatedRect> for BoundingBox<f32> {
72    fn from(rect: &RotatedRect) -> Self {
73        let c = rect.a.cos().abs();
74        let s = rect.a.sin().abs();
75
76        let ex = rect.w / 2.0;
77        let ey = rect.h / 2.0;
78
79        let x_radius = ex * c + ey * s;
80        let y_radius = ex * s + ey * c;
81
82        BoundingBox::<f32>::new(
83            rect.x - x_radius,
84            rect.y - y_radius,
85            rect.x + x_radius,
86            rect.y + y_radius,
87        )
88    }
89}
90
91pub fn intersects<T>(item: &T, other: &T) -> bool
92where
93    T: Intersection,
94{
95    item.intersects(other)
96}
97
98// Rotated Rectangles Collision Detection, Oren Becker, 2001
99impl Intersection for RotatedRect {
100    fn intersects(&self, other: &Self) -> bool {
101        let ang = self.a - other.a;
102        let cosa = ang.cos();
103        let sina = ang.sin();
104
105        // Move rr2 to make rr1 cannonic
106        let c = Vector(other.x, other.y) - Vector(self.x, self.y);
107
108        // Rotate rr2 clockwise by rr2->ang to make rr2 axis-aligned
109        let c = c.rotate(other.a);
110
111        // Calculate vertices of (moved and axis-aligned := 'ma') rr2
112        let size = Vector(other.w / 2.0, other.h / 2.0);
113        let bl = c - size;
114        let tr = c + size;
115
116        // Calculate vertices of (rotated := 'r') rr1
117        let (a, b) = {
118            let cost = self.w / 2.0 * cosa;
119            let sint = self.w / 2.0 * sina;
120
121            let ax = -self.h / 2.0 * sina;
122            let ay = self.h / 2.0 * cosa;
123
124            (Vector(ax + cost, ay + sint), Vector(ax - cost, ay - sint))
125        };
126
127        // Verify that A is vertical min/max, B is horizontal min/max
128        let t = sina * cosa;
129        let (a, b) = {
130            if t < 0.0 {
131                (b, a)
132            } else {
133                (a, b)
134            }
135        };
136
137        // Verify that B is horizontal minimum (left-most vertex)
138        let b = if sina < 0.0 { -b } else { b };
139
140        // If rr2(ma) isn't in the horizontal range of
141        // colliding with rr1(r), collision is impossible
142        if b.0 > tr.0 || b.0 > -(bl.0) {
143            return false;
144        }
145
146        // This defaults correspond to the case of axis-alignment.
147        let mut ext1 = a.1;
148        let mut ext2 = -a.1;
149
150        if t != 0.0 {
151            {
152                let mut dx1 = bl.0 - a.0;
153                let dx2 = tr.0 - a.0;
154
155                if dx1 * dx2 > 0.0 {
156                    let mut dx = a.0;
157                    if dx1 < 0.0 {
158                        dx -= b.0;
159                        ext1 -= b.1;
160                        dx1 = dx2;
161                    } else {
162                        dx += b.0;
163                        ext1 += b.1;
164                    }
165
166                    ext1 *= dx1;
167                    ext1 /= dx;
168                    ext1 += a.1;
169                }
170            }
171
172            {
173                let mut dx1 = bl.0 + a.0;
174                let dx2 = tr.0 + a.0;
175
176                if dx1 * dx2 > 0.0 {
177                    let mut dx = -a.0;
178                    if dx1 < 0.0 {
179                        dx -= b.0;
180                        ext2 -= b.1;
181                        dx1 = dx2;
182                    } else {
183                        dx += b.0;
184                        ext2 += b.1;
185                    }
186                    ext2 *= dx1;
187                    ext2 /= dx;
188                    ext2 -= a.1;
189                }
190            }
191        }
192
193        let (ext1, ext2) = if ext1 > ext2 {
194            (ext2, ext1)
195        } else {
196            (ext1, ext2)
197        };
198
199        !((ext1 < bl.1 && ext2 < bl.1) || (ext1 > tr.1 && ext2 > tr.1))
200    }
201}
quadtree_cd/geometry.rs

quadtree_cd/
geometry.rs
