A*寻路算法的主要思路
该算法主要是给了寻路时提供了一个方向（永远去看代价F最小的点）
伪代码
- 不断从可行走队列内取出预估代价F最小的点，判断是否结束点
- 计算其附近可行走的点的预估代价F，然后放进行走队列内
* 附近可行走的点不能是墙、不能是已走过的
* 可行走队列内有点A与新探索的点B位置相同，则比较代价F谁更小，若点A更小则忽略点B，若点B更小，则将点B替换成点A
* 预估代价F=起始点到当前点的代价G+当前点到结束点的预估待机H
* H有很多算法，根据场景不一样可采用相对的，这里采用的是忽略墙壁存在的直接距离
package org.lqh.game.framework.test.afind;
import lombok.Getter;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.HashSet;
import java.util.List;
import java.util.Map;
import java.util.PriorityQueue;
import java.util.Set;
import java.util.concurrent.ThreadLocalRandom;
/**
* TODO
*
* @author LeeQH
* @createTime 2021-3-6 10:42:39
*/
public class AFindPath {
//开放队列
private PriorityQueue<TryNode> openQueue = new PriorityQueue<>();
//开放集合
private Map<NodePos, TryNode> openMap = new HashMap<>();
//关闭集合
private Set<NodePos> closeSet = new HashSet<>();
//开始坐标
@Getter
private NodePos start;
//结束坐标
@Getter
private NodePos end;
//地图
private MapNode[][] mapNodes;
public AFindPath(MapNode[][] mapNodes, NodePos start, NodePos end) {
this.start = start;
this.end = end;
this.mapNodes = mapNodes;
}
private void openQueueAdd(TryNode tryNode) {
openQueue.add(tryNode);
openMap.put(tryNode.getPos(), tryNode);
}
private TryNode openQueuePoll() {
TryNode poll = openQueue.poll();
if (poll != null) {
openMap.remove(poll);
}
return poll;
}
public TryNode go() {
if (start.getX() < 0 || start.getY() < 0 || start.getX() >= mapNodes.length || start.getY() >= mapNodes[0].length || mapNodes[start.getX()][start.getY()].isWall()) {
System.out.println("start error");
return null;
}
if (end.getX() < 0 || end.getY() < 0 || end.getX() >= mapNodes.length || end.getY() >= mapNodes[0].length || mapNodes[end.getX()][end.getY()].isWall()) {
System.out.println("end error");
return null;
}
TryNode startNode = new TryNode(new NodePos(start.getX(), start.getY()));
TryNode endNode = new TryNode(new NodePos(end.getX(), end.getY()));
openQueueAdd(startNode);
//F最小的点
TryNode min = null;
while ((min = openQueuePoll()) != null) {
NodePos pos = min.getPos();
if (pos.equals(end)) {
return min;
}
//附近的行走点
List<MapNode> tlNearNode = mapNodes[pos.getX()][pos.getY()].getTlNearNode();
for (MapNode mapNode : tlNearNode) {
NodePos nearNodePos = new NodePos(mapNode.getPos().getX(), mapNode.getPos().getY());
//已经走过的点忽略
if (closeSet.contains(nearNodePos)) {
continue;
}
TryNode nearTryNode = new TryNode(nearNodePos);
nearTryNode.setG(min.getG() + 1);
nearTryNode.setH(distance(nearTryNode, endNode));
nearTryNode.setF(nearTryNode.getG() + nearTryNode.getH());
nearTryNode.setParentNode(min);
TryNode openTryNode = openMap.get(nearNodePos);
//已经探索过的点比当前探索的点的F大，替换
if (openTryNode != null) {
if (nearTryNode.getF() < openTryNode.getF()) {
openTryNode.setF(nearTryNode.getF());
openTryNode.setG(nearTryNode.getG());
openTryNode.setH(nearTryNode.getH());
openTryNode.setParentNode(min);
}
} else {
openQueueAdd(nearTryNode);
}
}
closeSet.add(min.getPos());
}
System.out.println("no way");
return null;
}
//两点距离
private float distance(TryNode try1, TryNode try2) {
return Math.abs(try1.getPos().getX() - try2.getPos().getX()) + Math.abs(try1.getPos().getY() - try2.getPos().getY());
}
////////////////////////////////////////////////////////////////////////////////////////
// static int[][] map = {
//// {0, 1, 2, 3, 4, 5, 6, 7, 8, 9}
// {1, 1, 1, 1, 1, 1, 1, 1, 1, 1},//0
// {1, 0, 0, 0, 0, 0, 0, 0, 0, 1},//1
// {1, 0, 0, 0, 1, 0, 0, 0, 0, 1},//2
// {1, 0, 0, 0, 1, 0, 0, 0, 0, 1},//3
// {1, 0, 1, 1, 1, 1, 1, 1, 1, 1},//4
// {1, 0, 0, 0, 1, 0, 0, 0, 0, 1},//5
// {1, 0, 0, 0, 1, 0, 0, 0, 0, 1},//6
// {1, 0, 0, 0, 1, 0, 0, 0, 0, 1},//7
// {1, 0, 0, 0, 0, 0, 0, 0, 0, 1},//8
// {1, 1, 1, 1, 1, 1, 1, 1, 1, 1},//9
// {1, 1, 1, 1, 1, 1, 1, 1, 1, 1},//10
// {1, 1, 1, 1, 1, 1, 1, 1, 1, 1},//11
// };
//初始化地图
static MapNode[][] initMap(int[][] map) {
int row = map.length;
int col = map[0].length;
MapNode[][] nodes = new MapNode[row][col];
for (int i = 0; i < row; i++) {
for (int j = 0; j < col; j++) {
MapNode node = new MapNode(new NodePos(i, j), map[i][j]);
nodes[i][j] = node;
}
}
for (int i = 0; i < row; i++) {
for (int j = 0; j < col; j++) {
MapNode node = nodes[i][j];
if (node.isWall()) {//墙
continue;
}
//初始化临近点
List<MapNode> tlNearNode = new ArrayList<>();
//上
if (j - 1 >= 0 && !nodes[i][j - 1].isWall()) {
tlNearNode.add(nodes[i][j - 1]);
}
//下
if (j + 1 < col && !nodes[i][j + 1].isWall()) {
tlNearNode.add(nodes[i][j + 1]);
}
//左
if (i - 1 >= 0 && !nodes[i - 1][j].isWall()) {
tlNearNode.add(nodes[i - 1][j]);
}
//右
if (i + 1 < row && !nodes[i + 1][j].isWall()) {
tlNearNode.add(nodes[i + 1][j]);
}
//左上
if (i - 1 >= 0 && j - 1 >= 0 && !nodes[i - 1][j - 1].isWall()) {
tlNearNode.add(nodes[i - 1][j - 1]);
}
//左下
if (i - 1 >= 0 && j + 1 < col && !nodes[i - 1][j + 1].isWall()) {
tlNearNode.add(nodes[i - 1][j + 1]);
}
//右上
if (i + 1 < row && j - 1 >= 0 && !nodes[i + 1][j - 1].isWall()) {
tlNearNode.add(nodes[i + 1][j - 1]);
}
//右下
if (i + 1 < row && j + 1 < col && !nodes[i + 1][j + 1].isWall()) {
tlNearNode.add(nodes[i + 1][j + 1]);
}
if (!tlNearNode.isEmpty()) {
node.setTlNearNode(tlNearNode);
}
}
}
return nodes;
}
//打印行走区域
static void printWay(int[][] map, AFindPath findPath) {
System.out.println("\nstart:" + findPath.start + ",end:" + findPath.end);
TryNode node = findPath.go();
if (node == null) {
return;
}
int[][] clone = map.clone();
while (node != null) {
clone[node.getPos().getX()][node.getPos().getY()] = -1;
node = node.getParentNode();
}
printMap(clone);
}
//打印地图
static void printMap(int[][] map) {
for (int i = 0; i < map.length; i++) {
for (int j = 0; j < map[i].length; j++) {
if (map[i][j] == 1) {
System.out.print("■");
} else if (map[i][j] == 0) {
System.out.print("□");
} else {
System.out.print("★");
}
}
System.out.println();
}
}
//获取随机地图
static int[][] getRandomMap() {
int row = 30;
int col = 30;
int[][] map = new int[row][col];
for (int i = 0, size = row * col / 4; i < size; i++) {
int x = ThreadLocalRandom.current().nextInt(row);
int y = ThreadLocalRandom.current().nextInt(col);
map[x][y] = 1;
}
return map;
}
//地图上随机称称点
static NodePos randomPos(int[][] map) {
int x = ThreadLocalRandom.current().nextInt(map.length);
int y = ThreadLocalRandom.current().nextInt(map[0].length);
return new NodePos(x, y);
}
public static void main(String[] args) {
int[][] map = getRandomMap();
// printMap(map);
MapNode[][] mapNodes = initMap(map);
AFindPath findPath = new AFindPath(mapNodes, randomPos(map), randomPos(map));
printWay(map, findPath);
}
}
package org.lqh.game.framework.test.afind;
import lombok.Getter;
import lombok.Setter;
import java.util.List;
import java.util.Objects;
/**
* TODO
*
* @author LeeQH
* @createTime 2021-3-6 10:43:08
*/
@Setter
@Getter
public class MapNode {
private NodePos pos;//位置
private boolean wall;//强
private List<MapNode> tlNearNode;//附近的点
public MapNode(NodePos nodePos, int wall) {
this.pos = nodePos;
this.wall = wall == 1;
}
@Override
public boolean equals(Object o) {
if (this == o) return true;
if (o == null || getClass() != o.getClass()) return false;
MapNode node = (MapNode) o;
return Objects.equals(pos, node.pos);
}
@Override
public int hashCode() {
return Objects.hash(pos);
}
}
package org.lqh.game.framework.test.afind;
import lombok.Getter;
import lombok.Setter;
import java.util.Objects;
/**
* TODO
*
* @author LeeQH
* @createTime 2021-3-6 16:35:26
*/
@Getter
@Setter
public class NodePos {
private int x;
private int y;
public NodePos(int x, int y) {
this.x = x;
this.y = y;
}
@Override
public boolean equals(Object o) {
if (this == o)
return true;
if (o == null || getClass() != o.getClass())
return false;
NodePos nodePos = (NodePos) o;
return x == nodePos.x && y == nodePos.y;
}
@Override
public int hashCode() {
return Objects.hash(x, y);
}
@Override
public String toString() {
return "[" + x + "," + y + "]";
}
}
package org.lqh.game.framework.test.afind;
import lombok.Getter;
import lombok.Setter;
import java.util.Objects;
/**
* TODO
*
* @author LeeQH
* @createTime 2021-3-6 10:43:08
*/
@Setter
@Getter
public class TryNode implements Comparable<TryNode> {
private NodePos pos;//位置
private TryNode parentNode;//父节点
private float G;//父节点的G值 + 父节点到当前点的移动代价
private float H;//当前点到结束点的预估代价
private float F;//起始点到终点的代价
public TryNode(NodePos nodePos) {
this.pos = nodePos;
}
@Override
public int compareTo(TryNode o) {
if (F > o.getF()) {
return 1;
} else if (F < o.getF()) {
return -1;
} else {
return 0;
}
}
@Override
public boolean equals(Object o) {
if (this == o) return true;
if (o == null || getClass() != o.getClass()) return false;
TryNode tryNode = (TryNode) o;
return Objects.equals(pos, tryNode.pos);
}
@Override
public int hashCode() {
return Objects.hash(pos);
}
}
网友评论