A* pathfinding help?!

AzureBoers · January 27, 2020, 6:20pm

I have been wanting to learn how to do pathfinding for awhile and decided to dive it but I need help? When I put everything in to this while look it crashes unity. Any input would be nice and how to improve the current code.

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class AStar : MonoBehaviour
{
    private Grid grid;
   public Node start;
    public Node end;
    public Node cNode;

    // Start is called before the first frame update
    void Start()
    {
        grid = GetComponent<Grid>();
     
    }

    // Update is called once per frame
    void Update()
    {
        Star();
    }

    void Star()
    {
        Node startNode = grid.grid[0, 0];
        Node endNode = grid.grid[4, 5];
        start = startNode;
        end = endNode;

        List<Node> openSet = new List<Node>();
        HashSet<Node> closedSet = new HashSet<Node>();

        openSet.Add(startNode);

        Node currentNode = openSet[0];
        cNode = currentNode;

        while (openSet.Count > 0)
        {
            for (int i = 0; i < openSet.Count; i++)
            {
                if (openSet[i].fCost < currentNode.fCost)
                {
                    currentNode = openSet[i];
                }
            }

            openSet.Remove(currentNode);
            closedSet.Add(currentNode);

            if (currentNode == endNode)
            {
                RetracePath(startNode, endNode);
                return;
            }

            foreach (Node n in currentNode.neibou)
            {
                if (n.walkAble == false || closedSet.Contains(n))
                {
                    continue;
                }
                int newCostToNeighbour = currentNode.gCost + GetDistance(currentNode, n);

                if (newCostToNeighbour < n.gCost || !openSet.Contains(n))
                {
                    n.gCost = newCostToNeighbour;
                    n.hCost = GetDistance(n, endNode);
                    n.parent = currentNode;

                    if (!openSet.Contains(n))
                        openSet.Add(n);
                }
            }
        }
    }

    void RetracePath(Node startNode, Node endNode)
    {
        List<Node> path = new List<Node>();
        Node currentNode = endNode;

        while (currentNode != startNode)
        {
            path.Add(currentNode);
            currentNode = currentNode.parent;
        }
        path.Reverse();

        grid.path = path;

    }

  

    int GetDistance(Node nodeA, Node nodeB)
    {
        int dstX = Mathf.Abs(nodeA.gridX - nodeB.gridX);
        int dstY = Mathf.Abs(nodeA.gridY - nodeB.gridY);

        if (dstX > dstY)
            return 14 * dstY + 10 * (dstX - dstY);
        return 14 * dstX + 10 * (dstY - dstX);
    }
}

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.Tilemaps;


public class Grid : MonoBehaviour
{
  
    public GameObject holder;
    public GameObject nodePrefab;
    public List<Node> nodes = new List<Node>();
    public int X;
    public int Y;
    public Node[,] grid;
    // Start is called before the first frame update
    void Start()
    {

        CreateGrid();
        wall();
        TiletoTile();
       
    }


    private void Update()
    {
        if (grid != null)
        {
            foreach (Node n in grid)
            {
                n.gameObject.GetComponent<SpriteRenderer>().color = (n.walkAble) ? Color.white : Color.red;

                if (path != null)
                    if (path.Contains(n))
                        n.gameObject.GetComponent<SpriteRenderer>().color = Color.black;

            }
        }
    }
    public void CreateGrid()
    {
        grid = new Node[X, Y];
       // grid[X, Y].walkAble = true;
        for (int x = 0; x < X; x++)
        {
            for (int y = 0; y < Y; y++)
            {

                Node newNode = Instantiate(nodePrefab, transform.position, Quaternion.identity).GetComponent<Node>();
                newNode.transform.position = new Vector2(x, y);
                newNode.transform.parent = holder.transform;
                newNode.walkAble = true;
                grid[x, y] = newNode;
                //Vector2 worldPoint = new Vector2(x, y);
                //Node newNode = Instantiate(nodePrefab, transform.position, Quaternion.identity).GetComponent<Node>();
                //newNode.transform.parent = holder.transform;
                //newNode.transform.position = new Vector2(x, y);
                //nodes.Add(newNode);
                //grid[x, y] = new Node(x, y,worldPoint);

            }
        }

       
    }


    void wall()
    {
        grid[3, 2].walkAble = false;
        grid[4, 5].walkAble = false;
    }

