ARAstar java实现

ARA*算法是一种启发式搜索算法，用于在图形搜索中寻找最短路径。其算法流程如下：

1.初始化：将起点加入OPEN列表中，将终点加入CLOSED列表中。

2.重复以下步骤，直到找到一条路径或者确定没有路径：

a.从OPEN列表中选取f值最低的节点n。

b.将n从OPEN列表中删除，并将其加入CLOSED列表中。

c.如果n为终点，则返回路径。

d.否则，对n的所有邻居进行以下操作：

i.如果邻居在CLOSED列表中，则忽略它。

ii.如果邻居不在OPEN列表中，则将其加入OPEN列表，并计算其f值、g值、h值。

iii.如果邻居已经在OPEN列表中，计算其新的g值。如果新的g值更优，则更新其父节点为n，并重新计算该节点的f值、g值、h值。

3.如果OPEN列表为空，则不存在路径。

下面是一个Java实现的示例代码：

import java.util.*;

public class ARAstar {
    private final double heuristicWeight;
    private final int maxIterations;
    private final double epsilon;
    
    public ARAstar(double heuristicWeight, int maxIterations, double epsilon) {
        this.heuristicWeight = heuristicWeight;
        this.maxIterations = maxIterations;
        this.epsilon = epsilon;
    }
    
    public List<Node> search(Node start, Node goal, Map<Node, List<Edge>> graph) {
        Map<Node, Double> gValues = new HashMap<>();
        Map<Node, Double> fValues = new HashMap<>();
        Map<Node, Node> parents = new HashMap<>();
        PriorityQueue<Node> openList = new PriorityQueue<>(Comparator.comparingDouble(fValues::get));
        Set<Node> closedList = new HashSet<>();
        
        gValues.put(start, 0.0);
        fValues.put(start, heuristicWeight * start.heuristic(goal));
        openList.offer(start);
        
        for (int i = 0; i < maxIterations; i++) {
            if (openList.isEmpty()) {
                break;
            }
            Node current = openList.poll();
            if (current.equals(goal)) {
                return reconstructPath(current, parents);
            }
            closedList.add(current);
            for (Edge edge : graph.getOrDefault(current, Collections.emptyList())) {
                Node neighbor = edge.dest;
                if (closedList.contains(neighbor)) {
                    continue;
                }
                double tentativeG = gValues.get(current) + edge.cost;
                double tentativeF = tentativeG + epsilon * neighbor.heuristic(goal);
                if (!openList.contains(neighbor)) {
                    openList.offer(neighbor);
                } else if (tentativeF >= fValues.get(neighbor)) {
                    continue;
                }
                parents.put(neighbor, current);
                gValues.put(neighbor, tentativeG);
                fValues.put(neighbor, tentativeF);
                openList.remove(neighbor);
                openList.offer(neighbor);
            }
        }
        return Collections.emptyList();
    }
    
    private List<Node> reconstructPath(Node current, Map<Node, Node> parents) {
        List<Node> path = new ArrayList<>();
        while (current != null) {
            path.add(current);
            current = parents.get(current);
        }
        Collections.reverse(path);
        return path;
    }
}

class Node {
    public final int x;
    public final int y;
    
    public Node(int x, int y) {
        this.x = x;
        this.y = y;
    }
    
    public double heuristic(Node other) {
        return Math.sqrt((x - other.x) * (x - other.x) + (y - other.y) * (y - other.y));
    }
    
    @Override
    public boolean equals(Object obj) {
        if (obj == this) {
            return true;
        }
        if (!(obj instanceof Node)) {
            return false;
        }
        Node other = (Node) obj;
        return x == other.x && y == other.y;
    }
    
    @Override
    public int hashCode() {
        return Objects.hash(x, y);
    }
}

class Edge {
    public final Node dest;
    public final double cost;
    
    public Edge(Node dest, double cost) {
        this.dest = dest;
        this.cost = cost;
    }
}

