bevy_shape/bounding/bounded3d/
extrusion.rs

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

bevy_shape/bounding/bounded3d/
extrusion.rs
