java ARA*算法 请写注释
时间: 2023-12-23 18:02:11 浏览: 45
以下是基于网格地图的 Java 实现 ARA* 算法的代码,其中包含注释:
```java
import java.util.ArrayList;
import java.util.Comparator;
import java.util.HashMap;
import java.util.HashSet;
import java.util.List;
import java.util.Map;
import java.util.PriorityQueue;
import java.util.Set;
public class ARAStar {
// 内部类,表示网格地图上的一个节点
private class Node {
int x; // 节点的 x 坐标
int y; // 节点的 y 坐标
boolean blocked; // 节点是否被阻塞
double g; // 从起点到该节点的实际代价
double rhs; // 从起点到该节点的最小代价
double h; // 该节点到终点的估计代价
Node parent; // 该节点的父节点
Node(int x, int y, boolean blocked) {
this.x = x;
this.y = y;
this.blocked = blocked;
this.g = Double.POSITIVE_INFINITY;
this.rhs = Double.POSITIVE_INFINITY;
this.h = 0;
this.parent = null;
}
// 计算该节点的 f 值
double f() {
return Math.max(g, rhs) + h;
}
// 判断该节点是否是终点
boolean isGoal(Node goal) {
return x == goal.x && y == goal.y;
}
// 判断该节点是否相邻于指定节点
boolean isNeighbor(Node node) {
return Math.abs(x - node.x) + Math.abs(y - node.y) == 1;
}
// 获取该节点相邻的节点列表
List<Node> getNeighbors() {
List<Node> neighbors = new ArrayList<>();
if (x > 0 && !grid[x - 1][y].blocked) neighbors.add(grid[x - 1][y]);
if (x < width - 1 && !grid[x + 1][y].blocked) neighbors.add(grid[x + 1][y]);
if (y > 0 && !grid[x][y - 1].blocked) neighbors.add(grid[x][y - 1]);
if (y < height - 1 && !grid[x][y + 1].blocked) neighbors.add(grid[x][y + 1]);
return neighbors;
}
}
// 网格地图
private Node[][] grid;
private int width;
private int height;
// 起点和终点
private Node start;
private Node goal;
// 路径
private List<Node> path;
// 启发式函数,估计从节点 n 到终点的代价
private double heuristic(Node n) {
return Math.abs(n.x - goal.x) + Math.abs(n.y - goal.y);
}
// ARA* 算法的主体
public void search(Node[][] grid, int startX, int startY, int goalX, int goalY, double epsilon) {
// 初始化
this.grid = grid;
width = grid.length;
height = grid[0].length;
start = grid[startX][startY];
goal = grid[goalX][goalY];
path = new ArrayList<>();
// 优先队列,用于存储待扩展的节点
PriorityQueue<Node> open = new PriorityQueue<>(Comparator.comparingDouble((Node n) -> n.f()));
// 已扩展的节点集合
Set<Node> closed = new HashSet<>();
// 存储每个节点的 rhs 值
Map<Node, Double> rhsValues = new HashMap<>();
// 存储每个节点的 g 值
Map<Node, Double> gValues = new HashMap<>();
// 初始化 rhs 和 g 值
for (int x = 0; x < width; x++) {
for (int y = 0; y < height; y++) {
Node node = grid[x][y];
rhsValues.put(node, Double.POSITIVE_INFINITY);
gValues.put(node, Double.POSITIVE_INFINITY);
}
}
// 起点的 rhs 值为 0
rhsValues.put(start, 0.0);
// 起点的 g 值为 0
gValues.put(start, 0.0);
// 将起点加入 open 队列
start.h = epsilon * heuristic(start);
open.add(start);
// ARA* 算法的主循环
while (!open.isEmpty() && (open.peek().f() < goal.h || goal.rhs > goal.g)) {
Node current = open.poll();
// 如果当前节点已经扩展过了,则跳过
if (closed.contains(current)) continue;
// 更新 g 值
if (current.g > current.rhs) {
current.g = current.rhs;
} else {
current.g = Double.POSITIVE_INFINITY;
updateVertex(current);
}
// 如果当前节点是终点,则更新路径
if (current.isGoal(goal)) {
path.clear();
Node node = goal;
while (node != null) {
path.add(0, node);
node = node.parent;
}
return;
}
closed.add(current);
// 扩展相邻的节点
for (Node neighbor : current.getNeighbors()) {
if (closed.contains(neighbor)) continue;
double tentativeG = gValues.get(current) + 1;
if (tentativeG < gValues.get(neighbor)) {
neighbor.parent = current;
gValues.put(neighbor, tentativeG);
neighbor.rhs = tentativeG + epsilon * heuristic(neighbor);
if (!open.contains(neighbor)) {
neighbor.h = epsilon * heuristic(neighbor);
open.add(neighbor);
} else {
open.remove(neighbor);
neighbor.h = epsilon * heuristic(neighbor);
open.add(neighbor);
}
}
}
}
}
// 更新指定节点的 rhs 值
private void updateVertex(Node node) {
node.rhs = Double.POSITIVE_INFINITY;
for (Node neighbor : node.getNeighbors()) {
if (!neighbor.blocked) {
double rhs = gValues.get(neighbor) + 1;
if (rhs < node.rhs) {
node.rhs = rhs;
node.parent = neighbor;
}
}
}
}
// 获取计算出的路径
public List<Node> getPath() {
return path;
}
}
```
相关推荐
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![](https://csdnimg.cn/download_wenku/file_type_ask_c1.png)
![zip](https://img-home.csdnimg.cn/images/20210720083736.png)