A * поиск пути не идет по кратчайшему пути

Моя функция поиска пути A * всегда попадает в назначенное место назначения, но почти всегда уходит немного в сторону. Вот пример:

[Я сделал хорошее изображение, чтобы показать мою проблему, но, очевидно, она не будет опубликована, пока моя репутация не достигнет 10; извини, я новичок :П]

По сути, он тянет влево или вверх как можно больше, фактически не добавляя больше плиток к пути. Это похоже на проблему с вычислением gScore или, возможно, той части, где родительский элемент плитки может быть переназначен на основе gScores соседних плиток, но я просто не могу понять, где происходит ошибка. Я несколько недель прочесывал свой код и просмотрел десятки онлайн-постов, но я все еще застрял. Кстати, компилятор / отладчик, который я должен использовать, не поддерживает точки останова или пошаговую отладку, поэтому я застрял с простым выводом текста. Кто-нибудь может определить, что я делаю не так?

Вот основная функция (Примечание: это все в Angelscript. Она основана на C ++, но есть небольшие различия):

    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;
}

Моя эвристика использует метод Манхэттена:

    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;
}

И, наконец, вот мой путь восстановления функции:

    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;
}

    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;
}
}