工具类
AStarUtil.java
import java.util.*;
import java.util.stream.Collectors; /**
* A星算法工具类
*/
public class AStarUtil { private int[][] NODES; public AStarUtil(int[][] NODES) {
this.NODES = NODES;
} public AStarUtil() {
} public static final int STEP = 10; private ArrayList<Node> openList = new ArrayList<Node>();
private ArrayList<Node> closeList = new ArrayList<Node>(); public Node findMinFNodeInOpneList() {
Node tempNode = openList.get(0);
for (Node node : openList) {
if (node.F < tempNode.F) {
tempNode = node;
}
}
return tempNode;
} public ArrayList<Node> findNeighborNodes(Node currentNode) {
ArrayList<Node> arrayList = new ArrayList<Node>();
// 只考虑上下左右，不考虑斜对角
int topX = currentNode.x;
int topY = currentNode.y - 1;
if (canReach(topX, topY) && !exists(closeList, topX, topY)) {
arrayList.add(new Node(topX, topY));
}
int bottomX = currentNode.x;
int bottomY = currentNode.y + 1;
if (canReach(bottomX, bottomY) && !exists(closeList, bottomX, bottomY)) {
arrayList.add(new Node(bottomX, bottomY));
}
int leftX = currentNode.x - 1;
int leftY = currentNode.y;
if (canReach(leftX, leftY) && !exists(closeList, leftX, leftY)) {
arrayList.add(new Node(leftX, leftY));
}
int rightX = currentNode.x + 1;
int rightY = currentNode.y;
if (canReach(rightX, rightY) && !exists(closeList, rightX, rightY)) {
arrayList.add(new Node(rightX, rightY));
}
return arrayList;
} /**
* 可以行走
*
* @param x
* @param y
* @return
*/
public boolean canReach(int x, int y) {
if (x >= 0 && x < NODES.length && y >= 0 && y < NODES[0].length) {
return NODES[x][y] == 0;
}
return false;
} /**
* 寻找路径
*
* @param startNode
* @param endNode
* @return
*/
public Node findPath(Node startNode, Node endNode) { // 把起点加入 open list
openList.add(startNode); while (openList.size() > 0) {
// 遍历 open list ，查找 F值最小的节点，把它作为当前要处理的节点
Node currentNode = findMinFNodeInOpneList();
// 从open list中移除
openList.remove(currentNode);
// 把这个节点移到 close list
closeList.add(currentNode); ArrayList<Node> neighborNodes = findNeighborNodes(currentNode);
for (Node node : neighborNodes) {
if (exists(openList, node)) {
foundPoint(currentNode, node);
} else {
notFoundPoint(currentNode, endNode, node);
}
}
if (find(openList, endNode) != null) {
return find(openList, endNode);
}
} return find(openList, endNode);
} private void foundPoint(Node tempStart, Node node) {
int G = calcG(tempStart, node);
if (G < node.G) {
node.parent = tempStart;
node.G = G;
node.calcF();
}
} private void notFoundPoint(Node tempStart, Node end, Node node) {
node.parent = tempStart;
node.G = calcG(tempStart, node);
node.H = calcH(end, node);
node.calcF();
openList.add(node);
} private int calcG(Node start, Node node) {
int G = STEP;
int parentG = node.parent != null ? node.parent.G : 0;
return G + parentG;
} private int calcH(Node end, Node node) {
int step = Math.abs(node.x - end.x) + Math.abs(node.y - end.y);
return step * STEP;
} public static Node find(List<Node> nodes, Node point) {
for (Node n : nodes) {
if ((n.x == point.x) && (n.y == point.y)) {
return n;
}
}
return null;
} public static boolean exists(List<Node> nodes, Node node) {
for (Node n : nodes) {
if ((n.x == node.x) && (n.y == node.y)) {
return true;
}
}
return false;
} public static boolean exists(List<Node> nodes, int x, int y) {
for (Node n : nodes) {
if ((n.x == x) && (n.y == y)) {
return true;
}
}
return false;
} public static class Node {
public Node(int x, int y) {
this.x = x;
this.y = y;
} public int x;
public int y; public int getX() {
return x;
} public void setX(int x) {
this.x = x;
} public int getY() {
return y;
} public void setY(int y) {
this.y = y;
} public int F;
public int G;
public int H; @Override
public String toString() {
return "Node{" +
"x=" + x +
", y=" + y +
'}';
} public void calcF() {
this.F = this.G + this.H;
} public Node parent;
} /**
* 根据坐标点获取二维数组
*
* @param allNodeList 所有的坐标点
* @param obstaclesNodeList 所有的障碍物的坐标点
* @return
*/
public static int[][] get2DArrays(List<Node> allNodeList, List<Node> obstaclesNodeList) {
int maxX = allNodeList.stream().sorted(Comparator.comparing(Node::getX).reversed()).collect(Collectors.toList()).get(0).x + 1;
int maxY = allNodeList.stream().sorted(Comparator.comparing(Node::getY).reversed()).collect(Collectors.toList()).get(0).y + 1; int[][] ints = new int[maxX][maxY];
for (int x = 0; x < maxX; x++) {
for (int y = 0; y < maxY; y++) {
for (Node o : obstaclesNodeList) {
if (o.x == x && o.y == y) {
ints[x][y] = 1;
break;
} else {
ints[x][y] = 0;
}
}
}
}
toStringInts(ints);
System.out.println();
return ints;
} /**
* 规划路径
*
* @param startNode 起始点坐标
* @param endNode 要到达的重点坐标
* @return 返回路径的坐标点 为空表示无法到达
*/
public List<Node> PlanRoute(Node startNode, Node endNode) {
AStarUtil.Node parent = new AStarUtil(NODES).findPath(startNode, endNode);
LinkedList<AStarUtil.Node> arrayList = new LinkedList<>(); while (parent != null) {
arrayList.addFirst(new AStarUtil.Node(parent.x, parent.y));
parent = parent.parent;
} /**
* 以下只是为了路线打印出来 无实际作用
*/
for (int i = 0; i < NODES.length; i++) {
for (int j = 0; j < NODES[0].length; j++) {
if (AStarUtil.exists(arrayList, i, j)) {
System.out.print("@, ");
} else {
System.out.print(NODES[i][j] + ", ");
} }
System.out.println();
}
return arrayList;
} /**
* 打印二维坐标数据
*
* @param ints
*/
public static void toStringInts(int[][] ints) {
for (int i = 0; i < ints.length; i++) {
for (int j = 0; j < ints[0].length; j++) {
System.out.print(ints[i][j] + ", "); }
System.out.println();
}
}
}
使用方法
import java.util.Arrays;
import java.util.LinkedList;
import java.util.List; /**
*
* 注：左上角为原点，水平方向是 y轴,垂直方向为 x轴
*
* {
* { 0, 0, 0, 0, 0, 0, 0, 0, 0 }, ——> y
* { 0, 0, 0, 0, 1, 0, 0, 0, 0 },
* { 0, 0, 0, 0, 0, 0, 0, 0, 0 },
* { 0, 0, 0, 1, 0, 0, 0, 0, 0 },
* { 0, 0, 0, 1, 0, 0, 0, 0, 0 },
* { 0, 0, 0, 1, 0, 0, 0, 0, 0 },
* { 0, 0, 0, 1, 0, 0, 0, 0, 0 },
* { 0, 0, 0, 0, 0, 0, 0, 0, 0 },
* { 0, 0, 0, 0, 0, 0, 0, 0, 0 },
*
* ∣
* ∨
* x
* };
*
*/
public class AstartMain { public static void main(String[] args) { /**
* 模拟所有坐标
*/
LinkedList<AStarUtil.Node> nodeLinkedList = new LinkedList<>();
for (int i = 0; i < 9; i++) {
for (int j = 0; j < 9; j++) {
AStarUtil.Node ori = new AStarUtil.Node(j, i);
nodeLinkedList.add(ori);
}
} /**
* 模拟障碍物坐标
*/
List<AStarUtil.Node> obstaclesNodeList =new LinkedList<>();
obstaclesNodeList.add(new AStarUtil.Node(3, 3));
obstaclesNodeList.add(new AStarUtil.Node(4, 3));
obstaclesNodeList.add(new AStarUtil.Node(5, 3));
obstaclesNodeList.add(new AStarUtil.Node(1, 4));
obstaclesNodeList.add(new AStarUtil.Node(1, 7)); int[][] arrays = AStarUtil.get2DArrays(nodeLinkedList, obstaclesNodeList); AStarUtil aStarUtil = new AStarUtil(arrays);
/**
* 起点坐标
*/
AStarUtil.Node startNode = new AStarUtil.Node(0, 0); /**
* 终点坐标
*/
AStarUtil.Node endNode = new AStarUtil.Node(7, 4); /**
* 路线的所有二维坐标
*/
List<AStarUtil.Node> route = aStarUtil.PlanRoute(startNode, endNode);
System.out.println(Arrays.toString(route.toArray()));
}
}