佛洛依德路徑平滑算法（floyd）

　　常見的a*算法的結果是一串用來表示所經過的路徑點坐標。但是這樣的路徑通常是有“鋸齒”的，並不符合現實中的智能表現。

因此，需要進一步的進行平滑處理，比如 佛洛依德算法~

　　算法原理很簡單，分為兩步：

　　1.去掉相鄰的共線的點

　　2.去掉多余的拐彎的點

　　第一步實現起來很簡單，只需要遍歷一下，計算兩個向量的方向是否相同。

　　第二步的實現稍微麻煩一點，遍歷所有的點，去掉兩個可以直接通過的點之間的點。

　　有點繞。。。。

　　其實是很經典的畫直線算法，找兩個點作為端點畫一條線，這條先經過的網格如果都是可同行的，那么我們就認為在路徑中這兩個點中間的那些點是多余的。

其實第二步就可以完成優化，但是計算量比較大。所以先通過第一步來減少一部分計算量~

下面是代碼：

#region floyd
    //----------------------------------------弗洛伊德路徑平滑--------------------------------------//

    public List<Vector3> Floyd( List<Vector3> path)
    {
        if (path == null) 
        {
            return path;
        }


        int len = path.Count;
        //去掉同一條線上的點。
        if (len > 2) 
        {
            Vector3 vector = path[len -1] - path[len - 2];
            Vector3 tempvector;
            for (int i = len - 3; i>= 0; i--) 
            {
                tempvector = path[i+1] - path[i];
                if (Vector3.Cross(vector, tempvector).y == 0f)
                {
                    path.RemoveAt(i+1);
                }
                else
                {
                    vector = tempvector;
                }
            }
        }
        //去掉無用拐點
        len = path.Count;
        for (int i = len-1; i >= 0; i--) 
        {
            for (int j = 0; j<= i-1; j++) 
            {
                if (CheckCrossNoteWalkable(path[i],path[j])) 
                {
                    for (int k = i-1; k>=j; k--)
                    {
                        path.RemoveAt(k);
                    }
                    i=j;
                    //len = path.Count;
                    break;
                }
            }
        }
        return path;
    }
    
    float currentY; // 用於檢測攀爬與下落高度
    //判斷路徑上是否有障礙物
    public bool CheckCrossNoteWalkable(Vector3 p1, Vector3 p2)
    {
        currentY = p1.y; //記錄初始高度，用於檢測是否可通過
        bool changexz = Mathf.Abs(p2.z - p1.z) > Mathf.Abs(p2.x - p1.x);
        if (changexz) 
        {
            float temp = p1.x;
            p1.x = p1.z;
            p1.z = temp;
            temp = p2.x;
            p2.x = p2.z;
            p2.z = temp;
        }
        if (!Checkwalkable(changexz, p1.x, p1.z)) 
        {
            return false;
        }
        float stepX = p2.x > p1.x ? Tilesize : (p2.x < p1.x ? -Tilesize : 0);
        float stepY = p2.y > p1.y ? Tilesize : (p2.y < p1.y ? -Tilesize : 0);
        float deltay = Tilesize * ( (p2.z - p1.z) / Mathf.Abs(p2.x - p1.x) );
        float nowX = p1.x + stepX/2;
        float nowY = p1.z - stepY/2;
        float CheckY = nowY;
        
        while (nowX != p2.x)
        {
            if(!Checkwalkable(changexz, nowX, CheckY))
            {
                return false;
            }
            nowY += deltay;
            if(nowY >= CheckY + stepY)
            {
                CheckY += stepY;
                if (!Checkwalkable(changexz, nowX, CheckY)) 
                {
                    return false;
                }
            }
            nowX += stepX;
        }
        return true;
    }
    private bool Checkwalkable(bool changeXZ, float x, float z)
    {      
        int mapx = (MapStartPosition.x < 0F) ? Mathf.FloorToInt(((x + Mathf.Abs(MapStartPosition.x)) / Tilesize)) : Mathf.FloorToInt((x - MapStartPosition.x) / Tilesize);
        int mapz = (MapStartPosition.y < 0F) ? Mathf.FloorToInt(((z + Mathf.Abs(MapStartPosition.y)) / Tilesize)) : Mathf.FloorToInt((z - MapStartPosition.y) / Tilesize);
        if (mapx < 0 || mapz < 0 || mapx >= Map.GetLength(0) || mapz >= Map.GetLength(1))
        {
            return false;
        }

        Node note;
        if (changeXZ)
        {
            note = Map[mapz, mapx];
        }
        else
        {
            note = Map[mapx, mapz];
        }
        bool ret = note. walkable && ( (note.yCoord - currentY <= ClimbLimit && note.yCoord >= currentY) || (currentY - note.yCoord <= MaxFalldownHeight && currentY >= note.yCoord) );
        if (ret) 
         {
             currentY = note.yCoord;
         }
        return ret;
    }
#endregion end floyd