1use core::borrow::Borrow;
2
3use bevy_ecs::{component::Component, entity::EntityHashMap, reflect::ReflectComponent};
4use bevy_math::{Affine3A, Mat3A, Mat4, Vec3, Vec3A, Vec4, Vec4Swizzles};
5use bevy_reflect::prelude::*;
6
7#[derive(Component, Clone, Copy, Debug, Default, Reflect, PartialEq)]
32#[reflect(Component, Default, Debug, PartialEq)]
33pub struct Aabb {
34 pub center: Vec3A,
35 pub half_extents: Vec3A,
36}
37
38impl Aabb {
39 #[inline]
40 pub fn from_min_max(minimum: Vec3, maximum: Vec3) -> Self {
41 let minimum = Vec3A::from(minimum);
42 let maximum = Vec3A::from(maximum);
43 let center = 0.5 * (maximum + minimum);
44 let half_extents = 0.5 * (maximum - minimum);
45 Self {
46 center,
47 half_extents,
48 }
49 }
50
51 pub fn enclosing<T: Borrow<Vec3>>(iter: impl IntoIterator<Item = T>) -> Option<Self> {
65 let mut iter = iter.into_iter().map(|p| *p.borrow());
66 let mut min = iter.next()?;
67 let mut max = min;
68 for v in iter {
69 min = Vec3::min(min, v);
70 max = Vec3::max(max, v);
71 }
72 Some(Self::from_min_max(min, max))
73 }
74
75 #[inline]
77 pub fn relative_radius(&self, p_normal: &Vec3A, world_from_local: &Mat3A) -> f32 {
78 let half_extents = self.half_extents;
80 Vec3A::new(
81 p_normal.dot(world_from_local.x_axis),
82 p_normal.dot(world_from_local.y_axis),
83 p_normal.dot(world_from_local.z_axis),
84 )
85 .abs()
86 .dot(half_extents)
87 }
88
89 #[inline]
90 pub fn min(&self) -> Vec3A {
91 self.center - self.half_extents
92 }
93
94 #[inline]
95 pub fn max(&self) -> Vec3A {
96 self.center + self.half_extents
97 }
98}
99
100impl From<Sphere> for Aabb {
101 #[inline]
102 fn from(sphere: Sphere) -> Self {
103 Self {
104 center: sphere.center,
105 half_extents: Vec3A::splat(sphere.radius),
106 }
107 }
108}
109
110#[derive(Clone, Debug, Default)]
111pub struct Sphere {
112 pub center: Vec3A,
113 pub radius: f32,
114}
115
116impl Sphere {
117 #[inline]
118 pub fn intersects_obb(&self, aabb: &Aabb, world_from_local: &Affine3A) -> bool {
119 let aabb_center_world = world_from_local.transform_point3a(aabb.center);
120 let v = aabb_center_world - self.center;
121 let d = v.length();
122 let relative_radius = aabb.relative_radius(&(v / d), &world_from_local.matrix3);
123 d < self.radius + relative_radius
124 }
125}
126
127#[derive(Clone, Copy, Debug, Default)]
151pub struct HalfSpace {
152 normal_d: Vec4,
153}
154
155impl HalfSpace {
156 #[inline]
161 pub fn new(normal_d: Vec4) -> Self {
162 Self {
163 normal_d: normal_d * normal_d.xyz().length_recip(),
164 }
165 }
166
167 #[inline]
169 pub fn normal(&self) -> Vec3A {
170 Vec3A::from_vec4(self.normal_d)
171 }
172
173 #[inline]
176 pub fn d(&self) -> f32 {
177 self.normal_d.w
178 }
179
180 #[inline]
183 pub fn normal_d(&self) -> Vec4 {
184 self.normal_d
185 }
186}
187
188#[derive(Component, Clone, Copy, Debug, Default, Reflect)]
213#[reflect(Component, Default, Debug)]
214pub struct Frustum {
215 #[reflect(ignore)]
216 pub half_spaces: [HalfSpace; 6],
217}
218
219impl Frustum {
220 #[inline]
222 pub fn from_clip_from_world(clip_from_world: &Mat4) -> Self {
223 let mut frustum = Frustum::from_clip_from_world_no_far(clip_from_world);
224 frustum.half_spaces[5] = HalfSpace::new(clip_from_world.row(2));
225 frustum
226 }
227
228 #[inline]
231 pub fn from_clip_from_world_custom_far(
232 clip_from_world: &Mat4,
233 view_translation: &Vec3,
234 view_backward: &Vec3,
235 far: f32,
236 ) -> Self {
237 let mut frustum = Frustum::from_clip_from_world_no_far(clip_from_world);
238 let far_center = *view_translation - far * *view_backward;
239 frustum.half_spaces[5] =
240 HalfSpace::new(view_backward.extend(-view_backward.dot(far_center)));
241 frustum
242 }
243
244 fn from_clip_from_world_no_far(clip_from_world: &Mat4) -> Self {
250 let row3 = clip_from_world.row(3);
251 let mut half_spaces = [HalfSpace::default(); 6];
252 for (i, half_space) in half_spaces.iter_mut().enumerate().take(5) {
253 let row = clip_from_world.row(i / 2);
254 *half_space = HalfSpace::new(if (i & 1) == 0 && i != 4 {
255 row3 + row
256 } else {
257 row3 - row
258 });
259 }
260 Self { half_spaces }
261 }
262
263 #[inline]
265 pub fn intersects_sphere(&self, sphere: &Sphere, intersect_far: bool) -> bool {
266 let sphere_center = sphere.center.extend(1.0);
267 let max = if intersect_far { 6 } else { 5 };
268 for half_space in &self.half_spaces[..max] {
269 if half_space.normal_d().dot(sphere_center) + sphere.radius <= 0.0 {
270 return false;
271 }
272 }
273 true
274 }
275
276 #[inline]
278 pub fn intersects_obb(
279 &self,
280 aabb: &Aabb,
281 world_from_local: &Affine3A,
282 intersect_near: bool,
283 intersect_far: bool,
284 ) -> bool {
285 let aabb_center_world = world_from_local.transform_point3a(aabb.center).extend(1.0);
286 for (idx, half_space) in self.half_spaces.into_iter().enumerate() {
287 if idx == 4 && !intersect_near {
288 continue;
289 }
290 if idx == 5 && !intersect_far {
291 continue;
292 }
293 let p_normal = half_space.normal();
294 let relative_radius = aabb.relative_radius(&p_normal, &world_from_local.matrix3);
295 if half_space.normal_d().dot(aabb_center_world) + relative_radius <= 0.0 {
296 return false;
297 }
298 }
299 true
300 }
301}
302
303#[derive(Component, Clone, Debug, Default, Reflect)]
304#[reflect(Component, Default, Debug)]
305pub struct CubemapFrusta {
306 #[reflect(ignore)]
307 pub frusta: [Frustum; 6],
308}
309
310impl CubemapFrusta {
311 pub fn iter(&self) -> impl DoubleEndedIterator<Item = &Frustum> {
312 self.frusta.iter()
313 }
314 pub fn iter_mut(&mut self) -> impl DoubleEndedIterator<Item = &mut Frustum> {
315 self.frusta.iter_mut()
316 }
317}
318
319#[derive(Component, Debug, Default, Reflect, Clone)]
320#[reflect(Component, Default, Debug)]
321pub struct CascadesFrusta {
322 #[reflect(ignore)]
323 pub frusta: EntityHashMap<Vec<Frustum>>,
324}
325
326#[cfg(test)]
327mod tests {
328 use super::*;
329
330 fn big_frustum() -> Frustum {
332 Frustum {
333 half_spaces: [
334 HalfSpace::new(Vec4::new(-0.9701, -0.2425, -0.0000, 7.7611)),
335 HalfSpace::new(Vec4::new(-0.0000, 1.0000, -0.0000, 4.0000)),
336 HalfSpace::new(Vec4::new(-0.0000, -0.2425, -0.9701, 2.9104)),
337 HalfSpace::new(Vec4::new(-0.0000, -1.0000, -0.0000, 4.0000)),
338 HalfSpace::new(Vec4::new(-0.0000, -0.2425, 0.9701, 2.9104)),
339 HalfSpace::new(Vec4::new(0.9701, -0.2425, -0.0000, -1.9403)),
340 ],
341 }
342 }
343
344 #[test]
345 fn intersects_sphere_big_frustum_outside() {
346 let frustum = big_frustum();
348 let sphere = Sphere {
349 center: Vec3A::new(0.9167, 0.0000, 0.0000),
350 radius: 0.7500,
351 };
352 assert!(!frustum.intersects_sphere(&sphere, true));
353 }
354
355 #[test]
356 fn intersects_sphere_big_frustum_intersect() {
357 let frustum = big_frustum();
359 let sphere = Sphere {
360 center: Vec3A::new(7.9288, 0.0000, 2.9728),
361 radius: 2.0000,
362 };
363 assert!(frustum.intersects_sphere(&sphere, true));
364 }
365
366 fn frustum() -> Frustum {
368 Frustum {
369 half_spaces: [
370 HalfSpace::new(Vec4::new(-0.9701, -0.2425, -0.0000, 0.7276)),
371 HalfSpace::new(Vec4::new(-0.0000, 1.0000, -0.0000, 1.0000)),
372 HalfSpace::new(Vec4::new(-0.0000, -0.2425, -0.9701, 0.7276)),
373 HalfSpace::new(Vec4::new(-0.0000, -1.0000, -0.0000, 1.0000)),
374 HalfSpace::new(Vec4::new(-0.0000, -0.2425, 0.9701, 0.7276)),
375 HalfSpace::new(Vec4::new(0.9701, -0.2425, -0.0000, 0.7276)),
376 ],
377 }
378 }
379
380 #[test]
381 fn intersects_sphere_frustum_surrounding() {
382 let frustum = frustum();
384 let sphere = Sphere {
385 center: Vec3A::new(0.0000, 0.0000, 0.0000),
386 radius: 3.0000,
387 };
388 assert!(frustum.intersects_sphere(&sphere, true));
389 }
390
391 #[test]
392 fn intersects_sphere_frustum_contained() {
393 let frustum = frustum();
395 let sphere = Sphere {
396 center: Vec3A::new(0.0000, 0.0000, 0.0000),
397 radius: 0.7000,
398 };
399 assert!(frustum.intersects_sphere(&sphere, true));
400 }
401
402 #[test]
403 fn intersects_sphere_frustum_intersects_plane() {
404 let frustum = frustum();
406 let sphere = Sphere {
407 center: Vec3A::new(0.0000, 0.0000, 0.9695),
408 radius: 0.7000,
409 };
410 assert!(frustum.intersects_sphere(&sphere, true));
411 }
412
413 #[test]
414 fn intersects_sphere_frustum_intersects_2_planes() {
415 let frustum = frustum();
417 let sphere = Sphere {
418 center: Vec3A::new(1.2037, 0.0000, 0.9695),
419 radius: 0.7000,
420 };
421 assert!(frustum.intersects_sphere(&sphere, true));
422 }
423
424 #[test]
425 fn intersects_sphere_frustum_intersects_3_planes() {
426 let frustum = frustum();
428 let sphere = Sphere {
429 center: Vec3A::new(1.2037, -1.0988, 0.9695),
430 radius: 0.7000,
431 };
432 assert!(frustum.intersects_sphere(&sphere, true));
433 }
434
435 #[test]
436 fn intersects_sphere_frustum_dodges_1_plane() {
437 let frustum = frustum();
439 let sphere = Sphere {
440 center: Vec3A::new(-1.7020, 0.0000, 0.0000),
441 radius: 0.7000,
442 };
443 assert!(!frustum.intersects_sphere(&sphere, true));
444 }
445
446 fn long_frustum() -> Frustum {
448 Frustum {
449 half_spaces: [
450 HalfSpace::new(Vec4::new(-0.9998, -0.0222, -0.0000, -1.9543)),
451 HalfSpace::new(Vec4::new(-0.0000, 1.0000, -0.0000, 45.1249)),
452 HalfSpace::new(Vec4::new(-0.0000, -0.0168, -0.9999, 2.2718)),
453 HalfSpace::new(Vec4::new(-0.0000, -1.0000, -0.0000, 45.1249)),
454 HalfSpace::new(Vec4::new(-0.0000, -0.0168, 0.9999, 2.2718)),
455 HalfSpace::new(Vec4::new(0.9998, -0.0222, -0.0000, 7.9528)),
456 ],
457 }
458 }
459
460 #[test]
461 fn intersects_sphere_long_frustum_outside() {
462 let frustum = long_frustum();
464 let sphere = Sphere {
465 center: Vec3A::new(-4.4889, 46.9021, 0.0000),
466 radius: 0.7500,
467 };
468 assert!(!frustum.intersects_sphere(&sphere, true));
469 }
470
471 #[test]
472 fn intersects_sphere_long_frustum_intersect() {
473 let frustum = long_frustum();
475 let sphere = Sphere {
476 center: Vec3A::new(-4.9957, 0.0000, -0.7396),
477 radius: 4.4094,
478 };
479 assert!(frustum.intersects_sphere(&sphere, true));
480 }
481
482 #[test]
483 fn aabb_enclosing() {
484 assert_eq!(Aabb::enclosing(<[Vec3; 0]>::default()), None);
485 assert_eq!(
486 Aabb::enclosing(vec![Vec3::ONE]).unwrap(),
487 Aabb::from_min_max(Vec3::ONE, Vec3::ONE)
488 );
489 assert_eq!(
490 Aabb::enclosing(&[Vec3::Y, Vec3::X, Vec3::Z][..]).unwrap(),
491 Aabb::from_min_max(Vec3::ZERO, Vec3::ONE)
492 );
493 assert_eq!(
494 Aabb::enclosing([
495 Vec3::NEG_X,
496 Vec3::X * 2.0,
497 Vec3::NEG_Y * 5.0,
498 Vec3::Z,
499 Vec3::ZERO
500 ])
501 .unwrap(),
502 Aabb::from_min_max(Vec3::new(-1.0, -5.0, 0.0), Vec3::new(2.0, 0.0, 1.0))
503 );
504 }
505}