AStar算法()

　　把網上的AStar算法的論述自己實現了一遍，一開始只是最基礎的實現。當然，現在AStar算法已經演變出了各種優化的版本，這篇也會基於各種優化不斷的更新。

　　如果對算法不熟悉可以看下Stanford的這篇文章，我覺得是講解的十分仔細的了：http://theory.stanford.edu/~amitp/GameProgramming/，也附上國內的翻譯：http://blog.csdn.net/coutamg/article/details/53923717

　　講講我對上面這篇文章的理解：

　　（1）AStar算法的核心就在於這個公式了f(n) = g(n) + h(n)，算法的效果如何也都取決於這個公式。就如文章中說的，g(n)可以看做從start到current所花費的cost，h(n)從current到end的花費。

　　　　很多人會直接將這兩個當做距離來計算，這是在忽略地形等條件影響下最簡單的模型。對於每一步，我們可以確定g(n)的准確值，但是h(n)很難預估正確的值（特別是在復雜且大型的場景中）。文章中也提供了幾種解決方案比如waypoint等。

　　（2）算法維護着兩張表openlist和closelist，openlist初始化時將start加入。

　　　　在循環尋找路徑時，將openlist中優先級最高的元素取出，移入closelist中，表示該點已經“探測”過。對該點的周圍N個neighbor進行檢測，符合條件將其加入openlist中，並對openlist進行優先級排序。

　　既然是對基礎的簡單理解，就不多說直接貼上代碼：

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

public class AStar  {

    public enum POINT_TYPE
    {
        normal,
        obstacle,
    }

    public class POINT:System.IComparable
    {
        float _gValue, _hValue;
        public float gValue
        {
            get
            {
                return _gValue;
            }
            set
            {
                _gValue = value;
                fValue = AStar.GetFValue(pos,gValue,hValue);
            }
        }

        public float hValue
        {
            get
            {
                return _hValue;
            }
            set
            {
                _hValue = value;
                fValue = AStar.GetFValue(pos, gValue, hValue);
            }
        }
        public float fValue
        {
            get;
            private set;
        }
        public Vector2 pos, parent;
        public POINT_TYPE type;

        public int CompareTo(object obj)
        {
            POINT pt = obj as POINT;
            if (fValue < pt.fValue)
                return -1;
            else if (fValue == pt.fValue)
                return 0;
            else
                return 1;
        }

        
    }

    public Dictionary<Vector2, POINT> points = new Dictionary<Vector2, POINT>();
    public static Vector2 startPt, endPt;

    public List<POINT> openList = new List<POINT>();
    public List<POINT> closeList = new List<POINT>();

    public bool finish = false;


    public static float GetFValue(Vector2 pt,float gValue,float hValue)
    {
        Vector2 vec1 = pt - startPt;
        Vector2 vec2 = endPt - startPt;
        float fac = Vector3.Cross(new Vector3(vec1.x, vec1.y, 0), new Vector3(vec2.x, vec2.y, 0)).normalized.z > 0 ? 0.01f : -0.01f;
        return gValue + 2f * hValue + fac;
    }

    float GetManhattanDistance(Vector2 pos1, Vector2 pos2)
    {
        return Mathf.Abs(pos1.x - pos2.x) + Mathf.Abs(pos1.y - pos2.y);
    }

    List<POINT> GetNeighbours(Vector2 pt)
    {
        List<POINT> neighbouts = new List<POINT>();
        if (points.ContainsKey(new Vector2(pt.x - 1, pt.y)))
            neighbouts.Add(points[new Vector2(pt.x - 1, pt.y)]);
        if (points.ContainsKey(new Vector2(pt.x + 1, pt.y)))
            neighbouts.Add(points[new Vector2(pt.x + 1, pt.y)]);
        if (points.ContainsKey(new Vector2(pt.x, pt.y +1)))
            neighbouts.Add(points[new Vector2(pt.x, pt.y +1)]);
        if (points.ContainsKey(new Vector2(pt.x, pt.y - 1)))
            neighbouts.Add(points[new Vector2(pt.x, pt.y - 1)]);
        return neighbouts;
    }

    public void Init(List<POINT> pts,Vector2 start,Vector2 end)
    {
        foreach (POINT pt in pts)
        {
            points.Add(pt.pos, pt);
        }

        startPt = start;
        endPt = end;

        points[startPt].parent = start;
        points[startPt].gValue = 0;
        points[startPt].hValue = Mathf.Abs(startPt.x - endPt.x) + Mathf.Abs(startPt.y - endPt.y);

        openList.Add(points[startPt]);

        finish = false;
    }

    public void StepNext()
    {
        if (finish)
            return;

        POINT current = openList[0];
        openList.Remove(current);
        closeList.Add(current);
        if (current.pos == endPt)
        {
            finish = true;
            return;
        }

        List<POINT> neighbours = GetNeighbours(current.pos);
        for (int i = 0; i < neighbours.Count; i++)
        {
            if (neighbours[i].type == POINT_TYPE.obstacle || closeList.Contains(neighbours[i]))
                continue;

            bool needSort = false;
            float gValue =GetManhattanDistance(neighbours[i].pos,startPt);
            float hValue = GetManhattanDistance(neighbours[i].pos,endPt);
            float fValue = AStar.GetFValue(neighbours[i].pos,gValue,hValue);

            if (openList.Contains(neighbours[i]))
            {
                if (neighbours[i].fValue > fValue)
                {
                    neighbours[i].gValue = gValue;
                    neighbours[i].hValue = hValue;
                    needSort = true;
                }
            }
            else
            {
                neighbours[i].gValue = gValue;
                neighbours[i].hValue = hValue;
                neighbours[i].parent = current.pos;
                openList.Add(neighbours[i]);
                needSort = true;
            }


            if (needSort)
                openList.Sort();
        }

    }

}

　　簡單的標注這幾行：

　　49-58 ：繼承於IComparable接口類，並且重寫了CompareTo方法。這樣就可以利用List<T>.Sort()來排序了。在CompareTo方法中，當fValue相等時，hValue值小具有更高的priority。關於fValue相同的情況在論文中有闡述。

　　71-77：根據g(n),h(n)計算f(n)。上面說的算法的核心公式是f(n) = g(n) + h(n)，但是很多時候這樣簡單的相加並不能適應各種復雜的情況。考慮這樣一種情況：

            

（請忽略這張圖中坐標下的數值（f值），並不與代碼相符）

　　藍色為當前探測的點，黃色為待探測的點。若用公式f(n) = g(n) + h(n)此時有兩種相等（f和h都相同）的情況，這樣將增大計算的消耗。這里簡單的用了cross函數使相等時總是能選擇某一側作為偏向。

　　138-139：g和h的值分別為n點到start和end點的曼哈頓距離（x，y軸上距離相加），這里其實只是近似值，並且g的值其實來說並不准確。上文說道g值對於每一步的計算來說都是可以確定的。在本例中暫且這樣，下一例中改進。

　　來看一下算法的效果：

　　

　　第一張圖從（0,10）到（14,2），第二張圖從（0,12）到（14,2）。綠色為算法最終輸出的路徑，黃色與藍色為檢測的范圍。

　　而在實現過程中也法線了g和h對算法的影響。當g>>h時，算法偏向於全方位的檢測，當h>>g時，算法偏向於向end點方向檢測。