rustsim_pathfinding/

continuous_astar.rs

//! Continuous-space A* pathfinding.
//!
//! Discretizes a continuous 2D or 3D space into a grid, runs A* on the grid,
//! then maps the resulting path back to continuous coordinates. This mirrors
//! Julia Agents.jl `astar_continuous.jl`.
//!
//! # How it works
//!
//! 1. The continuous space `[0, extent_x) x [0, extent_y)` is overlaid with
//!    a walkability grid of `grid_w x grid_h` cells.
//! 2. Continuous positions are converted to discrete grid cells.
//! 3. A* runs on the grid using the configured cost metric.
//! 4. The discrete path is converted back to continuous waypoints
//!    (cell centers), with anti-backtracking optimization on the
//!    last waypoint.
//!
//! # Example
//!
//! ```
//! use rustsim_pathfinding::continuous_astar::{ContinuousAStar, ContinuousAStarOpts};
//!
//! // 100x100 continuous space, discretized to a 50x50 walkability grid
//! let walkmap = vec![true; 50 * 50]; // all walkable
//! let pathfinder = ContinuousAStar::new(
//!     100.0, 100.0,
//!     &walkmap, 50, 50,
//!     ContinuousAStarOpts::default(),
//! )
//! .unwrap();
//!
//! let path = pathfinder.find_path((10.0, 10.0), (90.0, 90.0));
//! assert!(path.is_some());
//! ```

use crate::astar::{astar_grid2d_opts, GridAStarOpts};
use crate::metrics::{CostMetric, DirectDistance};
use thiserror::Error;

/// Errors returned by continuous A* configuration validation.
#[derive(Debug, Clone, Copy, PartialEq, Eq, Error)]
pub enum ContinuousAStarConfigError {
    #[error("continuous-space extents must be positive")]
    InvalidExtent,
    #[error("grid dimensions must be positive")]
    InvalidGridDimensions,
    #[error("walkmap size mismatch: expected {expected}, got {actual}")]
    WalkmapSizeMismatch { expected: usize, actual: usize },
}

/// Options for continuous-space A* pathfinding.
#[derive(Debug)]
pub struct ContinuousAStarOpts {
    /// Allow diagonal movement on the underlying grid.
    pub diagonal: bool,
    /// Periodic (toroidal) boundary wrapping.
    pub periodic: bool,
    /// Admissibility factor (0.0 = optimal, higher = faster but suboptimal).
    pub admissibility: f64,
}

impl Default for ContinuousAStarOpts {
    fn default() -> Self {
        Self {
            diagonal: true,
            periodic: false,
            admissibility: 0.0,
        }
    }
}

/// Result of a continuous-space A* search.
#[derive(Debug, Clone)]
pub struct ContinuousPath {
    /// Waypoints in continuous coordinates, from (near) start to goal.
    /// The first waypoint is NOT the start position -- it is the first
    /// cell center the agent should walk toward. The last waypoint is
    /// the exact goal position.
    pub waypoints: Vec<(f64, f64)>,
    /// Approximate total cost (from the underlying grid A*).
    pub cost: f64,
}

/// 2D continuous-space A* pathfinder.
///
/// Wraps a walkability grid and continuous-space extent. Use
/// [`find_path`](Self::find_path) to compute paths between continuous
/// positions.
pub struct ContinuousAStar<'a> {
    extent_x: f64,
    extent_y: f64,
    walkmap: &'a [bool],
    grid_w: usize,
    grid_h: usize,
    opts: ContinuousAStarOpts,
    metric: Option<Box<dyn CostMetric>>,
}

impl<'a> ContinuousAStar<'a> {
    /// Create a new continuous-space pathfinder.
    pub fn new(
        extent_x: f64,
        extent_y: f64,
        walkmap: &'a [bool],
        grid_w: usize,
        grid_h: usize,
        opts: ContinuousAStarOpts,
    ) -> Result<Self, ContinuousAStarConfigError> {
        if extent_x <= 0.0 || extent_y <= 0.0 {
            return Err(ContinuousAStarConfigError::InvalidExtent);
        }
        if grid_w == 0 || grid_h == 0 {
            return Err(ContinuousAStarConfigError::InvalidGridDimensions);
        }
        let expected = grid_w * grid_h;
        if walkmap.len() != expected {
            return Err(ContinuousAStarConfigError::WalkmapSizeMismatch {
                expected,
                actual: walkmap.len(),
            });
        }
        Ok(Self {
            extent_x,
            extent_y,
            walkmap,
            grid_w,
            grid_h,
            opts,
            metric: None,
        })
    }

