wreck/convex_polytope/
array.rs

1use core::fmt;
2
3use glam::Vec3;
4
5use inherent::inherent;
6
7use super::refer::RefConvexPolytope;
8use crate::ConvexPolytope;
9use crate::cuboid::Cuboid;
10use crate::sphere::Sphere;
11use crate::{Collides, Scalable, Stretchable, Transformable};
12
13/// A convex polytope defined by half-planes and vertices, using const generics
14/// so it can be constructed and stored in `const` / `static` contexts.
15///
16/// `P` is the number of half-planes, `V` is the number of vertices.
17///
18/// This is mostly meant for usage with codegen where you statically define obstacles based on
19/// some sort of file at build time, and want to be able to use them in `const` contexts.
20#[derive(Debug, Clone, Copy)]
21#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
22pub struct ArrayConvexPolytope<const P: usize, const V: usize> {
23    #[cfg_attr(feature = "serde", serde(with = "crate::serde_arrays::array_as_seq"))]
24    pub planes: [(Vec3, f32); P],
25    #[cfg_attr(feature = "serde", serde(with = "crate::serde_arrays::array_as_seq"))]
26    pub vertices: [Vec3; V],
27    pub obb: Cuboid,
28}
29
30impl<const P: usize, const V: usize> ArrayConvexPolytope<P, V> {
31    /// Create a new const convex polytope. This is a `const fn` so it can be
32    /// used in `const` and `static` initializers.
33    pub const fn new(planes: [(Vec3, f32); P], vertices: [Vec3; V], obb: Cuboid) -> Self {
34        let mut i = 0;
35        while i < P {
36            let (n, d) = planes[i];
37            debug_assert!(
38                crate::dot(n, n) >= f32::EPSILON,
39                "Plane normal cannot be zero"
40            );
41            debug_assert!(d >= 0.0, "Plane distance cannot be negative");
42            i += 1;
43        }
44        i = 0;
45        while i < V {
46            let v = vertices[i];
47            debug_assert!(
48                !v.x.is_nan() && !v.y.is_nan() && !v.z.is_nan(),
49                "Vertex cannot contain NaN"
50            );
51            debug_assert!(obb.contains_point(v), "Vertex cannot be outside the OBB");
52            i += 1;
53        }
54        Self {
55            planes,
56            vertices,
57            obb,
58        }
59    }
60
61    #[inline]
62    fn as_ref(&self) -> RefConvexPolytope<'_> {
63        RefConvexPolytope::from_array(self)
64    }
65}
66
67#[inherent]
68impl<const P: usize, const V: usize> Scalable for ArrayConvexPolytope<P, V> {
69    pub fn scale(&mut self, factor: f32) {
70        for (_, d) in &mut self.planes {
71            *d *= factor;
72        }
73        for v in &mut self.vertices {
74            *v *= factor;
75        }
76        self.obb.scale(factor);
77    }
78}
79
80#[inherent]
81impl<const P: usize, const V: usize> Transformable for ArrayConvexPolytope<P, V> {
82    pub fn translate(&mut self, offset: glam::Vec3A) {
83        let off = Vec3::from(offset);
84        for (n, d) in &mut self.planes {
85            *d += n.dot(off);
86        }
87        for v in &mut self.vertices {
88            *v += off;
89        }
90        self.obb.translate(offset);
91    }
92
93    pub fn rotate_mat(&mut self, mat: glam::Mat3A) {
94        for (n, _) in &mut self.planes {
95            *n = Vec3::from(mat * glam::Vec3A::from(*n));
96        }
97        for v in &mut self.vertices {
98            *v = Vec3::from(mat * glam::Vec3A::from(*v));
99        }
100        self.obb.rotate_mat(mat);
101    }
102
103    pub fn rotate_quat(&mut self, quat: glam::Quat) {
104        for (n, _) in &mut self.planes {
105            *n = quat * *n;
106        }
107        for v in &mut self.vertices {
108            *v = quat * *v;
109        }
110        self.obb.rotate_quat(quat);
111    }
112
113    pub fn transform(&mut self, mat: glam::Affine3A) {
114        let rot = mat.matrix3;
115        let trans = Vec3::from(mat.translation);
116        for (n, d) in &mut self.planes {
117            *n = Vec3::from(rot * glam::Vec3A::from(*n));
118            *d += n.dot(trans);
119        }
120        for v in &mut self.vertices {
121            *v = Vec3::from(mat.transform_point3a(glam::Vec3A::from(*v)));
122        }
123        self.obb.transform(mat);
124    }
125}
126
127impl<const P: usize, const V: usize> Collides<ArrayConvexPolytope<P, V>> for Sphere {
128    #[inline]
129    fn test<const BROADPHASE: bool>(&self, other: &ArrayConvexPolytope<P, V>) -> bool {
130        other.as_ref().collides_sphere::<BROADPHASE>(self)
131    }
132}
133
134impl<const P: usize, const V: usize> Collides<Sphere> for ArrayConvexPolytope<P, V> {
135    #[inline]
136    fn test<const BROADPHASE: bool>(&self, other: &Sphere) -> bool {
137        self.as_ref().collides_sphere::<BROADPHASE>(other)
138    }
139}
140
141impl<const P: usize, const V: usize> Collides<ArrayConvexPolytope<P, V>> for Cuboid {
142    #[inline]
143    fn test<const BROADPHASE: bool>(&self, other: &ArrayConvexPolytope<P, V>) -> bool {
144        other.as_ref().collides_cuboid::<BROADPHASE>(self)
145    }
146}
147
148impl<const P: usize, const V: usize> Collides<Cuboid> for ArrayConvexPolytope<P, V> {
149    #[inline]
150    fn test<const BROADPHASE: bool>(&self, other: &Cuboid) -> bool {
151        self.as_ref().collides_cuboid::<BROADPHASE>(other)
152    }
153}
154
155impl<const P: usize, const V: usize> Collides<ArrayConvexPolytope<P, V>>
156    for crate::capsule::Capsule
157{
158    #[inline]
159    fn test<const BROADPHASE: bool>(&self, other: &ArrayConvexPolytope<P, V>) -> bool {
160        other.as_ref().collides_capsule::<BROADPHASE>(self)
161    }
162}
163
164impl<const P: usize, const V: usize> Collides<crate::capsule::Capsule>
165    for ArrayConvexPolytope<P, V>
166{
167    #[inline]
168    fn test<const BROADPHASE: bool>(&self, other: &crate::capsule::Capsule) -> bool {
169        self.as_ref().collides_capsule::<BROADPHASE>(other)
170    }
171}
172
173// ---------------------------------------------------------------------------
174// Collision: ArrayConvexPolytope vs ArrayConvexPolytope
175// ---------------------------------------------------------------------------
176
177impl<const P: usize, const V: usize, const P2: usize, const V2: usize>
178    Collides<ArrayConvexPolytope<P2, V2>> for ArrayConvexPolytope<P, V>
179{
180    #[inline]
181    fn test<const BROADPHASE: bool>(&self, other: &ArrayConvexPolytope<P2, V2>) -> bool {
182        self.as_ref()
183            .collides_polytope::<BROADPHASE>(&other.as_ref())
184    }
185}
186
187impl<const P: usize, const V: usize> Stretchable for ArrayConvexPolytope<P, V> {
188    type Output = ConvexPolytope;
189
190    fn stretch(&self, translation: Vec3) -> Self::Output {
191        let heap = ConvexPolytope::with_obb(self.planes.to_vec(), self.vertices.to_vec(), self.obb);
192        heap.stretch(translation)
193    }
194}
195
196impl<const P: usize, const V: usize> fmt::Display for ArrayConvexPolytope<P, V> {
197    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
198        write!(
199            f,
200            "ArrayConvexPolytope(planes: {}, vertices: {})",
201            P, V
202        )
203    }
204}
wreck/convex_polytope/array.rs

wreck/convex_polytope/
array.rs
