1use crate::math::{Point, Vector};
2use na::{self, RealField};
34/// Closest points between two lines.
5///
6/// The result, say `res`, is such that the closest points between both lines are
7/// `orig1 + dir1 * res.0` and `orig2 + dir2 * res.1`.
8#[inline]
9pub fn closest_points_line_line_parameters<N: RealField + Copy>(
10 orig1: &Point<N>,
11 dir1: &Vector<N>,
12 orig2: &Point<N>,
13 dir2: &Vector<N>,
14) -> (N, N) {
15let res =
16 closest_points_line_line_parameters_eps(orig1, dir1, orig2, dir2, N::default_epsilon());
17 (res.0, res.1)
18}
1920/// Closest points between two lines with a custom tolerance epsilon.
21///
22/// The result, say `res`, is such that the closest points between both lines are
23/// `orig1 + dir1 * res.0` and `orig2 + dir2 * res.1`. If the lines are parallel
24/// then `res.2` is set to `true` and the returned closest points are `orig1` and
25/// its projection on the second line.
26#[inline]
27pub fn closest_points_line_line_parameters_eps<N: RealField + Copy>(
28 orig1: &Point<N>,
29 dir1: &Vector<N>,
30 orig2: &Point<N>,
31 dir2: &Vector<N>,
32 eps: N,
33) -> (N, N, bool) {
34// Inspired by RealField-time collision detection by Christer Ericson.
35let r = *orig1 - *orig2;
3637let a = dir1.norm_squared();
38let e = dir2.norm_squared();
39let f = dir2.dot(&r);
4041let _0: N = na::zero();
42let _1: N = na::one();
4344if a <= eps && e <= eps {
45 (_0, _0, false)
46 } else if a <= eps {
47 (_0, f / e, false)
48 } else {
49let c = dir1.dot(&r);
50if e <= eps {
51 (-c / a, _0, false)
52 } else {
53let b = dir1.dot(dir2);
54let ae = a * e;
55let bb = b * b;
56let denom = ae - bb;
5758// Use absolute and ulps error to test collinearity.
59let parallel = denom <= eps || ulps_eq!(ae, bb);
6061let s = if !parallel {
62 (b * f - c * e) / denom
63 } else {
64 _0
65 };
6667 (s, (b * s + f) / e, parallel)
68 }
69 }
70}
7172// FIXME: can we re-used this for the segment/segment case?
73/// Closest points between two segments.
74#[inline]
75pub fn closest_points_line_line<N: RealField + Copy>(
76 orig1: &Point<N>,
77 dir1: &Vector<N>,
78 orig2: &Point<N>,
79 dir2: &Vector<N>,
80) -> (Point<N>, Point<N>) {
81let (s, t) = closest_points_line_line_parameters(orig1, dir1, orig2, dir2);
82 (*orig1 + *dir1 * s, *orig2 + *dir2 * t)
83}