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