A*多人尋路解決方案及優化策略

題外話

• 去年暑假開始接觸Unity，到現在(2019/06/23)已經快一年了，認爲自己成長了很多，感覺以前寫的代碼很醜陋
• 我的考試周(實際上連續了一個月)已經結束了，嘻嘻嘻
• 應一位在職美術的邀請，參與了某個遊戲的製作，這個也是遊戲的一個模塊，同時也因爲這個原因此篇博客只能提供解決方案和部分代碼，給不了項目文件
• IGG 2019G星計劃那個暑期實習，6月14號最終面了，結果需要等到7月5號，真是讓人心急啊，當場出結果多爽啊，就算是沒過心裏也不用惦記着（害怕7月5號那天，郵箱收到“很遺憾…”開頭的郵件）

如果你點開這篇博客，關於A*的基礎知識你一定已經知道了

如果不知道請點擊 https://blog.csdn.net/hitwhylz/article/details/23089415

多人尋路中存在的問題

1.不做任何設置直接尋路，會導致重疊

2.把周圍的點設置爲障礙物，直接二次尋路會導致性能消耗過大，額外專門開個線程還好一點

解決辦法

說的簡單一點就是：等待+二次尋路

這裏，我們把需要尋路的“角色”或“人物”稱爲 Role
具體思路：
1.爲每一個Role調用A*實例，得到路徑
2.設置CircleCastAll，如果此Role與其他Role產生“碰撞”，進入等待狀態，等待計時
3.在等待狀態中，對Role的前進方向投射CircleCast，如果前進方向沒有其他Role阻攔，繼續前進
4.等待計時超過一定限制，根據情況二次尋路，把阻擋的Role設置爲“不可通過”（實際上相當於又回到了第一步）
5.如果已經到達目的地，可以根據需求進行排列陣型（小範圍尋路）

效果圖

在下圖中我只點擊了一次

此效果圖並沒有步驟五的效果，其實我覺得沒寫View層，從這幾個點能看出個鬼，可能因爲我不是策劃？

關於優化

1. 使用二叉堆
什麼是二叉堆？http://www.ijiandao.com/2b/baijia/168869.html

反例：
去年我初次接觸A*的時候，用我魔改的紅黑樹存的節點，效率也有提高。
面試的時候我還說我的A*數據結構用的紅黑樹，但沒想到有二叉堆這麼合適的東西，可惡啊

2. 在二次尋路中縮小尋路範圍
3. 二次尋路範圍偏移
根據物體與目標的位置進行範圍偏移，總結的話就是一張圖，儘量合理利用尋路範圍
如何做到呢？
這裏有點A和點B，我們先求出兩點的中點C， 向量Vector = （mapCenterPos-mapStartPos），也就是尋路範圍中心點到尋路空間左下角的向量， 我們這次尋路的起點座標=C+Vector，就可以達到下圖中的效果，目的就是最大限度的利用尋路空間。

關於代碼

我在CSDN寫的第一篇博客就是Unity2D中A*尋路算法，當時初學，年輕不懂事，不知道優化，存儲節點用的紅黑樹（雖然也有性能提升，但是沒二叉堆合適），代碼複用性也不好
因爲項目原因不能提供多人尋路的代碼，只提供了新版本A*的代碼
其中包含我改好的二叉堆，如果想用的話只需要

MapGrids localMapGrids;
AStarWayFinding wayFind;
List<Vector2> wayList;

//初始化格子
localMapGrids = new MapGrids(mapSize, startPos, mapStep, layerMask);//地圖大小，起點，格子大小，碰撞投射層級
wayFind = new AStarWayFinding();
//把路徑存儲到wayList中
wayList=wayFind.WayFinding(rolePos, targetPos, localMapGrids);

