Improved pathfinding

This commit is contained in:
2024-07-28 02:47:46 +02:00
parent f2fcc67cb6
commit 6c30e8d277
3 changed files with 304 additions and 191 deletions

View File

@ -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;