I've been sitting with this problem the entire day today, and part of the day yesterday, and I can't figure out what I'm doing wrong.
Given a current position and a target position, I'm trying to find a path using the A* algorithm with a Navigation Mesh. The navigation mesh (the green mesh) and available paths (the white lines) can be seen in this screenshot:
http://img841.imageshack.us/img841/412/screenxh.png
The lines are drawn using the same neighbours vector as the one that the A* algorithm uses.
Instead of following the shortest path to the door that can be seen in the distance (if you look really closely you might see a white dot, that's the target), the A* Algorithm just stops because the open list is empty. By the way, I am not checking the exact position of the target, but instead I'm running the A* algorithm on the node that is closest to the goal, and closest to the current position.
Additionally, I'm also aware that the target that I'm using is not correctly found, but I'm getting incorrect output anyway regardless of that.
Here is my code for the A* Algorithm:
openList.clear(); //Empty and prepare the list of unprocessed nodes closedList.clear();//Empty and prepare the list of processed nodes openList.insert(pf_Nodes[ind]); //Add the first node to the open list. g_score[ind] = 0; h_score[ind] = getDistance(start, goal); f_score[ind] = h_score[ind]; //Whilst the open list is empty while(!openList.empty()) { int index; vector3df x = getLowest(&index, f_score); if (x == goal) { //goal has been reached construct_path(actualGoal, came_from, lastNode); printf("Found a path after %i Nodes\n", count); success = true; break; } openList.erase(x); closedList.insert(x); //pf_Adj_Nodes stores the indices of the neighbours that correspond to the //pf_Nodes vector vector3di tmpAdj = pf_Adj_Nodes[index]; adjNodes[0] = tmpAdj.X; adjNodes[1] = tmpAdj.Y; adjNodes[2] = tmpAdj.Z; for (int i = 0; i < 3; i++) { if (adjNodes == -1) //There is no neighbour on this edge continue; vector3df y = pf_Nodes[adjNodes]; float gScore = g_score[index] + getDistance(x,y); if(closedList.find(y) != closedList.end() && gScore < g_score[index]) { printf("Y Was in closed list\n"); came_from[adjNodes] = index; g_score[adjNodes] = gScore; } else if(openList.find(y) != openList.end() && gScore < g_score[index]) { came_from[adjNodes] = index; g_score[adjNodes] = gScore; } else if(closedList.find(y) == closedList.end() && openList.find(y) == openList.end()) { openList.insert(y); g_score[adjNodes] = gScore; } f_score[adjNodes] = gScore + getDistance(x,y); } } my other methods can be seen here
float getDistance(vector3df currentPos, vector3df targetPos){ return 10*(abs(currentPos.X - targetPos.X) + abs(currentPos.Y - targetPos.Y) + abs(currentPos.Z - targetPos.Z));}//An O(n^2) algorithm - Must be replaced. It's horrid. First gotta get it to work though.vector3df getLowest(int* index, float* f_score){ std::set<vector3df>::iterator it = openList.begin(); std::vector<vector3df>::iterator ity; int lowest = 0; for (ity = pf_Nodes.begin(); ity != pf_Nodes.end(); ity++) { if( (*ity) == (*it) ) { lowest = std::distance(pf_Nodes.begin(), ity); break; } } // This gets the first node, and sets it to be the lowest printf("Open List has size: %i\n", (int)openList.size()); for (it++; it != openList.end(); it++) { for (ity = pf_Nodes.begin(); ity != pf_Nodes.end(); ity++) { if( (*ity) == (*it) ) { (*index) = std::distance(pf_Nodes.begin(), ity); break; } } if( f_score[*index] < f_score[lowest] ) lowest = (*index); }//This method tries to find the lowest node in the open list (*index) = lowest; return pf_Nodes[lowest];}My Debugging output can be seen here, which displays the contents of the Open and Closed list after checking one neighbour, and does so for all neighbours.
----------------------------------- ----------------------------------------------Start Pos--------------- -----------Scheduled Goal--------------------------------------------- -----------------------------------[714.666687 368.000000 336.000000] [448.000000 160.000000 128.000000]----------------------------------- ----------------------------------------------Open List--------------- -------------Closed List---------------------------------------------- -----------------------------------[725.333374 368.000000 325.333344] [714.666687 368.000000 336.000000]----------------------------------- ----------------------------------------------Start Pos--------------- -----------Scheduled Goal--------------------------------------------- -----------------------------------[714.666687 368.000000 336.000000] [448.000000 160.000000 128.000000]----------------------------------- ----------------------------------------------Open List--------------- -------------Closed List---------------------------------------------- -----------------------------------[720.000000 368.000000 346.666687] [714.666687 368.000000 336.000000][725.333374 368.000000 325.333344] Open List has size: 2----------------------------------- ----------------------------------------------Start Pos--------------- -----------Scheduled Goal--------------------------------------------- -----------------------------------[714.666687 368.000000 336.000000] [448.000000 160.000000 128.000000]----------------------------------- ----------------------------------------------Open List--------------- -------------Closed List---------------------------------------------- -----------------------------------[725.333374 368.000000 325.333344] [714.666687 368.000000 336.000000] [720.000000 368.000000 346.666687]----------------------------------- ----------------------------------------------Start Pos--------------- -----------Scheduled Goal--------------------------------------------- -----------------------------------[714.666687 368.000000 336.000000] [448.000000 160.000000 128.000000]----------------------------------- ----------------------------------------------Open List--------------- -------------Closed List---------------------------------------------- -----------------------------------[714.666687 368.000000 373.333344] [714.666687 368.000000 336.000000][725.333374 368.000000 325.333344] [720.000000 368.000000 346.666687]Open List has size: 2----------------------------------- ----------------------------------------------Start Pos--------------- -----------Scheduled Goal--------------------------------------------- -----------------------------------[714.666687 368.000000 336.000000] [448.000000 160.000000 128.000000]----------------------------------- ----------------------------------------------Open List--------------- -------------Closed List---------------------------------------------- -----------------------------------[714.666687 368.000000 373.333344] [714.666687 368.000000 336.000000] [720.000000 368.000000 346.666687] [725.333374 368.000000 325.333344]----------------------------------- ----------------------------------------------Start Pos--------------- -----------Scheduled Goal--------------------------------------------- -----------------------------------[714.666687 368.000000 336.000000] [448.000000 160.000000 128.000000]----------------------------------- ----------------------------------------------Open List--------------- -------------Closed List---------------------------------------------- -----------------------------------[714.666687 368.000000 373.333344] [714.666687 368.000000 336.000000][757.333374 368.000000 330.666687] [720.000000 368.000000 346.666687] [725.333374 368.000000 325.333344]Open List has size: 2----------------------------------- ----------------------------------------------Start Pos--------------- -----------Scheduled Goal--------------------------------------------- -----------------------------------[714.666687 368.000000 336.000000] [448.000000 160.000000 128.000000]----------------------------------- ----------------------------------------------Open List--------------- -------------Closed List---------------------------------------------- -----------------------------------[757.333374 368.000000 330.666687] [714.666687 368.000000 336.000000] [714.666687 368.000000 373.333344] [720.000000 368.000000 346.666687] [725.333374 368.000000 325.333344]----------------------------------- ----------------------------------------------Start Pos--------------- -----------Scheduled Goal--------------------------------------------- -----------------------------------[714.666687 368.000000 336.000000] [448.000000 160.000000 128.000000]----------------------------------- ----------------------------------------------Open List--------------- -------------Closed List---------------------------------------------- -----------------------------------[709.333374 368.000000 394.666687] [714.666687 368.000000 336.000000][757.333374 368.000000 330.666687] [714.666687 368.000000 373.333344] [720.000000 368.000000 346.666687] [725.333374 368.000000 325.333344]Open List has size: 2----------------------------------- ----------------------------------------------Start Pos--------------- -----------Scheduled Goal--------------------------------------------- -----------------------------------[714.666687 368.000000 336.000000] [448.000000 160.000000 128.000000]----------------------------------- ----------------------------------------------Open List--------------- -------------Closed List---------------------------------------------- -----------------------------------[709.333374 368.000000 394.666687] [714.666687 368.000000 336.000000][778.666687 368.000000 325.333344] [714.666687 368.000000 373.333344] [720.000000 368.000000 346.666687] [725.333374 368.000000 325.333344] [757.333374 368.000000 330.666687]----------------------------------- ----------------------------------------------Start Pos--------------- -----------Scheduled Goal--------------------------------------------- -----------------------------------[714.666687 368.000000 336.000000] [448.000000 160.000000 128.000000]----------------------------------- ----------------------------------------------Open List--------------- -------------Closed List---------------------------------------------- -----------------------------------[709.333374 368.000000 394.666687] [714.666687 368.000000 336.000000][778.666687 368.000000 325.333344] [714.666687 368.000000 373.333344] [720.000000 368.000000 346.666687] [725.333374 368.000000 325.333344] [757.333374 368.000000 330.666687]Open List has size: 2----------------------------------- ----------------------------------------------Start Pos--------------- -----------Scheduled Goal--------------------------------------------- -----------------------------------[714.666687 368.000000 336.000000] [448.000000 160.000000 128.000000]----------------------------------- ----------------------------------------------Open List--------------- -------------Closed List---------------------------------------------- -----------------------------------[778.666687 368.000000 325.333344] [709.333374 368.000000 394.666687] [714.666687 368.000000 336.000000] [714.666687 368.000000 373.333344] [720.000000 368.000000 346.666687] [725.333374 368.000000 325.333344] [757.333374 368.000000 330.666687]Open List has size: 1----------------------------------- ----------------------------------------------Start Pos--------------- -----------Scheduled Goal--------------------------------------------- -----------------------------------[714.666687 368.000000 336.000000] [448.000000 160.000000 128.000000]----------------------------------- ----------------------------------------------Open List--------------- -------------Closed List---------------------------------------------- ----------------------------------- [709.333374 368.000000 394.666687] [714.666687 368.000000 336.000000] [714.666687 368.000000 373.333344] [720.000000 368.000000 346.666687] [725.333374 368.000000 325.333344] [757.333374 368.000000 330.666687] [778.666687 368.000000 325.333344]cannot open ../src/AI/Aggressive/scripts/Aggressive_FindEnemy_Exit.lua: No such file or directoryGetting Shortest ToLowest Index: 291Getting Shortest ToLowest Index: 166Open List has size: 1----------------------------------- ----------------------------------------------Start Pos--------------- -----------Scheduled Goal--------------------------------------------- -----------------------------------[714.666687 368.000000 336.000000] [448.000000 160.000000 128.000000]----------------------------------- ----------------------------------------------Open List--------------- -------------Closed List---------------------------------------------- -----------------------------------[725.333374 368.000000 325.333344] [714.666687 368.000000 336.000000]----------------------------------- ----------------------------------------------Start Pos--------------- -----------Scheduled Goal--------------------------------------------- -----------------------------------[714.666687 368.000000 336.000000] [448.000000 160.000000 128.000000]----------------------------------- ----------------------------------------------Open List--------------- -------------Closed List---------------------------------------------- -----------------------------------[720.000000 368.000000 346.666687] [714.666687 368.000000 336.000000][725.333374 368.000000 325.333344] Open List has size: 2----------------------------------- ----------------------------------------------Start Pos--------------- -----------Scheduled Goal--------------------------------------------- -----------------------------------[714.666687 368.000000 336.000000] [448.000000 160.000000 128.000000]----------------------------------- ----------------------------------------------Open List--------------- -------------Closed List---------------------------------------------- -----------------------------------[725.333374 368.000000 325.333344] [714.666687 368.000000 336.000000] [720.000000 368.000000 346.666687]----------------------------------- ----------------------------------------------Start Pos--------------- -----------Scheduled Goal--------------------------------------------- -----------------------------------[714.666687 368.000000 336.000000] [448.000000 160.000000 128.000000]----------------------------------- ----------------------------------------------Open List--------------- -------------Closed List---------------------------------------------- -----------------------------------[714.666687 368.000000 373.333344] [714.666687 368.000000 336.000000][725.333374 368.000000 325.333344] [720.000000 368.000000 346.666687]Open List has size: 2----------------------------------- ----------------------------------------------Start Pos--------------- -----------Scheduled Goal--------------------------------------------- -----------------------------------[714.666687 368.000000 336.000000] [448.000000 160.000000 128.000000]----------------------------------- ----------------------------------------------Open List--------------- -------------Closed List---------------------------------------------- -----------------------------------[714.666687 368.000000 373.333344] [714.666687 368.000000 336.000000] [720.000000 368.000000 346.666687] [725.333374 368.000000 325.333344]----------------------------------- ----------------------------------------------Start Pos--------------- -----------Scheduled Goal--------------------------------------------- -----------------------------------[714.666687 368.000000 336.000000] [448.000000 160.000000 128.000000]----------------------------------- ----------------------------------------------Open List--------------- -------------Closed List---------------------------------------------- -----------------------------------[714.666687 368.000000 373.333344] [714.666687 368.000000 336.000000][757.333374 368.000000 330.666687] [720.000000 368.000000 346.666687] [725.333374 368.000000 325.333344]Open List has size: 2----------------------------------- ----------------------------------------------Start Pos--------------- -----------Scheduled Goal--------------------------------------------- -----------------------------------[714.666687 368.000000 336.000000] [448.000000 160.000000 128.000000]----------------------------------- ----------------------------------------------Open List--------------- -------------Closed List---------------------------------------------- -----------------------------------[757.333374 368.000000 330.666687] [714.666687 368.000000 336.000000] [714.666687 368.000000 373.333344] [720.000000 368.000000 346.666687] [725.333374 368.000000 325.333344]----------------------------------- ----------------------------------------------Start Pos--------------- -----------Scheduled Goal--------------------------------------------- -----------------------------------[714.666687 368.000000 336.000000] [448.000000 160.000000 128.000000]----------------------------------- ----------------------------------------------Open List--------------- -------------Closed List---------------------------------------------- -----------------------------------[709.333374 368.000000 394.666687] [714.666687 368.000000 336.000000][757.333374 368.000000 330.666687] [714.666687 368.000000 373.333344] [720.000000 368.000000 346.666687] [725.333374 368.000000 325.333344]Open List has size: 2----------------------------------- ----------------------------------------------Start Pos--------------- -----------Scheduled Goal--------------------------------------------- -----------------------------------[714.666687 368.000000 336.000000] [448.000000 160.000000 128.000000]----------------------------------- ----------------------------------------------Open List--------------- -------------Closed List---------------------------------------------- -----------------------------------[709.333374 368.000000 394.666687] [714.666687 368.000000 336.000000][778.666687 368.000000 325.333344] [714.666687 368.000000 373.333344] [720.000000 368.000000 346.666687] [725.333374 368.000000 325.333344] [757.333374 368.000000 330.666687]----------------------------------- ----------------------------------------------Start Pos--------------- -----------Scheduled Goal--------------------------------------------- -----------------------------------[714.666687 368.000000 336.000000] [448.000000 160.000000 128.000000]----------------------------------- ----------------------------------------------Open List--------------- -------------Closed List---------------------------------------------- -----------------------------------[709.333374 368.000000 394.666687] [714.666687 368.000000 336.000000][778.666687 368.000000 325.333344] [714.666687 368.000000 373.333344] [720.000000 368.000000 346.666687] [725.333374 368.000000 325.333344] [757.333374 368.000000 330.666687]Open List has size: 2----------------------------------- ----------------------------------------------Start Pos--------------- -----------Scheduled Goal--------------------------------------------- -----------------------------------[714.666687 368.000000 336.000000] [448.000000 160.000000 128.000000]----------------------------------- ----------------------------------------------Open List--------------- -------------Closed List---------------------------------------------- -----------------------------------[778.666687 368.000000 325.333344] [709.333374 368.000000 394.666687] [714.666687 368.000000 336.000000] [714.666687 368.000000 373.333344] [720.000000 368.000000 346.666687] [725.333374 368.000000 325.333344] [757.333374 368.000000 330.666687]Open List has size: 1----------------------------------- ----------------------------------------------Start Pos--------------- -----------Scheduled Goal--------------------------------------------- -----------------------------------[714.666687 368.000000 336.000000] [448.000000 160.000000 128.000000]----------------------------------- ----------------------------------------------Open List--------------- -------------Closed List---------------------------------------------- ----------------------------------- [709.333374 368.000000 394.666687] [714.666687 368.000000 336.000000] [714.666687 368.000000 373.333344] [720.000000 368.000000 346.666687] [725.333374 368.000000 325.333344] [757.333374 368.000000 330.666687] [778.666687 368.000000 325.333344][Edited by - sjaakiejj on December 15, 2010 7:19:30 PM]
