flatland_pathfinding/

path.rs

//! A* grid pathfinding.

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

use crate::grid::{
    block_circle, cell_center, clear_door_cells, collides_player_at, mark_building_footprint,
    terrain_cost, world_to_cell, NavWorld, SEGMENT_SAMPLE_M,
};
use crate::z_nav::{cell_walkable_for_path, goal_z_for};

#[derive(Clone, Copy, Eq, PartialEq)]
struct OpenNode {
    f: u32,
    g: u32,
    x: i16,
    y: i16,
}

impl Ord for OpenNode {
    fn cmp(&self, other: &Self) -> Ordering {
        other.f.cmp(&self.f).then_with(|| other.g.cmp(&self.g))
    }
}

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

struct NavGrid {
    width: i16,
    height: i16,
    blocked: Vec<bool>,
    cost: Vec<u16>,
}

impl NavGrid {
    fn idx(&self, x: i16, y: i16) -> usize {
        (y as usize) * (self.width as usize) + (x as usize)
    }

    fn in_bounds(&self, x: i16, y: i16) -> bool {
        x >= 0 && y >= 0 && x < self.width && y < self.height
    }

    fn is_walkable(&self, x: i16, y: i16) -> bool {
        self.in_bounds(x, y) && !self.blocked[self.idx(x, y)]
    }

    fn move_cost(&self, x: i16, y: i16) -> u32 {
        self.cost[self.idx(x, y)] as u32
    }

    fn set_blocked(&mut self, x: i16, y: i16, blocked: bool) {
        if self.in_bounds(x, y) {
            let idx = self.idx(x, y);
            self.blocked[idx] = blocked;
        }
    }
}

fn build_grid(world: &NavWorld, player_z: f32, goal_z: f32) -> NavGrid {
    let width = world.world_width_m.max(1.0).ceil() as i16;
    let height = world.world_height_m.max(1.0).ceil() as i16;
    let len = (width as usize) * (height as usize);
    let mut blocked = vec![false; len];
    let mut cost = vec![10u16; len];

    for y in 0..height {
        for x in 0..width {
            let cx = x as f32 + 0.5;
            let cy = y as f32 + 0.5;
            let kind = world.terrain_kind_at(cx, cy);
            let tc = terrain_cost(kind);
            let idx = (y as usize) * (width as usize) + (x as usize);
            cost[idx] = tc;
            if tc == u16::MAX {
                blocked[idx] = true;
            }
        }
    }

    let mut grid = NavGrid {
        width,
        height,
        blocked,
        cost,
    };

    for circle in &world.circles {
        block_circle(
            &mut grid.blocked,
            grid.width,
            grid.height,
            circle.x,
            circle.y,
            circle.radius_m,
        );
    }

    for building in &world.buildings {
        mark_building_footprint(&mut grid.blocked, grid.width, grid.height, building);
    }

    clear_door_cells(&mut grid.blocked, grid.width, grid.height, &world.doors);

    // Z-band walkability is O(width*height); skip when the segment has no layers.
    if !world.z_platforms.is_empty() || !world.z_transitions.is_empty() {
        for y in 0..height {
            for x in 0..width {
                let cx = x as f32 + 0.5;
                let cy = y as f32 + 0.5;
                if !cell_walkable_for_path(world, cx, cy, player_z, goal_z) {
                    grid.set_blocked(x, y, true);
                }
            }
        }
    }

    grid
}

fn heuristic(ax: i16, ay: i16, bx: i16, by: i16) -> u32 {
    let dx = (ax - bx).unsigned_abs() as u32;
    let dy = (ay - by).unsigned_abs() as u32;
    let diag = dx.min(dy);
    let straight = dx.max(dy) - diag;
    diag * 14 + straight * 10
}

fn line_clear(grid: &NavGrid, from: (i16, i16), to: (i16, i16)) -> bool {
    let (mut x0, mut y0) = from;
    let (x1, y1) = to;
    let dx = (x1 - x0).abs();
    let dy = (y1 - y0).abs();
    let sx = if x0 < x1 { 1 } else { -1 };
    let sy = if y0 < y1 { 1 } else { -1 };
    let mut err = dx - dy;
    loop {
        if !grid.is_walkable(x0, y0) {
            return false;
        }
        if x0 == x1 && y0 == y1 {
            break;
        }
        let e2 = err * 2;
        if e2 > -dy {
            err -= dy;
            x0 += sx;
        }
        if e2 < dx {
            err += dx;
            y0 += sy;
        }
    }
    true
}

