bevy_math/bounding/bounded3d/
extrusion.rs

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

bevy_math/bounding/bounded3d/
extrusion.rs
