欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

A*寻路算法Java实现

程序员文章站 2024-03-16 22:30:52
...

A*算法:

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

伪代码

//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 . 
. . . @ @ . . . . . . . 
. . . @ . . . . . . . . 
. . . @ . . . . . . . . 
. . . . . . . . . . . . 
. . . . . . . . . . . . 
. . . . . . . . . . . .