bevy_math/bounding/bounded3d/
extrusion.rs

1use std::f32::consts::FRAC_PI_2;
2
3use glam::{Vec2, Vec3A, Vec3Swizzles};
4
5use crate::bounding::{BoundingCircle, BoundingVolume};
6use crate::primitives::{
7    BoxedPolygon, BoxedPolyline2d, Capsule2d, Cuboid, Cylinder, Ellipse, Extrusion, Line2d,
8    Polygon, Polyline2d, Primitive2d, Rectangle, RegularPolygon, Segment2d, Triangle2d,
9};
10use crate::{Quat, Vec3};
11
12use crate::{bounding::Bounded2d, primitives::Circle};
13
14use super::{Aabb3d, Bounded3d, BoundingSphere};
15
16impl BoundedExtrusion for Circle {
17    fn extrusion_aabb_3d(&self, half_depth: f32, translation: Vec3, rotation: Quat) -> Aabb3d {
18        // Reference: http://iquilezles.org/articles/diskbbox/
19
20        let segment_dir = rotation * Vec3::Z;
21        let top = (segment_dir * half_depth).abs();
22
23        let e = (Vec3::ONE - segment_dir * segment_dir).max(Vec3::ZERO);
24        let half_size = self.radius * Vec3::new(e.x.sqrt(), e.y.sqrt(), e.z.sqrt());
25
26        Aabb3d {
27            min: (translation - half_size - top).into(),
28            max: (translation + half_size + top).into(),
29        }
30    }
31}
32
33impl BoundedExtrusion for Ellipse {
34    fn extrusion_aabb_3d(&self, half_depth: f32, translation: Vec3, rotation: Quat) -> Aabb3d {
35        let Vec2 { x: a, y: b } = self.half_size;
36        let normal = rotation * Vec3::Z;
37        let conjugate_rot = rotation.conjugate();
38
39        let [max_x, max_y, max_z] = Vec3::AXES.map(|axis: Vec3| {
40            let Some(axis) = (conjugate_rot * axis.reject_from(normal))
41                .xy()
42                .try_normalize()
43            else {
44                return Vec3::ZERO;
45            };
46
47            if axis.element_product() == 0. {
48                return rotation * Vec3::new(a * axis.y, b * axis.x, 0.);
49            }
50            let m = -axis.x / axis.y;
51            let signum = axis.signum();
52
53            let y = signum.y * b * b / (b * b + m * m * a * a).sqrt();
54            let x = signum.x * a * (1. - y * y / b / b).sqrt();
55            rotation * Vec3::new(x, y, 0.)
56        });
57
58        let half_size = Vec3::new(max_x.x, max_y.y, max_z.z).abs() + (normal * half_depth).abs();
59        Aabb3d::new(translation, half_size)
60    }
61}
62
63impl BoundedExtrusion for Line2d {
64    fn extrusion_aabb_3d(&self, half_depth: f32, translation: Vec3, rotation: Quat) -> Aabb3d {
65        let dir = rotation * self.direction.extend(0.);
66        let half_depth = (rotation * Vec3::new(0., 0., half_depth)).abs();
67
68        let max = f32::MAX / 2.;
69        let half_size = Vec3::new(
70            if dir.x == 0. { half_depth.x } else { max },
71            if dir.y == 0. { half_depth.y } else { max },
72            if dir.z == 0. { half_depth.z } else { max },
73        );
74
75        Aabb3d::new(translation, half_size)
76    }
77}
78
79impl BoundedExtrusion for Segment2d {
80    fn extrusion_aabb_3d(&self, half_depth: f32, translation: Vec3, rotation: Quat) -> Aabb3d {
81        let half_size = rotation * self.point1().extend(0.);
82        let depth = rotation * Vec3::new(0., 0., half_depth);
83
84        Aabb3d::new(translation, half_size.abs() + depth.abs())
85    }
86}
87
88impl<const N: usize> BoundedExtrusion for Polyline2d<N> {
89    fn extrusion_aabb_3d(&self, half_depth: f32, translation: Vec3, rotation: Quat) -> Aabb3d {
90        let aabb = Aabb3d::from_point_cloud(
91            translation,
92            rotation,
93            self.vertices.map(|v| v.extend(0.)).into_iter(),
94        );
95        let depth = rotation * Vec3A::new(0., 0., half_depth);
96
97        aabb.grow(depth.abs())
98    }
99}
100
101impl BoundedExtrusion for BoxedPolyline2d {
102    fn extrusion_aabb_3d(&self, half_depth: f32, translation: Vec3, rotation: Quat) -> Aabb3d {
103        let aabb = Aabb3d::from_point_cloud(
104            translation,
105            rotation,
106            self.vertices.iter().map(|v| v.extend(0.)),
107        );
108        let depth = rotation * Vec3A::new(0., 0., half_depth);
109
110        aabb.grow(depth.abs())
111    }
112}
113
114impl BoundedExtrusion for Triangle2d {
115    fn extrusion_aabb_3d(&self, half_depth: f32, translation: Vec3, rotation: Quat) -> Aabb3d {
116        let aabb = Aabb3d::from_point_cloud(
117            translation,
118            rotation,
119            self.vertices.iter().map(|v| v.extend(0.)),
120        );
121        let depth = rotation * Vec3A::new(0., 0., half_depth);
122
123        aabb.grow(depth.abs())
124    }
125}
126
127impl BoundedExtrusion for Rectangle {
128    fn extrusion_aabb_3d(&self, half_depth: f32, translation: Vec3, rotation: Quat) -> Aabb3d {
129        Cuboid {
130            half_size: self.half_size.extend(half_depth),
131        }
132        .aabb_3d(translation, rotation)
133    }
134}
135
136impl<const N: usize> BoundedExtrusion for Polygon<N> {
137    fn extrusion_aabb_3d(&self, half_depth: f32, translation: Vec3, rotation: Quat) -> Aabb3d {
138        let aabb = Aabb3d::from_point_cloud(
139            translation,
140            rotation,
141            self.vertices.map(|v| v.extend(0.)).into_iter(),
142        );
143        let depth = rotation * Vec3A::new(0., 0., half_depth);
144
145        aabb.grow(depth.abs())
146    }
147}
148
149impl BoundedExtrusion for BoxedPolygon {
150    fn extrusion_aabb_3d(&self, half_depth: f32, translation: Vec3, rotation: Quat) -> Aabb3d {
151        let aabb = Aabb3d::from_point_cloud(
152            translation,
153            rotation,
154            self.vertices.iter().map(|v| v.extend(0.)),
155        );
156        let depth = rotation * Vec3A::new(0., 0., half_depth);
157
158        aabb.grow(depth.abs())
159    }
160}
161
162impl BoundedExtrusion for RegularPolygon {
163    fn extrusion_aabb_3d(&self, half_depth: f32, translation: Vec3, rotation: Quat) -> Aabb3d {
164        let aabb = Aabb3d::from_point_cloud(
165            translation,
166            rotation,
167            self.vertices(0.).into_iter().map(|v| v.extend(0.)),
168        );
169        let depth = rotation * Vec3A::new(0., 0., half_depth);
170
171        aabb.grow(depth.abs())
172    }
173}
174
175impl BoundedExtrusion for Capsule2d {
176    fn extrusion_aabb_3d(&self, half_depth: f32, translation: Vec3, rotation: Quat) -> Aabb3d {
177        let aabb = Cylinder {
178            half_height: half_depth,
179            radius: self.radius,
180        }
181        .aabb_3d(Vec3::ZERO, rotation * Quat::from_rotation_x(FRAC_PI_2));
182
183        let up = rotation * Vec3::new(0., self.half_length, 0.);
184        let half_size = Into::<Vec3>::into(aabb.max) + up.abs();
185        Aabb3d::new(translation, half_size)
186    }
187}
188
189impl<T: BoundedExtrusion> Bounded3d for Extrusion<T> {
190    fn aabb_3d(&self, translation: Vec3, rotation: Quat) -> Aabb3d {
191        self.base_shape
192            .extrusion_aabb_3d(self.half_depth, translation, rotation)
193    }
194
195    fn bounding_sphere(&self, translation: Vec3, rotation: Quat) -> BoundingSphere {
196        self.base_shape
197            .extrusion_bounding_sphere(self.half_depth, translation, rotation)
198    }
199}
200
201/// A trait implemented on 2D shapes which determines the 3D bounding volumes of their extrusions.
202///
203/// Since default implementations can be inferred from 2D bounding volumes, this allows a `Bounded2d`
204/// implementation on some shape `MyShape` to be extrapolated to a `Bounded3d` implementation on
205/// `Extrusion<MyShape>` without supplying any additional data; e.g.:
206/// `impl BoundedExtrusion for MyShape {}`
207pub trait BoundedExtrusion: Primitive2d + Bounded2d {
208    /// Get an axis-aligned bounding box for an extrusion with this shape as a base and the given `half_depth`, transformed by the given `translation` and `rotation`.
209    fn extrusion_aabb_3d(&self, half_depth: f32, translation: Vec3, rotation: Quat) -> Aabb3d {
210        let cap_normal = rotation * Vec3::Z;
211        let conjugate_rot = rotation.conjugate();
212
213        // The `(halfsize, offset)` for each axis
214        let axis_values = Vec3::AXES.map(|ax| {
215            // This is the direction of the line of intersection of a plane with the `ax` normal and the plane containing the cap of the extrusion.
216            let intersect_line = ax.cross(cap_normal);
217            if intersect_line.length_squared() <= f32::EPSILON {
218                return (0., 0.);
219            };
220
221            // This is the normal vector of the intersection line rotated to be in the XY-plane
222            let line_normal = (conjugate_rot * intersect_line).yx();
223            let angle = line_normal.to_angle();
224
225            // Since the plane containing the caps of the extrusion is not guaranteed to be orthgonal to the `ax` plane, only a certain "scale" factor
226            // of the `Aabb2d` will actually go towards the dimensions of the `Aabb3d`
227            let scale = cap_normal.reject_from(ax).length();
228
229            // Calculate the `Aabb2d` of the base shape. The shape is rotated so that the line of intersection is parallel to the Y axis in the `Aabb2d` calculations.
230            // This guarantees that the X value of the `Aabb2d` is closest to the `ax` plane
231            let aabb2d = self.aabb_2d(Vec2::ZERO, angle);
232            (aabb2d.half_size().x * scale, aabb2d.center().x * scale)
233        });
234
235        let offset = Vec3A::from_array(axis_values.map(|(_, offset)| offset));
236        let cap_size = Vec3A::from_array(axis_values.map(|(max_val, _)| max_val)).abs();
237        let depth = rotation * Vec3A::new(0., 0., half_depth);
238
239        Aabb3d::new(Vec3A::from(translation) - offset, cap_size + depth.abs())
240    }
241
242    /// Get a bounding sphere for an extrusion of the `base_shape` with the given `half_depth` with the given translation and rotation
243    fn extrusion_bounding_sphere(
244        &self,
245        half_depth: f32,
246        translation: Vec3,
247        rotation: Quat,
248    ) -> BoundingSphere {
249        // We calculate the bounding circle of the base shape.
250        // Since each of the extrusions bases will have the same distance from its center,
251        // and they are just shifted along the Z-axis, the minimum bounding sphere will be the bounding sphere
252        // of the cylinder defined by the two bounding circles of the bases for any base shape
253        let BoundingCircle {
254            center,
255            circle: Circle { radius },
256        } = self.bounding_circle(Vec2::ZERO, 0.);
257        let radius = radius.hypot(half_depth);
258        let center = translation + rotation * center.extend(0.);
259
260        BoundingSphere::new(center, radius)
261    }
262}
263
264#[cfg(test)]
265mod tests {
266    use std::f32::consts::FRAC_PI_4;
267
268    use glam::{EulerRot, Quat, Vec2, Vec3, Vec3A};
269
270    use crate::{
271        bounding::{Bounded3d, BoundingVolume},
272        primitives::{
273            Capsule2d, Circle, Ellipse, Extrusion, Line2d, Polygon, Polyline2d, Rectangle,
274            RegularPolygon, Segment2d, Triangle2d,
275        },
276        Dir2,
277    };
278
279    #[test]
280    fn circle() {
281        let cylinder = Extrusion::new(Circle::new(0.5), 2.0);
282        let translation = Vec3::new(2.0, 1.0, 0.0);
283
284        let aabb = cylinder.aabb_3d(translation, Quat::IDENTITY);
285        assert_eq!(aabb.center(), Vec3A::from(translation));
286        assert_eq!(aabb.half_size(), Vec3A::new(0.5, 0.5, 1.0));
287
288        let bounding_sphere = cylinder.bounding_sphere(translation, Quat::IDENTITY);
289        assert_eq!(bounding_sphere.center, translation.into());
290        assert_eq!(bounding_sphere.radius(), 1f32.hypot(0.5));
291    }
292
293    #[test]
294    fn ellipse() {
295        let extrusion = Extrusion::new(Ellipse::new(2.0, 0.5), 4.0);
296        let translation = Vec3::new(3., 4., 5.);
297        let rotation = Quat::from_euler(EulerRot::ZYX, FRAC_PI_4, FRAC_PI_4, FRAC_PI_4);
298
299        let aabb = extrusion.aabb_3d(translation, rotation);
300        assert_eq!(aabb.center(), Vec3A::from(translation));
301        assert_eq!(aabb.half_size(), Vec3A::new(2.709784, 1.3801551, 2.436141));
302
303        let bounding_sphere = extrusion.bounding_sphere(translation, rotation);
304        assert_eq!(bounding_sphere.center, translation.into());
305        assert_eq!(bounding_sphere.radius(), 8f32.sqrt());
306    }
307
308    #[test]
309    fn line() {
310        let extrusion = Extrusion::new(
311            Line2d {
312                direction: Dir2::new_unchecked(Vec2::Y),
313            },
314            4.,
315        );
316        let translation = Vec3::new(3., 4., 5.);
317        let rotation = Quat::from_rotation_y(FRAC_PI_4);
318
319        let aabb = extrusion.aabb_3d(translation, rotation);
320        assert_eq!(aabb.min, Vec3A::new(1.5857864, f32::MIN / 2., 3.5857865));
321        assert_eq!(aabb.max, Vec3A::new(4.4142136, f32::MAX / 2., 6.414213));
322
323        let bounding_sphere = extrusion.bounding_sphere(translation, rotation);
324        assert_eq!(bounding_sphere.center(), translation.into());
325        assert_eq!(bounding_sphere.radius(), f32::MAX / 2.);
326    }
327
328    #[test]
329    fn rectangle() {
330        let extrusion = Extrusion::new(Rectangle::new(2.0, 1.0), 4.0);
331        let translation = Vec3::new(3., 4., 5.);
332        let rotation = Quat::from_rotation_z(std::f32::consts::FRAC_PI_4);
333
334        let aabb = extrusion.aabb_3d(translation, rotation);
335        assert_eq!(aabb.center(), translation.into());
336        assert_eq!(aabb.half_size(), Vec3A::new(1.0606602, 1.0606602, 2.));
337
338        let bounding_sphere = extrusion.bounding_sphere(translation, rotation);
339        assert_eq!(bounding_sphere.center, translation.into());
340        assert_eq!(bounding_sphere.radius(), 2.291288);
341    }
342
343    #[test]
344    fn segment() {
345        let extrusion = Extrusion::new(Segment2d::new(Dir2::new_unchecked(Vec2::NEG_Y), 3.), 4.0);
346        let translation = Vec3::new(3., 4., 5.);
347        let rotation = Quat::from_rotation_x(FRAC_PI_4);
348
349        let aabb = extrusion.aabb_3d(translation, rotation);
350        assert_eq!(aabb.center(), translation.into());
351        assert_eq!(aabb.half_size(), Vec3A::new(0., 2.4748735, 2.4748735));
352
353        let bounding_sphere = extrusion.bounding_sphere(translation, rotation);
354        assert_eq!(bounding_sphere.center, translation.into());
355        assert_eq!(bounding_sphere.radius(), 2.5);
356    }
357
358    #[test]
359    fn polyline() {
360        let polyline = Polyline2d::<4>::new([
361            Vec2::ONE,
362            Vec2::new(-1.0, 1.0),
363            Vec2::NEG_ONE,
364            Vec2::new(1.0, -1.0),
365        ]);
366        let extrusion = Extrusion::new(polyline, 3.0);
367        let translation = Vec3::new(3., 4., 5.);
368        let rotation = Quat::from_rotation_x(FRAC_PI_4);
369
370        let aabb = extrusion.aabb_3d(translation, rotation);
371        assert_eq!(aabb.center(), translation.into());
372        assert_eq!(aabb.half_size(), Vec3A::new(1., 1.7677668, 1.7677668));
373
374        let bounding_sphere = extrusion.bounding_sphere(translation, rotation);
375        assert_eq!(bounding_sphere.center, translation.into());
376        assert_eq!(bounding_sphere.radius(), 2.0615528);
377    }
378
379    #[test]
380    fn triangle() {
381        let triangle = Triangle2d::new(
382            Vec2::new(0.0, 1.0),
383            Vec2::new(-10.0, -1.0),
384            Vec2::new(10.0, -1.0),
385        );
386        let extrusion = Extrusion::new(triangle, 3.0);
387        let translation = Vec3::new(3., 4., 5.);
388        let rotation = Quat::from_rotation_x(FRAC_PI_4);
389
390        let aabb = extrusion.aabb_3d(translation, rotation);
391        assert_eq!(aabb.center(), translation.into());
392        assert_eq!(aabb.half_size(), Vec3A::new(10., 1.7677668, 1.7677668));
393
394        let bounding_sphere = extrusion.bounding_sphere(translation, rotation);
395        assert_eq!(
396            bounding_sphere.center,
397            Vec3A::new(3.0, 3.2928934, 4.2928934)
398        );
399        assert_eq!(bounding_sphere.radius(), 10.111875);
400    }
401
402    #[test]
403    fn polygon() {
404        let polygon = Polygon::<4>::new([
405            Vec2::ONE,
406            Vec2::new(-1.0, 1.0),
407            Vec2::NEG_ONE,
408            Vec2::new(1.0, -1.0),
409        ]);
410        let extrusion = Extrusion::new(polygon, 3.0);
411        let translation = Vec3::new(3., 4., 5.);
412        let rotation = Quat::from_rotation_x(FRAC_PI_4);
413
414        let aabb = extrusion.aabb_3d(translation, rotation);
415        assert_eq!(aabb.center(), translation.into());
416        assert_eq!(aabb.half_size(), Vec3A::new(1., 1.7677668, 1.7677668));
417
418        let bounding_sphere = extrusion.bounding_sphere(translation, rotation);
419        assert_eq!(bounding_sphere.center, translation.into());
420        assert_eq!(bounding_sphere.radius(), 2.0615528);
421    }
422
423    #[test]
424    fn regular_polygon() {
425        let extrusion = Extrusion::new(RegularPolygon::new(2.0, 7), 4.0);
426        let translation = Vec3::new(3., 4., 5.);
427        let rotation = Quat::from_rotation_x(FRAC_PI_4);
428
429        let aabb = extrusion.aabb_3d(translation, rotation);
430        assert_eq!(
431            aabb.center(),
432            Vec3A::from(translation) + Vec3A::new(0., 0.0700254, 0.0700254)
433        );
434        assert_eq!(
435            aabb.half_size(),
436            Vec3A::new(1.9498558, 2.7584014, 2.7584019)
437        );
438
439        let bounding_sphere = extrusion.bounding_sphere(translation, rotation);
440        assert_eq!(bounding_sphere.center, translation.into());
441        assert_eq!(bounding_sphere.radius(), 8f32.sqrt());
442    }
443
444    #[test]
445    fn capsule() {
446        let extrusion = Extrusion::new(Capsule2d::new(0.5, 2.0), 4.0);
447        let translation = Vec3::new(3., 4., 5.);
448        let rotation = Quat::from_rotation_x(FRAC_PI_4);
449
450        let aabb = extrusion.aabb_3d(translation, rotation);
451        assert_eq!(aabb.center(), translation.into());
452        assert_eq!(aabb.half_size(), Vec3A::new(0.5, 2.4748735, 2.4748735));
453
454        let bounding_sphere = extrusion.bounding_sphere(translation, rotation);
455        assert_eq!(bounding_sphere.center, translation.into());
456        assert_eq!(bounding_sphere.radius(), 2.5);
457    }
458}
bevy_math/bounding/bounded3d/extrusion.rs

bevy_math/bounding/bounded3d/
extrusion.rs
