Pathfinding wasn't nearly as painful as I thought it'd be:
struct node
{
unsigned int gScore;
unsigned int fScore;
unsigned int hScore;
unsigned int x;
unsigned int y;
unsigned int cameFromX;
unsigned int cameFromY;
};
int getHeu(int startX,int startY,int finX,int finY)
{
return abs(startX-finX) + abs(startY-finY);
}
bool validPathTile(int actualTileX,int actualTileY)
{
if(actualTileX < 0)
return false;
if(actualTileX >= gridsize)
return false;
if(actualTileY < 0)
return false;
if(actualTileY >= gridsize)
return false;
if(occupied[actualTileX][actualTileY])
return false;
if(treeGrid[actualTileX][actualTileY] > 0)
return false;
if(terrainGrid[actualTileX][actualTileY])
return false;
return true;
}
void unit::newDestination(unsigned int nx,unsigned int ny)
{
if(nx == tileX && ny == tileY)
return;
if(!validPathTile(nx,ny))
return;
if(abs((int)nx-(int)tileX)+abs((int)ny-(int)tileY) > 100)
{
cout<<"Distance of new destination too far.\n";
return;
}
nodes.clear();
animState = 0;
state = 1;
vector <node> closedSet;
vector <node> openSet;
node startingNode;
startingNode.x = tileX;
startingNode.y = tileY;
startingNode.gScore = 0;
startingNode.hScore = getHeu(tileX,tileY,nx,ny);
openSet.push_back(startingNode);
bool acont = true;
while(acont)
{
if(openSet.size() < 1)
{
cout<<"Failed to find path!\n";
acont = false;
break;
}
else
{
unsigned int fValue = 0;
int lowestFValue = -1;
for(unsigned int i = 0; i<openSet.size(); i++)
{
if(openSet[i].x == nx && openSet[i].y == ny)
{
acont = false;
unsigned int tanX = openSet[i].cameFromX;
unsigned int tanY = openSet[i].cameFromY;
point yeh;
yeh.x = openSet[i].x;
yeh.y = openSet[i].y;
nodes.push_back(yeh);
while(tanX != tileX || tanY != tileY)
{
for(unsigned int v = 0; v<closedSet.size(); v++)
{
if(closedSet[v].x == tanX && closedSet[v].y == tanY)
{
tanX = closedSet[v].cameFromX;
tanY = closedSet[v].cameFromY;
yeh.x = tanX;
yeh.y = tanY;
nodes.push_back(yeh);
}
}
}
break;
}
if(lowestFValue == -1 || openSet[i].fScore < fValue)
{
fValue = openSet[i].fScore;
lowestFValue = i;
}
}
int dupFs = 0;
unsigned int hValue = 0;
int lowestHValue = -1;
for(unsigned int l = 0; l<openSet.size(); l++)
{
if(fValue == openSet[l].fScore)
{
if(lowestHValue == -1 || openSet[l].hScore < hValue)
{
hValue = openSet[l].hScore;
lowestHValue = l;
}
dupFs++;
}
}
lowestFValue = lowestHValue;
if(lowestFValue > -1)
{
node current = openSet[lowestFValue];
closedSet.push_back(current);
openSet.erase(openSet.begin() + lowestFValue);
for(int d = 0; d<4; d++)
{
if(validPathTile(current.x+dirX[d],current.y+dirY[d]))
{
int isInClosedSet = -1;
for(unsigned int i = 0; i<closedSet.size(); i++)
if(closedSet[i].x == current.x+dirX[d] && closedSet[i].y == current.y+dirY[d])
isInClosedSet = i;
if(isInClosedSet == -1)
{
node upNode;
upNode.x = current.x+dirX[d];
upNode.y = current.y+dirY[d];
upNode.hScore = getHeu(nx,ny,current.x+dirX[d],current.y+dirY[d]);
upNode.gScore = getHeu(tileX,tileY,current.x+dirX[d],current.y+dirY[d]);
upNode.fScore = upNode.hScore + upNode.gScore;
unsigned int tenGScore = current.gScore + 1;
int isInOpenSet = -1;
for(unsigned int i = 0; i<openSet.size(); i++)
if(openSet[i].x == current.x+dirX[d] && openSet[i].y == current.y+dirX[d])
isInOpenSet = i;
if(tenGScore >= upNode.gScore)
{
upNode.cameFromX = current.x;
upNode.cameFromY = current.y;
}
upNode.gScore = tenGScore;
upNode.fScore = upNode.gScore + getHeu(upNode.x,upNode.y,nx,ny);
if(isInOpenSet == -1)
openSet.push_back(upNode);
else
openSet[isInOpenSet] = upNode;
}
}
}
}
while(SDL_PollEvent(&e))
if(e.key.keysym.sym == SDLK_ESCAPE || e.type == SDL_QUIT)
acont = false;
}
}
reverse(nodes.begin(),nodes.end());
}