#![deny(missing_docs)]
use collide::{Collider, CollisionInfo, Transform, Transformable};
use inner_space::{InnerSpace, VectorSpace};
use num_traits::{Zero, real::Real};
#[derive(Clone)]
pub struct Convex<V: VectorSpace> {
pub rad: V::Scalar,
pub points: Box<[V]>,
}
impl<V: VectorSpace> Convex<V> {
pub fn new(rad: V::Scalar, points: Box<[V]>) -> Self {
Self { rad, points }
}
pub fn capsule(rad: V::Scalar, start: V, end: V) -> Self {
Self {
rad,
points: [start, end].into(),
}
}
pub fn point(pos: V) -> Self {
Self {
rad: V::Scalar::zero(),
points: [pos].into(),
}
}
pub fn line(start: V, end: V) -> Self {
Self {
rad: V::Scalar::zero(),
points: [start, end].into(),
}
}
pub fn sphere(pos: V, rad: V::Scalar) -> Self {
Self {
rad,
points: [pos].into(),
}
}
}
pub(crate) fn origin_offset<V: Copy + InnerSpace>(vertices: &[V]) -> V {
let Some(&first) = vertices.first() else {
return V::zero();
};
let mut ortho: Vec<V> = Vec::new();
for &vertex in &vertices[1..] {
let mut edge = vertex - first;
for basis in &ortho {
let mag2 = basis.magnitude2();
if !mag2.is_zero() {
edge = edge - *basis * (edge.dot(basis) / mag2);
}
}
if !edge.is_zero() {
ortho.push(edge);
}
}
let mut result = first;
for basis in &ortho {
let mag2 = basis.magnitude2();
if !mag2.is_zero() {
result = result - *basis * (result.dot(basis) / mag2);
}
}
result
}
fn is_projection_inside<V: Copy + InnerSpace>(offset: V, simplex: &[V]) -> bool {
if simplex.len() <= 1 {
return true;
}
let n = simplex.len();
for i in 0..n {
let face: Vec<V> = (0..n).filter(|&j| j != i).map(|j| simplex[j]).collect();
let face_offset = origin_offset(&face);
let toward_vertex = simplex[i] - face_offset;
let toward_projection = offset - face_offset;
if toward_vertex.dot(&toward_projection) < V::Scalar::zero() {
return false;
}
}
true
}
pub(crate) fn closest_subsimplex<V: Copy + InnerSpace>(points: &[V]) -> (Vec<V>, V) {
let n = points.len();
let last_bit = 1u32 << (n - 1);
let mut best: Option<(u32, V, V::Scalar)> = None;
for mask in 1u32..(1u32 << n) {
if mask & last_bit == 0 {
continue;
}
let subset: Vec<V> = (0..n)
.filter(|&i| mask & (1 << i) != 0)
.map(|i| points[i])
.collect();
let offset = origin_offset(&subset);
if !is_projection_inside(offset, &subset) {
continue;
}
let mag2 = offset.magnitude2();
if best.as_ref().is_some_and(|(_, _, m)| mag2 >= *m) {
continue;
}
best = Some((mask, offset, mag2));
}
let (mask, offset, _) =
best.unwrap_or_else(|| (last_bit, points[n - 1], points[n - 1].magnitude2()));
let subset = (0..n)
.filter(|&i| mask & (1 << i) != 0)
.map(|i| points[i])
.collect();
(subset, offset)
}
fn epa<V: Copy + InnerSpace>(initial_simplex: &[V], support: impl Fn(V) -> V) -> V {
if initial_simplex.len() <= 2 {
return epa_closest_initial_face(initial_simplex);
}
let mut vertices: Vec<V> = initial_simplex.to_vec();
let vertex_count = vertices.len();
let mut faces: Vec<Vec<usize>> = (0..vertex_count)
.map(|skip| (0..vertex_count).filter(|&i| i != skip).collect())
.collect();
let mut result = epa_closest_initial_face(initial_simplex);
for _ in 0..64 {
let Some((_, offset, distance)) = epa_find_closest_face(&vertices, &faces) else {
break;
};
if distance.is_zero() {
continue;
}
result = offset;
let new_point = support(offset);
let new_projection = new_point.dot(&offset);
let tolerance = V::Scalar::epsilon().sqrt();
if new_projection - distance <= tolerance * distance {
break;
}
let eps = V::Scalar::epsilon();
if vertices
.iter()
.any(|&vertex| (vertex - new_point).magnitude2() <= eps)
{
break;
}
let new_index = vertices.len();
vertices.push(new_point);
let visible: Vec<bool> = faces
.iter()
.map(|face| {
let points: Vec<V> = face.iter().map(|&i| vertices[i]).collect();
let face_offset = origin_offset(&points);
if face_offset.magnitude2().is_zero() {
return false;
}
(new_point - vertices[face[0]]).dot(&face_offset) > V::Scalar::zero()
})
.collect();
let mut ridges: Vec<Vec<usize>> = Vec::new();
for (face_index, face) in faces.iter().enumerate() {
if !visible[face_index] {
continue;
}
for skip in 0..face.len() {
let mut ridge: Vec<usize> = face
.iter()
.enumerate()
.filter(|&(j, _)| j != skip)
.map(|(_, &vertex)| vertex)
.collect();
ridge.sort_unstable();
ridges.push(ridge);
}
}
let horizon: Vec<Vec<usize>> = ridges
.iter()
.filter(|ridge| ridges.iter().filter(|other| other == ridge).count() == 1)
.cloned()
.collect();
faces = faces
.into_iter()
.enumerate()
.filter(|(index, _)| !visible[*index])
.map(|(_, face)| face)
.collect();
for ridge in horizon {
let mut face = ridge;
face.push(new_index);
faces.push(face);
}
if faces.is_empty() {
break;
}
}
result
}
fn epa_find_closest_face<V: Copy + InnerSpace>(
vertices: &[V],
faces: &[Vec<usize>],
) -> Option<(usize, V, V::Scalar)> {
let mut best: Option<(usize, V, V::Scalar)> = None;
for (index, face) in faces.iter().enumerate() {
let points: Vec<V> = face.iter().map(|&i| vertices[i]).collect();
let offset = origin_offset(&points);
if !is_projection_inside(offset, &points) {
continue;
}
let distance = offset.magnitude2();
if distance.is_zero() {
continue;
}
if best.as_ref().is_some_and(|(_, _, d)| distance >= *d) {
continue;
}
best = Some((index, offset, distance));
}
best
}
fn epa_closest_initial_face<V: Copy + InnerSpace>(simplex: &[V]) -> V {
let mut best: Option<(V::Scalar, V)> = None;
for skip in 0..simplex.len() {
let face: Vec<V> = (0..simplex.len())
.filter(|&j| j != skip)
.map(|j| simplex[j])
.collect();
if face.is_empty() {
continue;
}
let offset = origin_offset(&face);
let magnitude = offset.magnitude2();
if magnitude.is_zero() {
continue;
}
if best.as_ref().is_some_and(|(m, _)| magnitude >= *m) {
continue;
}
best = Some((magnitude, offset));
}
best.map_or_else(V::zero, |(_, offset)| offset)
}
enum Side {
Inside,
Outside,
}
impl<V: Copy + InnerSpace> Convex<V> {
fn support(&self, other: &Self, dir: V) -> V {
let mut found: Option<V> = None;
for &point in &self.points {
for &other_point in &other.points {
let diff = other_point - point;
if found.is_some_and(|found| diff.dot(&dir) <= found.dot(&dir)) {
continue;
}
found = Some(diff)
}
}
found.unwrap_or_else(V::zero)
}
fn offset(&self, other: &Self) -> (V, Side) {
let isd = other.points[0] - self.points[0];
if isd.is_zero() {
return (V::zero(), Side::Outside);
}
let initial = self.support(other, isd);
let mut simplex = vec![initial];
let mut dir = -initial;
for _ in 0..64 {
let new_point = self.support(other, dir);
if simplex.contains(&new_point) {
break;
}
simplex.push(new_point);
let (sub, offset) = closest_subsimplex(&simplex);
let inside = if offset.is_zero() {
true
} else if sub.len() == simplex.len() {
let scale = simplex
.iter()
.copied()
.map(|v| v.magnitude2())
.fold(V::Scalar::zero(), |a, b| if a > b { a } else { b });
let eps = V::Scalar::epsilon();
offset.magnitude2() <= scale * eps * eps
} else {
false
};
if inside {
let mut expanded = sub.clone();
'expand: for &self_point in self.points.iter() {
for &other_point in other.points.iter() {
let diff = other_point - self_point;
if expanded.contains(&diff) {
continue;
}
expanded.push(diff);
let (new_sub, new_offset) = closest_subsimplex(&expanded);
if new_offset.is_zero() {
expanded = new_sub;
if expanded.len() > 2 {
break 'expand;
}
} else {
expanded.pop();
}
}
}
let result = epa(&expanded, |direction| self.support(other, direction));
return (result, Side::Inside);
}
simplex = sub;
dir = -offset;
}
(origin_offset(&simplex), Side::Outside)
}
}
impl<V: Copy + InnerSpace, T: Transform<V>> Transformable<T> for Convex<V> {
fn transformed(&self, transform: &T) -> Self {
Self {
rad: self.rad,
points: self
.points
.iter()
.map(|&p| transform.apply_point(p))
.collect(),
}
}
}
#[cfg(feature = "sphere")]
mod bounding_sphere;
#[cfg(feature = "ray")]
mod ray;
impl<V: Copy + InnerSpace> Collider for Convex<V> {
type Vector = V;
fn collision_info(&self, other: &Self) -> Option<CollisionInfo<Self::Vector>> {
let (offset, side) = self.offset(other);
let mag2 = offset.magnitude2();
let rad = self.rad + other.rad;
match side {
Side::Inside => {
if mag2.is_zero() {
return Some(CollisionInfo {
self_contact: V::zero(),
other_contact: V::zero(),
vector: V::zero(),
});
}
let mag = mag2.sqrt();
let dir = offset / mag;
Some(CollisionInfo {
self_contact: V::zero(),
other_contact: V::zero(),
vector: dir * (mag + rad),
})
}
Side::Outside => {
if mag2 > rad * rad {
return None;
}
if mag2.is_zero() {
return Some(CollisionInfo {
self_contact: V::zero(),
other_contact: V::zero(),
vector: V::zero(),
});
}
let mag = mag2.sqrt();
let dir = offset / mag;
Some(CollisionInfo {
self_contact: V::zero(),
other_contact: V::zero(),
vector: dir * (mag - rad),
})
}
}
}
}