scirs2_spatial/pathplanning/

astar.rs

//! A* search algorithm implementation
//!
//! A* is an informed search algorithm that finds the least-cost path from a
//! given initial node to a goal node. It uses a best-first search and finds
//! the least-cost path to the goal.
//!
//! A* uses a heuristic function to estimate the cost from the current node to
//! the goal. The algorithm maintains a priority queue of nodes to be evaluated,
//! where the priority is determined by f(n) = g(n) + h(n), where:
//! - g(n) is the cost of the path from the start node to n
//! - h(n) is the heuristic estimate of the cost from n to the goal

use std::cmp::Ordering;
use std::collections::{BinaryHeap, HashMap};
use std::hash::{Hash, Hasher};
use std::rc::Rc;

use scirs2_core::ndarray::ArrayView1;

use crate::error::{SpatialError, SpatialResult};

/// A path found by the A* algorithm
#[derive(Debug, Clone)]
pub struct Path<N> {
    /// The nodes that make up the path, from start to goal
    pub nodes: Vec<N>,
    /// The total cost of the path
    pub cost: f64,
}

impl<N> Path<N> {
    /// Create a new path with the given nodes and cost
    pub fn new(nodes: Vec<N>, cost: f64) -> Self {
        Path { nodes, cost }
    }

    /// Check if the path is empty
    pub fn is_empty(&self) -> bool {
        self.nodes.is_empty()
    }

    /// Get the length of the path (number of nodes)
    pub fn len(&self) -> usize {
        self.nodes.len()
    }
}

/// A node in the search graph
#[derive(Debug, Clone)]
pub struct Node<N: Clone + Eq + Hash> {
    /// The state represented by this node
    pub state: N,
    /// The parent node
    pub parent: Option<Rc<Node<N>>>,
    /// The cost from the start node to this node (g-value)
    pub g: f64,
    /// The estimated cost from this node to the goal (h-value)
    pub h: f64,
}

impl<N: Clone + Eq + Hash> Node<N> {
    /// Create a new node
    pub fn new(state: N, parent: Option<Rc<Node<N>>>, g: f64, h: f64) -> Self {
        Node {
            state,
            parent,
            g,
            h,
        }
    }

    /// Get the f-value (f = g + h)
    pub fn f(&mut self) -> f64 {
        self.g + self.h
    }
}

impl<N: Clone + Eq + Hash> PartialEq for Node<N> {
    fn eq(&self, other: &Self) -> bool {
        self.state == other.state
    }
}

impl<N: Clone + Eq + Hash> Eq for Node<N> {}

// Custom ordering for the priority queue (min-heap based on f-value)
impl<N: Clone + Eq + Hash> Ord for Node<N> {
    fn cmp(&self, other: &Self) -> Ordering {
        // We want to prioritize nodes with lower f-values
        // But BinaryHeap is a max-heap, so we invert the comparison
        let self_f = self.g + self.h;
        let other_f = other.g + other.h;
        other_f.partial_cmp(&self_f).unwrap_or(Ordering::Equal)
    }
}

impl<N: Clone + Eq + Hash> PartialOrd for Node<N> {
    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
        Some(self.cmp(other))
    }
}

/// Function that returns neighboring states and costs
pub type NeighborFn<N> = dyn Fn(&N) -> Vec<(N, f64)>;

/// Function that estimates the cost from a state to the goal
pub type HeuristicFn<N> = dyn Fn(&N, &N) -> f64;

/// A wrapper type for f64 arrays that implements Hash and Eq
/// This allows using f64 arrays as keys in HashMaps
#[derive(Debug, Clone, Copy)]
pub struct HashableFloat2D {
    /// X coordinate
    pub x: f64,
    /// Y coordinate
    pub y: f64,
}

impl HashableFloat2D {
    /// Create a new HashableFloat2D from f64 coordinates
    pub fn new(x: f64, y: f64) -> Self {
        HashableFloat2D { x, y }
    }

