1use crate::{
2 geometry::{Angle, Point, PointExt, Real, Trigonometry},
3 primitives::{common::LineSide, Line},
4};
5
6/// Scaling factor for unit length normal vectors.
7pub const NORMAL_VECTOR_SCALE: i32 = 1 << 10;
8
9/// Linear equation.
10///
11/// The equation is stored as a normal vector and the distance to the origin.
12#[derive(Copy, Clone, PartialEq, PartialOrd, Debug)]
13#[cfg_attr(feature = "defmt", derive(::defmt::Format))]
14pub struct LinearEquation {
15 /// Normal vector, perpendicular to the line.
16 ///
17 /// The unit vector is scaled up to increase the resolution.
18 pub normal_vector: Point,
19
20 /// Distance from the origin.
21 ///
22 /// The distance doesn't directly correlate to the distance in pixels, but is
23 /// scaled up by the length of the normal vector.
24 pub origin_distance: i32,
25}
26
27impl LinearEquation {
28 /// Creates a new linear equation with the given angle and distance to the origin.
29 pub fn with_angle_and_distance(angle: Angle, origin_distance: i32) -> Self {
30 Self {
31 normal_vector: OriginLinearEquation::with_angle(angle).normal_vector,
32 origin_distance,
33 }
34 }
35
36 /// Creates a new linear equation from a line.
37 pub fn from_line(line: &Line) -> Self {
38 let normal_vector = line.delta().rotate_90();
39 let origin_distance = line.start.dot_product(normal_vector);
40
41 Self {
42 normal_vector,
43 origin_distance,
44 }
45 }
46
47 /// Returns the distance between the line and a point.
48 ///
49 /// The scaling of the returned value depends on the length of the normal vector.
50 /// Positive values will be returned for points on the left side of the line and negative
51 /// values for points on the right.
52 pub fn distance(&self, point: Point) -> i32 {
53 point.dot_product(self.normal_vector) - self.origin_distance
54 }
55
56 /// Checks if a point is on the given side of the line.
57 ///
58 /// Always returns `true` if the point is on the line.
59 pub fn check_side(&self, point: Point, side: LineSide) -> bool {
60 let distance = self.distance(point);
61
62 match side {
63 LineSide::Left => distance <= 0,
64 LineSide::Right => distance >= 0,
65 }
66 }
67}
68
69/// Linear equation with zero distance to the origin.
70#[derive(Copy, Clone, PartialEq, PartialOrd, Debug)]
71#[cfg_attr(feature = "defmt", derive(::defmt::Format))]
72pub struct OriginLinearEquation {
73 pub normal_vector: Point,
74}
75
76impl OriginLinearEquation {
77 /// Creates a new linear equation with the given angle.
78 pub fn with_angle(angle: Angle) -> Self {
79 // FIXME: angle.tan() for 180.0 degrees isn't exactly 0 which causes problems when drawing
80 // a single quadrant. Is there a better solution to fix this?
81 let normal_vector = if angle == Angle::from_degrees(180.0) {
82 Point::new(0, -NORMAL_VECTOR_SCALE)
83 } else {
84 Point::new(
85 i32::from(angle.cos() * Real::from(NORMAL_VECTOR_SCALE)),
86 i32::from(angle.sin() * Real::from(NORMAL_VECTOR_SCALE)),
87 )
88 .rotate_90()
89 };
90
91 Self { normal_vector }
92 }
93
94 /// Creates a new horizontal linear equation.
95 pub const fn new_horizontal() -> Self {
96 Self {
97 normal_vector: Point::new(0, NORMAL_VECTOR_SCALE),
98 }
99 }
100
101 /// Returns the distance between the line and a point.
102 ///
103 /// The scaling of the returned value depends on the length of the normal vector.
104 /// Positive values will be returned for points on the right side of the line and negative
105 /// values for points on the left.
106 pub fn distance(&self, point: Point) -> i32 {
107 point.dot_product(self.normal_vector)
108 }
109
110 /// Checks if a point is on the given side of the line.
111 ///
112 /// Always returns `true` if the point is on the line.
113 pub fn check_side(&self, point: Point, side: LineSide) -> bool {
114 let distance = self.distance(point);
115
116 match side {
117 LineSide::Left => distance <= 0,
118 LineSide::Right => distance >= 0,
119 }
120 }
121}
122
123#[cfg(test)]
124mod tests {
125 use super::*;
126 use crate::geometry::AngleUnit;
127
128 #[test]
129 fn from_line() {
130 assert_eq!(
131 LinearEquation::from_line(&Line::new(Point::zero(), Point::new(1, 0))),
132 LinearEquation {
133 normal_vector: Point::new(0, 1),
134 origin_distance: 0, // line goes through the origin
135 }
136 );
137
138 assert_eq!(
139 LinearEquation::from_line(&Line::new(Point::zero(), Point::new(0, 1))),
140 LinearEquation {
141 normal_vector: Point::new(-1, 0),
142 origin_distance: 0, // line goes through the origin
143 }
144 );
145
146 assert_eq!(
147 LinearEquation::from_line(&Line::new(Point::new(2, 3), Point::new(-2, 3))),
148 LinearEquation {
149 normal_vector: Point::new(0, -4),
150 // origin_distance = min. distance between line and origin * length of unit vector
151 // = 3 * 4
152 origin_distance: -12,
153 }
154 );
155 }
156
157 #[test]
158 fn with_angle() {
159 assert_eq!(
160 OriginLinearEquation::with_angle(0.0.deg()),
161 OriginLinearEquation {
162 normal_vector: Point::new(0, NORMAL_VECTOR_SCALE),
163 }
164 );
165
166 assert_eq!(
167 OriginLinearEquation::with_angle(90.0.deg()),
168 OriginLinearEquation {
169 normal_vector: Point::new(-NORMAL_VECTOR_SCALE, 0),
170 }
171 );
172 }
173
174 #[test]
175 fn distance() {
176 let line = OriginLinearEquation::with_angle(90.0.deg());
177 assert_eq!(line.distance(Point::new(-1, 0)), NORMAL_VECTOR_SCALE);
178 assert_eq!(line.distance(Point::new(1, 0)), -NORMAL_VECTOR_SCALE);
179 }
180
181 #[test]
182 fn check_side_horizontal_0deg() {
183 let eq1 = OriginLinearEquation::with_angle(0.0.deg());
184 let eq2 = LinearEquation::from_line(&Line::with_delta(Point::zero(), Point::new(10, 0)));
185
186 use LineSide::*;
187 for (point, side, expected) in [
188 ((0, 0), Left, true),
189 ((1, 0), Right, true),
190 ((-2, 1), Left, false),
191 ((3, 1), Right, true),
192 ((-4, -1), Left, true),
193 ((5, -1), Right, false),
194 ]
195 .into_iter()
196 {
197 assert_eq!(
198 eq1.check_side(point.into(), side),
199 expected,
200 "{:?}, {:?}",
201 point,
202 side
203 );
204
205 assert_eq!(
206 eq2.check_side(point.into(), side),
207 expected,
208 "{:?}, {:?}",
209 point,
210 side
211 );
212 }
213 }
214
215 #[test]
216 fn check_side_horizontal_180deg() {
217 let eq1 = OriginLinearEquation::with_angle(180.0.deg());
218 let eq2 = LinearEquation::from_line(&Line::with_delta(Point::zero(), Point::new(-10, 0)));
219
220 use LineSide::*;
221 for (point, side, expected) in [
222 ((0, 0), Left, true),
223 ((1, 0), Right, true),
224 ((-2, 1), Left, true),
225 ((3, 1), Right, false),
226 ((-4, -1), Left, false),
227 ((5, -1), Right, true),
228 ]
229 .into_iter()
230 {
231 assert_eq!(
232 eq1.check_side(point.into(), side),
233 expected,
234 "{:?}, {:?}",
235 point,
236 side
237 );
238
239 assert_eq!(
240 eq2.check_side(point.into(), side),
241 expected,
242 "{:?}, {:?}",
243 point,
244 side
245 );
246 }
247 }
248
249 #[test]
250 fn check_side_vertical_90deg() {
251 let eq1 = OriginLinearEquation::with_angle(90.0.deg());
252 let eq2 = LinearEquation::from_line(&Line::with_delta(Point::zero(), Point::new(0, 10)));
253
254 use LineSide::*;
255 for (point, side, expected) in [
256 ((0, 0), Left, true),
257 ((0, 1), Right, true),
258 ((-1, -2), Left, false),
259 ((-1, 3), Right, true),
260 ((1, -4), Left, true),
261 ((1, 5), Right, false),
262 ]
263 .into_iter()
264 {
265 assert_eq!(
266 eq1.check_side(point.into(), side),
267 expected,
268 "{:?}, {:?}",
269 point,
270 side
271 );
272
273 assert_eq!(
274 eq2.check_side(point.into(), side),
275 expected,
276 "{:?}, {:?}",
277 point,
278 side
279 );
280 }
281 }
282
283 #[test]
284 fn check_side_vertical_270deg() {
285 let eq1 = OriginLinearEquation::with_angle(270.0.deg());
286 let eq2 = LinearEquation::from_line(&Line::with_delta(Point::zero(), Point::new(0, -10)));
287
288 use LineSide::*;
289 for (point, side, expected) in [
290 ((0, 0), Left, true),
291 ((0, -1), Right, true),
292 ((-1, 2), Left, true),
293 ((-1, -3), Right, false),
294 ((1, 4), Left, false),
295 ((1, -5), Right, true),
296 ]
297 .into_iter()
298 {
299 assert_eq!(
300 eq1.check_side(point.into(), side),
301 expected,
302 "{:?}, {:?}",
303 point,
304 side
305 );
306
307 assert_eq!(
308 eq2.check_side(point.into(), side),
309 expected,
310 "{:?}, {:?}",
311 point,
312 side
313 );
314 }
315 }
316}
317