parry3d/query/closest_points/
closest_points_line_line.rs

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

parry3d/query/closest_points/
closest_points_line_line.rs
