1//! Line intersection parameters.
2
3use crate::{
4 geometry::{Point, PointExt},
5 primitives::{
6 common::{LineSide, LinearEquation},
7 Line,
8 },
9};
10
11/// Intersection test result.
12#[derive(Copy, Clone, Debug, PartialEq)]
13#[cfg_attr(feature = "defmt", derive(::defmt::Format))]
14pub enum Intersection {
15 /// Intersection at point
16 Point {
17 /// Intersection point.
18 point: Point,
19
20 /// The "outer" side of the intersection, i.e. the side that has the joint's reflex angle.
21 ///
22 /// For example:
23 ///
24 /// ```text
25 /// # Left outer side:
26 ///
27 /// ⎯
28 /// ╱
29 ///
30 /// # Right outer side:
31 /// │
32 /// ╱
33 /// ```
34 ///
35 /// This is used to find the outside edge of a corner.
36 outer_side: LineSide,
37 },
38
39 /// No intersection: lines are colinear or parallel.
40 Colinear,
41}
42/// Line intersection parameters.
43#[derive(Debug, Copy, Clone, PartialEq)]
44#[cfg_attr(feature = "defmt", derive(::defmt::Format))]
45pub struct IntersectionParams<'a> {
46 line1: &'a Line,
47 line2: &'a Line,
48 le1: LinearEquation,
49 le2: LinearEquation,
50
51 /// Determinant, used to solve linear equations using Cramer's rule.
52 denominator: i32,
53}
54
55impl<'a> IntersectionParams<'a> {
56 pub fn from_lines(line1: &'a Line, line2: &'a Line) -> Self {
57 let le1 = LinearEquation::from_line(line1);
58 let le2 = LinearEquation::from_line(line2);
59 let denominator = le1.normal_vector.determinant(le2.normal_vector);
60
61 Self {
62 line1,
63 line2,
64 le1,
65 le2,
66 denominator,
67 }
68 }
69
70 /// Check whether two almost-colinear lines are intersecting in the wrong place due to numerical
71 /// inaccuracies.
72 pub fn nearly_colinear_has_error(&self) -> bool {
73 self.denominator.pow(2) < self.line1.delta().dot_product(self.line2.delta()).abs()
74 }
75
76 /// Compute the intersection point.
77 pub fn intersection(&self) -> Intersection {
78 let Self {
79 denominator,
80 le1: line1,
81 le2: line2,
82 ..
83 } = *self;
84
85 // The system of linear equations has no solutions if the determinant is zero. In this case,
86 // the lines must be colinear.
87 if denominator == 0 {
88 return Intersection::Colinear;
89 }
90
91 let outer_side = if denominator < 0 {
92 LineSide::Left
93 } else {
94 LineSide::Right
95 };
96
97 // If we got here, line segments intersect. Compute intersection point using method similar
98 // to that described here: http://paulbourke.net/geometry/pointlineplane/#i2l
99
100 // The denominator/2 is to get rounding instead of truncating.
101 let offset = denominator.abs() / 2;
102
103 let origin_distances = Point::new(line1.origin_distance, line2.origin_distance);
104
105 let numerator =
106 origin_distances.determinant(Point::new(line1.normal_vector.y, line2.normal_vector.y));
107 let x_numerator = if numerator < 0 {
108 numerator - offset
109 } else {
110 numerator + offset
111 };
112
113 let numerator =
114 Point::new(line1.normal_vector.x, line2.normal_vector.x).determinant(origin_distances);
115 let y_numerator = if numerator < 0 {
116 numerator - offset
117 } else {
118 numerator + offset
119 };
120
121 Intersection::Point {
122 point: Point::new(x_numerator, y_numerator) / denominator,
123 outer_side,
124 }
125 }
126}
127
128#[cfg(test)]
129mod tests {
130 use super::*;
131
132 #[test]
133 fn point_left() {
134 let line1 = Line::new(Point::new(50, 0), Point::new(20, 0));
135 let line2 = Line::new(Point::new(0, 20), Point::new(0, 50));
136
137 let params = IntersectionParams::from_lines(&line1, &line2);
138 assert_eq!(
139 params.intersection(),
140 Intersection::Point {
141 point: Point::zero(),
142 outer_side: LineSide::Left,
143 }
144 );
145 }
146
147 #[test]
148 fn point_right() {
149 let line1 = Line::new(Point::new(0, 50), Point::new(0, 20));
150 let line2 = Line::new(Point::new(20, 0), Point::new(50, 0));
151
152 let params = IntersectionParams::from_lines(&line1, &line2);
153 assert_eq!(
154 params.intersection(),
155 Intersection::Point {
156 point: Point::zero(),
157 outer_side: LineSide::Right,
158 }
159 );
160 }
161
162 #[test]
163 fn colinear() {
164 let line1 = Line::new(Point::new(0, 50), Point::new(0, 20));
165 let line2 = Line::new(Point::new(10, 20), Point::new(10, 50));
166
167 let params = IntersectionParams::from_lines(&line1, &line2);
168 assert_eq!(params.intersection(), Intersection::Colinear);
169 }
170}
171