veloren_common/util/
lines.rs

1use vek::{LineSegment3, Vec3};
2
3// Get closest point between 2 3D line segments https://math.stackexchange.com/a/4289668
4pub fn closest_points_3d(n: LineSegment3<f32>, m: LineSegment3<f32>) -> (Vec3<f32>, Vec3<f32>) {
5    let p1 = n.start;
6    let p2 = n.end;
7    let p3 = m.start;
8    let p4 = m.end;
9
10    let d1 = p2 - p1;
11    let d2 = p4 - p3;
12    let d21 = p3 - p1;
13
14    let v22 = d2.dot(d2);
15    let v11 = d1.dot(d1);
16    let v21 = d2.dot(d1);
17    let v21_1 = d21.dot(d1);
18    let v21_2 = d21.dot(d2);
19
20    let denom = v21 * v21 - v22 * v11;
21
22    let (s, t) = if denom == 0.0 {
23        let s = 0.0;
24        let t = (v11 * s - v21_1) / v21;
25        (s, t)
26    } else {
27        let s = (v21_2 * v21 - v22 * v21_1) / denom;
28        let t = (-v21_1 * v21 + v11 * v21_2) / denom;
29        (s, t)
30    };
31
32    let (s, t) = (s.clamp(0.0, 1.0), t.clamp(0.0, 1.0));
33
34    let p_a = p1 + s * d1;
35    let p_b = p3 + t * d2;
36
37    (p_a, p_b)
38}
veloren_common/util/lines.rs

veloren_common/util/
lines.rs
