parry3d/query/closest_points/
closest_points_segment_segment.rs

1use crate::math::{Isometry, Real};
2use crate::query::ClosestPoints;
3use crate::shape::{Segment, SegmentPointLocation};
4
5use na::{self, Point};
6
7/// Closest points between segments.
8#[inline]
9pub fn closest_points_segment_segment(
10    pos12: &Isometry<Real>,
11    seg1: &Segment,
12    seg2: &Segment,
13    margin: Real,
14) -> ClosestPoints {
15    let (loc1, loc2) = closest_points_segment_segment_with_locations(pos12, seg1, seg2);
16    let p1 = seg1.point_at(&loc1);
17    let p2 = seg2.point_at(&loc2);
18
19    if na::distance_squared(&p1, &(pos12 * p2)) <= margin * margin {
20        ClosestPoints::WithinMargin(p1, p2)
21    } else {
22        ClosestPoints::Disjoint
23    }
24}
25
26// TODO: use this specialized procedure for distance/interference/contact determination as well.
27/// Closest points between two segments.
28#[inline]
29pub fn closest_points_segment_segment_with_locations(
30    pos12: &Isometry<Real>,
31    seg1: &Segment,
32    seg2: &Segment,
33) -> (SegmentPointLocation, SegmentPointLocation) {
34    let seg2_1 = seg2.transformed(pos12);
35    closest_points_segment_segment_with_locations_nD((&seg1.a, &seg1.b), (&seg2_1.a, &seg2_1.b))
36}
37
38/// Segment-segment closest points computation in an arbitrary dimension.
39#[allow(non_snake_case)]
40#[inline]
41pub fn closest_points_segment_segment_with_locations_nD<const D: usize>(
42    seg1: (&Point<Real, D>, &Point<Real, D>),
43    seg2: (&Point<Real, D>, &Point<Real, D>),
44) -> (SegmentPointLocation, SegmentPointLocation) {
45    // Inspired by RealField-time collision detection by Christer Ericson.
46    let d1 = seg1.1 - seg1.0;
47    let d2 = seg2.1 - seg2.0;
48    let r = seg1.0 - seg2.0;
49
50    let a = d1.norm_squared();
51    let e = d2.norm_squared();
52    let f = d2.dot(&r);
53
54    let mut s;
55    let mut t;
56
57    let _eps = crate::math::DEFAULT_EPSILON;
58    if a <= _eps && e <= _eps {
59        s = 0.0;
60        t = 0.0;
61    } else if a <= _eps {
62        s = 0.0;
63        t = na::clamp(f / e, 0.0, 1.0);
64    } else {
65        let c = d1.dot(&r);
66        if e <= _eps {
67            t = 0.0;
68            s = na::clamp(-c / a, 0.0, 1.0);
69        } else {
70            let b = d1.dot(&d2);
71            let ae = a * e;
72            let bb = b * b;
73            let denom = ae - bb;
74
75            // Use absolute and ulps error to test collinearity.
76            if denom > _eps && !ulps_eq!(ae, bb) {
77                s = na::clamp((b * f - c * e) / denom, 0.0, 1.0);
78            } else {
79                s = 0.0;
80            }
81
82            t = (b * s + f) / e;
83
84            if t < 0.0 {
85                t = 0.0;
86                s = na::clamp(-c / a, 0.0, 1.0);
87            } else if t > 1.0 {
88                t = 1.0;
89                s = na::clamp((b - c) / a, 0.0, 1.0);
90            }
91        }
92    }
93
94    let loc1 = if s == 0.0 {
95        SegmentPointLocation::OnVertex(0)
96    } else if s == 1.0 {
97        SegmentPointLocation::OnVertex(1)
98    } else {
99        SegmentPointLocation::OnEdge([1.0 - s, s])
100    };
101
102    let loc2 = if t == 0.0 {
103        SegmentPointLocation::OnVertex(0)
104    } else if t == 1.0 {
105        SegmentPointLocation::OnVertex(1)
106    } else {
107        SegmentPointLocation::OnEdge([1.0 - t, t])
108    };
109
110    (loc1, loc2)
111}
parry3d/query/closest_points/closest_points_segment_segment.rs

parry3d/query/closest_points/
closest_points_segment_segment.rs
