A* pathfinding not taking shortest path - c++

Question

My A* pathfinding function always gets to its intended destination, but it almost always goes a bit out of the way. Here's an example:
[I made a nice image to show my issue, but apparently it won't post until my reputation reaches 10; sorry, I'm new. :P]
Essentially, it pulls left or up as much as possible without actually adding more tiles to the path. It sounds like an issue with calculating the gScore or possibly the part where a tile's parent can be reassigned based on neighboring tiles' gScores, but I just can't figure out where it's going wrong. I've combed over my code for weeks and browsed dozens of online posts, but I'm still stuck. Fyi, the compiler/debugger I have to use doesn't support breakpoints or step-through debugging, so I'm stuck with simple text output. Can anyone spot what I'm doing wrong?
Here's the primary function (Note: this is all in Angelscript. It's based on C++, but there are small differences):
int CARDINAL_COST = 10;
int DIAGONAL_COST = 14;
array<vector2> findPath(vector2 startPosition, vector2 endPosition)
{
//Translate the start and end positions into grid coordinates
startPosition = _level.getTileGridPosition(startPosition);
endPosition = _level.getTileGridPosition(endPosition);
//The path to be returned
array<vector2> path(0);
//Create the closed
array<vector2> closedSet(0);
//Create the open set. These are nodes to be considered.
array<vector2> openSet(0);
//Add the startPosition to the open set.
openSet.insertLast(startPosition);
//Create the cameFrom (path) array. Each entry hods that tile's parent tile.
array<array<vector2>> cameFrom;
cameFrom = array<array<vector2>>(_level.width(), array<vector2>(_level.height()));
//Create the gScore array. gScore is the cost to get from the start to the current tile.
array<array<int>> gScore;
gScore = array<array<int>>(_level.width(), array<int>(_level.height()));
//Set the start position score to 0
gScore[startPosition.x][startPosition.y] = 0;
//Create the fScore array. fScore is the gScore + heuristic cost.
array<array<int>> fScore;
fScore = array<array<int>>(_level.width(), array<int>(_level.height()));
//Set the start position score to the estimated (heuristic) cost.
//gScore for start is 0, so that's not included in the equation.
fScore[startPosition.x][startPosition.y] = getHeuristicCost(startPosition, endPosition);
//Required variables
bool searchComplete = false;
vector2 currentTile = startPosition;
int x = 0;
int y = 0;
string tileType = "";
vector2 nextTile(0,0);
vector2 neighborTile(0,0);
int lowestScore = 0;
int tempScore = 0;
int index = 0;
while(!searchComplete)
{
//Find the tile in the openSet with the lowest fScore.
lowestScore = fScore[openSet[0].x][openSet[0].y];
neighborTile = openSet[0];//May not actually be a "neighbor" in this case, just looking for the lowest fScore.
for(int i = 0; i < openSet.length(); i++)
{
if(fScore[neighborTile.x][neighborTile.y] < lowestScore || i == 0)
{
lowestScore = fScore[neighborTile.x][neighborTile.y];
nextTile.x = neighborTile.x;
nextTile.y = neighborTile.y;
}
}
//Drop the "nextTile" from the openSet and add it to the closedSet
index = openSet.find(nextTile);
openSet.removeAt(openSet.find(nextTile));
closedSet.insertLast(nextTile);
//Set the currentTile
currentTile = nextTile;
//Get the fScore for each neighboring tile
for(x = currentTile.x - 1; x <= currentTile.x + 1; x++)
{
for(y = currentTile.y - 1; y <= currentTile.y + 1; y++)
{
//Safety: make sure x and y aren't out of bounds
if(x < 0)
x = 0;
else if(x > _level.width())
x = _level.width();
if(y < 0)
y = 0;
else if (y > _level.height())
y = _level.height();
//Set this x,y pair to be the neighborTile
neighborTile.x = x;
neighborTile.y = y;
//Get the tile type
if(_level.tileArray()[neighborTile.x][neighborTile.y] != null)
tileType = _level.tileArray()[neighborTile.x][neighborTile.y].GetString("type");
else
tileType = "";
//Make sure we aren't looking at the current tile, the tile is not closed, and the tile is a floor or door.
if(neighborTile != currentTile && closedSet.find(neighborTile) == -1 && (tileType == "floor" || tileType == "door"))
{
//If the neighboring tile is already in the open set, check to see if the currentTile's gScore would be less if that tile was its parent.
//If it is, set the it as the currentTile's parent and reset the fScore and gScore for it.
if(openSet.find(neighborTile) != -1)
{
if(gScore[neighborTile.x][neighborTile.y] < gScore[cameFrom[currentTile.x][currentTile.y].x][cameFrom[currentTile.x][currentTile.y].y])
{
cameFrom[currentTile.x][currentTile.y] = neighborTile;
//If the tile is a diagonal move
if(neighborTile.x - currentTile.x != 0 && neighborTile.y - currentTile.y != 0)
gScore[currentTile.x][currentTile.y] = gScore[neighborTile.x][neighborTile.y] + DIAGONAL_COST;
else//If the tile is a cardinal (N,S,E,W) move
gScore[currentTile.x][currentTile.y] = gScore[neighborTile.x][neighborTile.y] + CARDINAL_COST;
fScore[currentTile.x][currentTile.y] = gScore[currentTile.x][currentTile.y] + getHeuristicCost(currentTile, endPosition);
}
}
else//Add this tile to the open set
{
openSet.insertLast(neighborTile);
//Record this tile's parent
cameFrom[neighborTile.x][neighborTile.y] = currentTile;
//If the tile is a diagonal move
if(neighborTile.x - currentTile.x != 0 && neighborTile.y - currentTile.y != 0)
gScore[neighborTile.x][neighborTile.y] = gScore[currentTile.x][currentTile.y] + DIAGONAL_COST;
else//If the tile is a cardinal (N,S,E,W) move
gScore[neighborTile.x][neighborTile.y] = gScore[currentTile.x][currentTile.y] + CARDINAL_COST;
//Get the fScore for this tile
fScore[neighborTile.x][neighborTile.y] = gScore[neighborTile.x][neighborTile.y] + getHeuristicCost(neighborTile, endPosition);
}
}
}
}
//Check to see if we have arrived at the endTile
if(currentTile == endPosition)
{
searchComplete = true;
path = reconstructPath(cameFrom, startPosition, endPosition);
}
else
{
//Check to see if the openSet is empty
if(openSet.length() == 0)
searchComplete = true;
}
}//while(!searchComplete)
return path;
}
My heuristic uses the Manhattan method:
int getHeuristicCost(vector2 startPosition, vector2 endPosition)
{
//Using Manhattan method:
int x = abs(startPosition.x - endPosition.x)*10;
int y = abs(startPosition.y - endPosition.y)*10;
return x+y;
}
And finally, here's my path reconstructing function:
array<vector2> reconstructPath(array<array<vector2>> &in cameFrom, vector2 &in startPosition, vector2 &in endPosition)
{
//Start by adding in the end position
array<vector2> totalPath(1);
vector2 currentTile = endPosition;
totalPath[0] = endPosition;
int x = endPosition.x;
int y = endPosition.y;
int angle = 0;
while(vector2(x, y) != startPosition)
{
currentTile = cameFrom[x][y];
totalPath.insertAt(0,currentTile);
x = currentTile.x;
y = currentTile.y;
}
return totalPath;
}

Answer 1

for(int i = 0; i < openSet.length(); i++)
{
if(fScore[neighborTile.x][neighborTile.y] < lowestScore || i == 0)
{
lowestScore = fScore[neighborTile.x][neighborTile.y];
nextTile.x = neighborTile.x;
nextTile.y = neighborTile.y;
}
}
This loop just looks at neighborTile over and over. Did you mean to go over the elements of openSet?