parry2d/utils/
segments_intersection.rs

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

parry2d/utils/
segments_intersection.rs