    public void Ne(Node node)
    {
        for (int x = 0; x < X; x++)
        {
            for (int y = 0; y < Y; y++)
            {
                if (x - 1 >= 0 && grid[x - 1, y].walkAble)
                {
                    grid[x, y].neibou.Add(grid[x - 1, y]);
                }

                if (x + 1 < Y && grid[x + 1, y].walkAble)
                {
                    grid[x, y].neibou.Add(grid[x + 1, y]);
                }
                if (y - 1 >= 0 && grid[x, y - 1].walkAble)
                {
                    grid[x, y].neibou.Add(grid[x, y - 1]);
                }
                if (y + 1 < X && grid[x, y + 1].walkAble)
                {
                    grid[x, y].neibou.Add(grid[x, y + 1]);
                }
            }
        }
    }

    void TiletoTile()
    {
        for (int x = 0; x < X; x++)
        {
            for (int y = 0; y < Y; y++)
            {
                if (x - 1 >= 0 &&grid[x -1,y].walkAble)
                {
                    grid[x, y].neibou.Add(grid[x - 1, y]);
                }

                if (x + 1 < Y && grid[x +1, y].walkAble)
                {
                    grid[x, y].neibou.Add(grid[x + 1, y]);
                }
                if (y - 1 >= 0 && grid[x, y -1].walkAble)
                {
                    grid[x, y].neibou.Add(grid[x, y - 1]);
                }
                if (y + 1 < X && grid[x, y+1].walkAble)
                {
                    grid[x, y].neibou.Add(grid[x, y + 1]);
                }
            }
        }
    }

    public List<Node> GetNeighbours(Node node)
    {
        List<Node> neightbours = new List<Node>();

        for (int x = -1; x <= 1; x++)
        {
            for (int y = -1; y <=1; y++)
            {
                if(x == 0 && y == 0)
                    continue;
                int checkX = node.gridX + x;
                int checkY = node.gridY + y;

                if(checkX >= 0 && checkX < X && checkY >= 0 && checkY < Y)
                {
                    //node.neibou.Add(grid[checkX, checkY]);
                    neightbours.Add(grid[checkX, checkY]);
                }
            }
        }
        return neightbours;
    }
    public Node NodeFromWorldPoint(Vector3 worldPosition)
    {
        float percentX = (worldPosition.x + X/ 2) / X;
        float percentY = (worldPosition.z + Y / 2) / Y;
        percentX = Mathf.Clamp01(percentX);
        percentY = Mathf.Clamp01(percentY);

        int x = Mathf.RoundToInt((X - 1) * percentX);
        int y = Mathf.RoundToInt((Y - 1) * percentY);
        return grid[x, y];
    }

    public List<Node> path;
    //void OnDrawGizmos()
    //{
    //    Gizmos.DrawWireCube(transform.position, new Vector3(X, 1, Y));

    //    if (grid != null)
    //    {
    //        foreach (Node n in grid)
    //        {
    //            n.gameObject.GetComponent<SpriteRenderer>().color = (n.walkAble) ? Color.white : Color.red;

    //            if (path != null)
    //                if (path.Contains(n))
    //                    n.gameObject.GetComponent<SpriteRenderer>().color = Color.black;
    //           Gizmos.DrawCube(n.worldPosition, Vector3.one * (nodeDiameter - .1f));
    //        }
    //    }
    //}
}

using System.Collections;
using System.Collections.Generic;
using UnityEngine;


public class Node : MonoBehaviour
{
   
    public bool walkAble;
    public Vector2 worldPosition;
    public int gridX;
    public int gridY;

    public int gCost;
    public int hCost;
    public Node parent;
    public List<Node> neibou = new List<Node>();
    public Node(int _gridX,int _gridY/*,Vector2 _worldPos*/)
    {
        //worldPosition = _worldPos;
        gridX = _gridX;
        gridY = _gridY;
    }

    public int fCost
    {
        get
        {
            return gCost + hCost;
        }
    }

  

}

AzureBoers · January 27, 2020, 6:21pm

P.S I am using Sebastian Lague a* for ref but don’t know if that is a good place to start?

spryx · January 28, 2020, 12:40pm

I have the Pro version… and IMHO, it is way too complex for a simple 2d tile-based game in my opinion. I chose to rewrite the A* alg for my own purposes. The alg itself isn’t too hard to re-implement. You will need to adapt this, but:

