使用 Dijkstra 算法找到最佳路径的不可预测的结果
Unpredictable results finding optimal path with Dijkstra's Algorithm
我正在尝试实施 Dirjkstra 算法，为我提供表示二维矩阵的加权图中 2 个节点之间的最佳路径。
总结：
如果我对图使用相等的边权重，则最佳路径（据我所知）总是正确返回
如果我在路径中引入一个"wall"，大多数时候搜索结果将是无效的，通常会缺少边缘或无效跳转
有效移动是up/down/left/right
错误示例：
-- matrix nodeIds: --
[ 0][ 1][ 2][ 3][ 4]
[ 5][ 6][ 7][ 8][ 9]
[10][11][12][13][14]
[15][16][17][18][19]
[20][21][22][23][24]
-- matrix node Weights: --
[ 1][99][ 1][ 1][ 1]
[ 1][99][ 1][99][ 1]
[ 1][99][ 1][99][ 1]
[ 1][99][ 1][99][ 1]
[ 1][ 1][ 1][99][ 1]
-- Optimal Path Taken --
[*][ ][*][ ][*]
[*][ ][*][ ][*]
[ ][ ][*][ ][*]
[*][*][*][ ][*]
[ ][*][*][ ][*]
-- Optimal Path String --
-- NodeId->(Weight) --
24->(1)->19->(1)->22->(1)->9->(1)->16->(99)->7->(1)->12->(1)->17->(1)->14->(1)->21->(1)->4->(1)->15->(1)->2->(1)->5->(1)->0->(1)->
-- all paths searched: --
start->end(weight) 0->1(99)
start->end(weight) 5->2(1)
start->end(weight) 15->4(1)
start->end(weight) 0->5(1)
start->end(weight) 5->6(99)
start->end(weight) 12->7(1)
start->end(weight) 15->8(99)
start->end(weight) 16->9(1)
start->end(weight) 2->11(99)
start->end(weight) 17->12(1)
start->end(weight) 12->13(99)
start->end(weight) 21->14(1)
start->end(weight) 2->15(1)
start->end(weight) 7->16(99)
start->end(weight) 14->17(1)
start->end(weight) 17->18(99)
start->end(weight) 22->19(1)
start->end(weight) 4->21(1)
start->end(weight) 9->22(1)
start->end(weight) 14->23(99)
start->end(weight) 19->24(1)
这是我的代码，如果您愿意，您应该可以 copy/paste 和 运行 (C++98):
#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <map>
#include <queue>
const int BOARD_SIZE = 5;
const int NUM_ELEMENTS = BOARD_SIZE * BOARD_SIZE;
int gMatrix[BOARD_SIZE][BOARD_SIZE];
// The Weighted queue always returns the lowest weight value node from front()
class WeightedQueue {
public:
WeightedQueue();
int get(); // return the item with the lowest weight value and remove it from the map
void push(int weight, int NodeId); // add item
bool empty(); // is it empty
private:
std::map<int, int> mWeightedPositions; // weightValue, NodeId
};
WeightedQueue::WeightedQueue()
{
}
void WeightedQueue::push(int weight, int NodeId)
{
mWeightedPositions[weight] = NodeId;
}
bool WeightedQueue::empty()
{
return mWeightedPositions.empty();
}
int WeightedQueue::get()
{
std::map<int, int>::iterator iter = mWeightedPositions.begin();
int nodeId = iter->second; // nodeId
mWeightedPositions.erase(iter);
return nodeId;
}
// Matrix position row,col
struct MatrixPos
{
int row;
int col;
};
// get linear index from row, col
int getLinearIndex(int row, int col)
{
int linearIndex = BOARD_SIZE * col + row;
return linearIndex;
}
// convert linear index to matrix position row, col
MatrixPos matrixPos(int nodeId)
{
MatrixPos matrixPos = { nodeId / BOARD_SIZE, nodeId % BOARD_SIZE };
return matrixPos;
}
// reconstruct the optimal path and print it
void reconstructPath(int start, int goal, std::map<int, int> & cameFrom, std::map<int, int> & weights)
{
std::vector<int> path;
int current = goal;
path.push_back(current);
while (current != start)
{
current = cameFrom[current];
path.push_back(current);
}
printf("\n -- Optimal Path Taken -- \n");
for (int i = 0; i < NUM_ELEMENTS; i++)
{
if (matrixPos(i).col == 0)
{
printf("\n");
}
char tileValue = ' ';
for (int j = 0; j < NUM_ELEMENTS; j++)
{
if (path[j] == i)
{
tileValue = '*';
break;
}
}
printf("[%c]", tileValue);
}
printf("\n -- Optimal Path String -- \n");
printf("\n -- NodeId->(Weight) -- \n");
for (int i = 0; (int) i < path.size(); i++)
{
printf("%d->(%d)->", path[i], weights[path[i]]);
}
printf("\n");
}
// print all the paths taken by the search + the weight of the destination node
void printPaths(std::map<int, int> & pathMap, std::map<int, int> & weightMap)
{
printf("\n -- all paths searched: -- \n");
for (std::map<int, int>::iterator it = pathMap.begin(); it != pathMap.end(); ++it)
{
int destX = matrixPos(it->first).row;
int destY = matrixPos(it->first).col;
int startX = matrixPos(it->second).row;
int startY = matrixPos(it->second).col;
int startWeight = weightMap[it->second];
int endWeight = weightMap[it->first];
printf("start->end(weight) %d->%d(%d) \n", it->second, it->first, endWeight);
}
}
// populate the Matrix and weights map
void buildMatrix(std::map<int, int> & weightMap)
{
for (int i = 0; i < NUM_ELEMENTS; i++)
{
gMatrix[matrixPos(i).row][matrixPos(i).col] = i;
weightMap[i] = 1;
}
weightMap[1] = 99;
weightMap[6] = 99;
weightMap[11] = 99;
weightMap[16] = 99;
//
weightMap[23] = 99;
weightMap[18] = 99;
weightMap[13] = 99;
weightMap[8] = 99;
}
// print matrix displaying nodeIds and Weights
void printMatrix(std::map<int, int> & weightMap)
{
printf("\n -- matrix nodeIds: -- \n");
for (int i = 0; i < NUM_ELEMENTS; i++)
{
if (matrixPos(i).col == 0)
{
printf("\n");
}
printf("[%2d]", gMatrix[matrixPos(i).row][matrixPos(i).col]);
}
printf("\n");
printf("\n -- matrix node Weights: -- \n");
for (int i = 0; i < NUM_ELEMENTS; i++)
{
if (matrixPos(i).col == 0)
{
printf("\n");
}
printf("[%2d]", weightMap[i]);
}
printf("\n");
}
// identify the neighboring nodes for nodeId, up down left right
void collectNeighbors(int nodeId, std::vector<int> & neighbors)
{
int curRow = nodeId / BOARD_SIZE;
int curCol = nodeId % BOARD_SIZE;
// left
if (curRow - 1 > 0)
{
int shiftLeft = curRow - 1;
int neighborIndex = getLinearIndex(shiftLeft, curCol);
neighbors.push_back(neighborIndex);
}
// right
if (curRow + 1 < BOARD_SIZE)
{
int shiftRight = curRow + 1;
int neighborIndex = getLinearIndex(shiftRight, curCol);
neighbors.push_back(neighborIndex);
}
// up
if (curCol - 1 > 0)
{
int shiftUp = curCol - 1;
int neighborIndex = getLinearIndex(curRow, shiftUp);
neighbors.push_back(neighborIndex);
}
// down
if (curCol + 1 < BOARD_SIZE)
{
int shiftDown = curCol + 1;
int neighborIndex = getLinearIndex(curRow, shiftDown);
neighbors.push_back(neighborIndex);
}
}
void searchMatrix(int startNodeId, int goalNodeId, std::map<int, int> & weightMap)
{
std::map<int, int> cameFrom; // neighbor NodeId, current NodeId
std::map<int, int> costSoFar; // nodeId, cost
std::vector<int> neighbors; // list of the neighboring NodeIds
WeightedQueue weightedQueue;
weightedQueue.push(0, startNodeId); // the Queue of nodes, the lowest weight node is returned first by front()
costSoFar[startNodeId] = 0;
while (!weightedQueue.empty())
{
// current index we are working with
int currentNode = weightedQueue.get();
// exit if we've reached the goal node
if (currentNode == goalNodeId)
{
break;
}
// get all the neighbors for this position
neighbors.clear();
collectNeighbors(currentNode, neighbors);
for (int i = 0; i < neighbors.size(); i++)
{
int neighborNode = neighbors[i];
int totalCost = costSoFar[currentNode] + weightMap[neighborNode];
if (!costSoFar.count(neighborNode) || totalCost < costSoFar[neighborNode])
{
// if we haven't been here yet, add it to the weightedQueue
weightedQueue.push(weightMap[neighborNode], neighborNode);
cameFrom[neighborNode] = currentNode;
costSoFar[neighborNode] = totalCost;
}
}
}
printMatrix(weightMap);
reconstructPath(startNodeId, goalNodeId, cameFrom, weightMap);
printPaths(cameFrom, weightMap);
}
int main()
{
std::map<int, int> weightMap;
buildMatrix(weightMap);
searchMatrix(0, 24, weightMap);
}
您的代码段中可能还有其他问题，到目前为止，我注意到您使用的是 std::map，这是用于此目的的不寻常数据结构，因为 std::map 将覆盖之前的 nodeId 与相同的 weight。您可以使用 std::multimap 但您将需要更多 std::iterator 和用于在键值对上循环的东西。使用 std::priority_queue 可以使代码保持精确。
这是一个简单的片段解决方案，其中 std::priority_queue 计算从源节点到目标节点的最短距离。你可以针对你的问题修改或写类似的：
#define Max 20010 // MAX is the maximum nodeID possible
vector <pair<int, int>> adj[Max]; // adj[u] contains the adjacent nodes of node u
int dis[Max]; // dis[i] is the distance from source to node i
priority_queue <pair<int, int>, vector<pair<int, int> >, greater<pair<int, int> > > Q;
int dijkstra(int src, int des) {
memset(dis, MAX, sizeof dis);
Q = priority_queue <pair<int, int>, vector<pair<int, int> >, greater<pair<int, int> > > ();
dis[src] = 0;
Q.push({dis[src] , src});
while(!Q.empty()) {
int u = Q.top().second;
Q.pop();
if(u == des) {
return dis[des];
}
for(int i = 0; i < (int)adj[u].size(); ++i) {
int v = adj[u][i].first;
int cost = adj[u][i].second;
if(dis[v] > dis[u] + cost) {
dis[v] = dis[u] + cost;
Q.push({dis[v], v)});
}
}
}
return -1; // no path found
}
如果这对任何人都有帮助，我能够让它正常工作。有 2 个问题：
正如下面的评论者所指出的，我的 WeightedQueue 没有作为优先队列工作，所以我重新编写了它以在内部使用向量和自定义比较器来重新排序最低权重对象添加新项目时的顶部。 （使用std优先级队列会是一个更明智的选择）
- 我找邻居的功能是怪跳的根源。我重写了它以使其正常工作。
工作代码：
#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <map>
#include <queue>
#include <algorithm>
const int BOARD_SIZE = 5;
const int NUM_ELEMENTS = BOARD_SIZE * BOARD_SIZE;
int gMatrix[BOARD_SIZE][BOARD_SIZE];
struct WeightedNode
{
int nodeId;
int weightValue;
WeightedNode(int weight, int node) : weightValue(weight), nodeId(node)
{
}
};
struct orderLeastWeight
{
inline bool operator()(const WeightedNode& weightedNode1, const WeightedNode& weightedNode2)
{
return (weightedNode1.weightValue < weightedNode2.weightValue);
}
};
// The Weighted queue always returns the lowest weight value node from front()
class WeightedQueue {
public:
WeightedQueue();
int get(); // return the item with the lowest weight value and remove it from the map
void push(int weight, int NodeId); // add item
bool empty(); // is it empty
private:
std::vector <WeightedNode> mNodeVec; // nodeId
};
WeightedQueue::WeightedQueue()
{
}
void WeightedQueue::push(int weightValue, int nodeId)
{
WeightedNode weightedNode = WeightedNode(weightValue, nodeId);
mNodeVec.push_back(weightedNode);
std::sort(mNodeVec.begin(), mNodeVec.end(), orderLeastWeight());
}
bool WeightedQueue::empty()
{
return mNodeVec.empty();
}
int WeightedQueue::get()
{
int nodeId = mNodeVec.begin()->nodeId;
mNodeVec.erase(mNodeVec.begin());
return nodeId;
}
// Matrix position row,col
struct MatrixPos
{
uint row;
uint col;
};
// get linear index from row, col
uint getLinearIndex(uint x, uint y)
{
int linearIndex = BOARD_SIZE * y + x;
return linearIndex;
}
// convert linear index to matrix position row, col
MatrixPos matrixPos(uint nodeId)
{
MatrixPos matrixPos = { nodeId / BOARD_SIZE, nodeId % BOARD_SIZE };
return matrixPos;
}
// reconstruct the optimal path and print it
void reconstructPath(int start, int goal, std::map<int, int> & cameFrom, std::map<int, int> & weights)
{
std::vector<int> path;
int current = goal;
path.push_back(current);
while (current != start)
{
current = cameFrom[current];
path.push_back(current);
}
std::reverse(path.begin(), path.end());
printf("\n -- Optimal Path Taken -- \n");
for (int i = 0; i < NUM_ELEMENTS; i++)
{
if (matrixPos(i).col == 0)
{
printf("\n");
}
char tileValue = ' ';
for (int j = 0; j < NUM_ELEMENTS; j++)
{
if (path[j] == i)
{
tileValue = '*';
break;
}
}
printf("[%c]", tileValue);
}
printf("\n -- Optimal Path String -- \n");
printf("\n -- NodeId->(Weight) -- \n");
for (int i = 0; (int) i < path.size(); i++)
{
printf("%d->(%d)->", path[i], weights[path[i]]);
}
printf("\n");
}
// print all the paths taken by the search + the weight of the destination node
void printPaths(std::map<int, int> & pathMap, std::map<int, int> & weightMap)
{
printf("\n -- all paths searched: -- \n");
for (std::map<int, int>::iterator it = pathMap.begin(); it != pathMap.end(); ++it)
{
int destX = matrixPos(it->first).row;
int destY = matrixPos(it->first).col;
int startX = matrixPos(it->second).row;
int startY = matrixPos(it->second).col;
int startWeight = weightMap[it->second];
int endWeight = weightMap[it->first];
printf("start->end(weight) %d->%d(%d) \n", it->second, it->first, endWeight);
}
}
// populate the Matrix and weights map
void buildMatrix(std::map<int, int> & weightMap)
{
for (int i = 0; i < NUM_ELEMENTS; i++)
{
gMatrix[matrixPos(i).row][matrixPos(i).col] = i;
weightMap[i] = 1;
}
weightMap[1] = 99;
weightMap[6] = 99;
weightMap[11] = 93;
weightMap[16] = 94;
//
weightMap[23] = 95;
weightMap[18] = 96;
weightMap[13] = 97;
weightMap[8] = 98;
}
// print matrix displaying nodeIds and Weights
void printMatrix(std::map<int, int> & weightMap)
{
printf("\n -- matrix nodeIds: -- \n");
for (int i = 0; i < NUM_ELEMENTS; i++)
{
if (matrixPos(i).col == 0)
{
printf("\n");
}
printf("[%2d]", gMatrix[matrixPos(i).row][matrixPos(i).col]);
}
printf("\n");
printf("\n -- matrix node Weights: -- \n");
for (int i = 0; i < NUM_ELEMENTS; i++)
{
if (matrixPos(i).col == 0)
{
printf("\n");
}
printf("[%2d]", weightMap[i]);
}
printf("\n");
}
void collectNeighbors(int nodeId, std::vector<int> & neighbors)
{
// uint getLinearIndex(uint x, uint y)
const MatrixPos tile = matrixPos((uint) nodeId);
const uint x = tile.col;
const uint y = tile.row;
printf("\n -- collectNeighbors: -- nodeId %d y: %d x: %d\n", nodeId, tile.row, tile.col);
// up
if (y > 0) // otherwise an underflow occurred, so not a neighbour
{
uint up = y - 1;
uint neighborIndex = getLinearIndex(x, up);
neighbors.push_back((int) neighborIndex);
printf("up -- neighborIndex: %d y: %d x: %d\n", neighborIndex, y, x);
}
if (y < BOARD_SIZE - 1)
{
uint down = y + 1;
uint neighborIndex = getLinearIndex(x, down);
neighbors.push_back((int) neighborIndex);
printf("down -- neighborIndex: %d y: %d x: %d\n", neighborIndex, y, x);
}
if (x > 0)
{
uint left = x - 1;
uint neighborIndex = getLinearIndex(left, y);
neighbors.push_back((int) neighborIndex);
printf("left -- neighborIndex: %d y: %d x: %d\n", neighborIndex, y, x);
}
if (x < BOARD_SIZE - 1)
{
uint right = x + 1;
uint neighborIndex = getLinearIndex(right, y);
neighbors.push_back((int) neighborIndex);
printf("right -- neighborIndex: %d y: %d x: %d\n", neighborIndex, y, x);
}
}
void searchMatrix(int startNodeId, int goalNodeId, std::map<int, int> & weightMap)
{
std::map<int, int> cameFrom; // neighbor NodeId, current NodeId
std::map<int, int> costSoFar; // nodeId, cost
std::vector<int> neighbors; // list of the neighboring NodeIds
WeightedQueue weightedQueue;
weightedQueue.push(0, startNodeId); // the Queue of nodes, the lowest weight node is returned first by front()
costSoFar[startNodeId] = 0;
cameFrom[startNodeId] = startNodeId;
while (!weightedQueue.empty())
{
// current index we are working with
int currentNode = weightedQueue.get();
// exit if we've reached the goal node
if (currentNode == goalNodeId)
{
break;
}
// get all the neighbors for this position
neighbors.clear();
collectNeighbors(currentNode, neighbors);
for (int i = 0; i < neighbors.size(); i++)
{
int neighborNode = neighbors[i];
int totalCost = costSoFar[currentNode] + weightMap[neighborNode];
if (!costSoFar.count(neighborNode) || totalCost < costSoFar[neighborNode])
{
// if we haven't been here yet, add it to the weightedQueue
weightedQueue.push(weightMap[neighborNode], neighborNode);
cameFrom[neighborNode] = currentNode;
costSoFar[neighborNode] = totalCost;
}
}
}
printMatrix(weightMap);
reconstructPath(startNodeId, goalNodeId, cameFrom, weightMap);
printPaths(cameFrom, weightMap);
}
int main(int argc, char *argv[])
{
// int start = atoi(argv[0]);
// int end = atoi(argv[1]);
std::map<int, int> weightMap;
buildMatrix(weightMap);
searchMatrix(0, 24, weightMap);
}
示例输出：
-- matrix nodeIds: --
[ 0][ 1][ 2][ 3][ 4]
[ 5][ 6][ 7][ 8][ 9]
[10][11][12][13][14]
[15][16][17][18][19]
[20][21][22][23][24]
-- matrix node Weights: --
[ 1][99][ 1][ 1][ 1]
[ 1][99][ 1][98][ 1]
[ 1][93][ 1][97][ 1]
[ 1][94][ 1][96][ 1]
[ 1][ 1][ 1][95][ 1]
-- Optimal Path Taken --
[*][ ][*][*][*]
[*][ ][*][ ][*]
[*][ ][*][ ][*]
[*][ ][*][ ][*]
[*][*][*][ ][*]
-- Optimal Path String --
-- NodeId->(Weight) --
0->(1)->5->(1)->10->(1)->15->(1)->20->(1)->21->(1)->22->(1)->17->(1)->12->(1)->7->(1)->2->(1)->3->(1)->4->(1)->9->(1)->14->(1)->19->(1)->24->(1)->
-- all paths searched: --
start->end(weight) 0->0(1)
start->end(weight) 0->1(99)
start->end(weight) 7->2(1)
start->end(weight) 2->3(1)
start->end(weight) 3->4(1)
start->end(weight) 0->5(1)
start->end(weight) 5->6(99)
start->end(weight) 12->7(1)
start->end(weight) 7->8(98)
start->end(weight) 4->9(1)
start->end(weight) 5->10(1)
start->end(weight) 10->11(93)
start->end(weight) 17->12(1)
start->end(weight) 12->13(97)
start->end(weight) 9->14(1)
start->end(weight) 10->15(1)
start->end(weight) 15->16(94)
start->end(weight) 22->17(1)
start->end(weight) 17->18(96)
start->end(weight) 14->19(1)
start->end(weight) 15->20(1)
start->end(weight) 20->21(1)
start->end(weight) 21->22(1)
start->end(weight) 22->23(95)
start->end(weight) 19->24(1)
我正在尝试实施 Dirjkstra 算法，为我提供表示二维矩阵的加权图中 2 个节点之间的最佳路径。
总结：
如果我对图使用相等的边权重，则最佳路径（据我所知）总是正确返回
如果我在路径中引入一个"wall"，大多数时候搜索结果将是无效的，通常会缺少边缘或无效跳转
有效移动是up/down/left/right
错误示例：
-- matrix nodeIds: --
[ 0][ 1][ 2][ 3][ 4]
[ 5][ 6][ 7][ 8][ 9]
[10][11][12][13][14]
[15][16][17][18][19]
[20][21][22][23][24]
-- matrix node Weights: --
[ 1][99][ 1][ 1][ 1]
[ 1][99][ 1][99][ 1]
[ 1][99][ 1][99][ 1]
[ 1][99][ 1][99][ 1]
[ 1][ 1][ 1][99][ 1]
-- Optimal Path Taken --
[*][ ][*][ ][*]
[*][ ][*][ ][*]
[ ][ ][*][ ][*]
[*][*][*][ ][*]
[ ][*][*][ ][*]
-- Optimal Path String --
-- NodeId->(Weight) --
24->(1)->19->(1)->22->(1)->9->(1)->16->(99)->7->(1)->12->(1)->17->(1)->14->(1)->21->(1)->4->(1)->15->(1)->2->(1)->5->(1)->0->(1)->
-- all paths searched: --
start->end(weight) 0->1(99)
start->end(weight) 5->2(1)
start->end(weight) 15->4(1)
start->end(weight) 0->5(1)
start->end(weight) 5->6(99)
start->end(weight) 12->7(1)
start->end(weight) 15->8(99)
start->end(weight) 16->9(1)
start->end(weight) 2->11(99)
start->end(weight) 17->12(1)
start->end(weight) 12->13(99)
start->end(weight) 21->14(1)
start->end(weight) 2->15(1)
start->end(weight) 7->16(99)
start->end(weight) 14->17(1)
start->end(weight) 17->18(99)
start->end(weight) 22->19(1)
start->end(weight) 4->21(1)
start->end(weight) 9->22(1)
start->end(weight) 14->23(99)
start->end(weight) 19->24(1)
这是我的代码，如果您愿意，您应该可以 copy/paste 和 运行 (C++98):
#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <map>
#include <queue>
const int BOARD_SIZE = 5;
const int NUM_ELEMENTS = BOARD_SIZE * BOARD_SIZE;
int gMatrix[BOARD_SIZE][BOARD_SIZE];
// The Weighted queue always returns the lowest weight value node from front()
class WeightedQueue {
public:
WeightedQueue();
int get(); // return the item with the lowest weight value and remove it from the map
void push(int weight, int NodeId); // add item
bool empty(); // is it empty
private:
std::map<int, int> mWeightedPositions; // weightValue, NodeId
};
WeightedQueue::WeightedQueue()
{
}
void WeightedQueue::push(int weight, int NodeId)
{
mWeightedPositions[weight] = NodeId;
}
bool WeightedQueue::empty()
{
return mWeightedPositions.empty();
}
int WeightedQueue::get()
{
std::map<int, int>::iterator iter = mWeightedPositions.begin();
int nodeId = iter->second; // nodeId
mWeightedPositions.erase(iter);
return nodeId;
}
// Matrix position row,col
struct MatrixPos
{
int row;
int col;
};
// get linear index from row, col
int getLinearIndex(int row, int col)
{
int linearIndex = BOARD_SIZE * col + row;
return linearIndex;
}
// convert linear index to matrix position row, col
MatrixPos matrixPos(int nodeId)
{
MatrixPos matrixPos = { nodeId / BOARD_SIZE, nodeId % BOARD_SIZE };
return matrixPos;
}
// reconstruct the optimal path and print it
void reconstructPath(int start, int goal, std::map<int, int> & cameFrom, std::map<int, int> & weights)
{
std::vector<int> path;
int current = goal;
path.push_back(current);
while (current != start)
{
current = cameFrom[current];
path.push_back(current);
}
printf("\n -- Optimal Path Taken -- \n");
for (int i = 0; i < NUM_ELEMENTS; i++)
{
if (matrixPos(i).col == 0)
{
printf("\n");
}
char tileValue = ' ';
for (int j = 0; j < NUM_ELEMENTS; j++)
{
if (path[j] == i)
{
tileValue = '*';
break;
}
}
printf("[%c]", tileValue);
}
printf("\n -- Optimal Path String -- \n");
printf("\n -- NodeId->(Weight) -- \n");
for (int i = 0; (int) i < path.size(); i++)
{
printf("%d->(%d)->", path[i], weights[path[i]]);
}
printf("\n");
}
// print all the paths taken by the search + the weight of the destination node
void printPaths(std::map<int, int> & pathMap, std::map<int, int> & weightMap)
{
printf("\n -- all paths searched: -- \n");
for (std::map<int, int>::iterator it = pathMap.begin(); it != pathMap.end(); ++it)
{
int destX = matrixPos(it->first).row;
int destY = matrixPos(it->first).col;
int startX = matrixPos(it->second).row;
int startY = matrixPos(it->second).col;
int startWeight = weightMap[it->second];
int endWeight = weightMap[it->first];
printf("start->end(weight) %d->%d(%d) \n", it->second, it->first, endWeight);
}
}
// populate the Matrix and weights map
void buildMatrix(std::map<int, int> & weightMap)
{
for (int i = 0; i < NUM_ELEMENTS; i++)
{
gMatrix[matrixPos(i).row][matrixPos(i).col] = i;
weightMap[i] = 1;
}
weightMap[1] = 99;
weightMap[6] = 99;
weightMap[11] = 99;
weightMap[16] = 99;
//
weightMap[23] = 99;
weightMap[18] = 99;
weightMap[13] = 99;
weightMap[8] = 99;
}
// print matrix displaying nodeIds and Weights
void printMatrix(std::map<int, int> & weightMap)
{
printf("\n -- matrix nodeIds: -- \n");
for (int i = 0; i < NUM_ELEMENTS; i++)
{
if (matrixPos(i).col == 0)
{
printf("\n");
}
printf("[%2d]", gMatrix[matrixPos(i).row][matrixPos(i).col]);
}
printf("\n");
printf("\n -- matrix node Weights: -- \n");
for (int i = 0; i < NUM_ELEMENTS; i++)
{
if (matrixPos(i).col == 0)
{
printf("\n");
}
printf("[%2d]", weightMap[i]);
}
printf("\n");
}
// identify the neighboring nodes for nodeId, up down left right
void collectNeighbors(int nodeId, std::vector<int> & neighbors)
{
int curRow = nodeId / BOARD_SIZE;
int curCol = nodeId % BOARD_SIZE;
// left
if (curRow - 1 > 0)
{
int shiftLeft = curRow - 1;
int neighborIndex = getLinearIndex(shiftLeft, curCol);
neighbors.push_back(neighborIndex);
}
// right
if (curRow + 1 < BOARD_SIZE)
{
int shiftRight = curRow + 1;
int neighborIndex = getLinearIndex(shiftRight, curCol);
neighbors.push_back(neighborIndex);
}
// up
if (curCol - 1 > 0)
{
int shiftUp = curCol - 1;
int neighborIndex = getLinearIndex(curRow, shiftUp);
neighbors.push_back(neighborIndex);
}
// down
if (curCol + 1 < BOARD_SIZE)
{
int shiftDown = curCol + 1;
int neighborIndex = getLinearIndex(curRow, shiftDown);
neighbors.push_back(neighborIndex);
}
}
void searchMatrix(int startNodeId, int goalNodeId, std::map<int, int> & weightMap)
{
std::map<int, int> cameFrom; // neighbor NodeId, current NodeId
std::map<int, int> costSoFar; // nodeId, cost
std::vector<int> neighbors; // list of the neighboring NodeIds
WeightedQueue weightedQueue;
weightedQueue.push(0, startNodeId); // the Queue of nodes, the lowest weight node is returned first by front()
costSoFar[startNodeId] = 0;
while (!weightedQueue.empty())
{
// current index we are working with
int currentNode = weightedQueue.get();
// exit if we've reached the goal node
if (currentNode == goalNodeId)
{
break;
}
// get all the neighbors for this position
neighbors.clear();
collectNeighbors(currentNode, neighbors);
for (int i = 0; i < neighbors.size(); i++)
{
int neighborNode = neighbors[i];
int totalCost = costSoFar[currentNode] + weightMap[neighborNode];
if (!costSoFar.count(neighborNode) || totalCost < costSoFar[neighborNode])
{
// if we haven't been here yet, add it to the weightedQueue
weightedQueue.push(weightMap[neighborNode], neighborNode);
cameFrom[neighborNode] = currentNode;
costSoFar[neighborNode] = totalCost;
}
}
}
printMatrix(weightMap);
reconstructPath(startNodeId, goalNodeId, cameFrom, weightMap);
printPaths(cameFrom, weightMap);
}
int main()
{
std::map<int, int> weightMap;
buildMatrix(weightMap);
searchMatrix(0, 24, weightMap);
}
您的代码段中可能还有其他问题，到目前为止，我注意到您使用的是 std::map，这是用于此目的的不寻常数据结构，因为 std::map 将覆盖之前的 nodeId 与相同的 weight。您可以使用 std::multimap 但您将需要更多 std::iterator 和用于在键值对上循环的东西。使用 std::priority_queue 可以使代码保持精确。
这是一个简单的片段解决方案，其中 std::priority_queue 计算从源节点到目标节点的最短距离。你可以针对你的问题修改或写类似的：
#define Max 20010 // MAX is the maximum nodeID possible
vector <pair<int, int>> adj[Max]; // adj[u] contains the adjacent nodes of node u
int dis[Max]; // dis[i] is the distance from source to node i
priority_queue <pair<int, int>, vector<pair<int, int> >, greater<pair<int, int> > > Q;
int dijkstra(int src, int des) {
memset(dis, MAX, sizeof dis);
Q = priority_queue <pair<int, int>, vector<pair<int, int> >, greater<pair<int, int> > > ();
dis[src] = 0;
Q.push({dis[src] , src});
while(!Q.empty()) {
int u = Q.top().second;
Q.pop();
if(u == des) {
return dis[des];
}
for(int i = 0; i < (int)adj[u].size(); ++i) {
int v = adj[u][i].first;
int cost = adj[u][i].second;
if(dis[v] > dis[u] + cost) {
dis[v] = dis[u] + cost;
Q.push({dis[v], v)});
}
}
}
return -1; // no path found
}
如果这对任何人都有帮助，我能够让它正常工作。有 2 个问题：
正如下面的评论者所指出的，我的 WeightedQueue 没有作为优先队列工作，所以我重新编写了它以在内部使用向量和自定义比较器来重新排序最低权重对象添加新项目时的顶部。 （使用std优先级队列会是一个更明智的选择）
- 我找邻居的功能是怪跳的根源。我重写了它以使其正常工作。
工作代码：
#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <map>
#include <queue>
#include <algorithm>
const int BOARD_SIZE = 5;
const int NUM_ELEMENTS = BOARD_SIZE * BOARD_SIZE;
int gMatrix[BOARD_SIZE][BOARD_SIZE];
struct WeightedNode
{
int nodeId;
int weightValue;
WeightedNode(int weight, int node) : weightValue(weight), nodeId(node)
{
}
};
struct orderLeastWeight
{
inline bool operator()(const WeightedNode& weightedNode1, const WeightedNode& weightedNode2)
{
return (weightedNode1.weightValue < weightedNode2.weightValue);
}
};
// The Weighted queue always returns the lowest weight value node from front()
class WeightedQueue {
public:
WeightedQueue();
int get(); // return the item with the lowest weight value and remove it from the map
void push(int weight, int NodeId); // add item
bool empty(); // is it empty
private:
std::vector <WeightedNode> mNodeVec; // nodeId
};
WeightedQueue::WeightedQueue()
{
}
void WeightedQueue::push(int weightValue, int nodeId)
{
WeightedNode weightedNode = WeightedNode(weightValue, nodeId);
mNodeVec.push_back(weightedNode);
std::sort(mNodeVec.begin(), mNodeVec.end(), orderLeastWeight());
}
bool WeightedQueue::empty()
{
return mNodeVec.empty();
}
int WeightedQueue::get()
{
int nodeId = mNodeVec.begin()->nodeId;
mNodeVec.erase(mNodeVec.begin());
return nodeId;
}
// Matrix position row,col
struct MatrixPos
{
uint row;
uint col;
};
// get linear index from row, col
uint getLinearIndex(uint x, uint y)
{
int linearIndex = BOARD_SIZE * y + x;
return linearIndex;
}
// convert linear index to matrix position row, col
MatrixPos matrixPos(uint nodeId)
{
MatrixPos matrixPos = { nodeId / BOARD_SIZE, nodeId % BOARD_SIZE };
return matrixPos;
}
// reconstruct the optimal path and print it
void reconstructPath(int start, int goal, std::map<int, int> & cameFrom, std::map<int, int> & weights)
{
std::vector<int> path;
int current = goal;
path.push_back(current);
while (current != start)
{
current = cameFrom[current];
path.push_back(current);
}
std::reverse(path.begin(), path.end());
printf("\n -- Optimal Path Taken -- \n");
for (int i = 0; i < NUM_ELEMENTS; i++)
{
if (matrixPos(i).col == 0)
{
printf("\n");
}
char tileValue = ' ';
for (int j = 0; j < NUM_ELEMENTS; j++)
{
if (path[j] == i)
{
tileValue = '*';
break;
}
}
printf("[%c]", tileValue);
}
printf("\n -- Optimal Path String -- \n");
printf("\n -- NodeId->(Weight) -- \n");
for (int i = 0; (int) i < path.size(); i++)
{
printf("%d->(%d)->", path[i], weights[path[i]]);
}
printf("\n");
}
// print all the paths taken by the search + the weight of the destination node
void printPaths(std::map<int, int> & pathMap, std::map<int, int> & weightMap)
{
printf("\n -- all paths searched: -- \n");
for (std::map<int, int>::iterator it = pathMap.begin(); it != pathMap.end(); ++it)
{
int destX = matrixPos(it->first).row;
int destY = matrixPos(it->first).col;
int startX = matrixPos(it->second).row;
int startY = matrixPos(it->second).col;
int startWeight = weightMap[it->second];
int endWeight = weightMap[it->first];
printf("start->end(weight) %d->%d(%d) \n", it->second, it->first, endWeight);
}
}
// populate the Matrix and weights map
void buildMatrix(std::map<int, int> & weightMap)
{
for (int i = 0; i < NUM_ELEMENTS; i++)
{
gMatrix[matrixPos(i).row][matrixPos(i).col] = i;
weightMap[i] = 1;
}
weightMap[1] = 99;
weightMap[6] = 99;
weightMap[11] = 93;
weightMap[16] = 94;
//
weightMap[23] = 95;
weightMap[18] = 96;
weightMap[13] = 97;
weightMap[8] = 98;
}
// print matrix displaying nodeIds and Weights
void printMatrix(std::map<int, int> & weightMap)
{
printf("\n -- matrix nodeIds: -- \n");
for (int i = 0; i < NUM_ELEMENTS; i++)
{
if (matrixPos(i).col == 0)
{
printf("\n");
}
printf("[%2d]", gMatrix[matrixPos(i).row][matrixPos(i).col]);
}
printf("\n");
printf("\n -- matrix node Weights: -- \n");
for (int i = 0; i < NUM_ELEMENTS; i++)
{
if (matrixPos(i).col == 0)
{
printf("\n");
}
printf("[%2d]", weightMap[i]);
}
printf("\n");
}
void collectNeighbors(int nodeId, std::vector<int> & neighbors)
{
// uint getLinearIndex(uint x, uint y)
const MatrixPos tile = matrixPos((uint) nodeId);
const uint x = tile.col;
const uint y = tile.row;
printf("\n -- collectNeighbors: -- nodeId %d y: %d x: %d\n", nodeId, tile.row, tile.col);
// up
if (y > 0) // otherwise an underflow occurred, so not a neighbour
{
uint up = y - 1;
uint neighborIndex = getLinearIndex(x, up);
neighbors.push_back((int) neighborIndex);
printf("up -- neighborIndex: %d y: %d x: %d\n", neighborIndex, y, x);
}
if (y < BOARD_SIZE - 1)
{
uint down = y + 1;
uint neighborIndex = getLinearIndex(x, down);
neighbors.push_back((int) neighborIndex);
printf("down -- neighborIndex: %d y: %d x: %d\n", neighborIndex, y, x);
}
if (x > 0)
{
uint left = x - 1;
uint neighborIndex = getLinearIndex(left, y);
neighbors.push_back((int) neighborIndex);
printf("left -- neighborIndex: %d y: %d x: %d\n", neighborIndex, y, x);
}
if (x < BOARD_SIZE - 1)
{
uint right = x + 1;
uint neighborIndex = getLinearIndex(right, y);
neighbors.push_back((int) neighborIndex);
printf("right -- neighborIndex: %d y: %d x: %d\n", neighborIndex, y, x);
}
}
void searchMatrix(int startNodeId, int goalNodeId, std::map<int, int> & weightMap)
{
std::map<int, int> cameFrom; // neighbor NodeId, current NodeId
std::map<int, int> costSoFar; // nodeId, cost
std::vector<int> neighbors; // list of the neighboring NodeIds
WeightedQueue weightedQueue;
weightedQueue.push(0, startNodeId); // the Queue of nodes, the lowest weight node is returned first by front()
costSoFar[startNodeId] = 0;
cameFrom[startNodeId] = startNodeId;
while (!weightedQueue.empty())
{
// current index we are working with
int currentNode = weightedQueue.get();
// exit if we've reached the goal node
if (currentNode == goalNodeId)
{
break;
}
// get all the neighbors for this position
neighbors.clear();
collectNeighbors(currentNode, neighbors);
for (int i = 0; i < neighbors.size(); i++)
{
int neighborNode = neighbors[i];
int totalCost = costSoFar[currentNode] + weightMap[neighborNode];
if (!costSoFar.count(neighborNode) || totalCost < costSoFar[neighborNode])
{
// if we haven't been here yet, add it to the weightedQueue
weightedQueue.push(weightMap[neighborNode], neighborNode);
cameFrom[neighborNode] = currentNode;
costSoFar[neighborNode] = totalCost;
}
}
}
printMatrix(weightMap);
reconstructPath(startNodeId, goalNodeId, cameFrom, weightMap);
printPaths(cameFrom, weightMap);
}
int main(int argc, char *argv[])
{
// int start = atoi(argv[0]);
// int end = atoi(argv[1]);
std::map<int, int> weightMap;
buildMatrix(weightMap);
searchMatrix(0, 24, weightMap);
}
示例输出：
-- matrix nodeIds: --
[ 0][ 1][ 2][ 3][ 4]
[ 5][ 6][ 7][ 8][ 9]
[10][11][12][13][14]
[15][16][17][18][19]
[20][21][22][23][24]
-- matrix node Weights: --
[ 1][99][ 1][ 1][ 1]
[ 1][99][ 1][98][ 1]
[ 1][93][ 1][97][ 1]
[ 1][94][ 1][96][ 1]
[ 1][ 1][ 1][95][ 1]
-- Optimal Path Taken --
[*][ ][*][*][*]
[*][ ][*][ ][*]
[*][ ][*][ ][*]
[*][ ][*][ ][*]
[*][*][*][ ][*]
-- Optimal Path String --
-- NodeId->(Weight) --
0->(1)->5->(1)->10->(1)->15->(1)->20->(1)->21->(1)->22->(1)->17->(1)->12->(1)->7->(1)->2->(1)->3->(1)->4->(1)->9->(1)->14->(1)->19->(1)->24->(1)->
-- all paths searched: --
start->end(weight) 0->0(1)
start->end(weight) 0->1(99)
start->end(weight) 7->2(1)
start->end(weight) 2->3(1)
start->end(weight) 3->4(1)
start->end(weight) 0->5(1)
start->end(weight) 5->6(99)
start->end(weight) 12->7(1)
start->end(weight) 7->8(98)
start->end(weight) 4->9(1)
start->end(weight) 5->10(1)
start->end(weight) 10->11(93)
start->end(weight) 17->12(1)
start->end(weight) 12->13(97)
start->end(weight) 9->14(1)
start->end(weight) 10->15(1)
start->end(weight) 15->16(94)
start->end(weight) 22->17(1)
start->end(weight) 17->18(96)
start->end(weight) 14->19(1)
start->end(weight) 15->20(1)
start->end(weight) 20->21(1)
start->end(weight) 21->22(1)
start->end(weight) 22->23(95)
start->end(weight) 19->24(1)