parry2d/query/closest_points/
closest_points_segment_segment.rs

1use crate::math::{Pose, Real, Vector2};
2use crate::query::ClosestPoints;
3use crate::shape::{Segment, SegmentPointLocation};
4
5use crate::math::Vector;
6
7/// Closest points between segments.
8#[inline]
9pub fn closest_points_segment_segment(
10    pos12: &Pose,
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 (p1 - (pos12 * p2)).length_squared() <= 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: &Pose,
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
38macro_rules! impl_closest_points(
39    ($Vect: ty, $fname: ident) => {
40        /// Segment-segment closest points computation in an arbitrary dimension.
41        #[allow(non_snake_case)]
42        #[inline]
43        pub fn $fname(
44            seg1: (&$Vect, &$Vect),
45            seg2: (&$Vect, &$Vect),
46        ) -> (SegmentPointLocation, SegmentPointLocation) {
47            // Inspired by RealField-time collision detection by Christer Ericson.
48            let d1 = seg1.1 - seg1.0;
49            let d2 = seg2.1 - seg2.0;
50            let r = seg1.0 - seg2.0;
51
52            let a = d1.length_squared();
53            let e = d2.length_squared();
54            let f = d2.dot(r);
55
56            let mut s;
57            let mut t;
58
59            let _eps = crate::math::DEFAULT_EPSILON;
60            if a <= _eps && e <= _eps {
61                s = 0.0;
62                t = 0.0;
63            } else if a <= _eps {
64                s = 0.0;
65                t = (f / e).clamp(0.0, 1.0);
66            } else {
67                let c = d1.dot(r);
68                if e <= _eps {
69                    t = 0.0;
70                    s = (-c / a).clamp(0.0, 1.0);
71                } else {
72                    let b = d1.dot(d2);
73                    let ae = a * e;
74                    let bb = b * b;
75                    let denom = ae - bb;
76
77                    // Use absolute and ulps error to test collinearity.
78                    if denom > _eps && !ulps_eq!(ae, bb) {
79                        s = ((b * f - c * e) / denom).clamp(0.0, 1.0);
80                    } else {
81                        s = 0.0;
82                    }
83
84                    t = (b * s + f) / e;
85
86                    if t < 0.0 {
87                        t = 0.0;
88                        s = (-c / a).clamp(0.0, 1.0);
89                    } else if t > 1.0 {
90                        t = 1.0;
91                        s = ((b - c) / a).clamp(0.0, 1.0);
92                    }
93                }
94            }
95
96            let loc1 = if s == 0.0 {
97                SegmentPointLocation::OnVertex(0)
98            } else if s == 1.0 {
99                SegmentPointLocation::OnVertex(1)
100            } else {
101                SegmentPointLocation::OnEdge([1.0 - s, s])
102            };
103
104            let loc2 = if t == 0.0 {
105                SegmentPointLocation::OnVertex(0)
106            } else if t == 1.0 {
107                SegmentPointLocation::OnVertex(1)
108            } else {
109                SegmentPointLocation::OnEdge([1.0 - t, t])
110            };
111
112            (loc1, loc2)
113        }
114    }
115);
116
117impl_closest_points!(Vector2, closest_points_segment_segment_2d);
118impl_closest_points!(Vector, closest_points_segment_segment_with_locations_nD);
parry2d/query/closest_points/closest_points_segment_segment.rs

parry2d/query/closest_points/
closest_points_segment_segment.rs
