Struggling with A* Pathfinding in C#

30 Views Asked by At

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

There are 0 best solutions below