Unpredictable results finding optimal path with Dijkstra's Algorithm

Question

I am trying to implement Dirjkstra's Algorithm to give me the optimal path between 2 nodes in a weighted graph representing a 2d matrix.

Summary:

• If I use equal edge weights for the graph, the optimal path has been (as far as I can tell) always been returned correctly

• If I introduce a "wall" within the path, most of the time the search result won't be valid, often with missing edges or invalid jumps

• Valid movements are up/down/left/right

An example of the error:

 -- 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) 

Here is my code, which you should be able to copy/paste and run if you like (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);
}

Answer 1

In case this is helpful to anyone, I was able to get this working correctly. There were 2 issues:

1. As indicated by the commenters below my WeightedQueue was not working as a priority queue, so I re-wrote this to use a vector internally and a custom comparator to re-sort the lowest weight object at the top when a new item was added. (Using the std priority queue would be a wiser choice)

   2. My function to find the neighbors was the root of the strange jumping. I re-wrote this to work correctly.

Working Code:

#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);
}

Example Output:

 -- 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) 

Answer 2

There might be other problems in your snippet, so far I've noticed you're using std::map which is unusual data-structure for this purpose as std::map will overwrite previous nodeId with same weight. You can use std::multimap but you will need some more std::iterator and stuffs for looping on key-value pairs. Use std::priority_queue which will allow you to keep the code precise.

Here is a simple snippet solution with std::priority_queue which calculate shortest distance from source to destination node. You can modify or write similarly for your problems:

#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
}