use bevy_ecs::{component::Component, prelude::Entity, reflect::ReflectComponent};
use bevy_math::{Mat4, Vec3, Vec3A, Vec4, Vec4Swizzles};
use bevy_reflect::Reflect;
use bevy_utils::HashMap;
#[derive(Component, Clone, Copy, Debug, Default, Reflect)]
#[reflect(Component)]
pub struct Aabb {
pub center: Vec3A,
pub half_extents: Vec3A,
}
impl Aabb {
#[inline]
pub fn from_min_max(minimum: Vec3, maximum: Vec3) -> Self {
let minimum = Vec3A::from(minimum);
let maximum = Vec3A::from(maximum);
let center = 0.5 * (maximum + minimum);
let half_extents = 0.5 * (maximum - minimum);
Self {
center,
half_extents,
}
}
#[inline]
pub fn relative_radius(&self, p_normal: &Vec3A, axes: &[Vec3A]) -> f32 {
let half_extents = self.half_extents;
Vec3A::new(
p_normal.dot(axes[0]),
p_normal.dot(axes[1]),
p_normal.dot(axes[2]),
)
.abs()
.dot(half_extents)
}
#[inline]
pub fn min(&self) -> Vec3A {
self.center - self.half_extents
}
#[inline]
pub fn max(&self) -> Vec3A {
self.center + self.half_extents
}
}
impl From<Sphere> for Aabb {
#[inline]
fn from(sphere: Sphere) -> Self {
Self {
center: sphere.center,
half_extents: Vec3A::splat(sphere.radius),
}
}
}
#[derive(Clone, Debug, Default)]
pub struct Sphere {
pub center: Vec3A,
pub radius: f32,
}
impl Sphere {
#[inline]
pub fn intersects_obb(&self, aabb: &Aabb, local_to_world: &Mat4) -> bool {
let aabb_center_world = *local_to_world * aabb.center.extend(1.0);
let axes = [
Vec3A::from(local_to_world.x_axis),
Vec3A::from(local_to_world.y_axis),
Vec3A::from(local_to_world.z_axis),
];
let v = Vec3A::from(aabb_center_world) - self.center;
let d = v.length();
let relative_radius = aabb.relative_radius(&(v / d), &axes);
d < self.radius + relative_radius
}
}
#[derive(Clone, Copy, Debug, Default)]
pub struct HalfSpace {
normal_d: Vec4,
}
impl HalfSpace {
#[inline]
pub fn new(normal_d: Vec4) -> Self {
Self {
normal_d: normal_d * normal_d.xyz().length_recip(),
}
}
#[inline]
pub fn normal(&self) -> Vec3A {
Vec3A::from(self.normal_d)
}
#[inline]
pub fn d(&self) -> f32 {
self.normal_d.w
}
#[inline]
pub fn normal_d(&self) -> Vec4 {
self.normal_d
}
}
#[derive(Component, Clone, Copy, Debug, Default, Reflect)]
#[reflect(Component)]
pub struct Frustum {
#[reflect(ignore)]
pub half_spaces: [HalfSpace; 6],
}
impl Frustum {
#[inline]
pub fn from_view_projection(view_projection: &Mat4) -> Self {
let mut frustum = Frustum::from_view_projection_no_far(view_projection);
frustum.half_spaces[5] = HalfSpace::new(view_projection.row(2));
frustum
}
#[inline]
pub fn from_view_projection_custom_far(
view_projection: &Mat4,
view_translation: &Vec3,
view_backward: &Vec3,
far: f32,
) -> Self {
let mut frustum = Frustum::from_view_projection_no_far(view_projection);
let far_center = *view_translation - far * *view_backward;
frustum.half_spaces[5] =
HalfSpace::new(view_backward.extend(-view_backward.dot(far_center)));
frustum
}
fn from_view_projection_no_far(view_projection: &Mat4) -> Self {
let row3 = view_projection.row(3);
let mut half_spaces = [HalfSpace::default(); 6];
for (i, half_space) in half_spaces.iter_mut().enumerate().take(5) {
let row = view_projection.row(i / 2);
*half_space = HalfSpace::new(if (i & 1) == 0 && i != 4 {
row3 + row
} else {
row3 - row
});
}
Self { half_spaces }
}
#[inline]
pub fn intersects_sphere(&self, sphere: &Sphere, intersect_far: bool) -> bool {
let sphere_center = sphere.center.extend(1.0);
let max = if intersect_far { 6 } else { 5 };
for half_space in &self.half_spaces[..max] {
if half_space.normal_d().dot(sphere_center) + sphere.radius <= 0.0 {
return false;
}
}
true
}
#[inline]
pub fn intersects_obb(
&self,
aabb: &Aabb,
model_to_world: &Mat4,
intersect_near: bool,
intersect_far: bool,
) -> bool {
let aabb_center_world = model_to_world.transform_point3a(aabb.center).extend(1.0);
let axes = [
Vec3A::from(model_to_world.x_axis),
Vec3A::from(model_to_world.y_axis),
Vec3A::from(model_to_world.z_axis),
];
for (idx, half_space) in self.half_spaces.into_iter().enumerate() {
if idx == 4 && !intersect_near {
continue;
}
if idx == 5 && !intersect_far {
continue;
}
let p_normal = half_space.normal();
let relative_radius = aabb.relative_radius(&p_normal, &axes);
if half_space.normal_d().dot(aabb_center_world) + relative_radius <= 0.0 {
return false;
}
}
true
}
}
#[derive(Component, Debug, Default, Reflect)]
#[reflect(Component)]
pub struct CubemapFrusta {
#[reflect(ignore)]
pub frusta: [Frustum; 6],
}
impl CubemapFrusta {
pub fn iter(&self) -> impl DoubleEndedIterator<Item = &Frustum> {
self.frusta.iter()
}
pub fn iter_mut(&mut self) -> impl DoubleEndedIterator<Item = &mut Frustum> {
self.frusta.iter_mut()
}
}
#[derive(Component, Debug, Default, Reflect)]
#[reflect(Component)]
pub struct CascadesFrusta {
#[reflect(ignore)]
pub frusta: HashMap<Entity, Vec<Frustum>>,
}
#[cfg(test)]
mod tests {
use super::*;
fn big_frustum() -> Frustum {
Frustum {
half_spaces: [
HalfSpace::new(Vec4::new(-0.9701, -0.2425, -0.0000, 7.7611)),
HalfSpace::new(Vec4::new(-0.0000, 1.0000, -0.0000, 4.0000)),
HalfSpace::new(Vec4::new(-0.0000, -0.2425, -0.9701, 2.9104)),
HalfSpace::new(Vec4::new(-0.0000, -1.0000, -0.0000, 4.0000)),
HalfSpace::new(Vec4::new(-0.0000, -0.2425, 0.9701, 2.9104)),
HalfSpace::new(Vec4::new(0.9701, -0.2425, -0.0000, -1.9403)),
],
}
}
#[test]
fn intersects_sphere_big_frustum_outside() {
let frustum = big_frustum();
let sphere = Sphere {
center: Vec3A::new(0.9167, 0.0000, 0.0000),
radius: 0.7500,
};
assert!(!frustum.intersects_sphere(&sphere, true));
}
#[test]
fn intersects_sphere_big_frustum_intersect() {
let frustum = big_frustum();
let sphere = Sphere {
center: Vec3A::new(7.9288, 0.0000, 2.9728),
radius: 2.0000,
};
assert!(frustum.intersects_sphere(&sphere, true));
}
fn frustum() -> Frustum {
Frustum {
half_spaces: [
HalfSpace::new(Vec4::new(-0.9701, -0.2425, -0.0000, 0.7276)),
HalfSpace::new(Vec4::new(-0.0000, 1.0000, -0.0000, 1.0000)),
HalfSpace::new(Vec4::new(-0.0000, -0.2425, -0.9701, 0.7276)),
HalfSpace::new(Vec4::new(-0.0000, -1.0000, -0.0000, 1.0000)),
HalfSpace::new(Vec4::new(-0.0000, -0.2425, 0.9701, 0.7276)),
HalfSpace::new(Vec4::new(0.9701, -0.2425, -0.0000, 0.7276)),
],
}
}
#[test]
fn intersects_sphere_frustum_surrounding() {
let frustum = frustum();
let sphere = Sphere {
center: Vec3A::new(0.0000, 0.0000, 0.0000),
radius: 3.0000,
};
assert!(frustum.intersects_sphere(&sphere, true));
}
#[test]
fn intersects_sphere_frustum_contained() {
let frustum = frustum();
let sphere = Sphere {
center: Vec3A::new(0.0000, 0.0000, 0.0000),
radius: 0.7000,
};
assert!(frustum.intersects_sphere(&sphere, true));
}
#[test]
fn intersects_sphere_frustum_intersects_plane() {
let frustum = frustum();
let sphere = Sphere {
center: Vec3A::new(0.0000, 0.0000, 0.9695),
radius: 0.7000,
};
assert!(frustum.intersects_sphere(&sphere, true));
}
#[test]
fn intersects_sphere_frustum_intersects_2_planes() {
let frustum = frustum();
let sphere = Sphere {
center: Vec3A::new(1.2037, 0.0000, 0.9695),
radius: 0.7000,
};
assert!(frustum.intersects_sphere(&sphere, true));
}
#[test]
fn intersects_sphere_frustum_intersects_3_planes() {
let frustum = frustum();
let sphere = Sphere {
center: Vec3A::new(1.2037, -1.0988, 0.9695),
radius: 0.7000,
};
assert!(frustum.intersects_sphere(&sphere, true));
}
#[test]
fn intersects_sphere_frustum_dodges_1_plane() {
let frustum = frustum();
let sphere = Sphere {
center: Vec3A::new(-1.7020, 0.0000, 0.0000),
radius: 0.7000,
};
assert!(!frustum.intersects_sphere(&sphere, true));
}
fn long_frustum() -> Frustum {
Frustum {
half_spaces: [
HalfSpace::new(Vec4::new(-0.9998, -0.0222, -0.0000, -1.9543)),
HalfSpace::new(Vec4::new(-0.0000, 1.0000, -0.0000, 45.1249)),
HalfSpace::new(Vec4::new(-0.0000, -0.0168, -0.9999, 2.2718)),
HalfSpace::new(Vec4::new(-0.0000, -1.0000, -0.0000, 45.1249)),
HalfSpace::new(Vec4::new(-0.0000, -0.0168, 0.9999, 2.2718)),
HalfSpace::new(Vec4::new(0.9998, -0.0222, -0.0000, 7.9528)),
],
}
}
#[test]
fn intersects_sphere_long_frustum_outside() {
let frustum = long_frustum();
let sphere = Sphere {
center: Vec3A::new(-4.4889, 46.9021, 0.0000),
radius: 0.7500,
};
assert!(!frustum.intersects_sphere(&sphere, true));
}
#[test]
fn intersects_sphere_long_frustum_intersect() {
let frustum = long_frustum();
let sphere = Sphere {
center: Vec3A::new(-4.9957, 0.0000, -0.7396),
radius: 4.4094,
};
assert!(frustum.intersects_sphere(&sphere, true));
}
}