1use 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
30pub 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 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 let (c, s) = fresnel_series(t_abs);
55 if t < 0.0 {
56 (-c, -s)
57 } else {
58 (c, s)
59 }
60}
61
62fn 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; let mut sign = 1.0_f64;
72
73 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 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
94fn 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 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
112pub fn clothoid_eval(theta0: f64, kappa0: f64, kappa1: f64, s: f64) -> (f64, f64, f64) {
121 let n = 100; 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
147pub 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 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
190pub 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 let mut length = chord;
215
216 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 let (ex, ey, _etheta) = clothoid_eval(alpha, k0, k1, length);
229
230 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 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 let dtheta_dk0 = length;
256 let dtheta_dk1 = 0.5 * length * length;
257 let dtheta_dl = k0 + k1 * length;
258
259 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 if length < 1e-10 {
278 length = chord * 0.5;
279 }
280 }
281
282 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#[derive(Debug, Clone)]
298pub struct ClothoidPath {
299 pub start: Pose2D,
301 pub goal: Pose2D,
303 pub kappa0: f64,
305 pub kappa1: f64,
307 pub length: f64,
309 pub path: Path2D,
311 pub yaw: Vec<f64>,
313 pub curvature: Vec<f64>,
315}
316
317#[derive(Debug, Clone)]
319pub struct ClothoidConfig {
320 pub n_points: usize,
322}
323
324impl Default for ClothoidConfig {
325 fn default() -> Self {
326 Self { n_points: 100 }
327 }
328}
329
330#[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 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 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#[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 #[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 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 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 #[test]
439 fn test_clothoid_eval_straight_line() {
440 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 let radius = 10.0;
451 let kappa0 = 1.0 / radius;
452 let arc_length = FRAC_PI_2 * radius; let (x, y, theta) = clothoid_eval(0.0, kappa0, 0.0, arc_length);
455 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 let (x, y, theta) = clothoid_eval(0.0, 0.0, 0.1, 3.0);
465 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 #[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 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 #[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 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 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 #[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 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 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 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}