1#![allow(dead_code, clippy::needless_borrows_for_generic_args)]
2
3use 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 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 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 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 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 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 pub fn get_tree(&self) -> &[Node] {
415 &self.node_list
416 }
417
418 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 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}