Skip to main content

rust_robotics_planning/
clothoid_path.rs

1//! Clothoid (Euler Spiral) Path Planner
2//!
3//! A clothoid is a curve whose curvature changes linearly with arc length.
4//! It is the industry standard for road geometry design in highway engineering
5//! and autonomous driving, providing smooth curvature transitions.
6//!
7//! The clothoid is parametrized by:
8//!   `kappa(s) = kappa0 + kappa1 * s`
9//!
10//! where `kappa0` is the initial curvature, `kappa1` is the curvature rate
11//! (sharpness), and `s` is the arc length.
12//!
13//! The position along the curve is obtained by integrating:
14//!   `x(s) = integral cos(theta(t)) dt`
15//!   `y(s) = integral sin(theta(t)) dt`
16//!
17//! where `theta(s) = theta0 + kappa0 * s + 0.5 * kappa1 * s^2`.
18//!
19//! # References
20//!
21//! * Bertolazzi, E. & Frego, M. (2015). "G1 fitting with clothoids".
22//!   *Mathematical Methods in the Applied Sciences*, 38(5), 881-897.
23
24use std::f64::consts::PI;
25
26use rust_robotics_core::error::{RoboticsError, RoboticsResult};
27use rust_robotics_core::normalize_angle;
28use rust_robotics_core::types::{Path2D, Pose2D};
29
30// --- Fresnel integrals -------------------------------------------------------
31
32/// Evaluate the Fresnel integrals C(t) and S(t) defined as:
33///   C(t) = integral_0^t cos(pi/2 * u^2) du
34///   S(t) = integral_0^t sin(pi/2 * u^2) du
35///
36/// Uses a power-series expansion, which converges well for moderate |t|.
37pub fn fresnel(t: f64) -> (f64, f64) {
38    let t_abs = t.abs();
39
40    if t_abs < 1e-15 {
41        return (0.0, 0.0);
42    }
43
44    // For large arguments, use asymptotic approximation
45    if t_abs > 6.0 {
46        let (c, s) = fresnel_asymptotic(t_abs);
47        if t < 0.0 {
48            return (-c, -s);
49        }
50        return (c, s);
51    }
52
53    // Power-series expansion
54    let (c, s) = fresnel_series(t_abs);
55    if t < 0.0 {
56        (-c, -s)
57    } else {
58        (c, s)
59    }
60}
61
62/// Fresnel integrals via power-series expansion.
63/// Good convergence for |t| <= ~6.
64fn fresnel_series(t: f64) -> (f64, f64) {
65    let half_pi = PI / 2.0;
66    let x = half_pi * t * t;
67
68    let mut c_sum = 0.0;
69    let mut s_sum = 0.0;
70    let mut term = t; // first term of C series: t
71    let mut sign = 1.0_f64;
72
73    // C(t) = sum_{k=0}^{inf} (-1)^k * (pi/2)^{2k} * t^{4k+1} / ((4k+1) * (2k)!)
74    // S(t) = sum_{k=0}^{inf} (-1)^k * (pi/2)^{2k+1} * t^{4k+3} / ((4k+3) * (2k+1)!)
75    for k in 0..25 {
76        let c_term = sign * term / (4 * k + 1) as f64;
77        c_sum += c_term;
78
79        let s_term = sign * term * x / (4 * k + 3) as f64 / (2 * k + 1) as f64;
80        s_sum += s_term;
81
82        if c_term.abs() < 1e-16 && s_term.abs() < 1e-16 {
83            break;
84        }
85
86        // Update term: multiply by (pi/2)^2 * t^4 / ((2k+1)*(2k+2))
87        term *= x * x / ((2 * k + 1) as f64 * (2 * k + 2) as f64);
88        sign = -sign;
89    }
90
91    (c_sum, s_sum)
92}
93
94/// Asymptotic expansion for large arguments.
95fn fresnel_asymptotic(t: f64) -> (f64, f64) {
96    let x = PI * t * t;
97    let sin_x = (x / 2.0).sin();
98    let cos_x = (x / 2.0).cos();
99    let inv_x = 1.0 / (PI * t);
100
101    // f(t) ~ 1/(pi*t) * (1 - 3/(pi*t)^4 + ...)
102    // g(t) ~ 1/(pi*t)^2 * (1 - 15/(pi*t)^4 + ...)
103    let pi_t2 = (PI * t).powi(2);
104    let f = inv_x * (1.0 - 3.0 / (pi_t2 * pi_t2));
105    let g = inv_x / (PI * t) * (1.0 - 15.0 / (pi_t2 * pi_t2));
106
107    let c = 0.5 + f * sin_x - g * cos_x;
108    let s = 0.5 - f * cos_x - g * sin_x;
109    (c, s)
110}
111
112// --- Clothoid primitives -----------------------------------------------------
113
114/// Evaluate a clothoid curve at arc length `s`, starting from the origin
115/// with heading `theta0`, initial curvature `kappa0`, and sharpness `kappa1`.
116///
117/// Returns `(x, y, theta)` at arc length `s`.
118///
119/// Integration is performed via Simpson's rule for numerical stability.
120pub fn clothoid_eval(theta0: f64, kappa0: f64, kappa1: f64, s: f64) -> (f64, f64, f64) {
121    // Integrate position by numerical quadrature (Simpson's rule)
122    let n = 100; // number of intervals (must be even)
123    let ds = s / n as f64;
124    let mut x = 0.0;
125    let mut y = 0.0;
126
127    for i in 0..=n {
128        let si = ds * i as f64;
129        let theta = theta0 + kappa0 * si + 0.5 * kappa1 * si * si;
130        let weight = if i == 0 || i == n {
131            1.0
132        } else if i % 2 == 1 {
133            4.0
134        } else {
135            2.0
136        };
137        x += weight * theta.cos();
138        y += weight * theta.sin();
139    }
140    x *= ds / 3.0;
141    y *= ds / 3.0;
142
143    let theta_end = theta0 + kappa0 * s + 0.5 * kappa1 * s * s;
144    (x, y, theta_end)
145}
146
147/// Sample `n_points` along a clothoid starting at pose `(x0, y0, theta0)`,
148/// with initial curvature `kappa0`, sharpness `kappa1`, and total arc length `length`.
149///
150/// Returns `(xs, ys, thetas, kappas)` vectors at each sample point.
151pub fn clothoid_sample(
152    x0: f64,
153    y0: f64,
154    theta0: f64,
155    kappa0: f64,
156    kappa1: f64,
157    length: f64,
158    n_points: usize,
159) -> (Vec<f64>, Vec<f64>, Vec<f64>, Vec<f64>) {
160    let mut xs = Vec::with_capacity(n_points);
161    let mut ys = Vec::with_capacity(n_points);
162    let mut thetas = Vec::with_capacity(n_points);
163    let mut kappas = Vec::with_capacity(n_points);
164
165    let cos0 = theta0.cos();
166    let sin0 = theta0.sin();
167
168    for i in 0..n_points {
169        let s = if n_points > 1 {
170            length * i as f64 / (n_points - 1) as f64
171        } else {
172            0.0
173        };
174
175        let (dx, dy, theta) = clothoid_eval(0.0, kappa0, kappa1, s);
176        // Rotate by theta0 and translate
177        let x = x0 + cos0 * dx - sin0 * dy;
178        let y = y0 + sin0 * dx + cos0 * dy;
179        let kappa = kappa0 + kappa1 * s;
180
181        xs.push(x);
182        ys.push(y);
183        thetas.push(theta0 + theta);
184        kappas.push(kappa);
185    }
186
187    (xs, ys, thetas, kappas)
188}
189
190// --- G1 Hermite interpolation (clothoid fitting) ----------------------------
191
192/// Solve the G1 Hermite interpolation problem: find a clothoid connecting
193/// two poses `(x0, y0, theta0)` and `(x1, y1, theta1)`.
194///
195/// Returns `(kappa0, kappa1, length)` -- initial curvature, sharpness, arc length.
196///
197/// Uses an iterative Newton-Raphson method on the clothoid parameters.
198pub fn clothoid_g1_fit(start: &Pose2D, goal: &Pose2D) -> RoboticsResult<(f64, f64, f64)> {
199    let dx = goal.x - start.x;
200    let dy = goal.y - start.y;
201    let chord = (dx * dx + dy * dy).sqrt();
202
203    if chord < 1e-12 {
204        return Err(RoboticsError::InvalidParameter(
205            "start and goal poses are coincident".to_string(),
206        ));
207    }
208
209    let phi = dy.atan2(dx);
210    let alpha = normalize_angle(start.yaw - phi);
211    let beta = normalize_angle(goal.yaw - phi);
212
213    // Initial guess for the length
214    let mut length = chord;
215
216    // Newton iteration to find kappa0, kappa1, and length.
217    // In the rotated frame the clothoid starts at (0,0) with heading alpha
218    // and must reach (chord, 0) with heading beta.
219
220    let max_iter = 50;
221    let tol = 1e-12;
222
223    let mut k0 = (alpha + beta) / length;
224    let mut k1 = 2.0 * (beta - alpha) / (length * length);
225
226    for _iter in 0..max_iter {
227        // Evaluate clothoid at current parameters
228        let (ex, ey, _etheta) = clothoid_eval(alpha, k0, k1, length);
229
230        // Residuals: endpoint should be at (chord, 0) with heading beta
231        let rx = ex - chord;
232        let ry = ey;
233        let r_theta = alpha + k0 * length + 0.5 * k1 * length * length - beta;
234
235        if rx.abs() < tol && ry.abs() < tol && r_theta.abs() < tol {
236            break;
237        }
238
239        // Compute Jacobian numerically
240        let eps = 1e-8;
241
242        let (ex_dk0, ey_dk0, _) = clothoid_eval(alpha, k0 + eps, k1, length);
243        let dex_dk0 = (ex_dk0 - ex) / eps;
244        let dey_dk0 = (ey_dk0 - ey) / eps;
245
246        let (ex_dk1, ey_dk1, _) = clothoid_eval(alpha, k0, k1 + eps, length);
247        let dex_dk1 = (ex_dk1 - ex) / eps;
248        let dey_dk1 = (ey_dk1 - ey) / eps;
249
250        let (ex_dl, ey_dl, _) = clothoid_eval(alpha, k0, k1, length + eps);
251        let dex_dl = (ex_dl - ex) / eps;
252        let dey_dl = (ey_dl - ey) / eps;
253
254        // 3x3 Jacobian for (rx, ry, r_theta) w.r.t. (k0, k1, length)
255        let dtheta_dk0 = length;
256        let dtheta_dk1 = 0.5 * length * length;
257        let dtheta_dl = k0 + k1 * length;
258
259        // Solve 3x3 system using nalgebra
260        let jac = nalgebra::Matrix3::new(
261            dex_dk0, dex_dk1, dex_dl, dey_dk0, dey_dk1, dey_dl, dtheta_dk0, dtheta_dk1, dtheta_dl,
262        );
263
264        let residual = nalgebra::Vector3::new(rx, ry, r_theta);
265
266        let Some(delta) = jac.try_inverse().map(|inv| inv * residual) else {
267            return Err(RoboticsError::NumericalError(
268                "Jacobian is singular during clothoid fitting".to_string(),
269            ));
270        };
271
272        k0 -= delta[0];
273        k1 -= delta[1];
274        length -= delta[2];
275
276        // Keep length positive
277        if length < 1e-10 {
278            length = chord * 0.5;
279        }
280    }
281
282    // Verify solution
283    let (ex, ey, _) = clothoid_eval(alpha, k0, k1, length);
284    let err = ((ex - chord).powi(2) + ey.powi(2)).sqrt();
285    if err > 1e-6 {
286        return Err(RoboticsError::NumericalError(format!(
287            "clothoid G1 fitting did not converge (residual={err:.6e})"
288        )));
289    }
290
291    Ok((k0, k1, length))
292}
293
294// --- ClothoidPath struct -----------------------------------------------------
295
296/// A clothoid (Euler spiral) path connecting two poses.
297#[derive(Debug, Clone)]
298pub struct ClothoidPath {
299    /// Start pose
300    pub start: Pose2D,
301    /// End pose
302    pub goal: Pose2D,
303    /// Initial curvature
304    pub kappa0: f64,
305    /// Curvature rate (sharpness)
306    pub kappa1: f64,
307    /// Total arc length
308    pub length: f64,
309    /// Sampled path points
310    pub path: Path2D,
311    /// Heading at each sample point
312    pub yaw: Vec<f64>,
313    /// Curvature at each sample point
314    pub curvature: Vec<f64>,
315}
316
317/// Configuration for the clothoid path planner.
318#[derive(Debug, Clone)]
319pub struct ClothoidConfig {
320    /// Number of sample points along the path
321    pub n_points: usize,
322}
323
324impl Default for ClothoidConfig {
325    fn default() -> Self {
326        Self { n_points: 100 }
327    }
328}
329
330/// Clothoid path planner.
331///
332/// Computes a clothoid curve (Euler spiral) that connects two poses with
333/// G1 continuity (matching position and heading at both endpoints).
334#[derive(Debug, Clone)]
335pub struct ClothoidPlanner {
336    pub config: ClothoidConfig,
337}
338
339impl ClothoidPlanner {
340    pub fn new(config: ClothoidConfig) -> Self {
341        Self { config }
342    }
343
344    /// Plan a clothoid path from `start` to `goal`.
345    pub fn plan(&self, start: &Pose2D, goal: &Pose2D) -> RoboticsResult<ClothoidPath> {
346        let (kappa0, kappa1, length) = clothoid_g1_fit(start, goal)?;
347
348        let (xs, ys, yaw, curvature) = clothoid_sample(
349            start.x,
350            start.y,
351            start.yaw,
352            kappa0,
353            kappa1,
354            length,
355            self.config.n_points,
356        );
357
358        let path = Path2D::from_xy(&xs, &ys);
359
360        Ok(ClothoidPath {
361            start: *start,
362            goal: *goal,
363            kappa0,
364            kappa1,
365            length,
366            path,
367            yaw,
368            curvature,
369        })
370    }
371
372    /// Plan a clothoid path from raw coordinates.
373    pub fn plan_from_xy(
374        &self,
375        x0: f64,
376        y0: f64,
377        yaw0: f64,
378        x1: f64,
379        y1: f64,
380        yaw1: f64,
381    ) -> RoboticsResult<ClothoidPath> {
382        self.plan(&Pose2D::new(x0, y0, yaw0), &Pose2D::new(x1, y1, yaw1))
383    }
384}
385
386// --- Tests -------------------------------------------------------------------
387
388#[cfg(test)]
389mod tests {
390    use super::*;
391    use std::f64::consts::FRAC_PI_2;
392
393    const TOL: f64 = 1e-6;
394
395    fn assert_close(actual: f64, expected: f64, tol: f64, label: &str) {
396        assert!(
397            (actual - expected).abs() < tol,
398            "{label}: actual={actual}, expected={expected}, diff={}",
399            (actual - expected).abs()
400        );
401    }
402
403    // -- Fresnel integral tests -----------------------------------------------
404
405    #[test]
406    fn test_fresnel_zero() {
407        let (c, s) = fresnel(0.0);
408        assert_close(c, 0.0, 1e-15, "C(0)");
409        assert_close(s, 0.0, 1e-15, "S(0)");
410    }
411
412    #[test]
413    fn test_fresnel_known_values() {
414        // C(1) ~ 0.7798934, S(1) ~ 0.4382591
415        let (c, s) = fresnel(1.0);
416        assert_close(c, 0.7798934003768228, 1e-10, "C(1)");
417        assert_close(s, 0.4382591473903548, 1e-10, "S(1)");
418    }
419
420    #[test]
421    fn test_fresnel_odd_symmetry() {
422        let (c_pos, s_pos) = fresnel(2.0);
423        let (c_neg, s_neg) = fresnel(-2.0);
424        assert_close(c_neg, -c_pos, 1e-14, "C(-t) = -C(t)");
425        assert_close(s_neg, -s_pos, 1e-14, "S(-t) = -S(t)");
426    }
427
428    #[test]
429    fn test_fresnel_large_argument_limit() {
430        // As t -> inf, C(t) -> 0.5, S(t) -> 0.5
431        let (c, s) = fresnel(100.0);
432        assert_close(c, 0.5, 0.005, "C(inf)");
433        assert_close(s, 0.5, 0.005, "S(inf)");
434    }
435
436    // -- Clothoid evaluation tests --------------------------------------------
437
438    #[test]
439    fn test_clothoid_eval_straight_line() {
440        // Zero curvature, heading = 0 -> straight line along x
441        let (x, y, theta) = clothoid_eval(0.0, 0.0, 0.0, 5.0);
442        assert_close(x, 5.0, TOL, "x straight");
443        assert_close(y, 0.0, TOL, "y straight");
444        assert_close(theta, 0.0, TOL, "theta straight");
445    }
446
447    #[test]
448    fn test_clothoid_eval_circular_arc() {
449        // Constant curvature (kappa1 = 0) -> circular arc
450        let radius = 10.0;
451        let kappa0 = 1.0 / radius;
452        let arc_length = FRAC_PI_2 * radius; // quarter circle
453
454        let (x, y, theta) = clothoid_eval(0.0, kappa0, 0.0, arc_length);
455        // Quarter circle: endpoint should be at (R, R) approximately
456        assert_close(x, radius, 0.01, "x circular arc");
457        assert_close(y, radius, 0.01, "y circular arc");
458        assert_close(theta, FRAC_PI_2, TOL, "theta circular arc");
459    }
460
461    #[test]
462    fn test_clothoid_eval_with_sharpness() {
463        // Non-zero kappa1: curvature grows linearly
464        let (x, y, theta) = clothoid_eval(0.0, 0.0, 0.1, 3.0);
465        // theta(3) = 0 + 0*3 + 0.5*0.1*9 = 0.45
466        assert_close(theta, 0.45, TOL, "theta with sharpness");
467        assert!(x.is_finite(), "x should be finite");
468        assert!(y.is_finite(), "y should be finite");
469        assert!(y > 0.0, "curve should bend left (positive y)");
470    }
471
472    // -- Clothoid sampling tests ----------------------------------------------
473
474    #[test]
475    fn test_clothoid_sample_straight() {
476        let (xs, ys, thetas, kappas) = clothoid_sample(0.0, 0.0, 0.0, 0.0, 0.0, 10.0, 11);
477
478        assert_eq!(xs.len(), 11);
479        for i in 0..11 {
480            let s = i as f64;
481            assert_close(xs[i], s, TOL, &format!("x[{i}]"));
482            assert_close(ys[i], 0.0, TOL, &format!("y[{i}]"));
483            assert_close(thetas[i], 0.0, TOL, &format!("theta[{i}]"));
484            assert_close(kappas[i], 0.0, TOL, &format!("kappa[{i}]"));
485        }
486    }
487
488    #[test]
489    fn test_clothoid_sample_rotated_straight() {
490        // Straight line at 45 degrees
491        let angle = PI / 4.0;
492        let length = 10.0;
493        let (xs, ys, _thetas, _kappas) = clothoid_sample(1.0, 2.0, angle, 0.0, 0.0, length, 11);
494
495        let end_x = 1.0 + length * angle.cos();
496        let end_y = 2.0 + length * angle.sin();
497        assert_close(*xs.last().unwrap(), end_x, TOL, "end x");
498        assert_close(*ys.last().unwrap(), end_y, TOL, "end y");
499    }
500
501    // -- G1 fitting tests -----------------------------------------------------
502
503    #[test]
504    fn test_g1_fit_straight_line() {
505        let start = Pose2D::new(0.0, 0.0, 0.0);
506        let goal = Pose2D::new(10.0, 0.0, 0.0);
507
508        let (kappa0, kappa1, length) = clothoid_g1_fit(&start, &goal).unwrap();
509        assert_close(kappa0, 0.0, 1e-4, "kappa0 straight");
510        assert_close(kappa1, 0.0, 1e-4, "kappa1 straight");
511        assert_close(length, 10.0, 0.1, "length straight");
512    }
513
514    #[test]
515    fn test_g1_fit_simple_curve() {
516        let start = Pose2D::new(0.0, 0.0, 0.0);
517        let goal = Pose2D::new(5.0, 3.0, PI / 4.0);
518
519        let result = clothoid_g1_fit(&start, &goal);
520        assert!(result.is_ok(), "G1 fit should converge");
521
522        let (kappa0, kappa1, length) = result.unwrap();
523        assert!(length > 0.0, "length should be positive");
524
525        // Verify the endpoint matches
526        let (xs, ys, thetas, _) =
527            clothoid_sample(start.x, start.y, start.yaw, kappa0, kappa1, length, 100);
528
529        let end_x = *xs.last().unwrap();
530        let end_y = *ys.last().unwrap();
531        let end_theta = *thetas.last().unwrap();
532
533        assert_close(end_x, goal.x, 0.01, "fitted end x");
534        assert_close(end_y, goal.y, 0.01, "fitted end y");
535        assert_close(
536            normalize_angle(end_theta),
537            normalize_angle(goal.yaw),
538            0.01,
539            "fitted end yaw",
540        );
541    }
542
543    #[test]
544    fn test_g1_fit_coincident_points_error() {
545        let start = Pose2D::new(1.0, 2.0, 0.0);
546        let goal = Pose2D::new(1.0, 2.0, PI / 2.0);
547
548        let result = clothoid_g1_fit(&start, &goal);
549        assert!(result.is_err(), "coincident points should fail");
550    }
551
552    #[test]
553    fn test_g1_fit_symmetry() {
554        // Symmetric case: start at (0,0,0), end at (10,0,0)
555        // Should produce a straight line with near-zero curvature
556        let start = Pose2D::new(0.0, 0.0, 0.0);
557        let goal = Pose2D::new(10.0, 0.0, 0.0);
558
559        let (kappa0, kappa1, length) = clothoid_g1_fit(&start, &goal).unwrap();
560
561        assert_close(kappa0, 0.0, 1e-4, "symmetric kappa0");
562        assert_close(kappa1, 0.0, 1e-4, "symmetric kappa1");
563        assert!(length > 0.0, "length positive");
564    }
565
566    // -- Planner integration tests --------------------------------------------
567
568    #[test]
569    fn test_planner_basic() {
570        let planner = ClothoidPlanner::new(ClothoidConfig { n_points: 50 });
571        let start = Pose2D::new(0.0, 0.0, 0.0);
572        let goal = Pose2D::new(8.0, 4.0, PI / 3.0);
573
574        let result = planner.plan(&start, &goal);
575        assert!(result.is_ok(), "planner should succeed");
576
577        let clothoid = result.unwrap();
578        assert_eq!(clothoid.path.len(), 50);
579        assert_eq!(clothoid.yaw.len(), 50);
580        assert_eq!(clothoid.curvature.len(), 50);
581
582        // First point should match start
583        assert_close(clothoid.path.points[0].x, start.x, TOL, "start x");
584        assert_close(clothoid.path.points[0].y, start.y, TOL, "start y");
585
586        // Curvature should change linearly
587        let first_k = clothoid.curvature[0];
588        let last_k = *clothoid.curvature.last().unwrap();
589        let mid_k = clothoid.curvature[25];
590        let expected_mid = (first_k + last_k) / 2.0;
591        assert_close(mid_k, expected_mid, 0.01, "linear curvature at midpoint");
592    }
593
594    #[test]
595    fn test_planner_from_xy() {
596        let planner = ClothoidPlanner::new(ClothoidConfig::default());
597        let result = planner.plan_from_xy(0.0, 0.0, 0.0, 5.0, 0.0, 0.0);
598        assert!(result.is_ok());
599        assert_eq!(result.unwrap().path.len(), 100);
600    }
601
602    #[test]
603    fn test_curvature_linearity() {
604        let planner = ClothoidPlanner::new(ClothoidConfig { n_points: 101 });
605        let start = Pose2D::new(0.0, 0.0, 0.0);
606        let goal = Pose2D::new(6.0, 2.0, 0.5);
607
608        let clothoid = planner.plan(&start, &goal).unwrap();
609
610        // The curvature profile should be linear: kappa(s) = kappa0 + kappa1 * s
611        // Check that equally spaced samples form an arithmetic progression
612        let n = clothoid.curvature.len();
613        let step = (clothoid.curvature[n - 1] - clothoid.curvature[0]) / (n - 1) as f64;
614        for i in 0..n {
615            let expected = clothoid.curvature[0] + step * i as f64;
616            assert_close(
617                clothoid.curvature[i],
618                expected,
619                1e-10,
620                &format!("curvature linearity at {i}"),
621            );
622        }
623    }
624}