0
\$\begingroup\$

I want to make a 2D turn-based-strategy board game, so I'll need to give my game some pathfinding logic so that units know how to move from place to place. However, I want my levels to have multiple floors, bridges, tunnels, cliffs, etc, so I need a pathfinding algorithm that works in 3D.

Many searches have pointed me the direction of using a navmesh, but since I don't know what my terrain's geometry will be like (I'm in the very beginning stages of development), I don't think that I want to go that route. Instead, I've been following Sebastian Lague's A* Pathfinding tutorial with Unity and slightly modifying it for usage in a 3D space.

I've gotten to the point where the algorithm is supposed to work, but the algorithm is not returning a path. Can someone help me figure out where I've gone wrong?

I know it's a lot, but here is my code:

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

public class aStarPathfinding : MonoBehaviour
{
    public GameObject[] pathTargets;
    public List<Node> path;
    public Vector3 areaSize;
    public static Vector3 nodeSize = Vector3.one;
    Vector3 gridOrigin;

    public class Node
    {
        // position of the node in world space
        public Vector3 worldPosition;
        // is walkable
        public bool walkable = true;
        // parent node during pathfinding
        public Node parent;
        // grid positions
        public int xGrid, yGrid, zGrid;
        // costs
        public int gCost, hCost;
        public int fCost
        {
            get
            {
                return gCost + hCost;
            }
        }

        // find distance between two nodes
        public static int GetDistance(Node start, Node end)
        {
            // calculate deltas in all dimensions
            int dX = Mathf.Abs(start.xGrid - end.xGrid);
            int dY = Mathf.Abs(start.yGrid - end.yGrid);
            int dZ = Mathf.Abs(start.zGrid - end.zGrid);

            // sort the deltas in ascending order
            List<int> moves = new List<int>();
            moves.Add(dX);
            moves.Add(dY);
            moves.Add(dZ);
            moves.Sort((a, b) => a.CompareTo(b));
            // use the list to calculate the g cost
            return 17 * moves[0] + 14 * (moves[1] - moves[0]) + 10 * (moves[2] - moves[1]);
        }

        // constructor
        public Node(Vector3 worldPosition, int xGrid, int yGrid, int zGrid)
        {
            // set the world position and grid positions
            this.worldPosition = worldPosition;
            this.xGrid = xGrid;
            this.yGrid = yGrid;
            this.zGrid = zGrid;
        }
    }

    public Node[,,] grid;

    private void Start()
    {
        CalculateGrid();
    }

    private void Update()
    {
        if (pathTargets.Length == 2 && Input.GetKeyDown(KeyCode.Space))
        {
            CalculatePath(pathTargets[0].transform.position, pathTargets[1].transform.position);
        }
    }

    void CalculateGrid()
    {
        // create grid
        grid = new Node[Mathf.FloorToInt(areaSize.x / nodeSize.x), Mathf.FloorToInt(areaSize.y / nodeSize.y), Mathf.FloorToInt(areaSize.z / nodeSize.z)];

        // calculate grid origin
        gridOrigin = transform.position;
        gridOrigin.x -= grid.GetLength(0) / 2 * nodeSize.x;
        gridOrigin.y -= grid.GetLength(1) / 2 * nodeSize.y;
        gridOrigin.z -= grid.GetLength(2) / 2 * nodeSize.z;
        gridOrigin -= Vector3.up * nodeSize.y;

        // populate grid with nodes
        int x = 0;
        int y = 0;
        int z = 0;

        // iterate through the dimensions
        while (y < grid.GetLength(1))
        {
            while (x < grid.GetLength(0))
            {
                while (z < grid.GetLength(2))
                {
                    // create node
                    Node n = new Node(gridOrigin + new Vector3(x * nodeSize.x, y * nodeSize.y, z * nodeSize.z), x, y, z);
                    if (n.walkable) grid[x, y, z] = n;
                    z++;
                }
                z = 0;
                x++;
            }
            x = 0;
            y++;
        }

    // convert world position to node
    Node GetNodeFromWorldPoint(Vector3 worldPos)
    {
        // determine the point's position on the grid
        Vector3 v = worldPos - gridOrigin;
        int x = Mathf.FloorToInt(v.x / nodeSize.x);
        int y = Mathf.FloorToInt(v.y / nodeSize.y);
        int z = Mathf.FloorToInt(v.z / nodeSize.z);

        // determine whether point is inside the pathfinding area
        if (x < 0 || x >= grid.GetLength(0)) return null;
        if (y < 0 || y >= grid.GetLength(1)) return null;
        if (z < 0 || z >= grid.GetLength(2)) return null;

        // return the new node
        return grid[x, y, z];
    }


    // calculate path given two nodes
    void CalculatePath(Vector3 startPosition, Vector3 endPosition)
    {
        // calculate start and end nodes
        Node startNode = GetNodeFromWorldPoint(startPosition);
        Node targetNode = GetNodeFromWorldPoint(endPosition);
        if (startNode == null) throw new System.ArgumentException("The start position must be within the pathfinding area.");
        if (targetNode == null) throw new System.ArgumentException("The end position must be area.");

        // create open and closed sets
        List<Node> openSet = new List<Node>();
        HashSet<Node> closedSet = new HashSet<Node>();
        openSet.Add(startNode);
        closedSet.Add(targetNode);

        // iterate through the open set
        while (openSet.Count > 0)
        {
            // find the node in the open set with the lowest f cost OR the lowest h cost
            Node currentNode = openSet[0];
            for (int i = 1; i < openSet.Count; i++)
            {
                if (openSet[i].fCost < currentNode.fCost || openSet[i].fCost == currentNode.fCost && openSet[i].hCost < currentNode.hCost)
                    currentNode = openSet[i];
            }

            // remove the current node from the open set
            openSet.Remove(currentNode);
            closedSet.Add(currentNode);

            // path found
            if (currentNode == targetNode)
            {
                List<Node> path = new List<Node>();
                // retrace the path
                while (currentNode != startNode)
                {
                    path.Add(currentNode);
                    currentNode = currentNode.parent;
                    path.Reverse();

                    this.path = path;
                    return;
                }
            }

            // check all neighbors of the current node
            foreach (Node neighbor in GetNeighborsToNode(currentNode))
            {
                // if the neighbor is not walkable or closed
                if (!neighbor.walkable || closedSet.Contains(neighbor)) continue;

                // calculate f cost of neighbor
                int newMovementCostToNeighbor = currentNode.gCost + Node.GetDistance(currentNode, neighbor);
                if (newMovementCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor))
                {
                    neighbor.gCost = newMovementCostToNeighbor;
                    neighbor.hCost = Node.GetDistance(neighbor, targetNode);
                    neighbor.parent = currentNode;

                    // add neighbor to open set
                    if (!openSet.Contains(neighbor))
                        openSet.Add(neighbor);              
                }
            }
        }
    }

    // find all neighbors to the node
    public List<Node> GetNeighborsToNode(Node n)
    {
        // get starting location
        int xStart = n.xGrid;
        int yStart = n.yGrid;
        int zStart = n.zGrid;
        List<Node> neighbors = new List<Node>();

        // search in 3x3x3 block centered on original node
        for (int y = -1; y <= 1; y++)            
        {
            int yCurrent = yStart + y;
            for (int x = -1; x <= 1; x++)
            {
                int xCurrent = xStart + x;
                for (int z = -1; z <= 1; z++)
                {
                    int zCurrent = zStart + z;
                    // skip center block
                    if (x == 0 && y == 0 && z == 0) continue;
                    // skip blocks that would be outside the grid
                    if (xCurrent < 0 || xCurrent >= grid.GetLength(0) || yCurrent < 0 || yCurrent >= grid.GetLength(1) || zCurrent < 0 || zCurrent >= grid.GetLength(2)) continue;
                    // if there is a node at the queried grid location, add it to the list of neighbors
                    Node neighbor = grid[xCurrent, yCurrent, zCurrent];
                    if (neighbor != null) neighbors.Add(neighbor);
                }
            }
        }

        return neighbors;
    }
}

I know that the methods GetNeighborsToNode and GetNodeFromWorldPoint work fine due to some OnDrawGizmos methods that I've ommitted to make the post shorter, but I've included those two methods for completeness and context. I also omitted a Debug.Log that showed me the total number of iterations after the CalculatePath method had no more nodes to check, and no matter where i placed my start and end nodes, the number of iterations was always the same.

Can anyone help me?

UPDATE: So I edited my code so that I can step through it using the space key, and the pathfinding works fine until it reaches the target node, at which point currentNode == targetNode will always return false, even if the two nodes are the same. To check if they are the same instance, I checked the grid coordinates (which matched) and also set up a test to change a dummy property of the Node using the currentNode reference, then checked the targetNode reference to see if it changed, and it did not, which confirms that they are referencing two different instances.

To confirm that there were no other issues with the algorithm, I changed it to compare the coordinates of the current and target nodes rather than comparing the nodes themselves, and it worked perfectly.

\$\endgroup\$
  • \$\begingroup\$ Try putting together a very small test case, one where the goal node is so close to the start that you can comfortably work out on paper what the algorithm should do in every iteration to reach it. Then run your game with a debugger, and step through each iteration of the pathfinding routine. This will let you spot exactly where your implementation diverges from the expected behaviour. Given that, we'll be in a much better position to investigate why. \$\endgroup\$ – DMGregory Dec 7 '19 at 14:15
  • \$\begingroup\$ Alright, I suppose i'll try that. Should i post the project here as well? \$\endgroup\$ – Agent Tatsu Dec 7 '19 at 15:10
  • \$\begingroup\$ am I wrong or do you never set the fCost .. ? \$\endgroup\$ – reiti.net Dec 7 '19 at 17:30
  • \$\begingroup\$ @reiti.net fCost doesn't have a set accessor - it's get only, and it returns gCost+hCost which are both assigned to near the end of the algorithm \$\endgroup\$ – Agent Tatsu Dec 7 '19 at 18:54
  • \$\begingroup\$ @DMGregory I updated the post with some more information I gained from debugging today. \$\endgroup\$ – Agent Tatsu Dec 7 '19 at 20:44
1
\$\begingroup\$

I was stepping through the algorithm using debugs to highlight nodes and color coding them based on what was being queried, and I realized that during the setup of the algorithm, targetNode was being added to the closed set, thus making it so that targetNode will never be currentNode since currentNode is only ever chosen from the open set.

Removing the line...

closedSet.Add(targetNode);

...seems to have solved the issue.

It's strange however that I was still able to reach a node with the same co-ordinates as targetNode without it actually being targetNode. I'll update this answer if I figure that part out.

| improve this answer | |
\$\endgroup\$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.