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

bevy_math/bounding/bounded3d/
extrusion.rs
