1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104

#![deny(missing_docs)]

/*!
This crate contains a sphere collider, which implements the collide trait from the `collide` crate.
**/

use collide::{Collider, CollisionInfo};
use num_traits::{real::Real, Zero};
use vector_space::{InnerSpace, VectorSpace};

#[derive(Copy, Clone)]
/// The capsule collider defined as a convex hull around two spheres having the same radius.
pub struct Capsule<V: VectorSpace> {
    /// The position of one sphere.
    pub start: V,
    /// The position of the other sphere.
    pub end: V,
    /// The radius of the spheres.
    pub rad: V::Scalar,
}

impl<V: InnerSpace> Capsule<V> {
    /// Creates a new capsule collider.
    pub fn new(rad: V::Scalar, start: V, end: V) -> Self {
        Self { start, end, rad }
    }

    /// Creates a new capsule collider representing a point.
    pub fn point(pos: V) -> Self {
        Self {
            start: pos,
            end: pos,
            rad: V::Scalar::zero(),
        }
    }

    /// Creates a new capsule collider representing a line.
    pub fn line(start: V, end: V) -> Self {
        Self {
            start,
            end,
            rad: V::Scalar::zero(),
        }
    }

    /// Creates a new capsule collider representing a sphere.
    pub fn sphere(pos: V, rad: V::Scalar) -> Self {
        Self {
            start: pos,
            end: pos,
            rad,
        }
    }

    fn closest(&self, point: V) -> V {
        if self.start == self.end {
            self.start
        } else {
            let dis = self.end - self.start;
            let mag = dis.magnitude();
            let dir = dis / mag;
            let dot = dir.dot(point - self.start);
            self.start + dir * dot.max(V::Scalar::zero()).min(mag)
        }
    }
}

impl<V: InnerSpace> Collider for Capsule<V> {
    type Vector = V;

    fn collision_info(&self, other: &Self) -> Option<CollisionInfo<Self::Vector>> {
        let start_to_other_start = (other.start - self.start).magnitude2();
        let start_to_other_end = (other.end - self.start).magnitude2();
        let end_to_other_start = (other.start - self.end).magnitude2();
        let end_to_other_end = (other.end - self.end).magnitude2();

        let point = if end_to_other_start < start_to_other_start
            || end_to_other_start < start_to_other_end
            || end_to_other_end < start_to_other_start
            || end_to_other_end < start_to_other_end
        {
            self.end
        } else {
            self.start
        };

        let other_point = other.closest(point);
        let point = self.closest(other_point);

        let dis = other_point - point;
        let mag = dis.magnitude();
        let rad = self.rad + other.rad;
        if mag <= rad {
            let dir = dis / mag;
            Some(CollisionInfo {
                self_contact: point + dir * self.rad,
                other_contact: other_point - dir * other.rad,
                vector: dir * (mag - rad),
            })
        } else {
            None
        }
    }
}