    /// Convert from array representation
    pub fn from_array(arr: [f64; 2]) -> Self {
        HashableFloat2D {
            x: arr[0],
            y: arr[1],
        }
    }

    /// Convert to array representation
    pub fn to_array(&self) -> [f64; 2] {
        [self.x, self.y]
    }

    /// Calculate Euclidean distance to another point
    pub fn distance(&self, other: &HashableFloat2D) -> f64 {
        let dx = self.x - other.x;
        let dy = self.y - other.y;
        (dx * dx + dy * dy).sqrt()
    }
}

impl PartialEq for HashableFloat2D {
    fn eq(&self, other: &Self) -> bool {
        // Use high precision for point equality to avoid floating point issues
        const EPSILON: f64 = 1e-10;
        (self.x - other.x).abs() < EPSILON && (self.y - other.y).abs() < EPSILON
    }
}

impl Eq for HashableFloat2D {}

impl Hash for HashableFloat2D {
    fn hash<H: Hasher>(&self, state: &mut H) {
        // Use rounded values for hashing to handle floating point imprecision
        let precision = 1_000_000.0; // 6 decimal places
        let x_rounded = (self.x * precision).round() as i64;
        let y_rounded = (self.y * precision).round() as i64;

        x_rounded.hash(state);
        y_rounded.hash(state);
    }
}

/// A* search algorithm planner
#[derive(Debug)]
pub struct AStarPlanner {
    // Optional: configuration for the planner
    max_iterations: Option<usize>,
    weight: f64,
}

impl Default for AStarPlanner {
    fn default() -> Self {
        Self::new()
    }
}

impl AStarPlanner {
    /// Create a new A* planner with default configuration
    pub fn new() -> Self {
        AStarPlanner {
            max_iterations: None,
            weight: 1.0,
        }
    }

    /// Set the maximum number of iterations
    pub fn with_max_iterations(mut self, maxiterations: usize) -> Self {
        self.max_iterations = Some(maxiterations);
        self
    }

    /// Set the heuristic weight (for weighted A*)
    pub fn with_weight(mut self, weight: f64) -> Self {
        if weight < 0.0 {
            self.weight = 0.0;
        } else {
            self.weight = weight;
        }
        self
    }

    /// Run the A* search algorithm
    ///
    /// # Arguments
    ///
    /// * `start` - The start state
    /// * `goal` - The goal state
    /// * `neighbors_fn` - Function that returns neighboring states and costs
    /// * `heuristic_fn` - Function that estimates the cost from a state to the goal
    ///
    /// # Returns
    ///
    /// * `Ok(Some(Path))` - A path was found
    /// * `Ok(None)` - No path was found
    /// * `Err(SpatialError)` - An error occurred
    pub fn search<N: Clone + Eq + Hash>(
        &self,
        start: N,
        goal: N,
        neighbors_fn: &dyn Fn(&N) -> Vec<(N, f64)>,
        heuristic_fn: &dyn Fn(&N, &N) -> f64,
    ) -> SpatialResult<Option<Path<N>>> {
        // Initialize the open set with the start node
        let mut open_set = BinaryHeap::new();
        let mut closed_set = HashMap::new();

        let h_start = heuristic_fn(&start, &goal);
        let start_node = Rc::new(Node::new(start, None, 0.0, self.weight * h_start));
        open_set.push(Rc::clone(&start_node));

        // Keep track of the best g-value for each state
        let mut g_values = HashMap::new();
        g_values.insert(start_node.state.clone(), 0.0);

        let mut iterations = 0;

        while let Some(current) = open_set.pop() {
            // Check if we've reached the goal
            if current.state == goal {
                return Ok(Some(AStarPlanner::reconstruct_path(goal.clone(), current)));
            }

            // Check if we've exceeded the maximum number of iterations
            if let Some(max_iter) = self.max_iterations {
                iterations += 1;
                if iterations > max_iter {
                    return Ok(None);
                }
            }

            // Skip if we've already processed this state
            if closed_set.contains_key(&current.state) {
                continue;
            }

            // Add the current node to the closed set
            closed_set.insert(current.state.clone(), Rc::clone(&current));

            // Process each neighbor
            for (neighbor_state, cost) in neighbors_fn(&current.state) {
                // Skip if the neighbor is already in the closed set
                if closed_set.contains_key(&neighbor_state) {
                    continue;
                }

                // Calculate the tentative g-value
                let tentative_g = current.g + cost;

                // Check if we've found a better path to the neighbor
                let in_open_set = g_values.contains_key(&neighbor_state);
                if in_open_set
                    && tentative_g >= *g_values.get(&neighbor_state).expect("Operation failed")
                {
                    continue;
                }

                // Update the g-value
                g_values.insert(neighbor_state.clone(), tentative_g);

                // Create a new node for the neighbor
                let h = self.weight * heuristic_fn(&neighbor_state, &goal);
                let neighbor_node = Rc::new(Node::new(
                    neighbor_state,
                    Some(Rc::clone(&current)),
                    tentative_g,
                    h,
                ));

                // Add the neighbor to the open set
                open_set.push(neighbor_node);
            }
        }

        // If we've exhausted the open set without finding the goal, there's no path
        Ok(None)
    }

