The (1st) problem was that I forgot to reset the least costly square so far...
Having some trouble with a* in c++:
struct Node
{
int parent;
int x;
int y;
int z;
int g; /// Cost to this point
int h; /// Est. cost to target
int f; /// Total Est. Cost
MOV_DIR direction;
Node(int,int,int,int,int,int, MOV_DIR);
};
Node::Node(int parentS, int xS, int yS, int zS, int gS, int hS, MOV_DIR dir)
{
parent = parentS;
x = xS;
y = yS;
z = zS;
g = gS;
h = hS;
f = g + h;
direction = dir;
}
bool AgentKing::aStarVer1(ObjLoc ori, ObjLoc tar, MovementPath& path)
{
bool debug = true;
const int NSEW_COST = 10;
bool done = false;
bool fail = false;
int nodeCount = 0;
vector<Node> node;
int nodesThisRound = 0;
vector<int> open;
vector<int> closed;
node.push_back(Node(-1, ori.x, ori.y, ori.z, 0, 0, MD_DWN));
closed.push_back(nodeCount); nodeCount++;
int nodeToCheck = 0;
int n2cP = ori.p;
int n2cX;
int n2cY;
int n2cZ;
int leastEstCost = 999999999;
bool nodeClosed;
while(done == false && fail == false)
{
///Open all valid adjacent nodes
///First get the current location
if(debug)printf("\n\nNODE BEING CHECKED = %i", nodeToCheck);
if(debug)printf("\nNODE X =%i", node[nodeToCheck].x);
if(debug)printf("\nNODE Y =%i", node[nodeToCheck].y);
if(debug)printf("\nNODE Z =%i", node[nodeToCheck].z);
n2cX = node[nodeToCheck].x;
n2cY = node[nodeToCheck].y;
n2cZ = node[nodeToCheck].z;
if (manhattanCostEst(n2cZ, n2cY, n2cX, tar) == 0)
{
done = true;
if(debug)printf("\nPATHING TARGET REACHED");
}
else
{
///Then check all 6-degrees
if (n2cY > 0 && locationPassable(n2cP, n2cZ, n2cY, n2cX, MD_N)) ///Northern Check
{
node.push_back(Node(nodeToCheck, n2cX, n2cY+1, n2cZ,
node[nodeToCheck].g + NSEW_COST, manhattanCostEst(n2cZ, n2cY+1, n2cX, tar), MD_N));
open.push_back(nodeCount); nodeCount++;
nodesThisRound++;
}
if (n2cY < planetYdimension - 1 && locationPassable(n2cP, n2cZ, n2cY, n2cX, MD_S)) ///Southern Check
{
node.push_back(Node(nodeToCheck, n2cX, n2cY-1, n2cZ,
node[nodeToCheck].g + NSEW_COST, manhattanCostEst(n2cZ, n2cY-1, n2cX, tar), MD_S));
open.push_back(nodeCount); nodeCount++;
nodesThisRound++;
}
if (n2cX > 0 && locationPassable(n2cP, n2cZ, n2cY, n2cX, MD_E)) ///Eastern Check
{
node.push_back(Node(nodeToCheck, n2cX+1, n2cY, n2cZ,
node[nodeToCheck].g + NSEW_COST, manhattanCostEst(n2cZ, n2cY, n2cX+1, tar), MD_E));
open.push_back(nodeCount); nodeCount++;
nodesThisRound++;
}
if (n2cX < planetXdimension - 1 && locationPassable(n2cP, n2cZ, n2cY, n2cX, MD_W)) ///Western Check
{
node.push_back(Node(nodeToCheck, n2cX-1, n2cY, n2cZ,
node[nodeToCheck].g + NSEW_COST, manhattanCostEst(n2cZ, n2cY, n2cX-1, tar), MD_W));
open.push_back(nodeCount); nodeCount++;
nodesThisRound++;
}
if (n2cZ < planetZdimension - 1 && locationPassable(n2cP, n2cZ, n2cY, n2cX, MD_UP)) ///Upper Check
{
node.push_back(Node(nodeToCheck, n2cX, n2cY, n2cZ+1,
node[nodeToCheck].g + NSEW_COST, manhattanCostEst(n2cZ+1, n2cY, n2cX, tar), MD_UP));
open.push_back(nodeCount); nodeCount++;
nodesThisRound++;
}
if (n2cZ > 0 && locationPassable(n2cP, n2cZ, n2cY, n2cX, MD_DWN)) ///Lower Check
{
node.push_back(Node(nodeToCheck, n2cX, n2cY, n2cZ-1,
node[nodeToCheck].g + NSEW_COST, manhattanCostEst(n2cZ-1, n2cY, n2cX, tar), MD_DWN));
open.push_back(nodeCount); nodeCount++;
nodesThisRound++;
}
while(nodesThisRound > 0) /// Find the lowest est cost of the <= 6 nodes opened and register node to check
{
//if(debug)printf("\nNODES THIS ROUND = %i", nodesThisRound);
if (node[nodeCount - nodesThisRound].f < leastEstCost)
{
leastEstCost = node[nodeCount - nodesThisRound].f;
nodeToCheck = nodeCount - nodesThisRound;
}
nodesThisRound--;
}
///find if an opened node picked, if not, pick last new node created
nodeClosed = false;
for(int i = 0; i < closed.size(); i++)
{
if (closed[i] == nodeToCheck)
nodeClosed = true;
}
if (nodeClosed == true)
{
nodeToCheck = open[open.size()-1];
}
///Finally close the open node to be checked
for(int i = 0; i < open.size(); i++)
{
if(open[i] == nodeToCheck)
{
///if(debug)printf("\nNODES OPEN = %i", node.size());
open.erase(open.begin()+i);
closed.push_back(nodeToCheck);
fail = false;
break;
}
else
{
fail = true;
}
}
}
if (done == true && fail == false)
{
if(debug)printf("DONE WITH PATHING, CAPTURING PATH");
while (nodeToCheck != 0 && path.pathLength < 255)
{
nodeCount = 0;
path.direction[nodeCount] = node[nodeToCheck].direction;
path.pathLength++;
nodeToCheck = node[nodeToCheck].parent;
nodeCount++;
}
}
}
return done;
}
I realize this is a massive block of code. For some reason it just keep recycling through a few points very close to origin without venturing outwards(the node checker I mean, it's an infinite loop). It's probably something obvious that I'm just not seeing...