rust_robotics_planning/

bidirectional_a_star.rs

//! Bidirectional A* path planning algorithm
//!
//! Grid-based path planning using bidirectional A* search.
//! Searches simultaneously from start and goal, terminating when
//! the two search frontiers meet.

use std::cmp::Ordering;
use std::collections::{BinaryHeap, HashMap};

use crate::grid::{GridMap, Node};
use rust_robotics_core::{Obstacles, Path2D, PathPlanner, Point2D, RoboticsError, RoboticsResult};

/// Node with priority for bidirectional A* open set (min-heap)
#[derive(Debug)]
struct PriorityNode {
    grid_index: i32,
    x: i32,
    y: i32,
    cost: f64,
    priority: f64, // f = g + h
    storage_index: usize,
}

impl Eq for PriorityNode {}

impl PartialEq for PriorityNode {
    fn eq(&self, other: &Self) -> bool {
        self.priority == other.priority
    }
}

impl Ord for PriorityNode {
    fn cmp(&self, other: &Self) -> Ordering {
        // Reverse ordering for min-heap behavior
        other
            .priority
            .partial_cmp(&self.priority)
            .unwrap_or(Ordering::Equal)
    }
}

impl PartialOrd for PriorityNode {
    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
        Some(self.cmp(other))
    }
}

/// Configuration for Bidirectional A* planner
#[derive(Debug, Clone)]
pub struct BidirectionalAStarConfig {
    /// Grid resolution in meters
    pub resolution: f64,
    /// Robot radius for obstacle inflation
    pub robot_radius: f64,
    /// Heuristic weight (1.0 = optimal, >1.0 = faster but suboptimal)
    pub heuristic_weight: f64,
}

impl Default for BidirectionalAStarConfig {
    fn default() -> Self {
        Self {
            resolution: 1.0,
            robot_radius: 0.5,
            heuristic_weight: 1.0,
        }
    }
}

impl BidirectionalAStarConfig {
    pub fn validate(&self) -> RoboticsResult<()> {
        if !self.resolution.is_finite() || self.resolution <= 0.0 {
            return Err(RoboticsError::InvalidParameter(format!(
                "resolution must be positive and finite, got {}",
                self.resolution
            )));
        }
        if !self.robot_radius.is_finite() || self.robot_radius < 0.0 {
            return Err(RoboticsError::InvalidParameter(format!(
                "robot_radius must be non-negative and finite, got {}",
                self.robot_radius
            )));
        }
        if !self.heuristic_weight.is_finite() || self.heuristic_weight <= 0.0 {
            return Err(RoboticsError::InvalidParameter(format!(
                "heuristic_weight must be positive and finite, got {}",
                self.heuristic_weight
            )));
        }

        Ok(())
    }
}

/// Bidirectional A* path planner
pub struct BidirectionalAStarPlanner {
    grid_map: GridMap,
    config: BidirectionalAStarConfig,
    motion: Vec<(i32, i32, f64)>,
}

impl BidirectionalAStarPlanner {
    /// Create a new Bidirectional A* planner with obstacle positions
    pub fn new(ox: &[f64], oy: &[f64], config: BidirectionalAStarConfig) -> Self {
        Self::try_new(ox, oy, config).expect(
            "invalid Bidirectional A* planner input: obstacle list must be non-empty and valid, and config values must be positive/finite",
        )
    }

    /// Create a validated Bidirectional A* planner with obstacle positions
    pub fn try_new(
        ox: &[f64],
        oy: &[f64],
        config: BidirectionalAStarConfig,
    ) -> RoboticsResult<Self> {
        config.validate()?;
        let grid_map = GridMap::try_new(ox, oy, config.resolution, config.robot_radius)?;
        let motion = Self::get_motion_model();

        Ok(BidirectionalAStarPlanner {
            grid_map,
            config,
            motion,
        })
    }

    /// Create from obstacle x/y vectors with default config
    pub fn from_obstacles(ox: &[f64], oy: &[f64], resolution: f64, robot_radius: f64) -> Self {
        let config = BidirectionalAStarConfig {
            resolution,
            robot_radius,
            ..Default::default()
        };
        Self::new(ox, oy, config)
    }

