use super::{SpatialError, valid_distance};
use crate::collision::{Aabb2, Aabb3};
use glam::{Vec2, Vec3};
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Ray2 {
pub(crate) origin: Vec2,
pub(crate) direction: Vec2,
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Ray3 {
pub(crate) origin: Vec3,
pub(crate) direction: Vec3,
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct RayHit2 {
pub distance: f32,
pub point: Vec2,
pub normal: Vec2,
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct RayHit3 {
pub distance: f32,
pub point: Vec3,
pub normal: Vec3,
}
fn unit<const N: usize>(origin: [f32; N], direction: [f32; N]) -> Result<[f32; N], SpatialError> {
if !origin.iter().chain(direction.iter()).all(|v| v.is_finite()) {
return Err(SpatialError::InvalidRay);
}
let scale = direction
.iter()
.fold(0.0_f32, |largest, v| largest.max(v.abs()));
if scale == 0.0 {
return Err(SpatialError::InvalidRay);
}
let scaled = direction.map(|v| v / scale);
let squared: f32 = scaled.iter().map(|v| v * v).sum();
let length = squared.sqrt();
Ok(scaled.map(|v| v / length))
}
pub(crate) fn slab<const N: usize>(
origin: [f32; N],
direction: [f32; N],
min: [f32; N],
max: [f32; N],
limit: f32,
) -> Option<(f32, [f32; N])> {
let mut enter = 0.0_f32;
let mut exit = limit;
let mut normal = [0.0; N];
for axis in 0..N {
let d = direction[axis];
if d == 0.0 {
if origin[axis] < min[axis] || origin[axis] > max[axis] {
return None;
}
continue;
}
let (near, far, side) = if d > 0.0 {
(
(min[axis] - origin[axis]) / d,
(max[axis] - origin[axis]) / d,
-1.0,
)
} else {
(
(max[axis] - origin[axis]) / d,
(min[axis] - origin[axis]) / d,
1.0,
)
};
if near >= 0.0 && near >= enter {
enter = near;
normal = [0.0; N];
normal[axis] = side;
}
exit = exit.min(far);
if enter > exit {
return None;
}
}
if enter.is_finite() && exit >= 0.0 {
Some((enter, normal))
} else {
None
}
}
macro_rules! ray_impl {
($ray:ident, $hit:ident, $vec:ident, $box:ident, $n:expr) => {
impl $ray {
pub fn origin(self) -> $vec {
self.origin
}
pub fn direction(self) -> $vec {
self.direction
}
pub fn new(origin: $vec, direction: $vec) -> Result<Self, SpatialError> {
let direction =
<$vec>::from_array(unit::<$n>(origin.to_array(), direction.to_array())?);
Ok(Self { origin, direction })
}
pub fn at(self, distance: f32) -> $vec {
self.origin + self.direction * distance
}
pub fn cast(
self,
bounds: $box,
max_distance: f32,
) -> Result<Option<$hit>, SpatialError> {
valid_distance(max_distance)?;
if !bounds.min.is_finite()
|| !bounds.max.is_finite()
|| !bounds.min.cmple(bounds.max).all()
{
return Err(SpatialError::InvalidBounds(0));
}
Ok(slab::<$n>(
self.origin.to_array(),
self.direction.to_array(),
bounds.min.to_array(),
bounds.max.to_array(),
max_distance,
)
.map(|(distance, normal)| $hit {
distance,
point: self.at(distance),
normal: <$vec>::from_array(normal),
}))
}
}
};
}
ray_impl!(Ray2, RayHit2, Vec2, Aabb2, 2);
ray_impl!(Ray3, RayHit3, Vec3, Aabb3, 3);