    /// Set a custom cost metric for the underlying grid A*.
    pub fn with_metric(mut self, metric: impl CostMetric + 'static) -> Self {
        self.metric = Some(Box::new(metric));
        self
    }

    /// Convert a continuous position to a discrete grid cell.
    fn to_discrete(&self, pos: (f64, f64)) -> (usize, usize) {
        let gx = (pos.0 / self.extent_x * self.grid_w as f64)
            .floor()
            .max(0.0) as usize;
        let gy = (pos.1 / self.extent_y * self.grid_h as f64)
            .floor()
            .max(0.0) as usize;
        (gx.min(self.grid_w - 1), gy.min(self.grid_h - 1))
    }

    /// Convert a discrete grid cell to the continuous position at its center.
    fn to_continuous(&self, cell: (usize, usize)) -> (f64, f64) {
        let cell_w = self.extent_x / self.grid_w as f64;
        let cell_h = self.extent_y / self.grid_h as f64;
        (
            cell.0 as f64 * cell_w + cell_w * 0.5,
            cell.1 as f64 * cell_h + cell_h * 0.5,
        )
    }

    /// Squared distance between two continuous positions, respecting periodicity.
    fn sqr_distance(&self, a: (f64, f64), b: (f64, f64)) -> f64 {
        let mut dx = (a.0 - b.0).abs();
        let mut dy = (a.1 - b.1).abs();
        if self.opts.periodic {
            if dx > self.extent_x * 0.5 {
                dx = self.extent_x - dx;
            }
            if dy > self.extent_y * 0.5 {
                dy = self.extent_y - dy;
            }
        }
        dx * dx + dy * dy
    }

    /// Find a path from `from` to `to` in continuous coordinates.
    ///
    /// Returns `None` if no path exists (start or goal is unwalkable,
    /// or no connected path through walkable cells).
    ///
    /// The returned [`ContinuousPath`] contains waypoints in continuous
    /// coordinates. The last waypoint is the exact `to` position.
    pub fn find_path(&self, from: (f64, f64), to: (f64, f64)) -> Option<ContinuousPath> {
        let discrete_from = self.to_discrete(from);
        let discrete_to = self.to_discrete(to);

        let walkmap = self.walkmap;
        let grid_w = self.grid_w;
        let walkable_fn = move |x: usize, y: usize| -> bool { walkmap[y * grid_w + x] };

        let default_metric = DirectDistance::new();
        let metric_ref: &dyn CostMetric = match &self.metric {
            Some(m) => m.as_ref(),
            None => &default_metric,
        };

        let mut grid_opts = GridAStarOpts::new(self.grid_w, self.grid_h);
        grid_opts.diagonal = self.opts.diagonal;
        grid_opts.periodic = self.opts.periodic;
        grid_opts.admissibility = self.opts.admissibility;
        grid_opts.walkable = Some(&walkable_fn);
        grid_opts.cost_metric = Some(metric_ref);

        let grid_result = astar_grid2d_opts(discrete_from, discrete_to, &grid_opts)?;

        // If the discrete path is just the start cell (from and to are in the
        // same cell), the only waypoint is the exact goal.
        if grid_result.path.len() <= 1 {
            return Some(ContinuousPath {
                waypoints: vec![to],
                cost: grid_result.cost,
            });
        }

        // Convert discrete waypoints to continuous (skip the start cell).
        let mut waypoints: Vec<(f64, f64)> = grid_result.path[1..]
            .iter()
            .map(|&cell| self.to_continuous(cell))
            .collect();

        // Anti-backtracking optimization on the last waypoint.
        // If the second-to-last waypoint is closer to the goal than the last
        // waypoint, remove the last waypoint to prevent overshooting.
        // Mirrors Agents.jl's anti-backtracking logic.
        if waypoints.len() >= 2 {
            let last = waypoints[waypoints.len() - 1];
            let second_last = waypoints[waypoints.len() - 2];
            let last_to_goal = self.sqr_distance(last, to);
            let second_last_to_goal = self.sqr_distance(second_last, to);

            if second_last_to_goal < last_to_goal {
                waypoints.pop();
            }
        } else if waypoints.len() == 1 {
            // Only one waypoint (the last cell center); check if it's
            // farther from goal than the start
            let last = waypoints[0];
            let last_to_goal = self.sqr_distance(last, to);
            if last_to_goal < 1e-12 {
                // Already at goal cell center
                waypoints.clear();
            }
        }

        // Append exact goal position (unless it's already the last waypoint)
        let goal_already_last = waypoints
            .last()
            .map(|&wp| self.sqr_distance(wp, to) < 1e-12)
            .unwrap_or(false);
        if !goal_already_last {
            waypoints.push(to);
        }

        Some(ContinuousPath {
            waypoints,
            cost: grid_result.cost,
        })
    }
}