    /// Create a validated Bidirectional A* planner from typed obstacle points
    pub fn from_obstacle_points(
        obstacles: &Obstacles,
        config: BidirectionalAStarConfig,
    ) -> RoboticsResult<Self> {
        config.validate()?;
        let grid_map = GridMap::from_obstacles(obstacles, config.resolution, config.robot_radius)?;
        let motion = Self::get_motion_model();

        Ok(BidirectionalAStarPlanner {
            grid_map,
            config,
            motion,
        })
    }

    /// Plan a path returning (rx, ry) vectors (legacy interface)
    #[deprecated(note = "use plan() or plan_xy() instead")]
    pub fn planning(&self, sx: f64, sy: f64, gx: f64, gy: f64) -> Option<(Vec<f64>, Vec<f64>)> {
        match self.plan_xy(sx, sy, gx, gy) {
            Ok(path) => Some((path.x_coords(), path.y_coords())),
            Err(_) => None,
        }
    }

    /// Plan a path without requiring the PathPlanner trait in scope
    pub fn plan(&self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
        self.plan_impl(start, goal)
    }

    /// Plan a path from raw coordinates without requiring the PathPlanner trait in scope
    pub fn plan_xy(&self, sx: f64, sy: f64, gx: f64, gy: f64) -> RoboticsResult<Path2D> {
        self.plan_impl(Point2D::new(sx, sy), Point2D::new(gx, gy))
    }

    /// Get reference to the grid map
    pub fn grid_map(&self) -> &GridMap {
        &self.grid_map
    }

    fn calc_heuristic(&self, n1_x: i32, n1_y: i32, n2_x: i32, n2_y: i32) -> f64 {
        self.config.heuristic_weight * (((n1_x - n2_x).pow(2) + (n1_y - n2_y).pow(2)) as f64).sqrt()
    }

    fn get_motion_model() -> Vec<(i32, i32, f64)> {
        // dx, dy, cost (8-connected grid)
        vec![
            (1, 0, 1.0),
            (0, 1, 1.0),
            (-1, 0, 1.0),
            (0, -1, 1.0),
            (-1, -1, std::f64::consts::SQRT_2),
            (-1, 1, std::f64::consts::SQRT_2),
            (1, -1, std::f64::consts::SQRT_2),
            (1, 1, std::f64::consts::SQRT_2),
        ]
    }

    /// Find the best known storage index for a grid cell across both open g-values
    /// and closed set. Returns the index with the lowest g-cost.
    fn best_known_index(
        g_values: &HashMap<i32, (f64, usize)>,
        closed_set: &HashMap<i32, usize>,
        node_storage: &[Node],
        grid_index: i32,
    ) -> Option<usize> {
        match (g_values.get(&grid_index), closed_set.get(&grid_index)) {
            (Some(&(g_cost, open_idx)), Some(&closed_idx)) => {
                if g_cost <= node_storage[closed_idx].cost {
                    Some(open_idx)
                } else {
                    Some(closed_idx)
                }
            }
            (Some(&(_, open_idx)), None) => Some(open_idx),
            (None, Some(&closed_idx)) => Some(closed_idx),
            (None, None) => None,
        }
    }

    fn update_best_meeting(
        best_meeting: &mut Option<(usize, usize, f64)>,
        forward_index: usize,
        forward_cost: f64,
        backward_index: usize,
        backward_cost: f64,
    ) {
        let total_cost = forward_cost + backward_cost;
        if best_meeting
            .as_ref()
            .map_or(true, |(_, _, best_cost)| total_cost < *best_cost)
        {
            *best_meeting = Some((forward_index, backward_index, total_cost));
        }
    }

    /// Build a path segment by tracing parent pointers from `start_index` back to the root.
    /// Returns points in order from `start_index` back to root.
    fn trace_path(&self, start_index: usize, node_storage: &[Node]) -> Vec<Point2D> {
        let mut points = Vec::new();
        let mut current = Some(start_index);

        while let Some(idx) = current {
            let node = &node_storage[idx];
            points.push(Point2D::new(
                self.grid_map.calc_x_position(node.x),
                self.grid_map.calc_y_position(node.y),
            ));
            current = node.parent_index;
        }

        points
    }

