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