1#![allow(clippy::collapsible_if)]
2
3use std::cmp::Ordering;
13use std::collections::{BinaryHeap, HashMap};
14
15use crate::grid::{GridMap, Node};
16use rust_robotics_core::{Obstacles, Path2D, PathPlanner, Point2D, RoboticsError, RoboticsResult};
17
18#[derive(Debug, Clone)]
20pub struct JPSConfig {
21 pub resolution: f64,
23 pub robot_radius: f64,
25 pub heuristic_weight: f64,
27}
28
29impl Default for JPSConfig {
30 fn default() -> Self {
31 Self {
32 resolution: 1.0,
33 robot_radius: 0.5,
34 heuristic_weight: 1.0,
35 }
36 }
37}
38
39impl JPSConfig {
40 pub fn validate(&self) -> RoboticsResult<()> {
41 if !self.resolution.is_finite() || self.resolution <= 0.0 {
42 return Err(RoboticsError::InvalidParameter(format!(
43 "resolution must be positive and finite, got {}",
44 self.resolution
45 )));
46 }
47 if !self.robot_radius.is_finite() || self.robot_radius < 0.0 {
48 return Err(RoboticsError::InvalidParameter(format!(
49 "robot_radius must be non-negative and finite, got {}",
50 self.robot_radius
51 )));
52 }
53 if !self.heuristic_weight.is_finite() || self.heuristic_weight <= 0.0 {
54 return Err(RoboticsError::InvalidParameter(format!(
55 "heuristic_weight must be positive and finite, got {}",
56 self.heuristic_weight
57 )));
58 }
59
60 Ok(())
61 }
62}
63
64#[derive(Debug, Clone, Copy, PartialEq, Eq)]
66pub enum JPSFallbackReason {
67 InvalidJumpPath,
69 SuboptimalJumpPath,
71 SearchExhausted,
73}
74
75impl JPSFallbackReason {
76 pub fn as_str(self) -> &'static str {
77 match self {
78 Self::InvalidJumpPath => "invalid_jump_path",
79 Self::SuboptimalJumpPath => "suboptimal_jump_path",
80 Self::SearchExhausted => "search_exhausted",
81 }
82 }
83}
84
85#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
87pub enum JPSInvalidJumpPathDetail {
88 InvalidStepSequence,
90 CostMismatch,
93 InvalidStepSequenceAndCostMismatch,
95}
96
97impl JPSInvalidJumpPathDetail {
98 pub fn as_str(self) -> &'static str {
99 match self {
100 Self::InvalidStepSequence => "invalid_step_sequence",
101 Self::CostMismatch => "cost_mismatch",
102 Self::InvalidStepSequenceAndCostMismatch => "invalid_step_and_cost_mismatch",
103 }
104 }
105}
106
107#[derive(Debug, Clone, PartialEq)]
109pub struct JPSDetourSegment {
110 pub from_x: i32,
111 pub from_y: i32,
112 pub to_x: i32,
113 pub to_y: i32,
114 pub expected_length: f64,
115 pub reconstructed_length: f64,
116}
117
118#[derive(Debug, Clone, PartialEq)]
120pub struct JPSCostMismatchMetrics {
121 pub search_cost: f64,
122 pub reconstructed_path_length: f64,
123 pub delta: f64,
124 pub detour_segment_count: usize,
125 pub detour_segments: Vec<JPSDetourSegment>,
126}
127
128#[derive(Debug, Clone, PartialEq)]
130pub struct JPSPlanDiagnostics {
131 pub used_fallback: bool,
132 pub fallback_reason: Option<JPSFallbackReason>,
133 pub invalid_jump_path_detail: Option<JPSInvalidJumpPathDetail>,
134 pub cost_mismatch_metrics: Option<JPSCostMismatchMetrics>,
135}
136
137#[derive(Debug, Clone)]
139pub struct JPSPlanResult {
140 pub path: Path2D,
141 pub diagnostics: JPSPlanDiagnostics,
142}
143
144#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
146struct Direction {
147 dx: i32,
148 dy: i32,
149}
150
151impl Direction {
152 fn new(dx: i32, dy: i32) -> Self {
153 Self { dx, dy }
154 }
155
156 fn is_diagonal(&self) -> bool {
157 self.dx != 0 && self.dy != 0
158 }
159
160 fn is_cardinal(&self) -> bool {
161 !self.is_diagonal()
162 }
163}
164
165#[derive(Debug)]
167struct PriorityNode {
168 x: i32,
169 y: i32,
170 cost: f64,
171 priority: f64,
172 index: usize,
173}
174
175impl Eq for PriorityNode {}
176
177impl PartialEq for PriorityNode {
178 fn eq(&self, other: &Self) -> bool {
179 self.priority == other.priority
180 }
181}
182
183impl Ord for PriorityNode {
184 fn cmp(&self, other: &Self) -> Ordering {
185 other
187 .priority
188 .partial_cmp(&self.priority)
189 .unwrap_or(Ordering::Equal)
190 }
191}
192
193impl PartialOrd for PriorityNode {
194 fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
195 Some(self.cmp(other))
196 }
197}
198
199pub struct JPSPlanner {
201 grid_map: GridMap,
202 config: JPSConfig,
203}
204
205impl JPSPlanner {
206 fn state_key(&self, x: i32, y: i32, dir: Option<Direction>) -> (i32, Option<Direction>) {
207 (self.grid_map.calc_index(x, y), dir)
208 }
209
210 pub fn new(ox: &[f64], oy: &[f64], config: JPSConfig) -> Self {
212 Self::try_new(ox, oy, config).expect(
213 "invalid JPS planner input: obstacle list must be non-empty and valid, and config values must be positive/finite",
214 )
215 }
216
217 pub fn try_new(ox: &[f64], oy: &[f64], config: JPSConfig) -> RoboticsResult<Self> {
219 config.validate()?;
220 let grid_map = GridMap::try_new(ox, oy, config.resolution, config.robot_radius)?;
221 Ok(JPSPlanner { grid_map, config })
222 }
223
224 pub fn from_obstacles(ox: &[f64], oy: &[f64], resolution: f64, robot_radius: f64) -> Self {
226 let config = JPSConfig {
227 resolution,
228 robot_radius,
229 ..Default::default()
230 };
231 Self::new(ox, oy, config)
232 }
233
234 pub fn from_obstacle_points(obstacles: &Obstacles, config: JPSConfig) -> RoboticsResult<Self> {
236 config.validate()?;
237 let grid_map = GridMap::from_obstacles(obstacles, config.resolution, config.robot_radius)?;
238 Ok(JPSPlanner { grid_map, config })
239 }
240
241 #[deprecated(note = "use plan() or plan_xy() instead")]
243 pub fn planning(&self, sx: f64, sy: f64, gx: f64, gy: f64) -> Option<(Vec<f64>, Vec<f64>)> {
244 match self.plan_xy(sx, sy, gx, gy) {
245 Ok(path) => Some((path.x_coords(), path.y_coords())),
246 Err(_) => None,
247 }
248 }
249
250 pub fn plan(&self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
252 self.plan_with_diagnostics(start, goal)
253 .map(|result| result.path)
254 }
255
256 pub fn plan_xy(&self, sx: f64, sy: f64, gx: f64, gy: f64) -> RoboticsResult<Path2D> {
258 self.plan_with_diagnostics(Point2D::new(sx, sy), Point2D::new(gx, gy))
259 .map(|result| result.path)
260 }
261
262 pub fn plan_with_diagnostics(
264 &self,
265 start: Point2D,
266 goal: Point2D,
267 ) -> RoboticsResult<JPSPlanResult> {
268 self.plan_impl(start, goal)
269 }
270
271 pub fn grid_map(&self) -> &GridMap {
273 &self.grid_map
274 }
275
276 fn calc_heuristic(&self, n1_x: i32, n1_y: i32, n2_x: i32, n2_y: i32) -> f64 {
277 let dx = (n1_x - n2_x).abs() as f64;
279 let dy = (n1_y - n2_y).abs() as f64;
280 let d_min = dx.min(dy);
281 let d_max = dx.max(dy);
282 self.config.heuristic_weight * (d_min * std::f64::consts::SQRT_2 + (d_max - d_min))
283 }
284
285 fn has_forced_neighbor(&self, x: i32, y: i32, dir: Direction) -> bool {
287 if dir.is_cardinal() {
288 if dir.dx != 0 {
290 let blocked_up = !self.grid_map.is_valid(x - dir.dx, y + 1);
292 let open_up = self.grid_map.is_valid(x, y + 1);
293 let blocked_down = !self.grid_map.is_valid(x - dir.dx, y - 1);
294 let open_down = self.grid_map.is_valid(x, y - 1);
295 (blocked_up && open_up) || (blocked_down && open_down)
296 } else {
297 let blocked_right = !self.grid_map.is_valid(x + 1, y - dir.dy);
299 let open_right = self.grid_map.is_valid(x + 1, y);
300 let blocked_left = !self.grid_map.is_valid(x - 1, y - dir.dy);
301 let open_left = self.grid_map.is_valid(x - 1, y);
302 (blocked_right && open_right) || (blocked_left && open_left)
303 }
304 } else {
305 false
306 }
307 }
308
309 fn jump(&self, x: i32, y: i32, dir: Direction, goal_x: i32, goal_y: i32) -> Option<(i32, i32)> {
311 let nx = x + dir.dx;
312 let ny = y + dir.dy;
313
314 if !self.grid_map.is_valid_step(x, y, nx, ny) {
315 return None;
316 }
317
318 if nx == goal_x && ny == goal_y {
319 return Some((nx, ny));
320 }
321
322 if self.has_forced_neighbor(nx, ny, dir) {
323 return Some((nx, ny));
324 }
325
326 if dir.is_diagonal() {
327 if self
328 .jump(nx, ny, Direction::new(dir.dx, 0), goal_x, goal_y)
329 .is_some()
330 {
331 return Some((nx, ny));
332 }
333 if self
334 .jump(nx, ny, Direction::new(0, dir.dy), goal_x, goal_y)
335 .is_some()
336 {
337 return Some((nx, ny));
338 }
339 }
340
341 self.jump(nx, ny, dir, goal_x, goal_y)
342 }
343
344 fn get_successors(
345 &self,
346 x: i32,
347 y: i32,
348 parent_dir: Option<Direction>,
349 goal_x: i32,
350 goal_y: i32,
351 ) -> Vec<(i32, i32, Direction)> {
352 let mut successors = Vec::new();
353 let directions = self.get_neighbors(x, y, parent_dir);
354
355 for dir in directions {
356 if let Some((jx, jy)) = self.jump(x, y, dir, goal_x, goal_y) {
357 successors.push((jx, jy, dir));
358 }
359 }
360
361 successors
362 }
363
364 fn get_neighbors(&self, x: i32, y: i32, parent_dir: Option<Direction>) -> Vec<Direction> {
365 match parent_dir {
366 None => {
367 let mut dirs = Vec::new();
368 for dx in -1..=1 {
369 for dy in -1..=1 {
370 if dx == 0 && dy == 0 {
371 continue;
372 }
373 let dir = Direction::new(dx, dy);
374 if self.grid_map.is_valid_step(x, y, x + dx, y + dy) {
375 dirs.push(dir);
376 }
377 }
378 }
379 dirs
380 }
381 Some(dir) => {
382 let mut dirs = Vec::new();
383
384 if dir.is_diagonal() {
385 if self.grid_map.is_valid_step(x, y, x + dir.dx, y + dir.dy) {
386 dirs.push(dir);
387 }
388 if self.grid_map.is_valid_step(x, y, x + dir.dx, y) {
389 dirs.push(Direction::new(dir.dx, 0));
390 }
391 if self.grid_map.is_valid_step(x, y, x, y + dir.dy) {
392 dirs.push(Direction::new(0, dir.dy));
393 }
394
395 if !self.grid_map.is_valid(x - dir.dx, y) {
396 if self.grid_map.is_valid_step(x, y, x - dir.dx, y + dir.dy) {
397 dirs.push(Direction::new(-dir.dx, dir.dy));
398 }
399 }
400 if !self.grid_map.is_valid(x, y - dir.dy) {
401 if self.grid_map.is_valid_step(x, y, x + dir.dx, y - dir.dy) {
402 dirs.push(Direction::new(dir.dx, -dir.dy));
403 }
404 }
405 } else if dir.dx != 0 {
406 if self.grid_map.is_valid_step(x, y, x + dir.dx, y) {
407 dirs.push(dir);
408 }
409 let blocked_lower = !self.grid_map.is_valid(x - dir.dx, y + 1);
410 if blocked_lower {
411 if self.grid_map.is_valid_step(x, y, x + dir.dx, y + 1) {
412 dirs.push(Direction::new(dir.dx, 1));
413 }
414 if self.grid_map.is_valid_step(x, y, x, y + 1) {
415 dirs.push(Direction::new(0, 1));
416 }
417 }
418 let blocked_upper = !self.grid_map.is_valid(x - dir.dx, y - 1);
419 if blocked_upper {
420 if self.grid_map.is_valid_step(x, y, x + dir.dx, y - 1) {
421 dirs.push(Direction::new(dir.dx, -1));
422 }
423 if self.grid_map.is_valid_step(x, y, x, y - 1) {
424 dirs.push(Direction::new(0, -1));
425 }
426 }
427 } else {
428 if self.grid_map.is_valid_step(x, y, x, y + dir.dy) {
429 dirs.push(dir);
430 }
431 let blocked_right = !self.grid_map.is_valid(x + 1, y - dir.dy);
432 if blocked_right {
433 if self.grid_map.is_valid_step(x, y, x + 1, y + dir.dy) {
434 dirs.push(Direction::new(1, dir.dy));
435 }
436 if self.grid_map.is_valid_step(x, y, x + 1, y) {
437 dirs.push(Direction::new(1, 0));
438 }
439 }
440 let blocked_left = !self.grid_map.is_valid(x - 1, y - dir.dy);
441 if blocked_left {
442 if self.grid_map.is_valid_step(x, y, x - 1, y + dir.dy) {
443 dirs.push(Direction::new(-1, dir.dy));
444 }
445 if self.grid_map.is_valid_step(x, y, x - 1, y) {
446 dirs.push(Direction::new(-1, 0));
447 }
448 }
449 }
450
451 dirs
452 }
453 }
454 }
455
456 fn calc_distance(&self, x1: i32, y1: i32, x2: i32, y2: i32) -> f64 {
457 let dx = (x2 - x1).abs() as f64;
458 let dy = (y2 - y1).abs() as f64;
459 let d_min = dx.min(dy);
460 let d_max = dx.max(dy);
461 d_min * std::f64::consts::SQRT_2 + (d_max - d_min)
462 }
463
464 fn grid_point(&self, x: i32, y: i32) -> Point2D {
465 Point2D::new(
466 self.grid_map.calc_x_position(x),
467 self.grid_map.calc_y_position(y),
468 )
469 }
470
471 fn collect_jump_points(&self, goal_index: usize, node_storage: &[Node]) -> Vec<(i32, i32)> {
472 let mut jump_points = Vec::new();
473 let mut current_index = Some(goal_index);
474
475 while let Some(index) = current_index {
476 let node = &node_storage[index];
477 jump_points.push((node.x, node.y));
478 current_index = node.parent_index;
479 }
480
481 jump_points.reverse();
482 jump_points
483 }
484
485 fn append_reconstructed_segment(
486 &self,
487 points: &mut Vec<Point2D>,
488 from: (i32, i32),
489 to: (i32, i32),
490 ) -> f64 {
491 let (px, py) = from;
492 let (x, y) = to;
493 let dx = (x - px).signum();
494 let dy = (y - py).signum();
495 let mut cx = px;
496 let mut cy = py;
497 let mut reconstructed_length = 0.0;
498
499 while cx != x || cy != y {
500 let previous_x = cx;
501 let previous_y = cy;
502
503 if cx != x && cy != y {
504 if self.grid_map.is_valid_step(cx, cy, cx + dx, cy + dy) {
505 cx += dx;
506 cy += dy;
507 } else if self.grid_map.is_valid_step(cx, cy, cx + dx, cy) {
508 cx += dx;
509 } else if self.grid_map.is_valid_step(cx, cy, cx, cy + dy) {
510 cy += dy;
511 } else {
512 cx += dx;
513 cy += dy;
514 }
515 } else if cx != x {
516 cx += dx;
517 } else {
518 cy += dy;
519 }
520
521 reconstructed_length += self.calc_distance(previous_x, previous_y, cx, cy);
522 points.push(self.grid_point(cx, cy));
523 }
524
525 reconstructed_length
526 }
527
528 fn build_path_with_metrics(
529 &self,
530 goal_index: usize,
531 node_storage: &[Node],
532 ) -> (Path2D, Vec<JPSDetourSegment>) {
533 let jump_points = self.collect_jump_points(goal_index, node_storage);
534 let mut points = Vec::new();
535 let mut detour_segments = Vec::new();
536
537 if let Some(&(start_x, start_y)) = jump_points.first() {
538 points.push(self.grid_point(start_x, start_y));
539 }
540
541 for segment in jump_points.windows(2) {
542 let from = segment[0];
543 let to = segment[1];
544 let reconstructed_length = self.append_reconstructed_segment(&mut points, from, to);
545 let expected_length = self.calc_distance(from.0, from.1, to.0, to.1);
546 if (reconstructed_length - expected_length).abs() > 1e-6 {
547 detour_segments.push(JPSDetourSegment {
548 from_x: from.0,
549 from_y: from.1,
550 to_x: to.0,
551 to_y: to.1,
552 expected_length,
553 reconstructed_length,
554 });
555 }
556 }
557
558 (Path2D::from_points(points), detour_segments)
559 }
560
561 fn build_search_path(&self, goal_index: usize, node_storage: &[Node]) -> Path2D {
562 let points: Vec<Point2D> = self
563 .collect_jump_points(goal_index, node_storage)
564 .into_iter()
565 .map(|(x, y)| self.grid_point(x, y))
566 .collect();
567 Path2D::from_points(points)
568 }
569
570 fn cost_mismatch_metrics(
571 &self,
572 detail: Option<JPSInvalidJumpPathDetail>,
573 path: &Path2D,
574 search_cost: f64,
575 detour_segments: Vec<JPSDetourSegment>,
576 ) -> Option<JPSCostMismatchMetrics> {
577 match detail {
578 Some(JPSInvalidJumpPathDetail::CostMismatch)
579 | Some(JPSInvalidJumpPathDetail::InvalidStepSequenceAndCostMismatch) => {
580 let reconstructed_path_length = path.total_length();
581 Some(JPSCostMismatchMetrics {
582 search_cost,
583 reconstructed_path_length,
584 delta: reconstructed_path_length - search_cost,
585 detour_segment_count: detour_segments.len(),
586 detour_segments,
587 })
588 }
589 _ => None,
590 }
591 }
592
593 fn path_has_only_valid_steps(&self, path: &Path2D) -> bool {
594 path.points.windows(2).all(|segment| {
595 let from_x = self.grid_map.calc_x_index(segment[0].x);
596 let from_y = self.grid_map.calc_y_index(segment[0].y);
597 let to_x = self.grid_map.calc_x_index(segment[1].x);
598 let to_y = self.grid_map.calc_y_index(segment[1].y);
599 self.grid_map.is_valid_step(from_x, from_y, to_x, to_y)
600 })
601 }
602
603 fn classify_invalid_jump_path(
604 &self,
605 path: &Path2D,
606 expected_cost: f64,
607 ) -> Option<JPSInvalidJumpPathDetail> {
608 let has_only_valid_steps = self.path_has_only_valid_steps(path);
609 let expected_length = expected_cost * self.grid_map.resolution;
611 let has_cost_mismatch = (path.total_length() - expected_length).abs() > 1e-6;
612
613 match (has_only_valid_steps, has_cost_mismatch) {
614 (true, false) => None,
615 (false, false) => Some(JPSInvalidJumpPathDetail::InvalidStepSequence),
616 (true, true) => Some(JPSInvalidJumpPathDetail::CostMismatch),
617 (false, true) => Some(JPSInvalidJumpPathDetail::InvalidStepSequenceAndCostMismatch),
618 }
619 }
620
621 fn plan_with_grid_a_star(
622 &self,
623 start_x: i32,
624 start_y: i32,
625 goal_x: i32,
626 goal_y: i32,
627 ) -> RoboticsResult<Path2D> {
628 let mut open_set = BinaryHeap::new();
629 let mut closed_set: HashMap<i32, usize> = HashMap::new();
630 let mut node_storage: Vec<Node> = Vec::new();
631
632 node_storage.push(Node::new(start_x, start_y, 0.0, None));
633 open_set.push(PriorityNode {
634 x: start_x,
635 y: start_y,
636 cost: 0.0,
637 priority: self.calc_heuristic(start_x, start_y, goal_x, goal_y),
638 index: 0,
639 });
640
641 while let Some(current) = open_set.pop() {
642 let current_grid_index = self.grid_map.calc_index(current.x, current.y);
643
644 if current.x == goal_x && current.y == goal_y {
645 return Ok(self.build_search_path(current.index, &node_storage));
646 }
647
648 if let Some(&existing_index) = closed_set.get(¤t_grid_index) {
649 if node_storage[existing_index].cost <= current.cost {
650 continue;
651 }
652 }
653
654 closed_set.insert(current_grid_index, current.index);
655
656 for dx in -1..=1 {
657 for dy in -1..=1 {
658 if dx == 0 && dy == 0 {
659 continue;
660 }
661
662 if !self.grid_map.is_valid_offset(current.x, current.y, dx, dy) {
663 continue;
664 }
665
666 let new_x = current.x + dx;
667 let new_y = current.y + dy;
668 let move_cost = if dx != 0 && dy != 0 {
669 std::f64::consts::SQRT_2
670 } else {
671 1.0
672 };
673 let new_cost = current.cost + move_cost;
674 let new_grid_index = self.grid_map.calc_index(new_x, new_y);
675
676 if let Some(&existing_index) = closed_set.get(&new_grid_index) {
677 if node_storage[existing_index].cost <= new_cost {
678 continue;
679 }
680 }
681
682 node_storage.push(Node::new(new_x, new_y, new_cost, Some(current.index)));
683 let new_index = node_storage.len() - 1;
684 open_set.push(PriorityNode {
685 x: new_x,
686 y: new_y,
687 cost: new_cost,
688 priority: new_cost + self.calc_heuristic(new_x, new_y, goal_x, goal_y),
689 index: new_index,
690 });
691 }
692 }
693 }
694
695 Err(RoboticsError::PlanningError("No path found".to_string()))
696 }
697
698 fn direct_result(&self, path: Path2D) -> JPSPlanResult {
699 JPSPlanResult {
700 path,
701 diagnostics: JPSPlanDiagnostics {
702 used_fallback: false,
703 fallback_reason: None,
704 invalid_jump_path_detail: None,
705 cost_mismatch_metrics: None,
706 },
707 }
708 }
709
710 fn fallback_result_with_path(
711 &self,
712 path: Path2D,
713 fallback_reason: JPSFallbackReason,
714 invalid_jump_path_detail: Option<JPSInvalidJumpPathDetail>,
715 cost_mismatch_metrics: Option<JPSCostMismatchMetrics>,
716 ) -> JPSPlanResult {
717 JPSPlanResult {
718 path,
719 diagnostics: JPSPlanDiagnostics {
720 used_fallback: true,
721 fallback_reason: Some(fallback_reason),
722 invalid_jump_path_detail,
723 cost_mismatch_metrics,
724 },
725 }
726 }
727
728 #[allow(clippy::too_many_arguments)]
729 fn fallback_result(
730 &self,
731 start_x: i32,
732 start_y: i32,
733 goal_x: i32,
734 goal_y: i32,
735 fallback_reason: JPSFallbackReason,
736 invalid_jump_path_detail: Option<JPSInvalidJumpPathDetail>,
737 cost_mismatch_metrics: Option<JPSCostMismatchMetrics>,
738 ) -> RoboticsResult<JPSPlanResult> {
739 self.plan_with_grid_a_star(start_x, start_y, goal_x, goal_y)
740 .map(|path| {
741 self.fallback_result_with_path(
742 path,
743 fallback_reason,
744 invalid_jump_path_detail,
745 cost_mismatch_metrics,
746 )
747 })
748 }
749
750 fn ensure_query_is_valid(&self, x: i32, y: i32, label: &str) -> RoboticsResult<()> {
751 if self.grid_map.is_valid(x, y) {
752 return Ok(());
753 }
754
755 Err(RoboticsError::PlanningError(format!(
756 "{} position is invalid",
757 label
758 )))
759 }
760
761 fn plan_impl(&self, start: Point2D, goal: Point2D) -> RoboticsResult<JPSPlanResult> {
762 let start_x = self.grid_map.calc_x_index(start.x);
763 let start_y = self.grid_map.calc_y_index(start.y);
764 let goal_x = self.grid_map.calc_x_index(goal.x);
765 let goal_y = self.grid_map.calc_y_index(goal.y);
766
767 self.ensure_query_is_valid(start_x, start_y, "Start")?;
768 self.ensure_query_is_valid(goal_x, goal_y, "Goal")?;
769
770 let mut open_set = BinaryHeap::new();
771 let mut closed_set: HashMap<(i32, Option<Direction>), usize> = HashMap::new();
772 let mut node_storage: Vec<Node> = Vec::new();
773 let mut direction_map: HashMap<usize, Option<Direction>> = HashMap::new();
774
775 node_storage.push(Node::new(start_x, start_y, 0.0, None));
776 let start_index = 0;
777 direction_map.insert(start_index, None);
778
779 open_set.push(PriorityNode {
780 x: start_x,
781 y: start_y,
782 cost: 0.0,
783 priority: self.calc_heuristic(start_x, start_y, goal_x, goal_y),
784 index: start_index,
785 });
786
787 while let Some(current) = open_set.pop() {
788 let current_dir = direction_map.get(¤t.index).copied().flatten();
789 let current_state_key = self.state_key(current.x, current.y, current_dir);
790
791 if current.x == goal_x && current.y == goal_y {
792 let (path, detour_segments) =
793 self.build_path_with_metrics(current.index, &node_storage);
794 let invalid_jump_path_detail = self.classify_invalid_jump_path(&path, current.cost);
795 if invalid_jump_path_detail.is_none() {
796 let reference_path =
797 self.plan_with_grid_a_star(start_x, start_y, goal_x, goal_y)?;
798 if reference_path.total_length() + 1e-6 < path.total_length() {
799 return Ok(self.fallback_result_with_path(
800 reference_path,
801 JPSFallbackReason::SuboptimalJumpPath,
802 None,
803 None,
804 ));
805 }
806 return Ok(self.direct_result(path));
807 }
808 let cost_mismatch_metrics = self.cost_mismatch_metrics(
809 invalid_jump_path_detail,
810 &path,
811 current.cost,
812 detour_segments,
813 );
814
815 return self.fallback_result(
816 start_x,
817 start_y,
818 goal_x,
819 goal_y,
820 JPSFallbackReason::InvalidJumpPath,
821 invalid_jump_path_detail,
822 cost_mismatch_metrics,
823 );
824 }
825
826 if let Some(&existing_index) = closed_set.get(¤t_state_key) {
827 if node_storage[existing_index].cost <= current.cost {
828 continue;
829 }
830 }
831
832 closed_set.insert(current_state_key, current.index);
833
834 let successors = self.get_successors(current.x, current.y, current_dir, goal_x, goal_y);
835
836 for (jx, jy, dir) in successors {
837 let new_cost = current.cost + self.calc_distance(current.x, current.y, jx, jy);
838 let new_state_key = self.state_key(jx, jy, Some(dir));
839
840 if let Some(&existing_index) = closed_set.get(&new_state_key) {
841 if node_storage[existing_index].cost <= new_cost {
842 continue;
843 }
844 }
845
846 node_storage.push(Node::new(jx, jy, new_cost, Some(current.index)));
847 let new_index = node_storage.len() - 1;
848 direction_map.insert(new_index, Some(dir));
849
850 let priority = new_cost + self.calc_heuristic(jx, jy, goal_x, goal_y);
851 open_set.push(PriorityNode {
852 x: jx,
853 y: jy,
854 cost: new_cost,
855 priority,
856 index: new_index,
857 });
858 }
859 }
860
861 self.fallback_result(
862 start_x,
863 start_y,
864 goal_x,
865 goal_y,
866 JPSFallbackReason::SearchExhausted,
867 None,
868 None,
869 )
870 }
871}
872
873impl PathPlanner for JPSPlanner {
874 fn plan(&self, start: Point2D, goal: Point2D) -> Result<Path2D, RoboticsError> {
875 self.plan_with_diagnostics(start, goal)
876 .map(|result| result.path)
877 }
878}
879
880#[cfg(test)]
881mod tests {
882 #![allow(dead_code)]
883
884 use super::*;
885
886 use crate::a_star::{AStarConfig, AStarPlanner};
887 use crate::moving_ai::{MovingAiMap, MovingAiScenario};
888 use rust_robotics_core::Obstacles;
889 use std::collections::{BinaryHeap, HashMap, HashSet};
890 use std::hint::black_box;
891
892 #[test]
893 fn a_finer_grid_still_plans_as_pure_jps() {
894 let mut points = Vec::new();
897 for i in 0..=20 {
898 let t = i as f64 * 0.5;
899 for (x, y) in [(t, 0.0), (t, 10.0), (0.0, t), (10.0, t)] {
900 points.push(Point2D::new(x, y));
901 }
902 }
903 let planner = JPSPlanner::from_obstacle_points(
904 &Obstacles::from_points(points),
905 JPSConfig {
906 resolution: 0.5,
907 robot_radius: 0.5,
908 heuristic_weight: 1.0,
909 },
910 )
911 .expect("planner");
912 let result = planner
913 .plan_with_diagnostics(Point2D::new(1.0, 1.0), Point2D::new(8.0, 6.0))
914 .expect("path");
915 assert_eq!(result.diagnostics.fallback_reason, None);
916 assert!(!result.diagnostics.used_fallback);
917 }
918 use std::time::Instant;
919
920 #[derive(Debug)]
921 struct RawTraceSuccessor {
922 x: i32,
923 y: i32,
924 dir: Direction,
925 }
926
927 #[derive(Debug)]
928 struct RawTraceExpansion {
929 node_index: usize,
930 parent_index: Option<usize>,
931 x: i32,
932 y: i32,
933 cost: f64,
934 parent_dir: Option<Direction>,
935 successors: Vec<RawTraceSuccessor>,
936 }
937
938 #[derive(Debug)]
939 enum RawTraceOutcome {
940 GoalReached {
941 jump_points: Vec<(i32, i32)>,
942 path_length: f64,
943 search_cost: f64,
944 invalid_jump_path_detail: Option<JPSInvalidJumpPathDetail>,
945 },
946 SearchExhausted,
947 }
948
949 #[derive(Debug)]
950 struct RawTrace {
951 expansions: Vec<RawTraceExpansion>,
952 outcome: RawTraceOutcome,
953 }
954
955 #[derive(Debug)]
956 struct RawJumpChainStats {
957 segment_count: usize,
958 short_segment_count: usize,
959 direction_change_count: usize,
960 }
961
962 #[derive(Debug)]
963 struct NodeProbeCandidate {
964 dir: Direction,
965 jump_target: Option<(i32, i32)>,
966 }
967
968 fn trace_raw_jps(
969 planner: &JPSPlanner,
970 start: Point2D,
971 goal: Point2D,
972 max_expansions: usize,
973 ) -> RawTrace {
974 let start_x = planner.grid_map.calc_x_index(start.x);
975 let start_y = planner.grid_map.calc_y_index(start.y);
976 let goal_x = planner.grid_map.calc_x_index(goal.x);
977 let goal_y = planner.grid_map.calc_y_index(goal.y);
978
979 planner
980 .ensure_query_is_valid(start_x, start_y, "Start")
981 .expect("trace start should be valid");
982 planner
983 .ensure_query_is_valid(goal_x, goal_y, "Goal")
984 .expect("trace goal should be valid");
985
986 let mut open_set = BinaryHeap::new();
987 let mut closed_set: HashMap<(i32, Option<Direction>), usize> = HashMap::new();
988 let mut node_storage: Vec<Node> = Vec::new();
989 let mut direction_map: HashMap<usize, Option<Direction>> = HashMap::new();
990 let mut expansions = Vec::new();
991
992 node_storage.push(Node::new(start_x, start_y, 0.0, None));
993 direction_map.insert(0, None);
994 open_set.push(PriorityNode {
995 x: start_x,
996 y: start_y,
997 cost: 0.0,
998 priority: planner.calc_heuristic(start_x, start_y, goal_x, goal_y),
999 index: 0,
1000 });
1001
1002 while let Some(current) = open_set.pop() {
1003 let current_dir = direction_map.get(¤t.index).copied().flatten();
1004 let current_state_key = planner.state_key(current.x, current.y, current_dir);
1005
1006 if current.x == goal_x && current.y == goal_y {
1007 let (path, _) = planner.build_path_with_metrics(current.index, &node_storage);
1008 let invalid_jump_path_detail =
1009 planner.classify_invalid_jump_path(&path, current.cost);
1010 return RawTrace {
1011 expansions,
1012 outcome: RawTraceOutcome::GoalReached {
1013 jump_points: planner.collect_jump_points(current.index, &node_storage),
1014 path_length: path.total_length(),
1015 search_cost: current.cost,
1016 invalid_jump_path_detail,
1017 },
1018 };
1019 }
1020
1021 if let Some(&existing_index) = closed_set.get(¤t_state_key) {
1022 if node_storage[existing_index].cost <= current.cost {
1023 continue;
1024 }
1025 }
1026
1027 closed_set.insert(current_state_key, current.index);
1028
1029 let successors =
1030 planner.get_successors(current.x, current.y, current_dir, goal_x, goal_y);
1031 let trace_successors = successors
1032 .iter()
1033 .map(|(x, y, dir)| RawTraceSuccessor {
1034 x: *x,
1035 y: *y,
1036 dir: *dir,
1037 })
1038 .collect();
1039 expansions.push(RawTraceExpansion {
1040 node_index: current.index,
1041 parent_index: node_storage[current.index].parent_index,
1042 x: current.x,
1043 y: current.y,
1044 cost: current.cost,
1045 parent_dir: current_dir,
1046 successors: trace_successors,
1047 });
1048 if expansions.len() >= max_expansions {
1049 return RawTrace {
1050 expansions,
1051 outcome: RawTraceOutcome::SearchExhausted,
1052 };
1053 }
1054
1055 for (jx, jy, dir) in successors {
1056 let new_cost = current.cost + planner.calc_distance(current.x, current.y, jx, jy);
1057 let new_state_key = planner.state_key(jx, jy, Some(dir));
1058
1059 if let Some(&existing_index) = closed_set.get(&new_state_key) {
1060 if node_storage[existing_index].cost <= new_cost {
1061 continue;
1062 }
1063 }
1064
1065 node_storage.push(Node::new(jx, jy, new_cost, Some(current.index)));
1066 let new_index = node_storage.len() - 1;
1067 direction_map.insert(new_index, Some(dir));
1068 open_set.push(PriorityNode {
1069 x: jx,
1070 y: jy,
1071 cost: new_cost,
1072 priority: new_cost + planner.calc_heuristic(jx, jy, goal_x, goal_y),
1073 index: new_index,
1074 });
1075 }
1076 }
1077
1078 RawTrace {
1079 expansions,
1080 outcome: RawTraceOutcome::SearchExhausted,
1081 }
1082 }
1083
1084 fn trace_chain_to_root(
1085 expansions: &[RawTraceExpansion],
1086 terminal_node_index: usize,
1087 ) -> Vec<&RawTraceExpansion> {
1088 let expansion_index_by_node: HashMap<usize, usize> = expansions
1089 .iter()
1090 .enumerate()
1091 .map(|(ix, expansion)| (expansion.node_index, ix))
1092 .collect();
1093
1094 let mut chain = Vec::new();
1095 let mut current = Some(terminal_node_index);
1096 while let Some(node_index) = current {
1097 let Some(&expansion_ix) = expansion_index_by_node.get(&node_index) else {
1098 break;
1099 };
1100 let expansion = &expansions[expansion_ix];
1101 chain.push(expansion);
1102 current = expansion.parent_index;
1103 }
1104 chain.reverse();
1105 chain
1106 }
1107
1108 fn summarize_jump_chain(jump_points: &[(i32, i32)]) -> RawJumpChainStats {
1109 if jump_points.len() < 2 {
1110 return RawJumpChainStats {
1111 segment_count: 0,
1112 short_segment_count: 0,
1113 direction_change_count: 0,
1114 };
1115 }
1116
1117 let mut short_segment_count = 0;
1118 let mut directions = Vec::new();
1119
1120 for segment in jump_points.windows(2) {
1121 let dx = segment[1].0 - segment[0].0;
1122 let dy = segment[1].1 - segment[0].1;
1123 let length = ((dx * dx + dy * dy) as f64).sqrt();
1124 if length <= std::f64::consts::SQRT_2 + 1e-6 {
1125 short_segment_count += 1;
1126 }
1127 directions.push((dx.signum(), dy.signum()));
1128 }
1129
1130 let direction_change_count = directions
1131 .windows(2)
1132 .filter(|pair| pair[0] != pair[1])
1133 .count();
1134
1135 RawJumpChainStats {
1136 segment_count: jump_points.len() - 1,
1137 short_segment_count,
1138 direction_change_count,
1139 }
1140 }
1141
1142 fn probe_node(
1143 planner: &JPSPlanner,
1144 x: i32,
1145 y: i32,
1146 parent_dir: Option<Direction>,
1147 goal_x: i32,
1148 goal_y: i32,
1149 ) -> Vec<NodeProbeCandidate> {
1150 planner
1151 .get_neighbors(x, y, parent_dir)
1152 .into_iter()
1153 .map(|dir| NodeProbeCandidate {
1154 jump_target: planner.jump(x, y, dir, goal_x, goal_y),
1155 dir,
1156 })
1157 .collect()
1158 }
1159
1160 fn densest_direction_change_window(
1161 jump_points: &[(i32, i32)],
1162 window_segments: usize,
1163 ) -> Option<(usize, usize, usize)> {
1164 if jump_points.len() < window_segments + 1 || window_segments < 2 {
1165 return None;
1166 }
1167
1168 let directions: Vec<(i32, i32)> = jump_points
1169 .windows(2)
1170 .map(|segment| {
1171 (
1172 (segment[1].0 - segment[0].0).signum(),
1173 (segment[1].1 - segment[0].1).signum(),
1174 )
1175 })
1176 .collect();
1177
1178 let mut best: Option<(usize, usize, usize)> = None;
1179 for start in 0..=directions.len() - window_segments {
1180 let end = start + window_segments;
1181 let change_count = directions[start..end]
1182 .windows(2)
1183 .filter(|pair| pair[0] != pair[1])
1184 .count();
1185 if best
1186 .map(|(_, _, best_count)| change_count > best_count)
1187 .unwrap_or(true)
1188 {
1189 best = Some((start, end, change_count));
1190 }
1191 }
1192 best
1193 }
1194
1195 fn first_short_segment_run(
1196 jump_points: &[(i32, i32)],
1197 min_run_len: usize,
1198 ) -> Option<(usize, usize)> {
1199 if jump_points.len() < 2 {
1200 return None;
1201 }
1202
1203 let mut run_start = None;
1204 let mut run_len = 0usize;
1205 for (ix, segment) in jump_points.windows(2).enumerate() {
1206 let dx = segment[1].0 - segment[0].0;
1207 let dy = segment[1].1 - segment[0].1;
1208 let length = ((dx * dx + dy * dy) as f64).sqrt();
1209 if length <= std::f64::consts::SQRT_2 + 1e-6 {
1210 if run_start.is_none() {
1211 run_start = Some(ix);
1212 }
1213 run_len += 1;
1214 if run_len >= min_run_len {
1215 return run_start.map(|start| (start, run_len));
1216 }
1217 } else {
1218 run_start = None;
1219 run_len = 0;
1220 }
1221 }
1222 None
1223 }
1224
1225 fn grid_points_from_path(planner: &JPSPlanner, path: &Path2D) -> Vec<(i32, i32)> {
1226 path.points
1227 .iter()
1228 .map(|point| {
1229 (
1230 planner.grid_map.calc_x_index(point.x),
1231 planner.grid_map.calc_y_index(point.y),
1232 )
1233 })
1234 .collect()
1235 }
1236
1237 fn turn_points_from_grid_path(points: &[(i32, i32)]) -> Vec<(i32, i32)> {
1238 if points.len() < 2 {
1239 return points.to_vec();
1240 }
1241
1242 let mut turn_points = vec![points[0]];
1243 let mut last_dir = (
1244 (points[1].0 - points[0].0).signum(),
1245 (points[1].1 - points[0].1).signum(),
1246 );
1247
1248 for window in points.windows(3) {
1249 let current_dir = (
1250 (window[2].0 - window[1].0).signum(),
1251 (window[2].1 - window[1].1).signum(),
1252 );
1253 if current_dir != last_dir {
1254 turn_points.push(window[1]);
1255 last_dir = current_dir;
1256 }
1257 }
1258
1259 turn_points.push(*points.last().unwrap());
1260 turn_points
1261 }
1262
1263 fn first_missing_reference_turn_point(
1264 reference_turn_points: &[(i32, i32)],
1265 raw_trace: &RawTrace,
1266 ) -> Option<(usize, (i32, i32), bool, bool)> {
1267 let expanded_positions: HashSet<(i32, i32)> = raw_trace
1268 .expansions
1269 .iter()
1270 .map(|expansion| (expansion.x, expansion.y))
1271 .collect();
1272 let generated_positions: HashSet<(i32, i32)> = raw_trace
1273 .expansions
1274 .iter()
1275 .flat_map(|expansion| expansion.successors.iter().map(|succ| (succ.x, succ.y)))
1276 .collect();
1277
1278 reference_turn_points
1279 .iter()
1280 .enumerate()
1281 .skip(1)
1282 .find_map(|(ix, &point)| {
1283 let expanded = expanded_positions.contains(&point);
1284 let generated = generated_positions.contains(&point);
1285 if expanded || generated {
1286 None
1287 } else {
1288 Some((ix, point, expanded, generated))
1289 }
1290 })
1291 }
1292
1293 #[allow(clippy::type_complexity)]
1294 fn first_divergent_turn_point(
1295 reference_turn_points: &[(i32, i32)],
1296 raw_jump_points: &[(i32, i32)],
1297 ) -> Option<(usize, Option<(i32, i32)>, Option<(i32, i32)>)> {
1298 let max_len = reference_turn_points.len().max(raw_jump_points.len());
1299 for ix in 0..max_len {
1300 let reference_point = reference_turn_points.get(ix).copied();
1301 let raw_point = raw_jump_points.get(ix).copied();
1302 if reference_point != raw_point {
1303 return Some((ix, reference_point, raw_point));
1304 }
1305 }
1306 None
1307 }
1308
1309 fn create_simple_obstacles() -> (Vec<f64>, Vec<f64>) {
1310 let mut ox = Vec::new();
1311 let mut oy = Vec::new();
1312
1313 for i in 0..61 {
1314 ox.push(i as f64);
1315 oy.push(0.0);
1316 ox.push(i as f64);
1317 oy.push(60.0);
1318 ox.push(0.0);
1319 oy.push(i as f64);
1320 ox.push(60.0);
1321 oy.push(i as f64);
1322 }
1323
1324 for i in 20..40 {
1325 ox.push(30.0);
1326 oy.push(i as f64);
1327 }
1328
1329 (ox, oy)
1330 }
1331
1332 const MOVING_AI_CASES: [(&str, &str, &str); 3] = [
1333 (
1334 "arena2",
1335 include_str!("../benchdata/moving_ai/dao/arena2.map"),
1336 include_str!("../benchdata/moving_ai/dao/arena2.map.scen"),
1337 ),
1338 (
1339 "8room_000",
1340 include_str!("../benchdata/moving_ai/room/8room_000.map"),
1341 include_str!("../benchdata/moving_ai/room/8room_000.map.scen"),
1342 ),
1343 (
1344 "random512-10-0",
1345 include_str!("../benchdata/moving_ai/random/random512-10-0.map"),
1346 include_str!("../benchdata/moving_ai/random/random512-10-0.map.scen"),
1347 ),
1348 ];
1349
1350 fn assert_pure_jps_matches_moving_ai_bucket(
1351 name: &str,
1352 map_str: &str,
1353 scen_str: &str,
1354 bucket: u32,
1355 ) {
1356 let map = MovingAiMap::parse_str(map_str)
1357 .unwrap_or_else(|_| panic!("{name} MovingAI map should parse"));
1358 let scenario = MovingAiScenario::parse_str(scen_str)
1359 .unwrap_or_else(|_| panic!("{name} MovingAI scenarios should parse"))
1360 .into_iter()
1361 .find(|row| row.bucket == bucket)
1362 .unwrap_or_else(|| panic!("{name} MovingAI bucket {bucket} scenario should exist"));
1363
1364 let planner = JPSPlanner::from_obstacle_points(
1365 &map.to_obstacles(),
1366 JPSConfig {
1367 resolution: 1.0,
1368 robot_radius: 0.5,
1369 heuristic_weight: 1.0,
1370 },
1371 )
1372 .unwrap_or_else(|_| panic!("JPS planner should build from {name} obstacles"));
1373
1374 let start = map
1375 .planning_point(scenario.start_x, scenario.start_y)
1376 .unwrap_or_else(|_| panic!("{name} start should be valid"));
1377 let goal = map
1378 .planning_point(scenario.goal_x, scenario.goal_y)
1379 .unwrap_or_else(|_| panic!("{name} goal should be valid"));
1380
1381 let result = planner
1382 .plan_with_diagnostics(start, goal)
1383 .unwrap_or_else(|_| panic!("JPS should solve the {name} bucket {bucket} scenario"));
1384
1385 assert!(
1386 !result.path.is_empty(),
1387 "{name} bucket {bucket} path should not be empty"
1388 );
1389 println!(
1390 "{name}/bucket{bucket}: used_fallback={}, reason={:?}, detail={:?}, path_length={:.6}",
1391 result.diagnostics.used_fallback,
1392 result.diagnostics.fallback_reason,
1393 result.diagnostics.invalid_jump_path_detail,
1394 result.path.total_length()
1395 );
1396 assert!(
1397 !result.diagnostics.used_fallback,
1398 "{name} bucket {bucket} should solve as pure JPS, got reason={:?}, detail={:?}, path_length={:.6}",
1399 result.diagnostics.fallback_reason,
1400 result.diagnostics.invalid_jump_path_detail,
1401 result.path.total_length()
1402 );
1403 assert_eq!(
1404 result.diagnostics.fallback_reason, None,
1405 "{name} bucket {bucket} pure-JPS regression should not report a fallback reason"
1406 );
1407 assert_eq!(
1408 result.diagnostics.invalid_jump_path_detail,
1409 None,
1410 "{name} bucket {bucket} pure-JPS regression should not report an invalid_jump_path detail"
1411 );
1412 assert!(
1413 (result.path.total_length() - scenario.optimal_length).abs() <= 1e-6,
1414 "{name} bucket {bucket} pure JPS path length {} should match MovingAI optimal {}",
1415 result.path.total_length(),
1416 scenario.optimal_length
1417 );
1418
1419 let grid = planner.grid_map();
1420 for segment in result.path.points.windows(2) {
1421 let from_x = grid.calc_x_index(segment[0].x);
1422 let from_y = grid.calc_y_index(segment[0].y);
1423 let to_x = grid.calc_x_index(segment[1].x);
1424 let to_y = grid.calc_y_index(segment[1].y);
1425 assert!(
1426 grid.is_valid_step(from_x, from_y, to_x, to_y),
1427 "{name} bucket {bucket} path contains an invalid step from ({from_x}, {from_y}) to ({to_x}, {to_y})"
1428 );
1429 }
1430 }
1431
1432 fn build_moving_ai_bucket_planners(
1433 name: &str,
1434 map_str: &str,
1435 scen_str: &str,
1436 bucket: u32,
1437 ) -> (AStarPlanner, JPSPlanner, Point2D, Point2D, f64) {
1438 let map = MovingAiMap::parse_str(map_str)
1439 .unwrap_or_else(|_| panic!("{name} MovingAI map should parse"));
1440 let scenario = MovingAiScenario::parse_str(scen_str)
1441 .unwrap_or_else(|_| panic!("{name} MovingAI scenarios should parse"))
1442 .into_iter()
1443 .find(|row| row.bucket == bucket)
1444 .unwrap_or_else(|| panic!("{name} MovingAI bucket {bucket} scenario should exist"));
1445 let obstacles = map.to_obstacles();
1446 let a_star = AStarPlanner::from_obstacle_points(
1447 &obstacles,
1448 AStarConfig {
1449 resolution: 1.0,
1450 robot_radius: 0.5,
1451 heuristic_weight: 1.0,
1452 },
1453 )
1454 .unwrap_or_else(|_| panic!("A* planner should build from {name} obstacles"));
1455 let jps = JPSPlanner::from_obstacle_points(
1456 &obstacles,
1457 JPSConfig {
1458 resolution: 1.0,
1459 robot_radius: 0.5,
1460 heuristic_weight: 1.0,
1461 },
1462 )
1463 .unwrap_or_else(|_| panic!("JPS planner should build from {name} obstacles"));
1464 let start = map
1465 .planning_point(scenario.start_x, scenario.start_y)
1466 .unwrap_or_else(|_| panic!("{name} start should be valid"));
1467 let goal = map
1468 .planning_point(scenario.goal_x, scenario.goal_y)
1469 .unwrap_or_else(|_| panic!("{name} goal should be valid"));
1470
1471 (a_star, jps, start, goal, scenario.optimal_length)
1472 }
1473
1474 fn build_moving_ai_family_planners(
1475 name: &str,
1476 map_str: &str,
1477 scen_str: &str,
1478 ) -> (MovingAiMap, Vec<MovingAiScenario>, AStarPlanner, JPSPlanner) {
1479 let map = MovingAiMap::parse_str(map_str)
1480 .unwrap_or_else(|_| panic!("{name} MovingAI map should parse"));
1481 let scenarios = MovingAiScenario::parse_str(scen_str)
1482 .unwrap_or_else(|_| panic!("{name} MovingAI scenarios should parse"));
1483 let obstacles = map.to_obstacles();
1484 let a_star = AStarPlanner::from_obstacle_points(
1485 &obstacles,
1486 AStarConfig {
1487 resolution: 1.0,
1488 robot_radius: 0.5,
1489 heuristic_weight: 1.0,
1490 },
1491 )
1492 .unwrap_or_else(|_| panic!("A* planner should build from {name} obstacles"));
1493 let jps = JPSPlanner::from_obstacle_points(
1494 &obstacles,
1495 JPSConfig {
1496 resolution: 1.0,
1497 robot_radius: 0.5,
1498 heuristic_weight: 1.0,
1499 },
1500 )
1501 .unwrap_or_else(|_| panic!("JPS planner should build from {name} obstacles"));
1502
1503 (map, scenarios, a_star, jps)
1504 }
1505
1506 fn build_sampled_moving_ai_bucket_cases(
1507 name: &str,
1508 map: &MovingAiMap,
1509 scenarios: &[MovingAiScenario],
1510 bucket: u32,
1511 sample_slots: &[usize],
1512 ) -> Vec<(usize, Point2D, Point2D, f64)> {
1513 let bucket_scenarios: Vec<&MovingAiScenario> = scenarios
1514 .iter()
1515 .filter(|row| row.bucket == bucket)
1516 .collect();
1517 assert!(
1518 !bucket_scenarios.is_empty(),
1519 "{name} MovingAI bucket {bucket} scenarios should exist"
1520 );
1521 let max_slot = sample_slots.iter().copied().max().unwrap_or(0);
1522 assert!(
1523 bucket_scenarios.len() > max_slot,
1524 "{name} bucket {bucket} should have at least {} scenarios, found {}",
1525 max_slot + 1,
1526 bucket_scenarios.len()
1527 );
1528
1529 sample_slots
1530 .iter()
1531 .map(|&slot| {
1532 let scenario = bucket_scenarios[slot];
1533 let start = map
1534 .planning_point(scenario.start_x, scenario.start_y)
1535 .unwrap_or_else(|_| panic!("{name} start should be valid"));
1536 let goal = map
1537 .planning_point(scenario.goal_x, scenario.goal_y)
1538 .unwrap_or_else(|_| panic!("{name} goal should be valid"));
1539 (slot, start, goal, scenario.optimal_length)
1540 })
1541 .collect()
1542 }
1543
1544 fn measure_planner_median_us<F>(iterations: usize, mut plan: F) -> f64
1545 where
1546 F: FnMut() -> Path2D,
1547 {
1548 let mut samples = Vec::with_capacity(iterations);
1549 for _ in 0..iterations {
1550 let started = Instant::now();
1551 let path = plan();
1552 black_box(path.points.len());
1553 black_box(path.total_length());
1554 samples.push(started.elapsed().as_secs_f64() * 1_000_000.0);
1555 }
1556 samples.sort_by(|a, b| a.partial_cmp(b).unwrap());
1557 samples[iterations / 2]
1558 }
1559
1560 fn median_value(mut samples: Vec<f64>) -> f64 {
1561 assert!(
1562 !samples.is_empty(),
1563 "median_value requires at least one sample"
1564 );
1565 samples.sort_by(|a, b| a.partial_cmp(b).unwrap());
1566 samples[samples.len() / 2]
1567 }
1568
1569 #[derive(Debug, Clone, Copy)]
1570 struct BucketRuntimeAggregate {
1571 a_star_median_us: f64,
1572 jps_median_us: f64,
1573 a_star_min_us: f64,
1574 a_star_max_us: f64,
1575 jps_min_us: f64,
1576 jps_max_us: f64,
1577 jps_wins: usize,
1578 sample_count: usize,
1579 }
1580
1581 #[allow(clippy::too_many_arguments)]
1582 fn measure_moving_ai_bucket_runtime_aggregate(
1583 name: &str,
1584 map: &MovingAiMap,
1585 scenarios: &[MovingAiScenario],
1586 a_star: &AStarPlanner,
1587 jps: &JPSPlanner,
1588 bucket: u32,
1589 sample_slots: &[usize],
1590 iterations: usize,
1591 ) -> BucketRuntimeAggregate {
1592 let sampled_cases =
1593 build_sampled_moving_ai_bucket_cases(name, map, scenarios, bucket, sample_slots);
1594 let mut a_star_samples = Vec::with_capacity(sampled_cases.len());
1595 let mut jps_samples = Vec::with_capacity(sampled_cases.len());
1596 let mut jps_wins = 0usize;
1597
1598 for (slot, start, goal, optimal_length) in sampled_cases {
1599 let warmup_a = a_star
1600 .plan(start, goal)
1601 .unwrap_or_else(|_| panic!("A* should solve {name} bucket {bucket} slot {slot}"));
1602 assert!(
1603 (warmup_a.total_length() - optimal_length).abs() <= 1e-6,
1604 "A* warmup path for {name} bucket {bucket} slot {slot} should match MovingAI optimal {}",
1605 optimal_length
1606 );
1607 let warmup_j = jps
1608 .plan(start, goal)
1609 .unwrap_or_else(|_| panic!("JPS should solve {name} bucket {bucket} slot {slot}"));
1610 assert!(
1611 (warmup_j.total_length() - optimal_length).abs() <= 1e-6,
1612 "JPS warmup path for {name} bucket {bucket} slot {slot} should match MovingAI optimal {}",
1613 optimal_length
1614 );
1615
1616 let a_star_median_us = measure_planner_median_us(iterations, || {
1617 let path = a_star.plan(start, goal).unwrap_or_else(|_| {
1618 panic!("A* should solve {name} bucket {bucket} slot {slot}")
1619 });
1620 assert!(
1621 (path.total_length() - optimal_length).abs() <= 1e-6,
1622 "A* path length {} should match MovingAI optimal {} on {name} bucket {bucket} slot {slot}",
1623 path.total_length(),
1624 optimal_length
1625 );
1626 path
1627 });
1628 let jps_median_us = measure_planner_median_us(iterations, || {
1629 let path = jps.plan(start, goal).unwrap_or_else(|_| {
1630 panic!("JPS should solve {name} bucket {bucket} slot {slot}")
1631 });
1632 assert!(
1633 (path.total_length() - optimal_length).abs() <= 1e-6,
1634 "JPS path length {} should match MovingAI optimal {} on {name} bucket {bucket} slot {slot}",
1635 path.total_length(),
1636 optimal_length
1637 );
1638 path
1639 });
1640 if jps_median_us < a_star_median_us {
1641 jps_wins += 1;
1642 }
1643 a_star_samples.push(a_star_median_us);
1644 jps_samples.push(jps_median_us);
1645 }
1646
1647 let a_star_median_us = median_value(a_star_samples.clone());
1648 let jps_median_us = median_value(jps_samples.clone());
1649 let a_star_min_us = a_star_samples.iter().copied().fold(f64::INFINITY, f64::min);
1650 let a_star_max_us = a_star_samples
1651 .iter()
1652 .copied()
1653 .fold(f64::NEG_INFINITY, f64::max);
1654 let jps_min_us = jps_samples.iter().copied().fold(f64::INFINITY, f64::min);
1655 let jps_max_us = jps_samples
1656 .iter()
1657 .copied()
1658 .fold(f64::NEG_INFINITY, f64::max);
1659
1660 BucketRuntimeAggregate {
1661 a_star_median_us,
1662 jps_median_us,
1663 a_star_min_us,
1664 a_star_max_us,
1665 jps_min_us,
1666 jps_max_us,
1667 jps_wins,
1668 sample_count: sample_slots.len(),
1669 }
1670 }
1671
1672 fn create_small_obstacles() -> (Vec<f64>, Vec<f64>) {
1673 let mut ox = Vec::new();
1674 let mut oy = Vec::new();
1675
1676 for i in 0..11 {
1677 ox.push(i as f64);
1678 oy.push(0.0);
1679 ox.push(i as f64);
1680 oy.push(10.0);
1681 ox.push(0.0);
1682 oy.push(i as f64);
1683 ox.push(10.0);
1684 oy.push(i as f64);
1685 }
1686
1687 for i in 4..7 {
1688 ox.push(5.0);
1689 oy.push(i as f64);
1690 }
1691
1692 (ox, oy)
1693 }
1694
1695 fn draw_horizontal_line(
1696 start_x: i32,
1697 start_y: i32,
1698 length: i32,
1699 ox: &mut Vec<f64>,
1700 oy: &mut Vec<f64>,
1701 ) {
1702 for i in start_x..start_x + length {
1703 for j in start_y..start_y + 2 {
1704 ox.push(i as f64);
1705 oy.push(j as f64);
1706 }
1707 }
1708 }
1709
1710 fn draw_vertical_line(
1711 start_x: i32,
1712 start_y: i32,
1713 length: i32,
1714 ox: &mut Vec<f64>,
1715 oy: &mut Vec<f64>,
1716 ) {
1717 for i in start_x..start_x + 2 {
1718 for j in start_y..start_y + length {
1719 ox.push(i as f64);
1720 oy.push(j as f64);
1721 }
1722 }
1723 }
1724
1725 fn create_upstream_jump_point_maze() -> (Vec<f64>, Vec<f64>) {
1726 let mut ox = Vec::new();
1727 let mut oy = Vec::new();
1728
1729 draw_vertical_line(0, 0, 50, &mut ox, &mut oy);
1730 draw_vertical_line(48, 0, 50, &mut ox, &mut oy);
1731 draw_horizontal_line(0, 0, 50, &mut ox, &mut oy);
1732 draw_horizontal_line(0, 48, 50, &mut ox, &mut oy);
1733
1734 let vertical_x = [10, 10, 10, 15, 20, 20, 30, 30, 35, 30, 40, 45];
1735 let vertical_y = [10, 30, 45, 20, 5, 40, 10, 40, 5, 40, 10, 25];
1736 let vertical_len = [10, 10, 5, 10, 10, 5, 20, 10, 25, 10, 35, 15];
1737 for ((x, y), len) in vertical_x
1738 .iter()
1739 .zip(vertical_y.iter())
1740 .zip(vertical_len.iter())
1741 {
1742 draw_vertical_line(*x, *y, *len, &mut ox, &mut oy);
1743 }
1744
1745 let horizontal_x = [35, 40, 15, 10, 45, 20, 10, 15, 25, 45, 10, 30, 10, 40];
1746 let horizontal_y = [5, 10, 15, 20, 20, 25, 30, 35, 35, 35, 40, 40, 45, 45];
1747 let horizontal_len = [10, 5, 10, 10, 5, 5, 10, 5, 10, 5, 10, 5, 5, 5];
1748 for ((x, y), len) in horizontal_x
1749 .iter()
1750 .zip(horizontal_y.iter())
1751 .zip(horizontal_len.iter())
1752 {
1753 draw_horizontal_line(*x, *y, *len, &mut ox, &mut oy);
1754 }
1755
1756 (ox, oy)
1757 }
1758
1759 fn upstream_jump_point_reference_polyline() -> Vec<Point2D> {
1760 vec![
1761 Point2D::new(5.0, 5.0),
1762 Point2D::new(7.0, 6.0),
1763 Point2D::new(9.0, 10.0),
1764 Point2D::new(9.0, 15.0),
1765 Point2D::new(9.0, 20.0),
1766 Point2D::new(10.0, 22.0),
1767 Point2D::new(11.0, 26.0),
1768 Point2D::new(10.0, 29.0),
1769 Point2D::new(9.0, 33.0),
1770 Point2D::new(9.0, 38.0),
1771 Point2D::new(10.0, 42.0),
1772 Point2D::new(14.0, 44.0),
1773 Point2D::new(19.0, 44.0),
1774 Point2D::new(22.0, 47.0),
1775 Point2D::new(25.0, 45.0),
1776 Point2D::new(29.0, 43.0),
1777 Point2D::new(29.0, 40.0),
1778 Point2D::new(32.0, 39.0),
1779 Point2D::new(35.0, 40.0),
1780 Point2D::new(36.0, 44.0),
1781 Point2D::new(35.0, 45.0),
1782 ]
1783 }
1784
1785 fn polyline_length(points: &[Point2D]) -> f64 {
1786 points
1787 .windows(2)
1788 .map(|window| window[0].distance(&window[1]))
1789 .sum()
1790 }
1791
1792 #[test]
1793 fn test_jps_finds_path() {
1794 let (ox, oy) = create_simple_obstacles();
1795 let planner = JPSPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
1796
1797 let start = Point2D::new(10.0, 10.0);
1798 let goal = Point2D::new(50.0, 50.0);
1799
1800 let result = planner.plan(start, goal);
1801 assert!(result.is_ok());
1802
1803 let path = result.unwrap();
1804 assert!(!path.is_empty());
1805
1806 let first = &path.points[0];
1807 let last = &path.points[path.len() - 1];
1808 assert!((first.x - 10.0).abs() < 2.0);
1809 assert!((first.y - 10.0).abs() < 2.0);
1810 assert!((last.x - 50.0).abs() < 2.0);
1811 assert!((last.y - 50.0).abs() < 2.0);
1812 }
1813
1814 #[test]
1815 fn test_jps_small_map() {
1816 let (ox, oy) = create_small_obstacles();
1817 let planner = JPSPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
1818
1819 let start = Point2D::new(2.0, 2.0);
1820 let goal = Point2D::new(8.0, 8.0);
1821
1822 let result = planner.plan(start, goal);
1823 assert!(result.is_ok());
1824
1825 let path = result.unwrap();
1826 assert!(!path.is_empty());
1827 }
1828
1829 #[test]
1830 fn test_jps_diagnostics_are_consistent_on_small_map() {
1831 let (ox, oy) = create_small_obstacles();
1832 let planner = JPSPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
1833
1834 let result = planner
1835 .plan_with_diagnostics(Point2D::new(2.0, 2.0), Point2D::new(8.0, 8.0))
1836 .expect("small-map JPS query should succeed");
1837
1838 assert!(!result.path.is_empty());
1839 assert_eq!(
1840 result.diagnostics.used_fallback,
1841 result.diagnostics.fallback_reason.is_some()
1842 );
1843 if result.diagnostics.fallback_reason != Some(JPSFallbackReason::InvalidJumpPath) {
1844 assert_eq!(result.diagnostics.invalid_jump_path_detail, None);
1845 assert_eq!(result.diagnostics.cost_mismatch_metrics, None);
1846 }
1847 if !matches!(
1848 result.diagnostics.invalid_jump_path_detail,
1849 Some(JPSInvalidJumpPathDetail::CostMismatch)
1850 | Some(JPSInvalidJumpPathDetail::InvalidStepSequenceAndCostMismatch)
1851 ) {
1852 assert_eq!(result.diagnostics.cost_mismatch_metrics, None);
1853 }
1854 }
1855
1856 #[test]
1857 #[allow(deprecated)]
1858 fn test_jps_legacy_interface() {
1859 let (ox, oy) = create_small_obstacles();
1860 let planner = JPSPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
1861
1862 let result = planner.planning(2.0, 2.0, 8.0, 8.0);
1863 assert!(result.is_some());
1864
1865 let (rx, ry) = result.unwrap();
1866 assert!(!rx.is_empty());
1867 assert_eq!(rx.len(), ry.len());
1868 }
1869
1870 #[test]
1871 fn test_jps_no_path() {
1872 let mut ox = Vec::new();
1873 let mut oy = Vec::new();
1874
1875 for i in 0..10 {
1876 ox.push(i as f64);
1877 oy.push(0.0);
1878 ox.push(i as f64);
1879 oy.push(9.0);
1880 ox.push(0.0);
1881 oy.push(i as f64);
1882 ox.push(9.0);
1883 oy.push(i as f64);
1884 }
1885
1886 for i in 1..9 {
1887 ox.push(5.0);
1888 oy.push(i as f64);
1889 }
1890
1891 let planner = JPSPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
1892
1893 let start = Point2D::new(2.0, 5.0);
1894 let goal = Point2D::new(7.0, 5.0);
1895
1896 let result = planner.plan(start, goal);
1897 assert!(result.is_err());
1898 }
1899
1900 #[test]
1901 fn test_jps_diagonal_path() {
1902 let mut ox = Vec::new();
1903 let mut oy = Vec::new();
1904
1905 for i in 0..21 {
1906 ox.push(i as f64);
1907 oy.push(0.0);
1908 ox.push(i as f64);
1909 oy.push(20.0);
1910 ox.push(0.0);
1911 oy.push(i as f64);
1912 ox.push(20.0);
1913 oy.push(i as f64);
1914 }
1915
1916 let planner = JPSPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);
1917
1918 let start = Point2D::new(2.0, 2.0);
1919 let goal = Point2D::new(18.0, 18.0);
1920
1921 let result = planner.plan(start, goal);
1922 assert!(result.is_ok());
1923
1924 let path = result.unwrap();
1925 assert!(path.len() >= 2, "Path should have at least start and goal");
1926 let total_len = path.total_length();
1927 assert!(
1928 total_len < 30.0,
1929 "Path should be efficient, got length {}",
1930 total_len
1931 );
1932 }
1933
1934 #[test]
1935 fn test_jps_from_obstacle_points() {
1936 let (ox, oy) = create_small_obstacles();
1937 let obstacles = Obstacles::try_from_xy(&ox, &oy).unwrap();
1938 let planner = JPSPlanner::from_obstacle_points(&obstacles, JPSConfig::default()).unwrap();
1939
1940 let path = planner.plan_xy(2.0, 2.0, 8.0, 8.0).unwrap();
1941 assert!(!path.is_empty());
1942 }
1943
1944 #[test]
1945 fn test_jps_try_new_rejects_invalid_config() {
1946 let (ox, oy) = create_small_obstacles();
1947 let config = JPSConfig {
1948 heuristic_weight: 0.0,
1949 ..Default::default()
1950 };
1951
1952 let err = match JPSPlanner::try_new(&ox, &oy, config) {
1953 Ok(_) => panic!("expected invalid config to be rejected"),
1954 Err(err) => err,
1955 };
1956 assert!(matches!(err, RoboticsError::InvalidParameter(_)));
1957 }
1958
1959 #[test]
1960 fn test_jps_preserves_asymmetric_world_coordinates() {
1961 let mut obstacles = Obstacles::new();
1962
1963 for x in 10..=20 {
1964 obstacles.push(Point2D::new(x as f64, -4.0));
1965 obstacles.push(Point2D::new(x as f64, 6.0));
1966 }
1967 for y in -4..=6 {
1968 obstacles.push(Point2D::new(10.0, y as f64));
1969 obstacles.push(Point2D::new(20.0, y as f64));
1970 }
1971
1972 let planner = JPSPlanner::from_obstacle_points(&obstacles, JPSConfig::default()).unwrap();
1973 let path = planner.plan_xy(12.0, -2.0, 18.0, 4.0).unwrap();
1974
1975 let first = path.points.first().unwrap();
1976 let last = path.points.last().unwrap();
1977 assert!((first.x - 12.0).abs() < 1e-6);
1978 assert!((first.y + 2.0).abs() < 1e-6);
1979 assert!((last.x - 18.0).abs() < 1e-6);
1980 assert!((last.y - 4.0).abs() < 1e-6);
1981 }
1982
1983 #[test]
1984 fn test_jps_solves_upstream_jump_point_maze() {
1985 let (ox, oy) = create_upstream_jump_point_maze();
1986 let planner = JPSPlanner::from_obstacles(&ox, &oy, 1.0, 0.0);
1987
1988 let path = planner
1989 .plan(Point2D::new(5.0, 5.0), Point2D::new(35.0, 45.0))
1990 .unwrap();
1991
1992 let first = path.points.first().unwrap();
1993 let last = path.points.last().unwrap();
1994 assert!((first.x - 5.0).abs() < 1e-6);
1995 assert!((first.y - 5.0).abs() < 1e-6);
1996 assert!((last.x - 35.0).abs() < 1e-6);
1997 assert!((last.y - 45.0).abs() < 1e-6);
1998
1999 let upstream_reference_length = polyline_length(&upstream_jump_point_reference_polyline());
2000 assert!(
2001 path.total_length() <= upstream_reference_length + 1e-6,
2002 "Rust JPS path should stay no worse than PythonRobotics jump-point variant reference, got {} vs {}",
2003 path.total_length(),
2004 upstream_reference_length,
2005 );
2006
2007 let grid = planner.grid_map();
2008 for point in &path.points {
2009 let ix = grid.calc_x_index(point.x);
2010 let iy = grid.calc_y_index(point.y);
2011 assert!(
2012 grid.is_valid(ix, iy),
2013 "path point ({}, {}) should stay collision-free",
2014 point.x,
2015 point.y
2016 );
2017 }
2018 }
2019
2020 #[test]
2021 #[ignore = "long-running MovingAI benchmark"]
2022 fn test_jps_solves_moving_ai_arena2_bucket_80() {
2023 assert_pure_jps_matches_moving_ai_bucket(
2024 "arena2",
2025 include_str!("../benchdata/moving_ai/dao/arena2.map"),
2026 include_str!("../benchdata/moving_ai/dao/arena2.map.scen"),
2027 80,
2028 );
2029 }
2030
2031 #[test]
2032 #[ignore = "long-running MovingAI benchmark"]
2033 fn test_jps_reports_cost_mismatch_metrics_on_moving_ai_bucket_80_subset() {
2034 for (name, map_str, scen_str) in MOVING_AI_CASES {
2035 assert_pure_jps_matches_moving_ai_bucket(name, map_str, scen_str, 80);
2036 }
2037 }
2038
2039 #[test]
2040 #[ignore = "long-running MovingAI benchmark"]
2041 fn test_jps_solves_sampled_moving_ai_buckets_without_fallback() {
2042 const BUCKETS: [u32; 5] = [0, 20, 40, 60, 80];
2043
2044 for (name, map_str, scen_str) in MOVING_AI_CASES {
2045 for bucket in BUCKETS {
2046 assert_pure_jps_matches_moving_ai_bucket(name, map_str, scen_str, bucket);
2047 }
2048 }
2049 }
2050
2051 #[test]
2052 #[ignore = "long-running MovingAI benchmark"]
2053 fn test_jps_solves_long_tail_moving_ai_buckets_without_fallback() {
2054 let cases: [(&str, &str, &str, &[u32]); 3] = [
2055 (
2056 "arena2",
2057 include_str!("../benchdata/moving_ai/dao/arena2.map"),
2058 include_str!("../benchdata/moving_ai/dao/arena2.map.scen"),
2059 &[90],
2060 ),
2061 (
2062 "8room_000",
2063 include_str!("../benchdata/moving_ai/room/8room_000.map"),
2064 include_str!("../benchdata/moving_ai/room/8room_000.map.scen"),
2065 &[120, 160, 213],
2066 ),
2067 (
2068 "random512-10-0",
2069 include_str!("../benchdata/moving_ai/random/random512-10-0.map"),
2070 include_str!("../benchdata/moving_ai/random/random512-10-0.map.scen"),
2071 &[100, 140, 177],
2072 ),
2073 ];
2074
2075 for (name, map_str, scen_str, buckets) in cases {
2076 for bucket in buckets {
2077 assert_pure_jps_matches_moving_ai_bucket(name, map_str, scen_str, *bucket);
2078 }
2079 }
2080 }
2081
2082 #[test]
2083 #[ignore = "long-running MovingAI benchmark"]
2084 fn test_jps_solves_sampled_moving_ai_maze_buckets_without_fallback() {
2085 const BUCKETS: [u32; 6] = [0, 20, 40, 60, 80, 120];
2086
2087 for bucket in BUCKETS {
2088 assert_pure_jps_matches_moving_ai_bucket(
2089 "maze512-1-0",
2090 include_str!("../benchdata/moving_ai/maze/maze512-1-0.map"),
2091 include_str!("../benchdata/moving_ai/maze/maze512-1-0.map.scen"),
2092 bucket,
2093 );
2094 }
2095 }
2096
2097 #[test]
2098 #[ignore = "long-running MovingAI benchmark"]
2099 fn test_jps_solves_long_tail_moving_ai_maze_buckets_without_fallback() {
2100 const BUCKETS: [u32; 4] = [200, 400, 800, 1211];
2101
2102 for bucket in BUCKETS {
2103 assert_pure_jps_matches_moving_ai_bucket(
2104 "maze512-1-0",
2105 include_str!("../benchdata/moving_ai/maze/maze512-1-0.map"),
2106 include_str!("../benchdata/moving_ai/maze/maze512-1-0.map.scen"),
2107 bucket,
2108 );
2109 }
2110 }
2111
2112 #[test]
2113 #[ignore = "long-running MovingAI benchmark"]
2114 fn test_jps_solves_sampled_moving_ai_street_buckets_without_fallback() {
2115 const BUCKETS: [u32; 8] = [0, 20, 40, 60, 80, 120, 160, 186];
2116
2117 for bucket in BUCKETS {
2118 assert_pure_jps_matches_moving_ai_bucket(
2119 "Berlin_0_512",
2120 include_str!("../benchdata/moving_ai/street/Berlin_0_512.map"),
2121 include_str!("../benchdata/moving_ai/street/Berlin_0_512.map.scen"),
2122 bucket,
2123 );
2124 }
2125 }
2126
2127 #[test]
2128 #[ignore = "long-running MovingAI benchmark"]
2129 fn test_jps_vs_a_star_runtime_on_long_tail_moving_ai_subset() {
2130 const ITERATIONS: usize = 7;
2131 let cases: [(&str, &str, &str, u32); 5] = [
2132 (
2133 "arena2",
2134 include_str!("../benchdata/moving_ai/dao/arena2.map"),
2135 include_str!("../benchdata/moving_ai/dao/arena2.map.scen"),
2136 90,
2137 ),
2138 (
2139 "8room_000",
2140 include_str!("../benchdata/moving_ai/room/8room_000.map"),
2141 include_str!("../benchdata/moving_ai/room/8room_000.map.scen"),
2142 213,
2143 ),
2144 (
2145 "random512-10-0",
2146 include_str!("../benchdata/moving_ai/random/random512-10-0.map"),
2147 include_str!("../benchdata/moving_ai/random/random512-10-0.map.scen"),
2148 177,
2149 ),
2150 (
2151 "maze512-1-0",
2152 include_str!("../benchdata/moving_ai/maze/maze512-1-0.map"),
2153 include_str!("../benchdata/moving_ai/maze/maze512-1-0.map.scen"),
2154 1211,
2155 ),
2156 (
2157 "Berlin_0_512",
2158 include_str!("../benchdata/moving_ai/street/Berlin_0_512.map"),
2159 include_str!("../benchdata/moving_ai/street/Berlin_0_512.map.scen"),
2160 186,
2161 ),
2162 ];
2163
2164 for (name, map_str, scen_str, bucket) in cases {
2165 let (a_star, jps, start, goal, optimal_length) =
2166 build_moving_ai_bucket_planners(name, map_str, scen_str, bucket);
2167
2168 let warmup_a = a_star
2169 .plan(start, goal)
2170 .unwrap_or_else(|_| panic!("A* should solve {name} bucket {bucket}"));
2171 assert!(
2172 (warmup_a.total_length() - optimal_length).abs() <= 1e-6,
2173 "A* warmup path for {name} bucket {bucket} should match MovingAI optimal {}",
2174 optimal_length
2175 );
2176 let warmup_j = jps
2177 .plan(start, goal)
2178 .unwrap_or_else(|_| panic!("JPS should solve {name} bucket {bucket}"));
2179 assert!(
2180 (warmup_j.total_length() - optimal_length).abs() <= 1e-6,
2181 "JPS warmup path for {name} bucket {bucket} should match MovingAI optimal {}",
2182 optimal_length
2183 );
2184
2185 let a_star_median_us = measure_planner_median_us(ITERATIONS, || {
2186 let path = a_star
2187 .plan(start, goal)
2188 .unwrap_or_else(|_| panic!("A* should solve {name} bucket {bucket}"));
2189 assert!(
2190 (path.total_length() - optimal_length).abs() <= 1e-6,
2191 "A* path length {} should match MovingAI optimal {} on {name} bucket {bucket}",
2192 path.total_length(),
2193 optimal_length
2194 );
2195 path
2196 });
2197 let jps_median_us = measure_planner_median_us(ITERATIONS, || {
2198 let path = jps
2199 .plan(start, goal)
2200 .unwrap_or_else(|_| panic!("JPS should solve {name} bucket {bucket}"));
2201 assert!(
2202 (path.total_length() - optimal_length).abs() <= 1e-6,
2203 "JPS path length {} should match MovingAI optimal {} on {name} bucket {bucket}",
2204 path.total_length(),
2205 optimal_length
2206 );
2207 path
2208 });
2209 println!(
2210 "{name}/bucket{bucket}: a_star_median_us={:.2}, jps_median_us={:.2}, a_over_j={:.3}",
2211 a_star_median_us,
2212 jps_median_us,
2213 a_star_median_us / jps_median_us
2214 );
2215 }
2216 }
2217
2218 #[test]
2219 #[ignore = "long-running MovingAI benchmark"]
2220 fn test_jps_vs_a_star_runtime_crossover_samples_on_moving_ai() {
2221 const ITERATIONS: usize = 5;
2222 let families: [(&str, &str, &str, &[u32]); 5] = [
2223 (
2224 "arena2",
2225 include_str!("../benchdata/moving_ai/dao/arena2.map"),
2226 include_str!("../benchdata/moving_ai/dao/arena2.map.scen"),
2227 &[70, 75, 80, 85, 90],
2228 ),
2229 (
2230 "8room_000",
2231 include_str!("../benchdata/moving_ai/room/8room_000.map"),
2232 include_str!("../benchdata/moving_ai/room/8room_000.map.scen"),
2233 &[80, 120, 160, 213],
2234 ),
2235 (
2236 "random512-10-0",
2237 include_str!("../benchdata/moving_ai/random/random512-10-0.map"),
2238 include_str!("../benchdata/moving_ai/random/random512-10-0.map.scen"),
2239 &[80, 90, 100, 120, 140, 160, 177],
2240 ),
2241 (
2242 "maze512-1-0",
2243 include_str!("../benchdata/moving_ai/maze/maze512-1-0.map"),
2244 include_str!("../benchdata/moving_ai/maze/maze512-1-0.map.scen"),
2245 &[80, 120, 200, 400, 800, 1211],
2246 ),
2247 (
2248 "Berlin_0_512",
2249 include_str!("../benchdata/moving_ai/street/Berlin_0_512.map"),
2250 include_str!("../benchdata/moving_ai/street/Berlin_0_512.map.scen"),
2251 &[80, 120, 160, 186],
2252 ),
2253 ];
2254
2255 for (name, map_str, scen_str, buckets) in families {
2256 for bucket in buckets {
2257 let (a_star, jps, start, goal, optimal_length) =
2258 build_moving_ai_bucket_planners(name, map_str, scen_str, *bucket);
2259
2260 let warmup_a = a_star
2261 .plan(start, goal)
2262 .unwrap_or_else(|_| panic!("A* should solve {name} bucket {bucket}"));
2263 assert!(
2264 (warmup_a.total_length() - optimal_length).abs() <= 1e-6,
2265 "A* warmup path for {name} bucket {bucket} should match MovingAI optimal {}",
2266 optimal_length
2267 );
2268 let warmup_j = jps
2269 .plan(start, goal)
2270 .unwrap_or_else(|_| panic!("JPS should solve {name} bucket {bucket}"));
2271 assert!(
2272 (warmup_j.total_length() - optimal_length).abs() <= 1e-6,
2273 "JPS warmup path for {name} bucket {bucket} should match MovingAI optimal {}",
2274 optimal_length
2275 );
2276
2277 let a_star_median_us = measure_planner_median_us(ITERATIONS, || {
2278 let path = a_star
2279 .plan(start, goal)
2280 .unwrap_or_else(|_| panic!("A* should solve {name} bucket {bucket}"));
2281 assert!(
2282 (path.total_length() - optimal_length).abs() <= 1e-6,
2283 "A* path length {} should match MovingAI optimal {} on {name} bucket {bucket}",
2284 path.total_length(),
2285 optimal_length
2286 );
2287 path
2288 });
2289 let jps_median_us = measure_planner_median_us(ITERATIONS, || {
2290 let path = jps
2291 .plan(start, goal)
2292 .unwrap_or_else(|_| panic!("JPS should solve {name} bucket {bucket}"));
2293 assert!(
2294 (path.total_length() - optimal_length).abs() <= 1e-6,
2295 "JPS path length {} should match MovingAI optimal {} on {name} bucket {bucket}",
2296 path.total_length(),
2297 optimal_length
2298 );
2299 path
2300 });
2301 println!(
2302 "{name}/bucket{bucket}: a_star_median_us={:.2}, jps_median_us={:.2}, a_over_j={:.3}",
2303 a_star_median_us,
2304 jps_median_us,
2305 a_star_median_us / jps_median_us
2306 );
2307 }
2308 }
2309 }
2310
2311 #[test]
2312 #[ignore = "long-running MovingAI benchmark"]
2313 fn test_jps_vs_a_star_runtime_bucket_aggregate_samples_on_moving_ai() {
2314 const ITERATIONS: usize = 1;
2315 const SAMPLE_SLOTS: [usize; 3] = [0, 4, 9];
2316 let families: [(&str, &str, &str, &[u32]); 5] = [
2317 (
2318 "arena2",
2319 include_str!("../benchdata/moving_ai/dao/arena2.map"),
2320 include_str!("../benchdata/moving_ai/dao/arena2.map.scen"),
2321 &[80, 81, 82, 83, 84, 85],
2322 ),
2323 (
2324 "8room_000",
2325 include_str!("../benchdata/moving_ai/room/8room_000.map"),
2326 include_str!("../benchdata/moving_ai/room/8room_000.map.scen"),
2327 &[80, 90, 100, 110, 120],
2328 ),
2329 (
2330 "random512-10-0",
2331 include_str!("../benchdata/moving_ai/random/random512-10-0.map"),
2332 include_str!("../benchdata/moving_ai/random/random512-10-0.map.scen"),
2333 &[120, 125, 130, 135, 140],
2334 ),
2335 (
2336 "maze512-1-0",
2337 include_str!("../benchdata/moving_ai/maze/maze512-1-0.map"),
2338 include_str!("../benchdata/moving_ai/maze/maze512-1-0.map.scen"),
2339 &[80, 200, 1211],
2340 ),
2341 (
2342 "Berlin_0_512",
2343 include_str!("../benchdata/moving_ai/street/Berlin_0_512.map"),
2344 include_str!("../benchdata/moving_ai/street/Berlin_0_512.map.scen"),
2345 &[80, 100, 120, 140, 160, 180, 186],
2346 ),
2347 ];
2348
2349 for (name, map_str, scen_str, buckets) in families {
2350 let (map, scenarios, a_star, jps) =
2351 build_moving_ai_family_planners(name, map_str, scen_str);
2352
2353 for bucket in buckets {
2354 let aggregate = measure_moving_ai_bucket_runtime_aggregate(
2355 name,
2356 &map,
2357 &scenarios,
2358 &a_star,
2359 &jps,
2360 *bucket,
2361 &SAMPLE_SLOTS,
2362 ITERATIONS,
2363 );
2364 println!(
2365 "{name}/bucket{bucket}: sample_slots={:?}, a_star_bucket_median_us={:.2}, jps_bucket_median_us={:.2}, a_over_j={:.3}, jps_wins={}/{}",
2366 SAMPLE_SLOTS,
2367 aggregate.a_star_median_us,
2368 aggregate.jps_median_us,
2369 aggregate.a_star_median_us / aggregate.jps_median_us,
2370 aggregate.jps_wins,
2371 aggregate.sample_count
2372 );
2373 }
2374 }
2375 }
2376
2377 #[test]
2378 #[ignore = "long-running MovingAI benchmark"]
2379 fn test_jps_vs_a_star_runtime_bucket_full_aggregate_on_narrowed_windows() {
2380 const ITERATIONS: usize = 1;
2381 const FULL_BUCKET_SLOTS: [usize; 10] = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9];
2382 let families: [(&str, &str, &str, &[u32]); 4] = [
2383 (
2384 "arena2",
2385 include_str!("../benchdata/moving_ai/dao/arena2.map"),
2386 include_str!("../benchdata/moving_ai/dao/arena2.map.scen"),
2387 &[86, 87, 88, 89, 90],
2388 ),
2389 (
2390 "8room_000",
2391 include_str!("../benchdata/moving_ai/room/8room_000.map"),
2392 include_str!("../benchdata/moving_ai/room/8room_000.map.scen"),
2393 &[80, 85, 90],
2394 ),
2395 (
2396 "random512-10-0",
2397 include_str!("../benchdata/moving_ai/random/random512-10-0.map"),
2398 include_str!("../benchdata/moving_ai/random/random512-10-0.map.scen"),
2399 &[130, 132, 135],
2400 ),
2401 (
2402 "Berlin_0_512",
2403 include_str!("../benchdata/moving_ai/street/Berlin_0_512.map"),
2404 include_str!("../benchdata/moving_ai/street/Berlin_0_512.map.scen"),
2405 &[120, 140, 160, 186],
2406 ),
2407 ];
2408
2409 for (name, map_str, scen_str, buckets) in families {
2410 let (map, scenarios, a_star, jps) =
2411 build_moving_ai_family_planners(name, map_str, scen_str);
2412
2413 for bucket in buckets {
2414 let aggregate = measure_moving_ai_bucket_runtime_aggregate(
2415 name,
2416 &map,
2417 &scenarios,
2418 &a_star,
2419 &jps,
2420 *bucket,
2421 &FULL_BUCKET_SLOTS,
2422 ITERATIONS,
2423 );
2424 println!(
2425 "{name}/bucket{bucket}: full_bucket_slots=0..9, a_star_bucket_median_us={:.2}, jps_bucket_median_us={:.2}, a_over_j={:.3}, a_star_range_us=[{:.2}, {:.2}], jps_range_us=[{:.2}, {:.2}], jps_wins={}/{}",
2426 aggregate.a_star_median_us,
2427 aggregate.jps_median_us,
2428 aggregate.a_star_median_us / aggregate.jps_median_us,
2429 aggregate.a_star_min_us,
2430 aggregate.a_star_max_us,
2431 aggregate.jps_min_us,
2432 aggregate.jps_max_us,
2433 aggregate.jps_wins,
2434 aggregate.sample_count
2435 );
2436 }
2437 }
2438 }
2439}