    fn build_meeting_path(
        &self,
        forward_index: usize,
        backward_index: usize,
        node_storage_a: &[Node],
        node_storage_b: &[Node],
    ) -> Path2D {
        let mut forward_points = self.trace_path(forward_index, node_storage_a);
        forward_points.reverse();

        let backward_points = self.trace_path(backward_index, node_storage_b);

        let mut all_points = forward_points;
        all_points.extend(backward_points.into_iter().skip(1));
        Path2D::from_points(all_points)
    }

    fn ensure_query_is_valid(&self, x: i32, y: i32, label: &str) -> RoboticsResult<()> {
        if self.grid_map.is_valid(x, y) {
            return Ok(());
        }

        Err(RoboticsError::PlanningError(format!(
            "{} position is invalid",
            label
        )))
    }

    fn plan_impl(&self, start: Point2D, goal: Point2D) -> RoboticsResult<Path2D> {
        let start_x = self.grid_map.calc_x_index(start.x);
        let start_y = self.grid_map.calc_y_index(start.y);
        let goal_x = self.grid_map.calc_x_index(goal.x);
        let goal_y = self.grid_map.calc_y_index(goal.y);

        self.ensure_query_is_valid(start_x, start_y, "Start")?;
        self.ensure_query_is_valid(goal_x, goal_y, "Goal")?;

        // Forward search (from start)
        let mut node_storage_a: Vec<Node> = Vec::new();
        let mut open_set_a: BinaryHeap<PriorityNode> = BinaryHeap::new();
        let mut g_values_a: HashMap<i32, (f64, usize)> = HashMap::new(); // grid_index -> (g_cost, storage_index)
        let mut closed_set_a: HashMap<i32, usize> = HashMap::new();

        // Backward search (from goal)
        let mut node_storage_b: Vec<Node> = Vec::new();
        let mut open_set_b: BinaryHeap<PriorityNode> = BinaryHeap::new();
        let mut g_values_b: HashMap<i32, (f64, usize)> = HashMap::new();
        let mut closed_set_b: HashMap<i32, usize> = HashMap::new();

        // Initialize forward start node
        node_storage_a.push(Node::new(start_x, start_y, 0.0, None));
        let start_grid_index = self.grid_map.calc_index(start_x, start_y);
        let start_priority = self.calc_heuristic(start_x, start_y, goal_x, goal_y);
        open_set_a.push(PriorityNode {
            grid_index: start_grid_index,
            x: start_x,
            y: start_y,
            cost: 0.0,
            priority: start_priority,
            storage_index: 0,
        });
        g_values_a.insert(start_grid_index, (0.0, 0));

        // Initialize backward start node (goal)
        node_storage_b.push(Node::new(goal_x, goal_y, 0.0, None));
        let goal_grid_index = self.grid_map.calc_index(goal_x, goal_y);
        let goal_priority = self.calc_heuristic(goal_x, goal_y, start_x, start_y);
        open_set_b.push(PriorityNode {
            grid_index: goal_grid_index,
            x: goal_x,
            y: goal_y,
            cost: 0.0,
            priority: goal_priority,
            storage_index: 0,
        });
        g_values_b.insert(goal_grid_index, (0.0, 0));

        let mut best_meeting: Option<(usize, usize, f64)> = None;

        loop {
            // Peek both heaps to get min f-costs
            let min_f_a = open_set_a.peek().map(|n| n.priority);
            let min_f_b = open_set_b.peek().map(|n| n.priority);

            // If either heap is empty, return best meeting or error
            if min_f_a.is_none() || min_f_b.is_none() {
                return best_meeting
                    .map(|(forward_idx, backward_idx, _)| {
                        self.build_meeting_path(
                            forward_idx,
                            backward_idx,
                            &node_storage_a,
                            &node_storage_b,
                        )
                    })
                    .ok_or_else(|| RoboticsError::PlanningError("No path found".to_string()));
            }

            let min_f_a = min_f_a.unwrap();
            let min_f_b = min_f_b.unwrap();

            // Termination: both heaps' min f-cost >= best meeting cost
            if let Some((forward_idx, backward_idx, best_cost)) = best_meeting {
                if min_f_a >= best_cost && min_f_b >= best_cost {
                    return Ok(self.build_meeting_path(
                        forward_idx,
                        backward_idx,
                        &node_storage_a,
                        &node_storage_b,
                    ));
                }
            }

            if min_f_a <= min_f_b {
                // Expand forward search
                let current = open_set_a.pop().unwrap();
                let c_id_a = current.grid_index;

                // Skip if already closed with equal or better cost
                if closed_set_a.contains_key(&c_id_a) {
                    continue;
                }

                // Skip stale entries (g-value in heap is worse than current best)
                if let Some(&(best_g, _)) = g_values_a.get(&c_id_a) {
                    if current.cost > best_g {
                        continue;
                    }
                }

                let current_a_storage_idx = current.storage_index;
                let current_a_x = current.x;
                let current_a_y = current.y;
                let current_a_cost = current.cost;

                // Check meeting with backward search
                if let Some(other_idx) =
                    Self::best_known_index(&g_values_b, &closed_set_b, &node_storage_b, c_id_a)
                {
                    Self::update_best_meeting(
                        &mut best_meeting,
                        current_a_storage_idx,
                        current_a_cost,
                        other_idx,
                        node_storage_b[other_idx].cost,
                    );
                }

                closed_set_a.insert(c_id_a, current_a_storage_idx);
                g_values_a.remove(&c_id_a);

                for &(dx, dy, move_cost) in &self.motion {
                    let new_a_x = current_a_x + dx;
                    let new_a_y = current_a_y + dy;
                    let new_a_grid_index = self.grid_map.calc_index(new_a_x, new_a_y);
                    let new_cost = current_a_cost + move_cost;

                    if !self
                        .grid_map
                        .is_valid_offset(current_a_x, current_a_y, dx, dy)
                    {
                        continue;
                    }

                    if let Some(&closed_idx) = closed_set_a.get(&new_a_grid_index) {
                        if node_storage_a[closed_idx].cost <= new_cost {
                            continue;
                        }
                    }

                    if let Some(&(existing_g, _)) = g_values_a.get(&new_a_grid_index) {
                        if existing_g <= new_cost {
                            continue;
                        }
                    }

                    node_storage_a.push(Node::new(
                        new_a_x,
                        new_a_y,
                        new_cost,
                        Some(current_a_storage_idx),
                    ));
                    let new_idx = node_storage_a.len() - 1;
                    let priority = new_cost + self.calc_heuristic(new_a_x, new_a_y, goal_x, goal_y);
                    open_set_a.push(PriorityNode {
                        grid_index: new_a_grid_index,
                        x: new_a_x,
                        y: new_a_y,
                        cost: new_cost,
                        priority,
                        storage_index: new_idx,
                    });
                    g_values_a.insert(new_a_grid_index, (new_cost, new_idx));

                    if let Some(other_idx) = Self::best_known_index(
                        &g_values_b,
                        &closed_set_b,
                        &node_storage_b,
                        new_a_grid_index,
                    ) {
                        Self::update_best_meeting(
                            &mut best_meeting,
                            new_idx,
                            node_storage_a[new_idx].cost,
                            other_idx,
                            node_storage_b[other_idx].cost,
                        );
                    }
                }
            } else {
                // Expand backward search
                let current = open_set_b.pop().unwrap();
                let c_id_b = current.grid_index;

                // Skip if already closed
                if closed_set_b.contains_key(&c_id_b) {
                    continue;
                }

                // Skip stale entries
                if let Some(&(best_g, _)) = g_values_b.get(&c_id_b) {
                    if current.cost > best_g {
                        continue;
                    }
                }

                let current_b_storage_idx = current.storage_index;
                let current_b_x = current.x;
                let current_b_y = current.y;
                let current_b_cost = current.cost;

                // Check meeting with forward search
                if let Some(other_idx) =
                    Self::best_known_index(&g_values_a, &closed_set_a, &node_storage_a, c_id_b)
                {
                    Self::update_best_meeting(
                        &mut best_meeting,
                        other_idx,
                        node_storage_a[other_idx].cost,
                        current_b_storage_idx,
                        current_b_cost,
                    );
                }

                closed_set_b.insert(c_id_b, current_b_storage_idx);
                g_values_b.remove(&c_id_b);

                for &(dx, dy, move_cost) in &self.motion {
                    let new_b_x = current_b_x + dx;
                    let new_b_y = current_b_y + dy;
                    let new_b_grid_index = self.grid_map.calc_index(new_b_x, new_b_y);
                    let new_cost = current_b_cost + move_cost;

                    if !self
                        .grid_map
                        .is_valid_offset(current_b_x, current_b_y, dx, dy)
                    {
                        continue;
                    }

                    if let Some(&closed_idx) = closed_set_b.get(&new_b_grid_index) {
                        if node_storage_b[closed_idx].cost <= new_cost {
                            continue;
                        }
                    }

                    if let Some(&(existing_g, _)) = g_values_b.get(&new_b_grid_index) {
                        if existing_g <= new_cost {
                            continue;
                        }
                    }

                    node_storage_b.push(Node::new(
                        new_b_x,
                        new_b_y,
                        new_cost,
                        Some(current_b_storage_idx),
                    ));
                    let new_idx = node_storage_b.len() - 1;
                    let priority =
                        new_cost + self.calc_heuristic(new_b_x, new_b_y, start_x, start_y);
                    open_set_b.push(PriorityNode {
                        grid_index: new_b_grid_index,
                        x: new_b_x,
                        y: new_b_y,
                        cost: new_cost,
                        priority,
                        storage_index: new_idx,
                    });
                    g_values_b.insert(new_b_grid_index, (new_cost, new_idx));

                    if let Some(other_idx) = Self::best_known_index(
                        &g_values_a,
                        &closed_set_a,
                        &node_storage_a,
                        new_b_grid_index,
                    ) {
                        Self::update_best_meeting(
                            &mut best_meeting,
                            other_idx,
                            node_storage_a[other_idx].cost,
                            new_idx,
                            node_storage_b[new_idx].cost,
                        );
                    }
                }
            }
        }
    }
}

