use collide::{Collider, CollisionInfo};
use collide_ray::Ray;
use inner_space::InnerSpace;
use scalars::{Real, Zero};
use crate::{Convex, closest_subsimplex, epsilon};
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>
where
V::Scalar: Real,
{
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 = epsilon::<V>();
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,
})
}
}