/// 3D continuous-space A* pathfinder.
///
/// Discretizes a 3D continuous space into a 3D voxel grid and runs A* on it.
/// The 3D A* uses 26-connected (diagonal) or 6-connected (cardinal) neighbors.
pub struct ContinuousAStar3D<'a> {
    extent_x: f64,
    extent_y: f64,
    extent_z: f64,
    walkmap: &'a [bool],
    grid_w: usize,
    grid_h: usize,
    grid_d: usize,
    periodic: bool,
    diagonal: bool,
    admissibility: f64,
}

/// Result of a 3D continuous-space A* search.
#[derive(Debug, Clone)]
pub struct ContinuousPath3D {
    /// Waypoints in continuous 3D coordinates.
    pub waypoints: Vec<(f64, f64, f64)>,
    /// Approximate total cost.
    pub cost: f64,
}

impl<'a> ContinuousAStar3D<'a> {
    /// Create a 3D continuous-space pathfinder.
    pub fn new(
        extent_x: f64,
        extent_y: f64,
        extent_z: f64,
        walkmap: &'a [bool],
        grid_w: usize,
        grid_h: usize,
        grid_d: usize,
    ) -> Result<Self, ContinuousAStarConfigError> {
        if extent_x <= 0.0 || extent_y <= 0.0 || extent_z <= 0.0 {
            return Err(ContinuousAStarConfigError::InvalidExtent);
        }
        if grid_w == 0 || grid_h == 0 || grid_d == 0 {
            return Err(ContinuousAStarConfigError::InvalidGridDimensions);
        }
        let expected = grid_w * grid_h * grid_d;
        if walkmap.len() != expected {
            return Err(ContinuousAStarConfigError::WalkmapSizeMismatch {
                expected,
                actual: walkmap.len(),
            });
        }
        Ok(Self {
            extent_x,
            extent_y,
            extent_z,
            walkmap,
            grid_w,
            grid_h,
            grid_d,
            periodic: false,
            diagonal: true,
            admissibility: 0.0,
        })
    }

    /// Enable periodic boundaries.
    pub fn periodic(mut self, periodic: bool) -> Self {
        self.periodic = periodic;
        self
    }

    /// Enable/disable diagonal movement (26- vs 6-connected).
    pub fn diagonal(mut self, diagonal: bool) -> Self {
        self.diagonal = diagonal;
        self
    }

    /// Set admissibility factor.
    pub fn admissibility(mut self, admissibility: f64) -> Self {
        self.admissibility = admissibility;
        self
    }

    fn to_discrete(&self, pos: (f64, f64, f64)) -> (usize, usize, usize) {
        let gx = (pos.0 / self.extent_x * self.grid_w as f64)
            .floor()
            .max(0.0) as usize;
        let gy = (pos.1 / self.extent_y * self.grid_h as f64)
            .floor()
            .max(0.0) as usize;
        let gz = (pos.2 / self.extent_z * self.grid_d as f64)
            .floor()
            .max(0.0) as usize;
        (
            gx.min(self.grid_w - 1),
            gy.min(self.grid_h - 1),
            gz.min(self.grid_d - 1),
        )
    }

    fn to_continuous(&self, cell: (usize, usize, usize)) -> (f64, f64, f64) {
        let cw = self.extent_x / self.grid_w as f64;
        let ch = self.extent_y / self.grid_h as f64;
        let cd = self.extent_z / self.grid_d as f64;
        (
            cell.0 as f64 * cw + cw * 0.5,
            cell.1 as f64 * ch + ch * 0.5,
            cell.2 as f64 * cd + cd * 0.5,
        )
    }

