Skip to main content

rust_robotics_planning/
reeds_shepp_path.rs

1#![allow(
2    dead_code,
3    clippy::needless_borrows_for_generic_args,
4    clippy::new_without_default,
5    clippy::type_complexity,
6    clippy::too_many_arguments,
7    clippy::assign_op_pattern
8)]
9
10//! Reeds-Shepp Path Planner
11//!
12//! Computes optimal paths for a car-like robot that can move both
13//! forward and backward, using combinations of straight and arc segments.
14
15use rust_robotics_core::normalize_angle;
16use std::f64::consts::PI;
17
18#[derive(Debug, Clone)]
19pub struct Path {
20    pub lengths: Vec<f64>,
21    pub ctypes: Vec<char>,
22    pub l: f64,
23    pub x: Vec<f64>,
24    pub y: Vec<f64>,
25    pub yaw: Vec<f64>,
26    pub directions: Vec<i32>,
27}
28
29impl Path {
30    pub fn new() -> Self {
31        Path {
32            lengths: Vec::new(),
33            ctypes: Vec::new(),
34            l: 0.0,
35            x: Vec::new(),
36            y: Vec::new(),
37            yaw: Vec::new(),
38            directions: Vec::new(),
39        }
40    }
41}
42
43fn mod2pi(x: f64) -> f64 {
44    let v = x % (2.0 * PI);
45    if v < -PI {
46        v + 2.0 * PI
47    } else if v > PI {
48        v - 2.0 * PI
49    } else {
50        v
51    }
52}
53
54fn polar(x: f64, y: f64) -> (f64, f64) {
55    let r = (x * x + y * y).sqrt();
56    let theta = y.atan2(x);
57    (r, theta)
58}
59
60fn left_straight_left(x: f64, y: f64, phi: f64) -> (bool, Vec<f64>, Vec<char>) {
61    let (u, t) = polar(x - phi.sin(), y - 1.0 + phi.cos());
62    if (0.0..=PI).contains(&t) {
63        let v = mod2pi(phi - t);
64        if (0.0..=PI).contains(&v) {
65            return (true, vec![t, u, v], vec!['L', 'S', 'L']);
66        }
67    }
68    (false, Vec::new(), Vec::new())
69}
70
71fn left_straight_right(x: f64, y: f64, phi: f64) -> (bool, Vec<f64>, Vec<char>) {
72    let (u1, t1) = polar(x + phi.sin(), y - 1.0 - phi.cos());
73    let u1_sq = u1 * u1;
74    if u1_sq >= 4.0 {
75        let u = (u1_sq - 4.0).sqrt();
76        let theta = (2.0_f64).atan2(u);
77        let t = mod2pi(t1 + theta);
78        let v = mod2pi(t - phi);
79
80        if t >= 0.0 && v >= 0.0 {
81            return (true, vec![t, u, v], vec!['L', 'S', 'R']);
82        }
83    }
84    (false, Vec::new(), Vec::new())
85}
86
87fn left_x_right_x_left(x: f64, y: f64, phi: f64) -> (bool, Vec<f64>, Vec<char>) {
88    let zeta = x - phi.sin();
89    let eeta = y - 1.0 + phi.cos();
90    let (u1, theta) = polar(zeta, eeta);
91
92    if u1 <= 4.0 {
93        let a = (0.25 * u1).acos();
94        let t = mod2pi(a + theta + PI / 2.0);
95        let u = mod2pi(PI - 2.0 * a);
96        let v = mod2pi(phi - t - u);
97        return (true, vec![t, -u, v], vec!['L', 'R', 'L']);
98    }
99    (false, Vec::new(), Vec::new())
100}
101
102fn left_x_right_left(x: f64, y: f64, phi: f64) -> (bool, Vec<f64>, Vec<char>) {
103    let zeta = x - phi.sin();
104    let eeta = y - 1.0 + phi.cos();
105    let (u1, theta) = polar(zeta, eeta);
106
107    if u1 <= 4.0 {
108        let a = (0.25 * u1).acos();
109        let t = mod2pi(a + theta + PI / 2.0);
110        let u = mod2pi(PI - 2.0 * a);
111        let v = mod2pi(-phi + t + u);
112        return (true, vec![t, -u, -v], vec!['L', 'R', 'L']);
113    }
114    (false, Vec::new(), Vec::new())
115}
116
117fn left_right_x_left(x: f64, y: f64, phi: f64) -> (bool, Vec<f64>, Vec<char>) {
118    let zeta = x - phi.sin();
119    let eeta = y - 1.0 + phi.cos();
120    let (u1, theta) = polar(zeta, eeta);
121
122    if u1 <= 4.0 {
123        let u = (1.0 - u1 * u1 * 0.125).acos();
124        let a = (2.0 * u.sin() / u1).asin();
125        let t = mod2pi(-a + theta + PI / 2.0);
126        let v = mod2pi(t - u - phi);
127        return (true, vec![t, u, -v], vec!['L', 'R', 'L']);
128    }
129    (false, Vec::new(), Vec::new())
130}
131
132fn left_right_x_left_right(x: f64, y: f64, phi: f64) -> (bool, Vec<f64>, Vec<char>) {
133    let zeta = x + phi.sin();
134    let eeta = y - 1.0 - phi.cos();
135    let (u1, theta) = polar(zeta, eeta);
136
137    if u1 <= 2.0 {
138        let a = ((u1 + 2.0) * 0.25).acos();
139        let t = mod2pi(theta + a + PI / 2.0);
140        let u = mod2pi(a);
141        let v = mod2pi(phi - t + 2.0 * u);
142        if t >= 0.0 && u >= 0.0 && v >= 0.0 {
143            return (true, vec![t, u, -u, -v], vec!['L', 'R', 'L', 'R']);
144        }
145    }
146    (false, Vec::new(), Vec::new())
147}
148
149fn left_x_right_left_x_right(x: f64, y: f64, phi: f64) -> (bool, Vec<f64>, Vec<char>) {
150    let zeta = x + phi.sin();
151    let eeta = y - 1.0 - phi.cos();
152    let (u1, theta) = polar(zeta, eeta);
153    let u2 = (20.0 - u1 * u1) / 16.0;
154
155    if (0.0..=1.0).contains(&u2) {
156        let u = u2.acos();
157        let a = (2.0 * u.sin() / u1).asin();
158        let t = mod2pi(theta + a + PI / 2.0);
159        let v = mod2pi(t - phi);
160        if t >= 0.0 && v >= 0.0 {
161            return (true, vec![t, -u, -u, v], vec!['L', 'R', 'L', 'R']);
162        }
163    }
164    (false, Vec::new(), Vec::new())
165}
166
167fn left_x_right90_straight_left(x: f64, y: f64, phi: f64) -> (bool, Vec<f64>, Vec<char>) {
168    let zeta = x - phi.sin();
169    let eeta = y - 1.0 + phi.cos();
170    let (u1, theta) = polar(zeta, eeta);
171
172    if u1 >= 2.0 {
173        let u = (u1 * u1 - 4.0).sqrt() - 2.0;
174        let a = (2.0_f64).atan2((u1 * u1 - 4.0).sqrt());
175        let t = mod2pi(theta + a + PI / 2.0);
176        let v = mod2pi(t - phi + PI / 2.0);
177        if t >= 0.0 && v >= 0.0 {
178            return (true, vec![t, -PI / 2.0, -u, -v], vec!['L', 'R', 'S', 'L']);
179        }
180    }
181    (false, Vec::new(), Vec::new())
182}
183
184fn left_straight_right90_x_left(x: f64, y: f64, phi: f64) -> (bool, Vec<f64>, Vec<char>) {
185    let zeta = x - phi.sin();
186    let eeta = y - 1.0 + phi.cos();
187    let (u1, theta) = polar(zeta, eeta);
188
189    if u1 >= 2.0 {
190        let u = (u1 * u1 - 4.0).sqrt() - 2.0;
191        let a = ((u1 * u1 - 4.0).sqrt()).atan2(2.0);
192        let t = mod2pi(theta - a + PI / 2.0);
193        let v = mod2pi(t - phi - PI / 2.0);
194        if t >= 0.0 && v >= 0.0 {
195            return (true, vec![t, u, PI / 2.0, -v], vec!['L', 'S', 'R', 'L']);
196        }
197    }
198    (false, Vec::new(), Vec::new())
199}
200
201fn left_x_right90_straight_right(x: f64, y: f64, phi: f64) -> (bool, Vec<f64>, Vec<char>) {
202    let zeta = x + phi.sin();
203    let eeta = y - 1.0 - phi.cos();
204    let (u1, theta) = polar(zeta, eeta);
205
206    if u1 >= 2.0 {
207        let t = mod2pi(theta + PI / 2.0);
208        let u = u1 - 2.0;
209        let v = mod2pi(phi - t - PI / 2.0);
210        if t >= 0.0 && v >= 0.0 {
211            return (true, vec![t, -PI / 2.0, -u, -v], vec!['L', 'R', 'S', 'R']);
212        }
213    }
214    (false, Vec::new(), Vec::new())
215}
216
217fn left_straight_left90_x_right(x: f64, y: f64, phi: f64) -> (bool, Vec<f64>, Vec<char>) {
218    let zeta = x + phi.sin();
219    let eeta = y - 1.0 - phi.cos();
220    let (u1, theta) = polar(zeta, eeta);
221
222    if u1 >= 2.0 {
223        let t = mod2pi(theta);
224        let u = u1 - 2.0;
225        let v = mod2pi(phi - t - PI / 2.0);
226        if t >= 0.0 && v >= 0.0 {
227            return (true, vec![t, u, PI / 2.0, -v], vec!['L', 'S', 'L', 'R']);
228        }
229    }
230    (false, Vec::new(), Vec::new())
231}
232
233fn left_x_right90_straight_left90_x_right(x: f64, y: f64, phi: f64) -> (bool, Vec<f64>, Vec<char>) {
234    let zeta = x + phi.sin();
235    let eeta = y - 1.0 - phi.cos();
236    let (u1, theta) = polar(zeta, eeta);
237
238    if u1 >= 4.0 {
239        let u = (u1 * u1 - 4.0).sqrt() - 4.0;
240        let a = (2.0_f64).atan2((u1 * u1 - 4.0).sqrt());
241        let t = mod2pi(theta + a + PI / 2.0);
242        let v = mod2pi(t - phi);
243        if t >= 0.0 && v >= 0.0 {
244            return (
245                true,
246                vec![t, -PI / 2.0, -u, -PI / 2.0, v],
247                vec!['L', 'R', 'S', 'L', 'R'],
248            );
249        }
250    }
251    (false, Vec::new(), Vec::new())
252}
253
254fn timeflip(travel_distances: Vec<f64>) -> Vec<f64> {
255    travel_distances.iter().map(|x| -x).collect()
256}
257
258fn reflect(steering_directions: Vec<char>) -> Vec<char> {
259    steering_directions
260        .iter()
261        .map(|&dirn| match dirn {
262            'L' => 'R',
263            'R' => 'L',
264            _ => 'S',
265        })
266        .collect()
267}
268
269fn set_path(paths: &mut Vec<Path>, lengths: Vec<f64>, ctypes: Vec<char>, step_size: f64) {
270    let mut path = Path::new();
271    path.ctypes = ctypes;
272    path.lengths = lengths;
273    path.l = path.lengths.iter().map(|x| x.abs()).sum();
274
275    for existing_path in paths.iter() {
276        let type_is_same = existing_path.ctypes == path.ctypes;
277        let length_is_close = (existing_path.l - path.l) <= step_size;
278        if type_is_same && length_is_close {
279            return;
280        }
281    }
282
283    if path.l <= step_size {
284        return;
285    }
286
287    paths.push(path);
288}
289
290fn has_too_small_segment(travel_distances: &[f64], step_size: f64) -> bool {
291    let min_dist = 0.1 * travel_distances.iter().map(|d| d.abs()).sum::<f64>();
292    travel_distances
293        .iter()
294        .any(|&distance| min_dist < distance.abs() && distance.abs() < step_size)
295}
296
297fn generate_path(q0: [f64; 3], q1: [f64; 3], max_curvature: f64, step_size: f64) -> Vec<Path> {
298    let dx = q1[0] - q0[0];
299    let dy = q1[1] - q0[1];
300    let dth = q1[2] - q0[2];
301    let c = q0[2].cos();
302    let s = q0[2].sin();
303    let x = (c * dx + s * dy) * max_curvature;
304    let y = (-s * dx + c * dy) * max_curvature;
305    let step_size = step_size * max_curvature;
306
307    let mut paths = Vec::new();
308
309    let path_functions: Vec<fn(f64, f64, f64) -> (bool, Vec<f64>, Vec<char>)> = vec![
310        left_straight_left,
311        left_straight_right,
312        left_x_right_x_left,
313        left_x_right_left,
314        left_right_x_left,
315        left_right_x_left_right,
316        left_x_right_left_x_right,
317        left_x_right90_straight_left,
318        left_x_right90_straight_right,
319        left_straight_right90_x_left,
320        left_straight_left90_x_right,
321        left_x_right90_straight_left90_x_right,
322    ];
323
324    // A candidate with a segment too short to sample is skipped on its own
325    // (this used to discard every candidate).
326    for path_func in path_functions {
327        // Original
328        let (flag, travel_distances, steering_dirns) = path_func(x, y, dth);
329        if flag && !has_too_small_segment(&travel_distances, step_size) {
330            set_path(&mut paths, travel_distances, steering_dirns, step_size);
331        }
332
333        // Timeflip
334        let (flag, travel_distances, steering_dirns) = path_func(-x, y, -dth);
335        if flag && !has_too_small_segment(&travel_distances, step_size) {
336            let travel_distances = timeflip(travel_distances);
337            set_path(&mut paths, travel_distances, steering_dirns, step_size);
338        }
339
340        // Reflect
341        let (flag, travel_distances, steering_dirns) = path_func(x, -y, -dth);
342        if flag && !has_too_small_segment(&travel_distances, step_size) {
343            let steering_dirns = reflect(steering_dirns);
344            set_path(&mut paths, travel_distances, steering_dirns, step_size);
345        }
346
347        // Timeflip + Reflect
348        let (flag, travel_distances, steering_dirns) = path_func(-x, -y, dth);
349        if flag && !has_too_small_segment(&travel_distances, step_size) {
350            let travel_distances = timeflip(travel_distances);
351            let steering_dirns = reflect(steering_dirns);
352            set_path(&mut paths, travel_distances, steering_dirns, step_size);
353        }
354    }
355
356    paths
357}
358
359fn calc_interpolate_dists_list(lengths: &[f64], step_size: f64) -> Vec<Vec<f64>> {
360    let mut interpolate_dists_list = Vec::new();
361
362    for &length in lengths {
363        let d_dist = if length >= 0.0 { step_size } else { -step_size };
364        let mut interp_dists = Vec::new();
365        let mut current = 0.0;
366
367        while (length >= 0.0 && current < length) || (length < 0.0 && current > length) {
368            interp_dists.push(current);
369            current += d_dist;
370        }
371        interp_dists.push(length);
372        interpolate_dists_list.push(interp_dists);
373    }
374
375    interpolate_dists_list
376}
377
378fn interpolate(
379    dist: f64,
380    length: f64,
381    mode: char,
382    max_curvature: f64,
383    origin_x: f64,
384    origin_y: f64,
385    origin_yaw: f64,
386) -> (f64, f64, f64, i32) {
387    if mode == 'S' {
388        let x = origin_x + dist / max_curvature * origin_yaw.cos();
389        let y = origin_y + dist / max_curvature * origin_yaw.sin();
390        let yaw = origin_yaw;
391        (x, y, yaw, if length > 0.0 { 1 } else { -1 })
392    } else {
393        let ldx = dist.sin() / max_curvature;
394        let (ldy, yaw) = if mode == 'L' {
395            ((1.0 - dist.cos()) / max_curvature, origin_yaw + dist)
396        } else {
397            // 'R'
398            ((1.0 - dist.cos()) / -max_curvature, origin_yaw - dist)
399        };
400
401        let gdx = (-origin_yaw).cos() * ldx + (-origin_yaw).sin() * ldy;
402        let gdy = -(-origin_yaw).sin() * ldx + (-origin_yaw).cos() * ldy;
403        let x = origin_x + gdx;
404        let y = origin_y + gdy;
405
406        (x, y, yaw, if length > 0.0 { 1 } else { -1 })
407    }
408}
409
410fn generate_local_course(
411    lengths: &[f64],
412    modes: &[char],
413    max_curvature: f64,
414    step_size: f64,
415) -> (Vec<f64>, Vec<f64>, Vec<f64>, Vec<i32>) {
416    let interpolate_dists_list = calc_interpolate_dists_list(lengths, step_size * max_curvature);
417
418    let mut origin_x = 0.0;
419    let mut origin_y = 0.0;
420    let mut origin_yaw = 0.0;
421
422    let mut xs = Vec::new();
423    let mut ys = Vec::new();
424    let mut yaws = Vec::new();
425    let mut directions = Vec::new();
426
427    for ((interp_dists, &mode), &length) in interpolate_dists_list.iter().zip(modes).zip(lengths) {
428        // A zero-length segment adds no motion, only a duplicate sample
429        // whose direction (-1 for a length of 0) would read as two gear
430        // changes.
431        if length == 0.0 {
432            continue;
433        }
434        for &dist in interp_dists {
435            let (x, y, yaw, direction) = interpolate(
436                dist,
437                length,
438                mode,
439                max_curvature,
440                origin_x,
441                origin_y,
442                origin_yaw,
443            );
444            xs.push(x);
445            ys.push(y);
446            yaws.push(yaw);
447            directions.push(direction);
448        }
449        if !xs.is_empty() {
450            origin_x = xs[xs.len() - 1];
451            origin_y = ys[ys.len() - 1];
452            origin_yaw = yaws[yaws.len() - 1];
453        }
454    }
455
456    (xs, ys, yaws, directions)
457}
458
459fn calc_paths(
460    sx: f64,
461    sy: f64,
462    syaw: f64,
463    gx: f64,
464    gy: f64,
465    gyaw: f64,
466    maxc: f64,
467    step_size: f64,
468) -> Vec<Path> {
469    let q0 = [sx, sy, syaw];
470    let q1 = [gx, gy, gyaw];
471
472    let mut paths = generate_path(q0, q1, maxc, step_size);
473
474    for path in &mut paths {
475        let (xs, ys, yaws, directions) =
476            generate_local_course(&path.lengths, &path.ctypes, maxc, step_size);
477
478        path.x = xs
479            .iter()
480            .zip(ys.iter())
481            .map(|(&ix, &iy)| (-q0[2]).cos() * ix + (-q0[2]).sin() * iy + q0[0])
482            .collect();
483
484        path.y = xs
485            .iter()
486            .zip(ys.iter())
487            .map(|(&ix, &iy)| -(-q0[2]).sin() * ix + (-q0[2]).cos() * iy + q0[1])
488            .collect();
489
490        path.yaw = yaws
491            .iter()
492            .map(|&yaw| normalize_angle(yaw + q0[2]))
493            .collect();
494        path.directions = directions;
495        path.lengths = path.lengths.iter().map(|&length| length / maxc).collect();
496        path.l = path.l / maxc;
497    }
498
499    paths
500}
501
502/// Every Reeds-Shepp path from `(sx, sy, syaw)` to `(gx, gy, gyaw)` (not
503/// just the shortest), sampled every `step_size` \[m\], with the direction
504/// of travel (+1 forward, -1 reverse) at each sample. Lets a caller pick
505/// by its own cost, e.g. the shortest collision-free path or one with
506/// fewer gear changes.
507#[allow(clippy::too_many_arguments)]
508pub fn reeds_shepp_paths(
509    sx: f64,
510    sy: f64,
511    syaw: f64,
512    gx: f64,
513    gy: f64,
514    gyaw: f64,
515    maxc: f64,
516    step_size: f64,
517) -> Vec<Path> {
518    calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size)
519}
520
521pub fn reeds_shepp_path_planning(
522    sx: f64,
523    sy: f64,
524    syaw: f64,
525    gx: f64,
526    gy: f64,
527    gyaw: f64,
528    maxc: f64,
529    step_size: f64,
530) -> Option<(Vec<f64>, Vec<f64>, Vec<f64>, Vec<char>, Vec<f64>)> {
531    let paths = calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size);
532
533    if paths.is_empty() {
534        return None;
535    }
536
537    let best_path = paths.iter().min_by(|a, b| a.l.partial_cmp(&b.l).unwrap())?;
538
539    Some((
540        best_path.x.clone(),
541        best_path.y.clone(),
542        best_path.yaw.clone(),
543        best_path.ctypes.clone(),
544        best_path.lengths.clone(),
545    ))
546}
547
548pub struct ReedsSheppPlanner {
549    pub path_x: Vec<f64>,
550    pub path_y: Vec<f64>,
551    pub path_yaw: Vec<f64>,
552    pub modes: Vec<char>,
553    pub lengths: Vec<f64>,
554}
555
556impl ReedsSheppPlanner {
557    pub fn new() -> Self {
558        ReedsSheppPlanner {
559            path_x: Vec::new(),
560            path_y: Vec::new(),
561            path_yaw: Vec::new(),
562            modes: Vec::new(),
563            lengths: Vec::new(),
564        }
565    }
566
567    pub fn planning(
568        &mut self,
569        sx: f64,
570        sy: f64,
571        syaw: f64,
572        gx: f64,
573        gy: f64,
574        gyaw: f64,
575        max_curvature: f64,
576        step_size: f64,
577    ) -> bool {
578        if let Some((x, y, yaw, modes, lengths)) =
579            reeds_shepp_path_planning(sx, sy, syaw, gx, gy, gyaw, max_curvature, step_size)
580        {
581            self.path_x = x;
582            self.path_y = y;
583            self.path_yaw = yaw;
584            self.modes = modes;
585            self.lengths = lengths;
586            true
587        } else {
588            false
589        }
590    }
591}
592
593impl Default for ReedsSheppPlanner {
594    fn default() -> Self {
595        Self::new()
596    }
597}
598
599#[cfg(test)]
600#[allow(clippy::excessive_precision)]
601mod tests {
602    use super::*;
603    use std::f64::consts::PI;
604
605    fn approx_eq(a: f64, b: f64, tol: f64) -> bool {
606        (a - b).abs() < tol
607    }
608
609    fn path_fingerprint(path: &Path) -> (f64, f64, f64, f64, f64, f64) {
610        let sum_x: f64 = path.x.iter().sum();
611        let sum_y: f64 = path.y.iter().sum();
612        let sum_yaw: f64 = path.yaw.iter().sum();
613        let weighted_sum_x: f64 = path
614            .x
615            .iter()
616            .enumerate()
617            .map(|(index, value)| (index + 1) as f64 * value)
618            .sum();
619        let weighted_sum_y: f64 = path
620            .y
621            .iter()
622            .enumerate()
623            .map(|(index, value)| (index + 1) as f64 * value)
624            .sum();
625        let weighted_sum_yaw: f64 = path
626            .yaw
627            .iter()
628            .enumerate()
629            .map(|(index, value)| (index + 1) as f64 * value)
630            .sum();
631
632        (
633            sum_x,
634            sum_y,
635            sum_yaw,
636            weighted_sum_x,
637            weighted_sum_y,
638            weighted_sum_yaw,
639        )
640    }
641
642    fn assert_fingerprint_close(
643        actual: (f64, f64, f64, f64, f64, f64),
644        expected: (f64, f64, f64, f64, f64, f64),
645    ) {
646        assert!(approx_eq(actual.0, expected.0, 1e-9));
647        assert!(approx_eq(actual.1, expected.1, 1e-9));
648        assert!(approx_eq(actual.2, expected.2, 1e-9));
649        assert!(approx_eq(actual.3, expected.3, 1e-6));
650        assert!(approx_eq(actual.4, expected.4, 1e-6));
651        assert!(approx_eq(actual.5, expected.5, 1e-6));
652    }
653
654    #[test]
655    fn test_reeds_shepp_planning() {
656        let mut planner = ReedsSheppPlanner::new();
657        let result = planner.planning(
658            -1.0,
659            -4.0,
660            (-20.0_f64).to_radians(),
661            5.0,
662            5.0,
663            (25.0_f64).to_radians(),
664            0.1,
665            0.05,
666        );
667        assert!(result);
668        assert!(!planner.path_x.is_empty());
669        assert!(!planner.modes.is_empty());
670    }
671
672    #[test]
673    fn test_reeds_shepp_path_planning_fn() {
674        let result = reeds_shepp_path_planning(0.0, 0.0, 0.0, 5.0, 5.0, 0.5, 0.2, 0.1);
675        assert!(result.is_some());
676    }
677
678    #[test]
679    fn test_calc_paths_matches_upstream_main_example_candidates() {
680        let paths = calc_paths(
681            -1.0,
682            -4.0,
683            (-20.0_f64).to_radians(),
684            5.0,
685            5.0,
686            25.0_f64.to_radians(),
687            0.1,
688            0.05,
689        );
690
691        assert_eq!(paths.len(), 5);
692
693        let best_path = paths
694            .iter()
695            .min_by(|a, b| a.l.partial_cmp(&b.l).unwrap())
696            .unwrap();
697        assert_eq!(best_path.ctypes, vec!['R', 'L', 'R']);
698        assert!(approx_eq(best_path.l, 19.647_318_205_087_014, 1e-12));
699        assert!(approx_eq(
700            best_path.lengths[0],
701            -5.228_525_224_523_852,
702            1e-12
703        ));
704        assert!(approx_eq(
705            best_path.lengths[1],
706            8.522_124_695_006_896,
707            1e-12
708        ));
709        assert!(approx_eq(
710            best_path.lengths[2],
711            5.896_668_285_556_266,
712            1e-12
713        ));
714        assert_eq!(best_path.x.len(), 397);
715        assert_eq!(best_path.directions.len(), 397);
716        assert_eq!(best_path.directions.iter().sum::<i32>(), 185);
717        assert_eq!(
718            best_path
719                .directions
720                .iter()
721                .enumerate()
722                .map(|(index, direction)| (index + 1) as i32 * direction)
723                .sum::<i32>(),
724            67_661
725        );
726    }
727
728    #[test]
729    fn test_reeds_shepp_path_planning_matches_upstream_main_example() {
730        let (x, y, yaw, modes, lengths) = reeds_shepp_path_planning(
731            -1.0,
732            -4.0,
733            (-20.0_f64).to_radians(),
734            5.0,
735            5.0,
736            25.0_f64.to_radians(),
737            0.1,
738            0.05,
739        )
740        .unwrap();
741
742        assert_eq!(modes, vec!['R', 'L', 'R']);
743        assert!(approx_eq(lengths[0], -5.228_525_224_523_852, 1e-12));
744        assert!(approx_eq(lengths[1], 8.522_124_695_006_896, 1e-12));
745        assert!(approx_eq(lengths[2], 5.896_668_285_556_266, 1e-12));
746        assert_eq!(x.len(), 397);
747        assert_eq!(y.len(), 397);
748        assert_eq!(yaw.len(), 397);
749
750        let expected_samples = [
751            (0usize, -1.0, -4.0, -0.349_065_850_398_865_95),
752            (
753                50,
754                -3.431_162_528_468_660_4,
755                -3.445_956_303_130_415_2,
756                -0.099_065_850_398_865_95,
757            ),
758            (
759                100,
760                -5.923_818_705_704_953,
761                -3.510_615_718_197_37,
762                0.150_934_149_601_134_05,
763            ),
764            (
765                150,
766                -4.041_585_620_182_658,
767                -2.932_812_755_977_549,
768                0.393_786_672_053_519_47,
769            ),
770            (396, 5.000_000_000_000_005, 5.0, 0.436_332_312_998_582_33),
771        ];
772
773        for (index, expected_x, expected_y, expected_yaw) in expected_samples {
774            assert!(approx_eq(x[index], expected_x, 1e-12));
775            assert!(approx_eq(y[index], expected_y, 1e-12));
776            assert!(approx_eq(yaw[index], expected_yaw, 1e-12));
777        }
778
779        let sum_x: f64 = x.iter().sum();
780        let sum_y: f64 = y.iter().sum();
781        let sum_yaw: f64 = yaw.iter().sum();
782        let weighted_sum_x: f64 = x
783            .iter()
784            .enumerate()
785            .map(|(index, value)| (index + 1) as f64 * value)
786            .sum();
787        let weighted_sum_y: f64 = y
788            .iter()
789            .enumerate()
790            .map(|(index, value)| (index + 1) as f64 * value)
791            .sum();
792        let weighted_sum_yaw: f64 = yaw
793            .iter()
794            .enumerate()
795            .map(|(index, value)| (index + 1) as f64 * value)
796            .sum();
797
798        assert!(approx_eq(sum_x, -474.632_647_731_712_4, 1e-9));
799        assert!(approx_eq(sum_y, -278.042_684_871_068_6, 1e-9));
800        assert!(approx_eq(sum_yaw, 181.229_623_459_273_85, 1e-9));
801        assert!(approx_eq(weighted_sum_x, 24_450.581_151_074_42, 1e-6));
802        assert!(approx_eq(weighted_sum_y, 72_297.919_198_954_09, 1e-6));
803        assert!(approx_eq(weighted_sum_yaw, 50_733.291_448_638_01, 1e-6));
804    }
805
806    #[test]
807    fn test_calc_paths_matches_upstream_representative_mode_coverage() {
808        struct Scenario {
809            goal: (f64, f64, f64),
810            path_count: usize,
811            mode: &'static [char],
812            lengths: &'static [f64],
813            total: f64,
814            sample_count: usize,
815            direction_sum: i32,
816            direction_weighted: i32,
817            fingerprint: (f64, f64, f64, f64, f64, f64),
818        }
819
820        let start = (0.0, 0.0, -PI / 2.0);
821        let scenarios = [
822            Scenario {
823                goal: (2.0, -6.0, -PI / 4.0),
824                path_count: 9,
825                mode: &['L', 'S', 'L'],
826                lengths: &[
827                    1.069_877_996_599_901_2,
828                    2.521_981_303_104_907_6,
829                    2.857_112_820_387_34,
830                ],
831                total: 6.448_972_120_092_149,
832                sample_count: 69,
833                direction_sum: 69,
834                direction_weighted: 2_415,
835                fingerprint: (
836                    47.658_101_248_550_764,
837                    -214.691_563_475_246_82,
838                    -86.182_939_224_211_54,
839                    2_416.980_927_612_332_4,
840                    -9_962.039_734_007_922,
841                    -2_763.536_289_485_696,
842                ),
843            },
844            Scenario {
845                goal: (4.0, -6.0, -PI / 4.0),
846                path_count: 8,
847                mode: &['L', 'S', 'R'],
848                lengths: &[
849                    3.999_542_478_656_482,
850                    3.390_792_633_449_932_8,
851                    0.072_551_661_669_240_65,
852                ],
853                total: 7.462_886_773_775_654,
854                sample_count: 78,
855                direction_sum: 78,
856                direction_weighted: 3_081,
857                fingerprint: (
858                    125.110_316_958_156_83,
859                    -256.540_636_968_477_1,
860                    -76.540_100_984_546_51,
861                    7_190.184_713_371_897_5,
862                    -13_176.153_323_150_214,
863                    -2_605.762_179_928_655_3,
864                ),
865            },
866            Scenario {
867                goal: (-4.0, -6.0, -3.0 * PI / 4.0),
868                path_count: 8,
869                mode: &['R', 'S', 'L'],
870                lengths: &[
871                    3.999_542_478_656_482_4,
872                    3.390_792_633_449_932_8,
873                    0.072_551_661_669_241_21,
874                ],
875                total: 7.462_886_773_775_656,
876                sample_count: 78,
877                direction_sum: 78,
878                direction_weighted: 3_081,
879                fingerprint: (
880                    -125.110_316_958_156_82,
881                    -256.540_636_968_477_14,
882                    -168.504_125_995_457_34,
883                    -7_190.184_713_371_896,
884                    -13_176.153_323_150_216,
885                    -7_073.484_785_781_497_5,
886                ),
887            },
888            Scenario {
889                goal: (-6.0, -6.0, PI),
890                path_count: 9,
891                mode: &['R', 'S', 'R'],
892                lengths: &[
893                    3.926_990_816_987_239,
894                    1.414_213_562_373_095_6,
895                    3.926_990_816_987_246,
896                ],
897                total: 9.268_195_196_347_58,
898                sample_count: 98,
899                direction_sum: 98,
900                direction_weighted: 4_851,
901                fingerprint: (
902                    -224.230_783_041_927_37,
903                    -368.245_146_496_736_9,
904                    -225.193_346_359_169_7,
905                    -16_127.734_907_593_596,
906                    -23_204.350_340_917_714,
907                    -11_989.089_837_846_506,
908                ),
909            },
910            Scenario {
911                goal: (-6.0, -6.0, -3.0 * PI / 4.0),
912                path_count: 6,
913                mode: &['L', 'R', 'L'],
914                lengths: &[
915                    -0.068_966_848_925_894_33,
916                    6.503_746_832_829_835,
917                    2.645_722_864_768_488,
918                ],
919                total: 9.218_436_546_524_217,
920                sample_count: 97,
921                direction_sum: 93,
922                direction_weighted: 4_747,
923                fingerprint: (
924                    -230.574_821_137_002_54,
925                    -331.875_810_770_970_8,
926                    -226.763_994_255_941_5,
927                    -16_521.117_485_464_518,
928                    -20_916.307_531_974_84,
929                    -11_997.697_736_223_088,
930                ),
931            },
932            Scenario {
933                goal: (-6.0, -6.0, -PI / 4.0),
934                path_count: 6,
935                mode: &['R', 'L', 'R'],
936                lengths: &[
937                    5.000_868_664_582_23,
938                    5.561_988_484_973_19,
939                    -3.365_870_996_596_280_8,
940                ],
941                total: 13.928_728_146_151_7,
942                sample_count: 144,
943                direction_sum: 74,
944                direction_weighted: 1_550,
945                fingerprint: (
946                    -443.806_209_833_689_9,
947                    -744.765_650_985_955,
948                    -261.971_336_928_568_35,
949                    -43_201.590_996_218_62,
950                    -67_652.561_148_743_36,
951                    -16_979.120_856_723_654,
952                ),
953            },
954            Scenario {
955                goal: (-6.0, -6.0, -PI / 2.0),
956                path_count: 6,
957                mode: &['L', 'R', 'L', 'R'],
958                lengths: &[
959                    -0.823_349_563_605_059_2,
960                    5.119_726_880_494_763,
961                    5.119_726_880_494_763,
962                    -0.823_349_563_605_059_2,
963                ],
964                total: 11.886_152_888_199_643,
965                sample_count: 126,
966                direction_sum: 86,
967                direction_weighted: 5_461,
968                fingerprint: (
969                    -380.403_475_990_598_96,
970                    -380.444_909_002_660_5,
971                    -271.291_151_985_038_8,
972                    -34_379.127_386_055_37,
973                    -34_617.891_225_324_86,
974                    -17_198.177_166_350_25,
975                ),
976            },
977            Scenario {
978                goal: (-6.0, -6.0, PI / 4.0),
979                path_count: 4,
980                mode: &['L', 'R', 'S', 'L'],
981                lengths: &[
982                    4.566_911_224_275_087,
983                    -7.853_981_633_974_483,
984                    -0.833_066_927_667_685,
985                    -0.639_920_407_287_846_2,
986                ],
987                total: 13.893_880_193_205_101,
988                sample_count: 145,
989                direction_sum: -51,
990                direction_weighted: -8_329,
991                fingerprint: (
992                    -203.973_341_223_846_2,
993                    -476.554_271_600_198_1,
994                    -25.723_479_016_967_794,
995                    -28_387.645_373_178_693,
996                    -41_452.300_874_567_176,
997                    2_865.645_752_604_772,
998                ),
999            },
1000            Scenario {
1001                goal: (-4.0, -6.0, PI),
1002                path_count: 6,
1003                mode: &['L', 'S', 'R', 'L'],
1004                lengths: &[
1005                    0.473_590_382_777_882,
1006                    0.099_504_938_362_080_52,
1007                    7.853_981_633_974_483,
1008                    -0.473_590_382_777_882,
1009                ],
1010                total: 8.900_667_337_892_326,
1011                sample_count: 94,
1012                direction_sum: 82,
1013                direction_weighted: 3_367,
1014                fingerprint: (
1015                    -145.451_641_721_792_1,
1016                    -351.919_906_224_341_96,
1017                    -211.935_238_269_393_84,
1018                    -10_708.096_596_694_699,
1019                    -21_581.461_832_452_35,
1020                    -11_398.570_917_831_472,
1021                ),
1022            },
1023            Scenario {
1024                goal: (4.0, -6.0, 0.0),
1025                path_count: 6,
1026                mode: &['R', 'S', 'L', 'R'],
1027                lengths: &[
1028                    0.473_590_382_777_883_1,
1029                    0.099_504_938_362_076_08,
1030                    7.853_981_633_974_483,
1031                    -0.473_590_382_777_883_1,
1032                ],
1033                total: 8.900_667_337_892_324,
1034                sample_count: 94,
1035                direction_sum: 82,
1036                direction_weighted: 3_367,
1037                fingerprint: (
1038                    145.451_641_721_792_13,
1039                    -351.919_906_224_341_6,
1040                    -83.374_471_168_046_72,
1041                    10_708.096_596_694_699,
1042                    -21_581.461_832_452_34,
1043                    -2_628.640_280_446_955,
1044                ),
1045            },
1046            Scenario {
1047                goal: (-6.0, -6.0, 3.0 * PI / 4.0),
1048                path_count: 4,
1049                mode: &['R', 'S', 'R', 'L'],
1050                lengths: &[
1051                    2.219_874_422_095_635_2,
1052                    0.559_236_463_071_481_9,
1053                    7.853_981_633_974_483,
1054                    -1.707_116_394_891_605_7,
1055                ],
1056                total: 12.340_208_914_033_205,
1057                sample_count: 130,
1058                direction_sum: 92,
1059                direction_weighted: 3_917,
1060                fingerprint: (
1061                    -435.220_953_689_669,
1062                    -527.340_252_871_888_3,
1063                    -89.312_402_029_516_8,
1064                    -40_676.492_201_712_46,
1065                    -42_014.939_961_467_01,
1066                    2_377.125_258_488_301,
1067                ),
1068            },
1069            Scenario {
1070                goal: (-6.0, -2.0, -PI / 4.0),
1071                path_count: 5,
1072                mode: &['R', 'L', 'R', 'L'],
1073                lengths: &[
1074                    1.605_151_609_076_620_5,
1075                    4.327_807_515_918_15,
1076                    -4.327_807_515_918_15,
1077                    -3.123_472_605_772_438,
1078                ],
1079                total: 13.384_239_246_685_356,
1080                sample_count: 141,
1081                direction_sum: -15,
1082                direction_weighted: -5_979,
1083                fingerprint: (
1084                    -234.424_840_465_174_4,
1085                    -469.790_322_454_025_9,
1086                    -138.906_286_644_584_9,
1087                    -26_289.793_852_112_638,
1088                    -35_567.135_089_521_57,
1089                    -6_993.944_431_851_961,
1090                ),
1091            },
1092        ];
1093
1094        for scenario in scenarios {
1095            let paths = calc_paths(
1096                start.0,
1097                start.1,
1098                start.2,
1099                scenario.goal.0,
1100                scenario.goal.1,
1101                scenario.goal.2,
1102                0.2,
1103                0.1,
1104            );
1105            assert_eq!(paths.len(), scenario.path_count);
1106
1107            let best_path = paths
1108                .iter()
1109                .min_by(|a, b| a.l.partial_cmp(&b.l).unwrap())
1110                .unwrap();
1111
1112            assert_eq!(best_path.ctypes, scenario.mode);
1113            assert_eq!(best_path.lengths.len(), scenario.lengths.len());
1114            for (actual, expected) in best_path.lengths.iter().zip(scenario.lengths.iter()) {
1115                assert!(approx_eq(*actual, *expected, 1e-12));
1116            }
1117            assert!(approx_eq(best_path.l, scenario.total, 1e-12));
1118            assert_eq!(best_path.x.len(), scenario.sample_count);
1119            assert_eq!(best_path.y.len(), scenario.sample_count);
1120            assert_eq!(best_path.yaw.len(), scenario.sample_count);
1121            assert_eq!(
1122                best_path.directions.iter().sum::<i32>(),
1123                scenario.direction_sum
1124            );
1125            assert_eq!(
1126                best_path
1127                    .directions
1128                    .iter()
1129                    .enumerate()
1130                    .map(|(index, direction)| (index + 1) as i32 * direction)
1131                    .sum::<i32>(),
1132                scenario.direction_weighted
1133            );
1134            assert!(approx_eq(
1135                *best_path.x.last().unwrap(),
1136                scenario.goal.0,
1137                1e-12
1138            ));
1139            assert!(approx_eq(
1140                *best_path.y.last().unwrap(),
1141                scenario.goal.1,
1142                1e-12
1143            ));
1144            assert!(approx_eq(
1145                normalize_angle(*best_path.yaw.last().unwrap() - scenario.goal.2),
1146                0.0,
1147                1e-12
1148            ));
1149            assert_fingerprint_close(path_fingerprint(best_path), scenario.fingerprint);
1150        }
1151    }
1152}