public void GenerateDisjktraMatrix(NodeType startNode, float infinite)
{
if (mAdjacencyLists.Count == 0)
{
return;
}
mCostMatrix = new float[mAdjacencyLists.Count * mAdjacencyLists.Count];
mLeastNodeArray = new NodeType[mAdjacencyLists.Count];
//mCostMatrixIndices.Clear();
//foreach (var key in mAdjacencyLists.Keys)
//{
// mCostMatrixIndices.Add(key);
//}
//mCostMatrixIndices.Sort((NodeType node1, NodeType node2) => { return mCompareFunc(node1, node2); }); // 由小到大排序
for (int i = 0; i < mAdjacencyLists.Count * mAdjacencyLists.Count; ++i)
{
mCostMatrix[i] = infinite;
}
int adjacentIndex = GetIndexByKey(startNode);
if (adjacentIndex == -1)
{
throw new Exception("-1 null");
}
for (int r = 0; r < mAdjacencyLists.Count; ++r)
{
mCostMatrix[r * mAdjacencyLists.Count + adjacentIndex] = 0.0f;
}
mRootNode = startNode;
int row = 0;
NodeType currentNode = startNode;
Dictionary <NodeType, float> adjacencyList;
float edgeWeight, adjacentNodeWeight, currentNodeWeight = 0.0f;
NodeType adjacentKey;
Dictionary <NodeType, float> openSet = new Dictionary <NodeType, float>();
while (row < mAdjacencyLists.Count - 1)
{
adjacencyList = mAdjacencyLists[currentNode];
foreach (var v in adjacencyList)
{
edgeWeight = v.Value;
adjacentKey = v.Key;
adjacentIndex = GetIndexByKey(adjacentKey);
adjacentNodeWeight = mCostMatrix[row * mAdjacencyLists.Count + adjacentIndex];
if (currentNodeWeight + edgeWeight < adjacentNodeWeight)
{
// Update the weight for the adjacent node
for (int r = row; r < mAdjacencyLists.Count; r++)
{
mCostMatrix[r * mAdjacencyLists.Count + adjacentIndex] = currentNodeWeight + edgeWeight;
}
if (!openSet.ContainsKey(adjacentKey))
{
openSet.Add(adjacentKey, currentNodeWeight + edgeWeight);
}
else
{
openSet[adjacentKey] = currentNodeWeight + edgeWeight;
}
}
}
KeyedPriorityQueue <NodeType, NodeType, float> minHeap = new KeyedPriorityQueue <NodeType, NodeType, float>();
foreach (var open in openSet)
{
minHeap.Enqueue(open.Key, open.Key, open.Value);
}
if (minHeap.Count == 0)
{
isValidPath = true;
break;
}
currentNodeWeight = minHeap.PeekPriority();
mLeastNodeArray[row] = minHeap.Dequeue();
currentNode = mLeastNodeArray[row];
openSet.Remove(currentNode);
row++;
}
isValidPath = true;
}
