collide-convex 0.5.4

Convex collider implementation for the collide crate
Documentation
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,
        })
    }
}