Skip to main content

rust_robotics_planning/
informed_rrt_star.rs

1#![allow(dead_code, clippy::needless_borrows_for_generic_args)]
2
3//! Informed RRT* path planning algorithm
4//!
5//! An extension of RRT* that uses ellipsoidal sampling to focus the search
6//! once an initial solution is found.
7
8use rand::Rng;
9use std::f64::consts::PI;
10
11use rust_robotics_core::{Path2D, Point2D, RoboticsError, RoboticsResult};
12
13#[derive(Clone, Debug)]
14pub struct Node {
15    pub x: f64,
16    pub y: f64,
17    pub cost: f64,
18    pub parent: Option<usize>,
19}
20
21impl Node {
22    pub fn new(x: f64, y: f64) -> Self {
23        Node {
24            x,
25            y,
26            cost: 0.0,
27            parent: None,
28        }
29    }
30}
31
32pub struct InformedRRTStar {
33    pub start: Node,
34    pub goal: Node,
35    pub min_rand: f64,
36    pub max_rand: f64,
37    pub expand_dis: f64,
38    pub goal_sample_rate: i32,
39    pub max_iter: usize,
40    pub obstacle_list: Vec<(f64, f64, f64)>,
41    pub node_list: Vec<Node>,
42}
43
44impl InformedRRTStar {
45    pub fn new(
46        start: (f64, f64),
47        goal: (f64, f64),
48        obstacle_list: Vec<(f64, f64, f64)>,
49        rand_area: (f64, f64),
50        expand_dis: f64,
51        goal_sample_rate: i32,
52        max_iter: usize,
53    ) -> Self {
54        InformedRRTStar {
55            start: Node::new(start.0, start.1),
56            goal: Node::new(goal.0, goal.1),
57            min_rand: rand_area.0,
58            max_rand: rand_area.1,
59            expand_dis,
60            goal_sample_rate,
61            max_iter,
62            obstacle_list,
63            node_list: Vec::new(),
64        }
65    }
66
67    pub fn planning(&mut self) -> Option<Vec<[f64; 2]>> {
68        self.planning_with_sampler(|planner, c_best, c_min, x_center, rotation_matrix| {
69            planner.informed_sample(c_best, c_min, x_center, rotation_matrix)
70        })
71    }
72
73    fn sampling_frame(&self) -> (f64, [f64; 2], [[f64; 2]; 2]) {
74        let c_min =
75            ((self.start.x - self.goal.x).powi(2) + (self.start.y - self.goal.y).powi(2)).sqrt();
76        let x_center = [
77            (self.start.x + self.goal.x) / 2.0,
78            (self.start.y + self.goal.y) / 2.0,
79        ];
80        // Start on the goal: any orientation of the (circular) region works.
81        let a1 = if c_min > 1e-12 {
82            [
83                (self.goal.x - self.start.x) / c_min,
84                (self.goal.y - self.start.y) / c_min,
85            ]
86        } else {
87            [1.0, 0.0]
88        };
89        let e_theta = a1[1].atan2(a1[0]);
90        let cos_theta = e_theta.cos();
91        let sin_theta = e_theta.sin();
92        let rotation_matrix = [[cos_theta, -sin_theta], [sin_theta, cos_theta]];
93
94        (c_min, x_center, rotation_matrix)
95    }
96
97    fn reset_search(&mut self) {
98        self.node_list = vec![self.start.clone()];
99    }
100
101    fn planning_with_sampler<F>(&mut self, mut sample_node: F) -> Option<Vec<[f64; 2]>>
102    where
103        F: FnMut(&InformedRRTStar, f64, f64, &[f64; 2], &[[f64; 2]; 2]) -> [f64; 2],
104    {
105        self.reset_search();
106        let mut c_best = f64::INFINITY;
107        let mut path = None;
108
109        let (c_min, x_center, rotation_matrix) = self.sampling_frame();
110
111        for _i in 0..self.max_iter {
112            let rnd = sample_node(self, c_best, c_min, &x_center, &rotation_matrix);
113            let n_ind = self.get_nearest_list_index(&rnd);
114            let nearest_node = &self.node_list[n_ind];
115
116            let theta = (rnd[1] - nearest_node.y).atan2(rnd[0] - nearest_node.x);
117            let new_node = self.get_new_node(theta, n_ind, nearest_node);
118            let d = self.line_cost(nearest_node, &new_node);
119
120            let no_collision = self.check_collision(nearest_node, theta, d);
121
122            if no_collision {
123                let near_inds = self.find_near_nodes(&new_node);
124                let new_node = self.choose_parent(new_node, &near_inds);
125
126                let new_node_index = self.node_list.len();
127                self.node_list.push(new_node);
128                self.rewire(new_node_index, &near_inds);
129
130                if self.is_near_goal(&self.node_list[new_node_index])
131                    && self.check_segment_collision(
132                        self.node_list[new_node_index].x,
133                        self.node_list[new_node_index].y,
134                        self.goal.x,
135                        self.goal.y,
136                    )
137                {
138                    let temp_path = self.get_final_course(new_node_index);
139                    let temp_path_len = self.get_path_len(&temp_path);
140                    if temp_path_len < c_best {
141                        path = Some(temp_path);
142                        c_best = temp_path_len;
143                    }
144                }
145            }
146        }
147
148        path
149    }
150
151    fn choose_parent(&self, mut new_node: Node, near_inds: &[usize]) -> Node {
152        if near_inds.is_empty() {
153            return new_node;
154        }
155
156        let mut d_list = Vec::new();
157        for &i in near_inds {
158            let dx = new_node.x - self.node_list[i].x;
159            let dy = new_node.y - self.node_list[i].y;
160            let d = (dx * dx + dy * dy).sqrt();
161            let theta = dy.atan2(dx);
162            if self.check_collision(&self.node_list[i], theta, d) {
163                d_list.push(self.node_list[i].cost + d);
164            } else {
165                d_list.push(f64::INFINITY);
166            }
167        }
168
169        let min_cost = d_list.iter().fold(f64::INFINITY, |a, &b| a.min(b));
170        if let Some(min_index) = d_list.iter().position(|&x| x == min_cost) {
171            if min_cost != f64::INFINITY {
172                new_node.cost = min_cost;
173                new_node.parent = Some(near_inds[min_index]);
174            }
175        }
176
177        new_node
178    }
179
180    fn find_near_nodes(&self, new_node: &Node) -> Vec<usize> {
181        let n_node = self.node_list.len();
182        let r = 50.0 * ((n_node as f64).ln() / n_node as f64).sqrt();
183        let mut near_inds = Vec::new();
184
185        for (i, node) in self.node_list.iter().enumerate() {
186            let d_sq = (node.x - new_node.x).powi(2) + (node.y - new_node.y).powi(2);
187            if d_sq <= r * r {
188                near_inds.push(i);
189            }
190        }
191
192        near_inds
193    }
194
195    fn informed_sample(
196        &self,
197        c_max: f64,
198        c_min: f64,
199        x_center: &[f64; 2],
200        rotation_matrix: &[[f64; 2]; 2],
201    ) -> [f64; 2] {
202        if c_max < f64::INFINITY {
203            let x_ball = self.sample_unit_ball();
204            self.informed_sample_from_unit_ball(c_max, c_min, x_center, rotation_matrix, x_ball)
205        } else {
206            self.sample_free_space()
207        }
208    }
209
210    fn informed_sample_from_unit_ball(
211        &self,
212        c_max: f64,
213        c_min: f64,
214        x_center: &[f64; 2],
215        rotation_matrix: &[[f64; 2]; 2],
216        x_ball: [f64; 2],
217    ) -> [f64; 2] {
218        // A path barely longer than c_min (rounding) must not give NaN.
219        let r = [
220            c_max / 2.0,
221            (c_max * c_max - c_min * c_min).max(0.0).sqrt() / 2.0,
222        ];
223        let scaled = [r[0] * x_ball[0], r[1] * x_ball[1]];
224        let rotated = [
225            rotation_matrix[0][0] * scaled[0] + rotation_matrix[0][1] * scaled[1],
226            rotation_matrix[1][0] * scaled[0] + rotation_matrix[1][1] * scaled[1],
227        ];
228
229        [rotated[0] + x_center[0], rotated[1] + x_center[1]]
230    }
231
232    fn sample_unit_ball(&self) -> [f64; 2] {
233        let mut rng = rand::rng();
234        let a: f64 = rng.random();
235        let b: f64 = rng.random();
236
237        Self::sample_unit_ball_from_uniforms(a, b)
238    }
239
240    fn sample_unit_ball_from_uniforms(a: f64, b: f64) -> [f64; 2] {
241        let (a, b) = if b < a { (b, a) } else { (a, b) };
242        let sample = (b * (2.0 * PI * a / b).cos(), b * (2.0 * PI * a / b).sin());
243        [sample.0, sample.1]
244    }
245
246    fn sample_free_space(&self) -> [f64; 2] {
247        let mut rng = rand::rng();
248        if rng.random_range(0..=100) > self.goal_sample_rate {
249            [
250                rng.random_range(self.min_rand..=self.max_rand),
251                rng.random_range(self.min_rand..=self.max_rand),
252            ]
253        } else {
254            [self.goal.x, self.goal.y]
255        }
256    }
257
258    fn get_path_len(&self, path: &[[f64; 2]]) -> f64 {
259        let mut path_len = 0.0;
260        for i in 1..path.len() {
261            let dx = path[i][0] - path[i - 1][0];
262            let dy = path[i][1] - path[i - 1][1];
263            path_len += (dx * dx + dy * dy).sqrt();
264        }
265        path_len
266    }
267
268    fn line_cost(&self, node1: &Node, node2: &Node) -> f64 {
269        ((node1.x - node2.x).powi(2) + (node1.y - node2.y).powi(2)).sqrt()
270    }
271
272    fn get_nearest_list_index(&self, rnd: &[f64; 2]) -> usize {
273        let mut min_dist = f64::INFINITY;
274        let mut min_index = 0;
275
276        for (i, node) in self.node_list.iter().enumerate() {
277            let dist = (node.x - rnd[0]).powi(2) + (node.y - rnd[1]).powi(2);
278            if dist < min_dist {
279                min_dist = dist;
280                min_index = i;
281            }
282        }
283
284        min_index
285    }
286
287    fn get_new_node(&self, theta: f64, n_ind: usize, nearest_node: &Node) -> Node {
288        let mut new_node = nearest_node.clone();
289        new_node.x += self.expand_dis * theta.cos();
290        new_node.y += self.expand_dis * theta.sin();
291        new_node.cost += self.expand_dis;
292        new_node.parent = Some(n_ind);
293        new_node
294    }
295
296    fn is_near_goal(&self, node: &Node) -> bool {
297        let d = self.line_cost(node, &self.goal);
298        d < self.expand_dis
299    }
300
301    fn rewire(&mut self, new_node_index: usize, near_inds: &[usize]) {
302        for &i in near_inds {
303            let near_node = &self.node_list[i];
304            let new_node = &self.node_list[new_node_index];
305
306            let d =
307                ((near_node.x - new_node.x).powi(2) + (near_node.y - new_node.y).powi(2)).sqrt();
308            let s_cost = new_node.cost + d;
309
310            if near_node.cost > s_cost {
311                let theta = (new_node.y - near_node.y).atan2(new_node.x - near_node.x);
312                if self.check_collision(near_node, theta, d) {
313                    self.node_list[i].parent = Some(new_node_index);
314                    self.node_list[i].cost = s_cost;
315                    self.propagate_cost_to_leaves(i);
316                }
317            }
318        }
319    }
320
321    /// After a rewire lowered `parent`'s cost, lowers its descendants' too
322    /// (otherwise later parent choices compare against stale costs).
323    fn propagate_cost_to_leaves(&mut self, parent: usize) {
324        let mut stack = vec![parent];
325        while let Some(p) = stack.pop() {
326            let (px, py, pcost) = {
327                let node = &self.node_list[p];
328                (node.x, node.y, node.cost)
329            };
330            for i in 0..self.node_list.len() {
331                if self.node_list[i].parent == Some(p) {
332                    let node = &mut self.node_list[i];
333                    node.cost = pcost + (node.x - px).hypot(node.y - py);
334                    stack.push(i);
335                }
336            }
337        }
338    }
339
340    fn distance_squared_point_to_segment(&self, v: [f64; 2], w: [f64; 2], p: [f64; 2]) -> f64 {
341        if v[0] == w[0] && v[1] == w[1] {
342            return (p[0] - v[0]).powi(2) + (p[1] - v[1]).powi(2);
343        }
344
345        let l2 = (w[0] - v[0]).powi(2) + (w[1] - v[1]).powi(2);
346        let t =
347            (((p[0] - v[0]) * (w[0] - v[0]) + (p[1] - v[1]) * (w[1] - v[1])) / l2).clamp(0.0, 1.0);
348        let projection = [v[0] + t * (w[0] - v[0]), v[1] + t * (w[1] - v[1])];
349        (p[0] - projection[0]).powi(2) + (p[1] - projection[1]).powi(2)
350    }
351
352    fn check_segment_collision(&self, x1: f64, y1: f64, x2: f64, y2: f64) -> bool {
353        // A non-finite segment is never free (NaN comparisons are all false
354        // and would otherwise read as "no collision").
355        if ![x1, y1, x2, y2].iter().all(|v| v.is_finite()) {
356            return false;
357        }
358        for &(ox, oy, size) in &self.obstacle_list {
359            let dd = self.distance_squared_point_to_segment([x1, y1], [x2, y2], [ox, oy]);
360            if dd <= size * size {
361                return false;
362            }
363        }
364        true
365    }
366
367    fn check_collision(&self, near_node: &Node, theta: f64, d: f64) -> bool {
368        let end_x = near_node.x + theta.cos() * d;
369        let end_y = near_node.y + theta.sin() * d;
370        self.check_segment_collision(near_node.x, near_node.y, end_x, end_y)
371    }
372
373    fn get_final_course(&self, last_index: usize) -> Vec<[f64; 2]> {
374        let mut path = vec![[self.goal.x, self.goal.y]];
375        let mut current_index = last_index;
376
377        while let Some(parent_index) = self.node_list[current_index].parent {
378            let node = &self.node_list[current_index];
379            path.push([node.x, node.y]);
380            current_index = parent_index;
381        }
382
383        path.push([self.start.x, self.start.y]);
384        path
385    }
386
387    /// Plan a path from the given start to goal, returning a [`Path2D`].
388    ///
389    /// This is a convenience wrapper around [`planning()`](Self::planning) that accepts
390    /// [`Point2D`], sets the start/goal, runs the planner, and returns [`Path2D`].
391    /// Requires `&mut self` because the underlying algorithm mutates internal state.
392    pub fn plan_from(&mut self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
393        self.start = Node::new(start.x, start.y);
394        self.goal = Node::new(goal.x, goal.y);
395
396        self.planning()
397            .map(|raw_path| {
398                Path2D::from_points(
399                    raw_path
400                        .into_iter()
401                        .rev()
402                        .map(|p| Point2D::new(p[0], p[1]))
403                        .collect(),
404                )
405            })
406            .ok_or_else(|| {
407                RoboticsError::PlanningError(
408                    "InformedRRT*: Cannot find path within max iterations".to_string(),
409                )
410            })
411    }
412
413    /// Get the tree nodes for external inspection
414    pub fn get_tree(&self) -> &[Node] {
415        &self.node_list
416    }
417
418    /// Get the obstacle list
419    pub fn get_obstacles(&self) -> &[(f64, f64, f64)] {
420        &self.obstacle_list
421    }
422}
423
424#[cfg(test)]
425mod tests {
426    use super::*;
427
428    #[test]
429    fn start_on_the_goal_adds_no_nan_nodes() {
430        let mut planner = InformedRRTStar::new(
431            (5.0, 5.0),
432            (5.0, 5.0),
433            vec![(8.0, 8.0, 1.0)],
434            (0.0, 10.0),
435            0.5,
436            10,
437            200,
438        );
439        let _ = planner.planning();
440        assert!(planner
441            .node_list
442            .iter()
443            .all(|n| n.x.is_finite() && n.y.is_finite() && n.cost.is_finite()));
444    }
445
446    #[test]
447    fn rewiring_keeps_descendant_costs_consistent() {
448        let mut planner = InformedRRTStar::new(
449            (0.0, 0.0),
450            (9.0, 9.0),
451            vec![(5.0, 5.0, 1.5), (3.0, 7.0, 1.0)],
452            (-1.0, 10.0),
453            1.0,
454            10,
455            600,
456        );
457        let _ = planner.planning();
458        // Every node's cost is its parent's cost plus the edge length.
459        for node in &planner.node_list {
460            if let Some(p) = node.parent {
461                let parent = &planner.node_list[p];
462                let expected = parent.cost + (node.x - parent.x).hypot(node.y - parent.y);
463                assert!(
464                    (node.cost - expected).abs() < 1e-6,
465                    "stale cost {} vs {expected}",
466                    node.cost
467                );
468            }
469        }
470    }
471
472    fn assert_close(actual: f64, expected: f64) {
473        assert!(
474            (actual - expected).abs() < 1.0e-12,
475            "expected {expected}, got {actual}"
476        );
477    }
478
479    fn parse_xy_fixture(csv: &str) -> Vec<[f64; 2]> {
480        csv.lines()
481            .skip(1)
482            .filter(|line| !line.trim().is_empty())
483            .map(|line| {
484                let (x, y) = line
485                    .split_once(',')
486                    .expect("xy fixture rows must contain a comma");
487                [x.parse().unwrap(), y.parse().unwrap()]
488            })
489            .collect()
490    }
491
492    fn create_pythonrobotics_main_planner() -> InformedRRTStar {
493        InformedRRTStar::new(
494            (0.0, 0.0),
495            (5.0, 10.0),
496            vec![
497                (5.0, 5.0, 0.5),
498                (9.0, 6.0, 1.0),
499                (7.0, 5.0, 1.0),
500                (1.0, 5.0, 1.0),
501                (3.0, 6.0, 1.0),
502                (7.0, 9.0, 1.0),
503            ],
504            (-2.0, 15.0),
505            0.5,
506            10,
507            200,
508        )
509    }
510
511    #[test]
512    fn test_informed_rrt_star_config() {
513        let rrt = InformedRRTStar::new(
514            (0.0, 0.0),
515            (5.0, 10.0),
516            vec![(5.0, 5.0, 0.5)],
517            (-2.0, 15.0),
518            0.5,
519            10,
520            300,
521        );
522        assert_eq!(rrt.expand_dis, 0.5);
523        assert_eq!(rrt.max_iter, 300);
524    }
525
526    #[test]
527    fn test_informed_rrt_star_sampling_frame_matches_upstream_reference() {
528        let rrt = create_pythonrobotics_main_planner();
529        let (c_min, x_center, rotation_matrix) = rrt.sampling_frame();
530
531        assert_close(c_min, 11.180_339_887_498_949);
532        assert_close(x_center[0], 2.5);
533        assert_close(x_center[1], 5.0);
534        assert_close(rotation_matrix[0][0], 0.447_213_595_499_958);
535        assert_close(rotation_matrix[0][1], -0.894_427_190_999_916);
536        assert_close(rotation_matrix[1][0], 0.894_427_190_999_916);
537        assert_close(rotation_matrix[1][1], 0.447_213_595_499_958);
538    }
539
540    #[test]
541    fn test_informed_rrt_star_sample_unit_ball_matches_upstream_reference() {
542        let expected = [
543            ((0.2, 0.8), [0.0, 0.8]),
544            ((0.1, 0.4), [0.0, 0.4]),
545            ((0.33, 0.9), [-0.602_217_545_722_972, 0.668_830_342_929_655]),
546        ];
547
548        for ((a, b), xy) in expected {
549            let sample = InformedRRTStar::sample_unit_ball_from_uniforms(a, b);
550            assert_close(sample[0], xy[0]);
551            assert_close(sample[1], xy[1]);
552        }
553    }
554
555    #[test]
556    fn test_informed_rrt_star_finite_ellipse_sampling_matches_upstream_reference() {
557        let rrt = create_pythonrobotics_main_planner();
558        let (c_min, x_center, rotation_matrix) = rrt.sampling_frame();
559        let unit_ball_samples = [
560            [0.0, 0.8],
561            [0.0, 0.4],
562            [-0.602_217_545_722_972, 0.668_830_342_929_655],
563        ];
564        let expected = [
565            (
566                12.0,
567                [
568                    [0.940_512_904_830_566, 5.779_743_547_584_717],
569                    [1.720_256_452_415_283, 5.389_871_773_792_358],
570                    [-0.419_709_604_196_265, 2.420_056_693_659_169],
571                ],
572            ),
573            (
574                14.0,
575                [
576                    [-0.514_630_989_026_684, 6.507_315_494_513_342],
577                    [0.992_684_505_486_658, 5.753_657_747_256_671],
578                    [-1.905_584_965_017_868, 2.489_694_689_330_144],
579                ],
580            ),
581            (
582                20.0,
583                [
584                    [-3.432_958_789_676_533, 7.966_479_394_838_267],
585                    [-0.466_479_394_838_267, 6.483_239_697_419_133],
586                    [-5.153_377_316_317_89, 2.093_691_810_760_673],
587                ],
588            ),
589        ];
590
591        for (c_best, xy_samples) in expected {
592            for (x_ball, xy) in unit_ball_samples.iter().zip(xy_samples.iter()) {
593                let sample = rrt.informed_sample_from_unit_ball(
594                    c_best,
595                    c_min,
596                    &x_center,
597                    &rotation_matrix,
598                    *x_ball,
599                );
600                assert_close(sample[0], xy[0]);
601                assert_close(sample[1], xy[1]);
602            }
603        }
604    }
605
606    #[test]
607    fn test_informed_rrt_star_plan_from_returns_start_to_goal_path() {
608        let mut rrt = InformedRRTStar::new(
609            (0.0, 0.0),
610            (5.0, 10.0),
611            vec![],
612            (-2.0, 15.0),
613            5.0_f64.hypot(10.0),
614            100,
615            1,
616        );
617
618        let path = rrt
619            .plan_from(Point2D::new(0.0, 0.0), Point2D::new(5.0, 10.0))
620            .unwrap();
621
622        assert_eq!(path.points.first().unwrap().x, 0.0);
623        assert_eq!(path.points.first().unwrap().y, 0.0);
624        assert_eq!(path.points.last().unwrap().x, 5.0);
625        assert_eq!(path.points.last().unwrap().y, 10.0);
626    }
627
628    #[test]
629    fn test_informed_rrt_star_upstream_seeded_main_prefix_matches_pythonrobotics_reference() {
630        let mut rrt = create_pythonrobotics_main_planner();
631        rrt.max_iter = 20;
632        let samples = [
633            [10.455_649_682_677_358, 11.942_970_283_541_907],
634            [12.537_351_811_401_535, 13.840_339_744_778_298],
635            [7.622_138_868_390_643, 0.748_693_006_799_259],
636            [12.860_963_734_685_752, 2.438_529_994_751_022],
637            [0.963_840_532_303_441, 7.404_758_454_678_607],
638            [10.548_525_179_081_42, 10.458_784_524_738_785],
639            [14.636_879_924_696_97, 5.006_029_679_968_117],
640            [0.831_739_168_751_872, 1.497_141_169_178_794],
641            [5.0, 10.0],
642            [12.230_194_783_259_385, 3.474_659_848_212_157],
643            [3.771_802_130_764_652, 14.447_201_800_957_814],
644            [10.657_010_682_397_589, -1.941_271_617_163_375],
645            [12.803_038_468_843_097, 11.104_183_754_785_783],
646            [2.871_774_581_756_395, 9.093_185_216_538_995],
647            [13.054_119_941_473_152, 7.827_462_465_227_494],
648            [4.375_040_433_962_334, 14.322_895_906_129_173],
649            [10.059_569_437_230_238, 12.022_177_097_262_272],
650            [5.0, 10.0],
651            [0.036_638_892_026_463, 6.486_823_094_475_769],
652            [11.632_287_039_256_342, 7.436_450_081_952_417],
653        ];
654        let mut sample_index = 0_usize;
655
656        let path = rrt.planning_with_sampler(|_, _, _, _, _| {
657            let sample = *samples
658                .get(sample_index)
659                .expect("python reference sample sequence exhausted");
660            sample_index += 1;
661            sample
662        });
663
664        assert!(path.is_none());
665        assert_eq!(sample_index, samples.len());
666        assert_eq!(rrt.node_list.len(), 20);
667
668        let expected_nodes = [
669            (
670                1,
671                [0.329_351_320_180_549, 0.376_201_685_130_901],
672                0.5,
673                Some(0),
674            ),
675            (
676                2,
677                [0.665_203_546_734_372, 0.746_611_298_827_467],
678                0.999_962_094_343_993,
679                Some(0),
680            ),
681            (
682                5,
683                [1.607_494_092_207_533, 1.315_569_836_427_121],
684                2.077_200_339_639_631,
685                Some(0),
686            ),
687            (
688                10,
689                [3.085_807_934_983_019, 2.339_074_098_076_378],
690                3.872_141_300_094_301,
691                Some(0),
692            ),
693            (
694                15,
695                [3.900_997_195_468_872, 3.858_949_080_306_712],
696                5.487_191_187_069_758,
697                Some(0),
698            ),
699        ];
700
701        for (index, xy, cost, parent) in expected_nodes {
702            let node = &rrt.node_list[index];
703            assert_close(node.x, xy[0]);
704            assert_close(node.y, xy[1]);
705            assert_close(node.cost, cost);
706            assert_eq!(node.parent, parent);
707        }
708    }
709
710    #[test]
711    fn test_informed_rrt_star_upstream_seeded_main_matches_pythonrobotics_reference() {
712        let mut rrt = create_pythonrobotics_main_planner();
713        let samples = parse_xy_fixture(include_str!(
714            "testdata/informed_rrt_star_main_seed12345_samples.csv"
715        ));
716        let expected_path = parse_xy_fixture(include_str!(
717            "testdata/informed_rrt_star_main_seed12345_path.csv"
718        ));
719        let mut sample_index = 0_usize;
720
721        let path = rrt
722            .planning_with_sampler(|_, _, _, _, _| {
723                let sample = *samples
724                    .get(sample_index)
725                    .expect("python reference sample sequence exhausted");
726                sample_index += 1;
727                sample
728            })
729            .expect("python reference run should find a path");
730
731        assert_eq!(sample_index, samples.len());
732        assert_eq!(rrt.node_list.len(), 158);
733        assert_eq!(path.len(), expected_path.len());
734        for (actual, expected) in path.iter().zip(expected_path.iter()) {
735            assert_close(actual[0], expected[0]);
736            assert_close(actual[1], expected[1]);
737        }
738
739        let expected_nodes = [
740            (
741                1,
742                [0.329_351_320_180_549, 0.376_201_685_130_901],
743                0.5,
744                Some(0),
745            ),
746            (
747                2,
748                [0.665_203_546_734_372, 0.746_611_298_827_467],
749                0.999_962_094_343_993,
750                Some(0),
751            ),
752            (
753                5,
754                [1.607_494_092_207_533, 1.315_569_836_427_121],
755                2.077_200_339_639_631,
756                Some(0),
757            ),
758            (
759                10,
760                [3.085_807_934_983_019, 2.339_074_098_076_378],
761                3.872_141_300_094_301,
762                Some(0),
763            ),
764            (
765                20,
766                [2.839_225_131_469_749, 4.429_224_288_562_779],
767                5.261_105_125_880_291,
768                Some(0),
769            ),
770            (
771                40,
772                [7.308_140_070_034_553, 7.411_628_497_256_421],
773                10.487_807_493_996_291,
774                Some(18),
775            ),
776            (
777                80,
778                [2.374_311_082_825_791, 4.245_227_203_331_836],
779                4.864_083_379_829_957,
780                Some(0),
781            ),
782            (
783                120,
784                [1.995_074_025_995_299, 5.624_254_528_604_578],
785                6.049_311_878_312_089,
786                Some(77),
787            ),
788            (
789                157,
790                [1.654_832_682_319_505, 3.738_064_093_591_951],
791                4.087_981_699_356_565,
792                Some(0),
793            ),
794        ];
795
796        for (index, xy, cost, parent) in expected_nodes {
797            let node = &rrt.node_list[index];
798            assert_close(node.x, xy[0]);
799            assert_close(node.y, xy[1]);
800            assert_close(node.cost, cost);
801            assert_eq!(node.parent, parent);
802        }
803    }
804}