Use Vec3A for 3D bounding volumes and raycasts (#13087)
# Objective - People have reported bounding volumes being slower than their existing solution because it doesn't use SIMD aligned types. ## Solution - Use `Vec3A` internally for bounding volumes, accepting `Into<Vec3A>` wherever possible - Change some code to make it more likely SIMD operations are used. --- ## Changelog - Use `Vec3A` for 3D bounding volumes and raycasts ## Migration Guide - 3D bounding volumes now use `Vec3A` types internally, return values from methods on them now return `Vec3A` instead of `Vec3`
This commit is contained in:
NiseVoid
GitHub
parent
9c17fc2d8d
commit
414abb4959
@ -133,7 +133,8 @@ impl BoundingVolume for Aabb2d {
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
fn grow(&self, amount: Self::HalfSize) -> Self {
|
||||
fn grow(&self, amount: impl Into<Self::HalfSize>) -> Self {
|
||||
let amount = amount.into();
|
||||
let b = Self {
|
||||
min: self.min - amount,
|
||||
max: self.max + amount,
|
||||
@ -143,7 +144,8 @@ impl BoundingVolume for Aabb2d {
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
fn shrink(&self, amount: Self::HalfSize) -> Self {
|
||||
fn shrink(&self, amount: impl Into<Self::HalfSize>) -> Self {
|
||||
let amount = amount.into();
|
||||
let b = Self {
|
||||
min: self.min + amount,
|
||||
max: self.max - amount,
|
||||
@ -153,7 +155,8 @@ impl BoundingVolume for Aabb2d {
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
fn scale_around_center(&self, scale: Self::HalfSize) -> Self {
|
||||
fn scale_around_center(&self, scale: impl Into<Self::HalfSize>) -> Self {
|
||||
let scale = scale.into();
|
||||
let b = Self {
|
||||
min: self.center() - (self.half_size() * scale),
|
||||
max: self.center() + (self.half_size() * scale),
|
||||
@ -172,7 +175,7 @@ impl BoundingVolume for Aabb2d {
|
||||
#[inline(always)]
|
||||
fn transformed_by(
|
||||
mut self,
|
||||
translation: Self::Translation,
|
||||
translation: impl Into<Self::Translation>,
|
||||
rotation: impl Into<Self::Rotation>,
|
||||
) -> Self {
|
||||
self.transform_by(translation, rotation);
|
||||
@ -189,7 +192,7 @@ impl BoundingVolume for Aabb2d {
|
||||
#[inline(always)]
|
||||
fn transform_by(
|
||||
&mut self,
|
||||
translation: Self::Translation,
|
||||
translation: impl Into<Self::Translation>,
|
||||
rotation: impl Into<Self::Rotation>,
|
||||
) {
|
||||
self.rotate_by(rotation);
|
||||
@ -197,7 +200,8 @@ impl BoundingVolume for Aabb2d {
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
fn translate_by(&mut self, translation: Self::Translation) {
|
||||
fn translate_by(&mut self, translation: impl Into<Self::Translation>) {
|
||||
let translation = translation.into();
|
||||
self.min += translation;
|
||||
self.max += translation;
|
||||
}
|
||||
@ -557,27 +561,30 @@ impl BoundingVolume for BoundingCircle {
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
fn grow(&self, amount: Self::HalfSize) -> Self {
|
||||
fn grow(&self, amount: impl Into<Self::HalfSize>) -> Self {
|
||||
let amount = amount.into();
|
||||
debug_assert!(amount >= 0.);
|
||||
Self::new(self.center, self.radius() + amount)
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
fn shrink(&self, amount: Self::HalfSize) -> Self {
|
||||
fn shrink(&self, amount: impl Into<Self::HalfSize>) -> Self {
|
||||
let amount = amount.into();
|
||||
debug_assert!(amount >= 0.);
|
||||
debug_assert!(self.radius() >= amount);
|
||||
Self::new(self.center, self.radius() - amount)
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
fn scale_around_center(&self, scale: Self::HalfSize) -> Self {
|
||||
fn scale_around_center(&self, scale: impl Into<Self::HalfSize>) -> Self {
|
||||
let scale = scale.into();
|
||||
debug_assert!(scale >= 0.);
|
||||
Self::new(self.center, self.radius() * scale)
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
fn translate_by(&mut self, translation: Self::Translation) {
|
||||
self.center += translation;
|
||||
fn translate_by(&mut self, translation: impl Into<Self::Translation>) {
|
||||
self.center += translation.into();
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
|
||||
@ -3,18 +3,20 @@ mod primitive_impls;
|
||||
use glam::Mat3;
|
||||
|
||||
use super::{BoundingVolume, IntersectsVolume};
|
||||
use crate::prelude::{Quat, Vec3};
|
||||
use crate::{Quat, Vec3, Vec3A};
|
||||
|
||||
/// Computes the geometric center of the given set of points.
|
||||
#[inline(always)]
|
||||
fn point_cloud_3d_center(points: &[Vec3]) -> Vec3 {
|
||||
fn point_cloud_3d_center(points: impl Iterator<Item = impl Into<Vec3A>>) -> Vec3A {
|
||||
let (acc, len) = points.fold((Vec3A::ZERO, 0), |(acc, len), point| {
|
||||
(acc + point.into(), len + 1)
|
||||
});
|
||||
|
||||
assert!(
|
||||
!points.is_empty(),
|
||||
len > 0,
|
||||
"cannot compute the center of an empty set of points"
|
||||
);
|
||||
|
||||
let denom = 1.0 / points.len() as f32;
|
||||
points.iter().fold(Vec3::ZERO, |acc, point| acc + *point) * denom
|
||||
acc / len as f32
|
||||
}
|
||||
|
||||
/// A trait with methods that return 3D bounded volumes for a shape
|
||||
@ -29,15 +31,16 @@ pub trait Bounded3d {
|
||||
#[derive(Clone, Copy, Debug)]
|
||||
pub struct Aabb3d {
|
||||
/// The minimum point of the box
|
||||
pub min: Vec3,
|
||||
pub min: Vec3A,
|
||||
/// The maximum point of the box
|
||||
pub max: Vec3,
|
||||
pub max: Vec3A,
|
||||
}
|
||||
|
||||
impl Aabb3d {
|
||||
/// Constructs an AABB from its center and half-size.
|
||||
#[inline(always)]
|
||||
pub fn new(center: Vec3, half_size: Vec3) -> Self {
|
||||
pub fn new(center: impl Into<Vec3A>, half_size: impl Into<Vec3A>) -> Self {
|
||||
let (center, half_size) = (center.into(), half_size.into());
|
||||
debug_assert!(half_size.x >= 0.0 && half_size.y >= 0.0 && half_size.z >= 0.0);
|
||||
Self {
|
||||
min: center - half_size,
|
||||
@ -52,9 +55,13 @@ impl Aabb3d {
|
||||
///
|
||||
/// Panics if the given set of points is empty.
|
||||
#[inline(always)]
|
||||
pub fn from_point_cloud(translation: Vec3, rotation: Quat, points: &[Vec3]) -> Aabb3d {
|
||||
pub fn from_point_cloud(
|
||||
translation: impl Into<Vec3A>,
|
||||
rotation: Quat,
|
||||
points: impl Iterator<Item = impl Into<Vec3A>>,
|
||||
) -> Aabb3d {
|
||||
// Transform all points by rotation
|
||||
let mut iter = points.iter().map(|point| rotation * *point);
|
||||
let mut iter = points.map(|point| rotation * point.into());
|
||||
|
||||
let first = iter
|
||||
.next()
|
||||
@ -64,6 +71,7 @@ impl Aabb3d {
|
||||
(point.min(prev_min), point.max(prev_max))
|
||||
});
|
||||
|
||||
let translation = translation.into();
|
||||
Aabb3d {
|
||||
min: min + translation,
|
||||
max: max + translation,
|
||||
@ -82,16 +90,16 @@ impl Aabb3d {
|
||||
/// If the point is outside the AABB, the returned point will be on the surface of the AABB.
|
||||
/// Otherwise, it will be inside the AABB and returned as is.
|
||||
#[inline(always)]
|
||||
pub fn closest_point(&self, point: Vec3) -> Vec3 {
|
||||
pub fn closest_point(&self, point: impl Into<Vec3A>) -> Vec3A {
|
||||
// Clamp point coordinates to the AABB
|
||||
point.clamp(self.min, self.max)
|
||||
point.into().clamp(self.min, self.max)
|
||||
}
|
||||
}
|
||||
|
||||
impl BoundingVolume for Aabb3d {
|
||||
type Translation = Vec3;
|
||||
type Translation = Vec3A;
|
||||
type Rotation = Quat;
|
||||
type HalfSize = Vec3;
|
||||
type HalfSize = Vec3A;
|
||||
|
||||
#[inline(always)]
|
||||
fn center(&self) -> Self::Translation {
|
||||
@ -111,12 +119,7 @@ impl BoundingVolume for Aabb3d {
|
||||
|
||||
#[inline(always)]
|
||||
fn contains(&self, other: &Self) -> bool {
|
||||
other.min.x >= self.min.x
|
||||
&& other.min.y >= self.min.y
|
||||
&& other.min.z >= self.min.z
|
||||
&& other.max.x <= self.max.x
|
||||
&& other.max.y <= self.max.y
|
||||
&& other.max.z <= self.max.z
|
||||
other.min.cmpge(self.min).all() && other.max.cmple(self.max).all()
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
@ -128,32 +131,35 @@ impl BoundingVolume for Aabb3d {
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
fn grow(&self, amount: Self::HalfSize) -> Self {
|
||||
fn grow(&self, amount: impl Into<Self::HalfSize>) -> Self {
|
||||
let amount = amount.into();
|
||||
let b = Self {
|
||||
min: self.min - amount,
|
||||
max: self.max + amount,
|
||||
};
|
||||
debug_assert!(b.min.x <= b.max.x && b.min.y <= b.max.y && b.min.z <= b.max.z);
|
||||
debug_assert!(b.min.cmple(b.max).all());
|
||||
b
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
fn shrink(&self, amount: Self::HalfSize) -> Self {
|
||||
fn shrink(&self, amount: impl Into<Self::HalfSize>) -> Self {
|
||||
let amount = amount.into();
|
||||
let b = Self {
|
||||
min: self.min + amount,
|
||||
max: self.max - amount,
|
||||
};
|
||||
debug_assert!(b.min.x <= b.max.x && b.min.y <= b.max.y && b.min.z <= b.max.z);
|
||||
debug_assert!(b.min.cmple(b.max).all());
|
||||
b
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
fn scale_around_center(&self, scale: Self::HalfSize) -> Self {
|
||||
fn scale_around_center(&self, scale: impl Into<Self::HalfSize>) -> Self {
|
||||
let scale = scale.into();
|
||||
let b = Self {
|
||||
min: self.center() - (self.half_size() * scale),
|
||||
max: self.center() + (self.half_size() * scale),
|
||||
};
|
||||
debug_assert!(b.min.x <= b.max.x && b.min.y <= b.max.y);
|
||||
debug_assert!(b.min.cmple(b.max).all());
|
||||
b
|
||||
}
|
||||
|
||||
@ -167,7 +173,7 @@ impl BoundingVolume for Aabb3d {
|
||||
#[inline(always)]
|
||||
fn transformed_by(
|
||||
mut self,
|
||||
translation: Self::Translation,
|
||||
translation: impl Into<Self::Translation>,
|
||||
rotation: impl Into<Self::Rotation>,
|
||||
) -> Self {
|
||||
self.transform_by(translation, rotation);
|
||||
@ -184,7 +190,7 @@ impl BoundingVolume for Aabb3d {
|
||||
#[inline(always)]
|
||||
fn transform_by(
|
||||
&mut self,
|
||||
translation: Self::Translation,
|
||||
translation: impl Into<Self::Translation>,
|
||||
rotation: impl Into<Self::Rotation>,
|
||||
) {
|
||||
self.rotate_by(rotation);
|
||||
@ -192,7 +198,8 @@ impl BoundingVolume for Aabb3d {
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
fn translate_by(&mut self, translation: Self::Translation) {
|
||||
fn translate_by(&mut self, translation: impl Into<Self::Translation>) {
|
||||
let translation = translation.into();
|
||||
self.min += translation;
|
||||
self.max += translation;
|
||||
}
|
||||
@ -233,10 +240,7 @@ impl BoundingVolume for Aabb3d {
|
||||
impl IntersectsVolume<Self> for Aabb3d {
|
||||
#[inline(always)]
|
||||
fn intersects(&self, other: &Self) -> bool {
|
||||
let x_overlaps = self.min.x <= other.max.x && self.max.x >= other.min.x;
|
||||
let y_overlaps = self.min.y <= other.max.y && self.max.y >= other.min.y;
|
||||
let z_overlaps = self.min.z <= other.max.z && self.max.z >= other.min.z;
|
||||
x_overlaps && y_overlaps && z_overlaps
|
||||
self.min.cmple(other.max).all() && self.max.cmpge(other.min).all()
|
||||
}
|
||||
}
|
||||
|
||||
@ -255,42 +259,42 @@ mod aabb3d_tests {
|
||||
use super::Aabb3d;
|
||||
use crate::{
|
||||
bounding::{BoundingSphere, BoundingVolume, IntersectsVolume},
|
||||
Quat, Vec3,
|
||||
Quat, Vec3, Vec3A,
|
||||
};
|
||||
|
||||
#[test]
|
||||
fn center() {
|
||||
let aabb = Aabb3d {
|
||||
min: Vec3::new(-0.5, -1., -0.5),
|
||||
max: Vec3::new(1., 1., 2.),
|
||||
min: Vec3A::new(-0.5, -1., -0.5),
|
||||
max: Vec3A::new(1., 1., 2.),
|
||||
};
|
||||
assert!((aabb.center() - Vec3::new(0.25, 0., 0.75)).length() < std::f32::EPSILON);
|
||||
assert!((aabb.center() - Vec3A::new(0.25, 0., 0.75)).length() < std::f32::EPSILON);
|
||||
let aabb = Aabb3d {
|
||||
min: Vec3::new(5., 5., -10.),
|
||||
max: Vec3::new(10., 10., -5.),
|
||||
min: Vec3A::new(5., 5., -10.),
|
||||
max: Vec3A::new(10., 10., -5.),
|
||||
};
|
||||
assert!((aabb.center() - Vec3::new(7.5, 7.5, -7.5)).length() < std::f32::EPSILON);
|
||||
assert!((aabb.center() - Vec3A::new(7.5, 7.5, -7.5)).length() < std::f32::EPSILON);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn half_size() {
|
||||
let aabb = Aabb3d {
|
||||
min: Vec3::new(-0.5, -1., -0.5),
|
||||
max: Vec3::new(1., 1., 2.),
|
||||
min: Vec3A::new(-0.5, -1., -0.5),
|
||||
max: Vec3A::new(1., 1., 2.),
|
||||
};
|
||||
assert!((aabb.half_size() - Vec3::new(0.75, 1., 1.25)).length() < std::f32::EPSILON);
|
||||
assert!((aabb.half_size() - Vec3A::new(0.75, 1., 1.25)).length() < std::f32::EPSILON);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn area() {
|
||||
let aabb = Aabb3d {
|
||||
min: Vec3::new(-1., -1., -1.),
|
||||
max: Vec3::new(1., 1., 1.),
|
||||
min: Vec3A::new(-1., -1., -1.),
|
||||
max: Vec3A::new(1., 1., 1.),
|
||||
};
|
||||
assert!((aabb.visible_area() - 12.).abs() < std::f32::EPSILON);
|
||||
let aabb = Aabb3d {
|
||||
min: Vec3::new(0., 0., 0.),
|
||||
max: Vec3::new(1., 0.5, 0.25),
|
||||
min: Vec3A::new(0., 0., 0.),
|
||||
max: Vec3A::new(1., 0.5, 0.25),
|
||||
};
|
||||
assert!((aabb.visible_area() - 0.875).abs() < std::f32::EPSILON);
|
||||
}
|
||||
@ -298,17 +302,17 @@ mod aabb3d_tests {
|
||||
#[test]
|
||||
fn contains() {
|
||||
let a = Aabb3d {
|
||||
min: Vec3::new(-1., -1., -1.),
|
||||
max: Vec3::new(1., 1., 1.),
|
||||
min: Vec3A::new(-1., -1., -1.),
|
||||
max: Vec3A::new(1., 1., 1.),
|
||||
};
|
||||
let b = Aabb3d {
|
||||
min: Vec3::new(-2., -1., -1.),
|
||||
max: Vec3::new(1., 1., 1.),
|
||||
min: Vec3A::new(-2., -1., -1.),
|
||||
max: Vec3A::new(1., 1., 1.),
|
||||
};
|
||||
assert!(!a.contains(&b));
|
||||
let b = Aabb3d {
|
||||
min: Vec3::new(-0.25, -0.8, -0.9),
|
||||
max: Vec3::new(1., 1., 0.9),
|
||||
min: Vec3A::new(-0.25, -0.8, -0.9),
|
||||
max: Vec3A::new(1., 1., 0.9),
|
||||
};
|
||||
assert!(a.contains(&b));
|
||||
}
|
||||
@ -316,16 +320,16 @@ mod aabb3d_tests {
|
||||
#[test]
|
||||
fn merge() {
|
||||
let a = Aabb3d {
|
||||
min: Vec3::new(-1., -1., -1.),
|
||||
max: Vec3::new(1., 0.5, 1.),
|
||||
min: Vec3A::new(-1., -1., -1.),
|
||||
max: Vec3A::new(1., 0.5, 1.),
|
||||
};
|
||||
let b = Aabb3d {
|
||||
min: Vec3::new(-2., -0.5, -0.),
|
||||
max: Vec3::new(0.75, 1., 2.),
|
||||
min: Vec3A::new(-2., -0.5, -0.),
|
||||
max: Vec3A::new(0.75, 1., 2.),
|
||||
};
|
||||
let merged = a.merge(&b);
|
||||
assert!((merged.min - Vec3::new(-2., -1., -1.)).length() < std::f32::EPSILON);
|
||||
assert!((merged.max - Vec3::new(1., 1., 2.)).length() < std::f32::EPSILON);
|
||||
assert!((merged.min - Vec3A::new(-2., -1., -1.)).length() < std::f32::EPSILON);
|
||||
assert!((merged.max - Vec3A::new(1., 1., 2.)).length() < std::f32::EPSILON);
|
||||
assert!(merged.contains(&a));
|
||||
assert!(merged.contains(&b));
|
||||
assert!(!a.contains(&merged));
|
||||
@ -335,12 +339,12 @@ mod aabb3d_tests {
|
||||
#[test]
|
||||
fn grow() {
|
||||
let a = Aabb3d {
|
||||
min: Vec3::new(-1., -1., -1.),
|
||||
max: Vec3::new(1., 1., 1.),
|
||||
min: Vec3A::new(-1., -1., -1.),
|
||||
max: Vec3A::new(1., 1., 1.),
|
||||
};
|
||||
let padded = a.grow(Vec3::ONE);
|
||||
assert!((padded.min - Vec3::new(-2., -2., -2.)).length() < std::f32::EPSILON);
|
||||
assert!((padded.max - Vec3::new(2., 2., 2.)).length() < std::f32::EPSILON);
|
||||
let padded = a.grow(Vec3A::ONE);
|
||||
assert!((padded.min - Vec3A::new(-2., -2., -2.)).length() < std::f32::EPSILON);
|
||||
assert!((padded.max - Vec3A::new(2., 2., 2.)).length() < std::f32::EPSILON);
|
||||
assert!(padded.contains(&a));
|
||||
assert!(!a.contains(&padded));
|
||||
}
|
||||
@ -348,12 +352,12 @@ mod aabb3d_tests {
|
||||
#[test]
|
||||
fn shrink() {
|
||||
let a = Aabb3d {
|
||||
min: Vec3::new(-2., -2., -2.),
|
||||
max: Vec3::new(2., 2., 2.),
|
||||
min: Vec3A::new(-2., -2., -2.),
|
||||
max: Vec3A::new(2., 2., 2.),
|
||||
};
|
||||
let shrunk = a.shrink(Vec3::ONE);
|
||||
assert!((shrunk.min - Vec3::new(-1., -1., -1.)).length() < std::f32::EPSILON);
|
||||
assert!((shrunk.max - Vec3::new(1., 1., 1.)).length() < std::f32::EPSILON);
|
||||
let shrunk = a.shrink(Vec3A::ONE);
|
||||
assert!((shrunk.min - Vec3A::new(-1., -1., -1.)).length() < std::f32::EPSILON);
|
||||
assert!((shrunk.max - Vec3A::new(1., 1., 1.)).length() < std::f32::EPSILON);
|
||||
assert!(a.contains(&shrunk));
|
||||
assert!(!shrunk.contains(&a));
|
||||
}
|
||||
@ -361,12 +365,12 @@ mod aabb3d_tests {
|
||||
#[test]
|
||||
fn scale_around_center() {
|
||||
let a = Aabb3d {
|
||||
min: Vec3::NEG_ONE,
|
||||
max: Vec3::ONE,
|
||||
min: Vec3A::NEG_ONE,
|
||||
max: Vec3A::ONE,
|
||||
};
|
||||
let scaled = a.scale_around_center(Vec3::splat(2.));
|
||||
assert!((scaled.min - Vec3::splat(-2.)).length() < std::f32::EPSILON);
|
||||
assert!((scaled.max - Vec3::splat(2.)).length() < std::f32::EPSILON);
|
||||
let scaled = a.scale_around_center(Vec3A::splat(2.));
|
||||
assert!((scaled.min - Vec3A::splat(-2.)).length() < std::f32::EPSILON);
|
||||
assert!((scaled.max - Vec3A::splat(2.)).length() < std::f32::EPSILON);
|
||||
assert!(!a.contains(&scaled));
|
||||
assert!(scaled.contains(&a));
|
||||
}
|
||||
@ -374,64 +378,64 @@ mod aabb3d_tests {
|
||||
#[test]
|
||||
fn transform() {
|
||||
let a = Aabb3d {
|
||||
min: Vec3::new(-2.0, -2.0, -2.0),
|
||||
max: Vec3::new(2.0, 2.0, 2.0),
|
||||
min: Vec3A::new(-2.0, -2.0, -2.0),
|
||||
max: Vec3A::new(2.0, 2.0, 2.0),
|
||||
};
|
||||
let transformed = a.transformed_by(
|
||||
Vec3::new(2.0, -2.0, 4.0),
|
||||
Vec3A::new(2.0, -2.0, 4.0),
|
||||
Quat::from_rotation_z(std::f32::consts::FRAC_PI_4),
|
||||
);
|
||||
let half_length = 2_f32.hypot(2.0);
|
||||
assert_eq!(
|
||||
transformed.min,
|
||||
Vec3::new(2.0 - half_length, -half_length - 2.0, 2.0)
|
||||
Vec3A::new(2.0 - half_length, -half_length - 2.0, 2.0)
|
||||
);
|
||||
assert_eq!(
|
||||
transformed.max,
|
||||
Vec3::new(2.0 + half_length, half_length - 2.0, 6.0)
|
||||
Vec3A::new(2.0 + half_length, half_length - 2.0, 6.0)
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn closest_point() {
|
||||
let aabb = Aabb3d {
|
||||
min: Vec3::NEG_ONE,
|
||||
max: Vec3::ONE,
|
||||
min: Vec3A::NEG_ONE,
|
||||
max: Vec3A::ONE,
|
||||
};
|
||||
assert_eq!(aabb.closest_point(Vec3::X * 10.0), Vec3::X);
|
||||
assert_eq!(aabb.closest_point(Vec3::NEG_ONE * 10.0), Vec3::NEG_ONE);
|
||||
assert_eq!(aabb.closest_point(Vec3A::X * 10.0), Vec3A::X);
|
||||
assert_eq!(aabb.closest_point(Vec3A::NEG_ONE * 10.0), Vec3A::NEG_ONE);
|
||||
assert_eq!(
|
||||
aabb.closest_point(Vec3::new(0.25, 0.1, 0.3)),
|
||||
Vec3::new(0.25, 0.1, 0.3)
|
||||
aabb.closest_point(Vec3A::new(0.25, 0.1, 0.3)),
|
||||
Vec3A::new(0.25, 0.1, 0.3)
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn intersect_aabb() {
|
||||
let aabb = Aabb3d {
|
||||
min: Vec3::NEG_ONE,
|
||||
max: Vec3::ONE,
|
||||
min: Vec3A::NEG_ONE,
|
||||
max: Vec3A::ONE,
|
||||
};
|
||||
assert!(aabb.intersects(&aabb));
|
||||
assert!(aabb.intersects(&Aabb3d {
|
||||
min: Vec3::splat(0.5),
|
||||
max: Vec3::splat(2.0),
|
||||
min: Vec3A::splat(0.5),
|
||||
max: Vec3A::splat(2.0),
|
||||
}));
|
||||
assert!(aabb.intersects(&Aabb3d {
|
||||
min: Vec3::splat(-2.0),
|
||||
max: Vec3::splat(-0.5),
|
||||
min: Vec3A::splat(-2.0),
|
||||
max: Vec3A::splat(-0.5),
|
||||
}));
|
||||
assert!(!aabb.intersects(&Aabb3d {
|
||||
min: Vec3::new(1.1, 0.0, 0.0),
|
||||
max: Vec3::new(2.0, 0.5, 0.25),
|
||||
min: Vec3A::new(1.1, 0.0, 0.0),
|
||||
max: Vec3A::new(2.0, 0.5, 0.25),
|
||||
}));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn intersect_bounding_sphere() {
|
||||
let aabb = Aabb3d {
|
||||
min: Vec3::NEG_ONE,
|
||||
max: Vec3::ONE,
|
||||
min: Vec3A::NEG_ONE,
|
||||
max: Vec3A::ONE,
|
||||
};
|
||||
assert!(aabb.intersects(&BoundingSphere::new(Vec3::ZERO, 1.0)));
|
||||
assert!(aabb.intersects(&BoundingSphere::new(Vec3::ONE * 1.5, 1.0)));
|
||||
@ -446,17 +450,17 @@ use crate::primitives::Sphere;
|
||||
#[derive(Clone, Copy, Debug)]
|
||||
pub struct BoundingSphere {
|
||||
/// The center of the bounding sphere
|
||||
pub center: Vec3,
|
||||
pub center: Vec3A,
|
||||
/// The sphere
|
||||
pub sphere: Sphere,
|
||||
}
|
||||
|
||||
impl BoundingSphere {
|
||||
/// Constructs a bounding sphere from its center and radius.
|
||||
pub fn new(center: Vec3, radius: f32) -> Self {
|
||||
pub fn new(center: impl Into<Vec3A>, radius: f32) -> Self {
|
||||
debug_assert!(radius >= 0.);
|
||||
Self {
|
||||
center,
|
||||
center: center.into(),
|
||||
sphere: Sphere { radius },
|
||||
}
|
||||
}
|
||||
@ -466,19 +470,26 @@ impl BoundingSphere {
|
||||
///
|
||||
/// The bounding sphere is not guaranteed to be the smallest possible.
|
||||
#[inline(always)]
|
||||
pub fn from_point_cloud(translation: Vec3, rotation: Quat, points: &[Vec3]) -> BoundingSphere {
|
||||
let center = point_cloud_3d_center(points);
|
||||
let mut radius_squared = 0.0;
|
||||
pub fn from_point_cloud(
|
||||
translation: impl Into<Vec3A>,
|
||||
rotation: Quat,
|
||||
points: &[impl Copy + Into<Vec3A>],
|
||||
) -> BoundingSphere {
|
||||
let center = point_cloud_3d_center(points.iter().map(|v| Into::<Vec3A>::into(*v)));
|
||||
let mut radius_squared: f32 = 0.0;
|
||||
|
||||
for point in points {
|
||||
// Get squared version to avoid unnecessary sqrt calls
|
||||
let distance_squared = point.distance_squared(center);
|
||||
let distance_squared = Into::<Vec3A>::into(*point).distance_squared(center);
|
||||
if distance_squared > radius_squared {
|
||||
radius_squared = distance_squared;
|
||||
}
|
||||
}
|
||||
|
||||
BoundingSphere::new(rotation * center + translation, radius_squared.sqrt())
|
||||
BoundingSphere::new(
|
||||
rotation * center + translation.into(),
|
||||
radius_squared.sqrt(),
|
||||
)
|
||||
}
|
||||
|
||||
/// Get the radius of the bounding sphere
|
||||
@ -491,8 +502,8 @@ impl BoundingSphere {
|
||||
#[inline(always)]
|
||||
pub fn aabb_3d(&self) -> Aabb3d {
|
||||
Aabb3d {
|
||||
min: self.center - Vec3::splat(self.radius()),
|
||||
max: self.center + Vec3::splat(self.radius()),
|
||||
min: self.center - self.radius(),
|
||||
max: self.center + self.radius(),
|
||||
}
|
||||
}
|
||||
|
||||
@ -501,13 +512,25 @@ impl BoundingSphere {
|
||||
/// If the point is outside the sphere, the returned point will be on the surface of the sphere.
|
||||
/// Otherwise, it will be inside the sphere and returned as is.
|
||||
#[inline(always)]
|
||||
pub fn closest_point(&self, point: Vec3) -> Vec3 {
|
||||
self.sphere.closest_point(point - self.center) + self.center
|
||||
pub fn closest_point(&self, point: impl Into<Vec3A>) -> Vec3A {
|
||||
let point = point.into();
|
||||
let radius = self.radius();
|
||||
let distance_squared = (point - self.center).length_squared();
|
||||
|
||||
if distance_squared <= radius.powi(2) {
|
||||
// The point is inside the sphere.
|
||||
point
|
||||
} else {
|
||||
// The point is outside the sphere.
|
||||
// Find the closest point on the surface of the sphere.
|
||||
let dir_to_point = point / distance_squared.sqrt();
|
||||
self.center + radius * dir_to_point
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl BoundingVolume for BoundingSphere {
|
||||
type Translation = Vec3;
|
||||
type Translation = Vec3A;
|
||||
type Rotation = Quat;
|
||||
type HalfSize = f32;
|
||||
|
||||
@ -550,7 +573,8 @@ impl BoundingVolume for BoundingSphere {
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
fn grow(&self, amount: Self::HalfSize) -> Self {
|
||||
fn grow(&self, amount: impl Into<Self::HalfSize>) -> Self {
|
||||
let amount = amount.into();
|
||||
debug_assert!(amount >= 0.);
|
||||
Self {
|
||||
center: self.center,
|
||||
@ -561,7 +585,8 @@ impl BoundingVolume for BoundingSphere {
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
fn shrink(&self, amount: Self::HalfSize) -> Self {
|
||||
fn shrink(&self, amount: impl Into<Self::HalfSize>) -> Self {
|
||||
let amount = amount.into();
|
||||
debug_assert!(amount >= 0.);
|
||||
debug_assert!(self.radius() >= amount);
|
||||
Self {
|
||||
@ -573,14 +598,15 @@ impl BoundingVolume for BoundingSphere {
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
fn scale_around_center(&self, scale: Self::HalfSize) -> Self {
|
||||
fn scale_around_center(&self, scale: impl Into<Self::HalfSize>) -> Self {
|
||||
let scale = scale.into();
|
||||
debug_assert!(scale >= 0.);
|
||||
Self::new(self.center, self.radius() * scale)
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
fn translate_by(&mut self, translation: Self::Translation) {
|
||||
self.center += translation;
|
||||
fn translate_by(&mut self, translation: impl Into<Self::Translation>) {
|
||||
self.center += translation.into();
|
||||
}
|
||||
|
||||
#[inline(always)]
|
||||
@ -613,7 +639,7 @@ mod bounding_sphere_tests {
|
||||
use super::BoundingSphere;
|
||||
use crate::{
|
||||
bounding::{BoundingVolume, IntersectsVolume},
|
||||
Quat, Vec3,
|
||||
Quat, Vec3, Vec3A,
|
||||
};
|
||||
|
||||
#[test]
|
||||
@ -645,7 +671,7 @@ mod bounding_sphere_tests {
|
||||
let a = BoundingSphere::new(Vec3::ONE, 5.);
|
||||
let b = BoundingSphere::new(Vec3::new(1., 1., -4.), 1.);
|
||||
let merged = a.merge(&b);
|
||||
assert!((merged.center - Vec3::new(1., 1., 0.5)).length() < std::f32::EPSILON);
|
||||
assert!((merged.center - Vec3A::new(1., 1., 0.5)).length() < std::f32::EPSILON);
|
||||
assert!((merged.radius() - 5.5).abs() < std::f32::EPSILON);
|
||||
assert!(merged.contains(&a));
|
||||
assert!(merged.contains(&b));
|
||||
@ -710,7 +736,7 @@ mod bounding_sphere_tests {
|
||||
);
|
||||
assert_relative_eq!(
|
||||
transformed.center,
|
||||
Vec3::new(2.0, std::f32::consts::SQRT_2 - 2.0, 5.0)
|
||||
Vec3A::new(2.0, std::f32::consts::SQRT_2 - 2.0, 5.0)
|
||||
);
|
||||
assert_eq!(transformed.radius(), 5.0);
|
||||
}
|
||||
@ -718,14 +744,14 @@ mod bounding_sphere_tests {
|
||||
#[test]
|
||||
fn closest_point() {
|
||||
let sphere = BoundingSphere::new(Vec3::ZERO, 1.0);
|
||||
assert_eq!(sphere.closest_point(Vec3::X * 10.0), Vec3::X);
|
||||
assert_eq!(sphere.closest_point(Vec3::X * 10.0), Vec3A::X);
|
||||
assert_eq!(
|
||||
sphere.closest_point(Vec3::NEG_ONE * 10.0),
|
||||
Vec3::NEG_ONE.normalize()
|
||||
Vec3A::NEG_ONE.normalize()
|
||||
);
|
||||
assert_eq!(
|
||||
sphere.closest_point(Vec3::new(0.25, 0.1, 0.3)),
|
||||
Vec3::new(0.25, 0.1, 0.3)
|
||||
Vec3A::new(0.25, 0.1, 0.3)
|
||||
);
|
||||
}
|
||||
|
||||
|
||||
@ -79,7 +79,7 @@ impl Bounded3d for Segment3d {
|
||||
|
||||
impl<const N: usize> Bounded3d for Polyline3d<N> {
|
||||
fn aabb_3d(&self, translation: Vec3, rotation: Quat) -> Aabb3d {
|
||||
Aabb3d::from_point_cloud(translation, rotation, &self.vertices)
|
||||
Aabb3d::from_point_cloud(translation, rotation, self.vertices.iter().copied())
|
||||
}
|
||||
|
||||
fn bounding_sphere(&self, translation: Vec3, rotation: Quat) -> BoundingSphere {
|
||||
@ -89,7 +89,7 @@ impl<const N: usize> Bounded3d for Polyline3d<N> {
|
||||
|
||||
impl Bounded3d for BoxedPolyline3d {
|
||||
fn aabb_3d(&self, translation: Vec3, rotation: Quat) -> Aabb3d {
|
||||
Aabb3d::from_point_cloud(translation, rotation, &self.vertices)
|
||||
Aabb3d::from_point_cloud(translation, rotation, self.vertices.iter().copied())
|
||||
}
|
||||
|
||||
fn bounding_sphere(&self, translation: Vec3, rotation: Quat) -> BoundingSphere {
|
||||
@ -113,12 +113,7 @@ impl Bounded3d for Cuboid {
|
||||
}
|
||||
|
||||
fn bounding_sphere(&self, translation: Vec3, _rotation: Quat) -> BoundingSphere {
|
||||
BoundingSphere {
|
||||
center: translation,
|
||||
sphere: Sphere {
|
||||
radius: self.half_size.length(),
|
||||
},
|
||||
}
|
||||
BoundingSphere::new(translation, self.half_size.length())
|
||||
}
|
||||
}
|
||||
|
||||
@ -134,8 +129,8 @@ impl Bounded3d for Cylinder {
|
||||
let half_size = self.radius * Vec3::new(e.x.sqrt(), e.y.sqrt(), e.z.sqrt());
|
||||
|
||||
Aabb3d {
|
||||
min: translation + (top - half_size).min(bottom - half_size),
|
||||
max: translation + (top + half_size).max(bottom + half_size),
|
||||
min: (translation + (top - half_size).min(bottom - half_size)).into(),
|
||||
max: (translation + (top + half_size).max(bottom + half_size)).into(),
|
||||
}
|
||||
}
|
||||
|
||||
@ -160,8 +155,8 @@ impl Bounded3d for Capsule3d {
|
||||
let max = a.max(b) + Vec3::splat(self.radius);
|
||||
|
||||
Aabb3d {
|
||||
min: min + translation,
|
||||
max: max + translation,
|
||||
min: (min + translation).into(),
|
||||
max: (max + translation).into(),
|
||||
}
|
||||
}
|
||||
|
||||
@ -182,8 +177,8 @@ impl Bounded3d for Cone {
|
||||
let half_extents = Vec3::new(e.x.sqrt(), e.y.sqrt(), e.z.sqrt());
|
||||
|
||||
Aabb3d {
|
||||
min: translation + top.min(bottom - self.radius * half_extents),
|
||||
max: translation + top.max(bottom + self.radius * half_extents),
|
||||
min: (translation + top.min(bottom - self.radius * half_extents)).into(),
|
||||
max: (translation + top.max(bottom + self.radius * half_extents)).into(),
|
||||
}
|
||||
}
|
||||
|
||||
@ -216,12 +211,14 @@ impl Bounded3d for ConicalFrustum {
|
||||
let half_extents = Vec3::new(e.x.sqrt(), e.y.sqrt(), e.z.sqrt());
|
||||
|
||||
Aabb3d {
|
||||
min: translation
|
||||
min: (translation
|
||||
+ (top - self.radius_top * half_extents)
|
||||
.min(bottom - self.radius_bottom * half_extents),
|
||||
max: translation
|
||||
.min(bottom - self.radius_bottom * half_extents))
|
||||
.into(),
|
||||
max: (translation
|
||||
+ (top + self.radius_top * half_extents)
|
||||
.max(bottom + self.radius_bottom * half_extents),
|
||||
.max(bottom + self.radius_bottom * half_extents))
|
||||
.into(),
|
||||
}
|
||||
}
|
||||
|
||||
@ -358,7 +355,7 @@ impl Bounded3d for Triangle3d {
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use glam::{Quat, Vec3};
|
||||
use glam::{Quat, Vec3, Vec3A};
|
||||
|
||||
use crate::{
|
||||
bounding::Bounded3d,
|
||||
@ -375,11 +372,11 @@ mod tests {
|
||||
let translation = Vec3::new(2.0, 1.0, 0.0);
|
||||
|
||||
let aabb = sphere.aabb_3d(translation, Quat::IDENTITY);
|
||||
assert_eq!(aabb.min, Vec3::new(1.0, 0.0, -1.0));
|
||||
assert_eq!(aabb.max, Vec3::new(3.0, 2.0, 1.0));
|
||||
assert_eq!(aabb.min, Vec3A::new(1.0, 0.0, -1.0));
|
||||
assert_eq!(aabb.max, Vec3A::new(3.0, 2.0, 1.0));
|
||||
|
||||
let bounding_sphere = sphere.bounding_sphere(translation, Quat::IDENTITY);
|
||||
assert_eq!(bounding_sphere.center, translation);
|
||||
assert_eq!(bounding_sphere.center, translation.into());
|
||||
assert_eq!(bounding_sphere.radius(), 1.0);
|
||||
}
|
||||
|
||||
@ -388,24 +385,24 @@ mod tests {
|
||||
let translation = Vec3::new(2.0, 1.0, 0.0);
|
||||
|
||||
let aabb1 = InfinitePlane3d::new(Vec3::X).aabb_3d(translation, Quat::IDENTITY);
|
||||
assert_eq!(aabb1.min, Vec3::new(2.0, -f32::MAX / 2.0, -f32::MAX / 2.0));
|
||||
assert_eq!(aabb1.max, Vec3::new(2.0, f32::MAX / 2.0, f32::MAX / 2.0));
|
||||
assert_eq!(aabb1.min, Vec3A::new(2.0, -f32::MAX / 2.0, -f32::MAX / 2.0));
|
||||
assert_eq!(aabb1.max, Vec3A::new(2.0, f32::MAX / 2.0, f32::MAX / 2.0));
|
||||
|
||||
let aabb2 = InfinitePlane3d::new(Vec3::Y).aabb_3d(translation, Quat::IDENTITY);
|
||||
assert_eq!(aabb2.min, Vec3::new(-f32::MAX / 2.0, 1.0, -f32::MAX / 2.0));
|
||||
assert_eq!(aabb2.max, Vec3::new(f32::MAX / 2.0, 1.0, f32::MAX / 2.0));
|
||||
assert_eq!(aabb2.min, Vec3A::new(-f32::MAX / 2.0, 1.0, -f32::MAX / 2.0));
|
||||
assert_eq!(aabb2.max, Vec3A::new(f32::MAX / 2.0, 1.0, f32::MAX / 2.0));
|
||||
|
||||
let aabb3 = InfinitePlane3d::new(Vec3::Z).aabb_3d(translation, Quat::IDENTITY);
|
||||
assert_eq!(aabb3.min, Vec3::new(-f32::MAX / 2.0, -f32::MAX / 2.0, 0.0));
|
||||
assert_eq!(aabb3.max, Vec3::new(f32::MAX / 2.0, f32::MAX / 2.0, 0.0));
|
||||
assert_eq!(aabb3.min, Vec3A::new(-f32::MAX / 2.0, -f32::MAX / 2.0, 0.0));
|
||||
assert_eq!(aabb3.max, Vec3A::new(f32::MAX / 2.0, f32::MAX / 2.0, 0.0));
|
||||
|
||||
let aabb4 = InfinitePlane3d::new(Vec3::ONE).aabb_3d(translation, Quat::IDENTITY);
|
||||
assert_eq!(aabb4.min, Vec3::splat(-f32::MAX / 2.0));
|
||||
assert_eq!(aabb4.max, Vec3::splat(f32::MAX / 2.0));
|
||||
assert_eq!(aabb4.min, Vec3A::splat(-f32::MAX / 2.0));
|
||||
assert_eq!(aabb4.max, Vec3A::splat(f32::MAX / 2.0));
|
||||
|
||||
let bounding_sphere =
|
||||
InfinitePlane3d::new(Vec3::Y).bounding_sphere(translation, Quat::IDENTITY);
|
||||
assert_eq!(bounding_sphere.center, translation);
|
||||
assert_eq!(bounding_sphere.center, translation.into());
|
||||
assert_eq!(bounding_sphere.radius(), f32::MAX / 2.0);
|
||||
}
|
||||
|
||||
@ -414,27 +411,27 @@ mod tests {
|
||||
let translation = Vec3::new(2.0, 1.0, 0.0);
|
||||
|
||||
let aabb1 = Line3d { direction: Dir3::Y }.aabb_3d(translation, Quat::IDENTITY);
|
||||
assert_eq!(aabb1.min, Vec3::new(2.0, -f32::MAX / 2.0, 0.0));
|
||||
assert_eq!(aabb1.max, Vec3::new(2.0, f32::MAX / 2.0, 0.0));
|
||||
assert_eq!(aabb1.min, Vec3A::new(2.0, -f32::MAX / 2.0, 0.0));
|
||||
assert_eq!(aabb1.max, Vec3A::new(2.0, f32::MAX / 2.0, 0.0));
|
||||
|
||||
let aabb2 = Line3d { direction: Dir3::X }.aabb_3d(translation, Quat::IDENTITY);
|
||||
assert_eq!(aabb2.min, Vec3::new(-f32::MAX / 2.0, 1.0, 0.0));
|
||||
assert_eq!(aabb2.max, Vec3::new(f32::MAX / 2.0, 1.0, 0.0));
|
||||
assert_eq!(aabb2.min, Vec3A::new(-f32::MAX / 2.0, 1.0, 0.0));
|
||||
assert_eq!(aabb2.max, Vec3A::new(f32::MAX / 2.0, 1.0, 0.0));
|
||||
|
||||
let aabb3 = Line3d { direction: Dir3::Z }.aabb_3d(translation, Quat::IDENTITY);
|
||||
assert_eq!(aabb3.min, Vec3::new(2.0, 1.0, -f32::MAX / 2.0));
|
||||
assert_eq!(aabb3.max, Vec3::new(2.0, 1.0, f32::MAX / 2.0));
|
||||
assert_eq!(aabb3.min, Vec3A::new(2.0, 1.0, -f32::MAX / 2.0));
|
||||
assert_eq!(aabb3.max, Vec3A::new(2.0, 1.0, f32::MAX / 2.0));
|
||||
|
||||
let aabb4 = Line3d {
|
||||
direction: Dir3::from_xyz(1.0, 1.0, 1.0).unwrap(),
|
||||
}
|
||||
.aabb_3d(translation, Quat::IDENTITY);
|
||||
assert_eq!(aabb4.min, Vec3::splat(-f32::MAX / 2.0));
|
||||
assert_eq!(aabb4.max, Vec3::splat(f32::MAX / 2.0));
|
||||
assert_eq!(aabb4.min, Vec3A::splat(-f32::MAX / 2.0));
|
||||
assert_eq!(aabb4.max, Vec3A::splat(f32::MAX / 2.0));
|
||||
|
||||
let bounding_sphere =
|
||||
Line3d { direction: Dir3::Y }.bounding_sphere(translation, Quat::IDENTITY);
|
||||
assert_eq!(bounding_sphere.center, translation);
|
||||
assert_eq!(bounding_sphere.center, translation.into());
|
||||
assert_eq!(bounding_sphere.radius(), f32::MAX / 2.0);
|
||||
}
|
||||
|
||||
@ -445,11 +442,11 @@ mod tests {
|
||||
Segment3d::from_points(Vec3::new(-1.0, -0.5, 0.0), Vec3::new(1.0, 0.5, 0.0)).0;
|
||||
|
||||
let aabb = segment.aabb_3d(translation, Quat::IDENTITY);
|
||||
assert_eq!(aabb.min, Vec3::new(1.0, 0.5, 0.0));
|
||||
assert_eq!(aabb.max, Vec3::new(3.0, 1.5, 0.0));
|
||||
assert_eq!(aabb.min, Vec3A::new(1.0, 0.5, 0.0));
|
||||
assert_eq!(aabb.max, Vec3A::new(3.0, 1.5, 0.0));
|
||||
|
||||
let bounding_sphere = segment.bounding_sphere(translation, Quat::IDENTITY);
|
||||
assert_eq!(bounding_sphere.center, translation);
|
||||
assert_eq!(bounding_sphere.center, translation.into());
|
||||
assert_eq!(bounding_sphere.radius(), 1.0_f32.hypot(0.5));
|
||||
}
|
||||
|
||||
@ -464,11 +461,11 @@ mod tests {
|
||||
let translation = Vec3::new(2.0, 1.0, 0.0);
|
||||
|
||||
let aabb = polyline.aabb_3d(translation, Quat::IDENTITY);
|
||||
assert_eq!(aabb.min, Vec3::new(1.0, 0.0, -1.0));
|
||||
assert_eq!(aabb.max, Vec3::new(3.0, 2.0, 1.0));
|
||||
assert_eq!(aabb.min, Vec3A::new(1.0, 0.0, -1.0));
|
||||
assert_eq!(aabb.max, Vec3A::new(3.0, 2.0, 1.0));
|
||||
|
||||
let bounding_sphere = polyline.bounding_sphere(translation, Quat::IDENTITY);
|
||||
assert_eq!(bounding_sphere.center, translation);
|
||||
assert_eq!(bounding_sphere.center, translation.into());
|
||||
assert_eq!(bounding_sphere.radius(), 1.0_f32.hypot(1.0).hypot(1.0));
|
||||
}
|
||||
|
||||
@ -481,12 +478,12 @@ mod tests {
|
||||
translation,
|
||||
Quat::from_rotation_z(std::f32::consts::FRAC_PI_4),
|
||||
);
|
||||
let expected_half_size = Vec3::new(1.0606601, 1.0606601, 0.5);
|
||||
assert_eq!(aabb.min, translation - expected_half_size);
|
||||
assert_eq!(aabb.max, translation + expected_half_size);
|
||||
let expected_half_size = Vec3A::new(1.0606601, 1.0606601, 0.5);
|
||||
assert_eq!(aabb.min, Vec3A::from(translation) - expected_half_size);
|
||||
assert_eq!(aabb.max, Vec3A::from(translation) + expected_half_size);
|
||||
|
||||
let bounding_sphere = cuboid.bounding_sphere(translation, Quat::IDENTITY);
|
||||
assert_eq!(bounding_sphere.center, translation);
|
||||
assert_eq!(bounding_sphere.center, translation.into());
|
||||
assert_eq!(bounding_sphere.radius(), 1.0_f32.hypot(0.5).hypot(0.5));
|
||||
}
|
||||
|
||||
@ -496,11 +493,17 @@ mod tests {
|
||||
let translation = Vec3::new(2.0, 1.0, 0.0);
|
||||
|
||||
let aabb = cylinder.aabb_3d(translation, Quat::IDENTITY);
|
||||
assert_eq!(aabb.min, translation - Vec3::new(0.5, 1.0, 0.5));
|
||||
assert_eq!(aabb.max, translation + Vec3::new(0.5, 1.0, 0.5));
|
||||
assert_eq!(
|
||||
aabb.min,
|
||||
Vec3A::from(translation) - Vec3A::new(0.5, 1.0, 0.5)
|
||||
);
|
||||
assert_eq!(
|
||||
aabb.max,
|
||||
Vec3A::from(translation) + Vec3A::new(0.5, 1.0, 0.5)
|
||||
);
|
||||
|
||||
let bounding_sphere = cylinder.bounding_sphere(translation, Quat::IDENTITY);
|
||||
assert_eq!(bounding_sphere.center, translation);
|
||||
assert_eq!(bounding_sphere.center, translation.into());
|
||||
assert_eq!(bounding_sphere.radius(), 1.0_f32.hypot(0.5));
|
||||
}
|
||||
|
||||
@ -510,11 +513,17 @@ mod tests {
|
||||
let translation = Vec3::new(2.0, 1.0, 0.0);
|
||||
|
||||
let aabb = capsule.aabb_3d(translation, Quat::IDENTITY);
|
||||
assert_eq!(aabb.min, translation - Vec3::new(0.5, 1.5, 0.5));
|
||||
assert_eq!(aabb.max, translation + Vec3::new(0.5, 1.5, 0.5));
|
||||
assert_eq!(
|
||||
aabb.min,
|
||||
Vec3A::from(translation) - Vec3A::new(0.5, 1.5, 0.5)
|
||||
);
|
||||
assert_eq!(
|
||||
aabb.max,
|
||||
Vec3A::from(translation) + Vec3A::new(0.5, 1.5, 0.5)
|
||||
);
|
||||
|
||||
let bounding_sphere = capsule.bounding_sphere(translation, Quat::IDENTITY);
|
||||
assert_eq!(bounding_sphere.center, translation);
|
||||
assert_eq!(bounding_sphere.center, translation.into());
|
||||
assert_eq!(bounding_sphere.radius(), 1.5);
|
||||
}
|
||||
|
||||
@ -527,11 +536,14 @@ mod tests {
|
||||
let translation = Vec3::new(2.0, 1.0, 0.0);
|
||||
|
||||
let aabb = cone.aabb_3d(translation, Quat::IDENTITY);
|
||||
assert_eq!(aabb.min, Vec3::new(1.0, 0.0, -1.0));
|
||||
assert_eq!(aabb.max, Vec3::new(3.0, 2.0, 1.0));
|
||||
assert_eq!(aabb.min, Vec3A::new(1.0, 0.0, -1.0));
|
||||
assert_eq!(aabb.max, Vec3A::new(3.0, 2.0, 1.0));
|
||||
|
||||
let bounding_sphere = cone.bounding_sphere(translation, Quat::IDENTITY);
|
||||
assert_eq!(bounding_sphere.center, translation + Vec3::NEG_Y * 0.25);
|
||||
assert_eq!(
|
||||
bounding_sphere.center,
|
||||
Vec3A::from(translation) + Vec3A::NEG_Y * 0.25
|
||||
);
|
||||
assert_eq!(bounding_sphere.radius(), 1.25);
|
||||
}
|
||||
|
||||
@ -545,11 +557,14 @@ mod tests {
|
||||
let translation = Vec3::new(2.0, 1.0, 0.0);
|
||||
|
||||
let aabb = conical_frustum.aabb_3d(translation, Quat::IDENTITY);
|
||||
assert_eq!(aabb.min, Vec3::new(1.0, 0.0, -1.0));
|
||||
assert_eq!(aabb.max, Vec3::new(3.0, 2.0, 1.0));
|
||||
assert_eq!(aabb.min, Vec3A::new(1.0, 0.0, -1.0));
|
||||
assert_eq!(aabb.max, Vec3A::new(3.0, 2.0, 1.0));
|
||||
|
||||
let bounding_sphere = conical_frustum.bounding_sphere(translation, Quat::IDENTITY);
|
||||
assert_eq!(bounding_sphere.center, translation + Vec3::NEG_Y * 0.1875);
|
||||
assert_eq!(
|
||||
bounding_sphere.center,
|
||||
Vec3A::from(translation) + Vec3A::NEG_Y * 0.1875
|
||||
);
|
||||
assert_eq!(bounding_sphere.radius(), 1.2884705);
|
||||
}
|
||||
|
||||
@ -563,13 +578,16 @@ mod tests {
|
||||
let translation = Vec3::new(2.0, 1.0, 0.0);
|
||||
|
||||
let aabb = conical_frustum.aabb_3d(translation, Quat::IDENTITY);
|
||||
assert_eq!(aabb.min, Vec3::new(-3.0, 0.5, -5.0));
|
||||
assert_eq!(aabb.max, Vec3::new(7.0, 1.5, 5.0));
|
||||
assert_eq!(aabb.min, Vec3A::new(-3.0, 0.5, -5.0));
|
||||
assert_eq!(aabb.max, Vec3A::new(7.0, 1.5, 5.0));
|
||||
|
||||
// For wide conical frusta like this, the circumcenter can be outside the frustum,
|
||||
// so the center and radius should be clamped to the longest side.
|
||||
let bounding_sphere = conical_frustum.bounding_sphere(translation, Quat::IDENTITY);
|
||||
assert_eq!(bounding_sphere.center, translation + Vec3::NEG_Y * 0.5);
|
||||
assert_eq!(
|
||||
bounding_sphere.center,
|
||||
Vec3A::from(translation) + Vec3A::NEG_Y * 0.5
|
||||
);
|
||||
assert_eq!(bounding_sphere.radius(), 5.0);
|
||||
}
|
||||
|
||||
@ -582,11 +600,11 @@ mod tests {
|
||||
let translation = Vec3::new(2.0, 1.0, 0.0);
|
||||
|
||||
let aabb = torus.aabb_3d(translation, Quat::IDENTITY);
|
||||
assert_eq!(aabb.min, Vec3::new(0.5, 0.5, -1.5));
|
||||
assert_eq!(aabb.max, Vec3::new(3.5, 1.5, 1.5));
|
||||
assert_eq!(aabb.min, Vec3A::new(0.5, 0.5, -1.5));
|
||||
assert_eq!(aabb.max, Vec3A::new(3.5, 1.5, 1.5));
|
||||
|
||||
let bounding_sphere = torus.bounding_sphere(translation, Quat::IDENTITY);
|
||||
assert_eq!(bounding_sphere.center, translation);
|
||||
assert_eq!(bounding_sphere.center, translation.into());
|
||||
assert_eq!(bounding_sphere.radius(), 1.5);
|
||||
}
|
||||
}
|
||||
|
||||
@ -43,18 +43,18 @@ pub trait BoundingVolume: Sized {
|
||||
fn merge(&self, other: &Self) -> Self;
|
||||
|
||||
/// Increases the size of the bounding volume in each direction by the given amount.
|
||||
fn grow(&self, amount: Self::HalfSize) -> Self;
|
||||
fn grow(&self, amount: impl Into<Self::HalfSize>) -> Self;
|
||||
|
||||
/// Decreases the size of the bounding volume in each direction by the given amount.
|
||||
fn shrink(&self, amount: Self::HalfSize) -> Self;
|
||||
fn shrink(&self, amount: impl Into<Self::HalfSize>) -> Self;
|
||||
|
||||
/// Scale the size of the bounding volume around its center by the given amount
|
||||
fn scale_around_center(&self, scale: Self::HalfSize) -> Self;
|
||||
fn scale_around_center(&self, scale: impl Into<Self::HalfSize>) -> Self;
|
||||
|
||||
/// Transforms the bounding volume by first rotating it around the origin and then applying a translation.
|
||||
fn transformed_by(
|
||||
mut self,
|
||||
translation: Self::Translation,
|
||||
translation: impl Into<Self::Translation>,
|
||||
rotation: impl Into<Self::Rotation>,
|
||||
) -> Self {
|
||||
self.transform_by(translation, rotation);
|
||||
@ -64,7 +64,7 @@ pub trait BoundingVolume: Sized {
|
||||
/// Transforms the bounding volume by first rotating it around the origin and then applying a translation.
|
||||
fn transform_by(
|
||||
&mut self,
|
||||
translation: Self::Translation,
|
||||
translation: impl Into<Self::Translation>,
|
||||
rotation: impl Into<Self::Rotation>,
|
||||
) {
|
||||
self.rotate_by(rotation);
|
||||
@ -72,13 +72,13 @@ pub trait BoundingVolume: Sized {
|
||||
}
|
||||
|
||||
/// Translates the bounding volume by the given translation.
|
||||
fn translated_by(mut self, translation: Self::Translation) -> Self {
|
||||
fn translated_by(mut self, translation: impl Into<Self::Translation>) -> Self {
|
||||
self.translate_by(translation);
|
||||
self
|
||||
}
|
||||
|
||||
/// Translates the bounding volume by the given translation.
|
||||
fn translate_by(&mut self, translation: Self::Translation);
|
||||
fn translate_by(&mut self, translation: impl Into<Self::Translation>);
|
||||
|
||||
/// Rotates the bounding volume around the origin by the given rotation.
|
||||
///
|
||||
|
||||
@ -1,71 +1,59 @@
|
||||
use super::{Aabb3d, BoundingSphere, IntersectsVolume};
|
||||
use crate::{Dir3, Ray3d, Vec3};
|
||||
use crate::{Dir3A, Ray3d, Vec3A};
|
||||
|
||||
/// A raycast intersection test for 3D bounding volumes
|
||||
#[derive(Clone, Debug)]
|
||||
pub struct RayCast3d {
|
||||
/// The ray for the test
|
||||
pub ray: Ray3d,
|
||||
/// The origin of the ray.
|
||||
pub origin: Vec3A,
|
||||
/// The direction of the ray.
|
||||
pub direction: Dir3A,
|
||||
/// The maximum distance for the ray
|
||||
pub max: f32,
|
||||
/// The multiplicative inverse direction of the ray
|
||||
direction_recip: Vec3,
|
||||
direction_recip: Vec3A,
|
||||
}
|
||||
|
||||
impl RayCast3d {
|
||||
/// Construct a [`RayCast3d`] from an origin, [`Dir3`], and max distance.
|
||||
pub fn new(origin: Vec3, direction: Dir3, max: f32) -> Self {
|
||||
Self::from_ray(Ray3d { origin, direction }, max)
|
||||
}
|
||||
|
||||
/// Construct a [`RayCast3d`] from a [`Ray3d`] and max distance.
|
||||
pub fn from_ray(ray: Ray3d, max: f32) -> Self {
|
||||
pub fn new(origin: impl Into<Vec3A>, direction: impl Into<Dir3A>, max: f32) -> Self {
|
||||
let direction = direction.into();
|
||||
Self {
|
||||
ray,
|
||||
direction_recip: ray.direction.recip(),
|
||||
origin: origin.into(),
|
||||
direction,
|
||||
direction_recip: direction.recip(),
|
||||
max,
|
||||
}
|
||||
}
|
||||
|
||||
/// Construct a [`RayCast3d`] from a [`Ray3d`] and max distance.
|
||||
pub fn from_ray(ray: Ray3d, max: f32) -> Self {
|
||||
Self::new(ray.origin, ray.direction, max)
|
||||
}
|
||||
|
||||
/// Get the cached multiplicative inverse of the direction of the ray.
|
||||
pub fn direction_recip(&self) -> Vec3 {
|
||||
pub fn direction_recip(&self) -> Vec3A {
|
||||
self.direction_recip
|
||||
}
|
||||
|
||||
/// Get the distance of an intersection with an [`Aabb3d`], if any.
|
||||
pub fn aabb_intersection_at(&self, aabb: &Aabb3d) -> Option<f32> {
|
||||
let (min_x, max_x) = if self.ray.direction.x.is_sign_positive() {
|
||||
(aabb.min.x, aabb.max.x)
|
||||
} else {
|
||||
(aabb.max.x, aabb.min.x)
|
||||
};
|
||||
let (min_y, max_y) = if self.ray.direction.y.is_sign_positive() {
|
||||
(aabb.min.y, aabb.max.y)
|
||||
} else {
|
||||
(aabb.max.y, aabb.min.y)
|
||||
};
|
||||
let (min_z, max_z) = if self.ray.direction.z.is_sign_positive() {
|
||||
(aabb.min.z, aabb.max.z)
|
||||
} else {
|
||||
(aabb.max.z, aabb.min.z)
|
||||
};
|
||||
let positive = self.direction.signum().cmpgt(Vec3A::ZERO);
|
||||
let min = Vec3A::select(positive, aabb.min, aabb.max);
|
||||
let max = Vec3A::select(positive, aabb.max, aabb.min);
|
||||
|
||||
// Calculate the minimum/maximum time for each axis based on how much the direction goes that
|
||||
// way. These values can get arbitrarily large, or even become NaN, which is handled by the
|
||||
// min/max operations below
|
||||
let tmin_x = (min_x - self.ray.origin.x) * self.direction_recip.x;
|
||||
let tmin_y = (min_y - self.ray.origin.y) * self.direction_recip.y;
|
||||
let tmin_z = (min_z - self.ray.origin.z) * self.direction_recip.z;
|
||||
let tmax_x = (max_x - self.ray.origin.x) * self.direction_recip.x;
|
||||
let tmax_y = (max_y - self.ray.origin.y) * self.direction_recip.y;
|
||||
let tmax_z = (max_z - self.ray.origin.z) * self.direction_recip.z;
|
||||
let tmin = (min - self.origin) * self.direction_recip;
|
||||
let tmax = (max - self.origin) * self.direction_recip;
|
||||
|
||||
// An axis that is not relevant to the ray direction will be NaN. When one of the arguments
|
||||
// to min/max is NaN, the other argument is used.
|
||||
// An axis for which the direction is the wrong way will return an arbitrarily large
|
||||
// negative value.
|
||||
let tmin = tmin_x.max(tmin_y).max(tmin_z).max(0.);
|
||||
let tmax = tmax_z.min(tmax_y).min(tmax_x).min(self.max);
|
||||
let tmin = tmin.max_element().max(0.);
|
||||
let tmax = tmax.min_element().min(self.max);
|
||||
|
||||
if tmin <= tmax {
|
||||
Some(tmin)
|
||||
@ -76,9 +64,9 @@ impl RayCast3d {
|
||||
|
||||
/// Get the distance of an intersection with a [`BoundingSphere`], if any.
|
||||
pub fn sphere_intersection_at(&self, sphere: &BoundingSphere) -> Option<f32> {
|
||||
let offset = self.ray.origin - sphere.center;
|
||||
let projected = offset.dot(*self.ray.direction);
|
||||
let closest_point = offset - projected * *self.ray.direction;
|
||||
let offset = self.origin - sphere.center;
|
||||
let projected = offset.dot(*self.direction);
|
||||
let closest_point = offset - projected * *self.direction;
|
||||
let distance_squared = sphere.radius().powi(2) - closest_point.length_squared();
|
||||
if distance_squared < 0. || projected.powi(2).copysign(-projected) < -distance_squared {
|
||||
None
|
||||
@ -116,16 +104,21 @@ pub struct AabbCast3d {
|
||||
|
||||
impl AabbCast3d {
|
||||
/// Construct an [`AabbCast3d`] from an [`Aabb3d`], origin, [`Dir3`], and max distance.
|
||||
pub fn new(aabb: Aabb3d, origin: Vec3, direction: Dir3, max: f32) -> Self {
|
||||
Self::from_ray(aabb, Ray3d { origin, direction }, max)
|
||||
pub fn new(
|
||||
aabb: Aabb3d,
|
||||
origin: impl Into<Vec3A>,
|
||||
direction: impl Into<Dir3A>,
|
||||
max: f32,
|
||||
) -> Self {
|
||||
Self {
|
||||
ray: RayCast3d::new(origin, direction, max),
|
||||
aabb,
|
||||
}
|
||||
}
|
||||
|
||||
/// Construct an [`AabbCast3d`] from an [`Aabb3d`], [`Ray3d`], and max distance.
|
||||
pub fn from_ray(aabb: Aabb3d, ray: Ray3d, max: f32) -> Self {
|
||||
Self {
|
||||
ray: RayCast3d::from_ray(ray, max),
|
||||
aabb,
|
||||
}
|
||||
Self::new(aabb, ray.origin, ray.direction, max)
|
||||
}
|
||||
|
||||
/// Get the distance at which the [`Aabb3d`]s collide, if at all.
|
||||
@ -153,16 +146,21 @@ pub struct BoundingSphereCast {
|
||||
|
||||
impl BoundingSphereCast {
|
||||
/// Construct a [`BoundingSphereCast`] from a [`BoundingSphere`], origin, [`Dir3`], and max distance.
|
||||
pub fn new(sphere: BoundingSphere, origin: Vec3, direction: Dir3, max: f32) -> Self {
|
||||
Self::from_ray(sphere, Ray3d { origin, direction }, max)
|
||||
pub fn new(
|
||||
sphere: BoundingSphere,
|
||||
origin: impl Into<Vec3A>,
|
||||
direction: impl Into<Dir3A>,
|
||||
max: f32,
|
||||
) -> Self {
|
||||
Self {
|
||||
ray: RayCast3d::new(origin, direction, max),
|
||||
sphere,
|
||||
}
|
||||
}
|
||||
|
||||
/// Construct a [`BoundingSphereCast`] from a [`BoundingSphere`], [`Ray3d`], and max distance.
|
||||
pub fn from_ray(sphere: BoundingSphere, ray: Ray3d, max: f32) -> Self {
|
||||
Self {
|
||||
ray: RayCast3d::from_ray(ray, max),
|
||||
sphere,
|
||||
}
|
||||
Self::new(sphere, ray.origin, ray.direction, max)
|
||||
}
|
||||
|
||||
/// Get the distance at which the [`BoundingSphere`]s collide, if at all.
|
||||
@ -182,6 +180,7 @@ impl IntersectsVolume<BoundingSphere> for BoundingSphereCast {
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::{Dir3, Vec3};
|
||||
|
||||
const EPSILON: f32 = 0.001;
|
||||
|
||||
@ -238,7 +237,7 @@ mod tests {
|
||||
actual_distance
|
||||
);
|
||||
|
||||
let inverted_ray = RayCast3d::new(test.ray.origin, -test.ray.direction, test.max);
|
||||
let inverted_ray = RayCast3d::new(test.origin, -test.direction, test.max);
|
||||
assert!(!inverted_ray.intersects(volume), "{}", case);
|
||||
}
|
||||
}
|
||||
@ -345,7 +344,7 @@ mod tests {
|
||||
actual_distance
|
||||
);
|
||||
|
||||
let inverted_ray = RayCast3d::new(test.ray.origin, -test.ray.direction, test.max);
|
||||
let inverted_ray = RayCast3d::new(test.origin, -test.direction, test.max);
|
||||
assert!(!inverted_ray.intersects(volume), "{}", case);
|
||||
}
|
||||
}
|
||||
@ -455,8 +454,7 @@ mod tests {
|
||||
actual_distance
|
||||
);
|
||||
|
||||
let inverted_ray =
|
||||
RayCast3d::new(test.ray.ray.origin, -test.ray.ray.direction, test.ray.max);
|
||||
let inverted_ray = RayCast3d::new(test.ray.origin, -test.ray.direction, test.ray.max);
|
||||
assert!(!inverted_ray.intersects(volume), "{}", case);
|
||||
}
|
||||
}
|
||||
@ -522,8 +520,7 @@ mod tests {
|
||||
actual_distance
|
||||
);
|
||||
|
||||
let inverted_ray =
|
||||
RayCast3d::new(test.ray.ray.origin, -test.ray.ray.direction, test.ray.max);
|
||||
let inverted_ray = RayCast3d::new(test.ray.origin, -test.ray.direction, test.ray.max);
|
||||
assert!(!inverted_ray.intersects(volume), "{}", case);
|
||||
}
|
||||
}
|
||||
|
||||
@ -470,6 +470,18 @@ impl Dir3A {
|
||||
}
|
||||
}
|
||||
|
||||
impl From<Dir3> for Dir3A {
|
||||
fn from(value: Dir3) -> Self {
|
||||
Self(value.0.into())
|
||||
}
|
||||
}
|
||||
|
||||
impl From<Dir3A> for Dir3 {
|
||||
fn from(value: Dir3A) -> Self {
|
||||
Self(value.0.into())
|
||||
}
|
||||
}
|
||||
|
||||
impl TryFrom<Vec3A> for Dir3A {
|
||||
type Error = InvalidDirectionError;
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user