parry3d/utils/
segments_intersection.rs

1use na::Point2;
2
3use crate::math::Real;
4use crate::shape::{SegmentPointLocation, Triangle, TriangleOrientation};
5
6#[cfg(not(feature = "alloc"))]
7use na::ComplexField;
8
9/// Intersection between two segments.
10pub enum SegmentsIntersection {
11    /// Single point of intersection.
12    Point {
13        /// Location of the intersection point on the first segment.
14        loc1: SegmentPointLocation,
15        /// Location of the intersection point on the second segment.
16        loc2: SegmentPointLocation,
17    },
18    /// Intersection along a segment (when both segments are collinear).
19    Segment {
20        /// Location of the first intersection point on the first segment.
21        first_loc1: SegmentPointLocation,
22        /// Location of the first intersection point on the second segment.
23        first_loc2: SegmentPointLocation,
24        /// Location of the second intersection point on the first segment.
25        second_loc1: SegmentPointLocation,
26        /// Location of the second intersection point on the second segment.
27        second_loc2: SegmentPointLocation,
28    },
29}
30
31/// Computes the intersection between two segments.
32pub fn segments_intersection2d(
33    a: &Point2<Real>,
34    b: &Point2<Real>,
35    c: &Point2<Real>,
36    d: &Point2<Real>,
37    epsilon: Real,
38) -> Option<SegmentsIntersection> {
39    let denom = a.x * (d.y - c.y) + b.x * (c.y - d.y) + d.x * (b.y - a.y) + c.x * (a.y - b.y);
40
41    // If denom is zero, then segments are parallel: handle separately.
42    if denom.abs() < epsilon || ulps_eq!(denom, 0.0) {
43        return parallel_intersection(a, b, c, d, epsilon);
44    }
45
46    let num = a.x * (d.y - c.y) + c.x * (a.y - d.y) + d.x * (c.y - a.y);
47    let s = num / denom;
48
49    let num = -(a.x * (c.y - b.y) + b.x * (a.y - c.y) + c.x * (b.y - a.y));
50    let t = num / denom;
51
52    if 0.0 > s || s > 1.0 || 0.0 > t || t > 1.0 {
53        None
54    } else {
55        let loc1 = if s == 0.0 {
56            SegmentPointLocation::OnVertex(0)
57        } else if s == denom {
58            SegmentPointLocation::OnVertex(1)
59        } else {
60            SegmentPointLocation::OnEdge([1.0 - s, s])
61        };
62
63        let loc2 = if t == 0.0 {
64            SegmentPointLocation::OnVertex(0)
65        } else if t == denom {
66            SegmentPointLocation::OnVertex(1)
67        } else {
68            SegmentPointLocation::OnEdge([1.0 - t, t])
69        };
70
71        Some(SegmentsIntersection::Point { loc1, loc2 })
72    }
73}
74
75fn parallel_intersection(
76    a: &Point2<Real>,
77    b: &Point2<Real>,
78    c: &Point2<Real>,
79    d: &Point2<Real>,
80    epsilon: Real,
81) -> Option<SegmentsIntersection> {
82    if Triangle::orientation2d(a, b, c, epsilon) != TriangleOrientation::Degenerate {
83        return None;
84    }
85
86    let ab_c = between(a, b, c);
87    let ab_d = between(a, b, d);
88    if let (Some(loc1), Some(loc2)) = (ab_c, ab_d) {
89        return Some(SegmentsIntersection::Segment {
90            first_loc1: loc1,
91            first_loc2: SegmentPointLocation::OnVertex(0),
92            second_loc1: loc2,
93            second_loc2: SegmentPointLocation::OnVertex(1),
94        });
95    }
96
97    let cd_a = between(c, d, a);
98    let cd_b = between(c, d, b);
99    if let (Some(loc1), Some(loc2)) = (cd_a, cd_b) {
100        return Some(SegmentsIntersection::Segment {
101            first_loc1: SegmentPointLocation::OnVertex(0),
102            first_loc2: loc1,
103            second_loc1: SegmentPointLocation::OnVertex(1),
104            second_loc2: loc2,
105        });
106    }
107
108    if let (Some(loc1), Some(loc2)) = (ab_c, cd_b) {
109        return Some(SegmentsIntersection::Segment {
110            first_loc1: loc1,
111            first_loc2: SegmentPointLocation::OnVertex(0),
112            second_loc1: SegmentPointLocation::OnVertex(1),
113            second_loc2: loc2,
114        });
115    }
116
117    if let (Some(loc1), Some(loc2)) = (ab_c, cd_a) {
118        return Some(SegmentsIntersection::Segment {
119            first_loc1: loc1,
120            first_loc2: SegmentPointLocation::OnVertex(0),
121            second_loc1: SegmentPointLocation::OnVertex(0),
122            second_loc2: loc2,
123        });
124    }
125
126    if let (Some(loc1), Some(loc2)) = (ab_d, cd_b) {
127        return Some(SegmentsIntersection::Segment {
128            first_loc1: loc1,
129            first_loc2: SegmentPointLocation::OnVertex(1),
130            second_loc1: SegmentPointLocation::OnVertex(1),
131            second_loc2: loc2,
132        });
133    }
134
135    if let (Some(loc1), Some(loc2)) = (ab_d, cd_a) {
136        return Some(SegmentsIntersection::Segment {
137            first_loc1: loc1,
138            first_loc2: SegmentPointLocation::OnVertex(1),
139            second_loc1: SegmentPointLocation::OnVertex(0),
140            second_loc2: loc2,
141        });
142    }
143
144    None
145}
146
147// Checks that `c` is in-between `a` and `b`.
148// Assumes the three points are collinear.
149fn between(a: &Point2<Real>, b: &Point2<Real>, c: &Point2<Real>) -> Option<SegmentPointLocation> {
150    // If ab not vertical, check betweenness on x; else on y.
151    // TODO: handle cases where we actually are on a vertex (to return OnEdge instead of OnVertex)?
152    if a.x != b.x {
153        if a.x <= c.x && c.x <= b.x {
154            let bcoord = (c.x - a.x) / (b.x - a.x);
155            return Some(SegmentPointLocation::OnEdge([1.0 - bcoord, bcoord]));
156        } else if a.x >= c.x && c.x >= b.x {
157            let bcoord = (c.x - b.x) / (a.x - b.x);
158            return Some(SegmentPointLocation::OnEdge([bcoord, 1.0 - bcoord]));
159        }
160    } else if a.y != b.y {
161        if a.y <= c.y && c.y <= b.y {
162            let bcoord = (c.y - a.y) / (b.y - a.y);
163            return Some(SegmentPointLocation::OnEdge([1.0 - bcoord, bcoord]));
164        } else if a.y >= c.y && c.y >= b.y {
165            let bcoord = (c.y - b.y) / (a.y - b.y);
166            return Some(SegmentPointLocation::OnEdge([bcoord, 1.0 - bcoord]));
167        }
168    } else if a.x == c.x && a.y == c.y {
169        return Some(SegmentPointLocation::OnVertex(0));
170    }
171
172    None
173}
parry3d/utils/segments_intersection.rs

parry3d/utils/
segments_intersection.rs
