Skip to main content

rust_robotics_planning/
jps.rs

1#![allow(clippy::collapsible_if)]
2
3//! Jump Point Search (JPS) path planning algorithm
4//!
5//! JPS is an optimization of A* for uniform-cost grids that reduces the
6//! number of nodes expanded by identifying "jump points" - nodes that
7//! have forced neighbors or are the goal.
8//!
9//! Reference: Harabor, D., & Grastien, A. (2011). Online Graph Pruning for
10//! Pathfinding on Grid Maps.
11
12use 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/// Configuration for JPS planner
19#[derive(Debug, Clone)]
20pub struct JPSConfig {
21    /// Grid resolution in meters
22    pub resolution: f64,
23    /// Robot radius for obstacle inflation
24    pub robot_radius: f64,
25    /// Heuristic weight (1.0 = optimal, >1.0 = faster but suboptimal)
26    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/// Why the JPS planner had to fall back to grid A*.
65#[derive(Debug, Clone, Copy, PartialEq, Eq)]
66pub enum JPSFallbackReason {
67    /// The reconstructed JPS path was invalid under the shared grid semantics.
68    InvalidJumpPath,
69    /// The reconstructed JPS path was valid but longer than the shared grid-optimal reference.
70    SuboptimalJumpPath,
71    /// The jump search failed to reach the goal, so the planner retried with grid A*.
72    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/// More specific classification for `InvalidJumpPath`.
86#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
87pub enum JPSInvalidJumpPathDetail {
88    /// The reconstructed jump path contains at least one invalid grid step.
89    InvalidStepSequence,
90    /// The reconstructed jump path is geometrically valid but its length no longer matches the
91    /// search cost accumulated on jump points.
92    CostMismatch,
93    /// The reconstructed jump path has both invalid steps and a cost mismatch.
94    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/// Additional metrics attached to `cost_mismatch` fallback cases.
108#[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/// Additional metrics attached to `cost_mismatch` fallback cases.
119#[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/// Extra execution diagnostics for one JPS planning query.
129#[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/// Planning result with path plus JPS-specific diagnostics.
138#[derive(Debug, Clone)]
139pub struct JPSPlanResult {
140    pub path: Path2D,
141    pub diagnostics: JPSPlanDiagnostics,
142}
143
144/// Direction for movement in JPS
145#[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/// Node with priority for JPS open set (min-heap)
166#[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        // Reverse ordering for min-heap behavior
186        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
199/// Jump Point Search path planner
200pub 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    /// Create a new JPS planner with obstacle positions
211    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    /// Create a validated JPS planner with obstacle positions
218    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    /// Create from obstacle x/y vectors with default config
225    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    /// Create a validated JPS planner from typed obstacle points
235    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    /// Plan a path returning (rx, ry) vectors (legacy interface)
242    #[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    /// Plan a path without requiring the PathPlanner trait in scope
251    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    /// Plan a path from raw coordinates without requiring the PathPlanner trait in scope
257    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    /// Plan a path and report whether the query used the grid A* fallback.
263    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    /// Get reference to the grid map
272    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        // Octile distance heuristic for 8-connected grid
278        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    /// Check if a position has forced neighbors in the given direction
286    fn has_forced_neighbor(&self, x: i32, y: i32, dir: Direction) -> bool {
287        if dir.is_cardinal() {
288            // For cardinal directions, check perpendicular neighbors
289            if dir.dx != 0 {
290                // Moving horizontally
291                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                // Moving vertically
298                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    /// Jump in a given direction from (x, y) until we find a jump point or hit an obstacle
310    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        // The search cost is in grid cells, the path in meters.
610        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(&current_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(&current.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(&current_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        // Search costs are in cells and path lengths in meters: at 0.5 m per
895        // cell they used to disagree, so every query fell back to A*.
896        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(&current.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(&current_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}