Improved pathfinding
This commit is contained in:
@ -1,84 +1,183 @@
|
||||
// utilities/AStar.ts
|
||||
|
||||
type Node = {
|
||||
x: number
|
||||
y: number
|
||||
parent?: Node
|
||||
g: number
|
||||
h: number
|
||||
f: number
|
||||
interface Position {
|
||||
x: number;
|
||||
y: number;
|
||||
}
|
||||
|
||||
class AStar {
|
||||
static findPath(start: Node, end: Node, grid: number[][]): Node[] {
|
||||
// Initialize the open and closed lists
|
||||
let openList: Node[] = []
|
||||
let closedList: Node[] = []
|
||||
export interface Node extends Position {
|
||||
parent?: Node;
|
||||
g: number;
|
||||
h: number;
|
||||
f: number;
|
||||
}
|
||||
|
||||
// Push the start node onto the open list
|
||||
openList.push(start)
|
||||
/**
|
||||
* A* pathfinding algorithm.
|
||||
*/
|
||||
class AStar {
|
||||
private static readonly ORTHOGONAL_DIRECTIONS: Position[] = [
|
||||
{ x: 0, y: -1 }, // up
|
||||
{ x: 0, y: 1 }, // down
|
||||
{ x: -1, y: 0 }, // left
|
||||
{ x: 1, y: 0 } // right
|
||||
];
|
||||
|
||||
private static readonly DIAGONAL_DIRECTIONS: Position[] = [
|
||||
{ x: -1, y: -1 }, // up-left
|
||||
{ x: -1, y: 1 }, // down-left
|
||||
{ x: 1, y: -1 }, // up-right
|
||||
{ x: 1, y: 1 } // down-right
|
||||
];
|
||||
|
||||
/**
|
||||
* Finds the shortest path from start to end on the given grid.
|
||||
* @param start - Start position.
|
||||
* @param end - End position.
|
||||
* @param grid - The grid representing the map (0 = open space, 1 = obstacle).
|
||||
* @param allowDiagonal - Whether diagonal movements are allowed.
|
||||
* @returns Array of `Node` representing the path from start to end.
|
||||
*/
|
||||
static findPath(start: Position, end: Position, grid: number[][], allowDiagonal: boolean = false): Node[] {
|
||||
const openList: Node[] = [];
|
||||
const closedSet = new Set<string>();
|
||||
|
||||
const startNode: Node = { ...start, g: 0, h: 0, f: 0 };
|
||||
openList.push(startNode);
|
||||
|
||||
while (openList.length > 0) {
|
||||
// Get the node with the lowest f value
|
||||
let currentNode = openList.reduce((prev, curr) => (prev.f < curr.f ? prev : curr))
|
||||
const currentNode = this.getLowestFScoreNode(openList);
|
||||
|
||||
// Move the current node to the closed list
|
||||
openList = openList.filter((node) => node !== currentNode)
|
||||
closedList.push(currentNode)
|
||||
|
||||
// Check if we've reached the end
|
||||
if (currentNode.x === end.x && currentNode.y === end.y) {
|
||||
let path: Node[] = []
|
||||
let curr = currentNode
|
||||
while (curr) {
|
||||
path.push(curr)
|
||||
curr = curr.parent as Node
|
||||
}
|
||||
return path.reverse()
|
||||
if (this.isEndNode(currentNode, end)) {
|
||||
return this.reconstructPath(currentNode);
|
||||
}
|
||||
|
||||
// Get the neighboring nodes
|
||||
let neighbors = this.getNeighbors(currentNode, grid)
|
||||
for (let neighbor of neighbors) {
|
||||
if (closedList.some((node) => node.x === neighbor.x && node.y === neighbor.y)) {
|
||||
continue
|
||||
}
|
||||
this.removeNodeFromOpenList(openList, currentNode);
|
||||
closedSet.add(this.nodeToString(currentNode));
|
||||
|
||||
neighbor.g = currentNode.g + 1
|
||||
neighbor.h = Math.abs(neighbor.x - end.x) + Math.abs(neighbor.y - end.y)
|
||||
neighbor.f = neighbor.g + neighbor.h
|
||||
neighbor.parent = currentNode
|
||||
for (const neighbor of this.getValidNeighbors(currentNode, grid, end, allowDiagonal)) {
|
||||
if (closedSet.has(this.nodeToString(neighbor))) continue;
|
||||
|
||||
if (!openList.some((node) => node.x === neighbor.x && node.y === neighbor.y)) {
|
||||
openList.push(neighbor)
|
||||
const tentativeGScore = currentNode.g + this.getDistance(currentNode, neighbor);
|
||||
|
||||
if (!this.isInOpenList(openList, neighbor) || tentativeGScore < neighbor.g) {
|
||||
neighbor.parent = currentNode;
|
||||
neighbor.g = tentativeGScore;
|
||||
neighbor.h = this.heuristic(neighbor, end);
|
||||
neighbor.f = neighbor.g + neighbor.h;
|
||||
|
||||
if (!this.isInOpenList(openList, neighbor)) {
|
||||
openList.push(neighbor);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// No path found
|
||||
return []
|
||||
return []; // No path found
|
||||
}
|
||||
|
||||
private static getNeighbors(node: Node, grid: number[][]): Node[] {
|
||||
let neighbors: Node[] = []
|
||||
let directions = [
|
||||
{ x: 0, y: -1 },
|
||||
{ x: 0, y: 1 },
|
||||
{ x: -1, y: 0 },
|
||||
{ x: 1, y: 0 }
|
||||
]
|
||||
/**
|
||||
* Gets the node with the lowest F score from a list.
|
||||
*/
|
||||
private static getLowestFScoreNode(nodes: Node[]): Node {
|
||||
return nodes.reduce((min, node) => (node.f < min.f ? node : min));
|
||||
}
|
||||
|
||||
for (let direction of directions) {
|
||||
let x = node.x + direction.x
|
||||
let y = node.y + direction.y
|
||||
/**
|
||||
* Checks if the given node is the end node.
|
||||
*/
|
||||
private static isEndNode(node: Node, end: Position): boolean {
|
||||
return node.x === end.x && node.y === end.y;
|
||||
}
|
||||
|
||||
if (x >= 0 && y >= 0 && x < grid.length && y < grid[0].length && grid[x][y] === 0) {
|
||||
neighbors.push({ x, y, g: 0, h: 0, f: 0 })
|
||||
}
|
||||
/**
|
||||
* Removes a node from the open list.
|
||||
*/
|
||||
private static removeNodeFromOpenList(openList: Node[], node: Node): void {
|
||||
const index = openList.findIndex(n => n.x === node.x && n.y === node.y);
|
||||
if (index !== -1) openList.splice(index, 1);
|
||||
}
|
||||
|
||||
/**
|
||||
* Converts a node to a string representation.
|
||||
*/
|
||||
private static nodeToString(node: Position): string {
|
||||
return `${node.x},${node.y}`;
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets valid neighbors of the given node.
|
||||
*/
|
||||
private static getValidNeighbors(node: Node, grid: number[][], end: Position, allowDiagonal: boolean): Node[] {
|
||||
const directions = allowDiagonal
|
||||
? [...this.ORTHOGONAL_DIRECTIONS, ...this.DIAGONAL_DIRECTIONS]
|
||||
: this.ORTHOGONAL_DIRECTIONS;
|
||||
|
||||
return directions
|
||||
.map((dir) => ({
|
||||
x: node.x + dir.x,
|
||||
y: node.y + dir.y,
|
||||
g: 0,
|
||||
h: 0,
|
||||
f: 0
|
||||
}))
|
||||
.filter((pos) => this.isValidPosition(pos, grid, end)) as Node[];
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks if the given position is valid.
|
||||
*/
|
||||
private static isValidPosition(pos: Position, grid: number[][], end: Position): boolean {
|
||||
const { x, y } = pos;
|
||||
return (
|
||||
x >= 0 && y >= 0 &&
|
||||
x < grid.length && y < grid[0].length &&
|
||||
(grid[y][x] === 0 || (x === end.x && y === end.y))
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks if the given node is in the open list.
|
||||
*/
|
||||
private static isInOpenList(openList: Node[], node: Position): boolean {
|
||||
return openList.some((n) => n.x === node.x && n.y === node.y);
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the distance between two positions.
|
||||
*/
|
||||
private static getDistance(a: Position, b: Position): number {
|
||||
const dx = Math.abs(a.x - b.x);
|
||||
const dy = Math.abs(a.y - b.y);
|
||||
|
||||
if (a.x === b.x || a.y === b.y) {
|
||||
// Orthogonal movement (horizontal/vertical)
|
||||
return Math.sqrt(dx * dx + dy * dy);
|
||||
} else {
|
||||
// Diagonal movement with cost sqrt(2)
|
||||
return Math.sqrt(dx * dx + dy * dy);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Heuristic function estimating the distance from a node to the goal.
|
||||
*/
|
||||
private static heuristic(node: Position, goal: Position): number {
|
||||
return this.getDistance(node, goal);
|
||||
}
|
||||
|
||||
/**
|
||||
* Reconstructs the path from the end node.
|
||||
*/
|
||||
private static reconstructPath(endNode: Node): Node[] {
|
||||
const path: Node[] = [];
|
||||
let currentNode: Node | undefined = endNode;
|
||||
|
||||
while (currentNode) {
|
||||
path.unshift(currentNode);
|
||||
currentNode = currentNode.parent;
|
||||
}
|
||||
|
||||
return neighbors
|
||||
return path;
|
||||
}
|
||||
}
|
||||
|
||||
export default AStar
|
||||
export default AStar;
|
Reference in New Issue
Block a user