记录使用 Java 实现AStar 寻路算法

Posted 2021-06-16 八脚鱼哥哥

试着使用 Java 写一下 AStar 算法，提升一下编码能力。尽量不引用其他代码，在此记录。

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

public class AStar {
    private static double WEIGHT_TO_START = 1;
    private static double WEIGHT_TO_END = 1;

    private static class AStarPosNode {
        private int row;
        private int col;
        private int step;
        private double weight;
        private boolean searched;
        private boolean opened;
        private boolean closed;
        private AStarPosNode prev;

        @Override
        public boolean equals(Object otherObject) {
            if (this == otherObject) {
                return true;
            }
            if (!(otherObject instanceof AStarPosNode)) {
                return false;
            }
            AStarPosNode other = (AStarPosNode) otherObject;
            return row == other.row && col == other.col;
        }
    }

    public static List<int[]> findRoad(boolean[][] map, int[] startPos, int[] endPos) {
        int maxRow = Math.max(0, map.length - 1);
        int maxCol = 0;
        for (boolean[] mapRow : map) {
            maxCol = Math.max(maxCol, mapRow.length - 1);
        }

        AStarPosNode[][] aStarMap = new AStarPosNode[maxRow][maxCol];
        for (int row = 0; row < maxRow; row++) {
            for (int col = 0; col < maxCol; col++) {
                AStarPosNode aStarPosNode = new AStarPosNode();
                aStarPosNode.row = row;
                aStarPosNode.col = col;
                aStarPosNode.step = 0;
                aStarPosNode.weight = 0;
                aStarPosNode.searched = false;
                aStarPosNode.opened = false;
                aStarPosNode.closed = !map[row][col];
                aStarPosNode.prev = null;
                aStarMap[row][col] = aStarPosNode;
            }
        }

        AStarPosNode startAStarPosNode = aStarMap[startPos[0]][startPos[1]];
        AStarPosNode endAStarPosNode = aStarMap[endPos[0]][endPos[1]];

        List<AStarPosNode> openList = new ArrayList<>();
        List<AStarPosNode> testList = new ArrayList<>();

        _appendToOpenList(openList, startAStarPosNode);

        while (!openList.isEmpty()) {
            AStarPosNode testAStarPosNode = _getFromOpenList(openList);
            if (testAStarPosNode.equals(endAStarPosNode)) {
                break;
            }

            List<AStarPosNode> notSearchedAroundAStarPosNodeList
                    = _getNotSearchedAroundAStarPosNodeList(aStarMap, testAStarPosNode);

            if (notSearchedAroundAStarPosNodeList.isEmpty()) {
                _closeTestList(testList);
            } else {
                notSearchedAroundAStarPosNodeList.forEach(notSearchedAroundAStarPosNode -> {
                    notSearchedAroundAStarPosNode.step = testAStarPosNode.step + 1;
                    notSearchedAroundAStarPosNode.weight = _calWeight(
                            notSearchedAroundAStarPosNode.step,
                            _calDistance(notSearchedAroundAStarPosNode, endAStarPosNode)
                    );
                    _appendToOpenList(openList, notSearchedAroundAStarPosNode);
                });
            }
        }

        if (endAStarPosNode.prev == null) {
            throw new RuntimeException("路径未找到");
        } else {
            List<int[]> road = new ArrayList<>();
            AStarPosNode roadStarPosNode = endAStarPosNode;
            while (roadStarPosNode != null) {
                road.add(new int[]{roadStarPosNode.row, roadStarPosNode.col});
            }
            return road;
        }
    }

    private static double _calWeight(double step, double distanceToEnd) {
        return WEIGHT_TO_START * step + WEIGHT_TO_END * distanceToEnd;
    }

    private static double _calDistance(AStarPosNode one, AStarPosNode another) {
        int dRow = one.row - another.row;
        int dCol = one.col - another.col;
        return Math.abs(dRow) + Math.abs(dCol);
    }

    private static void _closeTestList(List<AStarPosNode> testList) {
        testList.forEach(testAStarPosNode -> {
            testAStarPosNode.closed = true;
        });
        testList.clear();
    }

    private static List<AStarPosNode> _getNotSearchedAroundAStarPosNodeList(AStarPosNode[][] aStarMap, AStarPosNode aStarPosNode) {
        List<AStarPosNode> notSearchedAroundAStarPosNodeList = new ArrayList<>();
        int[][] aroundPositions = {
                {aStarPosNode.row + 1, aStarPosNode.col},
                {aStarPosNode.row - 1, aStarPosNode.col},
                {aStarPosNode.row, aStarPosNode.col + 1},
                {aStarPosNode.row, aStarPosNode.col - 1},
                {aStarPosNode.row + 1, aStarPosNode.col + 1},
                {aStarPosNode.row + 1, aStarPosNode.col - 1},
                {aStarPosNode.row - 1, aStarPosNode.col + 1},
                {aStarPosNode.row - 1, aStarPosNode.col - 1},
        };
        for (int[] aroundPosition : aroundPositions) {
            int row = aroundPosition[0];
            int col = aroundPosition[1];
            if ((row >= 0 && row < aStarMap.length) &&
                    (col >= 0 && col < aStarMap[row].length) &&
                    (!aStarMap[row][col].searched) &&
                    (!aStarMap[row][col].opened) &&
                    (!aStarMap[row][col].closed)) {
                aStarMap[row][col].prev = aStarPosNode;
                aStarMap[row][col].searched = true;
                notSearchedAroundAStarPosNodeList.add(aStarMap[row][col]);
            }
        }
        return notSearchedAroundAStarPosNodeList;
    }

    private static AStarPosNode _getFromOpenList(List<AStarPosNode> openList) {
        AStarPosNode aStarPosNode = openList.remove(0);
        aStarPosNode.opened = false;
        return aStarPosNode;
    }

    private static void _appendToOpenList(List<AStarPosNode> openList, AStarPosNode aStarPosNode) {
        aStarPosNode.opened = true;
        _appendToAscList(openList, aStarPosNode, (one, another) -> {
            double compareResult = one.weight - another.weight;
            if (compareResult < 0) {
                return -1;
            } else if (compareResult == 0) {
                return 0;
            } else {
                return 1;
            }
        });
    }

    private static <T> void _appendToAscList(List<T> ascList, T item, Comparator<T> comparator) {
        if (ascList.size() > 0) {
            if (comparator.compare(item, ascList.get(0)) < 0) {
                ascList.add(0, item);
            } else {
                for (int index = 0; index < ascList.size() - 1; index++) {
                    int compareToPrev = comparator.compare(item, ascList.get(index));
                    int compareToNext = comparator.compare(item, ascList.get(index + 1));
                    if (compareToPrev >= 0 && compareToNext <= 0) {
                        ascList.add(index + 1, item);
                        break;
                    }
                }
            }
        } else {
            ascList.add(item);
        }
    }
}

以上代码基本实现了八个方向的寻路，如有错误，请指正。