Here's the code, I put all the structure definitions involved and did some hardcore commenting. The indenting may be weird because of how the forum interprets whatever the hell my text editor is using for spaces.

struct pair_s{ int x; int y; pair_s(int x_, int y_){ x = x_; y = y_; } }; struct node_s{ vector<pair_s> pathToNode; int x,y; float distToDest; float costTravelled; float cumulativeCost; bool hasNodeBeenParsed; node_s(int x_, int y_, float distToDest_, float costTravelled_, vector<pair_s>* prevPath){ x = x_; y = y_; distToDest = distToDest_; costTravelled = costTravelled_; hasNodeBeenParsed = false; cumulativeCost = costTravelled + distToDest; if(prevPath != NULL){ pathToNode = (*prevPath); pathToNode.push_back(pair_s(x,y));//add the current node to path history } } }; void worldgen_c::getPathBetweenPointsAstr(int initial_x, int initial_y, int dest_x, int dest_y, vector<vector<float> >* costMap, vector<pair_s>* retPath, float costMultiplier){ vector<node_s> closedNodes; vector<node_s> openNodes; int gridWidth = costMap->size(); bool** nodesAcctFor = new bool*[gridWidth];//when it comes to checking if a tile has already been added to the node vectors, for(int i=0; i<gridWidth; i++){//it's more efficent to use a 2d array of booleans rather than iterating through a list of coordinates nodesAcctFor[i] = new bool[gridWidth]; } for(int x=0; x<gridWidth; x++){ for(int y=0; y<gridWidth; y++){ nodesAcctFor[x][y]=false; } } //initial node closedNodes.push_back( node_s(initial_x, initial_y, getDist(initial_x, initial_y, dest_x, dest_y), (*costMap)[initial_x][initial_y]*costMultiplier, NULL) ); while(1){ //first we add spots adjacent to closed nodes to the open nodes for(int node=0; node<closedNodes.size(); node++){ if( !closedNodes[node].hasNodeBeenParsed ){ //check to see if closed node in question has already had its adjacent friends added to open nodes for(int x_off=-1; x_off<=1; x_off++){ //checking cardinals and diagonals for(int y_off=-1; y_off<=1; y_off++){ if(x_off==0 && y_off==0)//we dont care about the node we're sitting on continue; if(closedNodes[node].x+x_off>=gridWidth || closedNodes[node].x+x_off<0 || closedNodes[node].y+y_off<0 || closedNodes[node].y+y_off>=gridWidth)//make sure node within b ounds continue; if(nodesAcctFor[closedNodes[node].x+x_off][closedNodes[node].y+y_off])//making sure node isnt already added to one of the tree things continue; //now add the node to the openNodes vector float pathcost=1; if(x_off != 0 && y_off != 0) //if going diagonally, higher pathcost pathcost=1.4; openNodes.push_back(node_s( closedNodes[node].x+x_off, //x val closedNodes[node].y+y_off, //y val getDist(closedNodes[node].x+x_off, closedNodes[node].y+y_off, dest_x, dest_y),//distance to destination closedNodes[node].costTravelled + (*costMap)[closedNodes[node].x+x_off][closedNodes[node].y+y_off]*costMultiplier + pathcost, //heuristic cost &closedNodes[node].pathToNode//path to old node ) ); nodesAcctFor[closedNodes[node].x+x_off][closedNodes[node].y+y_off] = true; } } closedNodes[node].hasNodeBeenParsed = true; } } //now we check each open node and see which one has the lowest cumulative cost int lowestCostIndex=-1; float lowestCostValue = 4294967296; for(int i=0; i<openNodes.size(); i++){ if( openNodes[i].cumulativeCost < lowestCostValue ){ lowestCostValue = openNodes[i].cumulativeCost; lowestCostIndex = i; } } //now convert the open node to a closed node closedNodes.push_back( openNodes[lowestCostIndex] ); openNodes.erase(lowestCostIndex + openNodes.begin() ); if(closedNodes[closedNodes.size()-1].x == dest_x && closedNodes[closedNodes.size()-1].y == dest_y){ //see if we're finally done (*retPath) = closedNodes[closedNodes.size()-1].pathToNode; break; } } for(int i=0; i<gridWidth; i++){//cleanup delete[] nodesAcctFor[i]; } delete[] nodesAcctFor; return; }