A*寻路算法Java实现

A*算法：

把起始格添加到开启列表。
重复如下的工作：
a) 寻找开启列表中F值最低的格子。我们称它为当前格。
b) 把它切换到关闭列表。
c) 对相邻的8格中的每一个
- 如果它不可通过或者已经在关闭列表中，略过它。反之如下。
- 如果它不在开启列表中。把当前格作为这一格的父节点。记录这一格的F,G,和H值，把它添加开启列表中。
- 如果它已经在开启列表中，用G值为参考检查新的路径是否更好。更低的G值意味着更好的路径。如果是这样，就把这一格的父节点改成当前格，并且重新计算这一格的G和F值。如果你保持你的开启列表按F值排序，改变之后你可能需要重新对开启列表排序。
d) 停止，当你
- 把目标格添加进了开启列表，这时候路径被找到，或者
- 没有找到目标格，开启列表已经空了。这时候，路径不存在。
保存路径。从目标格开始，沿着每一格的父节点移动直到回到起始格。这就是你的路径。

伪代码

//OPEN表可以使用优先队列实现
while(OPEN!=NULL)
{
    从OPEN表中取f(n)最小的节点n;
    将n节点插入CLOSE表中;
    if(n节点==目标节点)
        break;
    for(当前节点n的每个子节点X)
    {
        if(X in OPEN)
            计算f(X);
            if(新的f(X)<OPEN中的f(X))
            {
                把n设置为X的父亲;
                更新OPEN表中的f(n);
            }
        if(X in CLOSE)
            continue;
        if(X not inboth)
        {
            把n设置为X的父亲;
            求f(X);//设置到X中再放入OPEN表
            并将X插入OPEN表中;
        }
    }//endfor
}//endwhile(OPEN!=NULL)

代码实现

import android.support.annotation.NonNull;
import java.util.HashSet;
import java.util.LinkedList;
import java.util.List;
import java.util.PriorityQueue;
import java.util.Set;

/**
 * Created by 默默 on 2018/3/27.
 */

public class FindPath {

    public static final char BAR = '@';

    private class Coord implements Comparable<Coord> {
        char value;
        int x;
        int y;
        Coord parent;
        int g;
        int h;
        int f;

        @Override
        public int compareTo(@NonNull Coord o) {
            return f - o.f;
        }
    }

    private PriorityQueue<Coord> openMap = new PriorityQueue<>();

    private Set<Coord> closeMap = new HashSet<>();
    //函数
    private int G(Coord start, Coord now) {
        //如果斜着走，近似模拟斜边距离的比例
        if (start.x != now.x && start.y != now.y) {
            return Math.abs(start.x - now.x) * 14;
        }
        return (Math.abs(start.x - now.x) + Math.abs(start.y - now.y)) * 10;
    }
    //函数H 曼哈顿距离*10
    private int H(Coord end, Coord now) {
        return (Math.abs(end.x - now.x) + Math.abs(end.y - now.y)) * 10;
    }

    private int F(Coord start, Coord now, Coord end) {
        return G(start, now) + H(end, now);
    }

    public void findPath(char[][] map, int startX, int startY, int endX, int endY) {

        Coord[][] coords = convert(map);
        Coord startCoord = coords[startX][startY];
        Coord endCoord = coords[endX][endY];
        startCoord.g = 0;
        startCoord.h = H(endCoord, startCoord);
        startCoord.f = startCoord.g + startCoord.h;
        //1.把起始格添加到开启列表。
        openMap.add(startCoord);
        //2. 重复如下的工作：
        while (!openMap.isEmpty()) {
            //a) 寻找开启列表中F值最低的格子。我们称它为当前格。
            Coord nowCoord = openMap.poll();
            //b) 把它切换到关闭列表。
            closeMap.add(nowCoord);
            if (nowCoord.equals(endCoord)) {
                break;
            }
            //c) 对相邻的8格中的每一个
            for (Coord next : getNearByCoords(coords, nowCoord)) {
                //如果它不可通过或者已经在关闭列表中，略过它。反之如下。
                if (next.value == BAR || closeMap.contains(next)) {
                    continue;
                }
                //如果它不在开启列表中。把当前格作为这一格的父节点。记录这一格的F,G,和H值，把它添加开启列表中。
                if (!openMap.contains(next)) {
                    next.parent = nowCoord;
                    next.g = nowCoord.g + G(nowCoord, next);
                    next.h = H(endCoord, next);
                    next.f = next.g + next.h;
                    //计算值后再放入开启列表中
                    openMap.add(next);
                } else {//如果它已经在开启列表中，用G值为参考检查新的路径是否更好。更低的G值意味着更好的路径。如果是这样，就把这一格的父节点改成当前格，并且重新计算这一格的G和F值。如果你保持你的开启列表按F值排序，改变之后你可能需要重新对开启列表排序。
                    if (next.g > nowCoord.g + G(nowCoord, next)) {
                        next.parent = nowCoord;
                        next.g = nowCoord.g + G(nowCoord, next);
                        next.f = next.g + next.h;
                    }
                }
            }
        }

        if (openMap.isEmpty()) {
            System.out.println("没有找到路径");
        } else {
            printPath(endCoord);
            System.out.println();
            startCoord.value = 'S';
            endCoord.value = 'E';

            int col = map[0].length;
            int row = map.length;
            for (int i = 0; i < row; i++) {
                for (int j = 0; j < col; j++) {
                    System.out.print(coords[i][j].value + " ");
                }
                System.out.println();
            }
        }
    }

