用java写一个权值递减的重用搜索结果的ARAstar算法

ARA* 是一种变体的 A* 算法，可以在保证找到最优解的同时，尽可能地减少搜索的时间和空间。其核心思想是通过不断调整启发式函数的权值来平衡搜索速度和最优性。

下面是用 Java 实现基于权值递减的重用搜索结果的 ARA* 算法的代码：

import java.util.*;

public class ARAstar {
    private double w; // 启发式函数权值
    private Map<Node, Double> g; // 起点到各点的实际代价
    private Map<Node, Double> rhs; // 起点到各点的次优代价
    private Map<Node, Double> heuristic; // 各点的启发式代价
    private PriorityQueue<Node> open; // open 列表
    private Set<Node> closed; // closed 列表

    public ARAstar(double w) {
        this.w = w;
        g = new HashMap<>();
        rhs = new HashMap<>();
        heuristic = new HashMap<>();
        open = new PriorityQueue<>();
        closed = new HashSet<>();
    }

    public List<Node> search(Node start, Node goal, Map<Node, List<Node>> neighbors) {
        // 初始化
        g.put(start, 0.0);
        rhs.put(start, 0.0);
        heuristic.put(start, w * start.heuristicCost(goal));
        open.add(start);

        while (!open.isEmpty()) {
            Node current = open.peek();
            double fmin = heuristic.get(current);
            double gmin = g.get(current);

            if (gmin > fmin) { // 当前最优解不是最优的，重新计算启发式函数权值
                heuristic.replaceAll((node, v) -> w * node.heuristicCost(goal) + (g.containsKey(node) ? g.get(node) : Double.MAX_VALUE));
                fmin = open.stream().min(Comparator.comparingDouble(heuristic::get)).orElse(current).heuristicCost(goal);
            }

            if (current.equals(goal)) { // 找到最优解
                return reconstructPath(start, goal, neighbors);
            }

            open.remove(current);
            closed.add(current);

            for (Node neighbor : getNeighbors(current, neighbors)) {
                if (closed.contains(neighbor)) {
                    continue;
                }
                double tentativeG = g.get(current) + current.distanceTo(neighbor);
                if (!g.containsKey(neighbor) || tentativeG < g.get(neighbor)) {
                    g.put(neighbor, tentativeG);
                    rhs.put(neighbor, tentativeG + neighbor.heuristicCost(goal));
                    if (!open.contains(neighbor)) {
                        open.add(neighbor);
                    }
                }
            }
        }

        return new ArrayList<>(); // 没有找到可行解
    }

    private List<Node> getNeighbors(Node node, Map<Node, List<Node>> neighbors) {
        return neighbors.getOrDefault(node, Collections.emptyList());
    }

    private List<Node> reconstructPath(Node start, Node goal, Map<Node, List<Node>> neighbors) {
        List<Node> path = new ArrayList<>();
        Node current = goal;
        path.add(current);

        while (!current.equals(start)) {
            Node next = getNeighbors(current, neighbors).stream()
                    .filter(neighbor -> g.containsKey(neighbor) && g.get(neighbor) + neighbor.distanceTo(current) == g.get(current))
                    .findFirst().orElse(null);
            if (next == null) {
                return new ArrayList<>(); // 无法重用搜索结果
            }
            path.add(next);
            current = next;
        }

        Collections.reverse(path);
        return path;
    }
}

其中，`Node` 表示搜索图中的节点，包含了每个节点到目标节点的启发式代价、节点之间的实际代价、节点之间的距离等信息。

`search()` 方法接受起点、终点和邻居节点的映射表作为输入参数，返回一条从起点到终点的最优路径。当启发式函数权值需要重新计算时，我们对所有节点的启发式代价进行更新，然后重新计算当前最小启发式代价和最小实际代价。

在 `reconstructPath()` 方法中，我们利用了 ARA* 算法中的重用搜索结果的思想，即在找到最优解之后，从终点开始遍历路径，每次查找的下一个节点是当前节点的次优节点。如果无法找到次优节点，则说明无法重用搜索结果，需要重新搜索整个图。