    // Reconstruct the path from the goal node to the start node
    fn reconstruct_path<N: Clone + Eq + Hash>(goal: N, node: Rc<Node<N>>) -> Path<N> {
        let mut path = Vec::new();
        // The path's total cost is the accumulated g-value at the goal node.
        // Capture it up front: the loop below walks backwards from goal to
        // start via `parent`, and the start node's own g-value is always
        // 0.0, so overwriting `cost` on every iteration (as a previous
        // version of this function did) always produced 0.0 instead of the
        // real path cost.
        let cost = node.g;
        let mut current = Some(node);

        while let Some(_node) = current {
            path.push(_node.state.clone());
            current = _node.parent.clone();
        }

        // Reverse the path so it goes from start to goal
        path.reverse();

        Path::new(path, cost)
    }
}

// Useful heuristic functions

/// Manhattan distance heuristic for 2D grid-based pathfinding
#[allow(dead_code)]
pub fn manhattan_distance(a: &[i32; 2], b: &[i32; 2]) -> f64 {
    ((a[0] - b[0]).abs() + (a[1] - b[1]).abs()) as f64
}

/// Euclidean distance heuristic for continuous 2D space
#[allow(dead_code)]
pub fn euclidean_distance_2d(a: &[f64; 2], b: &[f64; 2]) -> f64 {
    let dx = a[0] - b[0];
    let dy = a[1] - b[1];
    (dx * dx + dy * dy).sqrt()
}

/// Euclidean distance for n-dimensional points
#[allow(dead_code)]
pub fn euclidean_distance(a: &ArrayView1<f64>, b: &ArrayView1<f64>) -> SpatialResult<f64> {
    if a.len() != b.len() {
        return Err(SpatialError::DimensionError(format!(
            "Mismatched dimensions: {} and {}",
            a.len(),
            b.len()
        )));
    }

    let mut sum = 0.0;
    for i in 0..a.len() {
        let diff = a[i] - b[i];
        sum += diff * diff;
    }

    Ok(sum.sqrt())
}

/// Grid-based A* planner for 2D grids with obstacles
#[derive(Clone)]
pub struct GridAStarPlanner {
    pub grid: Vec<Vec<bool>>, // true if cell is an obstacle
    pub diagonalsallowed: bool,
}

impl GridAStarPlanner {
    /// Create a new grid-based A* planner
    ///
    /// # Arguments
    ///
    /// * `grid` - 2D grid where true represents an obstacle
    /// * `diagonalsallowed` - Whether diagonal movements are allowed
    pub fn new(grid: Vec<Vec<bool>>, diagonalsallowed: bool) -> Self {
        GridAStarPlanner {
            grid,
            diagonalsallowed,
        }
    }

    /// Get the height of the grid
    pub fn height(&self) -> usize {
        self.grid.len()
    }

