1use std::collections::HashMap;
9
10use rust_robotics_core::{Obstacles, Path2D, PathPlanner, Point2D, RoboticsError, RoboticsResult};
11
12type GridCoord = (i32, i32);
13
14#[derive(Debug, Clone, Copy, PartialEq, Eq)]
16pub enum AStarVariantMode {
17 Standard,
18 BeamSearch,
19 IterativeDeepening,
20 DynamicWeighting,
21 ThetaStarLike,
22 JumpPointCorners,
23}
24
25#[derive(Debug, Clone)]
27pub struct AStarVariantConfig {
28 pub resolution: f64,
29 pub robot_radius: f64,
30 pub mode: AStarVariantMode,
31 pub beam_capacity: usize,
32 pub epsilon: f64,
33 pub upper_bound_depth: usize,
34 pub max_theta: usize,
35 pub only_corners: bool,
36 pub max_corner: f64,
37}
38
39impl Default for AStarVariantConfig {
40 fn default() -> Self {
41 Self {
42 resolution: 1.0,
43 robot_radius: 0.0,
44 mode: AStarVariantMode::Standard,
45 beam_capacity: 30,
46 epsilon: 4.0,
47 upper_bound_depth: 500,
48 max_theta: 5,
49 only_corners: false,
50 max_corner: 5.0,
51 }
52 }
53}
54
55impl AStarVariantConfig {
56 fn validate(&self) -> RoboticsResult<()> {
57 if !self.resolution.is_finite() || self.resolution <= 0.0 {
58 return Err(RoboticsError::InvalidParameter(format!(
59 "resolution must be positive and finite, got {}",
60 self.resolution
61 )));
62 }
63 if !self.robot_radius.is_finite() || self.robot_radius < 0.0 {
64 return Err(RoboticsError::InvalidParameter(format!(
65 "robot_radius must be non-negative and finite, got {}",
66 self.robot_radius
67 )));
68 }
69 if self.beam_capacity == 0 {
70 return Err(RoboticsError::InvalidParameter(
71 "beam_capacity must be greater than zero".to_string(),
72 ));
73 }
74 if !self.epsilon.is_finite() || self.epsilon < 0.0 {
75 return Err(RoboticsError::InvalidParameter(format!(
76 "epsilon must be non-negative and finite, got {}",
77 self.epsilon
78 )));
79 }
80 if self.upper_bound_depth == 0 {
81 return Err(RoboticsError::InvalidParameter(
82 "upper_bound_depth must be greater than zero".to_string(),
83 ));
84 }
85 if self.max_theta == 0 {
86 return Err(RoboticsError::InvalidParameter(
87 "max_theta must be greater than zero".to_string(),
88 ));
89 }
90 if !self.max_corner.is_finite() || self.max_corner <= 0.0 {
91 return Err(RoboticsError::InvalidParameter(format!(
92 "max_corner must be positive and finite, got {}",
93 self.max_corner
94 )));
95 }
96
97 Ok(())
98 }
99}
100
101#[derive(Debug, Clone)]
102struct VariantGrid {
103 resolution: f64,
104 min_x: f64,
105 min_y: f64,
106 x_width: i32,
107 y_width: i32,
108 obstacle_map: Vec<Vec<bool>>,
109}
110
111impl VariantGrid {
112 fn try_new(ox: &[f64], oy: &[f64], resolution: f64, robot_radius: f64) -> RoboticsResult<Self> {
113 if ox.len() != oy.len() {
114 return Err(RoboticsError::InvalidParameter(format!(
115 "obstacle x/y coordinates must have matching lengths, got {} and {}",
116 ox.len(),
117 oy.len()
118 )));
119 }
120 if ox.is_empty() {
121 return Err(RoboticsError::InvalidParameter(
122 "A* variants require at least one obstacle point".to_string(),
123 ));
124 }
125 if ox.iter().chain(oy.iter()).any(|value| !value.is_finite()) {
126 return Err(RoboticsError::InvalidParameter(
127 "obstacle coordinates must be finite".to_string(),
128 ));
129 }
130
131 let min_x = ox
132 .iter()
133 .fold(f64::INFINITY, |acc, &value| acc.min(value))
134 .round();
135 let min_y = oy
136 .iter()
137 .fold(f64::INFINITY, |acc, &value| acc.min(value))
138 .round();
139 let max_x = ox
140 .iter()
141 .fold(f64::NEG_INFINITY, |acc, &value| acc.max(value))
142 .round();
143 let max_y = oy
144 .iter()
145 .fold(f64::NEG_INFINITY, |acc, &value| acc.max(value))
146 .round();
147 let x_width = ((max_x - min_x) / resolution).round() as i32 + 1;
148 let y_width = ((max_y - min_y) / resolution).round() as i32 + 1;
149
150 let mut obstacle_map = vec![vec![false; y_width as usize]; x_width as usize];
151 for ix in 0..x_width {
152 let x = min_x + ix as f64 * resolution;
153 for iy in 0..y_width {
154 let y = min_y + iy as f64 * resolution;
155 if ox
156 .iter()
157 .zip(oy.iter())
158 .any(|(&obs_x, &obs_y)| (obs_x - x).hypot(obs_y - y) <= robot_radius)
159 {
160 obstacle_map[ix as usize][iy as usize] = true;
161 }
162 }
163 }
164
165 Ok(Self {
166 resolution,
167 min_x,
168 min_y,
169 x_width,
170 y_width,
171 obstacle_map,
172 })
173 }
174
175 fn calc_x_index(&self, x: f64) -> i32 {
176 ((x - self.min_x) / self.resolution).round() as i32
177 }
178
179 fn calc_y_index(&self, y: f64) -> i32 {
180 ((y - self.min_y) / self.resolution).round() as i32
181 }
182
183 fn calc_x_position(&self, ix: i32) -> f64 {
184 self.min_x + ix as f64 * self.resolution
185 }
186
187 fn calc_y_position(&self, iy: i32) -> f64 {
188 self.min_y + iy as f64 * self.resolution
189 }
190
191 fn is_valid(&self, x: i32, y: i32) -> bool {
192 x >= 0
193 && x < self.x_width
194 && y >= 0
195 && y < self.y_width
196 && !self.obstacle_map[x as usize][y as usize]
197 }
198
199 fn contains(&self, x: i32, y: i32) -> bool {
200 x >= 0 && x < self.x_width && y >= 0 && y < self.y_width
201 }
202
203 fn is_obstacle_or_outside(&self, x: i32, y: i32) -> bool {
204 !self.contains(x, y) || self.obstacle_map[x as usize][y as usize]
205 }
206}
207
208#[derive(Debug, Clone)]
209struct SearchNode {
210 pred: Option<GridCoord>,
211 gcost: f64,
212 hcost: f64,
213 fcost: f64,
214 open: bool,
215 in_open_list: bool,
216}
217
218impl SearchNode {
219 fn new(hcost: f64) -> Self {
220 Self {
221 pred: None,
222 gcost: f64::INFINITY,
223 hcost,
224 fcost: f64::INFINITY,
225 open: true,
226 in_open_list: false,
227 }
228 }
229}
230
231struct CostUpdate<'a> {
232 current_threshold: f64,
233 current: GridCoord,
234 offset: f64,
235 weight: Option<f64>,
236 f_cost_list: &'a mut Vec<f64>,
237 all_nodes: &'a mut HashMap<GridCoord, SearchNode>,
238 open_set: &'a mut Vec<GridCoord>,
239}
240
241#[derive(Debug)]
243pub struct AStarVariantPlanner {
244 grid: VariantGrid,
245 config: AStarVariantConfig,
246}
247
248impl AStarVariantPlanner {
249 pub fn new(ox: &[f64], oy: &[f64], config: AStarVariantConfig) -> Self {
250 Self::try_new(ox, oy, config).expect(
251 "invalid A* variant input: obstacle list must be non-empty and valid, and config values must be positive/finite",
252 )
253 }
254
255 pub fn try_new(ox: &[f64], oy: &[f64], config: AStarVariantConfig) -> RoboticsResult<Self> {
256 config.validate()?;
257 let grid = VariantGrid::try_new(ox, oy, config.resolution, config.robot_radius)?;
258 Ok(Self { grid, config })
259 }
260
261 pub fn from_obstacles(
262 ox: &[f64],
263 oy: &[f64],
264 resolution: f64,
265 robot_radius: f64,
266 mode: AStarVariantMode,
267 ) -> Self {
268 let config = AStarVariantConfig {
269 resolution,
270 robot_radius,
271 mode,
272 ..Default::default()
273 };
274 Self::new(ox, oy, config)
275 }
276
277 pub fn from_obstacle_points(
278 obstacles: &Obstacles,
279 config: AStarVariantConfig,
280 ) -> RoboticsResult<Self> {
281 Self::try_new(&obstacles.x_coords(), &obstacles.y_coords(), config)
282 }
283
284 pub fn plan(&self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
285 self.plan_impl(start, goal)
286 }
287
288 pub fn plan_xy(&self, sx: f64, sy: f64, gx: f64, gy: f64) -> RoboticsResult<Path2D> {
289 self.plan_impl(Point2D::new(sx, sy), Point2D::new(gx, gy))
290 }
291
292 fn heuristic(start: GridCoord, goal: GridCoord) -> f64 {
293 let dx = (start.0 - goal.0).abs() as f64;
294 let dy = (start.1 - goal.1).abs() as f64;
295 14.0 * dx.min(dy) + 10.0 * (dx.max(dy) - dx.min(dy))
296 }
297
298 fn motion_model() -> [(i32, i32, f64); 8] {
299 [
300 (-1, -1, 14.0),
301 (-1, 0, 10.0),
302 (-1, 1, 14.0),
303 (0, -1, 10.0),
304 (0, 1, 10.0),
305 (1, -1, 14.0),
306 (1, 0, 10.0),
307 (1, 1, 14.0),
308 ]
309 }
310
311 fn build_node_table(&self, goal: GridCoord) -> HashMap<GridCoord, SearchNode> {
312 let mut nodes = HashMap::new();
313 for x in 0..self.grid.x_width {
314 for y in 0..self.grid.y_width {
315 if self.grid.is_valid(x, y) {
316 nodes.insert((x, y), SearchNode::new(Self::heuristic((x, y), goal)));
317 }
318 }
319 }
320 nodes
321 }
322
323 fn line_of_sight(&self, from: GridCoord, to: GridCoord) -> Option<f64> {
324 let mut t = 0.0_f64;
325 while t <= 0.5 {
326 let forward_x = ((1.0 - t) * from.0 as f64 + t * to.0 as f64) as i32;
327 let forward_y = ((1.0 - t) * from.1 as f64 + t * to.1 as f64) as i32;
328 if self.grid.is_obstacle_or_outside(forward_x, forward_y) {
329 return None;
330 }
331
332 let reverse_x = ((1.0 - t) * to.0 as f64 + t * from.0 as f64) as i32;
333 let reverse_y = ((1.0 - t) * to.1 as f64 + t * from.1 as f64) as i32;
334 if self.grid.is_obstacle_or_outside(reverse_x, reverse_y) {
335 return None;
336 }
337
338 t += 0.001;
339 }
340
341 Some(((from.0 - to.0) as f64).hypot((from.1 - to.1) as f64))
342 }
343
344 fn key_points(&self) -> Vec<GridCoord> {
345 let offsets1 = [(1, 0), (0, 1), (-1, 0), (1, 0)];
346 let offsets2 = [(1, 1), (-1, 1), (-1, -1), (1, -1)];
347 let offsets3 = [(0, 1), (-1, 0), (0, -1), (0, -1)];
348
349 let mut corners = Vec::new();
350 for x in 0..self.grid.x_width {
351 for y in 0..self.grid.y_width {
352 if self.grid.is_obstacle_or_outside(x, y) {
353 continue;
354 }
355
356 let mut empty_space = true;
357 'neighbors: for dx in -1..=1 {
358 for dy in -1..=1 {
359 let nx = x + dx;
360 let ny = y + dy;
361 if !self.grid.contains(nx, ny) {
362 continue;
363 }
364 if self.grid.is_obstacle_or_outside(nx, ny) {
365 empty_space = false;
366 break 'neighbors;
367 }
368 }
369 }
370 if empty_space {
371 continue;
372 }
373
374 for ((i1, j1), ((i2, j2), (i3, j3))) in offsets1
375 .iter()
376 .copied()
377 .zip(offsets2.iter().copied().zip(offsets3.iter().copied()))
378 {
379 let n1 = (x + i1, y + j1);
380 let n2 = (x + i2, y + j2);
381 let n3 = (x + i3, y + j3);
382 if !self.grid.contains(n1.0, n1.1)
383 || !self.grid.contains(n2.0, n2.1)
384 || !self.grid.contains(n3.0, n3.1)
385 {
386 continue;
387 }
388
389 let mut obstacle_count = 0;
390 if self.grid.is_obstacle_or_outside(n1.0, n1.1) {
391 obstacle_count += 1;
392 }
393 if self.grid.is_obstacle_or_outside(n2.0, n2.1) {
394 obstacle_count += 1;
395 }
396 if self.grid.is_obstacle_or_outside(n3.0, n3.1) {
397 obstacle_count += 1;
398 }
399 if obstacle_count == 1 || obstacle_count == 3 {
400 corners.push((x, y));
401 break;
402 }
403 }
404 }
405 }
406
407 if self.config.only_corners {
408 return corners;
409 }
410
411 let mut key_points = corners.clone();
412 for &(x1, y1) in &corners {
413 for &(x2, y2) in &corners {
414 if x1 == x2 && y1 == y2 {
415 continue;
416 }
417 if self.line_of_sight((x1, y1), (x2, y2)).is_none() {
418 continue;
419 }
420 key_points.push(((x1 + x2) / 2, (y1 + y2) / 2));
421 }
422 }
423
424 key_points
425 }
426
427 fn get_farthest_point(
428 &self,
429 x: i32,
430 y: i32,
431 dx: i32,
432 dy: i32,
433 goal: GridCoord,
434 ) -> (GridCoord, usize, bool) {
435 let mut step_x = dx;
436 let mut step_y = dy;
437 let mut counter = 1usize;
438 let mut got_goal = false;
439
440 while !self.grid.is_obstacle_or_outside(x + step_x, y + step_y)
441 && counter < self.config.max_theta
442 {
443 step_x += dx;
444 step_y += dy;
445 counter += 1;
446
447 if (x + step_x, y + step_y) == goal {
448 got_goal = true;
449 break;
450 }
451 if !self.grid.contains(x + step_x, y + step_y) {
452 break;
453 }
454 }
455
456 (
457 (x + step_x - 2 * dx, y + step_y - 2 * dy),
458 counter,
459 got_goal,
460 )
461 }
462
463 fn build_corner_node_table(
464 &self,
465 start: GridCoord,
466 goal: GridCoord,
467 ) -> HashMap<GridCoord, SearchNode> {
468 let mut nodes = HashMap::new();
469 for point in self.key_points() {
470 if self.grid.is_valid(point.0, point.1) {
471 nodes
472 .entry(point)
473 .or_insert_with(|| SearchNode::new(Self::heuristic(point, goal)));
474 }
475 }
476 nodes.insert(goal, SearchNode::new(0.0));
477 nodes.insert(start, SearchNode::new(Self::heuristic(start, goal)));
478 nodes
479 }
480
481 fn build_path(
482 &self,
483 all_nodes: &HashMap<GridCoord, SearchNode>,
484 goal: GridCoord,
485 ) -> RoboticsResult<Path2D> {
486 let mut points = Vec::new();
487 let mut current = goal;
488 loop {
489 points.push(Point2D::new(
490 self.grid.calc_x_position(current.0),
491 self.grid.calc_y_position(current.1),
492 ));
493 let node = all_nodes.get(¤t).ok_or_else(|| {
494 RoboticsError::PlanningError(
495 "goal node missing during path reconstruction".to_string(),
496 )
497 })?;
498 match node.pred {
499 Some(pred) => current = pred,
500 None => break,
501 }
502 }
503 points.reverse();
504 Ok(Path2D::from_points(points))
505 }
506
507 fn update_node_cost(
508 &self,
509 candidate: GridCoord,
510 mut no_valid_f: bool,
511 update: &mut CostUpdate<'_>,
512 ) -> bool {
513 let current_cost = update.all_nodes[&update.current].gcost;
514 let candidate_node = update
515 .all_nodes
516 .get_mut(&candidate)
517 .expect("candidate node should be present for valid cell");
518 if !candidate_node.open {
519 return no_valid_f;
520 }
521
522 let g_cost = update.offset + current_cost;
523 let mut h_cost = candidate_node.hcost;
524 if let Some(weight) = update.weight {
525 h_cost *= weight;
526 }
527 let f_cost = g_cost + h_cost;
528
529 if f_cost < candidate_node.fcost && f_cost <= update.current_threshold {
530 update.f_cost_list.push(f_cost);
531 candidate_node.pred = Some(update.current);
532 candidate_node.gcost = g_cost;
533 candidate_node.fcost = f_cost;
534 if !candidate_node.in_open_list {
535 update.open_set.push(candidate);
536 candidate_node.in_open_list = true;
537 }
538 }
539
540 if update.current_threshold < f_cost && f_cost < candidate_node.fcost {
541 no_valid_f = true;
542 }
543
544 no_valid_f
545 }
546
547 fn plan_jump_point_corners(
548 &self,
549 start_coord: GridCoord,
550 goal_coord: GridCoord,
551 ) -> RoboticsResult<Path2D> {
552 let mut all_nodes = self.build_corner_node_table(start_coord, goal_coord);
553 let start_node = all_nodes
554 .get_mut(&start_coord)
555 .expect("start node should exist in jump-point corner graph");
556 start_node.gcost = 0.0;
557 start_node.fcost = start_node.hcost;
558 start_node.in_open_list = true;
559
560 let mut open_set = vec![start_coord];
561 while !open_set.is_empty() {
562 open_set
563 .sort_by(|left, right| all_nodes[left].fcost.total_cmp(&all_nodes[right].fcost));
564
565 let mut chosen_index = 0usize;
566 let lowest_f = all_nodes[&open_set[0]].fcost;
567 let mut lowest_h = all_nodes[&open_set[0]].hcost;
568 let mut lowest_g = all_nodes[&open_set[0]].gcost;
569 for candidate in open_set.iter().skip(1) {
570 let node = &all_nodes[candidate];
571 if node.fcost == lowest_f && node.gcost < lowest_g {
572 lowest_g = node.gcost;
573 chosen_index += 1;
574 } else if node.fcost == lowest_f && node.gcost == lowest_g && node.hcost < lowest_h
575 {
576 lowest_h = node.hcost;
577 chosen_index += 1;
578 } else {
579 break;
580 }
581 }
582
583 let current = open_set[chosen_index];
584 let candidate_points: Vec<_> = all_nodes.keys().copied().collect();
585 for candidate in candidate_points {
586 if candidate == current {
587 continue;
588 }
589 if ((current.0 - candidate.0) as f64).hypot((current.1 - candidate.1) as f64)
590 > self.config.max_corner
591 {
592 continue;
593 }
594 let Some(offset) = self.line_of_sight(current, candidate) else {
595 continue;
596 };
597
598 if candidate == goal_coord {
599 all_nodes.get_mut(&goal_coord).unwrap().pred = Some(current);
600 return self.build_path(&all_nodes, goal_coord);
601 }
602
603 let current_cost = all_nodes[¤t].gcost;
604 let candidate_node = all_nodes.get_mut(&candidate).unwrap();
605 if !candidate_node.open {
606 continue;
607 }
608
609 let g_cost = current_cost + offset;
610 let f_cost = g_cost + candidate_node.hcost;
611 if f_cost < candidate_node.fcost {
612 candidate_node.pred = Some(current);
613 candidate_node.gcost = g_cost;
614 candidate_node.fcost = f_cost;
615 if !candidate_node.in_open_list {
616 open_set.push(candidate);
617 candidate_node.in_open_list = true;
618 }
619 }
620 }
621
622 {
623 let current_node = all_nodes.get_mut(¤t).unwrap();
624 current_node.open = false;
625 current_node.in_open_list = false;
626 current_node.fcost = f64::INFINITY;
627 current_node.hcost = f64::INFINITY;
628 }
629 open_set.remove(chosen_index);
630 }
631
632 Err(RoboticsError::PlanningError("no path found".to_string()))
633 }
634
635 fn plan_grid_variant(
636 &self,
637 start_coord: GridCoord,
638 goal_coord: GridCoord,
639 ) -> RoboticsResult<Path2D> {
640 let mut all_nodes = self.build_node_table(goal_coord);
641 let start_node = all_nodes
642 .get_mut(&start_coord)
643 .expect("start node should exist in valid grid");
644 start_node.gcost = 0.0;
645 start_node.fcost = start_node.hcost;
646 start_node.in_open_list = true;
647
648 let mut open_set = vec![start_coord];
649 let mut goal_found = false;
650 let mut current_threshold = f64::INFINITY;
651 let mut depth = 0usize;
652 let mut no_valid_f = false;
653
654 while !open_set.is_empty() {
655 open_set
656 .sort_by(|left, right| all_nodes[left].fcost.total_cmp(&all_nodes[right].fcost));
657
658 let mut chosen_index = 0usize;
659 let lowest_f = all_nodes[&open_set[0]].fcost;
660 let mut lowest_h = all_nodes[&open_set[0]].hcost;
661 let mut lowest_g = all_nodes[&open_set[0]].gcost;
662 for candidate in open_set.iter().skip(1) {
663 let node = &all_nodes[candidate];
664 if node.fcost == lowest_f && node.gcost < lowest_g {
665 lowest_g = node.gcost;
666 chosen_index += 1;
667 } else if node.fcost == lowest_f && node.gcost == lowest_g && node.hcost < lowest_h
668 {
669 lowest_h = node.hcost;
670 chosen_index += 1;
671 } else {
672 break;
673 }
674 }
675
676 if matches!(self.config.mode, AStarVariantMode::BeamSearch) {
677 while open_set.len() > self.config.beam_capacity {
678 open_set.pop();
679 }
680 }
681
682 let current = open_set[chosen_index];
683 let mut f_cost_list = Vec::new();
684 let weight = if matches!(self.config.mode, AStarVariantMode::DynamicWeighting) {
685 Some(
686 1.0 + self.config.epsilon
687 - self.config.epsilon * depth as f64 / self.config.upper_bound_depth as f64,
688 )
689 } else {
690 None
691 };
692
693 for (dx, dy, offset) in Self::motion_model() {
694 let (candidate, offset, reached_goal) =
695 if matches!(self.config.mode, AStarVariantMode::ThetaStarLike) {
696 let (candidate, multiplier, reached_goal) =
697 self.get_farthest_point(current.0, current.1, dx, dy, goal_coord);
698 (candidate, offset * multiplier as f64, reached_goal)
699 } else {
700 ((current.0 + dx, current.1 + dy), offset, false)
701 };
702
703 if reached_goal {
704 all_nodes.get_mut(&goal_coord).unwrap().pred = Some(current);
705 goal_found = true;
706 break;
707 }
708 if !all_nodes.contains_key(&candidate) {
709 continue;
710 }
711
712 if candidate == goal_coord {
713 all_nodes.get_mut(&goal_coord).unwrap().pred = Some(current);
714 goal_found = true;
715 break;
716 }
717
718 let mut update = CostUpdate {
719 current_threshold,
720 current,
721 offset,
722 weight,
723 f_cost_list: &mut f_cost_list,
724 all_nodes: &mut all_nodes,
725 open_set: &mut open_set,
726 };
727 no_valid_f = self.update_node_cost(candidate, no_valid_f, &mut update);
728 }
729
730 if goal_found {
731 return self.build_path(&all_nodes, goal_coord);
732 }
733
734 if matches!(self.config.mode, AStarVariantMode::IterativeDeepening) {
735 if let Some(next_threshold) = f_cost_list
736 .iter()
737 .copied()
738 .min_by(|left, right| left.total_cmp(right))
739 {
740 current_threshold = next_threshold;
741 } else {
742 current_threshold = f64::INFINITY;
743 }
744
745 if f_cost_list.is_empty() && no_valid_f {
746 let current_node = all_nodes.get_mut(¤t).unwrap();
747 current_node.fcost = f64::INFINITY;
748 current_node.hcost = f64::INFINITY;
749 continue;
750 }
751 }
752
753 {
754 let current_node = all_nodes.get_mut(¤t).unwrap();
755 current_node.open = false;
756 current_node.in_open_list = false;
757 current_node.fcost = f64::INFINITY;
758 current_node.hcost = f64::INFINITY;
759 }
760 open_set.remove(chosen_index);
761 depth += 1;
762 }
763
764 Err(RoboticsError::PlanningError("no path found".to_string()))
765 }
766
767 fn plan_impl(&self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
768 let start_coord = (
769 self.grid.calc_x_index(start.x),
770 self.grid.calc_y_index(start.y),
771 );
772 let goal_coord = (
773 self.grid.calc_x_index(goal.x),
774 self.grid.calc_y_index(goal.y),
775 );
776 if !self.grid.is_valid(start_coord.0, start_coord.1) {
777 return Err(RoboticsError::PlanningError(
778 "start position is invalid".to_string(),
779 ));
780 }
781 if !self.grid.is_valid(goal_coord.0, goal_coord.1) {
782 return Err(RoboticsError::PlanningError(
783 "goal position is invalid".to_string(),
784 ));
785 }
786
787 if matches!(self.config.mode, AStarVariantMode::JumpPointCorners) {
788 self.plan_jump_point_corners(start_coord, goal_coord)
789 } else {
790 self.plan_grid_variant(start_coord, goal_coord)
791 }
792 }
793}
794
795impl PathPlanner for AStarVariantPlanner {
796 fn plan(&self, start: Point2D, goal: Point2D) -> Result<Path2D, RoboticsError> {
797 self.plan_impl(start, goal)
798 }
799}
800
801#[cfg(test)]
802mod tests {
803 use super::*;
804
805 fn draw_horizontal_line(
806 start_x: i32,
807 start_y: i32,
808 length: i32,
809 obstacle_x: &mut Vec<f64>,
810 obstacle_y: &mut Vec<f64>,
811 ) {
812 for x in start_x..start_x + length {
813 for y in start_y..start_y + 2 {
814 obstacle_x.push(x as f64);
815 obstacle_y.push(y as f64);
816 }
817 }
818 }
819
820 fn draw_vertical_line(
821 start_x: i32,
822 start_y: i32,
823 length: i32,
824 obstacle_x: &mut Vec<f64>,
825 obstacle_y: &mut Vec<f64>,
826 ) {
827 for x in start_x..start_x + 2 {
828 for y in start_y..start_y + length {
829 obstacle_x.push(x as f64);
830 obstacle_y.push(y as f64);
831 }
832 }
833 }
834
835 fn build_pythonrobotics_maze() -> (Vec<f64>, Vec<f64>) {
836 let mut obstacle_x = Vec::new();
837 let mut obstacle_y = Vec::new();
838
839 draw_vertical_line(0, 0, 50, &mut obstacle_x, &mut obstacle_y);
840 draw_vertical_line(48, 0, 50, &mut obstacle_x, &mut obstacle_y);
841 draw_horizontal_line(0, 0, 50, &mut obstacle_x, &mut obstacle_y);
842 draw_horizontal_line(0, 48, 50, &mut obstacle_x, &mut obstacle_y);
843
844 let all_x = [10, 10, 10, 15, 20, 20, 30, 30, 35, 30, 40, 45];
845 let all_y = [10, 30, 45, 20, 5, 40, 10, 40, 5, 40, 10, 25];
846 let all_len = [10, 10, 5, 10, 10, 5, 20, 10, 25, 10, 35, 15];
847 for ((x, y), length) in all_x.iter().zip(all_y.iter()).zip(all_len.iter()) {
848 draw_vertical_line(*x, *y, *length, &mut obstacle_x, &mut obstacle_y);
849 }
850
851 let all_x = [35, 40, 15, 10, 45, 20, 10, 15, 25, 45, 10, 30, 10, 40];
852 let all_y = [5, 10, 15, 20, 20, 25, 30, 35, 35, 35, 40, 40, 45, 45];
853 let all_len = [10, 5, 10, 10, 5, 5, 10, 5, 10, 5, 10, 5, 5, 5];
854 for ((x, y), length) in all_x.iter().zip(all_y.iter()).zip(all_len.iter()) {
855 draw_horizontal_line(*x, *y, *length, &mut obstacle_x, &mut obstacle_y);
856 }
857
858 (obstacle_x, obstacle_y)
859 }
860
861 fn parse_fixture(csv: &str) -> Path2D {
862 let points = csv
863 .lines()
864 .skip(1)
865 .filter(|line| !line.trim().is_empty())
866 .map(|line| {
867 let (x, y) = line
868 .split_once(',')
869 .expect("fixture rows must contain a comma");
870 Point2D::new(x.parse().unwrap(), y.parse().unwrap())
871 })
872 .collect();
873 Path2D::from_points(points)
874 }
875
876 fn assert_paths_match(actual: &Path2D, expected: &Path2D) {
877 assert_eq!(actual.len(), expected.len());
878 for (actual_point, expected_point) in actual.points.iter().zip(expected.points.iter()) {
879 assert!(
880 (actual_point.x - expected_point.x).abs() < 1.0e-12
881 && (actual_point.y - expected_point.y).abs() < 1.0e-12,
882 "expected ({}, {}), got ({}, {})",
883 expected_point.x,
884 expected_point.y,
885 actual_point.x,
886 actual_point.y
887 );
888 }
889 }
890
891 fn create_variant_planner(mode: AStarVariantMode) -> AStarVariantPlanner {
892 let (ox, oy) = build_pythonrobotics_maze();
893 AStarVariantPlanner::new(
894 &ox,
895 &oy,
896 AStarVariantConfig {
897 resolution: 1.0,
898 robot_radius: 0.0,
899 mode,
900 ..Default::default()
901 },
902 )
903 }
904
905 #[test]
906 fn test_beam_search_matches_pythonrobotics_reference() {
907 let planner = create_variant_planner(AStarVariantMode::BeamSearch);
908 let actual = planner.plan_xy(5.0, 5.0, 35.0, 45.0).unwrap();
909 let expected = parse_fixture(include_str!("testdata/a_star_variants_beam_python.csv"));
910 assert_paths_match(&actual, &expected);
911 }
912
913 #[test]
914 fn test_iterative_deepening_matches_pythonrobotics_reference() {
915 let planner = create_variant_planner(AStarVariantMode::IterativeDeepening);
916 let actual = planner.plan_xy(5.0, 5.0, 35.0, 45.0).unwrap();
917 let expected = parse_fixture(include_str!(
918 "testdata/a_star_variants_iterative_python.csv"
919 ));
920 assert_paths_match(&actual, &expected);
921 }
922
923 #[test]
924 fn test_dynamic_weighting_matches_pythonrobotics_reference() {
925 let planner = create_variant_planner(AStarVariantMode::DynamicWeighting);
926 let actual = planner.plan_xy(5.0, 5.0, 35.0, 45.0).unwrap();
927 let expected = parse_fixture(include_str!("testdata/a_star_variants_dynamic_python.csv"));
928 assert_paths_match(&actual, &expected);
929 }
930
931 #[test]
932 fn test_theta_star_like_matches_pythonrobotics_reference() {
933 let planner = create_variant_planner(AStarVariantMode::ThetaStarLike);
934 let actual = planner.plan_xy(5.0, 5.0, 35.0, 45.0).unwrap();
935 let expected = parse_fixture(include_str!("testdata/a_star_variants_theta_python.csv"));
936 assert_paths_match(&actual, &expected);
937 }
938
939 #[test]
940 fn test_jump_point_corners_matches_pythonrobotics_reference() {
941 let planner = create_variant_planner(AStarVariantMode::JumpPointCorners);
942 let actual = planner.plan_xy(5.0, 5.0, 35.0, 45.0).unwrap();
943 let expected = parse_fixture(include_str!("testdata/a_star_variants_jump_python.csv"));
944 assert_paths_match(&actual, &expected);
945 }
946
947 #[test]
948 fn test_variant_config_validation_rejects_invalid_beam_capacity() {
949 let (ox, oy) = build_pythonrobotics_maze();
950 let error = AStarVariantPlanner::try_new(
951 &ox,
952 &oy,
953 AStarVariantConfig {
954 beam_capacity: 0,
955 ..Default::default()
956 },
957 )
958 .unwrap_err();
959
960 assert!(matches!(error, RoboticsError::InvalidParameter(_)));
961 }
962}