|
| 1 | +//! The walkability grid over a map's tiles, and pathfinding across it. A* step costs are |
| 2 | +//! fixed-point — one tile = 1000, a diagonal = √2 ≈ 1414 — so search stays deterministic and the |
| 3 | +//! octile heuristic is exact. |
| 4 | +
|
| 5 | +use crate::core::math::{Pos, Size, Tiles}; |
| 6 | + |
| 7 | +const ORTHOGONAL: u32 = 1000; |
| 8 | +const DIAGONAL: u32 = 1414; |
| 9 | + |
| 10 | +/// A walkability grid in tile space. A [`Pos<Tiles>`] selects the integer cell it falls in, so |
| 11 | +/// callers pass world positions directly without flooring them first. |
| 12 | +#[derive(Clone)] |
| 13 | +pub struct Grid { |
| 14 | + size: Size<Tiles>, |
| 15 | + walkable: Vec<bool>, |
| 16 | +} |
| 17 | + |
| 18 | +impl Grid { |
| 19 | + pub fn new(size: Size<Tiles>, walkable: Vec<bool>) -> Grid { |
| 20 | + Grid { size, walkable } |
| 21 | + } |
| 22 | + |
| 23 | + pub fn size(&self) -> Size<Tiles> { |
| 24 | + self.size |
| 25 | + } |
| 26 | + |
| 27 | + pub fn walkable(&self, p: Pos<Tiles>) -> bool { |
| 28 | + self.cell_walkable(cell(p)) |
| 29 | + } |
| 30 | + |
| 31 | + /// The nearest walkable cell's position (its lower corner), spiralling outward; `None` if the |
| 32 | + /// whole grid is blocked. |
| 33 | + pub fn nearest_walkable(&self, p: Pos<Tiles>) -> Option<Pos<Tiles>> { |
| 34 | + let from = cell(p); |
| 35 | + if self.cell_walkable(from) { |
| 36 | + return Some(at(from)); |
| 37 | + } |
| 38 | + let (width, height) = self.dims(); |
| 39 | + for radius in 1..=width.max(height) { |
| 40 | + let mut best: Option<(i32, i32)> = None; |
| 41 | + let mut best_d2 = i64::MAX; |
| 42 | + for ny in (from.1 - radius)..=(from.1 + radius) { |
| 43 | + for nx in (from.0 - radius)..=(from.0 + radius) { |
| 44 | + if (nx - from.0).abs() != radius && (ny - from.1).abs() != radius { |
| 45 | + continue; |
| 46 | + } |
| 47 | + let d2 = i64::from(nx - from.0).pow(2) + i64::from(ny - from.1).pow(2); |
| 48 | + if self.cell_walkable((nx, ny)) && d2 < best_d2 { |
| 49 | + best_d2 = d2; |
| 50 | + best = Some((nx, ny)); |
| 51 | + } |
| 52 | + } |
| 53 | + } |
| 54 | + if let Some(best) = best { |
| 55 | + return Some(at(best)); |
| 56 | + } |
| 57 | + } |
| 58 | + None |
| 59 | + } |
| 60 | + |
| 61 | + fn dims(&self) -> (i32, i32) { |
| 62 | + (self.size.x.0 as i32, self.size.y.0 as i32) |
| 63 | + } |
| 64 | + |
| 65 | + fn cell_walkable(&self, c: (i32, i32)) -> bool { |
| 66 | + let (width, height) = self.dims(); |
| 67 | + c.0 >= 0 |
| 68 | + && c.1 >= 0 |
| 69 | + && c.0 < width |
| 70 | + && c.1 < height |
| 71 | + && self.walkable[(c.1 * width + c.0) as usize] |
| 72 | + } |
| 73 | +} |
| 74 | + |
| 75 | +/// The cheapest 8-connected path from `start` to `goal` as cell lower-corner positions, both |
| 76 | +/// inclusive; `None` when either end is blocked or no route exists. |
| 77 | +pub fn astar(grid: &Grid, start: Pos<Tiles>, goal: Pos<Tiles>) -> Option<Vec<Pos<Tiles>>> { |
| 78 | + let start = cell(start); |
| 79 | + let goal = cell(goal); |
| 80 | + if !grid.cell_walkable(start) || !grid.cell_walkable(goal) { |
| 81 | + return None; |
| 82 | + } |
| 83 | + let (path, _cost) = pathfinding::prelude::astar( |
| 84 | + &start, |
| 85 | + |&(x, y)| { |
| 86 | + NEIGHBOURS.iter().filter_map(move |&(dx, dy)| { |
| 87 | + let next = (x + dx, y + dy); |
| 88 | + let cost = if dx != 0 && dy != 0 { |
| 89 | + DIAGONAL |
| 90 | + } else { |
| 91 | + ORTHOGONAL |
| 92 | + }; |
| 93 | + grid.cell_walkable(next).then_some((next, cost)) |
| 94 | + }) |
| 95 | + }, |
| 96 | + |&(x, y)| { |
| 97 | + let dx = (x - goal.0).unsigned_abs(); |
| 98 | + let dy = (y - goal.1).unsigned_abs(); |
| 99 | + (dx.max(dy) - dx.min(dy)) * ORTHOGONAL + dx.min(dy) * DIAGONAL |
| 100 | + }, |
| 101 | + |&node| node == goal, |
| 102 | + )?; |
| 103 | + Some(path.into_iter().map(at).collect()) |
| 104 | +} |
| 105 | + |
| 106 | +const NEIGHBOURS: [(i32, i32); 8] = [ |
| 107 | + (1, 0), |
| 108 | + (-1, 0), |
| 109 | + (0, 1), |
| 110 | + (0, -1), |
| 111 | + (1, 1), |
| 112 | + (1, -1), |
| 113 | + (-1, 1), |
| 114 | + (-1, -1), |
| 115 | +]; |
| 116 | + |
| 117 | +/// The integer cell a position falls in. |
| 118 | +fn cell(p: Pos<Tiles>) -> (i32, i32) { |
| 119 | + (p.x.0.floor() as i32, p.y.0.floor() as i32) |
| 120 | +} |
| 121 | + |
| 122 | +/// A cell's lower-corner position. |
| 123 | +fn at(c: (i32, i32)) -> Pos<Tiles> { |
| 124 | + Pos::new(Tiles(c.0 as f32), Tiles(c.1 as f32)) |
| 125 | +} |
0 commit comments