    /// Get the width of the grid
    pub fn width(&self) -> usize {
        if self.grid.is_empty() {
            0
        } else {
            self.grid[0].len()
        }
    }

    /// Check if a position is valid and not an obstacle
    pub fn is_valid(&self, pos: &[i32; 2]) -> bool {
        let (rows, cols) = (self.height() as i32, self.width() as i32);

        if pos[0] < 0 || pos[0] >= rows || pos[1] < 0 || pos[1] >= cols {
            return false;
        }

        !self.grid[pos[0] as usize][pos[1] as usize]
    }

    /// Get valid neighbors for a given position
    fn get_neighbors(&self, pos: &[i32; 2]) -> Vec<([i32; 2], f64)> {
        let mut neighbors = Vec::new();
        let directions = if self.diagonalsallowed {
            // Include diagonal directions
            vec![
                [-1, 0],
                [1, 0],
                [0, -1],
                [0, 1], // Cardinal directions
                [-1, -1],
                [-1, 1],
                [1, -1],
                [1, 1], // Diagonal directions
            ]
        } else {
            // Only cardinal directions
            vec![[-1, 0], [1, 0], [0, -1], [0, 1]]
        };

        for dir in directions {
            let neighbor = [pos[0] + dir[0], pos[1] + dir[1]];
            if self.is_valid(&neighbor) {
                // Cost is 1.0 for cardinal moves, sqrt(2) for diagonal moves
                let cost = if dir[0] != 0 && dir[1] != 0 {
                    std::f64::consts::SQRT_2
                } else {
                    1.0
                };
                neighbors.push((neighbor, cost));
            }
        }

        neighbors
    }

    /// Find a path from start to goal using A* search
    pub fn find_path(
        &self,
        start: [i32; 2],
        goal: [i32; 2],
    ) -> SpatialResult<Option<Path<[i32; 2]>>> {
        // Check that start and goal are valid positions
        if !self.is_valid(&start) {
            return Err(SpatialError::ValueError(
                "Start position is invalid or an obstacle".to_string(),
            ));
        }
        if !self.is_valid(&goal) {
            return Err(SpatialError::ValueError(
                "Goal position is invalid or an obstacle".to_string(),
            ));
        }

        let planner = AStarPlanner::new();
        let grid_clone = self.clone();
        let neighbors_fn = move |pos: &[i32; 2]| grid_clone.get_neighbors(pos);
        let heuristic_fn = |a: &[i32; 2], b: &[i32; 2]| manhattan_distance(a, b);

        planner.search(start, goal, &neighbors_fn, &heuristic_fn)
    }
}

/// 2D continuous space A* planner with polygon obstacles
#[derive(Clone)]
pub struct ContinuousAStarPlanner {
    /// Obstacle polygons (each polygon is a vector of 2D points)
    pub obstacles: Vec<Vec<[f64; 2]>>,
    /// Step size for edge discretization
    pub step_size: f64,
    /// Collision distance threshold
    pub collisionthreshold: f64,
}

impl ContinuousAStarPlanner {
    /// Create a new continuous space A* planner
    pub fn new(obstacles: Vec<Vec<[f64; 2]>>, step_size: f64, collisionthreshold: f64) -> Self {
        ContinuousAStarPlanner {
            obstacles,
            step_size,
            collisionthreshold,
        }
    }

    /// Check if a point is in collision with any obstacle
    pub fn is_in_collision(&self, point: &[f64; 2]) -> bool {
        for obstacle in &self.obstacles {
            if Self::point_in_polygon(point, obstacle) {
                return true;
            }
        }
        false
    }

    /// Check if a line segment intersects with any obstacle
    pub fn line_in_collision(&self, start: &[f64; 2], end: &[f64; 2]) -> bool {
        // Discretize the line and check each point
        let dx = end[0] - start[0];
        let dy = end[1] - start[1];
        let distance = (dx * dx + dy * dy).sqrt();
        let steps = (distance / self.step_size).ceil() as usize;

        if steps == 0 {
            return self.is_in_collision(start) || self.is_in_collision(end);
        }

        for i in 0..=steps {
            let t = i as f64 / steps as f64;
            let x = start[0] + dx * t;
            let y = start[1] + dy * t;
            if self.is_in_collision(&[x, y]) {
                return true;
            }
        }

        false
    }

