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

use crate::math::{Point, Vector};
use na::{self, RealField};

/// Closest points between two lines.
///
/// The result, say `res`, is such that the closest points between both lines are
/// `orig1 + dir1 * res.0` and `orig2 + dir2 * res.1`.
#[inline]
pub fn closest_points_line_line_parameters<N: RealField>(
    orig1: &Point<N>,
    dir1: &Vector<N>,
    orig2: &Point<N>,
    dir2: &Vector<N>,
) -> (N, N) {
    let res =
        closest_points_line_line_parameters_eps(orig1, dir1, orig2, dir2, N::default_epsilon());
    (res.0, res.1)
}

/// Closest points between two lines with a custom tolerance epsilon.
///
/// The result, say `res`, is such that the closest points between both lines are
/// `orig1 + dir1 * res.0` and `orig2 + dir2 * res.1`. If the lines are parallel
/// then `res.2` is set to `true` and the returned closest points are `orig1` and
/// its projection on the second line.
#[inline]
pub fn closest_points_line_line_parameters_eps<N: RealField>(
    orig1: &Point<N>,
    dir1: &Vector<N>,
    orig2: &Point<N>,
    dir2: &Vector<N>,
    eps: N,
) -> (N, N, bool) {
    // Inspired by RealField-time collision detection by Christer Ericson.
    let r = *orig1 - *orig2;

    let a = dir1.norm_squared();
    let e = dir2.norm_squared();
    let f = dir2.dot(&r);

    let _0: N = na::zero();
    let _1: N = na::one();

    if a <= eps && e <= eps {
        (_0, _0, false)
    } else if a <= eps {
        (_0, f / e, false)
    } else {
        let c = dir1.dot(&r);
        if e <= eps {
            (-c / a, _0, false)
        } else {
            let b = dir1.dot(dir2);
            let ae = a * e;
            let bb = b * b;
            let denom = ae - bb;

            // Use absolute and ulps error to test collinearity.
            let parallel = denom <= eps || ulps_eq!(ae, bb);

            let s = if !parallel {
                (b * f - c * e) / denom
            } else {
                _0
            };

            (s, (b * s + f) / e, parallel)
        }
    }
}

// FIXME: can we re-used this for the segment/segment case?
/// Closest points between two segments.
#[inline]
pub fn closest_points_line_line<N: RealField>(
    orig1: &Point<N>,
    dir1: &Vector<N>,
    orig2: &Point<N>,
    dir2: &Vector<N>,
) -> (Point<N>, Point<N>) {
    let (s, t) = closest_points_line_line_parameters(orig1, dir1, orig2, dir2);
    (*orig1 + *dir1 * s, *orig2 + *dir2 * t)
}