以下是A*尋路所用到的全部代碼

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class HeapSort
{
    private OpenlistNode[] array;
    private int currentIndex;

    public HeapSort(int size)
    {
        array = new OpenlistNode[size];
        currentIndex = 0;
    }
    public OpenlistNode[] getList()
    {
        return this.array;
    }
    public int getSize()
    {
        return this.currentIndex;
    }
    //插入
    public void insertSort(OpenlistNode source)
    {
        array[currentIndex] = source;
        tickedUp(currentIndex++);
    }

    private void tickedUp(int index)
    {
        int parentIndex = (index - 1) / 2;//父節點索引
        OpenlistNode temp = array[index];
        while (index > 0 && array[parentIndex].F1 > temp.F1)
        {
            array[index] = array[parentIndex];
            index = parentIndex;
            parentIndex = (parentIndex - 1) / 2;
        }
        array[index] = temp;
    }
    public OpenlistNode getMin()
    {
        OpenlistNode minNode = array[0];
        array[0] = array[--currentIndex];
        trickleDown(0);
        return minNode;
    }
    private void trickleDown(int index)
    {
        OpenlistNode temp = array[index];
        int minChildIndex;
        while (index < currentIndex / 2)
        {
            int leftChildIndex = 2 * index + 1;
            int rightChildIndex = leftChildIndex + 1;

            //右子節點存在，且小於左子節點
            if (rightChildIndex < currentIndex && array[leftChildIndex].F1 > array[rightChildIndex].F1)
            {
                minChildIndex = rightChildIndex;
            }
            else
            {
                minChildIndex = leftChildIndex;
            }
            if (temp.F1 <= array[minChildIndex].F1)
            {
                break;
            }
            array[index] = array[minChildIndex];
            index = minChildIndex;
        }
        array[index] = temp;
    }
}
public class OpenlistNode
{
    private Grid grid;
    private int G;
    private int F;
    private OpenlistNode parent;
    private int currentX;
    private int currentY;
    public OpenlistNode(Grid grid, int g, int h, OpenlistNode parent)
    {
        this.grid = grid;
        this.G = g;
        this.F = h + g;
        this.parent = parent;
    }

    public Grid Grid { get => grid; set => grid = value; }
    public int G1 { get => G; set => G = value; }
    public int F1 { get => F; set => F = value; }
    public OpenlistNode Parent { get => parent; set => parent = value; }
    public int CurrentX { get => currentX; set => currentX = value; }
    public int CurrentY { get => currentY; set => currentY = value; }
}
public class Grid
{
    private bool wall;
    private Vector2 worldPos;

