parry3d/query/closest_points/
closest_points_line_line.rs

1use crate::math::{Point, Real, Vector};
2use crate::na::{Point as SPoint, SVector};
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(
10    orig1: &Point<Real>,
11    dir1: &Vector<Real>,
12    orig2: &Point<Real>,
13    dir2: &Vector<Real>,
14) -> (Real, Real) {
15    let res = closest_points_line_line_parameters_eps(
16        orig1,
17        dir1,
18        orig2,
19        dir2,
20        crate::math::DEFAULT_EPSILON,
21    );
22    (res.0, res.1)
23}
24
25/// Closest points between two lines with a custom tolerance epsilon.
26///
27/// The result, say `res`, is such that the closest points between both lines are
28/// `orig1 + dir1 * res.0` and `orig2 + dir2 * res.1`. If the lines are parallel
29/// then `res.2` is set to `true` and the returned closest points are `orig1` and
30/// its projection on the second line.
31#[inline]
32pub fn closest_points_line_line_parameters_eps<const D: usize>(
33    orig1: &SPoint<Real, D>,
34    dir1: &SVector<Real, D>,
35    orig2: &SPoint<Real, D>,
36    dir2: &SVector<Real, D>,
37    eps: Real,
38) -> (Real, Real, bool) {
39    // Inspired by RealField-time collision detection by Christer Ericson.
40    let r = orig1 - orig2;
41
42    let a = dir1.norm_squared();
43    let e = dir2.norm_squared();
44    let f = dir2.dot(&r);
45
46    if a <= eps && e <= eps {
47        (0.0, 0.0, false)
48    } else if a <= eps {
49        (0.0, f / e, false)
50    } else {
51        let c = dir1.dot(&r);
52        if e <= eps {
53            (-c / a, 0.0, false)
54        } else {
55            let b = dir1.dot(dir2);
56            let ae = a * e;
57            let bb = b * b;
58            let denom = ae - bb;
59
60            // Use absolute and ulps error to test collinearity.
61            let parallel = denom <= eps || ulps_eq!(ae, bb);
62
63            let s = if !parallel {
64                (b * f - c * e) / denom
65            } else {
66                0.0
67            };
68
69            (s, (b * s + f) / e, parallel)
70        }
71    }
72}
73
74// TODO: can we re-use this for the segment/segment case?
75/// Closest points between two segments.
76#[inline]
77pub fn closest_points_line_line(
78    orig1: &Point<Real>,
79    dir1: &Vector<Real>,
80    orig2: &Point<Real>,
81    dir2: &Vector<Real>,
82) -> (Point<Real>, Point<Real>) {
83    let (s, t) = closest_points_line_line_parameters(orig1, dir1, orig2, dir2);
84    (*orig1 + *dir1 * s, *orig2 + *dir2 * t)
85}
parry3d/query/closest_points/closest_points_line_line.rs

parry3d/query/closest_points/
closest_points_line_line.rs
