Непредсказуемые результаты поиска оптимального пути с алгоритмом Дийкстры

Я пытаюсь реализовать алгоритм Диркстра, чтобы дать мне оптимальный путь между двумя узлами в взвешенном графе, представляющем матрицу 2d.Непредсказуемые результаты поиска оптимального пути с алгоритмом Дийкстры

Резюме:

Если я использую равные края веса для графа, оптимальный путь был (насколько я могу сказать) всегда был возвращен правильно
Если я ввести " стена»в пути, большую часть времени, результат поиска не будет действовать, часто с отсутствующими краями или недействительный подскакивает
Правильные движения вверх/вниз/влево/вправо

Пример ошибки:

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

Вот мой код, который вы должны иметь возможность копировать/вставить и запустить, если вам нравится. (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); 
}

Благодарим за внимание, любые идеи оценили!

Edit - Решение

В случае, если это полезно для всех, я был в состоянии получить эту работу правильно. Были 2 проблемы:

Как указано комментаторам ниже моей WeightedQueue не работал в качестве приоритетной очереди, так что я снова написал это использовать вектор внутри и пользовательский компаратор заново сортировать низкий вес объект вверху, когда был добавлен новый элемент. (Использование очереди приоритетов std было бы более разумным выбором)
1. Моя функция поиска соседей была корнем странного прыжка. Я переписал это, чтобы работать правильно.

Рабочий код:

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

источник

2016-10-16 Hoofamon

Можете ли вы опубликовать ссылку на исходной проблемы, если какой-либо – PRP

Проблема, которую я пытаюсь решить это pathfinding для игры, в которой я работаю – Hoofamon

Кажется, все еще существует проблема с равными весами по фронту, так как 'mWeightedPositions [weight] = Nod eId; 'будет перезаписывать старый край, если край с весом' weight' уже существует. Попробуйте заменить свою 'std :: map ' на 'std :: multimap ' или использовать 'std :: priority_queue' вместо обычного пользовательского компаратора (быстрее). – Bernard

В случае, если это полезно для всех, я был в состоянии получить это работает правильно. Были 2 проблемы:

Как указано комментаторам ниже моей WeightedQueue не работал в качестве приоритетной очереди, так что я снова написал это использовать вектор внутри и пользовательский компаратор заново сортировать низкий вес объект вверху, когда был добавлен новый элемент. (Использование очереди приоритетов std было бы более разумным выбором)
1. Моя функция поиска соседей была корнем странного прыжка. Я переписал это, чтобы работать правильно.

Рабочий код:

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

источник

2016-10-16 18:09:51 Hoofamon

Там могут быть и другие проблемы в вашем фрагменте кода, до сих пор я заметил, что вы используете 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 
}

источник

2016-10-16 04:11:30

Спасибо, это была одна из проблем, которые я в конечном итоге исправил. Я обновил свой пост, чтобы включить окончательное решение, теперь все работает так, как ожидалось. Whew :) – Hoofamon

приветствую :) –

Непредсказуемые результаты поиска оптимального пути с алгоритмом Дийкстры

ответ

Смежные вопросы