collide_capsule/
lib.rs

1#![deny(missing_docs)]
2
3//! This crate contains a capsule collider, which implements the collide trait from the `collide` crate.
4
5use collide::{Collider, CollisionInfo};
6use num_traits::{One, Zero, real::Real};
7use vector_space::{InnerSpace, VectorSpace};
8
9trait ClampedVector: InnerSpace {
10    fn clamped_project(self, dis: Self) -> Self {
11        let mag = dis.magnitude();
12        let dir = dis / mag;
13        let new_mag = self.scalar(dir);
14        if new_mag <= Self::Scalar::zero() {
15            Self::zero()
16        } else if new_mag >= mag {
17            dis
18        } else {
19            dir * new_mag
20        }
21    }
22}
23
24impl<V: InnerSpace> ClampedVector for V {}
25
26/// The capsule collider defined as a convex hull around two spheres having the same radius.
27#[derive(Copy, Clone)]
28pub struct Capsule<V: VectorSpace> {
29    /// The position of one sphere.
30    pub start: V,
31    /// The position of the other sphere.
32    pub end: V,
33    /// The radius of the spheres.
34    pub rad: V::Scalar,
35}
36
37impl<V: InnerSpace> Capsule<V> {
38    /// Creates a new capsule collider.
39    pub fn new(rad: V::Scalar, start: V, end: V) -> Self {
40        Self { start, end, rad }
41    }
42
43    /// Creates a new capsule collider representing a point.
44    pub fn point(pos: V) -> Self {
45        Self {
46            start: pos,
47            end: pos,
48            rad: V::Scalar::zero(),
49        }
50    }
51
52    /// Creates a new capsule collider representing a line.
53    pub fn line(start: V, end: V) -> Self {
54        Self {
55            start,
56            end,
57            rad: V::Scalar::zero(),
58        }
59    }
60
61    /// Creates a new capsule collider representing a sphere.
62    pub fn sphere(pos: V, rad: V::Scalar) -> Self {
63        Self {
64            start: pos,
65            end: pos,
66            rad,
67        }
68    }
69
70    fn points(&self, other: &Self) -> (V, V) {
71        let p = self.end - self.start;
72        let q = other.end - other.start;
73        let d = self.start - other.start;
74
75        match (p.is_zero(), q.is_zero()) {
76            (false, false) => {
77                let p_mag2 = p.magnitude2();
78                let q_mag2 = q.magnitude2();
79                let angle = p.scalar(q);
80
81                let div = angle * angle - p_mag2 * q_mag2;
82                if div.is_zero() {
83                    let add = d.reject(p);
84                    return (self.start, self.start - add);
85                }
86
87                let a = (p * q_mag2 - q * angle).scalar(d) / div;
88                let b = (p * angle - q * p_mag2).scalar(d) / div;
89
90                let zero = V::Scalar::zero();
91                let one = V::Scalar::one();
92
93                let a_neg = a < zero;
94                let b_neg = b < zero;
95
96                let a_centered = !a_neg && a <= one;
97                let b_centered = !b_neg && b <= one;
98
99                if a_centered && b_centered {
100                    return (self.start + p * a, other.start + q * b);
101                }
102
103                if !a_centered {
104                    let a = if a_neg { zero } else { one };
105                    let b = (angle * a + q.scalar(d)) / q_mag2;
106                    if zero <= b && b <= one {
107                        return (self.start + p * a, other.start + q * b);
108                    }
109                }
110                if !b_centered {
111                    let b = if b_neg { zero } else { one };
112                    let a = (angle * b - p.scalar(d)) / p_mag2;
113                    if zero <= a && a <= one {
114                        return (self.start + p * a, other.start + q * b);
115                    }
116                }
117                (
118                    if a_neg { self.start } else { self.end },
119                    if b_neg { other.start } else { other.end },
120                )
121            }
122
123            (false, true) => (self.start + (-d).clamped_project(p), other.start),
124            (true, false) => (self.start, other.start + d.clamped_project(q)),
125            (true, true) => (self.start, other.start),
126        }
127    }
128}
129
130impl<V: InnerSpace> Collider for Capsule<V> {
131    type Vector = V;
132
133    fn collision_info(&self, other: &Self) -> Option<CollisionInfo<Self::Vector>> {
134        let (point, other_point) = self.points(other);
135
136        let dis = other_point - point;
137        let mag2 = dis.magnitude2();
138        let rad = self.rad + other.rad;
139        if mag2 > rad * rad {
140            return None;
141        }
142
143        let mag = mag2.sqrt();
144        let dir = dis / mag;
145        Some(CollisionInfo {
146            self_contact: point + dir * self.rad,
147            other_contact: other_point - dir * other.rad,
148            vector: dir * (mag - rad),
149        })
150    }
151}
collide_capsule/lib.rs

collide_capsule/
lib.rs