Pathfinding.cs

using System;
using System.Collections.Generic;
using System.Linq;
using Extensions;
using UnityEngine;

namespace Systems
{
    public static class Pathfinding
    {   //The following resources were used to write this class:
        //SEE https://www.raywenderlich.com/3016-introduction-to-a-pathfinding
        //AND https://medium.com/@nicholas.w.swift/easy-a-star-pathfinding-7e6689c7f7b2

        private class MapSquare : IComparable<MapSquare>
        {
            public readonly Vector2Int Position;

            private int _g;

            public int F => _g + H;

            public int G
            {
                get { return _g; }
                set { _g = value; }
            }

            public int H { get; set; }

            public MapSquare Parent;

            public MapSquare(Vector2Int location, int g, int h)
            {
                Position = location;
                _g = g;
                H = h;
            }

            public int CompareTo(MapSquare other)
            {
                if (F > other.F)
                {
                    return -1;
                }

                if (F == other.F)
                {
                    return 0;
                }

                return 1;
            }

            public bool Equals(MapSquare other)
            {
                return Position.x == other.Position.x && Position.y == other.Position.y;
            }

            public override int GetHashCode()
            {
                return 7 * Position.x + 11 * Position.y;
            }
        }

        public static bool GetPathTo(Vector2Int from, Vector2Int to, out List<Vector2Int> path, bool includeEndpoints = false)
        {
            //Get map maximum values
            Vector2Int mapMaxBounds = MapDataAccess.Service.mapMaxBounds;

            //Declare openList
            List<MapSquare> openList = new List<MapSquare>();

            //Declare closed list
            List<MapSquare> closedList = new List<MapSquare>();

            //The first square will contain the "from" location
            MapSquare firstSquare = new MapSquare(
                from, 0, from.GetDeltaTo(to)
            );

            //Add the original square to the open list
            openList.Add(firstSquare);

            bool pathFound = false;

            do
            {
                //Sort open list
                openList = openList.OrderBy(l => l.F).ToList();

                //Set current square equal to square with the lowest "F" = G + H
                MapSquare currentPathSquare = openList.First();

                //Add current square to the closed list
                closedList.Add(currentPathSquare);

                //Remove current square from open list
                openList.Remove(currentPathSquare);

                //Create a list of neighbor blocks
                List<MapSquare> neighborSquares = new List<MapSquare>();

                //Add open neighboring squares
                for (int delta = -1; delta < 2; delta += 2)
                {
                    Vector2Int dxPosition = new Vector2Int(currentPathSquare.Position.x + delta, currentPathSquare.Position.y);
                    Vector2Int dyPosition = new Vector2Int(currentPathSquare.Position.x, currentPathSquare.Position.y + delta);

                    //Ignore collider at the "to" position
                    if (dxPosition.x > 0 && dxPosition.x < mapMaxBounds.x && (dxPosition == to || !MapDataAccess.Service.IsCellBlocked(dxPosition)))
                    {
                        MapSquare mapSquare = new MapSquare(dxPosition, currentPathSquare.G + 1, dxPosition.GetDeltaTo(to));
                        neighborSquares.Add(mapSquare);
                    }

                    //Ignore collider at the "to" position
                    if (dyPosition.y > 0 && dyPosition.y < mapMaxBounds.y && (dyPosition == to || !MapDataAccess.Service.IsCellBlocked(dyPosition)))
                    {
                        MapSquare mapSquare = new MapSquare(dyPosition, currentPathSquare.G + 1, dyPosition.GetDeltaTo(to));
                        neighborSquares.Add(mapSquare);
                    }
                }

                //If any neighbor squares are the target, don't even worry about adding anything to the open list
                if (neighborSquares.Any(s => s.Position.Equals(to)))
                {
                    MapSquare targetSquare = neighborSquares.Single(s => s.Position.Equals(to));
                    targetSquare.Parent = currentPathSquare;
                    openList.Add(targetSquare);
                    pathFound = true;
                    break;
                }

                //Add/update neighbors in the open list or ignore them if already in the closed list.
                foreach (MapSquare square in neighborSquares)
                {
                    //Ignore any square that is in the closed list
                    if (closedList.Any(s => s.Equals(square)))
                    {
                        continue;
                    }

                    //Try to find matching square in open list
                    MapSquare openMatch = openList.SingleOrDefault(m => m.Equals(square));

                    //Add square that is not in the open list to the open list
                    if (openMatch == null)
                    {
                        //set parent square to current
                        square.Parent = currentPathSquare;
                        openList.Add(square);
                    }
                    else
                    {
                        //Neighbor is already in the open list. Update its G score and parent if it is
                        int nextPathG = currentPathSquare.G + 1;
                        if (nextPathG < openMatch.G)
                        {
                            int matchIndex = openList.IndexOf(openMatch);
                            openList[matchIndex].G = nextPathG;
                            openList[matchIndex].Parent = currentPathSquare;
                        }
                    }

                }

            } while (openList.Count != 0); //If there is nothing in the open list, then the path was not found


            //Closed list vis - For Debugging
            foreach (var n in closedList)
            {
                var offSet = 1f;
                Debug.DrawLine(new Vector3(n.Position.x, n.Position.y, 0f), new Vector3(n.Position.x + offSet, n.Position.y + offSet, 0f), Color.red, 1f);
                Debug.DrawLine(new Vector3(n.Position.x, n.Position.y + offSet, 0f), new Vector3(n.Position.x + offSet, n.Position.y, 0f), Color.red, 1f);
            }
            //End Vis

            path = new List<Vector2Int>();

            if (pathFound)
            {
                MapSquare lastNode = openList.Last();

                while (lastNode != null)
                {
                    path.Add(lastNode.Position);

                    //Open list vis - For Debugging
                    Debug.DrawLine(new Vector3(lastNode.Position.x, lastNode.Position.y, 0f), new Vector3(lastNode.Position.x + 1f, lastNode.Position.y + 1f, 0f), Color.cyan, 1f);
                    Debug.DrawLine(new Vector3(lastNode.Position.x, lastNode.Position.y + 1f, 0f), new Vector3(lastNode.Position.x + 1f, lastNode.Position.y, 0f), Color.cyan, 1f);
                    //End Vis
                  
                    lastNode = lastNode.Parent;
                }

                if (!includeEndpoints)
                {
                    //Remove the "from" node
                    path.RemoveAt(path.Count - 1);

                    //Remove the "to" node
                    path.RemoveAt(0);
                }

                //Because path will be "reversed", reverse it
                path.Reverse();

                //More Vis
                foreach (var pnode in path)
                {
                    Debug.DrawLine(new Vector3(pnode.x, pnode.y, 0f), new Vector3(pnode.x + 1f, pnode.y + 1f, 0f), Color.cyan, 1f);
                    Debug.DrawLine(new Vector3(pnode.x, pnode.y + 1f, 0f), new Vector3(pnode.x + 1f, pnode.y, 0f), Color.cyan, 1f);
                }
                //End Vis

                ////Remove
                //path = path.Except(new List<Vector2Int>{path.First()}).ToList();
            }

            return pathFound;
        }
    }
}

