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, Primitive2d, Rectangle,
10        RegularPolygon, Segment2d, Triangle2d,
11    },
12    Isometry2d, Isometry3d, Quat, Rot2,
13};
14
15#[cfg(feature = "alloc")]
16use crate::primitives::{Polygon, Polyline2d};
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
99#[cfg(feature = "alloc")]
100impl BoundedExtrusion for Polyline2d {
101    fn extrusion_aabb_3d(&self, half_depth: f32, isometry: impl Into<Isometry3d>) -> Aabb3d {
102        let isometry = isometry.into();
103        let aabb = Aabb3d::from_point_cloud(isometry, self.vertices.iter().map(|v| v.extend(0.)));
104        let depth = isometry.rotation * Vec3A::new(0., 0., half_depth);
105
106        aabb.grow(depth.abs())
107    }
108}
109
110impl BoundedExtrusion for Triangle2d {
111    fn extrusion_aabb_3d(&self, half_depth: f32, isometry: impl Into<Isometry3d>) -> Aabb3d {
112        let isometry = isometry.into();
113        let aabb = Aabb3d::from_point_cloud(isometry, self.vertices.iter().map(|v| v.extend(0.)));
114        let depth = isometry.rotation * Vec3A::new(0., 0., half_depth);
115
116        aabb.grow(depth.abs())
117    }
118}
119
120impl BoundedExtrusion for Rectangle {
121    fn extrusion_aabb_3d(&self, half_depth: f32, isometry: impl Into<Isometry3d>) -> Aabb3d {
122        Cuboid {
123            half_size: self.half_size.extend(half_depth),
124        }
125        .aabb_3d(isometry)
126    }
127}
128
129#[cfg(feature = "alloc")]
130impl BoundedExtrusion for Polygon {
131    fn extrusion_aabb_3d(&self, half_depth: f32, isometry: impl Into<Isometry3d>) -> Aabb3d {
132        let isometry = isometry.into();
133        let aabb = Aabb3d::from_point_cloud(isometry, self.vertices.iter().map(|v| v.extend(0.)));
134        let depth = isometry.rotation * Vec3A::new(0., 0., half_depth);
135
136        aabb.grow(depth.abs())
137    }
138}
139
140impl BoundedExtrusion for RegularPolygon {
141    fn extrusion_aabb_3d(&self, half_depth: f32, isometry: impl Into<Isometry3d>) -> Aabb3d {
142        let isometry = isometry.into();
143        let aabb = Aabb3d::from_point_cloud(
144            isometry,
145            self.vertices(0.).into_iter().map(|v| v.extend(0.)),
146        );
147        let depth = isometry.rotation * Vec3A::new(0., 0., half_depth);
148
149        aabb.grow(depth.abs())
150    }
151}
152
153impl BoundedExtrusion for Capsule2d {
154    fn extrusion_aabb_3d(&self, half_depth: f32, isometry: impl Into<Isometry3d>) -> Aabb3d {
155        let isometry = isometry.into();
156        let aabb = Cylinder {
157            half_height: half_depth,
158            radius: self.radius,
159        }
160        .aabb_3d(isometry.rotation * Quat::from_rotation_x(FRAC_PI_2));
161
162        let up = isometry.rotation * Vec3A::new(0., self.half_length, 0.);
163        let half_size = aabb.max + up.abs();
164        Aabb3d::new(isometry.translation, half_size)
165    }
166}
167
168impl<T: BoundedExtrusion> Bounded3d for Extrusion<T> {
169    fn aabb_3d(&self, isometry: impl Into<Isometry3d>) -> Aabb3d {
170        self.base_shape.extrusion_aabb_3d(self.half_depth, isometry)
171    }
172
173    fn bounding_sphere(&self, isometry: impl Into<Isometry3d>) -> BoundingSphere {
174        self.base_shape
175            .extrusion_bounding_sphere(self.half_depth, isometry)
176    }
177}
178
179/// A trait implemented on 2D shapes which determines the 3D bounding volumes of their extrusions.
180///
181/// Since default implementations can be inferred from 2D bounding volumes, this allows a `Bounded2d`
182/// implementation on some shape `MyShape` to be extrapolated to a `Bounded3d` implementation on
183/// `Extrusion<MyShape>` without supplying any additional data; e.g.:
184/// `impl BoundedExtrusion for MyShape {}`
185pub trait BoundedExtrusion: Primitive2d + Bounded2d {
186    /// 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`.
187    fn extrusion_aabb_3d(&self, half_depth: f32, isometry: impl Into<Isometry3d>) -> Aabb3d {
188        let isometry = isometry.into();
189        let cap_normal = isometry.rotation * Vec3A::Z;
190        let conjugate_rot = isometry.rotation.conjugate();
191
192        // The `(halfsize, offset)` for each axis
193        let axis_values = Vec3A::AXES.map(|ax| {
194            // 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.
195            let intersect_line = ax.cross(cap_normal);
196            if intersect_line.length_squared() <= f32::EPSILON {
197                return (0., 0.);
198            };
199
200            // This is the normal vector of the intersection line rotated to be in the XY-plane
201            let line_normal = (conjugate_rot * intersect_line).yx();
202            let angle = line_normal.to_angle();
203
204            // Since the plane containing the caps of the extrusion is not guaranteed to be orthogonal to the `ax` plane, only a certain "scale" factor
205            // of the `Aabb2d` will actually go towards the dimensions of the `Aabb3d`
206            let scale = cap_normal.reject_from(ax).length();
207
208            // 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.
209            // This guarantees that the X value of the `Aabb2d` is closest to the `ax` plane
210            let aabb2d = self.aabb_2d(Rot2::radians(angle));
211            (aabb2d.half_size().x * scale, aabb2d.center().x * scale)
212        });
213
214        let offset = Vec3A::from_array(axis_values.map(|(_, offset)| offset));
215        let cap_size = Vec3A::from_array(axis_values.map(|(max_val, _)| max_val)).abs();
216        let depth = isometry.rotation * Vec3A::new(0., 0., half_depth);
217
218        Aabb3d::new(isometry.translation - offset, cap_size + depth.abs())
219    }
220
221    /// Get a bounding sphere for an extrusion of the `base_shape` with the given `half_depth` with the given translation and rotation
222    fn extrusion_bounding_sphere(
223        &self,
224        half_depth: f32,
225        isometry: impl Into<Isometry3d>,
226    ) -> BoundingSphere {
227        let isometry = isometry.into();
228
229        // We calculate the bounding circle of the base shape.
230        // Since each of the extrusions bases will have the same distance from its center,
231        // and they are just shifted along the Z-axis, the minimum bounding sphere will be the bounding sphere
232        // of the cylinder defined by the two bounding circles of the bases for any base shape
233        let BoundingCircle {
234            center,
235            circle: Circle { radius },
236        } = self.bounding_circle(Isometry2d::IDENTITY);
237        let radius = ops::hypot(radius, half_depth);
238        let center = isometry * Vec3A::from(center.extend(0.));
239
240        BoundingSphere::new(center, radius)
241    }
242}
243
244#[cfg(test)]
245mod tests {
246    use core::f32::consts::FRAC_PI_4;
247
248    use glam::{EulerRot, Quat, Vec2, Vec3, Vec3A};
249
250    use crate::{
251        bounding::{Bounded3d, BoundingVolume},
252        ops,
253        primitives::{
254            Capsule2d, Circle, Ellipse, Extrusion, Line2d, Polygon, Polyline2d, Rectangle,
255            RegularPolygon, Segment2d, Triangle2d,
256        },
257        Dir2, Isometry3d,
258    };
259
260    #[test]
261    fn circle() {
262        let cylinder = Extrusion::new(Circle::new(0.5), 2.0);
263        let translation = Vec3::new(2.0, 1.0, 0.0);
264
265        let aabb = cylinder.aabb_3d(translation);
266        assert_eq!(aabb.center(), Vec3A::from(translation));
267        assert_eq!(aabb.half_size(), Vec3A::new(0.5, 0.5, 1.0));
268
269        let bounding_sphere = cylinder.bounding_sphere(translation);
270        assert_eq!(bounding_sphere.center, translation.into());
271        assert_eq!(bounding_sphere.radius(), ops::hypot(1.0, 0.5));
272    }
273
274    #[test]
275    fn ellipse() {
276        let extrusion = Extrusion::new(Ellipse::new(2.0, 0.5), 4.0);
277        let translation = Vec3::new(3., 4., 5.);
278        let rotation = Quat::from_euler(EulerRot::ZYX, FRAC_PI_4, FRAC_PI_4, FRAC_PI_4);
279        let isometry = Isometry3d::new(translation, rotation);
280
281        let aabb = extrusion.aabb_3d(isometry);
282        assert_eq!(aabb.center(), Vec3A::from(translation));
283        assert_eq!(aabb.half_size(), Vec3A::new(2.709784, 1.3801551, 2.436141));
284
285        let bounding_sphere = extrusion.bounding_sphere(isometry);
286        assert_eq!(bounding_sphere.center, translation.into());
287        assert_eq!(bounding_sphere.radius(), ops::sqrt(8f32));
288    }
289
290    #[test]
291    fn line() {
292        let extrusion = Extrusion::new(
293            Line2d {
294                direction: Dir2::new_unchecked(Vec2::Y),
295            },
296            4.,
297        );
298        let translation = Vec3::new(3., 4., 5.);
299        let rotation = Quat::from_rotation_y(FRAC_PI_4);
300        let isometry = Isometry3d::new(translation, rotation);
301
302        let aabb = extrusion.aabb_3d(isometry);
303        assert_eq!(aabb.min, Vec3A::new(1.5857864, f32::MIN / 2., 3.5857865));
304        assert_eq!(aabb.max, Vec3A::new(4.4142136, f32::MAX / 2., 6.414213));
305
306        let bounding_sphere = extrusion.bounding_sphere(isometry);
307        assert_eq!(bounding_sphere.center(), translation.into());
308        assert_eq!(bounding_sphere.radius(), f32::MAX / 2.);
309    }
310
311    #[test]
312    fn rectangle() {
313        let extrusion = Extrusion::new(Rectangle::new(2.0, 1.0), 4.0);
314        let translation = Vec3::new(3., 4., 5.);
315        let rotation = Quat::from_rotation_z(FRAC_PI_4);
316        let isometry = Isometry3d::new(translation, rotation);
317
318        let aabb = extrusion.aabb_3d(isometry);
319        assert_eq!(aabb.center(), translation.into());
320        assert_eq!(aabb.half_size(), Vec3A::new(1.0606602, 1.0606602, 2.));
321
322        let bounding_sphere = extrusion.bounding_sphere(isometry);
323        assert_eq!(bounding_sphere.center, translation.into());
324        assert_eq!(bounding_sphere.radius(), 2.291288);
325    }
326
327    #[test]
328    fn segment() {
329        let extrusion = Extrusion::new(
330            Segment2d::new(Vec2::new(0.0, -1.5), Vec2::new(0.0, 1.5)),
331            4.0,
332        );
333        let translation = Vec3::new(3., 4., 5.);
334        let rotation = Quat::from_rotation_x(FRAC_PI_4);
335        let isometry = Isometry3d::new(translation, rotation);
336
337        let aabb = extrusion.aabb_3d(isometry);
338        assert_eq!(aabb.center(), translation.into());
339        assert_eq!(aabb.half_size(), Vec3A::new(0., 2.4748735, 2.4748735));
340
341        let bounding_sphere = extrusion.bounding_sphere(isometry);
342        assert_eq!(bounding_sphere.center, translation.into());
343        assert_eq!(bounding_sphere.radius(), 2.5);
344    }
345
346    #[test]
347    fn polyline() {
348        let polyline = Polyline2d::new([
349            Vec2::ONE,
350            Vec2::new(-1.0, 1.0),
351            Vec2::NEG_ONE,
352            Vec2::new(1.0, -1.0),
353        ]);
354        let extrusion = Extrusion::new(polyline, 3.0);
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(1., 1.7677668, 1.7677668));
362
363        let bounding_sphere = extrusion.bounding_sphere(isometry);
364        assert_eq!(bounding_sphere.center, translation.into());
365        assert_eq!(bounding_sphere.radius(), 2.0615528);
366    }
367
368    #[test]
369    fn triangle() {
370        let triangle = Triangle2d::new(
371            Vec2::new(0.0, 1.0),
372            Vec2::new(-10.0, -1.0),
373            Vec2::new(10.0, -1.0),
374        );
375        let extrusion = Extrusion::new(triangle, 3.0);
376        let translation = Vec3::new(3., 4., 5.);
377        let rotation = Quat::from_rotation_x(FRAC_PI_4);
378        let isometry = Isometry3d::new(translation, rotation);
379
380        let aabb = extrusion.aabb_3d(isometry);
381        assert_eq!(aabb.center(), translation.into());
382        assert_eq!(aabb.half_size(), Vec3A::new(10., 1.7677668, 1.7677668));
383
384        let bounding_sphere = extrusion.bounding_sphere(isometry);
385        assert_eq!(
386            bounding_sphere.center,
387            Vec3A::new(3.0, 3.2928934, 4.2928934)
388        );
389        assert_eq!(bounding_sphere.radius(), 10.111875);
390    }
391
392    #[test]
393    fn polygon() {
394        let polygon = Polygon::new([
395            Vec2::ONE,
396            Vec2::new(-1.0, 1.0),
397            Vec2::NEG_ONE,
398            Vec2::new(1.0, -1.0),
399        ]);
400        let extrusion = Extrusion::new(polygon, 3.0);
401        let translation = Vec3::new(3., 4., 5.);
402        let rotation = Quat::from_rotation_x(FRAC_PI_4);
403        let isometry = Isometry3d::new(translation, rotation);
404
405        let aabb = extrusion.aabb_3d(isometry);
406        assert_eq!(aabb.center(), translation.into());
407        assert_eq!(aabb.half_size(), Vec3A::new(1., 1.7677668, 1.7677668));
408
409        let bounding_sphere = extrusion.bounding_sphere(isometry);
410        assert_eq!(bounding_sphere.center, translation.into());
411        assert_eq!(bounding_sphere.radius(), 2.0615528);
412    }
413
414    #[test]
415    fn regular_polygon() {
416        let extrusion = Extrusion::new(RegularPolygon::new(2.0, 7), 4.0);
417        let translation = Vec3::new(3., 4., 5.);
418        let rotation = Quat::from_rotation_x(FRAC_PI_4);
419        let isometry = Isometry3d::new(translation, rotation);
420
421        let aabb = extrusion.aabb_3d(isometry);
422        assert_eq!(
423            aabb.center(),
424            Vec3A::from(translation) + Vec3A::new(0., 0.0700254, 0.0700254)
425        );
426        assert_eq!(
427            aabb.half_size(),
428            Vec3A::new(1.9498558, 2.7584014, 2.7584019)
429        );
430
431        let bounding_sphere = extrusion.bounding_sphere(isometry);
432        assert_eq!(bounding_sphere.center, translation.into());
433        assert_eq!(bounding_sphere.radius(), ops::sqrt(8f32));
434    }
435
436    #[test]
437    fn capsule() {
438        let extrusion = Extrusion::new(Capsule2d::new(0.5, 2.0), 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!(aabb.center(), translation.into());
445        assert_eq!(aabb.half_size(), Vec3A::new(0.5, 2.4748735, 2.4748735));
446
447        let bounding_sphere = extrusion.bounding_sphere(isometry);
448        assert_eq!(bounding_sphere.center, translation.into());
449        assert_eq!(bounding_sphere.radius(), 2.5);
450    }
451}
bevy_math/bounding/bounded3d/extrusion.rs

bevy_math/bounding/bounded3d/
extrusion.rs
