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, Transform, Transformable};
6use inner_space::{InnerSpace, VectorSpace};
7use scalars::{One, Sqrt, Zero};
8
9trait ClampedVector: Copy + InnerSpace {
10    fn clamped_project(self, dis: Self) -> Self {
11        let mag = dis.magnitude();
12        let dir = dis / mag;
13        let new_mag = self.dot(&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: Copy + 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: Copy + 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; 2] {
71        let p = self.end - self.start;
72        let q = other.end - other.start;
73        let d = self.start - other.start;
74
75        let p0 = p.is_zero();
76        let q0 = q.is_zero();
77
78        if p0 != q0 {
79            return if p0 {
80                [self.start, other.start + d.clamped_project(q)]
81            } else {
82                [self.start + (-d).clamped_project(p), other.start]
83            };
84        }
85
86        if p0 {
87            return [self.start, other.start];
88        }
89
90        let p_mag2 = p.magnitude2();
91        let q_mag2 = q.magnitude2();
92        let angle = p.dot(&q);
93
94        let div = angle * angle - p_mag2 * q_mag2;
95        if div.is_zero() {
96            let add = d.reject(p);
97            return [self.start, self.start - add];
98        }
99
100        let a = (p * q_mag2 - q * angle).dot(&d) / div;
101        let b = (p * angle - q * p_mag2).dot(&d) / div;
102
103        let zero = V::Scalar::zero();
104        let one = V::Scalar::one();
105
106        let a_neg = a < zero;
107        let b_neg = b < zero;
108
109        let a_centered = !a_neg && a <= one;
110        let b_centered = !b_neg && b <= one;
111
112        if a_centered && b_centered {
113            return [self.start + p * a, other.start + q * b];
114        }
115
116        if !a_centered {
117            let a = if a_neg { zero } else { one };
118            let b = (angle * a + q.dot(&d)) / q_mag2;
119            if zero <= b && b <= one {
120                return [self.start + p * a, other.start + q * b];
121            }
122        }
123        if !b_centered {
124            let b = if b_neg { zero } else { one };
125            let a = (angle * b - p.dot(&d)) / p_mag2;
126            if zero <= a && a <= one {
127                return [self.start + p * a, other.start + q * b];
128            }
129        }
130        [
131            if a_neg { self.start } else { self.end },
132            if b_neg { other.start } else { other.end },
133        ]
134    }
135}
136
137impl<V: Copy + InnerSpace, T: Transform<V>> Transformable<T> for Capsule<V> {
138    fn transformed(&self, transform: &T) -> Self {
139        Self {
140            start: transform.apply_point(self.start),
141            end: transform.apply_point(self.end),
142            rad: self.rad,
143        }
144    }
145}
146
147#[cfg(feature = "sphere")]
148mod bounding_sphere;
149
150#[cfg(feature = "ray")]
151mod ray;
152
153impl<V: Copy + InnerSpace> Collider for Capsule<V> {
154    type Vector = V;
155
156    fn collision_info(&self, other: &Self) -> Option<CollisionInfo<Self::Vector>> {
157        let [point, other_point] = self.points(other);
158
159        let dis = other_point - point;
160        let mag2 = dis.magnitude2();
161        let rad = self.rad + other.rad;
162        if mag2 > rad * rad {
163            return None;
164        }
165
166        let mag = mag2.sqrt();
167        let dir = dis / mag;
168        Some(CollisionInfo {
169            self_contact: point + dir * self.rad,
170            other_contact: other_point - dir * other.rad,
171            vector: dir * (mag - rad),
172        })
173    }
174}
collide_capsule/lib.rs

collide_capsule/
lib.rs