    fn is_walkable(&self, x: usize, y: usize, z: usize) -> bool {
        self.walkmap[z * self.grid_h * self.grid_w + y * self.grid_w + x]
    }

    fn sqr_distance_3d(&self, a: (f64, f64, f64), b: (f64, f64, f64)) -> f64 {
        let mut dx = (a.0 - b.0).abs();
        let mut dy = (a.1 - b.1).abs();
        let mut dz = (a.2 - b.2).abs();
        if self.periodic {
            if dx > self.extent_x * 0.5 {
                dx = self.extent_x - dx;
            }
            if dy > self.extent_y * 0.5 {
                dy = self.extent_y - dy;
            }
            if dz > self.extent_z * 0.5 {
                dz = self.extent_z - dz;
            }
        }
        dx * dx + dy * dy + dz * dz
    }

    /// Find a path between two 3D continuous positions.
    pub fn find_path(
        &self,
        from: (f64, f64, f64),
        to: (f64, f64, f64),
    ) -> Option<ContinuousPath3D> {
        let d_from = self.to_discrete(from);
        let d_to = self.to_discrete(to);

        if !self.is_walkable(d_from.0, d_from.1, d_from.2)
            || !self.is_walkable(d_to.0, d_to.1, d_to.2)
        {
            return None;
        }

        let admissibility = self.admissibility;
        let periodic = self.periodic;
        let gw = self.grid_w;
        let gh = self.grid_h;
        let gd = self.grid_d;

        let heuristic = move |a: &(usize, usize, usize), b: &(usize, usize, usize)| -> f64 {
            let mut dx = (a.0 as isize - b.0 as isize).unsigned_abs();
            let mut dy = (a.1 as isize - b.1 as isize).unsigned_abs();
            let mut dz = (a.2 as isize - b.2 as isize).unsigned_abs();
            if periodic {
                dx = dx.min(gw - dx);
                dy = dy.min(gh - dy);
                dz = dz.min(gd - dz);
            }
            let mut dims = [dx, dy, dz];
            dims.sort_unstable();
            let min_d = dims[0] as f64;
            let mid_d = dims[1] as f64;
            let max_d = dims[2] as f64;
            // 3D octile distance: diagonal steps cost sqrt(3), face-diagonal sqrt(2), cardinal 1
            let h = min_d * 1.7320508075688772  // sqrt(3)
                + (mid_d - min_d) * std::f64::consts::SQRT_2
                + (max_d - mid_d) * 1.0;
            (1.0 + admissibility) * h
        };

        let walkmap = self.walkmap;
        let diagonal = self.diagonal;

        let neighbors = move |node: &(usize, usize, usize)| -> Vec<((usize, usize, usize), f64)> {
            let (x, y, z) = *node;
            let mut result = Vec::new();

            for &dz in &[-1i32, 0, 1] {
                for &dy in &[-1i32, 0, 1] {
                    for &dx in &[-1i32, 0, 1] {
                        if dx == 0 && dy == 0 && dz == 0 {
                            continue;
                        }
                        // If not diagonal, only allow cardinal moves
                        if !diagonal {
                            let nonzero = (dx != 0) as u32 + (dy != 0) as u32 + (dz != 0) as u32;
                            if nonzero > 1 {
                                continue;
                            }
                        }

                        let nx = x as i32 + dx;
                        let ny = y as i32 + dy;
                        let nz = z as i32 + dz;

                        let neighbor = if periodic {
                            let px = ((nx % gw as i32) + gw as i32) % gw as i32;
                            let py = ((ny % gh as i32) + gh as i32) % gh as i32;
                            let pz = ((nz % gd as i32) + gd as i32) % gd as i32;
                            Some((px as usize, py as usize, pz as usize))
                        } else if nx >= 0
                            && ny >= 0
                            && nz >= 0
                            && (nx as usize) < gw
                            && (ny as usize) < gh
                            && (nz as usize) < gd
                        {
                            Some((nx as usize, ny as usize, nz as usize))
                        } else {
                            None
                        };

                        if let Some(n) = neighbor {
                            if walkmap[n.2 * gh * gw + n.1 * gw + n.0] {
                                let nonzero =
                                    (dx != 0) as u32 + (dy != 0) as u32 + (dz != 0) as u32;
                                let cost = match nonzero {
                                    1 => 1.0,
                                    2 => std::f64::consts::SQRT_2,
                                    _ => 1.7320508075688772, // ?3
                                };
                                result.push((n, cost));
                            }
                        }
                    }
                }
            }

            result
        };

        let grid_result = crate::astar::astar(d_from, d_to, heuristic, neighbors)?;

        if grid_result.path.len() <= 1 {
            return Some(ContinuousPath3D {
                waypoints: vec![to],
                cost: grid_result.cost,
            });
        }

        let mut waypoints: Vec<(f64, f64, f64)> = grid_result.path[1..]
            .iter()
            .map(|&cell| self.to_continuous(cell))
            .collect();

        // Anti-backtracking
        if waypoints.len() >= 2 {
            let last = waypoints[waypoints.len() - 1];
            let second_last = waypoints[waypoints.len() - 2];
            if self.sqr_distance_3d(second_last, to) < self.sqr_distance_3d(last, to) {
                waypoints.pop();
            }
        }

        let goal_already_last = waypoints
            .last()
            .map(|&wp| self.sqr_distance_3d(wp, to) < 1e-12)
            .unwrap_or(false);
        if !goal_already_last {
            waypoints.push(to);
        }

        Some(ContinuousPath3D {
            waypoints,
            cost: grid_result.cost,
        })
    }
}

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

    #[test]
    fn continuous_2d_same_cell() {
        let walkmap = vec![true; 100];
        let pf = ContinuousAStar::new(10.0, 10.0, &walkmap, 10, 10, ContinuousAStarOpts::default())
            .unwrap();
        let result = pf.find_path((0.5, 0.5), (0.9, 0.9));
        let result = result.expect("same-cell path should succeed");
        assert_eq!(result.waypoints.last(), Some(&(0.9, 0.9)));
    }

    #[test]
    fn continuous_2d_across_cells() {
        let walkmap = vec![true; 100];
        let pf = ContinuousAStar::new(10.0, 10.0, &walkmap, 10, 10, ContinuousAStarOpts::default())
            .unwrap();
        let result = pf.find_path((0.5, 0.5), (9.5, 9.5));
        let result = result.expect("path should exist");
        assert!(result.waypoints.len() >= 2);
        assert_eq!(result.waypoints.last(), Some(&(9.5, 9.5)));
    }

    #[test]
    fn continuous_2d_blocked() {
        let mut walkmap = vec![true; 100];
        for y in 0..10 {
            walkmap[y * 10 + 5] = false;
        }
        let pf = ContinuousAStar::new(10.0, 10.0, &walkmap, 10, 10, ContinuousAStarOpts::default())
            .unwrap();
        let result = pf.find_path((0.5, 0.5), (9.5, 9.5));
        assert!(result.is_none(), "wall should block path");
    }

    #[test]
    fn continuous_2d_wall_with_gap() {
        let mut walkmap = vec![true; 100];
        for y in 0..10 {
            if y != 5 {
                walkmap[y * 10 + 5] = false;
            }
        }
        let pf = ContinuousAStar::new(10.0, 10.0, &walkmap, 10, 10, ContinuousAStarOpts::default())
            .unwrap();
        let result = pf.find_path((0.5, 0.5), (9.5, 9.5));
        assert!(result.is_some(), "should find path through gap");
    }

    #[test]
    fn continuous_3d_basic() {
        let walkmap = vec![true; 1000];
        let pf = ContinuousAStar3D::new(10.0, 10.0, 10.0, &walkmap, 10, 10, 10).unwrap();
        let result = pf.find_path((0.5, 0.5, 0.5), (9.5, 9.5, 9.5));
        let result = result.expect("3D path should exist");
        assert!(result.waypoints.len() >= 2);
        assert_eq!(result.waypoints.last(), Some(&(9.5, 9.5, 9.5)));
    }

    #[test]
    fn continuous_3d_blocked() {
        let mut walkmap = vec![true; 1000];
        for y in 0..10 {
            for x in 0..10 {
                walkmap[5 * 100 + y * 10 + x] = false;
            }
        }
        let pf = ContinuousAStar3D::new(10.0, 10.0, 10.0, &walkmap, 10, 10, 10)
            .unwrap()
            .diagonal(false);
        let result = pf.find_path((0.5, 0.5, 0.5), (9.5, 9.5, 9.5));
        assert!(result.is_none(), "blocked z-plane should prevent path");
    }
}