    public Grid(Vector2 worldPos)
    {
        this.WorldPos = worldPos;
    }
    public bool Wall { get => wall; set => wall = value; }
    public Vector2 WorldPos { get => worldPos; set => worldPos = value; }
}
//初始化和存儲所有格子所用的類
public class MapGrids
{
    private float mapSize;
    private Vector2 startPos;
    private Vector2 gridSize;
    public List<List<Grid>> Grids;
    public int xySize;
    public Vector2 getStartPos()
    {
        return this.startPos;
    }
    public Vector2 getGridSize()
    {
        return this.gridSize;
    }
    public float getMapSize()
    {
        return this.mapSize;
    }
    public MapGrids(float mapSize, Vector2 startPos, Vector2 gridSize, LayerMask layerMask)
    {
        this.mapSize = mapSize;
        this.startPos = startPos;
        this.gridSize = gridSize;
        Grids = new List<List<Grid>>();

        xySize = Mathf.FloorToInt(this.mapSize / this.gridSize.x);
        Vector2 direction = new Vector2(0.0f, 0.0f);
        Vector2 xStep = new Vector2(this.gridSize.x, 0.0f);
        Vector2 yStep = new Vector2(0.0f, this.gridSize.y);
        Vector2 worldPosStep = this.gridSize * 0.5f;

        for (int i = 0; i < xySize; i++)
        {
            Grids.Add(new List<Grid>());
            for (int j = 0; j < xySize; j++)
            {
                Grids[i].Add(new Grid(
                        this.startPos + xStep * i + yStep * j + worldPosStep
                    ));
                Grids[i][j].Wall = Physics2D.BoxCast(Grids[i][j].WorldPos, this.gridSize, 0.0f, direction, layerMask);
            }
        }
    }
}
public class AStarWayFinding
{
    private int[] startGrid;
    private int[] endGrid;
    private OpenlistNode current;
    private OpenlistNode pointer;
    private int[] currentPosXY;
    private HeapSort openList;
    private HashSet<Grid> closeList;
    private List<Vector2> way;
    private int[] gridXY;
    private bool isFirstTime;
    private int searchOpenCount;
    public List<Vector2> WayFinding(Vector2 startPos, Vector2 endPos, MapGrids mapGrids)
    {
        //初始化與計算座標所在格子
        way = new List<Vector2>();
        currentPosXY = new int[2];
        openList = new HeapSort(mapGrids.xySize*mapGrids.xySize);
        closeList = new HashSet<Grid>();
        startGrid = new int[2];
        endGrid = new int[2];
        gridXY = new int[2];
        isFirstTime = true;
        //如果不在界內
        if (endPos.x - mapGrids.getStartPos().x < 1
            || endPos.y - mapGrids.getStartPos().y < 1
            || endPos.x - mapGrids.getStartPos().x - mapGrids.getMapSize() > -1
            || endPos.y - mapGrids.getStartPos().y - mapGrids.getMapSize() > -1
            ) return null;

        startGrid[0] = Mathf.FloorToInt((startPos.x - mapGrids.getStartPos().x) / mapGrids.getGridSize().x);
        startGrid[1] = Mathf.FloorToInt((startPos.y - mapGrids.getStartPos().y) / mapGrids.getGridSize().y);
        endGrid[0] = Mathf.FloorToInt((endPos.x - mapGrids.getStartPos().x) / mapGrids.getGridSize().x);
        endGrid[1] = Mathf.FloorToInt((endPos.y - mapGrids.getStartPos().y) / mapGrids.getGridSize().y);

        //Debug.LogFormat("{0}{1}{2}{3}", startGrid[0], startGrid[1], endGrid[0], endGrid[1]);
        if (Mathf.Abs(startGrid[0] - endGrid[0]) < 2 && Mathf.Abs(startGrid[1] - endGrid[1]) < 2)
        {
            if (!mapGrids.Grids[endGrid[0]][endGrid[1]].Wall) return new List<Vector2>();
            return null;
        }

        //添加起點格子到OpenList
        current = new OpenlistNode(mapGrids.Grids[startGrid[0]][startGrid[1]], 0, getDistance(startGrid, endGrid), null);
        current.CurrentX = startGrid[0];
        current.CurrentY = startGrid[1];
        currentPosXY[0] = startGrid[0];
        currentPosXY[1] = startGrid[1];
        openList.insertSort(current);

        //尋路過程
        while (openList.getSize() != 0)
        {
            if (isFirstTime) openList.getMin();
            isFirstTime = false;
            closeList.Add(current.Grid);
            if (currentPosXY[0] < 2 || currentPosXY[0] > mapGrids.xySize - 2 || currentPosXY[1] < 2 || currentPosXY[1] > mapGrids.xySize - 2)
            {
                if (openList.getSize() == 0) break;
                current = openList.getMin();
                currentPosXY[0] = current.CurrentX;
                currentPosXY[1] = current.CurrentY;
            }
            else
            {
                if (aroundGrid(mapGrids))
                {
                    getWay(mapGrids);
                    return way;
                }
                if (openList.getSize() == 0) break;
                current = openList.getMin();
                currentPosXY[0] = current.CurrentX;
                currentPosXY[1] = current.CurrentY;
            }
        }
        //-----------Debug.Log("尋路失敗");
        return null;
    }
    //獲得路徑
    private void getWay(MapGrids mapGrids)
    {
        while (!(current.Parent.CurrentX == startGrid[0] && current.Parent.CurrentY == startGrid[1]))
        {
            way.Add(current.Grid.WorldPos);
            current = current.Parent;
        }
        way.Reverse();
    }
    //遍歷周圍8個格子，如果周圍存在目標那麼尋路成功
    private bool aroundGrid(MapGrids mapGrids)
    {
        for (int i = -1; i < 2; i++)
        {
            for (int j = -1; j < 2; j++)
            {
                if (i == 0 && j == 0) continue;
                gridXY[0] = currentPosXY[0] + i;
                gridXY[1] = currentPosXY[1] + j;
                if (gridXY[0] == endGrid[0] && gridXY[1] == endGrid[1])
                {
                    //-------------Debug.Log("尋路成功");
                    return true;
                }
                if (mapGrids.Grids[gridXY[0]][gridXY[1]].Wall || closeList.Contains(mapGrids.Grids[gridXY[0]][gridXY[1]]))
                {
                    continue;
                }
                if (searchOpen(mapGrids.Grids[gridXY[0]][gridXY[1]]))
                {
                    OpenlistNode opListNode = new OpenlistNode(mapGrids.Grids[gridXY[0]][gridXY[1]], current.G1 + getDistance(currentPosXY, gridXY), getDistance(gridXY, endGrid), current);
                    //Debug.LogFormat("{0},{1}", gridXY[0], gridXY[1]);
                    opListNode.CurrentX = gridXY[0];
                    opListNode.CurrentY = gridXY[1];
                    openList.insertSort(opListNode);
                }
            }
        }
        //Debug.Log("__________________");
        return false;
    }
    private bool searchOpen(Grid grid)
    {
        searchOpenCount = 0;
        foreach (OpenlistNode oln in openList.getList())
        {
            if (++searchOpenCount > openList.getSize()) return true;
            if (grid == oln.Grid) return false;
        }
        return true;
    }
    //節點消耗計算
    private int getDistance(int[] a, int[] b)
    {
        return Mathf.Abs(a[0] - b[0]) * 10 + Mathf.Abs(a[1] - b[1]) * 14;
    }
}