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, Clone)]
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 #[inline]
102 pub fn is_in_half_space(&self, half_space: &HalfSpace, world_from_local: &Affine3A) -> bool {
103 let half_extents_world = world_from_local.matrix3.abs() * self.half_extents.abs();
105 let p_normal = half_space.normal();
107 let r = half_extents_world.dot(p_normal.abs());
108 let aabb_center_world = world_from_local.transform_point3a(self.center);
109 let signed_distance = p_normal.dot(aabb_center_world) + half_space.d();
110 signed_distance > r
111 }
112}
113
114impl From<Sphere> for Aabb {
115 #[inline]
116 fn from(sphere: Sphere) -> Self {
117 Self {
118 center: sphere.center,
119 half_extents: Vec3A::splat(sphere.radius),
120 }
121 }
122}
123
124#[derive(Clone, Debug, Default)]
125pub struct Sphere {
126 pub center: Vec3A,
127 pub radius: f32,
128}
129
130impl Sphere {
131 #[inline]
132 pub fn intersects_obb(&self, aabb: &Aabb, world_from_local: &Affine3A) -> bool {
133 let aabb_center_world = world_from_local.transform_point3a(aabb.center);
134 let v = aabb_center_world - self.center;
135 let d = v.length();
136 let relative_radius = aabb.relative_radius(&(v / d), &world_from_local.matrix3);
137 d < self.radius + relative_radius
138 }
139}
140
141#[derive(Clone, Copy, Debug, Default)]
165pub struct HalfSpace {
166 normal_d: Vec4,
167}
168
169impl HalfSpace {
170 #[inline]
175 pub fn new(normal_d: Vec4) -> Self {
176 Self {
177 normal_d: normal_d * normal_d.xyz().length_recip(),
178 }
179 }
180
181 #[inline]
183 pub fn normal(&self) -> Vec3A {
184 Vec3A::from_vec4(self.normal_d)
185 }
186
187 #[inline]
190 pub fn d(&self) -> f32 {
191 self.normal_d.w
192 }
193
194 #[inline]
197 pub fn normal_d(&self) -> Vec4 {
198 self.normal_d
199 }
200}
201
202#[derive(Component, Clone, Copy, Debug, Default, Reflect)]
227#[reflect(Component, Default, Debug, Clone)]
228pub struct Frustum {
229 #[reflect(ignore, clone)]
230 pub half_spaces: [HalfSpace; 6],
231}
232
233impl Frustum {
234 #[inline]
236 pub fn from_clip_from_world(clip_from_world: &Mat4) -> Self {
237 let mut frustum = Frustum::from_clip_from_world_no_far(clip_from_world);
238 frustum.half_spaces[5] = HalfSpace::new(clip_from_world.row(2));
239 frustum
240 }
241
242 #[inline]
245 pub fn from_clip_from_world_custom_far(
246 clip_from_world: &Mat4,
247 view_translation: &Vec3,
248 view_backward: &Vec3,
249 far: f32,
250 ) -> Self {
251 let mut frustum = Frustum::from_clip_from_world_no_far(clip_from_world);
252 let far_center = *view_translation - far * *view_backward;
253 frustum.half_spaces[5] =
254 HalfSpace::new(view_backward.extend(-view_backward.dot(far_center)));
255 frustum
256 }
257
258 fn from_clip_from_world_no_far(clip_from_world: &Mat4) -> Self {
264 let row3 = clip_from_world.row(3);
265 let mut half_spaces = [HalfSpace::default(); 6];
266 for (i, half_space) in half_spaces.iter_mut().enumerate().take(5) {
267 let row = clip_from_world.row(i / 2);
268 *half_space = HalfSpace::new(if (i & 1) == 0 && i != 4 {
269 row3 + row
270 } else {
271 row3 - row
272 });
273 }
274 Self { half_spaces }
275 }
276
277 #[inline]
279 pub fn intersects_sphere(&self, sphere: &Sphere, intersect_far: bool) -> bool {
280 let sphere_center = sphere.center.extend(1.0);
281 let max = if intersect_far { 6 } else { 5 };
282 for half_space in &self.half_spaces[..max] {
283 if half_space.normal_d().dot(sphere_center) + sphere.radius <= 0.0 {
284 return false;
285 }
286 }
287 true
288 }
289
290 #[inline]
292 pub fn intersects_obb(
293 &self,
294 aabb: &Aabb,
295 world_from_local: &Affine3A,
296 intersect_near: bool,
297 intersect_far: bool,
298 ) -> bool {
299 let aabb_center_world = world_from_local.transform_point3a(aabb.center).extend(1.0);
300 for (idx, half_space) in self.half_spaces.into_iter().enumerate() {
301 if idx == 4 && !intersect_near {
302 continue;
303 }
304 if idx == 5 && !intersect_far {
305 continue;
306 }
307 let p_normal = half_space.normal();
308 let relative_radius = aabb.relative_radius(&p_normal, &world_from_local.matrix3);
309 if half_space.normal_d().dot(aabb_center_world) + relative_radius <= 0.0 {
310 return false;
311 }
312 }
313 true
314 }
315
316 #[inline]
319 pub fn contains_aabb(&self, aabb: &Aabb, world_from_local: &Affine3A) -> bool {
320 for half_space in &self.half_spaces {
321 if !aabb.is_in_half_space(half_space, world_from_local) {
322 return false;
323 }
324 }
325 true
326 }
327}
328
329#[derive(Component, Clone, Debug, Default, Reflect)]
330#[reflect(Component, Default, Debug, Clone)]
331pub struct CubemapFrusta {
332 #[reflect(ignore, clone)]
333 pub frusta: [Frustum; 6],
334}
335
336impl CubemapFrusta {
337 pub fn iter(&self) -> impl DoubleEndedIterator<Item = &Frustum> {
338 self.frusta.iter()
339 }
340 pub fn iter_mut(&mut self) -> impl DoubleEndedIterator<Item = &mut Frustum> {
341 self.frusta.iter_mut()
342 }
343}
344
345#[derive(Component, Debug, Default, Reflect, Clone)]
346#[reflect(Component, Default, Debug, Clone)]
347pub struct CascadesFrusta {
348 #[reflect(ignore, clone)]
349 pub frusta: EntityHashMap<Vec<Frustum>>,
350}
351
352#[cfg(test)]
353mod tests {
354 use core::f32::consts::PI;
355
356 use bevy_math::{ops, Quat};
357 use bevy_transform::components::GlobalTransform;
358
359 use crate::camera::{CameraProjection, PerspectiveProjection};
360
361 use super::*;
362
363 fn big_frustum() -> Frustum {
365 Frustum {
366 half_spaces: [
367 HalfSpace::new(Vec4::new(-0.9701, -0.2425, -0.0000, 7.7611)),
368 HalfSpace::new(Vec4::new(-0.0000, 1.0000, -0.0000, 4.0000)),
369 HalfSpace::new(Vec4::new(-0.0000, -0.2425, -0.9701, 2.9104)),
370 HalfSpace::new(Vec4::new(-0.0000, -1.0000, -0.0000, 4.0000)),
371 HalfSpace::new(Vec4::new(-0.0000, -0.2425, 0.9701, 2.9104)),
372 HalfSpace::new(Vec4::new(0.9701, -0.2425, -0.0000, -1.9403)),
373 ],
374 }
375 }
376
377 #[test]
378 fn intersects_sphere_big_frustum_outside() {
379 let frustum = big_frustum();
381 let sphere = Sphere {
382 center: Vec3A::new(0.9167, 0.0000, 0.0000),
383 radius: 0.7500,
384 };
385 assert!(!frustum.intersects_sphere(&sphere, true));
386 }
387
388 #[test]
389 fn intersects_sphere_big_frustum_intersect() {
390 let frustum = big_frustum();
392 let sphere = Sphere {
393 center: Vec3A::new(7.9288, 0.0000, 2.9728),
394 radius: 2.0000,
395 };
396 assert!(frustum.intersects_sphere(&sphere, true));
397 }
398
399 fn frustum() -> Frustum {
401 Frustum {
402 half_spaces: [
403 HalfSpace::new(Vec4::new(-0.9701, -0.2425, -0.0000, 0.7276)),
404 HalfSpace::new(Vec4::new(-0.0000, 1.0000, -0.0000, 1.0000)),
405 HalfSpace::new(Vec4::new(-0.0000, -0.2425, -0.9701, 0.7276)),
406 HalfSpace::new(Vec4::new(-0.0000, -1.0000, -0.0000, 1.0000)),
407 HalfSpace::new(Vec4::new(-0.0000, -0.2425, 0.9701, 0.7276)),
408 HalfSpace::new(Vec4::new(0.9701, -0.2425, -0.0000, 0.7276)),
409 ],
410 }
411 }
412
413 #[test]
414 fn intersects_sphere_frustum_surrounding() {
415 let frustum = frustum();
417 let sphere = Sphere {
418 center: Vec3A::new(0.0000, 0.0000, 0.0000),
419 radius: 3.0000,
420 };
421 assert!(frustum.intersects_sphere(&sphere, true));
422 }
423
424 #[test]
425 fn intersects_sphere_frustum_contained() {
426 let frustum = frustum();
428 let sphere = Sphere {
429 center: Vec3A::new(0.0000, 0.0000, 0.0000),
430 radius: 0.7000,
431 };
432 assert!(frustum.intersects_sphere(&sphere, true));
433 }
434
435 #[test]
436 fn intersects_sphere_frustum_intersects_plane() {
437 let frustum = frustum();
439 let sphere = Sphere {
440 center: Vec3A::new(0.0000, 0.0000, 0.9695),
441 radius: 0.7000,
442 };
443 assert!(frustum.intersects_sphere(&sphere, true));
444 }
445
446 #[test]
447 fn intersects_sphere_frustum_intersects_2_planes() {
448 let frustum = frustum();
450 let sphere = Sphere {
451 center: Vec3A::new(1.2037, 0.0000, 0.9695),
452 radius: 0.7000,
453 };
454 assert!(frustum.intersects_sphere(&sphere, true));
455 }
456
457 #[test]
458 fn intersects_sphere_frustum_intersects_3_planes() {
459 let frustum = frustum();
461 let sphere = Sphere {
462 center: Vec3A::new(1.2037, -1.0988, 0.9695),
463 radius: 0.7000,
464 };
465 assert!(frustum.intersects_sphere(&sphere, true));
466 }
467
468 #[test]
469 fn intersects_sphere_frustum_dodges_1_plane() {
470 let frustum = frustum();
472 let sphere = Sphere {
473 center: Vec3A::new(-1.7020, 0.0000, 0.0000),
474 radius: 0.7000,
475 };
476 assert!(!frustum.intersects_sphere(&sphere, true));
477 }
478
479 fn long_frustum() -> Frustum {
481 Frustum {
482 half_spaces: [
483 HalfSpace::new(Vec4::new(-0.9998, -0.0222, -0.0000, -1.9543)),
484 HalfSpace::new(Vec4::new(-0.0000, 1.0000, -0.0000, 45.1249)),
485 HalfSpace::new(Vec4::new(-0.0000, -0.0168, -0.9999, 2.2718)),
486 HalfSpace::new(Vec4::new(-0.0000, -1.0000, -0.0000, 45.1249)),
487 HalfSpace::new(Vec4::new(-0.0000, -0.0168, 0.9999, 2.2718)),
488 HalfSpace::new(Vec4::new(0.9998, -0.0222, -0.0000, 7.9528)),
489 ],
490 }
491 }
492
493 #[test]
494 fn intersects_sphere_long_frustum_outside() {
495 let frustum = long_frustum();
497 let sphere = Sphere {
498 center: Vec3A::new(-4.4889, 46.9021, 0.0000),
499 radius: 0.7500,
500 };
501 assert!(!frustum.intersects_sphere(&sphere, true));
502 }
503
504 #[test]
505 fn intersects_sphere_long_frustum_intersect() {
506 let frustum = long_frustum();
508 let sphere = Sphere {
509 center: Vec3A::new(-4.9957, 0.0000, -0.7396),
510 radius: 4.4094,
511 };
512 assert!(frustum.intersects_sphere(&sphere, true));
513 }
514
515 #[test]
516 fn aabb_enclosing() {
517 assert_eq!(Aabb::enclosing(<[Vec3; 0]>::default()), None);
518 assert_eq!(
519 Aabb::enclosing(vec![Vec3::ONE]).unwrap(),
520 Aabb::from_min_max(Vec3::ONE, Vec3::ONE)
521 );
522 assert_eq!(
523 Aabb::enclosing(&[Vec3::Y, Vec3::X, Vec3::Z][..]).unwrap(),
524 Aabb::from_min_max(Vec3::ZERO, Vec3::ONE)
525 );
526 assert_eq!(
527 Aabb::enclosing([
528 Vec3::NEG_X,
529 Vec3::X * 2.0,
530 Vec3::NEG_Y * 5.0,
531 Vec3::Z,
532 Vec3::ZERO
533 ])
534 .unwrap(),
535 Aabb::from_min_max(Vec3::new(-1.0, -5.0, 0.0), Vec3::new(2.0, 0.0, 1.0))
536 );
537 }
538
539 fn contains_aabb_test_frustum() -> Frustum {
541 let proj = PerspectiveProjection {
542 fov: 90.0_f32.to_radians(),
543 aspect_ratio: 1.0,
544 near: 1.0,
545 far: 100.0,
546 };
547 proj.compute_frustum(&GlobalTransform::from_translation(Vec3::new(2.0, 2.0, 0.0)))
548 }
549
550 fn contains_aabb_test_frustum_with_rotation() -> Frustum {
551 let half_extent_world = (((49.5 * 49.5) * 0.5) as f32).sqrt() + 0.5f32.sqrt();
552 let near = 50.5 - half_extent_world;
553 let far = near + 2.0 * half_extent_world;
554 let fov = 2.0 * ops::atan(half_extent_world / near);
555 let proj = PerspectiveProjection {
556 aspect_ratio: 1.0,
557 near,
558 far,
559 fov,
560 };
561 proj.compute_frustum(&GlobalTransform::IDENTITY)
562 }
563
564 #[test]
565 fn aabb_inside_frustum() {
566 let frustum = contains_aabb_test_frustum();
567 let aabb = Aabb {
568 center: Vec3A::ZERO,
569 half_extents: Vec3A::new(0.99, 0.99, 49.49),
570 };
571 let model = Affine3A::from_translation(Vec3::new(2.0, 2.0, -50.5));
572 assert!(frustum.contains_aabb(&aabb, &model));
573 }
574
575 #[test]
576 fn aabb_intersect_frustum() {
577 let frustum = contains_aabb_test_frustum();
578 let aabb = Aabb {
579 center: Vec3A::ZERO,
580 half_extents: Vec3A::new(0.99, 0.99, 49.6),
581 };
582 let model = Affine3A::from_translation(Vec3::new(2.0, 2.0, -50.5));
583 assert!(!frustum.contains_aabb(&aabb, &model));
584 }
585
586 #[test]
587 fn aabb_outside_frustum() {
588 let frustum = contains_aabb_test_frustum();
589 let aabb = Aabb {
590 center: Vec3A::ZERO,
591 half_extents: Vec3A::new(0.99, 0.99, 0.99),
592 };
593 let model = Affine3A::from_translation(Vec3::new(0.0, 0.0, 49.6));
594 assert!(!frustum.contains_aabb(&aabb, &model));
595 }
596
597 #[test]
598 fn aabb_inside_frustum_rotation() {
599 let frustum = contains_aabb_test_frustum_with_rotation();
600 let aabb = Aabb {
601 center: Vec3A::new(0.0, 0.0, 0.0),
602 half_extents: Vec3A::new(0.99, 0.99, 49.49),
603 };
604
605 let model = Affine3A::from_rotation_translation(
606 Quat::from_rotation_x(PI / 4.0),
607 Vec3::new(0.0, 0.0, -50.5),
608 );
609 assert!(frustum.contains_aabb(&aabb, &model));
610 }
611
612 #[test]
613 fn aabb_intersect_frustum_rotation() {
614 let frustum = contains_aabb_test_frustum_with_rotation();
615 let aabb = Aabb {
616 center: Vec3A::new(0.0, 0.0, 0.0),
617 half_extents: Vec3A::new(0.99, 0.99, 49.6),
618 };
619
620 let model = Affine3A::from_rotation_translation(
621 Quat::from_rotation_x(PI / 4.0),
622 Vec3::new(0.0, 0.0, -50.5),
623 );
624 assert!(!frustum.contains_aabb(&aabb, &model));
625 }
626}