1use std::f64::consts::PI;
2
3pub mod utils;
4pub use utils::Pose;
5pub use utils::normalize_angle_rad;
6
7#[derive(Debug, Clone, Copy, PartialEq)]
8pub enum Steering {
9 Left,
10 Right,
11 Straight,
12}
13
14impl Steering {
15 fn reverse(&self) -> Self {
16 match self {
17 Steering::Left => Steering::Right,
18 Steering::Right => Steering::Left,
19 Steering::Straight => Steering::Straight,
20 }
21 }
22}
23
24#[derive(Debug, Clone, Copy, PartialEq)]
25pub enum Gear {
26 Forward,
27 Backwards,
28}
29
30impl Gear {
31 fn reverse(&self) -> Self {
32 match self {
33 Gear::Forward => Gear::Backwards,
34 Gear::Backwards => Gear::Forward,
35 }
36 }
37}
38
39#[derive(Debug, Clone)]
40pub struct PathElement {
41 pub param: f64,
42 pub steering: Steering,
43 pub gear: Gear,
44}
45
46impl PathElement {
47 fn create(param: f64, steering: Steering, gear: Gear) -> Self {
48 if param >= 0. {
49 PathElement {
50 param,
51 steering,
52 gear,
53 }
54 } else {
55 PathElement {
56 param: -param,
57 steering,
58 gear: gear.reverse(),
59 }
60 }
61 }
62
63 fn reverse_steering(&mut self) {
64 self.steering = self.steering.reverse();
65 }
66
67 fn reverse_gear(&mut self) {
68 self.gear = self.gear.reverse();
69 }
70}
71
72pub type Path = Vec<PathElement>;
73
74pub fn get_optimal_path(start: Pose, end: Pose) -> Option<Path> {
75 let paths = get_all_paths(start, end);
76
77 paths
78 .into_iter()
79 .min_by(|a, b| path_length(a).partial_cmp(&path_length(b)).unwrap())
80}
81
82pub type PathFn = fn(f64, f64, f64) -> Path;
83pub const PATH_FNS: [PathFn; 12] = [
84 path1, path2, path3, path4, path5, path6, path7, path8, path9, path10, path11, path12,
85];
86
87pub fn get_all_paths(start: Pose, end: Pose) -> Vec<Path> {
88 let mut paths: Vec<Path> = Vec::new();
89
90 let pose = utils::change_of_basis(&start, &end);
91 let x = pose.x;
92 let y = pose.y;
93 let theta_degree = pose.theta_degree;
94 for get_path in PATH_FNS {
95 let p1 = get_path(x, y, theta_degree);
96 let p2 = timeflip(get_path(-x, y, -theta_degree));
97 let p3 = reflect(get_path(x, -y, -theta_degree));
98 let p4 = reflect(timeflip(get_path(-x, -y, theta_degree)));
99
100 paths.push(p1);
101 paths.push(p2);
102 paths.push(p3);
103 paths.push(p4);
104 }
105
106 paths
107 .into_iter()
108 .map(|path| {
109 path.into_iter()
110 .filter(|e| e.param.abs() > 1e-10)
111 .collect::<Path>()
112 })
113 .filter(|path| !path.is_empty())
114 .collect()
115}
116
117pub fn timeflip(path: Path) -> Path {
118 path.into_iter()
119 .map(|mut e| {
120 e.reverse_gear();
121 e
122 })
123 .collect()
124}
125
126pub fn reflect(path: Path) -> Path {
127 path.into_iter()
128 .map(|mut e| {
129 e.reverse_steering();
130 e
131 })
132 .collect()
133}
134
135pub fn path_length(path: &Path) -> f64 {
136 path.iter().map(|e| e.param.abs()).sum()
137}
138
139fn path1(x: f64, y: f64, phi_degree: f64) -> Path {
140 let phi_radians = utils::degree_to_radians(phi_degree);
141 let polar = utils::cartesian_to_polar(x - phi_radians.sin(), y - 1. + phi_radians.cos());
142 let v = utils::normalize_angle_rad(phi_radians - polar.theta);
143
144 vec![
145 PathElement::create(polar.theta, Steering::Left, Gear::Forward),
146 PathElement::create(polar.rho, Steering::Straight, Gear::Forward),
147 PathElement::create(v, Steering::Left, Gear::Forward),
148 ]
149}
150
151fn path2(x: f64, y: f64, phi_degree: f64) -> Path {
152 let phi_radians = utils::normalize_angle_rad(utils::degree_to_radians(phi_degree));
153 let polar = utils::cartesian_to_polar(x + phi_radians.sin(), y - 1. - phi_radians.cos());
154
155 let rho = polar.rho;
156 let theta = polar.theta;
157
158 if rho * rho >= 4. {
159 let u = (rho * rho - 4.).sqrt();
160 let t = utils::normalize_angle_rad(theta + (2.0_f64).atan2(u));
161 let v = utils::normalize_angle_rad(t - phi_radians);
162
163 vec![
164 PathElement::create(t, Steering::Left, Gear::Forward),
165 PathElement::create(u, Steering::Straight, Gear::Forward),
166 PathElement::create(v, Steering::Right, Gear::Forward),
167 ]
168 } else {
169 Vec::new()
170 }
171}
172
173fn path3(x: f64, y: f64, phi_degree: f64) -> Path {
174 let phi_radians = utils::degree_to_radians(phi_degree);
175
176 let xi = x - phi_radians.sin();
177 let eta = y - 1. + phi_radians.cos();
178 let polar = utils::cartesian_to_polar(xi, eta);
179
180 let rho = polar.rho;
181 let theta = polar.theta;
182
183 if polar.rho <= 4. {
184 let a = (rho / 4.).acos();
185 let t = utils::normalize_angle_rad(theta + PI / 2. + a);
186 let u = utils::normalize_angle_rad(PI - 2. * a);
187 let v = utils::normalize_angle_rad(phi_radians - t - u);
188
189 vec![
190 PathElement::create(t, Steering::Left, Gear::Forward),
191 PathElement::create(u, Steering::Right, Gear::Backwards),
192 PathElement::create(v, Steering::Left, Gear::Forward),
193 ]
194 } else {
195 Vec::new()
196 }
197}
198
199fn path4(x: f64, y: f64, phi_degree: f64) -> Path {
200 let phi_radians = utils::degree_to_radians(phi_degree);
201
202 let xi = x - phi_radians.sin();
203 let eta = y - 1. + phi_radians.cos();
204 let polar = utils::cartesian_to_polar(xi, eta);
205
206 let rho = polar.rho;
207 let theta = polar.theta;
208
209 if rho <= 4. {
210 let a = (rho / 4.).acos();
211 let t = utils::normalize_angle_rad(theta + PI / 2. + a);
212 let u = utils::normalize_angle_rad(PI - 2. * a);
213 let v = utils::normalize_angle_rad(t + u - phi_radians);
214
215 vec![
216 PathElement::create(t, Steering::Left, Gear::Forward),
217 PathElement::create(u, Steering::Right, Gear::Backwards),
218 PathElement::create(v, Steering::Left, Gear::Backwards),
219 ]
220 } else {
221 Vec::new()
222 }
223}
224
225fn path5(x: f64, y: f64, phi_degree: f64) -> Path {
226 let phi_radians = utils::degree_to_radians(phi_degree);
227
228 let xi = x - phi_radians.sin();
229 let eta = y - 1. + phi_radians.cos();
230 let polar = utils::cartesian_to_polar(xi, eta);
231
232 let rho = polar.rho;
233 let theta = polar.theta;
234
235 if rho <= 4. {
236 let u = (1. - rho * rho / 8.).acos();
237 let a = (2. * u.sin() / rho).asin();
238 let t = utils::normalize_angle_rad(theta + PI / 2. - a);
239 let v = utils::normalize_angle_rad(t - u - phi_radians);
240
241 vec![
242 PathElement::create(t, Steering::Left, Gear::Forward),
243 PathElement::create(u, Steering::Right, Gear::Forward),
244 PathElement::create(v, Steering::Left, Gear::Backwards),
245 ]
246 } else {
247 Vec::new()
248 }
249}
250
251fn path6(x: f64, y: f64, phi_degree: f64) -> Path {
252 let phi_radians = utils::degree_to_radians(phi_degree);
253
254 let xi = x + phi_radians.sin();
255 let eta = y - 1. - phi_radians.cos();
256 let polar = utils::cartesian_to_polar(xi, eta);
257
258 let rho = polar.rho;
259 let theta = polar.theta;
260
261 if rho <= 4. {
262 let (t, u, v);
263 if rho <= 2. {
264 let a = ((rho + 2.) / 4.).acos();
265 t = utils::normalize_angle_rad(theta + PI / 2. + a);
266 u = utils::normalize_angle_rad(a);
267 v = utils::normalize_angle_rad(phi_radians - t + 2. * u);
268 } else {
269 let a = ((rho - 2.) / 4.).acos();
270 t = utils::normalize_angle_rad(theta + PI / 2. - a);
271 u = utils::normalize_angle_rad(PI - a);
272 v = utils::normalize_angle_rad(phi_radians - t + 2. * u);
273 }
274
275 vec![
276 PathElement::create(t, Steering::Left, Gear::Forward),
277 PathElement::create(u, Steering::Right, Gear::Forward),
278 PathElement::create(u, Steering::Left, Gear::Backwards),
279 PathElement::create(v, Steering::Right, Gear::Backwards),
280 ]
281 } else {
282 Vec::new()
283 }
284}
285
286fn path7(x: f64, y: f64, phi_degree: f64) -> Path {
287 let phi_radians = utils::degree_to_radians(phi_degree);
288
289 let xi = x + phi_radians.sin();
290 let eta = y - 1. - phi_radians.cos();
291 let polar = utils::cartesian_to_polar(xi, eta);
292
293 let rho = polar.rho;
294 let theta = polar.theta;
295
296 let u1 = (20. - rho * rho) / 16.;
297
298 if rho <= 6. && (0. ..=1.).contains(&u1) {
299 let u = u1.acos();
300 let asin_arg = (2. * u.sin() / rho).clamp(-1.0, 1.0);
301 let a = asin_arg.asin();
302 let t = utils::normalize_angle_rad(theta + PI / 2. + a);
303 let v = utils::normalize_angle_rad(t - phi_radians);
304
305 vec![
306 PathElement::create(t, Steering::Left, Gear::Forward),
307 PathElement::create(u, Steering::Right, Gear::Backwards),
308 PathElement::create(u, Steering::Left, Gear::Backwards),
309 PathElement::create(v, Steering::Right, Gear::Forward),
310 ]
311 } else {
312 Vec::new()
313 }
314}
315
316fn path8(x: f64, y: f64, phi_degree: f64) -> Path {
317 let phi_radians = utils::degree_to_radians(phi_degree);
318
319 let xi = x - phi_radians.sin();
320 let eta = y - 1. + phi_radians.cos();
321 let polar = utils::cartesian_to_polar(xi, eta);
322
323 let rho = polar.rho;
324 let theta = polar.theta;
325
326 if rho >= 2. {
327 let sqrt_arg = rho * rho - 4.;
328 if sqrt_arg < 0. {
329 return Vec::new();
330 }
331
332 let s = sqrt_arg.sqrt();
333 let u_param = s - 2.;
334
335 let a = (2.0_f64).atan2(s);
336 let t = utils::normalize_angle_rad(theta + PI / 2. + a);
337 let v = utils::normalize_angle_rad(t - phi_radians + PI / 2.);
338
339 vec![
340 PathElement::create(t, Steering::Left, Gear::Forward),
341 PathElement::create(PI / 2., Steering::Right, Gear::Backwards),
342 PathElement::create(u_param, Steering::Straight, Gear::Backwards),
343 PathElement::create(v, Steering::Left, Gear::Backwards),
344 ]
345 } else {
346 Vec::new()
347 }
348}
349
350fn path9(x: f64, y: f64, phi_degree: f64) -> Path {
351 let phi_radians = utils::degree_to_radians(phi_degree);
352
353 let xi = x - phi_radians.sin();
354 let eta = y - 1. + phi_radians.cos();
355 let polar = utils::cartesian_to_polar(xi, eta);
356
357 let rho = polar.rho;
358 let theta = polar.theta;
359
360 if rho >= 2. {
361 let sqrt_arg = rho * rho - 4.;
362 if sqrt_arg < 0. {
363 return Vec::new();
364 }
365
366 let s = sqrt_arg.sqrt();
367 let u_param = s - 2.;
368
369 let a = s.atan2(2.0_f64);
370 let t = utils::normalize_angle_rad(theta + PI / 2. - a);
371 let v = utils::normalize_angle_rad(t - phi_radians - PI / 2.);
372
373 vec![
374 PathElement::create(t, Steering::Left, Gear::Forward),
375 PathElement::create(u_param, Steering::Straight, Gear::Forward),
376 PathElement::create(PI / 2., Steering::Right, Gear::Forward),
377 PathElement::create(v, Steering::Left, Gear::Backwards),
378 ]
379 } else {
380 Vec::new()
381 }
382}
383
384fn path10(x: f64, y: f64, phi_degree: f64) -> Path {
385 let phi_radians = utils::degree_to_radians(phi_degree);
386
387 let xi = x + phi_radians.sin();
388 let eta = y - 1. - phi_radians.cos();
389 let polar = utils::cartesian_to_polar(xi, eta);
390
391 let rho = polar.rho;
392 let theta = polar.theta;
393
394 if rho >= 2. {
395 let t = utils::normalize_angle_rad(theta + PI / 2.);
396 let u = rho - 2.;
397 let v = utils::normalize_angle_rad(phi_radians - t - PI / 2.);
398
399 vec![
400 PathElement::create(t, Steering::Left, Gear::Forward),
401 PathElement::create(PI / 2., Steering::Right, Gear::Backwards),
402 PathElement::create(u, Steering::Straight, Gear::Backwards),
403 PathElement::create(v, Steering::Right, Gear::Backwards),
404 ]
405 } else {
406 Vec::new()
407 }
408}
409
410fn path11(x: f64, y: f64, phi_degree: f64) -> Path {
411 let phi_radians = utils::degree_to_radians(phi_degree);
412
413 let xi = x + phi_radians.sin();
414 let eta = y - 1. - phi_radians.cos();
415 let polar = utils::cartesian_to_polar(xi, eta);
416
417 let rho = polar.rho;
418 let theta = polar.theta;
419
420 if rho >= 2. {
421 let t = utils::normalize_angle_rad(theta);
422 let u = rho - 2.;
423 let v = utils::normalize_angle_rad(phi_radians - t - PI / 2.);
424
425 vec![
426 PathElement::create(t, Steering::Left, Gear::Forward),
427 PathElement::create(u, Steering::Straight, Gear::Forward),
428 PathElement::create(PI / 2., Steering::Left, Gear::Forward),
429 PathElement::create(v, Steering::Right, Gear::Backwards),
430 ]
431 } else {
432 Vec::new()
433 }
434}
435
436fn path12(x: f64, y: f64, phi_degree: f64) -> Path {
437 let phi_radians = utils::degree_to_radians(phi_degree);
438
439 let xi = x + phi_radians.sin();
440 let eta = y - 1. - phi_radians.cos();
441 let polar = utils::cartesian_to_polar(xi, eta);
442
443 let rho = polar.rho;
444 let theta = polar.theta;
445
446 if rho >= 4. {
447 let sqrt_base_arg = rho * rho - 4.;
448 if sqrt_base_arg < 0. {
449 return Vec::new();
450 }
451
452 let u_base = sqrt_base_arg.sqrt();
453 let u_param = u_base - 4.;
454
455 let s_equiv = u_base;
456
457 let a = (2.0_f64).atan2(s_equiv);
458 let t = utils::normalize_angle_rad(theta + PI / 2. + a);
459 let v = utils::normalize_angle_rad(t - phi_radians);
460
461 vec![
462 PathElement::create(t, Steering::Left, Gear::Forward),
463 PathElement::create(PI / 2., Steering::Right, Gear::Backwards),
464 PathElement::create(u_param, Steering::Straight, Gear::Backwards),
465 PathElement::create(PI / 2., Steering::Left, Gear::Backwards),
466 PathElement::create(v, Steering::Right, Gear::Forward),
467 ]
468 } else {
469 Vec::new()
470 }
471}