|
| 1 | +import type { Point } from '../types.js'; |
| 2 | + |
| 3 | +export interface JumpPointSearchOptions { |
| 4 | + grid: number[][]; |
| 5 | + start: Point; |
| 6 | + goal: Point; |
| 7 | + allowDiagonal?: boolean; |
| 8 | + heuristic?: (a: Point, b: Point) => number; |
| 9 | +} |
| 10 | + |
| 11 | +interface Node extends Point { |
| 12 | + g: number; |
| 13 | + f: number; |
| 14 | + parent: Node | null; |
| 15 | +} |
| 16 | + |
| 17 | +/** |
| 18 | + * Jump Point Search optimisation for uniform-cost grids. |
| 19 | + * Useful for: large grid pathfinding, RTS unit movement, navigation meshes baked to grids. |
| 20 | + */ |
| 21 | +export function jumpPointSearch(options: JumpPointSearchOptions): Point[] | null { |
| 22 | + const { grid, start, goal, allowDiagonal = true, heuristic = manhattan } = options; |
| 23 | + if (!isWalkable(grid, start.x, start.y) || !isWalkable(grid, goal.x, goal.y)) { |
| 24 | + return null; |
| 25 | + } |
| 26 | + |
| 27 | + const open: Node[] = []; |
| 28 | + const startNode: Node = { ...start, g: 0, f: heuristic(start, goal), parent: null }; |
| 29 | + open.push(startNode); |
| 30 | + const closed = new Set<string>(); |
| 31 | + |
| 32 | + while (open.length > 0) { |
| 33 | + open.sort((a, b) => a.f - b.f); |
| 34 | + const current = open.shift()!; |
| 35 | + const key = nodeKey(current); |
| 36 | + |
| 37 | + if (current.x === goal.x && current.y === goal.y) { |
| 38 | + return reconstruct(current); |
| 39 | + } |
| 40 | + closed.add(key); |
| 41 | + |
| 42 | + const neighbors = identifySuccessors(current, grid, goal, allowDiagonal, heuristic, closed); |
| 43 | + for (const neighbor of neighbors) { |
| 44 | + open.push(neighbor); |
| 45 | + } |
| 46 | + } |
| 47 | + |
| 48 | + return null; |
| 49 | +} |
| 50 | + |
| 51 | +function identifySuccessors( |
| 52 | + node: Node, |
| 53 | + grid: number[][], |
| 54 | + goal: Point, |
| 55 | + allowDiagonal: boolean, |
| 56 | + heuristic: (a: Point, b: Point) => number, |
| 57 | + closed: Set<string> |
| 58 | +): Node[] { |
| 59 | + const successors: Node[] = []; |
| 60 | + const neighbors = pruneNeighbors(node, grid, allowDiagonal); |
| 61 | + |
| 62 | + for (const dir of neighbors) { |
| 63 | + const jumpPoint = jump(node, dir, grid, goal, allowDiagonal); |
| 64 | + if (!jumpPoint) { |
| 65 | + continue; |
| 66 | + } |
| 67 | + |
| 68 | + const key = nodeKey(jumpPoint); |
| 69 | + if (closed.has(key)) { |
| 70 | + continue; |
| 71 | + } |
| 72 | + |
| 73 | + const g = node.g + distance(node, jumpPoint); |
| 74 | + const f = g + heuristic(jumpPoint, goal); |
| 75 | + successors.push({ ...jumpPoint, g, f, parent: node }); |
| 76 | + } |
| 77 | + |
| 78 | + return successors; |
| 79 | +} |
| 80 | + |
| 81 | +function jump( |
| 82 | + node: Node, |
| 83 | + direction: Point, |
| 84 | + grid: number[][], |
| 85 | + goal: Point, |
| 86 | + allowDiagonal: boolean |
| 87 | +): Node | null { |
| 88 | + const next = { x: node.x + direction.x, y: node.y + direction.y }; |
| 89 | + if (!isWalkable(grid, next.x, next.y)) { |
| 90 | + return null; |
| 91 | + } |
| 92 | + if (next.x === goal.x && next.y === goal.y) { |
| 93 | + return { ...next, g: 0, f: 0, parent: null }; |
| 94 | + } |
| 95 | + |
| 96 | + const dx = direction.x; |
| 97 | + const dy = direction.y; |
| 98 | + |
| 99 | + if (dx !== 0 && dy !== 0) { |
| 100 | + if ( |
| 101 | + (isWalkable(grid, next.x - dx, next.y + dy) && !isWalkable(grid, next.x - dx, next.y)) || |
| 102 | + (isWalkable(grid, next.x + dx, next.y - dy) && !isWalkable(grid, next.x, next.y - dy)) |
| 103 | + ) { |
| 104 | + return { ...next, g: 0, f: 0, parent: null }; |
| 105 | + } |
| 106 | + } else { |
| 107 | + if (dx !== 0) { |
| 108 | + if ( |
| 109 | + (isWalkable(grid, next.x + dx, next.y + 1) && !isWalkable(grid, next.x, next.y + 1)) || |
| 110 | + (isWalkable(grid, next.x + dx, next.y - 1) && !isWalkable(grid, next.x, next.y - 1)) |
| 111 | + ) { |
| 112 | + return { ...next, g: 0, f: 0, parent: null }; |
| 113 | + } |
| 114 | + } else if (dy !== 0) { |
| 115 | + if ( |
| 116 | + (isWalkable(grid, next.x + 1, next.y + dy) && !isWalkable(grid, next.x + 1, next.y)) || |
| 117 | + (isWalkable(grid, next.x - 1, next.y + dy) && !isWalkable(grid, next.x - 1, next.y)) |
| 118 | + ) { |
| 119 | + return { ...next, g: 0, f: 0, parent: null }; |
| 120 | + } |
| 121 | + } |
| 122 | + } |
| 123 | + |
| 124 | + if (allowDiagonal && dx !== 0 && dy !== 0) { |
| 125 | + const horiz = jump({ ...next, g: 0, f: 0, parent: null }, { x: dx, y: 0 }, grid, goal, allowDiagonal); |
| 126 | + const vert = jump({ ...next, g: 0, f: 0, parent: null }, { x: 0, y: dy }, grid, goal, allowDiagonal); |
| 127 | + if (horiz || vert) { |
| 128 | + return { ...next, g: 0, f: 0, parent: null }; |
| 129 | + } |
| 130 | + } |
| 131 | + |
| 132 | + return jump({ ...next, g: 0, f: 0, parent: null }, direction, grid, goal, allowDiagonal); |
| 133 | +} |
| 134 | + |
| 135 | +function pruneNeighbors(node: Node, grid: number[][], allowDiagonal: boolean): Point[] { |
| 136 | + const successors: Point[] = []; |
| 137 | + if (!node.parent) { |
| 138 | + return getNeighbors(node, grid, allowDiagonal); |
| 139 | + } |
| 140 | + |
| 141 | + const dx = clamp(node.x - node.parent.x); |
| 142 | + const dy = clamp(node.y - node.parent.y); |
| 143 | + |
| 144 | + if (dx !== 0 && dy !== 0) { |
| 145 | + if (isWalkable(grid, node.x, node.y + dy)) { |
| 146 | + successors.push({ x: 0, y: dy }); |
| 147 | + } |
| 148 | + if (isWalkable(grid, node.x + dx, node.y)) { |
| 149 | + successors.push({ x: dx, y: 0 }); |
| 150 | + } |
| 151 | + if ( |
| 152 | + isWalkable(grid, node.x + dx, node.y + dy) && |
| 153 | + isWalkable(grid, node.x + dx, node.y) && |
| 154 | + isWalkable(grid, node.x, node.y + dy) |
| 155 | + ) { |
| 156 | + successors.push({ x: dx, y: dy }); |
| 157 | + } |
| 158 | + if (!isWalkable(grid, node.x - dx, node.y)) { |
| 159 | + successors.push({ x: -dx, y: dy }); |
| 160 | + } |
| 161 | + if (!isWalkable(grid, node.x, node.y - dy)) { |
| 162 | + successors.push({ x: dx, y: -dy }); |
| 163 | + } |
| 164 | + } else { |
| 165 | + if (dx === 0) { |
| 166 | + if (isWalkable(grid, node.x, node.y + dy)) { |
| 167 | + successors.push({ x: 0, y: dy }); |
| 168 | + if (!allowDiagonal) { |
| 169 | + if (!isWalkable(grid, node.x + 1, node.y)) { |
| 170 | + successors.push({ x: 1, y: dy }); |
| 171 | + } |
| 172 | + if (!isWalkable(grid, node.x - 1, node.y)) { |
| 173 | + successors.push({ x: -1, y: dy }); |
| 174 | + } |
| 175 | + } |
| 176 | + } |
| 177 | + if (allowDiagonal) { |
| 178 | + if (!isWalkable(grid, node.x + 1, node.y)) { |
| 179 | + successors.push({ x: 1, y: dy }); |
| 180 | + } |
| 181 | + if (!isWalkable(grid, node.x - 1, node.y)) { |
| 182 | + successors.push({ x: -1, y: dy }); |
| 183 | + } |
| 184 | + } |
| 185 | + } else { |
| 186 | + if (isWalkable(grid, node.x + dx, node.y)) { |
| 187 | + successors.push({ x: dx, y: 0 }); |
| 188 | + if (!allowDiagonal) { |
| 189 | + if (!isWalkable(grid, node.x, node.y + 1)) { |
| 190 | + successors.push({ x: dx, y: 1 }); |
| 191 | + } |
| 192 | + if (!isWalkable(grid, node.x, node.y - 1)) { |
| 193 | + successors.push({ x: dx, y: -1 }); |
| 194 | + } |
| 195 | + } |
| 196 | + } |
| 197 | + if (allowDiagonal) { |
| 198 | + if (!isWalkable(grid, node.x, node.y + 1)) { |
| 199 | + successors.push({ x: dx, y: 1 }); |
| 200 | + } |
| 201 | + if (!isWalkable(grid, node.x, node.y - 1)) { |
| 202 | + successors.push({ x: dx, y: -1 }); |
| 203 | + } |
| 204 | + } |
| 205 | + } |
| 206 | + } |
| 207 | + |
| 208 | + return successors; |
| 209 | +} |
| 210 | + |
| 211 | +function getNeighbors(node: Point, grid: number[][], allowDiagonal: boolean): Point[] { |
| 212 | + const neighbors: Point[] = []; |
| 213 | + for (let dy = -1; dy <= 1; dy += 1) { |
| 214 | + for (let dx = -1; dx <= 1; dx += 1) { |
| 215 | + if (dx === 0 && dy === 0) { |
| 216 | + continue; |
| 217 | + } |
| 218 | + if (!allowDiagonal && dx !== 0 && dy !== 0) { |
| 219 | + continue; |
| 220 | + } |
| 221 | + const nx = node.x + dx; |
| 222 | + const ny = node.y + dy; |
| 223 | + if (!isWalkable(grid, nx, ny)) { |
| 224 | + continue; |
| 225 | + } |
| 226 | + if (dx !== 0 && dy !== 0) { |
| 227 | + if (!isWalkable(grid, node.x + dx, node.y) || !isWalkable(grid, node.x, node.y + dy)) { |
| 228 | + continue; |
| 229 | + } |
| 230 | + } |
| 231 | + neighbors.push({ x: dx, y: dy }); |
| 232 | + } |
| 233 | + } |
| 234 | + return neighbors; |
| 235 | +} |
| 236 | + |
| 237 | +function isWalkable(grid: number[][], x: number, y: number): boolean { |
| 238 | + return grid[y]?.[x] === 0; |
| 239 | +} |
| 240 | + |
| 241 | +function nodeKey(node: Point): string { |
| 242 | + return `${node.x}:${node.y}`; |
| 243 | +} |
| 244 | + |
| 245 | +function reconstruct(node: Node | null): Point[] { |
| 246 | + const raw: Point[] = []; |
| 247 | + let current: Node | null = node; |
| 248 | + while (current) { |
| 249 | + raw.push({ x: current.x, y: current.y }); |
| 250 | + current = current.parent; |
| 251 | + } |
| 252 | + raw.reverse(); |
| 253 | + if (raw.length <= 1) { |
| 254 | + return raw; |
| 255 | + } |
| 256 | + |
| 257 | + const expanded: Point[] = []; |
| 258 | + for (let i = 0; i < raw.length - 1; i += 1) { |
| 259 | + const start = raw[i]; |
| 260 | + const end = raw[i + 1]; |
| 261 | + if (i === 0) { |
| 262 | + expanded.push({ ...start }); |
| 263 | + } |
| 264 | + const stepX = clamp(end.x - start.x); |
| 265 | + const stepY = clamp(end.y - start.y); |
| 266 | + let cx = start.x; |
| 267 | + let cy = start.y; |
| 268 | + while (cx !== end.x || cy !== end.y) { |
| 269 | + cx += stepX; |
| 270 | + cy += stepY; |
| 271 | + expanded.push({ x: cx, y: cy }); |
| 272 | + } |
| 273 | + } |
| 274 | + |
| 275 | + return expanded; |
| 276 | +} |
| 277 | + |
| 278 | +function distance(a: Point, b: Point): number { |
| 279 | + const dx = Math.abs(a.x - b.x); |
| 280 | + const dy = Math.abs(a.y - b.y); |
| 281 | + return dx && dy ? Math.sqrt(dx * dx + dy * dy) : dx + dy; |
| 282 | +} |
| 283 | + |
| 284 | +function clamp(value: number): number { |
| 285 | + if (value > 0) return 1; |
| 286 | + if (value < 0) return -1; |
| 287 | + return 0; |
| 288 | +} |
| 289 | + |
| 290 | +function manhattan(a: Point, b: Point): number { |
| 291 | + return Math.abs(a.x - b.x) + Math.abs(a.y - b.y); |
| 292 | +} |
0 commit comments