    /// Point-in-polygon test using ray casting algorithm
    fn point_in_polygon(point: &[f64; 2], polygon: &[[f64; 2]]) -> bool {
        if polygon.len() < 3 {
            return false;
        }

        let mut inside = false;
        let mut j = polygon.len() - 1;

        for i in 0..polygon.len() {
            let xi = polygon[i][0];
            let yi = polygon[i][1];
            let xj = polygon[j][0];
            let yj = polygon[j][1];

            let intersect = ((yi > point[1]) != (yj > point[1]))
                && (point[0] < (xj - xi) * (point[1] - yi) / (yj - yi) + xi);

            if intersect {
                inside = !inside;
            }

            j = i;
        }

        inside
    }

    /// Get valid neighbors for continuous space planning
    fn get_neighbors(&self, pos: &[f64; 2], radius: f64) -> Vec<([f64; 2], f64)> {
        let mut neighbors = Vec::new();

        // Generate neighbors in a circle around the current position
        let num_samples = 8; // Number of directions to sample

        for i in 0..num_samples {
            let angle = 2.0 * std::f64::consts::PI * (i as f64) / (num_samples as f64);
            let nx = pos[0] + radius * angle.cos();
            let ny = pos[1] + radius * angle.sin();
            let neighbor = [nx, ny];

            // Check if the path to the neighbor is collision-free
            if !self.line_in_collision(pos, &neighbor) {
                let cost = radius; // Cost is the distance
                neighbors.push((neighbor, cost));
            }
        }

        neighbors
    }