    private void printPath(Coord end) {
        if (end.parent != null) {
            end.parent.value = '#';
            printPath(end.parent);
        }
        System.out.print("(" + end.x + "," + end.y + ")-> ");
    }

    private List<Coord> getNearByCoords(Coord[][] coords, Coord nowCoord) {
        List<Coord> coordList = new LinkedList<>();
        //上
        if (nowCoord.x - 1 >= 0) {
            coordList.add(coords[nowCoord.x - 1][nowCoord.y]);
        }
        //右上
        if (nowCoord.x - 1 >= 0 && nowCoord.y + 1 < coords[0].length) {
            coordList.add(coords[nowCoord.x - 1][nowCoord.y + 1]);
        }
        //右
        if (nowCoord.y + 1 < coords[0].length) {
            coordList.add(coords[nowCoord.x][nowCoord.y + 1]);
        }
        //右下
        if (nowCoord.x + 1 < coords.length && nowCoord.y + 1 < coords[0].length) {
            coordList.add(coords[nowCoord.x + 1][nowCoord.y + 1]);
        }
        //下
        if (nowCoord.x + 1 < coords.length) {
            coordList.add(coords[nowCoord.x + 1][nowCoord.y]);
        }
        //左下
        if (nowCoord.x + 1 < coords.length && nowCoord.y - 1 >= 0) {
            coordList.add(coords[nowCoord.x + 1][nowCoord.y - 1]);
        }
        //左
        if (nowCoord.y - 1 >= 0) {
            coordList.add(coords[nowCoord.x][nowCoord.y - 1]);
        }
        //左上
        if (nowCoord.x - 1 >= 0 && nowCoord.y - 1 >= 0) {
            coordList.add(coords[nowCoord.x - 1][nowCoord.y - 1]);
        }
        return coordList;
    }

    private Coord[][] convert(char[][] map) {
        int col = map[0].length;
        int row = map.length;
        Coord[][] coords = new Coord[row][col];
        for (int i = 0; i < row; i++) {
            for (int j = 0; j < col; j++) {
                Coord coord = new Coord();
                coord.x = i;
                coord.y = j;
                coord.value = map[i][j];
                coords[i][j] = coord;
            }
        }
        return coords;
    }

    @org.junit.Test
    public void testFindPath() {
        char[][] map = {
                {'.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.'},
                {'.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.'},
                {'.', '.', '.', '@', '@', '.', '.', '.', '.', '.', '.', '.'},
                {'.', '.', '.', '.', '@', '.', '.', '.', '.', '.', '.', '.'},
                {'.', '.', '.', '.', '@', '.', '.', '.', '.', '.', '.', '.'},
                {'.', '.', '.', '.', '@', '.', '.', '.', '.', '.', '.', '.'},
                {'.', '.', '.', '@', '@', '.', '.', '.', '.', '.', '.', '.'},
                {'.', '.', '.', '@', '.', '.', '.', '.', '.', '.', '.', '.'},
                {'.', '.', '.', '@', '.', '.', '.', '.', '.', '.', '.', '.'},
                {'.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.'},
                {'.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.'},
                {'.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.', '.'},
        };
        findPath(map, 4, 2, 5, 10);
    }

}

结果

(4,2)-> (3,2)-> (2,2)-> (1,3)-> (1,4)-> (2,5)-> (3,6)-> (4,7)-> (5,8)-> (5,9)-> (5,10)-> 
. . . . . . . . . . . . 
. . . # # . . . . . . . 
. . # @ @ # . . . . . . 
. . # . @ . # . . . . . 
. . S . @ . . # . . . . 
. . . . @ . . . # # E . 
. . . @ @ . . . . . . . 
. . . @ . . . . . . . . 
. . . @ . . . . . . . . 
. . . . . . . . . . . . 
. . . . . . . . . . . . 
. . . . . . . . . . . .

A*寻路算法Java实现

A*算法：

伪代码

代码实现

结果

猜你喜欢