fn simplify_path(
    grid: &NavGrid,
    world: &NavWorld,
    came_from: &HashMap<(i16, i16), (i16, i16)>,
    start: (i16, i16),
    goal: (i16, i16),
    goal_center: (f32, f32),
) -> Vec<(f32, f32)> {
    let mut cells = vec![goal];
    let mut current = goal;
    while current != start {
        let Some(&prev) = came_from.get(&current) else {
            break;
        };
        cells.push(prev);
        current = prev;
    }
    cells.reverse();

    if cells.is_empty() {
        return vec![goal_center];
    }

    let mut waypoints: Vec<(i16, i16)> = Vec::new();
    let mut anchor = 0usize;
    waypoints.push(cells[0]);
    for i in 1..cells.len() {
        if i + 1 < cells.len() {
            let from = cell_center(cells[anchor].0, cells[anchor].1);
            let to = if cells[i + 1] == goal {
                goal_center
            } else {
                cell_center(cells[i + 1].0, cells[i + 1].1)
            };
            if line_clear(grid, cells[anchor], cells[i + 1])
                && segment_clear_world(grid, world, from, to)
            {
                continue;
            }
        }
        waypoints.push(cells[i]);
        anchor = i;
    }

    let mut out: Vec<(f32, f32)> = waypoints.iter().map(|&(x, y)| cell_center(x, y)).collect();
    if let Some(last) = out.last_mut() {
        *last = goal_center;
    }

    if path_segments_clear(grid, world, &out) {
        return out;
    }

    let mut fallback: Vec<(f32, f32)> = cells.iter().map(|&(x, y)| cell_center(x, y)).collect();
    if let Some(last) = fallback.last_mut() {
        *last = goal_center;
    }
    fallback
}

fn path_segments_clear(grid: &NavGrid, world: &NavWorld, path: &[(f32, f32)]) -> bool {
    path.windows(2)
        .all(|w| segment_clear_world(grid, world, w[0], w[1]))
}

fn segment_clear_world(grid: &NavGrid, world: &NavWorld, from: (f32, f32), to: (f32, f32)) -> bool {
    let (fx, fy) = from;
    let (tx, ty) = to;
    let dist = (tx - fx).hypot(ty - fy);
    let steps = (dist / SEGMENT_SAMPLE_M).ceil() as u32 + 1;
    for step in 0..=steps {
        let t = step as f32 / steps as f32;
        let x = fx + (tx - fx) * t;
        let y = fy + (ty - fy) * t;
        if collides_player_at(x, y, world) {
            return false;
        }
    }
    let (cx0, cy0) = world_to_cell(fx, fy);
    let (cx1, cy1) = world_to_cell(tx, ty);
    line_clear(grid, (cx0, cy0), (cx1, cy1))
}

/// Plan a path from `(from_x, from_y, from_z)` to `(to_x, to_y)` using goal z inferred from `from_z`.
pub fn find_path(
    world: &NavWorld,
    from_x: f32,
    from_y: f32,
    from_z: f32,
    to_x: f32,
    to_y: f32,
) -> Option<Vec<(f32, f32)>> {
    let to_z = goal_z_for(world, to_x, to_y, from_z);
    find_path_with_goal_z(world, from_x, from_y, from_z, to_x, to_y, to_z)
}

