use collide::{Collider, CollisionInfo};
use collide_ray::Ray;
use inner_space::InnerSpace;
use num_traits::{Zero, real::Real};
use crate::{Convex, closest_subsimplex};
fn hull_support<V: Copy + InnerSpace>(points: &[V], direction: V) -> V {
let mut best = points[0];
let mut best_dot = best.dot(&direction);
for &point in &points[1..] {
let dot = point.dot(&direction);
if dot > best_dot {
best_dot = dot;
best = point;
}
}
best
}
fn expanded_support<V: Copy + InnerSpace>(points: &[V], radius: V::Scalar, direction: V) -> V {
let hull = hull_support(points, direction);
let magnitude = direction.magnitude();
if magnitude.is_zero() {
hull
} else {
hull + direction * (radius / magnitude)
}
}
impl<V: Copy + InnerSpace> Collider<Ray<V>> for Convex<V> {
type Vector = V;
fn collision_info(&self, ray: &Ray<V>) -> Option<CollisionInfo<V>> {
if self.points.is_empty() {
return None;
}
let zero = V::Scalar::zero();
let epsilon = V::Scalar::epsilon();
let mut lambda = zero;
let mut position = ray.origin;
let mut normal = V::zero();
let initial_direction = position - self.points[0];
if initial_direction.is_zero() {
return Some(CollisionInfo {
self_contact: position,
other_contact: position,
vector: V::zero(),
});
}
let first_support = expanded_support(&self.points, self.rad, initial_direction);
let mut search = position - first_support;
let mut simplex: Vec<V> = vec![first_support];
for _ in 0..64 {
if search.magnitude2() <= epsilon {
break;
}
let support = expanded_support(&self.points, self.rad, search);
let witness = position - support;
if search.dot(&witness) > zero {
let along_ray = search.dot(&ray.direction);
if along_ray >= zero {
return None;
}
lambda = lambda - search.dot(&witness) / along_ray;
position = ray.origin + ray.direction * lambda;
normal = search;
simplex = vec![support];
} else {
simplex.push(support);
}
let translated: Vec<V> = simplex.iter().map(|&s| s - position).collect();
let (sub, offset) = closest_subsimplex(&translated);
simplex = sub.into_iter().map(|s| s + position).collect();
search = V::zero() - offset;
}
let hit_point = ray.origin + ray.direction * lambda;
let normal_magnitude = normal.magnitude();
if normal_magnitude <= epsilon {
return Some(CollisionInfo {
self_contact: hit_point,
other_contact: hit_point,
vector: V::zero(),
});
}
let surface_direction = normal / normal_magnitude;
Some(CollisionInfo {
self_contact: hit_point + surface_direction * self.rad,
other_contact: hit_point,
vector: V::zero() - ray.direction * lambda,
})
}
}
#[cfg(test)]
mod tests {
use super::*;
use simple_vectors::Vector;
type Vec3 = Vector<f32, 3>;
#[test]
fn ray_hits_sphere() {
let ray = Ray::new(Vec3::from([-5.0, 0.0, 0.0]), Vec3::from([1.0, 0.0, 0.0]));
let convex = Convex::sphere(Vec3::from([0.0, 0.0, 0.0]), 1.0);
let info = convex.collision_info(&ray).unwrap();
let distance = info.vector.magnitude();
assert!((distance - 4.0).abs() < 0.01);
}
#[test]
fn ray_misses_sphere() {
let ray = Ray::new(Vec3::from([-5.0, 0.0, 0.0]), Vec3::from([0.0, 1.0, 0.0]));
let convex = Convex::sphere(Vec3::from([0.0, 0.0, 0.0]), 1.0);
assert!(convex.collision_info(&ray).is_none());
}
#[test]
fn ray_hits_triangle() {
let ray = Ray::new(Vec3::from([0.0, 0.0, 5.0]), Vec3::from([0.0, 0.0, -1.0]));
let convex = Convex::new(
0.1,
vec![
Vec3::from([-1.0, -1.0, 0.0]),
Vec3::from([1.0, -1.0, 0.0]),
Vec3::from([0.0, 1.0, 0.0]),
]
.into(),
);
let info = convex.collision_info(&ray).unwrap();
let distance = info.vector.magnitude();
assert!((distance - 4.9).abs() < 0.1);
}
#[test]
fn ray_misses_triangle() {
let ray = Ray::new(Vec3::from([5.0, 5.0, 5.0]), Vec3::from([0.0, 0.0, -1.0]));
let convex = Convex::new(
0.1,
vec![
Vec3::from([-1.0, -1.0, 0.0]),
Vec3::from([1.0, -1.0, 0.0]),
Vec3::from([0.0, 1.0, 0.0]),
]
.into(),
);
assert!(convex.collision_info(&ray).is_none());
}
#[test]
fn ray_behind_origin_misses() {
let ray = Ray::new(Vec3::from([5.0, 0.0, 0.0]), Vec3::from([1.0, 0.0, 0.0]));
let convex = Convex::sphere(Vec3::from([0.0, 0.0, 0.0]), 1.0);
assert!(convex.collision_info(&ray).is_none());
}
#[test]
fn ray_hits_capsule_shape() {
let ray = Ray::new(Vec3::from([0.0, 5.0, 0.0]), Vec3::from([0.0, -1.0, 0.0]));
let convex = Convex::capsule(
1.0,
Vec3::from([-2.0, 0.0, 0.0]),
Vec3::from([2.0, 0.0, 0.0]),
);
let info = convex.collision_info(&ray).unwrap();
let distance = info.vector.magnitude();
assert!((distance - 4.0).abs() < 0.1);
}
}