1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
use crate::math::{Point, Real, Vector};
use crate::na::{DimName, Point as PointN, VectorN};
use na::allocator::Allocator;
use na::{self, DefaultAllocator};
#[inline]
pub fn closest_points_line_line_parameters(
orig1: &Point<Real>,
dir1: &Vector<Real>,
orig2: &Point<Real>,
dir2: &Vector<Real>,
) -> (Real, Real) {
let res = closest_points_line_line_parameters_eps(
orig1,
dir1,
orig2,
dir2,
crate::math::DEFAULT_EPSILON,
);
(res.0, res.1)
}
#[inline]
pub fn closest_points_line_line_parameters_eps<D: DimName>(
orig1: &PointN<Real, D>,
dir1: &VectorN<Real, D>,
orig2: &PointN<Real, D>,
dir2: &VectorN<Real, D>,
eps: Real,
) -> (Real, Real, bool)
where
DefaultAllocator: Allocator<Real, D>,
{
let r = orig1 - orig2;
let a = dir1.norm_squared();
let e = dir2.norm_squared();
let f = dir2.dot(&r);
if a <= eps && e <= eps {
(0.0, 0.0, false)
} else if a <= eps {
(0.0, f / e, false)
} else {
let c = dir1.dot(&r);
if e <= eps {
(-c / a, 0.0, false)
} else {
let b = dir1.dot(dir2);
let ae = a * e;
let bb = b * b;
let denom = ae - bb;
let parallel = denom <= eps || ulps_eq!(ae, bb);
let s = if !parallel {
(b * f - c * e) / denom
} else {
0.0
};
(s, (b * s + f) / e, parallel)
}
}
}
#[inline]
pub fn closest_points_line_line(
orig1: &Point<Real>,
dir1: &Vector<Real>,
orig2: &Point<Real>,
dir2: &Vector<Real>,
) -> (Point<Real>, Point<Real>) {
let (s, t) = closest_points_line_line_parameters(orig1, dir1, orig2, dir2);
(*orig1 + *dir1 * s, *orig2 + *dir2 * t)
}