ncollide3d/query/closest_points/
closest_points_line_line.rs

1use crate::math::{Point, Vector};
2use na::{self, RealField};
3
4/// 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) {
15    let res =
16        closest_points_line_line_parameters_eps(orig1, dir1, orig2, dir2, N::default_epsilon());
17    (res.0, res.1)
18}
19
20/// 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.
35    let r = *orig1 - *orig2;
36
37    let a = dir1.norm_squared();
38    let e = dir2.norm_squared();
39    let f = dir2.dot(&r);
40
41    let _0: N = na::zero();
42    let _1: N = na::one();
43
44    if a <= eps && e <= eps {
45        (_0, _0, false)
46    } else if a <= eps {
47        (_0, f / e, false)
48    } else {
49        let c = dir1.dot(&r);
50        if e <= eps {
51            (-c / a, _0, false)
52        } else {
53            let b = dir1.dot(dir2);
54            let ae = a * e;
55            let bb = b * b;
56            let denom = ae - bb;
57
58            // Use absolute and ulps error to test collinearity.
59            let parallel = denom <= eps || ulps_eq!(ae, bb);
60
61            let s = if !parallel {
62                (b * f - c * e) / denom
63            } else {
64                _0
65            };
66
67            (s, (b * s + f) / e, parallel)
68        }
69    }
70}
71
72// 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>) {
81    let (s, t) = closest_points_line_line_parameters(orig1, dir1, orig2, dir2);
82    (*orig1 + *dir1 * s, *orig2 + *dir2 * t)
83}
ncollide3d/query/closest_points/closest_points_line_line.rs

ncollide3d/query/closest_points/
closest_points_line_line.rs
