弗洛伊德算法（Floyd）和路径平滑弗洛伊德算法（Smooth Floyd）学习

系列文章目录

A*算法学习-CSDN博客

---

目录

系列文章目录

前言

一、路径平滑弗洛伊德算法（Smooth Floyd）

二、弗洛伊德算法（Floyd）

多源最短路问题

总结

---

前言

昨天看了下A*算法（A-Star（A*）寻路算法原理与实现 - 知乎 (zhihu.com)），发现常见的a*算法的结果是一串用来表示所经过的路径点坐标。但是这样的路径通常是有“锯齿”的，并不符合现实中的智能表现。因此，需要进一步的进行平滑处理，比如平滑佛洛依德算法（佛洛依德路径平滑算法（FLOYD） - 知乎 (zhihu.com)）。在探索的过程中发现其和最短路径弗洛伊德（Floyd(弗洛伊德)算法 详解+模板_弗洛伊德算法代码-CSDN博客，c++图论算法1——Floyd（弗洛伊德）算法_c++弗洛伊德-CSDN博客）还不太一样。所以都总结一下，还区分一下。

---

一、路径平滑弗洛伊德算法（Smooth Floyd）

算法原理很简单，分为两步：

　　1.去掉相邻的共线的点

　　2.去掉多余的拐弯的点

　　第一步实现起来很简单，只需要遍历一下，计算两个向量的方向是否相同。

　　第二步的实现稍微麻烦一点，遍历所有的点，去掉两个可以直接通过的点之间的点。

　　有点绕。。。。

　　其实是很经典的画直线算法，找两个点作为端点画一条线，这条线经过的网格如果都是可通行的，那么我们就认为在路径中这两个点中间的那些点是多余的。

其实第二步就可以完成优化，但是计算量比较大。所以先通过第一步来减少一部分计算量~

下面是代码：

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

// 弗洛伊德路径平滑函数
public List Floyd(List 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;
                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

二、弗洛伊德算法（Floyd）

Floyd算法又称为插点法，是一种利用动态规划的思想寻找给定的加权图中多源点之间最短路径的算法。

多源最短路问题

时间复杂度：O(n^3)

空间复杂度：O(n^2)

可以看出时间复杂度是非常惊人的，可是它有一个很重要的特性：

Floyd最终求出了图中每对点之间的最短路

（这就是所谓“多源”，即多个起点）

因此在求多对点之间的最短路时，它是非常有优势的。

Floyd(弗洛伊德)算法 详解+模板_弗洛伊德算法代码-CSDN博客看一下这里的图，很好理解了

初始状态：S是记录各个顶点间最短路径的矩阵。
第1步：初始化S。
矩阵S中顶点a[i][j]的距离为顶点i到顶点j的权值；如果i和j不相邻，则a[i][j]=∞。实际上，就是将图的原始矩阵复制到S中。
注:a[i][j]表示矩阵S中顶点i(第i个顶点)到顶点j(第j个顶点)的距离。
第2步：以顶点A(第1个顶点)为中介点，若a[i][j] > a[i][0]+a[0][j]，则设置a[i][j]=a[i][0]+a[0][j]。
以顶点a[1]6，上一步操作之后，a[1][6]=∞；而将A作为中介点时，(B,A)=12，(A,G)=14，因此B和G之间的距离可以更新为26。
同理，依次将顶点B,C,D,E,F,G作为中介点，并更新a[i][j]的大小。

#include
#include
#include
using namespace std;
int m,n,d[301][301];
int main()
{
	memset(d,0x3f,sizeof(d));  //这里不能赋值为127，否则之后的加法会爆 
	for(int i=1;i<=n;i++) d[i][i]=0;  //自己到自己的距离为0
	cin>>n>>m;  //n为点数，m为边数 
	for(int i=1;i<=m;i++)
	{
		int u,v,w;  //u,v,w分别为起点、终点、权值 
		cin>>u>>v>>w;
		d[u][v]=min(d[u][v],w);  //处理重边 
		d[v][u]=min(d[v][u],w);  //无向图的双向性 
	}
	for(int k=1;k<=n;k++)  //一定要先枚举中转点 
	{
		for(int i=1;i<=n;i++)  //枚举起点 
		{
			for(int j=1;j<=n;j++)  //枚举终点 
			{
				//核心代码 
				if(d[i][k]+d[k][j]

---

总结

弗洛伊德算法（Floyd）和路径平滑弗洛伊德算法（Smooth Floyd）在目标和应用方面有一些差别：

1. 最短路径弗洛伊德算法：

   • 目标：找到图中所有节点之间的最短路径。
   • 过程：通过计算每一对节点之间的最短路径，更新路径矩阵，最终得到任意两节点之间的最短路径。
   • 适用场景：主要用于解决图中节点之间的最短路径问题，例如网络路由、地图导航等。

2. 平滑弗洛伊德算法：

   • 目标：在给定路径上去除冗余节点，使路径更加平滑。
   • 过程：检测相邻的节点，去除在同一直线上的多余节点，以及去除无用的拐点，使路径更加平滑。
   • 适用场景：主要用于在已知路径的情况下，对路径进行平滑处理，以提高可视化效果或者减少路径点的数量。

总体来说，最短路径弗洛伊德算法关注于路径长度的最短，而平滑弗洛伊德算法关注于路径形状的平滑。在某些场景中，可以将这两种算法结合使用，先使用最短路径算法找到路径，然后再对路径进行平滑处理。