佛洛依德路径平滑算法（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