Edit: Probably need this too:
Vector2IntPathfindingExtensions.cs

using System;
using System.Collections.Generic;
using UnityEngine;

namespace Extensions
{
    public static class Vector2IntPathfindingExtensions
    {
        public static int GetDeltaTo(this Vector2Int from, Vector2Int to)
        {
            //Manhattan Dist
            return Math.Abs(from.x - to.x) + Math.Abs(from.y - to.y);
        }

        public static bool FindPathTo(this Vector2Int origin, Vector2Int destination, out List<Vector2Int> path)
        {
            return Systems.Pathfinding.GetPathTo(origin, destination, out path);
        }

        public static bool IsNeighborOf(this Vector2Int origin, Vector2Int cell)
        {
            return (Math.Abs(cell.x - origin.x) == 1 && cell.y - origin.y == 0 || Math.Abs(cell.y - origin.y) == 1 && cell.x - origin.x == 0);
        }
    }
}

Topic		Replies	Views
2D Pathfinding help Unity Engine 2D	2	1266	March 28, 2017
Need help with a bug in pathfinding. Unity Engine Scripting	4	1220	October 10, 2014
A* pathfinding help Unity Engine Scripting	7	1401	August 15, 2017
Having trouble with a pathfinding script Unity Engine Scripting	2	1079	March 16, 2011
Help with pathfinding function! Questions & Answers legacy-topics	2	2877	May 14, 2012

A* pathfinding help?!

Related topics