# 3D A* Pathfinding Algorithm Help

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.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>();

// 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);

// path found
if (currentNode == targetNode)
{
List<Node> path = new List<Node>();
// retrace the path
while (currentNode != startNode)
{
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))
}
}
}
}

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

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.

• 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. – DMGregory Dec 7 '19 at 14:15
• Alright, I suppose i'll try that. Should i post the project here as well? – Agent Tatsu Dec 7 '19 at 15:10
• am I wrong or do you never set the fCost .. ? – reiti.net Dec 7 '19 at 17:30
• @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 – Agent Tatsu Dec 7 '19 at 18:54
• @DMGregory I updated the post with some more information I gained from debugging today. – Agent Tatsu Dec 7 '19 at 20:44

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.
closedSet.Add(targetNode);

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.