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 == denom {
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 == denom {
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    // If ab not vertical, check betweenness on x; else on y.
148    // TODO: handle cases where we actually are on a vertex (to return OnEdge instead of OnVertex)?
149    if a.x != b.x {
150        if a.x <= c.x && c.x <= b.x {
151            let bcoord = (c.x - a.x) / (b.x - a.x);
152            return Some(SegmentPointLocation::OnEdge([1.0 - bcoord, bcoord]));
153        } else if a.x >= c.x && c.x >= b.x {
154            let bcoord = (c.x - b.x) / (a.x - b.x);
155            return Some(SegmentPointLocation::OnEdge([bcoord, 1.0 - bcoord]));
156        }
157    } else if a.y != b.y {
158        if a.y <= c.y && c.y <= b.y {
159            let bcoord = (c.y - a.y) / (b.y - a.y);
160            return Some(SegmentPointLocation::OnEdge([1.0 - bcoord, bcoord]));
161        } else if a.y >= c.y && c.y >= b.y {
162            let bcoord = (c.y - b.y) / (a.y - b.y);
163            return Some(SegmentPointLocation::OnEdge([bcoord, 1.0 - bcoord]));
164        }
165    } else if a.x == c.x && a.y == c.y {
166        return Some(SegmentPointLocation::OnVertex(0));
167    }
168
169    None
170}
parry2d/utils/segments_intersection.rs

parry2d/utils/
segments_intersection.rs
