mini_collide/
closest_point.rs

1use mini_math::{Point, Vector3};
2
3use crate::{Capsule, Distance, Line, LineSegment, Plane, Ray, Sphere, Triangle};
4
5/// Trait for finding the closest point to another object
6pub trait ClosestPoint<Other> {
7    /// The closest point to another object
8    fn closest_point(&self, other: &Other) -> Point;
9}
10
11impl ClosestPoint<Point> for Sphere {
12    fn closest_point(&self, other: &Point) -> Point {
13        self.center + (*other - self.center).normalized() * self.radius
14    }
15}
16
17impl ClosestPoint<Sphere> for Sphere {
18    fn closest_point(&self, other: &Sphere) -> Point {
19        self.closest_point(&other.center)
20    }
21}
22
23impl ClosestPoint<Point> for Line {
24    fn closest_point(&self, other: &Point) -> Point {
25        let dot = self.direction.dot(*other - self.point);
26        self.point + self.direction * dot
27    }
28}
29
30impl ClosestPoint<Line> for Line {
31    fn closest_point(&self, other: &Line) -> Point {
32        let w = self.point - other.point;
33        let b = self.direction.dot(other.direction);
34        let d = self.direction.dot(w);
35        let e = other.direction.dot(w);
36        let d_p = 1.0 - b * b;
37
38        if d_p < std::f32::EPSILON {
39            return self.point;
40        }
41
42        let sc = (b * e - d) / d_p;
43
44        self.point + self.direction * sc
45    }
46}
47
48impl ClosestPoint<Point> for Ray {
49    fn closest_point(&self, other: &Point) -> Point {
50        let dot = (*other - self.origin).dot(self.direction);
51
52        if dot <= 0.0 {
53            self.origin
54        } else {
55            self.origin + self.direction * dot
56        }
57    }
58}
59
60impl ClosestPoint<Sphere> for Ray {
61    fn closest_point(&self, other: &Sphere) -> Point {
62        self.closest_point(&other.center)
63    }
64}
65
66impl ClosestPoint<Ray> for Sphere {
67    fn closest_point(&self, other: &Ray) -> Point {
68        let p = other.closest_point(&self.center);
69        let diff = p - self.center;
70        let l = diff.magnitude();
71        self.center + (diff / l) * self.radius.min(l)
72    }
73}
74
75impl ClosestPoint<Capsule> for Ray {
76    fn closest_point(&self, other: &Capsule) -> Point {
77        self.closest_point(&other.axis)
78    }
79}
80
81impl ClosestPoint<Ray> for Capsule {
82    fn closest_point(&self, other: &Ray) -> Point {
83        let p = other.closest_point(&self.axis);
84        let q = self.axis.closest_point(other);
85        let diff = p - q;
86        let l = diff.magnitude();
87        q + (diff / l) * self.radius.min(l)
88    }
89}
90
91impl ClosestPoint<Line> for Ray {
92    fn closest_point(&self, other: &Line) -> Point {
93        let p = Line::new(self.origin, self.direction).closest_point(other);
94        self.closest_point(&p)
95    }
96}
97
98impl ClosestPoint<Ray> for Line {
99    fn closest_point(&self, other: &Ray) -> Point {
100        self.closest_point(&other.closest_point(self))
101    }
102}
103
104impl ClosestPoint<LineSegment> for Ray {
105    fn closest_point(&self, other: &LineSegment) -> Point {
106        let p = Line::new(self.origin, self.direction)
107            .closest_point(&Line::from_points(other.start, other.end));
108        let p = other.closest_point(&p);
109        self.closest_point(&p)
110    }
111}
112
113impl ClosestPoint<Ray> for LineSegment {
114    fn closest_point(&self, other: &Ray) -> Point {
115        self.closest_point(&other.closest_point(self))
116    }
117}
118
119impl ClosestPoint<Ray> for Ray {
120    fn closest_point(&self, other: &Ray) -> Point {
121        let p = Line::new(other.origin, other.direction)
122            .closest_point(&Line::new(self.origin, self.direction));
123        let p = other.closest_point(&p);
124        self.closest_point(&p)
125    }
126}
127
128impl ClosestPoint<Point> for LineSegment {
129    fn closest_point(&self, other: &Point) -> Point {
130        let mut direction = self.end - self.start;
131        let length = direction.magnitude();
132        direction /= length;
133
134        let dot = (*other - self.start).dot(direction);
135
136        if dot < 0.0 {
137            self.start
138        } else {
139            self.start + direction * dot.min(length)
140        }
141    }
142}
143
144impl ClosestPoint<Line> for LineSegment {
145    fn closest_point(&self, other: &Line) -> Point {
146        let p = other.closest_point(&Line::from_points(self.start, self.end));
147        self.closest_point(&p)
148    }
149}
150
151impl ClosestPoint<LineSegment> for LineSegment {
152    fn closest_point(&self, other: &LineSegment) -> Point {
153        let p = Line::from_points(other.start, other.end)
154            .closest_point(&Line::from_points(self.start, self.end));
155        let p = other.closest_point(&p);
156        self.closest_point(&p)
157    }
158}
159
160impl ClosestPoint<Point> for Plane {
161    fn closest_point(&self, other: &Point) -> Point {
162        let distance = self.distance(other);
163        *other - self.normal * distance
164    }
165}
166
167impl ClosestPoint<Ray> for Plane {
168    fn closest_point(&self, other: &Ray) -> Point {
169        let n_dot_r = self.normal.dot(other.direction);
170        // early exit if ray parallel to plane
171        if n_dot_r.abs() < std::f32::EPSILON {
172            return self.closest_point(&other.origin);
173        }
174
175        let e = self.normal.dot(Vector3::from(other.origin));
176        let t = (e + self.d) / n_dot_r;
177
178        other.origin + other.direction * -t
179    }
180}
181
182impl ClosestPoint<Point> for Triangle {
183    fn closest_point(&self, other: &Point) -> Point {
184        let plane = Plane::from(self);
185        let q = plane.closest_point(other);
186
187        let coordinates = self.barycentric_coordinates(q);
188        if coordinates.x >= 0.0 && coordinates.y >= 0.0 && coordinates.z >= 0.0 {
189            return q;
190        }
191
192        let p0 = LineSegment::new(self.a, self.b).closest_point(other);
193        let p1 = LineSegment::new(self.b, self.c).closest_point(other);
194        let p2 = LineSegment::new(self.c, self.a).closest_point(other);
195
196        let d0 = (p0 - *other).magnitude_squared();
197        let d1 = (p1 - *other).magnitude_squared();
198        let d2 = (p2 - *other).magnitude_squared();
199
200        if d0 < d1 && d0 < d2 {
201            p0
202        } else if d1 < d0 && d1 < d2 {
203            p1
204        } else {
205            p2
206        }
207    }
208}
209
210impl ClosestPoint<Ray> for Triangle {
211    fn closest_point(&self, other: &Ray) -> Point {
212        let plane = Plane::from(self);
213
214        let n_dot_r = plane.normal.dot(other.direction);
215        // early exit if ray parallel to plane
216        if n_dot_r.abs() < std::f32::EPSILON {
217            return self.closest_point(&other.origin);
218        }
219
220        let e = plane.normal.dot(Vector3::from(other.origin));
221        let t = (e + plane.d) / n_dot_r;
222
223        let intersection_point = other.origin + other.direction * -t;
224        self.closest_point(&intersection_point)
225    }
226}
227
228#[cfg(test)]
229mod tests {
230    use mini_math::Vector3;
231
232    use super::*;
233
234    #[test]
235    fn test_line_line() {
236        let line = Line::from_points(Point::new(0.0, 0.0, 0.0), Point::new(0.0, 0.0, 10.0));
237
238        let l = Line::from_points(Point::new(0.0, 0.0, 1.0), Point::new(0.0, 10.0, 10.0));
239        assert_eq!(line.closest_point(&l), Point::new(0.0, 0.0, 1.0));
240
241        let l = Line::from_points(Point::new(0.0, 5.0, 5.0), Point::new(0.0, 5.0, 15.0));
242        assert_eq!(line.closest_point(&l), Point::new(0.0, 0.0, 0.0));
243
244        let l = Line::from_points(Point::new(0.0, 5.0, 0.0), Point::new(25.0, 5.0, 0.0));
245        assert_eq!(line.closest_point(&l), Point::new(0.0, 0.0, 0.0));
246
247        let l = Line::from_points(Point::new(0.0, 5.0, 10.0), Point::new(25.0, 5.0, 10.0));
248        assert_eq!(line.closest_point(&l), Point::new(0.0, 0.0, 10.0));
249    }
250
251    #[test]
252    fn test_ray_point() {
253        let ray = Ray::new(Point::zero(), Vector3::new(0.0, 0.0, 1.0));
254
255        let p = Point::new(0.0, 0.0, -5.0);
256        assert_eq!(ray.closest_point(&p), Point::zero());
257
258        let p = Point::new(0.0, 5.0, 25.0);
259        assert_eq!(ray.closest_point(&p), Point::new(0.0, 0.0, 25.0));
260    }
261
262    #[test]
263    fn test_ray_line() {
264        let ray = Ray::new(Point::zero(), Vector3::new(0.0, 0.0, 1.0));
265
266        let l = Line::new(Point::new(0.0, 5.0, 0.0), Vector3::new(0.0, 0.0, 1.0));
267        assert_eq!(ray.closest_point(&l), Point::new(0.0, 0.0, 0.0));
268
269        let l = Line::new(Point::new(0.0, 0.0, -5.0), Vector3::new(0.0, 0.0, -1.0));
270        assert_eq!(ray.closest_point(&l), Point::new(0.0, 0.0, 0.0));
271
272        let l = Line::new(Point::new(0.0, 5.0, -5.0), Vector3::new(0.0, 1.0, 0.0));
273        assert_eq!(ray.closest_point(&l), Point::new(0.0, 0.0, 0.0));
274
275        let l = Line::new(Point::new(0.0, 5.0, 5.0), Vector3::new(0.0, 1.0, 0.0));
276        assert_eq!(ray.closest_point(&l), Point::new(0.0, 0.0, 5.0));
277    }
278
279    #[test]
280    fn test_plane_point() {
281        let plane = Plane::from_points(
282            Point::new(-1.0, 0.0, -1.0),
283            Point::new(1.0, 0.0, -1.0),
284            Point::new(0.0, 0.0, 1.0),
285        );
286
287        let p = Point::new(2.0, 1.0, 3.0);
288        assert_eq!(plane.closest_point(&p), Point::new(2.0, 0.0, 3.0));
289
290        let p = Point::new(-2.0, -1.0, -3.0);
291        assert_eq!(plane.closest_point(&p), Point::new(-2.0, 0.0, -3.0));
292    }
293
294    #[test]
295    fn test_triangle_point() {
296        let triangle = Triangle::new(
297            Point::new(-1.0, 0.0, -1.0),
298            Point::new(1.0, 0.0, -1.0),
299            Point::new(0.0, 0.0, 1.0),
300        );
301
302        let p = Point::new(0.0, 1.0, 0.0);
303        assert_eq!(triangle.closest_point(&p), Point::new(0.0, 0.0, 0.0));
304
305        let p = Point::new(0.0, 1.0, 2.0);
306        assert_eq!(triangle.closest_point(&p), Point::new(0.0, 0.0, 1.0));
307
308        let p = Point::new(0.0, -1.0, -2.0);
309        assert_eq!(triangle.closest_point(&p), Point::new(0.0, 0.0, -1.0));
310    }
311}
mini_collide/closest_point.rs

mini_collide/
closest_point.rs
