bsp_pathfinding/
shape.rs

1use std::{array, f32::consts::TAU};
2
3use glam::{Mat3, Mat4, Vec2, Vec3Swizzles};
4use smallvec::{smallvec, SmallVec};
5
6use crate::TOLERANCE;
7
8/// Defines a collection of faces.
9/// This struct is not neccesary to use, but helps in constructing squares and
10/// other primitives.
11#[derive(Default, Debug, Clone)]
12pub struct Shape {
13    vertices: SmallVec<[Vec2; 8]>,
14}
15
16impl Shape {
17    pub fn new(vertices: &[Vec2]) -> Self {
18        Self {
19            vertices: SmallVec::from_slice(vertices),
20        }
21    }
22
23    pub fn regular_polygon(sides: usize, radius: f32, origin: Vec2) -> Self {
24        let turn = TAU / sides as f32;
25        let vertices = (0..=sides)
26            .map(|val| {
27                let x = (turn * val as f32).cos();
28                let y = (turn * val as f32).sin();
29
30                Vec2::new(x, y) * radius + origin
31            })
32            .collect();
33
34        Self { vertices }
35    }
36
37    pub fn rect(size: Vec2, origin: Vec2) -> Self {
38        let half_size = size / 2.0;
39        let vertices = smallvec![
40            Vec2::new(-half_size.x, -half_size.y) + origin,
41            Vec2::new(half_size.x, -half_size.y) + origin,
42            Vec2::new(half_size.x, half_size.y) + origin,
43            Vec2::new(-half_size.x, half_size.y) + origin,
44            Vec2::new(-half_size.x, -half_size.y) + origin,
45        ];
46
47        Self { vertices }
48    }
49
50    pub fn faces(&self) -> Faces {
51        Faces {
52            vertices: &self.vertices,
53            current: 0,
54            len: self.vertices.len(),
55        }
56    }
57}
58
59#[derive(Default, Debug, Copy, Clone, PartialEq)]
60/// A two dimensional face of two vertices.
61/// Uses counterclockwise winding order to calculate a normal
62#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
63pub struct Face {
64    pub(crate) normal: Vec2,
65    pub vertices: [Vec2; 2],
66}
67
68impl Face {
69    pub fn new(vertices: [Vec2; 2]) -> Self {
70        let dir = (vertices[1] - vertices[0]).normalize();
71        let normal = Vec2::new(dir.y, -dir.x);
72        Self { normal, vertices }
73    }
74
75    // Return the length of the face
76    pub fn length(&self) -> f32 {
77        (self.vertices[0] - self.vertices[1]).length()
78    }
79
80    pub fn length_squared(&self) -> f32 {
81        (self.vertices[0] - self.vertices[1]).length_squared()
82    }
83
84    /// Get the face's vertices.
85    pub fn vertices(&self) -> [Vec2; 2] {
86        self.vertices
87    }
88
89    pub fn into_tuple(&self) -> (Vec2, Vec2) {
90        (self.vertices[0], self.vertices[1])
91    }
92
93    /// Get the face's normal.
94    #[inline]
95    pub fn normal(&self) -> Vec2 {
96        self.normal
97    }
98
99    /// Transforms the face
100    pub fn transform(&self, transform: Mat3) -> Self {
101        let [a, b] = self.vertices;
102        Face::new([transform.transform_point2(a), transform.transform_point2(b)])
103    }
104
105    /// Transforms the face using 3d space using xz plane
106    pub fn transform_3d(&self, transform: Mat4) -> Self {
107        let a = transform.transform_point3(self.vertices[0].extend(0.0).xzy());
108        let b = transform.transform_point3(self.vertices[1].extend(0.0).xzy());
109
110        Self::new([a.xz(), b.xz()])
111    }
112
113    /// Returns the side self is in respect to a point and normal
114    pub fn side_of(&self, p: Vec2, normal: Vec2) -> Side {
115        let a = (self.vertices[0] - p).dot(normal);
116        let b = (self.vertices[1] - p).dot(normal);
117
118        if a.abs() < TOLERANCE && b.abs() < TOLERANCE {
119            Side::Coplanar
120        } else if a >= -TOLERANCE && b >= -TOLERANCE {
121            Side::Front
122        } else if a <= TOLERANCE && b <= TOLERANCE {
123            Side::Back
124        } else {
125            Side::Intersecting
126        }
127    }
128
129    /// Splits the face around `p`
130    pub fn split(&self, p: Vec2, normal: Vec2) -> [Self; 2] {
131        let a = (self.vertices[0] - p).dot(normal);
132        if a >= -TOLERANCE {
133            [
134                Face::new([self.vertices[0], p]),
135                Face::new([p, self.vertices[1]]),
136            ]
137        } else {
138            [
139                Face::new([p, self.vertices[1]]),
140                Face::new([self.vertices[0], p]),
141            ]
142        }
143    }
144
145    /// Returns true if the face is touching the other face
146    pub fn adjacent(&self, other: Face) -> bool {
147        let p = other.midpoint();
148        let a = (self.vertices[0] - p).dot(other.normal);
149        let b = (self.vertices[1] - p).dot(other.normal);
150
151        // a.signum() != b.signum()
152        (a < -TOLERANCE && b > TOLERANCE) || (b < -TOLERANCE && a > TOLERANCE)
153    }
154
155    pub fn midpoint(&self) -> Vec2 {
156        (self.vertices[0] + self.vertices[1]) / 2.0
157    }
158
159    /// Returns true if `other` overlaps self
160    pub fn overlaps(&self, other: &Self) -> bool {
161        let dir = self.dir();
162
163        let p = (self.vertices[0]).dot(dir);
164        let q = (self.vertices[1]).dot(dir);
165        let a = (other.vertices[0]).dot(dir);
166        let b = (other.vertices[1]).dot(dir);
167
168        // a -- b in the direction of self
169        let (a, b) = if dir.dot(other.dir()) > 0.0 {
170            (a, b)
171        } else {
172            (b, a)
173        };
174
175        let la = q - a;
176        let lb = b - p;
177
178        let overlap = la.min(lb);
179
180        overlap > TOLERANCE
181    }
182
183    pub fn contains_point(&self, p: Vec2) -> bool {
184        let dir = self.dir();
185
186        let d = (p - self.vertices[0]).dot(dir);
187
188        d > -TOLERANCE && d < self.length() + TOLERANCE
189    }
190
191    pub fn dir(&self) -> Vec2 {
192        (self.vertices[1] - self.vertices[0]).normalize()
193    }
194}
195
196impl<'a> IntoIterator for &'a Shape {
197    type Item = Face;
198
199    type IntoIter = Faces<'a>;
200
201    fn into_iter(self) -> Self::IntoIter {
202        self.faces()
203    }
204}
205
206impl<'a> IntoIterator for Face {
207    type Item = Vec2;
208
209    type IntoIter = array::IntoIter<Vec2, 2>;
210
211    fn into_iter(self) -> Self::IntoIter {
212        self.vertices.into_iter()
213    }
214}
215
216impl<'a> IntoIterator for &'a Face {
217    type Item = Vec2;
218
219    type IntoIter = array::IntoIter<Vec2, 2>;
220
221    fn into_iter(self) -> Self::IntoIter {
222        self.vertices.into_iter()
223    }
224}
225
226#[doc(hidden)]
227pub struct Faces<'a> {
228    vertices: &'a [Vec2],
229    current: usize,
230    len: usize,
231}
232
233impl<'a> Iterator for Faces<'a> {
234    type Item = Face;
235
236    // Generate normals from winding
237    fn next(&mut self) -> Option<Self::Item> {
238        if self.current + 1 == self.len {
239            return None;
240        }
241
242        let a = self.vertices[self.current];
243        let b = self.vertices[(self.current + 1)];
244
245        self.current += 1;
246        Some(Face::new([a, b]))
247    }
248}
249
250#[cfg(test)]
251mod tests {
252    use glam::Vec2;
253
254    use super::Shape;
255
256    #[test]
257    fn shape_rect() {
258        let rect = Shape::rect(Vec2::new(2.0, 1.0), Vec2::new(1.0, 0.0));
259
260        let faces = rect.faces();
261
262        let normals = [-Vec2::Y, Vec2::X, Vec2::Y, -Vec2::X];
263
264        assert!(faces.map(|val| val.normal).eq(normals));
265    }
266}
267
268#[derive(Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Debug)]
269pub enum Side {
270    Front,
271    Back,
272    Coplanar,
273    Intersecting,
274}
275
276impl Side {
277    pub fn min_side(&self, other: Self) -> Self {
278        match (self, other) {
279            (Side::Back, _) | (_, Side::Back) => Side::Back,
280            _ => *self,
281        }
282    }
283}
bsp_pathfinding/shape.rs

bsp_pathfinding/
shape.rs
