Method findPolygonPath

packages/pathfinding/src/navmesh/NavMesh.ts:395–478

* @zh 在多边形图上寻路 * @en Find path on polygon graph

(
        start: INavPolygon,
        end: INavPolygon,
        opts: Required<IPathfindingOptions>
    )

     * @en Find path on polygon graph
     */
    private findPolygonPath(
        start: INavPolygon,
        end: INavPolygon,
        opts: Required<IPathfindingOptions>
    ): { found: boolean; polygons: INavPolygon[]; nodesSearched: number } {
        interface AStarState {
            polygon: INavPolygon;
            g: number;
            f: number;
            parent: AStarState | null;
        }

        const openList = new BinaryHeap<AStarState>((a, b) => a.f - b.f);
        const closed = new Set<number>();
        const states = new Map<number, AStarState>();

        const startState: AStarState = {
            polygon: start,
            g: 0,
            f: euclideanDistance(start.center, end.center) * opts.heuristicWeight,
            parent: null
        };

        states.set(start.id, startState);
        openList.push(startState);

        let nodesSearched = 0;

        while (!openList.isEmpty && nodesSearched < opts.maxNodes) {
            const current = openList.pop()!;
            nodesSearched++;

            if (current.polygon.id === end.id) {
                // Reconstruct path
                const path: INavPolygon[] = [];
                let state: AStarState | null = current;

                while (state) {
                    path.unshift(state.polygon);
                    state = state.parent;
                }

                return { found: true, polygons: path, nodesSearched };
            }

            closed.add(current.polygon.id);

            for (const neighborId of current.polygon.neighbors) {
                if (closed.has(neighborId)) {
                    continue;
                }

                const neighborPolygon = this.polygons.get(neighborId);
                if (!neighborPolygon) {
                    continue;
                }

                const g = current.g + euclideanDistance(