feat(pathfinding): 添加寻路系统模块 (#333)
* feat(pathfinding): 添加寻路系统模块 实现完整的寻路系统，支持 A* 算法、网格地图、导航网格和路径平滑: A* 寻路算法: - 高效的二叉堆优先队列 - 可配置的启发式权重 - 最大搜索节点限制 - 支持对角移动和穿角避免 网格地图 (GridMap): - 基于二维数组的网格地图 - 支持 4 方向和 8 方向移动 - 可变移动代价 - 从数组或字符串加载地图 导航网格 (NavMesh): - 凸多边形导航网格 - 自动检测相邻多边形 - 漏斗算法路径优化 - 适合复杂地形 路径平滑: - Bresenham 视线检测 - 射线投射视线检测 - 视线简化器 (移除不必要的拐点) - Catmull-Rom 曲线平滑 - 组合平滑器 启发式函数: - 曼哈顿距离 (4方向) - 欧几里得距离 (任意方向) - 切比雪夫距离 (8方向) - 八角距离 (8方向，对角线√2) 蓝图节点 (8个): - FindPath: 基础寻路 - FindPathSmooth: 平滑寻路 - IsWalkable: 检查可通行性 - GetPathLength: 获取路径点数 - GetPathDistance: 获取路径距离 - GetPathPoint: 获取路径点 - MoveAlongPath: 沿路径移动 - HasLineOfSight: 视线检测 * chore: update pnpm-lock.yaml for pathfinding package
This commit is contained in:
YHH
GitHub
235
packages/pathfinding/src/core/AStarPathfinder.ts
Normal file
235
packages/pathfinding/src/core/AStarPathfinder.ts
Normal file
@@ -0,0 +1,235 @@
|
||||
/**
|
||||
* @zh A* 寻路算法实现
|
||||
* @en A* Pathfinding Algorithm Implementation
|
||||
*/
|
||||
|
||||
import { BinaryHeap } from './BinaryHeap';
|
||||
import type {
|
||||
IPathfindingMap,
|
||||
IPathfinder,
|
||||
IPathResult,
|
||||
IPathfindingOptions,
|
||||
IPathNode,
|
||||
IPoint
|
||||
} from './IPathfinding';
|
||||
import { EMPTY_PATH_RESULT, DEFAULT_PATHFINDING_OPTIONS } from './IPathfinding';
|
||||
|
||||
// =============================================================================
|
||||
// 内部节点类型 | Internal Node Type
|
||||
// =============================================================================
|
||||
|
||||
interface AStarNode {
|
||||
node: IPathNode;
|
||||
g: number; // Cost from start
|
||||
h: number; // Heuristic to end
|
||||
f: number; // Total cost (g + h)
|
||||
parent: AStarNode | null;
|
||||
closed: boolean;
|
||||
opened: boolean;
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// A* 寻路器 | A* Pathfinder
|
||||
// =============================================================================
|
||||
|
||||
/**
|
||||
* @zh A* 寻路器
|
||||
* @en A* Pathfinder
|
||||
*
|
||||
* @zh 使用 A* 算法在地图上查找最短路径
|
||||
* @en Uses A* algorithm to find shortest path on a map
|
||||
*
|
||||
* @example
|
||||
* ```typescript
|
||||
* const map = new GridMap(width, height);
|
||||
* const pathfinder = new AStarPathfinder(map);
|
||||
* const result = pathfinder.findPath(0, 0, 10, 10);
|
||||
* if (result.found) {
|
||||
* console.log('Path:', result.path);
|
||||
* }
|
||||
* ```
|
||||
*/
|
||||
export class AStarPathfinder implements IPathfinder {
|
||||
private readonly map: IPathfindingMap;
|
||||
private nodeCache: Map<string | number, AStarNode> = new Map();
|
||||
private openList: BinaryHeap<AStarNode>;
|
||||
|
||||
constructor(map: IPathfindingMap) {
|
||||
this.map = map;
|
||||
this.openList = new BinaryHeap<AStarNode>((a, b) => a.f - b.f);
|
||||
}
|
||||
|
||||
/**
|
||||
* @zh 查找路径
|
||||
* @en Find path
|
||||
*/
|
||||
findPath(
|
||||
startX: number,
|
||||
startY: number,
|
||||
endX: number,
|
||||
endY: number,
|
||||
options?: IPathfindingOptions
|
||||
): IPathResult {
|
||||
const opts = { ...DEFAULT_PATHFINDING_OPTIONS, ...options };
|
||||
|
||||
// Clear previous state
|
||||
this.clear();
|
||||
|
||||
// Get start and end nodes
|
||||
const startNode = this.map.getNodeAt(startX, startY);
|
||||
const endNode = this.map.getNodeAt(endX, endY);
|
||||
|
||||
// Validate nodes
|
||||
if (!startNode || !endNode) {
|
||||
return EMPTY_PATH_RESULT;
|
||||
}
|
||||
|
||||
if (!startNode.walkable || !endNode.walkable) {
|
||||
return EMPTY_PATH_RESULT;
|
||||
}
|
||||
|
||||
// Same position
|
||||
if (startNode.id === endNode.id) {
|
||||
return {
|
||||
found: true,
|
||||
path: [startNode.position],
|
||||
cost: 0,
|
||||
nodesSearched: 1
|
||||
};
|
||||
}
|
||||
|
||||
// Initialize start node
|
||||
const start = this.getOrCreateAStarNode(startNode);
|
||||
start.g = 0;
|
||||
start.h = this.map.heuristic(startNode.position, endNode.position) * opts.heuristicWeight;
|
||||
start.f = start.h;
|
||||
start.opened = true;
|
||||
|
||||
this.openList.push(start);
|
||||
|
||||
let nodesSearched = 0;
|
||||
const endPosition = endNode.position;
|
||||
|
||||
// A* main loop
|
||||
while (!this.openList.isEmpty && nodesSearched < opts.maxNodes) {
|
||||
const current = this.openList.pop()!;
|
||||
current.closed = true;
|
||||
nodesSearched++;
|
||||
|
||||
// Found the goal
|
||||
if (current.node.id === endNode.id) {
|
||||
return this.buildPath(current, nodesSearched);
|
||||
}
|
||||
|
||||
// Explore neighbors
|
||||
const neighbors = this.map.getNeighbors(current.node);
|
||||
|
||||
for (const neighborNode of neighbors) {
|
||||
// Skip if not walkable or already closed
|
||||
if (!neighborNode.walkable) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const neighbor = this.getOrCreateAStarNode(neighborNode);
|
||||
|
||||
if (neighbor.closed) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Calculate tentative g score
|
||||
const movementCost = this.map.getMovementCost(current.node, neighborNode);
|
||||
const tentativeG = current.g + movementCost;
|
||||
|
||||
// Check if this path is better
|
||||
if (!neighbor.opened) {
|
||||
// First time visiting this node
|
||||
neighbor.g = tentativeG;
|
||||
neighbor.h = this.map.heuristic(neighborNode.position, endPosition) * opts.heuristicWeight;
|
||||
neighbor.f = neighbor.g + neighbor.h;
|
||||
neighbor.parent = current;
|
||||
neighbor.opened = true;
|
||||
this.openList.push(neighbor);
|
||||
} else if (tentativeG < neighbor.g) {
|
||||
// Found a better path
|
||||
neighbor.g = tentativeG;
|
||||
neighbor.f = neighbor.g + neighbor.h;
|
||||
neighbor.parent = current;
|
||||
this.openList.update(neighbor);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// No path found
|
||||
return {
|
||||
found: false,
|
||||
path: [],
|
||||
cost: 0,
|
||||
nodesSearched
|
||||
};
|
||||
}
|
||||
|
||||
/**
|
||||
* @zh 清理状态
|
||||
* @en Clear state
|
||||
*/
|
||||
clear(): void {
|
||||
this.nodeCache.clear();
|
||||
this.openList.clear();
|
||||
}
|
||||
|
||||
/**
|
||||
* @zh 获取或创建 A* 节点
|
||||
* @en Get or create A* node
|
||||
*/
|
||||
private getOrCreateAStarNode(node: IPathNode): AStarNode {
|
||||
let astarNode = this.nodeCache.get(node.id);
|
||||
|
||||
if (!astarNode) {
|
||||
astarNode = {
|
||||
node,
|
||||
g: Infinity,
|
||||
h: 0,
|
||||
f: Infinity,
|
||||
parent: null,
|
||||
closed: false,
|
||||
opened: false
|
||||
};
|
||||
this.nodeCache.set(node.id, astarNode);
|
||||
}
|
||||
|
||||
return astarNode;
|
||||
}
|
||||
|
||||
/**
|
||||
* @zh 构建路径结果
|
||||
* @en Build path result
|
||||
*/
|
||||
private buildPath(endNode: AStarNode, nodesSearched: number): IPathResult {
|
||||
const path: IPoint[] = [];
|
||||
let current: AStarNode | null = endNode;
|
||||
|
||||
while (current) {
|
||||
path.unshift(current.node.position);
|
||||
current = current.parent;
|
||||
}
|
||||
|
||||
return {
|
||||
found: true,
|
||||
path,
|
||||
cost: endNode.g,
|
||||
nodesSearched
|
||||
};
|
||||
}
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// 工厂函数 | Factory Function
|
||||
// =============================================================================
|
||||
|
||||
/**
|
||||
* @zh 创建 A* 寻路器
|
||||
* @en Create A* pathfinder
|
||||
*/
|
||||
export function createAStarPathfinder(map: IPathfindingMap): AStarPathfinder {
|
||||
return new AStarPathfinder(map);
|
||||
}
|
||||
Reference in New Issue
Block a user