collide_capsule/
lib.rs

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

collide_capsule/
lib.rs
