Dijkstra's algorithm for optimal path (1)
#1. Algorithm principle
-
First initialize the distance (i.e. weight) of the two-dimensional array according to the path graph, and the array stores the distance from the corresponding point to each node.
For example:Metro[0]=[0, 2, 3, 6,2048,2048]It means that the distance from A to A is 0, the distance to B is 2, the distance to C is 3...
Add initial node A to the determined points, and set the status of point A to determined. at this time:已确定点数组 S={A}, 未确定点数组 U={B,C,D,E,F} 节点A到各个点的距离 prev={0, 2, 3, 6,∞,∞} -
Select the unselected point with the smallest distance in the current node group and add it to the determined path points.
The minimum value is 2, then add point B to the determined points. at this time:*S={A,B}, U={C,D,E,F} prev={0, 2, 3, 6,∞,∞}* -
Since point B has been determined, it is obvious that the data group prev of point A also needs to be updated. There are several situations here (taking point P as an example):
First, point A cannot be reached but point B can be reached. The latest distance is A–>B–>P, and the distance needs to be accumulated.
Second, both points A and B can be reached, so you need to select the best distance at this time, that is, compare the distances of A–>P and A–>B–>P to select the best path.
Third, point A can reach B but not B. The latest distance is A–>P and there is no need to update. This situation does not need to be considered.
Fourth, points A and B are both inaccessible, so the data does not need to be updated, so this situation does not need to be considered.
The updated data is:*prev={0, 2, 3,6,6,8}* -
Repeat step 2 to add the third point to the determined points. By comparing the undetermined points with the smallest distance between point C and updating the data at the same time, you will have the following data:
S= {A,B,C} U= {D,E,F} prev= {0, 2, 3, 5,6,8}Continue to repeat steps 2 and 3 until there is no data in the array U. At this time, the optimal paths of all points have been calculated. The final array prev is the optimal distance from point A to all points.
prev= {0, 2, 3, 5,6,8} -
At this point, the entire algorithm calculation was completed, but then I found that after I finally got the optimal distance, I could not get the corresponding path, so I needed to store the path at the same time during the calculation of the optimal distance. Here I save the previous point in each node and push it up one by one to get the optimal path for any point.
public class PathNode { public int curIndex; public int lastIndex; public PathNode(int _curIndex, int _lastIndex) { curIndex = _curIndex; lastIndex = _lastIndex; } }
#3. Implementation code
class Program:MonoBehaviour
{
int[,] Metro = new int[6, 6] {
{ 0, 2, 3, 6,2048,2048},
{ 2, 0, 2048,2048,4,6},
{ 3, 2048, 0, 2,2048,2048},
{6, 2048, 2,0,1, 3},
{2048,4,2048, 1,0, 2048},
{2048,6,2048, 3,2048, 0}};
static int row = 6;
List<int> S = new List<int>(row);//S储存确定最短路径的顶点的下标
List<int> U = new List<int>(row);//U中存储尚未确定路径的顶点的下标
private List<PathNode> pathNode = new List<PathNode>();//储存节点的逻辑顺序
int[] prev = new int[row];//用以存储前一个最近顶点的数据
bool[] Isfor = new bool[6] { false, false, false, false, false, false };
public void Start()
{
for (int i = 0; i < row; i++)
{
pathNode.Add(new PathNode(-1,-1));
}
}
private void Update()
{
if (Input.GetKeyDown(KeyCode.P))
{
MyDijkstra(5);
}
}
void MyDijkstra(int _index)
{
int pathValue = 0;
S.Add(_index);
Isfor[_index] = true;
for (int i = 0; i < row; i++)
{
U.Add(i);
}
U.Remove(_index);
//初始化当前节点数据
for (int i = 0; i < row; i++)
{
prev[i] = Metro[_index,i];
}
InitPathNode(pathNode, prev.ToList(), _index);
while (U.Count>0)
{
int m_temp = 2048;
int curIndex = _index;//当前最小值下标
//找到最小值，获取下标
for (int i = 0; i < row; i++)
{
if (prev[i] < m_temp && !Isfor[i])
{
m_temp = prev[i];
curIndex = i;
}
}
pathValue = m_temp;
Isfor[curIndex] = true;
S.Add(curIndex);
U.Remove(curIndex);
//更新当前节点数据信息
for (int i = 0; i < row; i++)
{
if (!Isfor[i])
{
if (prev[i] < 2048 && Metro[curIndex, i] < 2048)
{
if (Metro[curIndex, i] + pathValue < prev[i])
{
prev[i]=Metro[curIndex, i] + pathValue;
pathNode[i] = new PathNode(i, curIndex);
}
}
else if (Metro[curIndex, i] < 2048)
{
prev[i] = Metro[curIndex, i] + pathValue;
pathNode[i]=new PathNode(i,curIndex);
}
}
}
}
//打印节点的最短路径顺序
for (int i = 0; i < row; i++)
{
string path = "";
List<int> indexList=new List<int>();
indexList.Add(i);
GetPathforIndex(pathNode, indexList, i, _index);
for (int j = 0; j < indexList.Count; j++)
{
if (path=="")
{
path = $"{(indexList[j] + 1)}";
}
else
{
path += $"-->{indexList[j] + 1}";
}
}
Debug.LogError($"从点{_index}到点{i + 1}的路径为：{path},距离为:{prev[i]}");
}
}
/// <summary>
/// 初始化节点的连接关系
/// </summary>
/// <param name="_pathNodes"></param>
/// <param name="_list"></param>
/// <param name="_index"></param>
void InitPathNode(List<PathNode> _pathNodes,List<int> _list,int _index)
{
for (int i = 0; i < _list.Count; i++)
{
if (_list[i]<2048)
{
_pathNodes[i]=new PathNode(i,_index);
}
}
}
/// <summary>
/// 根据下标获取节点的路径
/// </summary>
/// <param name="_pathNodes"></param>
/// <param name="_path">存储路径节点</param>
/// <param name="_index"></param>
/// <param name="_startIndex">起点</param>
/// <returns></returns>
List<int> GetPathforIndex(List<PathNode> _pathNodes,List<int> _path ,int _index,int _startIndex)
{
_path.Add(_pathNodes[_index].lastIndex);
if (_pathNodes[_index].lastIndex== _startIndex)
{
return _path;
}
else
{
return GetPathforIndex(_pathNodes, _path, _pathNodes[_index].lastIndex, _startIndex);
}
}
}
public class PathNode
{
public int curIndex;
public int lastIndex;
public PathNode(int _curIndex, int _lastIndex)
{
curIndex = _curIndex;
lastIndex = _lastIndex;
}
}