impl PathPlanner for BidirectionalAStarPlanner {
    fn plan(&self, start: Point2D, goal: Point2D) -> Result<Path2D, RoboticsError> {
        self.plan_impl(start, goal)
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::moving_ai::{MovingAiMap, MovingAiScenario};

    use rust_robotics_core::Obstacles;

    fn create_simple_obstacles() -> (Vec<f64>, Vec<f64>) {
        let mut ox = Vec::new();
        let mut oy = Vec::new();

        // Boundary
        for i in 0..11 {
            ox.push(i as f64);
            oy.push(0.0);
            ox.push(i as f64);
            oy.push(10.0);
            ox.push(0.0);
            oy.push(i as f64);
            ox.push(10.0);
            oy.push(i as f64);
        }

        // Internal obstacle
        for i in 4..7 {
            ox.push(5.0);
            oy.push(i as f64);
        }

        (ox, oy)
    }

    #[test]
    fn test_bidirectional_a_star_finds_path() {
        let (ox, oy) = create_simple_obstacles();
        let planner = BidirectionalAStarPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);

        let start = Point2D::new(2.0, 2.0);
        let goal = Point2D::new(8.0, 8.0);

        let result = planner.plan(start, goal);
        assert!(result.is_ok());

        let path = result.unwrap();
        assert!(!path.is_empty());
    }

