java ARA*算法 请写注释

以下是 Java 实现的 ARA* 算法，含有注释：

import java.util.*;

public class ARAStar {
    private static final int INFINITY = Integer.MAX_VALUE; // 定义无穷大值为整数最大值
    private static final int OBSTACLE = -1; // 定义障碍物的值为 -1

    private int[][] grid; // 二维网格地图
    private int rows, cols; // 地图的行数和列数
    private int startX, startY; // 起点的坐标
    private int goalX, goalY; // 终点的坐标
    private double epsilon; // 权重因子
    private int[][] gValues; // g 值矩阵
    private int[][] rhsValues; // rhs 值矩阵
    private PriorityQueue<Node> openSet; // 优先队列存储待扩展的节点
    private Map<Node, Integer> gValuesMap; // g 值的哈希表，用于快速查找节点的 g 值
    private Map<Node, Integer> rhsValuesMap; // rhs 值的哈希表，用于快速查找节点的 rhs 值

    // 构造函数
    public ARAStar(int[][] grid, int startX, int startY, int goalX, int goalY, double epsilon) {
        this.grid = grid;
        this.rows = grid.length;
        this.cols = grid[0].length;
        this.startX = startX;
        this.startY = startY;
        this.goalX = goalX;
        this.goalY = goalY;
        this.epsilon = epsilon;
        this.gValues = new int[rows][cols];
        this.rhsValues = new int[rows][cols];
        this.openSet = new PriorityQueue<>();
        this.gValuesMap = new HashMap<>();
        this.rhsValuesMap = new HashMap<>();
        initialize();
    }

    // 初始化各个数据结构
    private void initialize() {
        // 将所有格子的 g 值和 rhs 值初始化为无穷大
        for (int i = 0; i < rows; i++) {
            for (int j = 0; j < cols; j++) {
                gValues[i][j] = INFINITY;
                rhsValues[i][j] = INFINITY;
            }
        }
        // 将终点的 rhs 值设为 0
        rhsValues[goalX][goalY] = 0;
        // 将终点加入到优先队列中
        openSet.offer(new Node(goalX, goalY, calculateKey(goalX, goalY)));
        // 将终点的 rhs 值和优先队列中的节点的 rhs 值添加到哈希表中
        rhsValuesMap.put(new Node(goalX, goalY), 0);
        gValuesMap.put(new Node(goalX, goalY), INFINITY);
    }

    // 计算指定节点的启发式函数值
    private double heuristic(int x, int y) {
        return Math.sqrt(Math.pow(x - goalX, 2) + Math.pow(y - goalY, 2));
    }

    // 计算指定节点的 key 值
    private Key calculateKey(int x, int y) {
        int gValue = gValues[x][y];
        int rhsValue = rhsValues[x][y];
        return new Key(Math.min(gValue, rhsValue) + epsilon * heuristic(x, y), Math.min(gValue, rhsValue));
    }

    // 更新指定节点的 g 值
    private void updateGValue(Node node) {
        int x = node.x;
        int y = node.y;
        gValues[x][y] = INFINITY;
        for (Node neighbor : getNeighbors(node)) {
            int cost = getCost(node, neighbor);
            if (rhsValues[neighbor.x][neighbor.y] + cost < gValues[x][y]) {
                gValues[x][y] = rhsValues[neighbor.x][neighbor.y] + cost;
            }
        }
        gValuesMap.put(node, gValues[x][y]);
    }

    // 更新指定节点的 rhs 值
    private void updateRHSValue(Node node) {
        if (!node.equals(new Node(goalX, goalY)) && !isObstacle(node.x, node.y)) {
            rhsValues[node.x][node.y] = INFINITY;
            for (Node neighbor : getNeighbors(node)) {
                int cost = getCost(node, neighbor);
                if (gValues[neighbor.x][neighbor.y] + cost < rhsValues[node.x][node.y]) {
                    rhsValues[node.x][node.y] = gValues[neighbor.x][neighbor.y] + cost;
                }
            }
        }
        rhsValuesMap.put(node, rhsValues[node.x][node.y]);
    }

    // 判断指定节点是否为障碍物
    private boolean isObstacle(int x, int y) {
        return grid[x][y] == OBSTACLE;
    }

    // 获取指定节点的邻居节点
    private List<Node> getNeighbors(Node node) {
        int x = node.x;
        int y = node.y;
        List<Node> neighbors = new ArrayList<>();
        if (x > 0) neighbors.add(new Node(x - 1, y));
        if (y > 0) neighbors.add(new Node(x, y - 1));
        if (x < rows - 1) neighbors.add(new Node(x + 1, y));
        if (y < cols - 1) neighbors.add(new Node(x, y + 1));
        return neighbors;
    }

