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