    #[test]
    #[allow(deprecated)]
    fn test_bidirectional_a_star_legacy_interface() {
        let (ox, oy) = create_simple_obstacles();
        let planner = BidirectionalAStarPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);

        let result = planner.planning(2.0, 2.0, 8.0, 8.0);
        assert!(result.is_some());

        let (rx, ry) = result.unwrap();
        assert!(!rx.is_empty());
        assert_eq!(rx.len(), ry.len());
    }

    #[test]
    fn test_bidirectional_a_star_from_obstacle_points() {
        let (ox, oy) = create_simple_obstacles();
        let obstacles = Obstacles::try_from_xy(&ox, &oy).unwrap();
        let planner = BidirectionalAStarPlanner::from_obstacle_points(
            &obstacles,
            BidirectionalAStarConfig::default(),
        )
        .unwrap();

        let path = planner.plan_xy(2.0, 2.0, 8.0, 8.0).unwrap();
        assert!(!path.is_empty());
    }

    #[test]
    fn test_bidirectional_a_star_try_new_rejects_invalid_config() {
        let (ox, oy) = create_simple_obstacles();
        let config = BidirectionalAStarConfig {
            heuristic_weight: 0.0,
            ..Default::default()
        };

        let err = match BidirectionalAStarPlanner::try_new(&ox, &oy, config) {
            Ok(_) => panic!("expected invalid config to be rejected"),
            Err(err) => err,
        };
        assert!(matches!(err, RoboticsError::InvalidParameter(_)));
    }

    #[test]
    fn test_bidirectional_a_star_path_is_collision_free() {
        let (ox, oy) = create_simple_obstacles();
        let planner = BidirectionalAStarPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);

        let path = planner.plan_xy(2.0, 2.0, 8.0, 8.0).unwrap();

        // Every point on the path should map to a valid (non-obstacle) grid cell
        let grid = planner.grid_map();
        for pt in &path.points {
            let gx = grid.calc_x_index(pt.x);
            let gy = grid.calc_y_index(pt.y);
            assert!(
                grid.is_valid(gx, gy),
                "Path point ({}, {}) maps to invalid grid cell ({}, {})",
                pt.x,
                pt.y,
                gx,
                gy
            );
        }
    }

    #[test]
    fn test_bidirectional_a_star_start_and_goal_correct() {
        let (ox, oy) = create_simple_obstacles();
        let planner = BidirectionalAStarPlanner::from_obstacles(&ox, &oy, 1.0, 0.5);

        let path = planner.plan_xy(2.0, 2.0, 8.0, 8.0).unwrap();

        let first = path.points.first().unwrap();
        let last = path.points.last().unwrap();
        assert!((first.x - 2.0).abs() < 1e-6);
        assert!((first.y - 2.0).abs() < 1e-6);
        assert!((last.x - 8.0).abs() < 1e-6);
        assert!((last.y - 8.0).abs() < 1e-6);
    }

    #[test]
    fn test_bidirectional_a_star_preserves_asymmetric_world_coordinates() {
        let mut obstacles = Obstacles::new();

        for x in 10..=20 {
            obstacles.push(Point2D::new(x as f64, -4.0));
            obstacles.push(Point2D::new(x as f64, 6.0));
        }
        for y in -4..=6 {
            obstacles.push(Point2D::new(10.0, y as f64));
            obstacles.push(Point2D::new(20.0, y as f64));
        }

        let planner = BidirectionalAStarPlanner::from_obstacle_points(
            &obstacles,
            BidirectionalAStarConfig::default(),
        )
        .unwrap();
        let path = planner.plan_xy(12.0, -2.0, 18.0, 4.0).unwrap();

        let first = path.points.first().unwrap();
        let last = path.points.last().unwrap();
        assert!((first.x - 12.0).abs() < 1e-6);
        assert!((first.y + 2.0).abs() < 1e-6);
        assert!((last.x - 18.0).abs() < 1e-6);
        assert!((last.y - 4.0).abs() < 1e-6);
    }

    #[test]
    #[ignore = "long-running MovingAI benchmark"]
    fn test_bidirectional_a_star_matches_moving_ai_arena2_bucket_80_optimal_length() {
        let map = MovingAiMap::parse_str(include_str!("../benchdata/moving_ai/dao/arena2.map"))
            .expect("arena2 MovingAI map should parse");
        let scenario =
            MovingAiScenario::parse_str(include_str!("../benchdata/moving_ai/dao/arena2.map.scen"))
                .expect("arena2 MovingAI scenarios should parse")
                .into_iter()
                .find(|row| row.bucket == 80)
                .expect("arena2 MovingAI bucket 80 scenario should exist");

        let planner = BidirectionalAStarPlanner::from_obstacle_points(
            &map.to_obstacles(),
            BidirectionalAStarConfig {
                resolution: 1.0,
                robot_radius: 0.5,
                heuristic_weight: 1.0,
            },
        )
        .expect("Bidirectional A* planner should build from arena2 obstacles");

        let start = map
            .planning_point(scenario.start_x, scenario.start_y)
            .expect("arena2 start should be valid");
        let goal = map
            .planning_point(scenario.goal_x, scenario.goal_y)
            .expect("arena2 goal should be valid");

        let path = planner
            .plan(start, goal)
            .expect("Bidirectional A* should solve the arena2 bucket 80 scenario");

        assert!(
            (path.total_length() - scenario.optimal_length).abs() < 1e-6,
            "Bidirectional A* path length {} should match MovingAI optimal {}",
            path.total_length(),
            scenario.optimal_length
        );
    }
}