ncollide3d/query/closest_points/
closest_points_segment_segment.rs

Help
1use crate::math::Isometry;
2use crate::query::ClosestPoints;
3use crate::shape::{Segment, SegmentPointLocation};
4use na::{self, Point, RealField};
5
6/// Closest points between segments.
7#[inline]
8pub fn closest_points_segment_segment<N: RealField + Copy>(
9    m1: &Isometry<N>,
10    seg1: &Segment<N>,
11    m2: &Isometry<N>,
12    seg2: &Segment<N>,
13    margin: N,
14) -> ClosestPoints<N> {
15    let (loc1, loc2) = closest_points_segment_segment_with_locations(m1, seg1, m2, seg2);
16    let p1 = seg1.point_at(&loc1);
17    let p2 = seg2.point_at(&loc2);
18
19    if na::distance_squared(&p1, &p2) <= margin * margin {
20        ClosestPoints::WithinMargin(p1, p2)
21    } else {
22        ClosestPoints::Disjoint
23    }
24}
25
26// FIXME: 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<N: RealField + Copy>(
30    m1: &Isometry<N>,
31    seg1: &Segment<N>,
32    m2: &Isometry<N>,
33    seg2: &Segment<N>,
34) -> (SegmentPointLocation<N>, SegmentPointLocation<N>) {
35    let seg1 = seg1.transformed(m1);
36    let seg2 = seg2.transformed(m2);
37
38    closest_points_segment_segment_with_locations_nD((&seg1.a, &seg1.b), (&seg2.a, &seg2.b))
39}
40
41/// Segment-segment closest points computation in an arbitrary dimension.
42#[allow(non_snake_case)]
43#[inline]
44pub fn closest_points_segment_segment_with_locations_nD<N, const D: usize>(
45    seg1: (&Point<N, D>, &Point<N, D>),
46    seg2: (&Point<N, D>, &Point<N, D>),
47) -> (SegmentPointLocation<N>, SegmentPointLocation<N>)
48where
49    N: RealField + Copy,
50{
51    let res =
52        closest_points_segment_segment_with_locations_nD_eps(seg1, seg2, N::default_epsilon());
53    (res.0, res.1)
54}
55
56/// Segment-segment closest points computation in an arbitrary dimension.
57///
58/// This returns the location of the closest points, as well as a boolean indicating
59/// if the two segments were considered parallel.
60#[allow(non_snake_case)]
61#[inline]
62pub fn closest_points_segment_segment_with_locations_nD_eps<N, const D: usize>(
63    seg1: (&Point<N, D>, &Point<N, D>),
64    seg2: (&Point<N, D>, &Point<N, D>),
65    eps: N,
66) -> (SegmentPointLocation<N>, SegmentPointLocation<N>, bool)
67where
68    N: RealField + Copy,
69{
70    // Inspired by RealField-time collision detection by Christer Ericson.
71    let d1 = seg1.1 - seg1.0;
72    let d2 = seg2.1 - seg2.0;
73    let r = seg1.0 - seg2.0;
74
75    let a = d1.norm_squared();
76    let e = d2.norm_squared();
77    let f = d2.dot(&r);
78
79    let _0: N = na::zero();
80    let _1: N = na::one();
81
82    let mut s;
83    let mut t;
84    let mut parallel = false;
85
86    if a <= eps && e <= eps {
87        s = _0;
88        t = _0;
89    } else if a <= eps {
90        s = _0;
91        t = na::clamp(f / e, _0, _1);
92    } else {
93        let c = d1.dot(&r);
94        if e <= eps {
95            t = _0;
96            s = na::clamp(-c / a, _0, _1);
97        } else {
98            let b = d1.dot(&d2);
99            let ae = a * e;
100            let bb = b * b;
101            let denom = ae - bb;
102
103            parallel = denom <= eps || ulps_eq!(ae, bb);
104
105            // Use absolute and ulps error to test collinearity.
106            if !parallel {
107                s = na::clamp((b * f - c * e) / denom, _0, _1);
108            } else {
109                s = _0;
110            }
111
112            t = (b * s + f) / e;
113
114            if t < _0 {
115                t = _0;
116                s = na::clamp(-c / a, _0, _1);
117            } else if t > _1 {
118                t = _1;
119                s = na::clamp((b - c) / a, _0, _1);
120            }
121        }
122    }
123
124    let loc1 = if s == _0 {
125        SegmentPointLocation::OnVertex(0)
126    } else if s == _1 {
127        SegmentPointLocation::OnVertex(1)
128    } else {
129        SegmentPointLocation::OnEdge([_1 - s, s])
130    };
131
132    let loc2 = if t == _0 {
133        SegmentPointLocation::OnVertex(0)
134    } else if t == _1 {
135        SegmentPointLocation::OnVertex(1)
136    } else {
137        SegmentPointLocation::OnEdge([_1 - t, t])
138    };
139
140    (loc1, loc2, parallel)
141}
ncollide3d/query/closest_points/closest_points_segment_segment.rs

ncollide3d/query/closest_points/
closest_points_segment_segment.rs