pub fn find_path_with_goal_z(
    world: &NavWorld,
    from_x: f32,
    from_y: f32,
    from_z: f32,
    to_x: f32,
    to_y: f32,
    to_z: f32,
) -> Option<Vec<(f32, f32)>> {
    let grid = build_grid(world, from_z, to_z);
    let (sx, sy) = world_to_cell(from_x, from_y);
    let (gx, gy) = world_to_cell(to_x, to_y);

    if !grid.in_bounds(sx, sy) || !grid.in_bounds(gx, gy) {
        return None;
    }

    let mut goal_x = gx;
    let mut goal_y = gy;
    if !grid.is_walkable(goal_x, goal_y) {
        let mut found = None;
        'search: for radius in 1..=16i16 {
            for dy in -radius..=radius {
                for dx in -radius..=radius {
                    if dx.abs() != radius && dy.abs() != radius {
                        continue;
                    }
                    let x = gx + dx;
                    let y = gy + dy;
                    if grid.is_walkable(x, y) {
                        found = Some((x, y));
                        break 'search;
                    }
                }
            }
        }
        let (x, y) = found?;
        goal_x = x;
        goal_y = y;
    }

    let goal_key = (goal_x, goal_y);

    let mut start_x = sx;
    let mut start_y = sy;
    if !grid.is_walkable(start_x, start_y) {
        let mut found = None;
        'start: for radius in 1..=16i16 {
            for dy in -radius..=radius {
                for dx in -radius..=radius {
                    if dx.abs() != radius && dy.abs() != radius {
                        continue;
                    }
                    let x = sx + dx;
                    let y = sy + dy;
                    if grid.is_walkable(x, y) {
                        found = Some((x, y));
                        break 'start;
                    }
                }
            }
        }
        let (x, y) = found?;
        start_x = x;
        start_y = y;
    }
    let start_key = (start_x, start_y);

    if start_key == goal_key {
        return Some(vec![cell_center(goal_x, goal_y)]);
    }

    let mut open = BinaryHeap::new();
    let mut g_score: HashMap<(i16, i16), u32> = HashMap::new();
    let mut came_from: HashMap<(i16, i16), (i16, i16)> = HashMap::new();

    g_score.insert(start_key, 0);
    open.push(OpenNode {
        f: heuristic(start_x, start_y, goal_x, goal_y),
        g: 0,
        x: start_x,
        y: start_y,
    });

    const NEIGHBORS: [(i16, i16, u32); 8] = [
        (1, 0, 10),
        (-1, 0, 10),
        (0, 1, 10),
        (0, -1, 10),
        (1, 1, 14),
        (1, -1, 14),
        (-1, 1, 14),
        (-1, -1, 14),
    ];

    while let Some(current) = open.pop() {
        if (current.x, current.y) == goal_key {
            return Some(simplify_path(
                &grid,
                world,
                &came_from,
                start_key,
                goal_key,
                cell_center(goal_x, goal_y),
            ));
        }
        let Some(&best_g) = g_score.get(&(current.x, current.y)) else {
            continue;
        };
        if current.g > best_g {
            continue;
        }

        for (dx, dy, step_base) in NEIGHBORS {
            let nx = current.x + dx;
            let ny = current.y + dy;
            if !grid.is_walkable(nx, ny) {
                continue;
            }
            if dx != 0 && dy != 0 {
                if !grid.is_walkable(current.x + dx, current.y)
                    || !grid.is_walkable(current.x, current.y + dy)
                {
                    continue;
                }
            }
            let from = cell_center(current.x, current.y);
            let to = cell_center(nx, ny);
            if !segment_clear_world(&grid, world, from, to) {
                continue;
            }
            let step = step_base * grid.move_cost(nx, ny) / 10;
            let tentative = best_g + step;
            let key = (nx, ny);
            if tentative >= *g_score.get(&key).unwrap_or(&u32::MAX) {
                continue;
            }
            came_from.insert(key, (current.x, current.y));
            g_score.insert(key, tentative);
            open.push(OpenNode {
                f: tentative + heuristic(nx, ny, goal_x, goal_y),
                g: tentative,
                x: nx,
                y: ny,
            });
        }
    }

    None
}

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

    fn open_world() -> NavWorld {
        NavWorld {
            world_width_m: 64.0,
            world_height_m: 64.0,
            terrain_zones: vec![],
            z_platforms: vec![],
            z_transitions: vec![],
            buildings: vec![],
            doors: vec![],
            circles: vec![],
        }
    }

    #[test]
    fn path_on_open_field() {
        let world = open_world();
        let path = find_path(&world, 10.0, 10.0, 0.0, 20.0, 15.0).expect("path");
        assert!(!path.is_empty());
        let last = *path.last().unwrap();
        assert!((last.0 - 20.5).abs() < 1.0);
        assert!((last.1 - 15.5).abs() < 1.0);
    }

    #[test]
    fn path_routes_around_blocking_tree() {
        let mut world = open_world();
        world.circles.push(crate::grid::NavBlockingCircle {
            x: 15.0,
            y: 12.0,
            radius_m: 0.8,
        });
        let path = find_path(&world, 10.0, 12.0, 0.0, 20.0, 12.0).expect("path around tree");
        for (x, y) in &path {
            let near_tree = (*x - 15.0).abs() < 1.0 && (*y - 12.0).abs() < 1.0;
            assert!(!near_tree, "path should not cut through tree at ({x},{y})");
        }
    }
}