解决Java网格路径查找算法中的无限循环问题

private Queue<Point> findPath(Rectangle[][] matrix, Point destPoint) {
    Point move = null;
    var dir = new ArrayList<Point>();
    dir.add(new Point(1, 0)); // right
    dir.add(new Point(0, 1)); // down
    dir.add(new Point(-1, 0)); // left
    dir.add(new Point(0, -1)); // up

    Point start = new Point(0, 0);
    var tmpPath = new ArrayDeque<Point>(); // 临时路径
    var path = new ArrayDeque<Point>();    // 最终路径（或当前路径）
    tmpPath.add(new Point(0, 0));

    while (!tmpPath.isEmpty()) {
        for (int dc = 0; dc < dir.size(); dc++) {
            move = new Point(start.x() + dir.get(dc).x(), start.y() + dir.get(dc).y());
            if (!move.isValid(matrix[0].length, matrix.length)) {
                continue; // 越界
            }
            if (matrix[move.y()][move.x()].getFill() != Color.MAGENTA) { // 非墙体
                start = move; // 更新当前点
                tmpPath.add(move);
                path.add(tmpPath.poll()); // 从tmpPath取出并加入path
                System.out.println(path.peek());
                if (path.getLast().equals(destPoint)) {
                    path.poll(); // 这一行可能导致问题，通常不应移除第一个元素
                    return path;
                }
                break; // 找到一个有效移动后立即跳出内层循环
            }
        }
    }
    return null; // 未找到路径
}

import java.awt.Color; // 假设Color类存在
import java.util.ArrayDeque;
import java.util.ArrayList;
import java.util.Deque; // 使用Deque作为栈或队列

// 假设Point类定义如下，包含isValid方法
// public record Point(int x, int y) {
//     public boolean isValid(int gridWidth, int gridHeight) {
//         return x >= 0 && x < gridWidth && y >= 0 && y < gridHeight;
//     }
// }

// 假设Rectangle类定义如下，包含getFill方法
// public class Rectangle {
//     private Color fill;
//     public Color getFill() { return fill; }
//     public void setFill(Color fill) { this.fill = fill; }
// }

public class PathFinder {

    // 假设Point和Rectangle类已在其他地方定义
    // 为了示例完整性，这里提供一个简化的Point类
    static class Point {
        int x, y;
        public Point(int x, int y) { this.x = x; this.y = y; }
        public int x() { return x; }
        public int y() { return y; }
        public boolean isValid(int gridWidth, int gridHeight) {
            return x >= 0 && x < gridWidth && y >= 0 && y < gridHeight;
        }
        @Override
        public boolean equals(Object o) {
            if (this == o) return true;
            if (o == null || getClass() != o.getClass()) return false;
            Point point = (Point) o;
            return x == point.x && y == point.y;
        }
        @Override
        public int hashCode() {
            return 31 * x + y;
        }
        @Override
        public String toString() {
            return "(" + x + ", " + y + ")";
        }
    }

    // 假设Rectangle类存在且有getFill()方法
    static class Rectangle {
        private Color fill;
        public Rectangle(Color fill) { this.fill = fill; }
        public Color getFill() { return fill; }
    }


    public Deque<Point> findPath(Rectangle[][] matrix, Point destPoint) {
        Point start = new Point(0, 0); // 起始点

        // 存储所有待探索的路径。每条路径本身是一个Point的Deque。
        // 使用Deque作为栈，实现深度优先搜索 (DFS)。
        Deque<Deque<Point>> availablePaths = new ArrayDeque<>();

        // 初始化，将起始点作为第一条路径加入待探索列表
        Deque<Point> initialPath = new ArrayDeque<>();
        initialPath.add(start);
        availablePaths.add(initialPath);

        // 定义移动方向：右、下、左、上
        var dir = new ArrayList<Point>();
        dir.add(new Point(1, 0));  // 右
        dir.add(new Point(0, 1));  // 下
        dir.add(new Point(-1, 0)); // 左
        dir.add(new Point(0, -1)); // 上

        while (!availablePaths.isEmpty()) {
            // 取出最近添加的路径进行探索 (LIFO, 深度优先)
            Deque<Point> currentPath = availablePaths.removeLast();
            Point lastPointInPath = currentPath.getLast(); // 当前路径的最后一个点

            // 检查是否到达目的地
            if (lastPointInPath.equals(destPoint)) {
                return currentPath; // 找到路径，直接返回
            }

            // 探索所有可能的移动方向
            for (Point d : dir) {
                Point nextMove = new Point(lastPointInPath.x() + d.x(), lastPointInPath.y() + d.y());

                // 1. 检查新点是否越界
                if (!nextMove.isValid(matrix[0].length, matrix.length)) {
                    continue;
                }

                // 2. 检查新点是否已在当前路径中（防止自相交导致无限循环）
                if (currentPath.contains(nextMove)) {
                    continue;
                }

                // 3. 检查新点是否是墙体 (Color.MAGENTA)
                if (matrix[nextMove.y()][nextMove.x()].getFill() != Color.MAGENTA) {
                    // 创建一条新路径，它是当前路径的副本加上新的移动点
                    Deque<Point> newPath = new ArrayDeque<>(currentPath);
                    newPath.add(nextMove);
                    availablePaths.add(newPath); // 将新路径加入待探索列表
                }
            }
        }
        return null; // 所有路径探索完毕，未找到目标
    }
}