The Unity Nav Mesh Surface and Nav Mesh Agents were working great, just not for the type of game I wanted to build. The world could be dynamically updated and the agents could adapt, but in a voxel based world where the areas are a single mesh it just didn’t work. I could have attempted to each block type in the world be it’s own mesh and then assign different costs to them (which I might still experiment with), but I thought my old friend the A* algorithm might be a good fit.
Implementing it in 3D was a bit trickier than the previous 2D implementations due to the extra resources needed due there being 26 neighbors for each node instead of 6 or 8 (depending if you can cut corners).
Additionally the the G value in is taking into account the durability of the node. That way the path of least resistance if found. For example in the below image the path is going through wood blocks representing a door on the side of the building instead of trying to go through the stone walls.
In this example a path is being developed a point in a cave.
Now with the cave was closed up, the only way to get there is to go through some of the earth nodes.
The code is actually pretty simple, though I am using some tricks that are using more memory to optimize the speed. The function is also gating the number of iterations with maxAttempts so it doesn’t run away and search the whole world for a path.
During profiling found that I needed two data types for the open set of nodes. One, HashSet<BlockInfo> openSet, to check if the node was already visited which was ever fast doing a .Contains(). Another, PriorityQueue<BlockInfo, float> openQueue, which when a node was inserted it automatically sorted it based on the fCost. This is using extra memory, but dramatically reduced the CPU cost when developing difficult paths.
There is a still a lot that can be done, like maybe using an octree to represent the world and so open spaces are combined into single nodes, but for now it is working an will allow the computer controlled entities to find weakest points in the structures surrounding the target.
private List<BlockInfo> FindPath(BlockInfo start, BlockInfo target)
{
// The set of nodes to be evaluated
HashSet<BlockInfo> openSet = new HashSet<BlockInfo>();
PriorityQueue<BlockInfo, float> openQueue = new PriorityQueue<BlockInfo, float>();
// All nodes initially start off not in the open set (not evaluated)
HashSet<BlockInfo> closedSet = new HashSet<BlockInfo>();
nodes.Add(target.GetHashCode(), target);
openSet.Add(start);
openQueue.Enqueue(start, start.fCost);
int attempts = 0;
while (openQueue.Count > 0)
{
if (attempts++ > maxAttempts)
{
if(debug)Debug.LogWarning($"Pathfinding exceeded max attempts find a path from {start} to {target}!");
return null;
}
// Dequeue the node with the lowest fCost
openQueue.TryDequeue(out BlockInfo current, out _);
openSet.Remove(current);
// If the target is reached, retrace the path and return it
if (current.Equals(target))
{
if (debug) Debug.Log($"Found path in {attempts} attempts");
return RetracePath(start, target);
}
// Move the current node from the open set to the closed set!
closedSet.Add(current);
if (debug)
{
open.Remove(current.GetWorldPosition());
closed.Add(current.GetWorldPosition());
Debug.Log($"current: {current} with fCost of {current.fCost}, attempts: {attempts}, openSet.Count: {openQueue.Count}");
}
List<BlockInfo> neighbors = useDiagonals ? GetNeighborsDiagonals(current) : GetNeighborsNoDiagonals(current);
foreach (var neighborWorld in neighbors)
{
BlockInfo neighbor;
if (!nodes.ContainsKey(neighborWorld.GetHashCode()))
{
neighbor = neighborWorld.Clone();
nodes.Add(neighbor.GetHashCode(), neighbor);
}
else
{
neighbor = nodes[neighborWorld.GetHashCode()];
}
if(closedSet.Contains(neighbor))
{
continue;
}
float distanceToNeighbor = GetDistance(current, neighbor);
float distanceToTarget = GetDistance(neighbor, target);
float newMovementCostToNeighbor = weightDurablityByDistance ?
current.gCost + distanceToNeighbor + neighbor.durability * durabilityModifier * distanceToTarget :
current.gCost + distanceToNeighbor + neighbor.durability * durabilityModifier;
// Check if the path to this neighbor is shorter, or if the neighbor is not in the open set
if (newMovementCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor))
{
// Set the fCost of the neighbor
neighbor.gCost = newMovementCostToNeighbor;
neighbor.hCost = distanceToTarget * hModifier;
neighbor.parent = current;
if (!openSet.Contains(neighbor))
{
openSet.Add(neighbor);
openQueue.Enqueue(neighbor, neighbor.fCost);
if (debug)
{
//Debug.Log($"Adding neighbor to openSet: {neighbor}");
open.Add(neighbor.GetWorldPosition());
}
}
}
}
}
// If we get here, there is no path!
return null;
}