    // 获取指定节点到邻居节点的代价
    private int getCost(Node node, Node neighbor) {
        if (isObstacle(neighbor.x, neighbor.y)) {
            return INFINITY;
        }
        return 1;
    }

    // 执行 ARA* 算法
    public List<Node> run() {
        while (true) {
            if (openSet.isEmpty()) {
                return null;
            }
            Node node = openSet.peek();
            if (calculateKey(node.x, node.y).compareTo(node.key) < 0) {
                openSet.poll();
                node.key = calculateKey(node.x, node.y);
                openSet.offer(node);
            } else if (gValues[node.x][node.y] > rhsValues[node.x][node.y]) {
                updateGValue(node);
                for (Node neighbor : getNeighbors(node)) {
                    if (!neighbor.equals(new Node(startX, startY))) {
                        updateRHSValue(neighbor);
                    }
                    if (gValues[neighbor.x][neighbor.y] > rhsValues[neighbor.x][neighbor.y]) {
                        if (openSet.contains(neighbor)) {
                            openSet.remove(neighbor);
                        }
                        neighbor.key = calculateKey(neighbor.x, neighbor.y);
                        openSet.offer(neighbor);
                    } else if (gValues[neighbor.x][neighbor.y] < rhsValues[neighbor.x][neighbor.y] && openSet.contains(node)) {
                        if (openSet.contains(node)) {
                            openSet.remove(node);
                        }
                        node.key = calculateKey(node.x, node.y);
                        openSet.offer(node);
                    }
                }
            } else {
                int pathLength = gValues[startX][startY];
                List<Node> path = new ArrayList<>();
                path.add(new Node(startX, startY));
                while (!path.get(path.size() - 1).equals(new Node(goalX, goalY))) {
                    Node current = path.get(path.size() - 1);
                    List<Node> neighbors = getNeighbors(current);
                    Node next = null;
                    int minCost = INFINITY;
                    for (Node neighbor : neighbors) {
                        int cost = getCost(current, neighbor);
                        if (gValues[neighbor.x][neighbor.y] + cost < minCost) {
                            next = neighbor;
                            minCost = gValues[neighbor.x][neighbor.y] + cost;
                        }
                    }
                    path.add(next);
                }
                return path;
            }
        }
    }

    // 表示地图中的一个节点
    private static class Node {
        int x;
        int y;
        Key key;

        public Node(int x, int y) {
            this.x = x;
            this.y = y;
        }

        public Node(int x, int y, Key key) {
            this.x = x;
            this.y = y;
            this.key = key;
        }

        @Override
        public boolean equals(Object o) {
            if (this == o) return true;
            if (o == null || getClass() != o.getClass()) return false;
            Node node = (Node) o;
            return x == node.x &&
                    y == node.y;
        }

        @Override
        public int hashCode() {
            return Objects.hash(x, y);
        }
    }

    // 表示节点的 key 值，用于在优先队列中排序
    private static class Key implements Comparable<Key> {
        double first;
        int second;

        public Key(double first, int second) {
            this.first = first;
            this.second = second;
        }

        @Override
        public int compareTo(Key other) {
            if (first < other.first) {
                return -1;
            } else if (first > other.first) {
                return 1;
            } else if (second < other.second) {
                return -1;
            } else if (second > other.second) {
                return 1;
            } else {
                return 0;
            }
        }
    }
}

主要思路如下：

1. 初始化 ARA* 算法的各个数据结构。
2. 将终点加入到优先队列中，并将其 rhs 值设为 0。
3. 不断从优先队列中取出 key 值最小的节点进行扩展，直到找到起点或优先队列为空为止。
4. 如果当前节点的 key 值小于其存储的 key 值，则更新其 key 值并重新加入到优先队列中。
5. 否则，如果当前节点的 g 值大于其 rhs 值，则更新其 g 值，并对其所有邻居节点进行更新操作。
6. 如果某个邻居节点的 g 值小于其 rhs 值，则将其添加到优先队列中。
7. 如果某个邻居节点的 g 值大于其 rhs 值，则将当前节点添加到优先队列中。
8. 如果当前节点的 g 值等于其 rhs 值，并且其为起点，则找到了一条最短路径，返回该路径。
9. 否则，继续执行步骤 3。

注释中对各个函数的作用进行了说明，可以结合代码一起理解。