So I am trying to program A* pathfinding in a grid based game, and I am following the pseudocode shown on wikipedia's article . However, it never seems to make it to the destination. For my test, I am telling it to go from (-5,5) to (10,5), with blockers at (5,0), (5,3), (5,4), (5,5), (5,6), and (5,7). This is the code as I have it written currently
public void BuildPath(Vector2 start, Vector2 dest)
{
List<Vector2> openList = new List<Vector2>();
List<node> closedList = new List<node>();
Dictionary<Vector2, Vector2> cameFrom = new Dictionary<Vector2, Vector2>();
Dictionary<Vector2, float> fScoreMap = new Dictionary<Vector2, float>();
Dictionary<Vector2, float> gScoreMap = new Dictionary<Vector2, float>();
for (int y = -(mapHeight / 2); y <= (mapHeight / 2); y++)
{
for (int x = -(mapWidth / 2); x <= (mapWidth / 2); x++)
{
cameFrom.Add(new Vector2(x, y), new Vector2(x, y));
fScoreMap.Add(new Vector2(x, y), float.PositiveInfinity);
gScoreMap.Add(new Vector2(x, y), float.PositiveInfinity);
}
}
gScoreMap[start] = 0;
fScoreMap[start] = GetHScore(dest, start);
openList.Add(start);
Vector2 curNode = Vector2.positiveInfinity;
int attemptCount = 0;
while(openList.Count > 0 && attemptCount <= 10000)
{
attemptCount++;
float minFCost = Mathf.Infinity;
foreach (Vector2 n in openList)
{
// TODO: Streamline this
if (fScoreMap[n] < minFCost)
{
curNode = n;
minFCost = fScoreMap[n];
}
}
if(curNode == dest)
{
Debug.Log("DESTINATION REACHED");
// Do send path
return;
}
Debug.Log("Current: " + curNode);
openList.Remove(curNode);
foreach (Vector2 adj in GetAdjacents(curNode))
{
float tenativeGScore = gScoreMap[curNode] + 1; // All weights are the same
if (tenativeGScore < gScoreMap[adj])
{
cameFrom[adj] = curNode;
gScoreMap[adj] = tenativeGScore;
fScoreMap[adj] = tenativeGScore + GetHScore(dest, adj);
if(!openList.Contains(adj))
openList.Add(adj);
}
}
}
Debug.Log("OPEN LIST EMPTY");
}
float GetFCost(Vector2 cur, Vector2 start, float h)
{
return GetGCost(cur,start) + h;
}
float GetGCost(Vector2 cur, Vector2 start)
{
return Mathf.Pow(cur.x - start.x, 1) + Mathf.Pow(cur.y - start.y, 1);
}
float GetHScore(Vector2 dest, Vector2 start)
{
return Mathf.Sqrt(Mathf.Pow(start.x - dest.x, 2) + Mathf.Pow(start.y - dest.y, 2));
}
List<Vector2> GetAdjacents(Vector2 cur)
{
List<Vector2> adjacentNodes = new List<Vector2>();
for(int x = -1; x <= 1; x++)
{
for(int y = -1; y <= 1; y++)
{
if(mapGrid.ContainsKey(new Vector2(cur.x + x, cur.y + y)) &&
!mapGrid[new Vector2(cur.x + x, cur.y + y)].isBlocked &&
(x != 0 && y != 0))
{
adjacentNodes.Add(new Vector2(cur.x + x, cur.y + y));
}
}
}
return adjacentNodes;
}
My class for the node is
public class node
{
public Vector2 pos;
public bool isBlocked;
public node(Vector2 newPos)
{
pos = newPos;
}
}