rust_robotics_planning/
dubins_path.rs

1//! Dubins Path Planner
2//!
3//! Computes the shortest path between two poses (x, y, yaw) for a forward-only
4//! vehicle with a bounded minimum turning radius. Unlike Reeds-Shepp paths,
5//! Dubins paths only allow forward motion.
6//!
7//! There are exactly 6 candidate path types, each composed of at most 3 segments
8//! of Left (L), Right (R), or Straight (S) arcs:
9//! **LSL, RSR, LSR, RSL, RLR, LRL**.
10//!
11//! # References
12//!
13//! * Dubins, L.E. (1957). "On Curves of Minimal Length with a Constraint on
14//!   Average Curvature, and with Prescribed Initial and Terminal Positions and
15//!   Tangents". *American Journal of Mathematics*, 79(3), 497–516.
16
17use std::f64::consts::PI;
18
19use rust_robotics_core::error::{RoboticsError, RoboticsResult};
20use rust_robotics_core::types::{Path2D, Point2D, Pose2D};
21
22/// Segment type in a Dubins path.
23#[derive(Debug, Clone, Copy, PartialEq, Eq)]
24pub enum SegmentType {
25    /// Left turn (counter-clockwise arc)
26    Left,
27    /// Straight segment
28    Straight,
29    /// Right turn (clockwise arc)
30    Right,
31}
32
33/// One of the six Dubins path type families.
34#[derive(Debug, Clone, Copy, PartialEq, Eq)]
35pub enum DubinsPathType {
36    /// Left – Straight – Left
37    LSL,
38    /// Right – Straight – Right
39    RSR,
40    /// Left – Straight – Right
41    LSR,
42    /// Right – Straight – Left
43    RSL,
44    /// Right – Left – Right
45    RLR,
46    /// Left – Right – Left
47    LRL,
48}
49
50impl DubinsPathType {
51    /// Return the three segment types for this path family.
52    pub fn segments(self) -> [SegmentType; 3] {
53        match self {
54            Self::LSL => [SegmentType::Left, SegmentType::Straight, SegmentType::Left],
55            Self::RSR => [
56                SegmentType::Right,
57                SegmentType::Straight,
58                SegmentType::Right,
59            ],
60            Self::LSR => [SegmentType::Left, SegmentType::Straight, SegmentType::Right],
61            Self::RSL => [SegmentType::Right, SegmentType::Straight, SegmentType::Left],
62            Self::RLR => [SegmentType::Right, SegmentType::Left, SegmentType::Right],
63            Self::LRL => [SegmentType::Left, SegmentType::Right, SegmentType::Left],
64        }
65    }
66}
67
68const ALL_PATH_TYPES: [DubinsPathType; 6] = [
69    DubinsPathType::LSL,
70    DubinsPathType::RSR,
71    DubinsPathType::LSR,
72    DubinsPathType::RSL,
73    DubinsPathType::RLR,
74    DubinsPathType::LRL,
75];
76
77/// A computed Dubins path consisting of up to three segments.
78///
79/// The `lengths` array stores the arc-length of each segment in world units
80/// \[m\], and `total_length` is their sum.
81#[must_use]
82#[derive(Debug, Clone)]
83pub struct DubinsPath {
84    /// The family of this path (e.g. LSL, RSR, …).
85    pub path_type: DubinsPathType,
86    /// Arc-lengths of the three segments \[m\].
87    pub lengths: [f64; 3],
88    /// Total path length \[m\].
89    pub total_length: f64,
90    /// Curvature used when computing this path (1 / min turning radius).
91    pub curvature: f64,
92    /// Start pose used for interpolation.
93    pub start: Pose2D,
94}
95
96/// Dubins path planner.
97///
98/// Computes the shortest forward-only path between two `Pose2D` waypoints for a
99/// vehicle whose minimum turning radius is `1.0 / curvature`.
100///
101/// # Example
102///
103/// ```
104/// use rust_robotics_planning::dubins_path::DubinsPlanner;
105/// use rust_robotics_core::types::Pose2D;
106///
107/// let planner = DubinsPlanner::new(1.0);
108/// let start = Pose2D::new(0.0, 0.0, 0.0);
109/// let goal  = Pose2D::new(5.0, 5.0, std::f64::consts::FRAC_PI_2);
110/// let path  = planner.plan(start, goal).unwrap();
111/// assert!(path.total_length > 0.0);
112/// ```
113pub struct DubinsPlanner {
114    /// Curvature = 1 / minimum turning radius \[1/m\].
115    pub curvature: f64,
116}
117
118impl DubinsPlanner {
119    /// Create a new planner with the given curvature (1 / min turning radius).
120    ///
121    /// # Errors
122    ///
123    /// Returns `InvalidParameter` if `curvature` is not positive and finite.
124    pub fn new(curvature: f64) -> Self {
125        Self { curvature }
126    }
127
128    /// Plan the shortest Dubins path from `start` to `goal`.
129    ///
130    /// # Errors
131    ///
132    /// Returns `PlanningError` if no valid path can be found (should not happen
133    /// for finite inputs—included for robustness).
134    pub fn plan(&self, start: Pose2D, goal: Pose2D) -> RoboticsResult<DubinsPath> {
135        if !self.curvature.is_finite() || self.curvature <= 0.0 {
136            return Err(RoboticsError::InvalidParameter(
137                "curvature must be positive and finite".to_string(),
138            ));
139        }
140
141        // Transform goal into the start-centred, normalised frame.
142        let dx = goal.x - start.x;
143        let dy = goal.y - start.y;
144        let d = (dx * dx + dy * dy).sqrt() * self.curvature;
145
146        let theta = dy.atan2(dx);
147        let alpha = mod2pi(start.yaw - theta);
148        let beta = mod2pi(goal.yaw - theta);
149
150        let mut best: Option<DubinsPath> = None;
151
152        for &pt in &ALL_PATH_TYPES {
153            if let Some(lengths) = compute_path_params(alpha, beta, d, pt) {
154                // All segment lengths must be non-negative for a valid Dubins path.
155                if lengths.iter().any(|&l| l < 0.0) {
156                    continue;
157                }
158                // Convert normalised lengths to world lengths.
159                let scale = 1.0 / self.curvature;
160                let world_lengths = [lengths[0] * scale, lengths[1] * scale, lengths[2] * scale];
161                let total = world_lengths[0] + world_lengths[1] + world_lengths[2];
162
163                if best.as_ref().map_or(true, |b| total < b.total_length) {
164                    best = Some(DubinsPath {
165                        path_type: pt,
166                        lengths: world_lengths,
167                        total_length: total,
168                        curvature: self.curvature,
169                        start,
170                    });
171                }
172            }
173        }
174
175        best.ok_or_else(|| RoboticsError::PlanningError("no valid Dubins path found".to_string()))
176    }
177
178    /// Sample evenly-spaced points along a previously computed `DubinsPath`.
179    ///
180    /// `step` is the distance \[m\] between consecutive sample points.
181    pub fn sample_path(&self, path: &DubinsPath, step: f64) -> Path2D {
182        let mut points = vec![Point2D::new(path.start.x, path.start.y)];
183        let segments = path.path_type.segments();
184        let step = if step <= 0.0 { 0.1 } else { step };
185
186        for (i, &seg) in segments.iter().enumerate() {
187            let seg_len = path.lengths[i] * path.curvature;
188            if seg_len.abs() < 1e-12 {
189                continue;
190            }
191
192            let origin = *points.last().unwrap();
193            let origin_yaw = segment_end_yaw(&points, path, i);
194            let mut current_length = step;
195
196            while (current_length + step).abs() <= seg_len.abs() {
197                let (x, y, _) =
198                    interpolate_segment(current_length, seg, path.curvature, origin, origin_yaw);
199                points.push(Point2D::new(x, y));
200                current_length += step;
201            }
202
203            let (x, y, _) = interpolate_segment(seg_len, seg, path.curvature, origin, origin_yaw);
204            points.push(Point2D::new(x, y));
205        }
206
207        Path2D::from_points(points)
208    }
209}
210
211// ---------------------------------------------------------------------------
212// Internal helpers
213// ---------------------------------------------------------------------------
214
215/// Normalise an angle to \[0, 2π).
216fn mod2pi(angle: f64) -> f64 {
217    let mut v = angle % (2.0 * PI);
218    if v < 0.0 {
219        v += 2.0 * PI;
220    }
221    v
222}
223
224fn segment_end_yaw(points: &[Point2D], path: &DubinsPath, segment_index: usize) -> f64 {
225    let segments = path.path_type.segments();
226    let mut yaw = path.start.yaw;
227    for (i, seg) in segments.iter().enumerate().take(segment_index) {
228        let normalized_len = path.lengths[i] * path.curvature;
229        yaw = match seg {
230            SegmentType::Straight => yaw,
231            SegmentType::Left => yaw + normalized_len,
232            SegmentType::Right => yaw - normalized_len,
233        };
234    }
235    if points.is_empty() {
236        path.start.yaw
237    } else {
238        yaw
239    }
240}
241
242/// Interpolate a point at a normalized segment length.
243fn interpolate_segment(
244    length: f64,
245    seg: SegmentType,
246    curvature: f64,
247    origin: Point2D,
248    origin_yaw: f64,
249) -> (f64, f64, f64) {
250    match seg {
251        SegmentType::Straight => {
252            let nx = origin.x + length / curvature * origin_yaw.cos();
253            let ny = origin.y + length / curvature * origin_yaw.sin();
254            (nx, ny, origin_yaw)
255        }
256        SegmentType::Left => {
257            let r = 1.0 / curvature;
258            let ldx = length.sin() * r;
259            let ldy = (1.0 - length.cos()) * r;
260            let gdx = origin_yaw.cos() * ldx - origin_yaw.sin() * ldy;
261            let gdy = origin_yaw.sin() * ldx + origin_yaw.cos() * ldy;
262            (origin.x + gdx, origin.y + gdy, origin_yaw + length)
263        }
264        SegmentType::Right => {
265            let r = 1.0 / curvature;
266            let ldx = length.sin() * r;
267            let ldy = -(1.0 - length.cos()) * r;
268            let gdx = origin_yaw.cos() * ldx - origin_yaw.sin() * ldy;
269            let gdy = origin_yaw.sin() * ldx + origin_yaw.cos() * ldy;
270            (origin.x + gdx, origin.y + gdy, origin_yaw - length)
271        }
272    }
273}
274
275/// Compute the three normalised segment lengths for a given Dubins path type.
276///
277/// `alpha` and `beta` are the start and goal headings relative to the
278/// start-to-goal line, normalised to \[0, 2π). `d` is the normalised distance
279/// (Euclidean distance × curvature).
280///
281/// Returns `None` when the geometry has no real solution.
282fn compute_path_params(
283    alpha: f64,
284    beta: f64,
285    d: f64,
286    path_type: DubinsPathType,
287) -> Option<[f64; 3]> {
288    match path_type {
289        DubinsPathType::LSL => lsl(alpha, beta, d),
290        DubinsPathType::RSR => rsr(alpha, beta, d),
291        DubinsPathType::LSR => lsr(alpha, beta, d),
292        DubinsPathType::RSL => rsl(alpha, beta, d),
293        DubinsPathType::RLR => rlr(alpha, beta, d),
294        DubinsPathType::LRL => lrl(alpha, beta, d),
295    }
296}
297
298fn lsl(alpha: f64, beta: f64, d: f64) -> Option<[f64; 3]> {
299    let sa = alpha.sin();
300    let sb = beta.sin();
301    let ca = alpha.cos();
302    let cb = beta.cos();
303    let cos_ab = (alpha - beta).cos();
304    let p_sq = 2.0 + d * d - 2.0 * cos_ab + 2.0 * d * (sa - sb);
305    if p_sq < 0.0 {
306        return None;
307    }
308    let tmp = (cb - ca).atan2(d + sa - sb);
309    let t = mod2pi(-alpha + tmp);
310    let p = p_sq.sqrt();
311    let q = mod2pi(beta - tmp);
312    Some([t, p, q])
313}
314
315fn rsr(alpha: f64, beta: f64, d: f64) -> Option<[f64; 3]> {
316    let sa = alpha.sin();
317    let sb = beta.sin();
318    let ca = alpha.cos();
319    let cb = beta.cos();
320    let cos_ab = (alpha - beta).cos();
321    let p_sq = 2.0 + d * d - 2.0 * cos_ab + 2.0 * d * (sb - sa);
322    if p_sq < 0.0 {
323        return None;
324    }
325    let tmp = (ca - cb).atan2(d - sa + sb);
326    let t = mod2pi(alpha - tmp);
327    let p = p_sq.sqrt();
328    let q = mod2pi(-beta + tmp);
329    Some([t, p, q])
330}
331
332fn lsr(alpha: f64, beta: f64, d: f64) -> Option<[f64; 3]> {
333    let sa = alpha.sin();
334    let sb = beta.sin();
335    let ca = alpha.cos();
336    let cb = beta.cos();
337    let cos_ab = (alpha - beta).cos();
338    let p_sq = -2.0 + d * d + 2.0 * cos_ab + 2.0 * d * (sa + sb);
339    if p_sq < 0.0 {
340        return None;
341    }
342    let p = p_sq.sqrt();
343    let tmp = (-ca - cb).atan2(d + sa + sb) - (-2.0_f64).atan2(p);
344    let t = mod2pi(-alpha + tmp);
345    let q = mod2pi(-beta + tmp);
346    Some([t, p, q])
347}
348
349fn rsl(alpha: f64, beta: f64, d: f64) -> Option<[f64; 3]> {
350    let sa = alpha.sin();
351    let sb = beta.sin();
352    let ca = alpha.cos();
353    let cb = beta.cos();
354    let cos_ab = (alpha - beta).cos();
355    let p_sq = -2.0 + d * d + 2.0 * cos_ab - 2.0 * d * (sa + sb);
356    if p_sq < 0.0 {
357        return None;
358    }
359    let p = p_sq.sqrt();
360    let tmp = (ca + cb).atan2(d - sa - sb) - (2.0_f64).atan2(p);
361    let t = mod2pi(alpha - tmp);
362    let q = mod2pi(beta - tmp);
363    Some([t, p, q])
364}
365
366fn rlr(alpha: f64, beta: f64, d: f64) -> Option<[f64; 3]> {
367    let sa = alpha.sin();
368    let sb = beta.sin();
369    let ca = alpha.cos();
370    let cb = beta.cos();
371    let cos_ab = (alpha - beta).cos();
372    let val = (6.0 - d * d + 2.0 * cos_ab + 2.0 * d * (sa - sb)) / 8.0;
373    if val.abs() > 1.0 {
374        return None;
375    }
376    let p = mod2pi(2.0 * PI - val.acos());
377    let t = mod2pi(alpha - (ca - cb).atan2(d - sa + sb) + mod2pi(p / 2.0));
378    let q = mod2pi(alpha - beta - t + mod2pi(p));
379    Some([t, p, q])
380}
381
382fn lrl(alpha: f64, beta: f64, d: f64) -> Option<[f64; 3]> {
383    let sa = alpha.sin();
384    let sb = beta.sin();
385    let ca = alpha.cos();
386    let cb = beta.cos();
387    let cos_ab = (alpha - beta).cos();
388    let val = (6.0 - d * d + 2.0 * cos_ab - 2.0 * d * (sa - sb)) / 8.0;
389    if val.abs() > 1.0 {
390        return None;
391    }
392    let p = mod2pi(2.0 * PI - val.acos());
393    let t = mod2pi(-alpha - (ca - cb).atan2(d + sa - sb) + p / 2.0);
394    let q = mod2pi(mod2pi(beta) - alpha - t + mod2pi(p));
395    Some([t, p, q])
396}
397
398// ---------------------------------------------------------------------------
399// Tests
400// ---------------------------------------------------------------------------
401
402#[cfg(test)]
403#[allow(clippy::excessive_precision)]
404mod tests {
405    use super::*;
406    use std::f64::consts::FRAC_PI_2;
407
408    const CURVATURE: f64 = 1.0;
409    const EPS: f64 = 0.3;
410
411    fn approx_eq(a: f64, b: f64, tol: f64) -> bool {
412        (a - b).abs() < tol
413    }
414
415    fn assert_point_close(actual: &Point2D, expected: (f64, f64), tolerance: f64) {
416        assert!(
417            (actual.x - expected.0).abs() < tolerance,
418            "x mismatch: actual=({}, {}), expected={expected:?}",
419            actual.x,
420            actual.y
421        );
422        assert!(
423            (actual.y - expected.1).abs() < tolerance,
424            "y mismatch: actual=({}, {}), expected={expected:?}",
425            actual.x,
426            actual.y
427        );
428    }
429
430    #[test]
431    fn test_dubins_straight_line() {
432        // Same heading, goal straight ahead → should be close to pure straight.
433        let planner = DubinsPlanner::new(CURVATURE);
434        let start = Pose2D::new(0.0, 0.0, 0.0);
435        let goal = Pose2D::new(10.0, 0.0, 0.0);
436        let path = planner.plan(start, goal).unwrap();
437
438        // Total length should be approximately the Euclidean distance.
439        assert!(
440            approx_eq(path.total_length, 10.0, EPS),
441            "expected ~10.0, got {}",
442            path.total_length
443        );
444
445        // Sample and verify endpoints.
446        let sampled = planner.sample_path(&path, 0.1);
447        assert!(!sampled.is_empty());
448        let first = &sampled.points[0];
449        let last = &sampled.points[sampled.len() - 1];
450        assert!(approx_eq(first.x, 0.0, EPS) && approx_eq(first.y, 0.0, EPS));
451        assert!(
452            approx_eq(last.x, 10.0, EPS) && approx_eq(last.y, 0.0, EPS),
453            "last point = ({}, {})",
454            last.x,
455            last.y
456        );
457    }
458
459    #[test]
460    fn test_dubins_u_turn() {
461        // Goal at same position but opposite heading → requires a turn-around.
462        let planner = DubinsPlanner::new(CURVATURE);
463        let start = Pose2D::new(0.0, 0.0, 0.0);
464        let goal = Pose2D::new(0.0, 0.0, PI);
465        let path = planner.plan(start, goal).unwrap();
466
467        // A U-turn for curvature=1 (radius=1) should have length = π.
468        // (semicircle of radius 1)
469        assert!(
470            path.total_length >= PI - EPS,
471            "U-turn too short: {}",
472            path.total_length
473        );
474        assert!(path.total_length.is_finite());
475    }
476
477    #[test]
478    fn test_dubins_90_degree_turn() {
479        let planner = DubinsPlanner::new(1.0);
480        let start = Pose2D::new(0.0, 0.0, 0.0);
481        let goal = Pose2D::new(3.0, 3.0, FRAC_PI_2);
482        let path = planner.plan(start, goal).unwrap();
483
484        assert!(path.total_length > 0.0);
485        assert!(path.total_length.is_finite());
486
487        let sampled = planner.sample_path(&path, 0.05);
488        assert!(sampled.len() > 2);
489        let last = &sampled.points[sampled.len() - 1];
490        assert!(
491            approx_eq(last.x, 3.0, EPS) && approx_eq(last.y, 3.0, EPS),
492            "endpoint ({}, {}) far from goal (3, 3)",
493            last.x,
494            last.y
495        );
496    }
497
498    #[test]
499    fn test_dubins_all_types_finite() {
500        // Verify that the planner returns a finite path for a variety of goals.
501        let planner = DubinsPlanner::new(0.5);
502        let start = Pose2D::new(0.0, 0.0, 0.0);
503
504        let goals = [
505            Pose2D::new(4.0, 4.0, 1.0),
506            Pose2D::new(-3.0, 5.0, -1.5),
507            Pose2D::new(6.0, -2.0, PI),
508            Pose2D::new(0.0, 8.0, -FRAC_PI_2),
509        ];
510
511        for goal in &goals {
512            let path = planner.plan(start, *goal).unwrap();
513            assert!(
514                path.total_length.is_finite(),
515                "non-finite length for goal {:?}",
516                goal
517            );
518            assert!(
519                path.lengths.iter().all(|l| l.is_finite() && *l >= 0.0),
520                "invalid segment lengths for goal {:?}: {:?}",
521                goal,
522                path.lengths
523            );
524        }
525    }
526
527    #[test]
528    fn test_dubins_path_length_positive() {
529        let planner = DubinsPlanner::new(2.0);
530        let start = Pose2D::new(1.0, 2.0, 0.5);
531        let goal = Pose2D::new(5.0, 6.0, -0.5);
532        let path = planner.plan(start, goal).unwrap();
533
534        assert!(path.total_length > 0.0);
535        // Total length must be at least the straight-line distance.
536        let euclidean = ((goal.x - start.x).powi(2) + (goal.y - start.y).powi(2)).sqrt();
537        assert!(
538            path.total_length >= euclidean - 1e-9,
539            "Dubins path ({}) shorter than Euclidean ({})",
540            path.total_length,
541            euclidean
542        );
543    }
544
545    #[test]
546    fn test_dubins_invalid_curvature() {
547        let planner = DubinsPlanner::new(0.0);
548        let result = planner.plan(Pose2D::origin(), Pose2D::new(1.0, 0.0, 0.0));
549        assert!(result.is_err());
550
551        let planner = DubinsPlanner::new(-1.0);
552        let result = planner.plan(Pose2D::origin(), Pose2D::new(1.0, 0.0, 0.0));
553        assert!(result.is_err());
554    }
555
556    #[test]
557    fn test_dubins_sample_path_consistent_length() {
558        let planner = DubinsPlanner::new(1.0);
559        let start = Pose2D::new(0.0, 0.0, 0.0);
560        let goal = Pose2D::new(5.0, 0.0, 0.0);
561        let path = planner.plan(start, goal).unwrap();
562        let sampled = planner.sample_path(&path, 0.1);
563
564        // The polyline length should approximate the analytical total length.
565        let polyline_len = sampled.total_length();
566        assert!(
567            approx_eq(polyline_len, path.total_length, 0.5),
568            "polyline {} vs analytical {}",
569            polyline_len,
570            path.total_length
571        );
572    }
573
574    #[test]
575    fn test_compute_path_params_match_upstream_representative_window() {
576        let alpha = PI;
577        let beta = FRAC_PI_2;
578        let d = 5.656_854_249_492_38;
579
580        assert_eq!(
581            compute_path_params(alpha, beta, d, DubinsPathType::LSL),
582            Some([
583                3.353_117_643_613_227_3,
584                4.763_012_859_631_52,
585                1.359_271_336_771_462_2
586            ])
587        );
588        assert_eq!(
589            compute_path_params(alpha, beta, d, DubinsPathType::RSR),
590            Some([
591                3.290_698_833_617_270_2,
592                6.731_545_773_370_686,
593                4.563_282_800_357_213
594            ])
595        );
596        assert_eq!(
597            compute_path_params(alpha, beta, d, DubinsPathType::LSR),
598            Some([
599                3.592_361_893_573_783,
600                6.427_574_075_729_097,
601                5.163_158_220_368_679
602            ])
603        );
604        assert_eq!(
605            compute_path_params(alpha, beta, d, DubinsPathType::RSL),
606            Some([
607                3.786_455_298_170_522_6,
608                4.322_764_335_586_111,
609                2.215_658_971_375_626
610            ])
611        );
612        assert_eq!(
613            compute_path_params(alpha, beta, d, DubinsPathType::RLR),
614            None
615        );
616        assert_eq!(
617            compute_path_params(alpha, beta, d, DubinsPathType::LRL),
618            None
619        );
620    }
621
622    #[test]
623    fn test_compute_path_params_match_upstream_rlr_lrl_case() {
624        let alpha = 0.0;
625        let beta = 0.0;
626        let d = 0.5;
627
628        assert_eq!(
629            compute_path_params(alpha, beta, d, DubinsPathType::RLR),
630            Some([
631                3.016_264_822_421_727_7,
632                6.032_529_644_843_455,
633                3.016_264_822_421_727_7
634            ])
635        );
636        assert_eq!(
637            compute_path_params(alpha, beta, d, DubinsPathType::LRL),
638            Some([
639                3.016_264_822_421_727_7,
640                6.032_529_644_843_455,
641                3.016_264_822_421_727_7
642            ])
643        );
644    }
645
646    #[test]
647    fn test_dubins_plan_matches_upstream_main_example() {
648        let planner = DubinsPlanner::new(1.0);
649        let start = Pose2D::new(1.0, 1.0, 45.0_f64.to_radians());
650        let goal = Pose2D::new(-3.0, -3.0, -45.0_f64.to_radians());
651        let path = planner.plan(start, goal).unwrap();
652
653        assert_eq!(path.path_type, DubinsPathType::LSL);
654        assert!(approx_eq(path.lengths[0], 3.353_117_643_613_227, 1e-12));
655        assert!(approx_eq(path.lengths[1], 4.763_012_859_631_52, 1e-12));
656        assert!(approx_eq(path.lengths[2], 1.359_271_336_771_462, 1e-12));
657        assert!(approx_eq(path.total_length, 9.475_401_840_016_21, 1e-12));
658    }
659
660    #[test]
661    fn test_dubins_sample_path_matches_upstream_main_example() {
662        let planner = DubinsPlanner::new(1.0);
663        let start = Pose2D::new(1.0, 1.0, 45.0_f64.to_radians());
664        let goal = Pose2D::new(-3.0, -3.0, -45.0_f64.to_radians());
665        let path = planner.plan(start, goal).unwrap();
666        let sampled = planner.sample_path(&path, 0.1);
667
668        let expected_samples = [
669            (0usize, 1.0, 1.0),
670            (10, 1.269_954_482_712_928, 1.920_065_196_345_844),
671            (20, 0.641_603_345_345_556, 2.644_337_407_902_28),
672            (30, -0.307_350_274_196_291, 2.506_924_103_516_754),
673            (93, -3.0, -3.0),
674        ];
675
676        assert_eq!(sampled.len(), 94);
677        for (index, x, y) in expected_samples {
678            assert_point_close(&sampled.points[index], (x, y), 1e-12);
679        }
680
681        let sum_x: f64 = sampled.points.iter().map(|point| point.x).sum();
682        let sum_y: f64 = sampled.points.iter().map(|point| point.y).sum();
683        let weighted_sum_x: f64 = sampled
684            .points
685            .iter()
686            .enumerate()
687            .map(|(index, point)| (index + 1) as f64 * point.x)
688            .sum();
689        let weighted_sum_y: f64 = sampled
690            .points
691            .iter()
692            .enumerate()
693            .map(|(index, point)| (index + 1) as f64 * point.y)
694            .sum();
695
696        assert!(approx_eq(sum_x, -105.952_049_447_379_94, 1e-9));
697        assert!(approx_eq(sum_y, 52.686_985_733_395_81, 1e-9));
698        assert!(approx_eq(weighted_sum_x, -8_974.696_943_796_953, 1e-6));
699        assert!(approx_eq(weighted_sum_y, -1_434.362_763_693_722_6, 1e-6));
700    }
701}
rust_robotics_planning/dubins_path.rs

rust_robotics_planning/
dubins_path.rs
