A*尋路算法的主要思路
該算法主要是給了尋路時(shí)提供了一個(gè)方向（永遠(yuǎn)去看代價(jià)F最小的點(diǎn)）
偽代碼
- 不斷從可行走隊(duì)列內(nèi)取出預(yù)估代價(jià)F最小的點(diǎn)言秸，判斷是否結(jié)束點(diǎn)
- 計(jì)算其附近可行走的點(diǎn)的預(yù)估代價(jià)F命雀，然后放進(jìn)行走隊(duì)列內(nèi)
* 附近可行走的點(diǎn)不能是墻铛绰、不能是已走過(guò)的
* 可行走隊(duì)列內(nèi)有點(diǎn)A與新探索的點(diǎn)B位置相同算行，則比較代價(jià)F誰(shuí)更小呈驶，若點(diǎn)A更小則忽略點(diǎn)B努释，若點(diǎn)B更小碘梢，則將點(diǎn)B替換成點(diǎn)A
* 預(yù)估代價(jià)F=起始點(diǎn)到當(dāng)前點(diǎn)的代價(jià)G+當(dāng)前點(diǎn)到結(jié)束點(diǎn)的預(yù)估待機(jī)H
* H有很多算法，根據(jù)場(chǎng)景不一樣可采用相對(duì)的伐蒂，這里采用的是忽略墻壁存在的直接距離
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 {
//開(kāi)放隊(duì)列
private PriorityQueue<TryNode> openQueue = new PriorityQueue<>();
//開(kāi)放集合
private Map<NodePos, TryNode> openMap = new HashMap<>();
//關(guān)閉集合
private Set<NodePos> closeSet = new HashSet<>();
//開(kāi)始坐標(biāo)
@Getter
private NodePos start;
//結(jié)束坐標(biāo)
@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最小的點(diǎn)
TryNode min = null;
while ((min = openQueuePoll()) != null) {
NodePos pos = min.getPos();
if (pos.equals(end)) {
return min;
}
//附近的行走點(diǎn)
List<MapNode> tlNearNode = mapNodes[pos.getX()][pos.getY()].getTlNearNode();
for (MapNode mapNode : tlNearNode) {
NodePos nearNodePos = new NodePos(mapNode.getPos().getX(), mapNode.getPos().getY());
//已經(jīng)走過(guò)的點(diǎn)忽略
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);
//已經(jīng)探索過(guò)的點(diǎn)比當(dāng)前探索的點(diǎn)的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;
}
//兩點(diǎn)距離
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;
}
//初始化臨近點(diǎn)
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;
}
//打印行走區(qū)域
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();
}
}
//獲取隨機(jī)地圖
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;
}
//地圖上隨機(jī)稱稱點(diǎn)
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;//強(qiáng)
private List<MapNode> tlNearNode;//附近的點(diǎn)
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;//父節(jié)點(diǎn)
private float G;//父節(jié)點(diǎn)的G值 + 父節(jié)點(diǎn)到當(dāng)前點(diǎn)的移動(dòng)代價(jià)
private float H;//當(dāng)前點(diǎn)到結(jié)束點(diǎn)的預(yù)估代價(jià)
private float F;//起始點(diǎn)到終點(diǎn)的代價(jià)
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);
}
}