//Example usage
public class Main {
    public static void main(String[] args) {
        Node start = new Node(0, 0);
        Node goal = new Node(10, 10);
        Map<Node, List<Edge>> graph = new HashMap<>();
        graph.put(new Node(0, 0), Arrays.asList(new Edge(new Node(0, 1), 1), new Edge(new Node(1, 0), 1)));
        graph.put(new Node(0, 1), Arrays.asList(new Edge(new Node(0, 0), 1), new Edge(new Node(0, 2), 1)));
        graph.put(new Node(0, 2), Arrays.asList(new Edge(new Node(0, 1), 1), new Edge(new Node(1, 2), 1)));
        graph.put(new Node(1, 0), Arrays.asList(new Edge(new Node(0, 0), 1), new Edge(new Node(2, 0), 1)));
        graph.put(new Node(1, 2), Arrays.asList(new Edge(new Node(0, 2), 1), new Edge(new Node(2, 2), 1)));
        graph.put(new Node(2, 0), Arrays.asList(new Edge(new Node(1, 0), 1), new Edge(new Node(2, 1), 1)));
        graph.put(new Node(2, 1), Arrays.asList(new Edge(new Node(2, 0), 1), new Edge(new Node(2, 2), 1), new Edge(new Node(3, 1), 1)));
        graph.put(new Node(2, 2), Arrays.asList(new Edge(new Node(1, 2), 1), new Edge(new Node(2, 1), 1), new Edge(new Node(3, 2), 1)));
        graph.put(new Node(3, 1), Arrays.asList(new Edge(new Node(2, 1), 1), new Edge(new Node(3, 2), 1)));
        graph.put(new Node(3, 2), Arrays.asList(new Edge(new Node(2, 2), 1), new Edge(new Node(3, 1), 1), new Edge(new Node(4, 2), 1)));
        graph.put(new Node(4, 2), Arrays.asList(new Edge(new Node(3, 2), 1), new Edge(new Node(4, 1), 1), new Edge(new Node(5, 2), 1)));
        graph.put(new Node(4, 1), Arrays.asList(new Edge(new Node(4, 2), 1), new Edge(new Node(4, 0), 1)));
        graph.put(new Node(4, 0), Arrays.asList(new Edge(new Node(4, 1), 1), new Edge(new Node(5, 0), 1)));
        graph.put(new Node(5, 0), Arrays.asList(new Edge(new Node(4, 0), 1), new Edge(new Node(6, 0), 1)));
        graph.put(new Node(6, 0), Arrays.asList(new Edge(new Node(5, 0), 1), new Edge(new Node(6, 1), 1)));
        graph.put(new Node(6, 1), Arrays.asList(new Edge(new Node(6, 0), 1), new Edge(new Node(7, 1), 1)));
        graph.put(new Node(7, 1), Arrays.asList(new Edge(new Node(6, 1), 1), new Edge(new Node(8, 1), 1)));
        graph.put(new Node(8, 1), Arrays.asList(new Edge(new Node(7, 1), 1), new Edge(new Node(9, 1), 1)));
        graph.put(new Node(9, 1), Arrays.asList(new Edge(new Node(8, 1), 1), new Edge(new Node(10, 1), 1)));
        graph.put(new Node(10, 1), Arrays.asList(new Edge(new Node(9, 1), 1), new Edge(new Node(10, 2), 1), new Edge(new Node(10, 0), 1)));
        graph.put(new Node(10, 2), Arrays.asList(new Edge(new Node(10, 1), 1), new Edge(new Node(10, 3), 1)));
        graph.put(new Node(10, 3), Arrays.asList(new Edge(new Node(10, 2), 1), new Edge(new Node(10, 4), 1)));
        graph.put(new Node(10, 4), Arrays.asList(new Edge(new Node(10, 3), 1), new Edge(new Node(10, 5), 1)));
        graph.put(new Node(10, 5), Arrays.asList(new Edge(new Node(10, 4), 1), new Edge(new Node(10, 6), 1)));
        graph.put(new Node(10, 6), Arrays.asList(new Edge(new Node(10, 5), 1), new Edge(new Node(10, 7), 1)));
        graph.put(new Node(10, 7), Arrays.asList(new Edge(new Node(10, 6), 1), new Edge(new Node(10, 8), 1)));
        graph.put(new Node(10, 8), Arrays.asList(new Edge(new Node(10, 7), 1), new Edge(new Node(10, 9), 1)));
        graph.put(new Node(10, 9), Arrays.asList(new Edge(new Node(10, 8), 1), new Edge(new Node(10, 10), 1)));
        graph.put(new Node(10, 10), Arrays.asList(new Edge(new Node(10, 9), 1)));
        
        ARAstar araStar = new ARAstar(1.0, 1000, 1.0);
        List<Node> path = araStar.search(start, goal, graph);
        for (Node node : path) {
            System.out.print("(" + node.x + ", " + node.y + ") -> ");
        }
        System.out.println("done");
    }
}