    /// Find a path from start to goal in continuous space
    pub fn find_path(
        &self,
        start: [f64; 2],
        goal: [f64; 2],
        neighbor_radius: f64,
    ) -> SpatialResult<Option<Path<[f64; 2]>>> {
        // Create equality function for f64 points
        #[derive(Clone, Hash, PartialEq, Eq)]
        struct Point2D {
            x: i64,
            y: i64,
        }

        // Convert to discrete points for Eq/Hash
        let precision = 1000.0; // 3 decimal places
        let to_point = |p: [f64; 2]| -> Point2D {
            Point2D {
                x: (p[0] * precision).round() as i64,
                y: (p[1] * precision).round() as i64,
            }
        };

        let start_point = to_point(start);
        let goal_point = to_point(goal);

        // Check that start and goal are not in collision
        if self.is_in_collision(&start) {
            return Err(SpatialError::ValueError(
                "Start position is in collision with an obstacle".to_string(),
            ));
        }
        if self.is_in_collision(&goal) {
            return Err(SpatialError::ValueError(
                "Goal position is in collision with an obstacle".to_string(),
            ));
        }

        // If there's a direct path, return it immediately
        if !self.line_in_collision(&start, &goal) {
            let path = vec![start, goal];
            let cost = euclidean_distance_2d(&start, &goal);
            return Ok(Some(Path::new(path, cost)));
        }

        let planner = AStarPlanner::new();
        let radius = neighbor_radius;
        let planner_clone = self.clone();

        // Create neighbor and heuristic functions that work with the Point2D type
        let neighbors_fn = move |pos: &Point2D| {
            let float_pos = [pos.x as f64 / precision, pos.y as f64 / precision];
            planner_clone
                .get_neighbors(&float_pos, radius)
                .into_iter()
                .map(|(neighbor, cost)| (to_point(neighbor), cost))
                .collect()
        };

        let heuristic_fn = |a: &Point2D, b: &Point2D| {
            let a_float = [a.x as f64 / precision, a.y as f64 / precision];
            let b_float = [b.x as f64 / precision, b.y as f64 / precision];
            euclidean_distance_2d(&a_float, &b_float)
        };

        // Run A* search with discrete points
        let result = planner.search(start_point, goal_point, &neighbors_fn, &heuristic_fn)?;

        // Convert result back to f64 points
        if let Some(path) = result {
            let float_path = path
                .nodes
                .into_iter()
                .map(|p| [p.x as f64 / precision, p.y as f64 / precision])
                .collect();
            Ok(Some(Path::new(float_path, path.cost)))
        } else {
            Ok(None)
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn test_astar_grid_no_obstacles() {
        // Create a 5x5 grid with no obstacles
        let grid = vec![
            vec![false, false, false, false, false],
            vec![false, false, false, false, false],
            vec![false, false, false, false, false],
            vec![false, false, false, false, false],
            vec![false, false, false, false, false],
        ];

        let planner = GridAStarPlanner::new(grid, false);
        let start = [0, 0];
        let goal = [4, 4];

        let path = planner
            .find_path(start, goal)
            .expect("Operation failed")
            .expect("Operation failed");

        // The path should exist
        assert!(!path.is_empty());

        // The path should start at the start position and end at the goal
        assert_eq!(path.nodes.first().expect("Operation failed"), &start);
        assert_eq!(path.nodes.last().expect("Operation failed"), &goal);

        // The path length should be 9 (start, 7 steps, goal) with only cardinal moves
        assert_eq!(path.len(), 9);

        assert_eq!(path.cost, 8.0);
    }

    #[test]
    fn test_astar_grid_with_obstacles() {
        // Create a 5x5 grid with obstacles forming a wall
        let grid = vec![
            vec![false, false, false, false, false],
            vec![false, false, false, false, false],
            vec![false, true, true, true, false],
            vec![false, false, false, false, false],
            vec![false, false, false, false, false],
        ];

        let planner = GridAStarPlanner::new(grid, false);
        let start = [1, 1];
        let goal = [4, 3];

        let path = planner
            .find_path(start, goal)
            .expect("Operation failed")
            .expect("Operation failed");

        // The path should exist
        assert!(!path.is_empty());

        // The path should start at the start position and end at the goal
        assert_eq!(path.nodes.first().expect("Operation failed"), &start);
        assert_eq!(path.nodes.last().expect("Operation failed"), &goal);

        // The path should go around the obstacles
        // Check that none of the path nodes are obstacles
        for node in &path.nodes {
            assert!(!planner.grid[node[0] as usize][node[1] as usize]);
        }
    }

    #[test]
    fn test_astar_grid_no_path() {
        // Create a 5x5 grid with obstacles forming a complete wall
        let grid = vec![
            vec![false, false, false, false, false],
            vec![false, false, false, false, false],
            vec![true, true, true, true, true],
            vec![false, false, false, false, false],
            vec![false, false, false, false, false],
        ];

        let planner = GridAStarPlanner::new(grid, false);
        let start = [1, 1];
        let goal = [4, 1];

        let path = planner.find_path(start, goal).expect("Operation failed");

        // There should be no path
        assert!(path.is_none());
    }

    #[test]
    fn test_astar_grid_with_diagonals() {
        // Create a 5x5 grid with no obstacles
        let grid = vec![
            vec![false, false, false, false, false],
            vec![false, false, false, false, false],
            vec![false, false, false, false, false],
            vec![false, false, false, false, false],
            vec![false, false, false, false, false],
        ];

        let planner = GridAStarPlanner::new(grid, true);
        let start = [0, 0];
        let goal = [4, 4];

        let path = planner
            .find_path(start, goal)
            .expect("Operation failed")
            .expect("Operation failed");

        // The path should exist
        assert!(!path.is_empty());

        // The path should start at the start position and end at the goal
        assert_eq!(path.nodes.first().expect("Operation failed"), &start);
        assert_eq!(path.nodes.last().expect("Operation failed"), &goal);

        // With diagonals, the path should be shorter (5 nodes: start, 3 diagonal steps, goal)
        assert_eq!(path.len(), 5);

        assert!((path.cost - 4.0 * std::f64::consts::SQRT_2).abs() < 1e-6);
    }
}