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 84 85
use crate::math::{Point, Real, Vector};
use crate::na::{Point as SPoint, SVector};
/// 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(
orig1: &Point<Real>,
dir1: &Vector<Real>,
orig2: &Point<Real>,
dir2: &Vector<Real>,
) -> (Real, Real) {
let res = closest_points_line_line_parameters_eps(
orig1,
dir1,
orig2,
dir2,
crate::math::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<const D: usize>(
orig1: &SPoint<Real, D>,
dir1: &SVector<Real, D>,
orig2: &SPoint<Real, D>,
dir2: &SVector<Real, D>,
eps: Real,
) -> (Real, Real, 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);
if a <= eps && e <= eps {
(0.0, 0.0, false)
} else if a <= eps {
(0.0, f / e, false)
} else {
let c = dir1.dot(&r);
if e <= eps {
(-c / a, 0.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.0
};
(s, (b * s + f) / e, parallel)
}
}
}
// TODO: can we re-used this for the segment/segment case?
/// Closest points between two segments.
#[inline]
pub fn closest_points_line_line(
orig1: &Point<Real>,
dir1: &Vector<Real>,
orig2: &Point<Real>,
dir2: &Vector<Real>,
) -> (Point<Real>, Point<Real>) {
let (s, t) = closest_points_line_line_parameters(orig1, dir1, orig2, dir2);
(*orig1 + *dir1 * s, *orig2 + *dir2 * t)
}