Research on Floyd's Algorithm
theoretical basis
"Research on Indoor Robot Path Planning Based on Optimal Floyd Algorithm"
It is recommended to watch the first B station video and the third blog first, so that you can have a quick understanding of the Floyd algorithm
多源点The Floyd algorithm, also known as the interpolation method, is an algorithm for finding the shortest path between given weighted graphs . Floyd's algorithm is an algorithm suitable for solving the shortest path between any two points , and it is also used to calculate the transitive closure of a directed graph. This algorithm is simple and effective, and because of its compact triple loop structure, for dense graphs, the planning efficiency is higher than Dijkstra's algorithm
The Floyd algorithm is mainly used to find the shortest path with multiple sources and no negative weight edges. The Floyd algorithm is a classic dynamic programming algorithm.
The shortest path from any node ito any node jis nothing more than two possibilities, one is directly from ito j, and the other is from iseveral nodes ktoj
We assume
Dis(i,j)that it is the distance of the shortest path from nodeuto nodev. For each nodek, we checkDis(i,k) + Dis(k,j) < Dis(i,j)whether it is true. If it is true, it proves that the path fromito to tokis shorterjthan the pathito directlyj. We will set itDis(i,j) = Dis(i,k) + Dis(k,j). In this way, when we traverse After finishing all nodesk,Dis(i,j)the distance recorded in is the shortest path from i to j
Algorithm Description:
- Start with any one-sided path. The distance between all two points is the weight of the edge, if no edge connects the two points, the weight is infinite
- For each pair of vertices u and v, see if there exists a vertex w such that the path from u to w to v is shorter than the known path. if yes then update it
Floyd's algorithm uses two matrices to calculate the shortest path
- First, define an n×n matrix
D, where n is the number of vertices in the graph. Element D[i][j] of matrix D represents the weight of the shortest path from vertexito vertex .jInitially, the elements of the matrix D are initialized to the weights of the edges in the graph - Then, define an n×n matrix
Pfor recording the predecessor vertices of the shortest path. The element P[i][j] of the matrix P represents the number of the predecessor vertex of the vertex in the shortest path from theivertex to the vertex . Initially, the elements of the matrix P are initialized to the predecessor vertex numbers when there is an edge between the vertex and the vertexjjij - Next, the matrices D and P are calculated iteratively through two layers of loops. In each iteration, using the vertices
kas intermediate nodes, update the elements of the matrix sumDtoPfind a shorter path. The specific update rule is to determine whether to update the path and update the predecessor vertex by comparing the size of the matrix D[i][k] + D[k][j] with the current matrix D[i][j] - Finally, after all iterations are completed, the elements in the matrix D are the weights of the shortest path between each pair of vertices, and the elements in the matrix P can be used to restore the shortest path
The P matrix in the above figure is wrong, and the D matrix is easy to understand. The key is that it P 矩阵mainly involves 3 problems
- How the P matrix is initialized
- How the P matrix is updated in the loop
- How to restore the shortest path between two points through the P matrix
P matrix initialization
Before using the Floyd algorithm, the matrix P can be initialized to record the predecessor nodes for the shortest path between each pair of vertices
- If there is an edge from node i to node j, then P[i][j] can be initialized to
idenote that node i is the predecessor node of node j , and the path directly from node i to node j is part of the shortest path - If there is no edge from node i to node j, you can initialize P[i][j] to
-1denote that there is no path between node i and node j
Here is a sample code showing how to initialize the matrix P:
#include <iostream>
#include <vector>
void initializeP(std::vector<std::vector<int>>& P, const std::vector<std::vector<int>>& graph, int numVertices) {
for (int i = 0; i < numVertices; ++i) {
for (int j = 0; j < numVertices; ++j) {
if (graph[i][j] != INF && i != j) {
P[i][j] = i;
} else {
P[i][j] = -1;
}
}
}
}
int main() {
int numVertices = 4;
std::vector<std::vector<int>> graph = {
{
0, 3, 8, INF},
{
INF, 0, INF, 1},
{
INF, 4, 0, INF},
{
2, INF, -5, 0}};
std::vector<std::vector<int>> P(numVertices, std::vector<int>(numVertices));
// Initialize P matrix
initializeP(P, graph, numVertices);
// Print the initialized P matrix
for (int i = 0; i < numVertices; ++i) {
for (int j = 0; j < numVertices; ++j) {
std::cout << P[i][j] << " ";
}
std::cout << std::endl;
}
return 0;
}
The P matrix is updated in a loop
In Floyd's algorithm, the matrix P is used to record the predecessor vertices on the shortest path. The following is the update rule for the P matrix in the Floyd algorithm:
- Initialize matrix P: Suppose there is a graph G in which the vertices are numbered from 1 to n. Initially, the element P[i][j] of the matrix P is equal to
i, indicating that on the shortest path from vertex i to vertex j, the predecessor vertex of vertex j is vertex i - Iterative update of matrix P: Floyd algorithm updates matrix P and shortest path matrix D through step-by-step iteration. In each round of iteration, check whether vertex k can be used as the intermediate vertex of the shortest path from vertex i to vertex j. If possible, that is, there is a shorter path, the corresponding element P[i][j] in the update matrix P is
k
The specific update rules are as follows:
for k = 1 to n do
for i = 1 to n do
for j = 1 to n do
if (D[i][j] > D[i][k] + D[k][j]) then
D[i][j] = D[i][k] + D[k][j]
P[i][j] = P[k][j]
Among them, the D matrix represents the shortest path length between vertices. If it is found that the path from vertex i to vertex j is shorter when passing vertex k in the k-th iteration, then update D[i][j] to a smaller path length, and update P[i][j] to vertex k, indicating that the predecessor vertex of vertex j is vertex k
Through such iterative updates, we can finally get 最短路径矩阵Dand前驱矩阵P
Restoring the shortest path from P matrix
In the Floyd algorithm, the matrix P is used to record the predecessor nodes of the shortest path. The shortest path can be restored through the P matrix. Here are the steps to restore the shortest path:
- First, if
P[i][j] == -1, it means that there is no path from node i to node j, that is, there is no shortest path to restore - If
P[i][j] == i, indicates that the predecessor nodejof node is , that is, the path directly from node i to node j is part of the shortest pathi - If
P[i][j] ≠ i, it means thatjthe predecessor node of node isP[i][j]. It is necessary to recursively restore the path from nodejto nodeP[i][j], and output node j, that is, first restore the previous node, and then output node j
The D matrix is very easy to understand. The difficulty lies in the P matrix, which is not easy to define, resulting in misunderstanding. In fact, the P matrix can be understood as the pre-jump point of the target point that records the shortest path between vertices, and it does not directly obtain the shortest path. That is to say, a value in the P matrix cannot obtain the shortest path, but requires continuous iteration, using the preceding jump point of the previous step as the target point of this step, and continuing to search for the preceding jump point of the current target point , and proceed in this way until the starting point is found. Only in this way can the true shortest path be obtained
Talk is always cheap. Using the content in the third blog as an example, write test code
#include <vector>
#include <iostream>
using namespace std;
template<typename T>
void printArr(vector<T> arr)
{
for (T val : arr)
{
cout << val << "\t";
}
cout << endl;
}
template<typename T>
void printTwoDimensationArr(vector<vector<T>> arr)
{
int n = arr.size();
for (int i = 0; i < n; ++i)
{
printArr<T>(arr[i]);
}
cout << endl;
}
#define INF 99999
void restorePath(int i, int j, const vector<vector<int>>& P) {
if (i == j) {
cout << i << " ";
}
else if (P[i][j] == -1) {
cout << "No path exists";
}
else {
restorePath(i, P[i][j], P);
cout << j << " ";
}
}
void floydAlgorithm(vector<vector<int>>& graph, int numVertices) {
vector<vector<int>> dist(numVertices, vector<int>(numVertices));
vector<vector<int>> P(numVertices, vector<int>(numVertices));
// Initialize dist and P matrices
for (int i = 0; i < numVertices; ++i) {
for (int j = 0; j < numVertices; ++j) {
dist[i][j] = graph[i][j];
if (i == j || graph[i][j] == INF) {
P[i][j] = -1;
}
else {
P[i][j] = i;
}
}
}
// Floyd algorithm
cout << "Distance Array:" << endl;
printTwoDimensationArr<int>(dist);
cout << "Path Array:" << endl;
printTwoDimensationArr<int>(P);
for (int k = 0; k < numVertices; ++k) {
for (int i = 0; i < numVertices; ++i) {
for (int j = 0; j < numVertices; ++j) {
if (dist[i][j] > dist[i][k] + dist[k][j]) {
dist[i][j] = dist[i][k] + dist[k][j];
P[i][j] = P[k][j];
}
}
}
cout << "Distance Array:" << endl;
printTwoDimensationArr<int>(dist);
cout << "Path Array:" << endl;
printTwoDimensationArr<int>(P);
}
// Print shortest paths
for (int i = 0; i < numVertices; ++i) {
for (int j = 0; j < numVertices; ++j) {
if (i != j) {
cout << "Shortest path from " << i << " to " << j << ": ";
restorePath(i, j, P);
cout << endl;
}
}
}
}
int main() {
int numVertices = 4;
vector<vector<int>> graph = {
{
0, 2, 6, 4},
{
INF, 0, 3, INF},
{
7, INF, 0, 1},
{
5, INF, 12, 0} };
floydAlgorithm(graph, numVertices);
return 0;
}
In the above example code, graphrepresents the adjacency matrix of the graph, which INFrepresents the case where no direct edge exists between two vertices. numVerticesIndicates the number of vertices. The program calculates the shortest path through the Floyd algorithm, and usesrestorePath the function to restore the shortest path through the P matrix
The output is as follows
Distance Array:
0 2 6 4
99999 0 3 99999
7 99999 0 1
5 99999 12 0
Path Array:
-1 0 0 0
-1 -1 1 -1
2 -1 -1 2
3 -1 3 -1
Distance Array:
0 2 6 4
99999 0 3 99999
7 9 0 1
5 7 11 0
Path Array:
-1 0 0 0
-1 -1 1 -1
2 0 -1 2
3 0 0 -1
Distance Array:
0 2 5 4
99999 0 3 99999
7 9 0 1
5 7 10 0
Path Array:
-1 0 1 0
-1 -1 1 -1
2 0 -1 2
3 0 1 -1
Distance Array:
0 2 5 4
10 0 3 4
7 9 0 1
5 7 10 0
Path Array:
-1 0 1 0
2 -1 1 2
2 0 -1 2
3 0 1 -1
Distance Array:
0 2 5 4
9 0 3 4
6 8 0 1
5 7 10 0
Path Array:
-1 0 1 0
3 -1 1 2
3 0 -1 2
3 0 1 -1
Shortest path from 0 to 1: 0 1
Shortest path from 0 to 2: 0 1 2
Shortest path from 0 to 3: 0 3
Shortest path from 1 to 0: 1 2 3 0
Shortest path from 1 to 2: 1 2
Shortest path from 1 to 3: 1 2 3
Shortest path from 2 to 0: 2 3 0
Shortest path from 2 to 1: 2 3 0 1
Shortest path from 2 to 3: 2 3
Shortest path from 3 to 0: 3 0
Shortest path from 3 to 1: 3 0 1
Shortest path from 3 to 2: 3 0 1 2
Pushed the update process of D matrix and P matrix by hand, the initialization of P matrix has some minor flaws, and some positions are not initialized to -1
The right side simply simulates the process of recursively restoring the path from node 1 to node 0
