A*寻路算法

A*寻路算法就是启发式探索的一个典型实践，在寻路的过程中，给每个节点绑定了一个估计值（即启发式），在对节点的遍历过程中是采取估计值优先原则，估计值更优的节点会被优先遍历。

如下图区域，被简化成6*6的小方格。其中绿色表示起点，红色表示终点，黑色表示路障，不能通行。

先描述A*算法的大致过程：

1. 将初始节点放入到open列表中。
2. 判断open列表。如果为空，则搜索失败。如果open列表中存在目标节点，则搜索成功。
3. 从open列表中取出F值最小的节点作为当前节点，并将其加入到close列表中。
4. 计算当前节点的相邻的所有可到达节点，生成一组子节点。对于每一个子节点：
   • 如果该节点在close列表中，则丢弃它
   • 如果该节点在open列表中，则检查其通过当前节点计算得到的F值是否更小，如果更小则更新其F值，并将其父节点设置为当前节点。
   • 如果该节点不在open列表中，则将其加入到open列表，并计算F值，设置其父节点为当前节点。
5. 转到2步骤

概括：

1、两个集合+一个公式
* openlist\closelist --- 集合
* F=G+H  --- 公式
* openlist：可达到的格子
* closelist：已经到达的格子
* G:从起点到当前格子的成本
* H：不考虑障碍的情况下，当前格子到目标格子的距离
* F：G+H

代码如下：

package com.wemew.wmgame.sort;

import java.util.ArrayList;
import java.util.List;

/**
 * A*寻路算法
 * <p>
 * 寻路算法
 * 1、两个集合+一个公式
 * openlist\closelist
 * F=G+H
 * openlist：可达到的格子
 * closelist：已经到达的格子
 * G:从起点到当前格子的成本
 * H：不考虑障碍的情况下，当前格子到目标格子的距离
 * F：G+H
 *
 */
public class FindRoad {

    public static void main(String[] args) {
        Grid startgrid = new Grid(2, 1);
        Grid endgrid = new Grid(2, 5);
        Grid grid = aStarSearch(startgrid, endgrid);
        List<Grid> path = new ArrayList();
        while (grid != null) {
            path.add(new Grid(grid.x, grid.y));
            grid = grid.parent;
        }
        // 循环打印
        for (int i = 0; i < MAZE.length; i++) {
            for (int i1 = 0; i1 < MAZE[0].length; i1++) {
                if (containArrys(path, i, i1)) {
                    System.out.print("$" + " ,");
                } else {
                    System.out.print(MAZE[i][i1] + " ,");
                }
            }
            System.out.println();
        }

        /**
         * 0 ,0 ,$ ,$ ,$ ,$ ,0 ,
         * 0 ,0 ,$ ,1 ,0 ,$ ,0 ,
         * 0 ,$ ,$ ,1 ,0 ,$ ,0 ,
         * 0 ,0 ,0 ,1 ,0 ,0 ,0 ,
         * 0 ,0 ,0 ,0 ,0 ,0 ,0 ,
         */

    }

    public static final int[][] MAZE = {
            {0, 0, 0, 0, 0, 0, 0},
            {0, 0, 0, 1, 0, 0, 0},
            {0, 0, 0, 1, 0, 0, 0},
            {0, 0, 0, 1, 0, 0, 0},
            {0, 0, 0, 0, 0, 0, 0},
    };

    /**
     * 寻路主逻辑
     *
     * @param start
     * @param end
     * @return
     */
    public static Grid aStarSearch(Grid start, Grid end) {
        List<Grid> openList = new ArrayList<>();
        List<Grid> closeList = new ArrayList<>();
        // 先加入到openlist
        openList.add(start);
        while (openList.size() > 0) {
            // 找到当前最小的F值
            Grid cureentGrid = fineMinGrid(openList);
            // 移除 openList
            openList.remove(cureentGrid);
            // 移动到 closeList
            closeList.add(cureentGrid);
            // 查询出附近的最佳节点
            List<Grid> neibers = findNeibers(cureentGrid, openList, closeList);
            for (Grid neiber : neibers) {
                if (!openList.contains(neiber)) {
                    neiber.initGrid(cureentGrid, end);
                    openList.add(neiber);
                }
            }
            for (Grid grid : openList) {
                if (end.x == grid.x && end.y == grid.y) {
                    return grid;
                }
            }
        }
        return null;
    }

    public static Grid fineMinGrid(List<Grid> grids) {
        Grid grid = grids.get(0);
        for (Grid grid1 : grids) {
            if (grid.getF() > grid1.getF()) {
                grid = grid1;
            }
        }
        return grid;
    }

    public static List<Grid> findNeibers(Grid grid, List<Grid> openList, List<Grid> closeList) {
        List<Grid> arraylist = new ArrayList<>();
        // 上
        if (isValidate(grid.x, grid.y - 1, openList, closeList)) {
            arraylist.add(new Grid(grid.x, grid.y - 1));
        }
        // 下
        if (isValidate(grid.x, grid.y + 1, openList, closeList)) {
            arraylist.add(new Grid(grid.x, grid.y + 1));
        }
        // 左
        if (isValidate(grid.x - 1, grid.y, openList, closeList)) {
            arraylist.add(new Grid(grid.x - 1, grid.y));
        }
        // 右
        if (isValidate(grid.x + 1, grid.y, openList, closeList)) {
            arraylist.add(new Grid(grid.x + 1, grid.y));
        }
        return arraylist;
    }

    public static boolean isValidate(int x, int y, List<Grid> openList, List<Grid> closeList) {
        // 是否越过边界
        if (x < 0 || x >= MAZE.length || y < 0 || y >= MAZE[0].length) {
            return false;
        }
        // 是否越过障碍
        if (MAZE[x][y] == 1) {
            return false;
        }
        // 是否包含openList
        if (containArrys(openList, x, y)) {
            return false;
        }
        // 是否包含openList
        if (containArrys(closeList, x, y)) {
            return false;
        }
        return true;
    }


    public static boolean containArrys(List<Grid> openList, int x, int y) {
        for (Grid grid : openList) {
            if (grid.x == x && grid.y == y) {
                return true;
            }
        }
        return false;
    }

    static class Grid {
        int x;
        int y;
        int f;
        int g;
        int h;
        Grid parent;

        // f g h
        void initGrid(Grid parent, Grid end) {
            this.parent = parent;
            if (parent != null) {
                this.g = parent.g + 1;
            } else {
                this.g = 1;
            }
            // 计算h值
            this.h = Math.abs(this.x - end.x) + Math.abs(this.y - end.y);
            // 计算f值
            this.f = this.g + this.h;
        }

        public Grid(int x, int y) {
            this.x = x;
            this.y = y;
        }

        public int getY() {
            return y;
        }

        public void setY(int y) {
            this.y = y;
        }

        public int getF() {
            return f;
        }

        public void setF(int f) {
            this.f = f;
        }

        public int getG() {
            return g;
        }

        public void setG(int g) {
            this.g = g;
        }

        public int getH() {
            return h;
        }

        public void setH(int h) {
            this.h = h;
        }

        public Grid getParent() {
            return parent;
        }

        public void setParent(Grid parent) {
            this.parent = parent;
        }

        public int getX() {
            return x;
        }

        public void setX(int x) {
            this.x = x;
        }
    }
}