Skip to main content

rust_robotics_planning/
rrt_star.rs

1#![allow(dead_code, clippy::too_many_arguments)]
2
3//! RRT* (Rapidly-exploring Random Tree Star) path planning algorithm
4//!
5//! An optimized version of RRT that rewires the tree to find shorter paths.
6
7use rand::{rngs::StdRng, Rng, SeedableRng};
8
9use rust_robotics_core::{Path2D, PathPlanner, Point2D, RoboticsError, RoboticsResult};
10
11/// Internal node for RRT* tree
12#[derive(Debug, Clone)]
13pub struct Node {
14    pub x: f64,
15    pub y: f64,
16    pub path_x: Vec<f64>,
17    pub path_y: Vec<f64>,
18    pub cost: f64,
19    pub parent: Option<usize>,
20}
21
22impl Node {
23    pub fn new(x: f64, y: f64) -> Self {
24        Node {
25            x,
26            y,
27            path_x: Vec::new(),
28            path_y: Vec::new(),
29            cost: 0.0,
30            parent: None,
31        }
32    }
33}
34
35/// Lightweight node state for an actual RRT* search history.
36#[derive(Debug, Clone, PartialEq)]
37pub struct RrtStarTraceNode {
38    pub point: [f64; 2],
39    pub parent: Option<usize>,
40    pub cost: f64,
41}
42
43impl From<&Node> for RrtStarTraceNode {
44    fn from(node: &Node) -> Self {
45        Self {
46            point: [node.x, node.y],
47            parent: node.parent,
48            cost: node.cost,
49        }
50    }
51}
52
53/// A node insertion, reparenting, or propagated cost update in one iteration.
54#[derive(Debug, Clone, PartialEq)]
55pub struct RrtStarNodeUpdate {
56    pub index: usize,
57    pub before: Option<RrtStarTraceNode>,
58    pub after: RrtStarTraceNode,
59}
60
61/// State observed after one real planner iteration, including rejected samples.
62#[derive(Debug, Clone, PartialEq)]
63pub struct RrtStarSearchStep {
64    pub sample: [f64; 2],
65    pub nearest: usize,
66    pub candidate: Option<[f64; 2]>,
67    pub accepted: bool,
68    pub updates: Vec<RrtStarNodeUpdate>,
69    /// Current cheapest collision-free goal connection, in goal-to-start order.
70    pub path: Vec<[f64; 2]>,
71}
72
73/// A deterministic run recorded as node changes, rather than final-tree reveals.
74#[derive(Debug, Clone, PartialEq)]
75pub struct RrtStarSearchTrace {
76    pub root: RrtStarTraceNode,
77    pub steps: Vec<RrtStarSearchStep>,
78    pub path: Option<Vec<[f64; 2]>>,
79}
80
81impl RrtStarSearchTrace {
82    /// Reconstruct the tree after this many completed iterations. Zero is the root.
83    pub fn tree_at(&self, completed_iterations: usize) -> Vec<RrtStarTraceNode> {
84        let mut nodes = vec![self.root.clone()];
85        for step in self.steps.iter().take(completed_iterations) {
86            for update in &step.updates {
87                if update.index == nodes.len() {
88                    nodes.push(update.after.clone());
89                } else {
90                    nodes[update.index] = update.after.clone();
91                }
92            }
93        }
94        nodes
95    }
96}
97
98#[derive(Debug, Clone)]
99pub struct RRTStar {
100    pub start: Node,
101    pub end: Node,
102    pub min_rand: f64,
103    pub max_rand: f64,
104    pub expand_dis: f64,
105    pub path_resolution: f64,
106    pub goal_sample_rate: i32,
107    pub max_iter: i32,
108    pub connect_circle_dist: f64,
109    pub search_until_max_iter: bool,
110    pub robot_radius: f64,
111    pub obstacle_list: Vec<(f64, f64, f64)>, // (x, y, radius)
112    pub node_list: Vec<Node>,
113}
114
115impl RRTStar {
116    pub fn new(
117        start: (f64, f64),
118        goal: (f64, f64),
119        obstacle_list: Vec<(f64, f64, f64)>,
120        rand_area: (f64, f64),
121        expand_dis: f64,
122        path_resolution: f64,
123        goal_sample_rate: i32,
124        max_iter: i32,
125        connect_circle_dist: f64,
126        search_until_max_iter: bool,
127        robot_radius: f64,
128    ) -> Self {
129        RRTStar {
130            start: Node::new(start.0, start.1),
131            end: Node::new(goal.0, goal.1),
132            min_rand: rand_area.0,
133            max_rand: rand_area.1,
134            expand_dis,
135            path_resolution,
136            goal_sample_rate,
137            max_iter,
138            connect_circle_dist,
139            search_until_max_iter,
140            robot_radius,
141            obstacle_list,
142            node_list: Vec::new(),
143        }
144    }
145
146    pub fn planning(&mut self) -> Option<Vec<[f64; 2]>> {
147        self.planning_with_sampler(|planner| planner.get_random_node())
148    }
149
150    /// Run the same search with a seeded sampler and record each iteration.
151    /// Recording does not alter parent selection, rewiring, or the stop condition.
152    /// Memory stores only changed lightweight nodes and the current solution path.
153    pub fn planning_with_trace(&mut self, seed: u64) -> RrtStarSearchTrace {
154        let root = RrtStarTraceNode::from(&self.start);
155        let mut previous = vec![root.clone()];
156        let mut steps = Vec::new();
157        let mut rng = StdRng::seed_from_u64(seed);
158        let path = self.planning_with_observer(
159            |planner| planner.get_random_node_with_rng(&mut rng),
160            |planner, sample, nearest, candidate| {
161                let nodes: Vec<_> = planner
162                    .node_list
163                    .iter()
164                    .map(RrtStarTraceNode::from)
165                    .collect();
166                let accepted = nodes.len() > previous.len();
167                let updates = nodes
168                    .iter()
169                    .enumerate()
170                    .filter_map(|(index, node)| {
171                        let before = previous.get(index);
172                        (before != Some(node)).then(|| RrtStarNodeUpdate {
173                            index,
174                            before: before.cloned(),
175                            after: node.clone(),
176                        })
177                    })
178                    .collect();
179                let path = planner
180                    .search_best_goal_node()
181                    .map(|i| planner.generate_final_course(i))
182                    .unwrap_or_default();
183                steps.push(RrtStarSearchStep {
184                    sample,
185                    nearest,
186                    candidate,
187                    accepted,
188                    updates,
189                    path,
190                });
191                previous = nodes;
192            },
193        );
194        RrtStarSearchTrace { root, steps, path }
195    }
196
197    fn reset_search(&mut self) {
198        self.node_list = vec![self.start.clone()];
199    }
200
201    fn planning_with_sampler<F>(&mut self, mut sample_node: F) -> Option<Vec<[f64; 2]>>
202    where
203        F: FnMut(&RRTStar) -> Node,
204    {
205        self.planning_with_observer(&mut sample_node, |_, _, _, _| {})
206    }
207
208    fn planning_with_observer<F, O>(
209        &mut self,
210        mut sample_node: F,
211        mut observe: O,
212    ) -> Option<Vec<[f64; 2]>>
213    where
214        F: FnMut(&RRTStar) -> Node,
215        O: FnMut(&RRTStar, [f64; 2], usize, Option<[f64; 2]>),
216    {
217        self.reset_search();
218
219        for _i in 0..self.max_iter {
220            let rnd_node = sample_node(self);
221            let nearest_ind = self.get_nearest_node_index(&rnd_node);
222            let mut new_node = self.steer(nearest_ind, &rnd_node);
223            let candidate = new_node.as_ref().map(|n| [n.x, n.y]);
224
225            if let Some(ref mut node) = new_node {
226                let near_node = &self.node_list[nearest_ind];
227                node.cost = near_node.cost + self.calc_distance(near_node, node);
228
229                if self.check_collision_free(node) {
230                    let near_inds = self.find_near_nodes(node);
231                    let node_with_updated_parent = self.choose_parent(node.clone(), &near_inds);
232
233                    if let Some(updated_node) = node_with_updated_parent {
234                        let new_index = self.node_list.len();
235                        self.node_list.push(updated_node);
236                        self.rewire(new_index, &near_inds);
237                    } else {
238                        self.node_list.push(node.clone());
239                    }
240                }
241            }
242
243            observe(self, [rnd_node.x, rnd_node.y], nearest_ind, candidate);
244            if !self.search_until_max_iter && new_node.is_some() {
245                if let Some(last_index) = self.search_best_goal_node() {
246                    return Some(self.generate_final_course(last_index));
247                }
248            }
249        }
250
251        if let Some(last_index) = self.search_best_goal_node() {
252            return Some(self.generate_final_course(last_index));
253        }
254
255        None
256    }
257
258    fn get_random_node(&self) -> Node {
259        let mut rng = rand::rng();
260        self.get_random_node_with_rng(&mut rng)
261    }
262
263    fn get_random_node_with_rng(&self, rng: &mut impl Rng) -> Node {
264        if rng.random_range(0..=100) > self.goal_sample_rate {
265            Node::new(
266                rng.random_range(self.min_rand..=self.max_rand),
267                rng.random_range(self.min_rand..=self.max_rand),
268            )
269        } else {
270            Node::new(self.end.x, self.end.y)
271        }
272    }
273
274    fn get_nearest_node_index(&self, rnd_node: &Node) -> usize {
275        let mut min_dist = f64::INFINITY;
276        let mut nearest_ind = 0;
277
278        for (i, node) in self.node_list.iter().enumerate() {
279            let dist = self.calc_distance(node, rnd_node);
280            if dist < min_dist {
281                min_dist = dist;
282                nearest_ind = i;
283            }
284        }
285
286        nearest_ind
287    }
288
289    fn steer(&self, from_ind: usize, to_node: &Node) -> Option<Node> {
290        let from_node = &self.node_list[from_ind];
291        Some(self.steer_from_node(from_node, to_node, self.expand_dis, Some(from_ind)))
292    }
293
294    fn steer_from_node(
295        &self,
296        from_node: &Node,
297        to_node: &Node,
298        extend_length: f64,
299        parent: Option<usize>,
300    ) -> Node {
301        let mut new_node = Node::new(from_node.x, from_node.y);
302        let (dist, theta) = self.calc_distance_and_angle(&new_node, to_node);
303        let extend_length = extend_length.min(dist);
304
305        new_node.path_x = vec![new_node.x];
306        new_node.path_y = vec![new_node.y];
307
308        let n_expand = (extend_length / self.path_resolution).floor() as i32;
309        for _ in 0..n_expand {
310            new_node.x += self.path_resolution * theta.cos();
311            new_node.y += self.path_resolution * theta.sin();
312            new_node.path_x.push(new_node.x);
313            new_node.path_y.push(new_node.y);
314        }
315
316        let (remaining_dist, _) = self.calc_distance_and_angle(&new_node, to_node);
317        if remaining_dist <= self.path_resolution {
318            new_node.path_x.push(to_node.x);
319            new_node.path_y.push(to_node.y);
320            new_node.x = to_node.x;
321            new_node.y = to_node.y;
322        }
323
324        new_node.parent = parent;
325        new_node
326    }
327
328    fn check_collision_free(&self, node: &Node) -> bool {
329        if node.path_x.is_empty() || node.path_y.is_empty() {
330            return true;
331        }
332
333        // Every segment between the path points, not only the points.
334        !self.obstacle_list.iter().any(|&(ox, oy, size)| {
335            crate::rrt::polyline_within(
336                &node.path_x,
337                &node.path_y,
338                [ox, oy],
339                size + self.robot_radius,
340            )
341        })
342    }
343
344    fn find_near_nodes(&self, new_node: &Node) -> Vec<usize> {
345        let nnode = self.node_list.len() + 1;
346        let r = self.connect_circle_dist * ((nnode as f64).ln() / nnode as f64).sqrt();
347        let r = r.min(self.expand_dis);
348
349        self.node_list
350            .iter()
351            .enumerate()
352            .filter_map(|(i, node)| {
353                let dist_sq = (node.x - new_node.x).powi(2) + (node.y - new_node.y).powi(2);
354                if dist_sq <= r.powi(2) {
355                    Some(i)
356                } else {
357                    None
358                }
359            })
360            .collect()
361    }
362
363    fn choose_parent(&self, new_node: Node, near_inds: &[usize]) -> Option<Node> {
364        if near_inds.is_empty() {
365            return None;
366        }
367
368        let mut costs = Vec::new();
369        for &i in near_inds {
370            let near_node = &self.node_list[i];
371            let t_node = self.steer_from_node(near_node, &new_node, f64::INFINITY, Some(i));
372
373            if self.check_collision_free(&t_node) {
374                costs.push(self.calc_new_cost(near_node, &new_node));
375            } else {
376                costs.push(f64::INFINITY);
377            }
378        }
379
380        let min_cost = costs.iter().fold(f64::INFINITY, |a, &b| a.min(b));
381
382        if min_cost == f64::INFINITY {
383            return None;
384        }
385
386        let min_ind = costs.iter().position(|&x| x == min_cost)?;
387        let parent_ind = near_inds[min_ind];
388
389        let mut result_node = self.steer_from_node(
390            &self.node_list[parent_ind],
391            &new_node,
392            f64::INFINITY,
393            Some(parent_ind),
394        );
395        result_node.cost = min_cost;
396
397        Some(result_node)
398    }
399
400    fn search_best_goal_node(&self) -> Option<usize> {
401        let dist_to_goal_list: Vec<f64> = self
402            .node_list
403            .iter()
404            .map(|n| self.calc_dist_to_goal(n.x, n.y))
405            .collect();
406
407        let goal_inds: Vec<usize> = dist_to_goal_list
408            .iter()
409            .enumerate()
410            .filter_map(|(i, &dist)| {
411                if dist <= self.expand_dis {
412                    Some(i)
413                } else {
414                    None
415                }
416            })
417            .collect();
418
419        let safe_goal_inds: Vec<usize> = goal_inds
420            .into_iter()
421            .filter(|&goal_ind| {
422                let t_node = self.steer_from_node(
423                    &self.node_list[goal_ind],
424                    &self.end,
425                    f64::INFINITY,
426                    Some(goal_ind),
427                );
428                self.check_collision_free(&t_node)
429            })
430            .collect();
431
432        if safe_goal_inds.is_empty() {
433            return None;
434        }
435
436        let safe_goal_costs: Vec<f64> = safe_goal_inds
437            .iter()
438            .map(|&i| {
439                self.node_list[i].cost
440                    + self.calc_dist_to_goal(self.node_list[i].x, self.node_list[i].y)
441            })
442            .collect();
443
444        let min_cost = safe_goal_costs.iter().fold(f64::INFINITY, |a, &b| a.min(b));
445
446        safe_goal_inds
447            .into_iter()
448            .zip(safe_goal_costs)
449            .find(|(_, cost)| *cost == min_cost)
450            .map(|(i, _)| i)
451    }
452
453    fn rewire(&mut self, new_node_ind: usize, near_inds: &[usize]) {
454        for &i in near_inds {
455            let near_node = self.node_list[i].clone();
456            let new_node = &self.node_list[new_node_ind];
457
458            let mut edge_node =
459                self.steer_from_node(new_node, &near_node, f64::INFINITY, Some(new_node_ind));
460            edge_node.cost = self.calc_new_cost(new_node, &near_node);
461
462            let no_collision = self.check_collision_free(&edge_node);
463            let improved_cost = near_node.cost > edge_node.cost;
464
465            if no_collision && improved_cost {
466                self.node_list[i] = edge_node;
467                self.propagate_cost_to_leaves(i);
468            }
469        }
470    }
471
472    fn calc_new_cost(&self, from_node: &Node, to_node: &Node) -> f64 {
473        from_node.cost + self.calc_distance(from_node, to_node)
474    }
475
476    fn propagate_cost_to_leaves(&mut self, parent_ind: usize) {
477        let parent_node = self.node_list[parent_ind].clone();
478
479        for i in 0..self.node_list.len() {
480            if let Some(node_parent) = self.node_list[i].parent {
481                if node_parent == parent_ind {
482                    self.node_list[i].cost =
483                        self.calc_new_cost(&parent_node, &self.node_list[i].clone());
484                    self.propagate_cost_to_leaves(i);
485                }
486            }
487        }
488    }
489
490    fn generate_final_course(&self, goal_ind: usize) -> Vec<[f64; 2]> {
491        let mut path = vec![[self.end.x, self.end.y]];
492        let mut node = &self.node_list[goal_ind];
493
494        while let Some(parent_ind) = node.parent {
495            path.push([node.x, node.y]);
496            node = &self.node_list[parent_ind];
497        }
498        path.push([node.x, node.y]);
499
500        path
501    }
502
503    fn calc_dist_to_goal(&self, x: f64, y: f64) -> f64 {
504        let dx = x - self.end.x;
505        let dy = y - self.end.y;
506        (dx * dx + dy * dy).sqrt()
507    }
508
509    fn calc_distance(&self, from_node: &Node, to_node: &Node) -> f64 {
510        let dx = to_node.x - from_node.x;
511        let dy = to_node.y - from_node.y;
512        (dx * dx + dy * dy).sqrt()
513    }
514
515    fn calc_distance_and_angle(&self, from_node: &Node, to_node: &Node) -> (f64, f64) {
516        let dx = to_node.x - from_node.x;
517        let dy = to_node.y - from_node.y;
518        let d = (dx * dx + dy * dy).sqrt();
519        let theta = dy.atan2(dx);
520        (d, theta)
521    }
522
523    /// Plan a path from the given start to goal, returning a [`Path2D`].
524    ///
525    /// This is a convenience wrapper around [`planning()`](Self::planning) that accepts
526    /// [`Point2D`], sets the start/goal, runs the planner, and returns [`Path2D`].
527    /// Requires `&mut self` because the underlying algorithm mutates internal state.
528    pub fn plan_from(&mut self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
529        self.start = Node::new(start.x, start.y);
530        self.end = Node::new(goal.x, goal.y);
531
532        self.planning()
533            .map(|raw_path| {
534                Path2D::from_points(
535                    raw_path
536                        .into_iter()
537                        .rev()
538                        .map(|p| Point2D::new(p[0], p[1]))
539                        .collect(),
540                )
541            })
542            .ok_or_else(|| {
543                RoboticsError::PlanningError(
544                    "RRT*: Cannot find path within max iterations".to_string(),
545                )
546            })
547    }
548
549    /// Get the tree nodes for external inspection
550    pub fn get_tree(&self) -> &[Node] {
551        &self.node_list
552    }
553
554    /// Get the obstacle list
555    pub fn get_obstacles(&self) -> &[(f64, f64, f64)] {
556        &self.obstacle_list
557    }
558}
559
560impl PathPlanner for RRTStar {
561    /// Plans on a copy of this planner, so `&self` keeps its own tree; use
562    /// [`RRTStar::plan_from`] to keep the tree for inspection.
563    fn plan(&self, start: Point2D, goal: Point2D) -> Result<Path2D, RoboticsError> {
564        self.clone().plan_from(start, goal)
565    }
566}
567
568#[cfg(test)]
569mod tests {
570    use super::*;
571
572    fn assert_close(actual: f64, expected: f64) {
573        assert!(
574            (actual - expected).abs() < 1.0e-12,
575            "expected {expected}, got {actual}"
576        );
577    }
578
579    fn parse_xy_fixture(csv: &str) -> Vec<[f64; 2]> {
580        csv.lines()
581            .skip(1)
582            .filter(|line| !line.trim().is_empty())
583            .map(|line| {
584                let (x, y) = line
585                    .split_once(',')
586                    .expect("xy fixture rows must contain a comma");
587                [x.parse().unwrap(), y.parse().unwrap()]
588            })
589            .collect()
590    }
591
592    fn create_pythonrobotics_main_planner() -> RRTStar {
593        RRTStar::new(
594            (0.0, 0.0),
595            (6.0, 10.0),
596            vec![
597                (5.0, 5.0, 1.0),
598                (3.0, 6.0, 2.0),
599                (3.0, 8.0, 2.0),
600                (3.0, 10.0, 2.0),
601                (7.0, 5.0, 2.0),
602                (9.0, 5.0, 2.0),
603                (8.0, 10.0, 1.0),
604                (6.0, 12.0, 1.0),
605            ],
606            (-2.0, 15.0),
607            1.0,
608            1.0,
609            20,
610            300,
611            50.0,
612            false,
613            0.8,
614        )
615    }
616
617    #[test]
618    fn test_rrt_star_config() {
619        let rrt = RRTStar::new(
620            (0.0, 0.0),
621            (6.0, 10.0),
622            vec![(5.0, 5.0, 1.0)],
623            (-2.0, 15.0),
624            2.0,
625            0.5,
626            20,
627            500,
628            50.0,
629            false,
630            0.3,
631        );
632        assert_eq!(rrt.expand_dis, 2.0);
633        assert_eq!(rrt.max_iter, 500);
634    }
635
636    #[test]
637    fn test_rrt_star_upstream_no_obstacle_seeded_reference() {
638        for robot_radius in [0.0, 0.8] {
639            let mut rrt = RRTStar::new(
640                (0.0, 0.0),
641                (6.0, 10.0),
642                vec![],
643                (-2.0, 15.0),
644                30.0,
645                1.0,
646                20,
647                300,
648                50.0,
649                false,
650                robot_radius,
651            );
652            let sample = [10.455_649_682_677_358, 11.942_970_283_541_907];
653            let path = rrt
654                .planning_with_sampler(|_| Node::new(sample[0], sample[1]))
655                .unwrap();
656            assert_eq!(rrt.node_list.len(), 2);
657            assert_eq!(path, vec![[6.0, 10.0], [0.0, 0.0]]);
658            assert_close(rrt.node_list[1].x, sample[0]);
659            assert_close(rrt.node_list[1].y, sample[1]);
660            assert_close(rrt.node_list[1].cost, 15.873_095_144_943_73);
661            assert_eq!(rrt.node_list[1].parent, Some(0));
662        }
663    }
664
665    #[test]
666    fn test_rrt_star_upstream_seeded_main_prefix_matches_pythonrobotics_reference() {
667        let mut rrt = create_pythonrobotics_main_planner();
668        rrt.max_iter = 20;
669        let samples = parse_xy_fixture(include_str!("testdata/rrt_star_main_seed10_samples.csv"));
670        let mut sample_index = 0_usize;
671        let prefix_len = 20_usize;
672
673        let path = rrt.planning_with_sampler(|_| {
674            let sample = samples
675                .get(sample_index)
676                .filter(|_| sample_index < prefix_len)
677                .expect("python reference sample sequence exhausted");
678            sample_index += 1;
679            Node::new(sample[0], sample[1])
680        });
681
682        assert!(path.is_none());
683        assert_eq!(sample_index, prefix_len);
684        assert_eq!(rrt.node_list.len(), 14);
685
686        let expected_nodes = [
687            (
688                1,
689                [-0.227_015_105_864_128, 0.973_891_237_104_79],
690                1.0,
691                Some(0),
692            ),
693            (
694                2,
695                [0.340_848_395_898_016, 1.797_013_976_048_647],
696                2.0,
697                Some(1),
698            ),
699            (
700                5,
701                [2.912_922_340_312_151, 1.655_751_229_702_901],
702                5.0,
703                Some(4),
704            ),
705            (
706                10,
707                [5.856_411_905_724_674, 1.320_164_679_038_256],
708                8.0,
709                Some(9),
710            ),
711            (
712                13,
713                [8.543_112_538_843_18, 0.823_740_039_534_573],
714                11.0,
715                Some(12),
716            ),
717        ];
718        for (index, xy, cost, parent) in expected_nodes {
719            let node = &rrt.node_list[index];
720            assert_close(node.x, xy[0]);
721            assert_close(node.y, xy[1]);
722            assert_close(node.cost, cost);
723            assert_eq!(node.parent, parent);
724        }
725    }
726
727    #[test]
728    fn test_rrt_star_upstream_seeded_main_matches_pythonrobotics_reference() {
729        let mut rrt = create_pythonrobotics_main_planner();
730        let samples = parse_xy_fixture(include_str!("testdata/rrt_star_main_seed10_samples.csv"));
731        let expected_path =
732            parse_xy_fixture(include_str!("testdata/rrt_star_main_seed10_path.csv"));
733        let mut sample_index = 0_usize;
734
735        let path = rrt
736            .planning_with_sampler(|_| {
737                let sample = samples
738                    .get(sample_index)
739                    .expect("python reference sample sequence exhausted");
740                sample_index += 1;
741                Node::new(sample[0], sample[1])
742            })
743            .expect("python reference run should find a path");
744
745        assert_eq!(sample_index, samples.len());
746        assert_eq!(rrt.node_list.len(), 100);
747        assert_eq!(path.len(), expected_path.len());
748        for (actual, expected) in path.iter().zip(expected_path.iter()) {
749            assert_close(actual[0], expected[0]);
750            assert_close(actual[1], expected[1]);
751        }
752
753        let expected_nodes = [
754            (
755                1,
756                [-0.227_015_105_864_128, 0.973_891_237_104_79],
757                1.0,
758                Some(0),
759            ),
760            (
761                2,
762                [0.340_848_395_898_016, 1.797_013_976_048_647],
763                2.0,
764                Some(1),
765            ),
766            (
767                5,
768                [2.912_922_340_312_151, 1.655_751_229_702_901],
769                5.0,
770                Some(4),
771            ),
772            (
773                10,
774                [5.856_411_905_724_674, 1.320_164_679_038_256],
775                8.0,
776                Some(9),
777            ),
778            (
779                20,
780                [12.105_226_205_468_63, 1.607_428_066_363_632],
781                14.812_039_643_502_144,
782                Some(19),
783            ),
784            (
785                40,
786                [13.266_098_354_827_152, 11.032_918_978_213_733],
787                25.673_392_954_630_07,
788                Some(39),
789            ),
790            (
791                60,
792                [8.777_150_456_317_27, 12.593_447_860_337_104],
793                31.673_392_954_630_07,
794                Some(53),
795            ),
796            (
797                80,
798                [10.550_895_454_349_991, 1.108_429_868_595_935],
799                13.203_336_815_700_968,
800                Some(17),
801            ),
802            (99, [6.0, 10.0], 28.741_122_081_549_424, Some(97)),
803        ];
804
805        for (index, xy, cost, parent) in expected_nodes {
806            let node = &rrt.node_list[index];
807            assert_close(node.x, xy[0]);
808            assert_close(node.y, xy[1]);
809            assert_close(node.cost, cost);
810            assert_eq!(node.parent, parent);
811        }
812    }
813
814    fn trace_fixture() -> RRTStar {
815        RRTStar::new(
816            (1.5, 1.5),
817            (18.5, 18.5),
818            vec![(10.0, 10.0, 2.5)],
819            (0.0, 20.0),
820            1.5,
821            0.25,
822            10,
823            500,
824            15.0,
825            true,
826            0.3,
827        )
828    }
829
830    #[test]
831    fn recorded_search_matches_the_same_unobserved_seeded_run() {
832        let mut recorded = trace_fixture();
833        let trace = recorded.planning_with_trace(7);
834        let mut plain = trace_fixture();
835        let mut rng = StdRng::seed_from_u64(7);
836        let path = plain.planning_with_sampler(|p| p.get_random_node_with_rng(&mut rng));
837        assert_eq!(trace.path, path);
838        assert!(path.is_some());
839        assert_eq!(
840            trace.tree_at(trace.steps.len()),
841            plain
842                .node_list
843                .iter()
844                .map(RrtStarTraceNode::from)
845                .collect::<Vec<_>>()
846        );
847        assert_eq!(
848            recorded.planning_with_trace(7),
849            trace,
850            "a repeated run must reset all recorded state"
851        );
852    }
853
854    #[test]
855    fn every_recorded_prefix_has_real_parents_costs_and_feasible_improving_paths() {
856        let mut planner = trace_fixture();
857        let trace = planner.planning_with_trace(7);
858        let mut tree = vec![trace.root.clone()];
859        let mut best = f64::INFINITY;
860        let mut rewires = 0;
861        let mut improvements = 0;
862        for step in &trace.steps {
863            assert!(step.nearest < tree.len());
864            let before_count = tree.len();
865            for update in &step.updates {
866                assert_eq!(update.before.as_ref(), tree.get(update.index));
867                if let Some(before) = &update.before {
868                    if before.parent != update.after.parent {
869                        rewires += 1;
870                    }
871                }
872                if update.index == tree.len() {
873                    tree.push(update.after.clone());
874                } else {
875                    tree[update.index] = update.after.clone();
876                }
877            }
878            assert_eq!(tree.len() - before_count, usize::from(step.accepted));
879            for (i, node) in tree.iter().enumerate().skip(1) {
880                let parent = node.parent.expect("non-root node has a parent");
881                assert!(parent < tree.len() && parent != i);
882                let p = &tree[parent];
883                assert!(
884                    (node.cost
885                        - p.cost
886                        - (node.point[0] - p.point[0]).hypot(node.point[1] - p.point[1]))
887                    .abs()
888                        < 1e-7
889                );
890                assert!(!crate::rrt::polyline_within(
891                    &[p.point[0], node.point[0]],
892                    &[p.point[1], node.point[1]],
893                    [10.0, 10.0],
894                    2.8
895                ));
896                let mut cursor = i;
897                for depth in 0..=tree.len() {
898                    match tree[cursor].parent {
899                        Some(p) => cursor = p,
900                        None => break,
901                    }
902                    assert!(depth < tree.len(), "recorded tree has a parent cycle");
903                }
904            }
905            if !step.path.is_empty() {
906                assert_eq!(step.path.first(), Some(&[18.5, 18.5]));
907                assert_eq!(step.path.last(), Some(&[1.5, 1.5]));
908                let length: f64 = step
909                    .path
910                    .windows(2)
911                    .map(|p| (p[1][0] - p[0][0]).hypot(p[1][1] - p[0][1]))
912                    .sum();
913                assert!(length <= best + 1e-7);
914                if length < best - 1e-7 {
915                    improvements += 1;
916                }
917                best = length;
918            } else {
919                assert!(!best.is_finite(), "a discovered path disappeared");
920            }
921        }
922        assert!(rewires > 0 && improvements > 1);
923        assert_eq!(trace.tree_at(0), vec![trace.root.clone()]);
924        assert_eq!(trace.steps.last().unwrap().path, trace.path.unwrap());
925    }
926
927    #[test]
928    fn trace_preserves_rejections_and_the_early_stop_condition() {
929        let mut blocked = trace_fixture();
930        blocked.obstacle_list = vec![(10.0, 10.0, 100.0)];
931        let trace = blocked.planning_with_trace(7);
932        assert_eq!(trace.steps.len(), 500);
933        assert!(trace.path.is_none());
934        assert!(trace
935            .steps
936            .iter()
937            .all(|s| !s.accepted && s.updates.is_empty() && s.path.is_empty()));
938        let mut early = trace_fixture();
939        early.search_until_max_iter = false;
940        let trace = early.planning_with_trace(7);
941        assert!(trace.path.is_some() && trace.steps.len() < 500);
942        assert!(trace.steps[..trace.steps.len() - 1]
943            .iter()
944            .all(|s| s.path.is_empty()));
945    }
946}