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

bevy_math/bounding/bounded3d/
extrusion.rs
