The package is designed to provide a basic implementation of the A Star Path Finding algorithm. The goal of this project is to create a fully functional path finding system from scratch, in order to gain a thorough understanding of the algorithm's fundamental logic and to attempt to enhance it as needed. The package is primarily aimed at educational purposes and offers both a straightforward A Star Path Finding class and an optimized version that employs Unity's job system for better performance.
This package provides a useful grid graph structure for pathfinding. The GridData.cs file is a generic 3D grid structure that contains nodes implementing the IGridCoord interface, making it highly flexible and reusable. This approach allows any class or entity implementing the IGridCoord interface to store data of any type, making it easy to access specific node types and define logical interactions between nodes. This design makes sense from both a functional and a practical standpoint, as it provides a versatile and adaptable tool for various use cases.
using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
namespace MugCup_PathFinder.Runtime
{
public class GridData<T>: MonoBehaviour where T : IGridCoord
{
public IEnumerable<T> ValidNodes
{
get
{
foreach (var _node in GridNodes)
{
if (_node != null)
yield return _node;
}
}
}
public T[] GridNodes;
public Vector3Int GridSize ;
public int RowUnit ;
public int ColumnUnit;
public int LevelUnit ;
public GridData<T> SetGridSize(Vector3Int _gridSize)
{
GridSize = _gridSize;
return this;
}
public GridData<T> InitializeGridUnitSize(Vector3Int _gridSize)
{
GridSize = _gridSize;
return this;
}
public virtual void Initialized()
{
throw new NotImplementedException();
}
public GridData<T> InitializeGridArray()
{
int _rowUnit = GridSize.x;
int _columnUnit = GridSize.z;
int _levelUnit = GridSize.y;
GridNodes = new T[_rowUnit * _columnUnit * _levelUnit];
for (int _y = 0; _y < _levelUnit ; _y++)
for (int _x = 0; _x < _rowUnit ; _x++)
for (int _z = 0; _z < _columnUnit; _z++)
GridNodes[_z + GridSize.x * (_x + GridSize.y * _y)] = default(T);
return this;
}
public GridData<T> PopulateNodesByLevel(GameObject _prefab, int _level)
{
GridDataBuilder.PopulateNodesByLevel(GridNodes, GridSize, _level, _prefab);
return this;
}
public TU[] GetGridUnitArray<TU>() where TU : GridNode
{
var _gridUnitArray = new TU[GridNodes.Length];
for (var _i = 0; _i < _gridUnitArray.Length; _i++)
{
_gridUnitArray[_i] = GridNodes[_i] as TU;
}
return _gridUnitArray;
}
public IEnumerable<TU> AvailableNodes<TU>() where TU : class, INode
{
foreach (var _node in GridNodes)
{
if(_node == null) continue;
if (_node is TU _block)
yield return _block;
}
}
public void ApplyAllNode(Action<T> _action)
{
foreach (var _node in GridNodes)
{
if(_node == null) continue;
_action?.Invoke(_node);
}
}
public void ApplyAllNodes<TU>(Action<TU> _action) where TU : class, INode
{
foreach (var _node in GridNodes)
{
if(_node == null) continue;
if(_node is TU _castNode)
_action?.Invoke(_castNode);
}
}
public bool TryGetNode(Vector3Int _nodePos, out T _node)
{
_node = GetNode(_nodePos);
return _node != null;
}
public T GetNode(Vector3Int _nodePos)
{
return GridUtility.GetNode(_nodePos, GridSize, GridNodes);
}
public TU GetNode<TU>(Vector3Int _nodePos) where TU : class, INode
{
return GridUtility.GetNode(_nodePos, GridSize, GridNodes) as TU;
}
#region Get Node Via Direction
public T GetNodeForward(Vector3Int _nodePos)
{
return GridUtility.GetNodeForward(_nodePos, GridSize, GridNodes);
}
public T GetNodeRight(Vector3Int _nodePos)
{
return GridUtility.GetNodeRight(_nodePos, GridSize, GridNodes);
}
public T GetNodeBack(Vector3Int _nodePos)
{
return GridUtility.GetNodeBack(_nodePos, GridSize, GridNodes);
}
public T GetNodeLeft(Vector3Int _nodePos)
{
return GridUtility.GetNodeLeft(_nodePos, GridSize, GridNodes);
}
public T GetNodeUp(Vector3Int _nodePos)
{
return GridUtility.GetNodeUp(_nodePos, GridSize, GridNodes);
}
public T GetNodeDown(Vector3Int _nodePos)
{
return GridUtility.GetNodeDown(_nodePos, GridSize, GridNodes);
}
#endregion
public void AddNode(T _newNode, Vector3Int _nodePos)
{
GridUtility.AddNode(_newNode, _nodePos, GridSize, ref GridNodes);
}
public void RemoveNode(Vector3Int _nodePos)
{
GridUtility.RemoveNode(_nodePos, GridSize, ref GridNodes);
}
public void RemoveNode<TU>(Vector3Int _nodePos) where TU : class, INode
{
GridUtility.RemoveNode(_nodePos, GridSize, ref GridNodes);
}
public T[] GetAllNodeBasesAtLevel(int _gridLevel)
{
int _rowUnit = GridSize.x;
int _columnUnit = GridSize.z;
int _heightUnit = GridSize.y;
var _nodesAtLevel = new T[_rowUnit * _columnUnit];
for (var _x = 0; _x < _rowUnit ; _x++)
for (var _z = 0; _z < _columnUnit; _z++)
_nodesAtLevel[_z + _rowUnit * _x] = GridNodes[_z + _rowUnit * (_x + _heightUnit * _gridLevel)];
return _nodesAtLevel;
}
public TU[] GetAllNodeBasesAtLevel<TU>(int _gridLevel) where TU : class, INode
{
int _rowUnit = GridSize.x;
int _columnUnit = GridSize.z;
int _heightUnit = GridSize.y;
var _nodesAtLevel = new TU[_rowUnit * _columnUnit];
for (var _x = 0; _x < _rowUnit ; _x++)
for (var _z = 0; _z < _columnUnit; _z++)
_nodesAtLevel[_z + _rowUnit * _x] = GridNodes[_z + _rowUnit * (_x + _heightUnit * _gridLevel)] as TU;
return _nodesAtLevel;
}
public void EmptyGrid()
{
for (var _i = 0; _i < GridNodes.Length; _i++)
{
GridNodes[_i] = default;
}
}
public virtual void ClearData()
{
GridNodes = null;
}
}
}
The package contains a simple pathfinder that utilizes the AStar algorithm with the Grid data structure (GridData.cs). To use this utility, you will need to implement the INode interface on the node you want to add to the path list. The INode interface also implements IGridCoord, ensuring seamless cooperation with the Grid data structure. The example class provided below is a utility static class that is implemented with no async or parallelism. While this may cause a bottleneck when calculating paths for multiple agents, the class is compact and straightforward, making it ideal for prototyping.
using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using UnityEngine;
namespace MugCup_PathFinder.Runtime
{
public static class AStarPathFinder<T> where T: class, INode
{
public static Vector3Int GridSize;
public static T[] Nodes;
public static T Origin;
public static T Target;
public static void InitializeGridData(Vector3Int _gridSize, T[] _nodes)
{
GridSize = _gridSize;
Nodes = _nodes;
}
public static IEnumerable<T> FindPath(T _origin, T _target)
{
List<T> _openSet = new List<T>();
HashSet<T> _closedSet = new HashSet<T>();
_openSet.Add(_origin);
int _iter = 0;
while (_openSet.Count > 0 && _iter < 50)
{
_iter++;
T _node = GetNodeLeastCost(_openSet);
_openSet.Remove(_node);
_closedSet.Add(_node);
if (_node == _target)
return RetracePath(_origin, _target);
List<T> _adjacentNodes = GridUtility.GetAdjacentNodes8Dir(_node, GridSize, Nodes).ToList();
foreach (T _adjacent in _adjacentNodes)
{
if(_closedSet.Contains(_adjacent) || _adjacent == null)
continue;
int _newCostToAdjacent = _node.G_Cost + GetDistance(_node, _adjacent);
if (_newCostToAdjacent < _adjacent.G_Cost || !_openSet.Contains(_adjacent))
{
_adjacent.G_Cost = _newCostToAdjacent;
_adjacent.H_Cost = GetDistance(_adjacent, _target);
_adjacent.NodeParent = _node;
if(!_openSet.Contains(_adjacent))
_openSet.Add(_adjacent);
}
}
}
return null;
}
public static T GetNodeLeastCost(List<T> _openSet)
{
T _node = _openSet[0];
for (var _i = 1; _i < _openSet.Count; _i++)
{
if (_openSet[_i].F_Cost <= _node.F_Cost)
{
if (_openSet[_i].H_Cost < _node.H_Cost)
_node = _openSet[_i];
}
}
return _node;
}
public static IEnumerable<T> RetracePath(T _origin, T _target)
{
List<T> _path = new List<T>();
T _currentNode = _target;
while (_currentNode != _origin)
{
_path.Add(_currentNode);
_currentNode = _currentNode?.NodeParent as T;
}
_path.Reverse();
return _path;
}
public static void SetNodePathData(List<T> _path)
{
for (var _i = 0; _i < _path.Count - 1; _i++)
{
var _currentNode = _path[_i];
var _nextNode = _path[_i + 1];
_currentNode.SetNextNodeOnPath(_nextNode);
var _currentPos = _currentNode.NodeGridPosition;
var _nextPos = _nextNode .NodeGridPosition;
_currentNode.NextNodePosition = _nextPos;
NodeDirection _direction = _currentNode.GetDirectionTo(_nextNode);
_currentNode.SetNodePathDirection(_direction);
if (_i + 1 == _path.Count - 1)
_nextNode.SetNodePathDirection(_direction);
}
}
public static int GetDistance(T _nodeA, T _nodeB)
{
int _distanceX = Math.Abs(_nodeA.NodeGridPosition.x - _nodeB.NodeGridPosition.x);
int _distanceY = Math.Abs(_nodeA.NodeGridPosition.y - _nodeB.NodeGridPosition.y);
if (_distanceX > _distanceY)
return 14 * _distanceY + 10 * (_distanceX - _distanceY);
return 14 * _distanceX + 10 * (_distanceY - _distanceX);
}
}
}
The package offers an implementation of the Unity Job System to enable parallel path calculation. Although the AStar algorithm remains the same, incorporating the Grid data structure with NativeData poses a challenge. To address this issue, the package includes an extension method for GridData to seamlessly transfer data from the CPU side and translate it into NativeData.
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using Unity.Collections;
using Unity.Mathematics;
namespace MugCup_PathFinder.Runtime
{
public static class GridStructureBuilder
{
public static GridStructure GenerateGridStructure(Vector3Int _gridSize)
{
var _gridStructure = new GridStructure
{
WorldOffset = float3.zero,
GridSize = new int3(_gridSize.x, _gridSize.y, _gridSize.z),
Row = _gridSize.x,
Column = _gridSize.z,
Level = _gridSize.y,
Grid = new NativeArray<NodeNativeData>(_gridSize.x * _gridSize.y * _gridSize.z, Allocator.Persistent)
};
return _gridStructure;
}
public static GridStructure CopyData<T>(this GridData<T> _gridData) where T : INode
{
var _gridStructure = GenerateGridStructure(_gridData.GridSize);
for(var _i = 0; _i < _gridData.GridNodes.Length; _i++)
{
var _node = _gridData.GridNodes[_i];
if(_node == null) continue;
var _newNode = new NodeNativeData
{
GridPos = new int3 (_node.NodeGridPosition.x , _node.NodeGridPosition.y , _node.NodeGridPosition.z),
WorldPos = new float3(_node.NodeWorldPosition.x, _node.NodeWorldPosition.y, _node.NodeWorldPosition.z)
};
_gridStructure.Grid[_i] = _newNode;
}
return _gridStructure;
}
}
}
In addition to the AStar implementation, the package also includes the 'Breadth First Search' algorithm. While the Job system is ideal for parallel processing, it can be complex to implement, and not all data types can easily translate into Native data. Therefore, the package provides an alternative path calculation technique called 'Flow Field.' This method utilizes the breadth-first search algorithm to calculate and store node direction from the origin to the target. The calculation is performed once per origin-target pair, and multiple agents can access the shared path data from anywhere. While this approach creates a flock-like behavior among agents, as opposed to individually calculated paths using the Job system, it is ideal for certain situations, such as tower defense games.
In the following prototype, I utilize a grid graph structure that stores each node with connectable neighbor nodes. The GripGraph class generates a map that can run the breadth-first search algorithm.
using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using UnityEngine;
#if UNITY_EDITOR
using UnityEditor;
#endif
namespace MugCup_PathFinder.Runtime
{
public class GridGraph : MonoBehaviour, IGraph<VertexNode>
{
[Header("Selected Source Vertex Node")]
[SerializeField] private VertexNode SourceVertexNode;
[Header("Grid Data")]
[SerializeField] private GridNodeData GridData;
[SerializeField] private GridVertexData GridVertexData;
[field: SerializeField] public List<GraphEdgeVertexNode> GraphEdges { get; private set; } = new List<GraphEdgeVertexNode>();
public Dictionary<VertexNode, GridNode> VertexToGirdNodeTable { get; private set; } = new Dictionary<VertexNode, GridNode>();
[Header("Game Object")]
[SerializeField] private VertexNode VertexPrefab;
[SerializeField] private Transform VertexParent;
[Header("Debug")]
[SerializeField] private bool IsDebug;
[SerializeField] private bool DisplayBezier;
[SerializeField] private bool DisplayLine;
[SerializeField] private float EdgeWidth;
private void OnValidate()
{
if (VertexParent == null)
{
var _vertexParentObj = new GameObject("#Vertex Parent")
{
transform = { position = Vector3.zero }
};
VertexParent = _vertexParentObj.transform;
}
}
public GridGraph Initialized(GridNodeData _gridData)
{
GridData = _gridData;
return this;
}
public void GenerateValidVertices()
{
GridVertexData
.InitializeGridUnitSize(GridData.GridSize)
.InitializeGridArray();
foreach (var _node in GridData.GridNodes.Where(_node => _node != null))
{
if (!GridUtility.HasNodeOnTop(_node, GridData.GridSize, GridData.GridNodes))
{
AddVertex(_node);
}
else
{
var _topNode = GridUtility.GetNodeUp(_node.NodeGridPosition, GridData.GridSize, GridData.GridNodes);
if (!_topNode.IsObstacle)
{
AddVertex(_node);
}
}
}
MapGraph();
#if UNITY_EDITOR
if (!Application.isPlaying)
EditorUtility.SetDirty(GridVertexData);
#endif
}
private void AddVertex(GridNode _node)
{
if(_node.IsVertexNodeInit) return;
_node.SetVertexNodeInit(true);
foreach ((Vector3 _vertexWorldPos, Vector3Int _vertexGridPos) in _node.GenerateVertexNodePositions())
{
var _vertexNode = AddVertex(_vertexGridPos, _vertexWorldPos);
if (!VertexToGirdNodeTable.ContainsKey(_vertexNode))
VertexToGirdNodeTable.Add(_vertexNode, _node);
}
}
private VertexNode AddVertex(Vector3Int _atGridPos, Vector3 _atWorldPosition)
{
var _vertexNode = Instantiate(VertexPrefab, _atWorldPosition, Quaternion.identity, VertexParent);
_vertexNode.SetNodeWorldPosition(_atWorldPosition);
_vertexNode.SetNodePosition (_atGridPos);
GridVertexData.AddNode(_vertexNode, _atGridPos);
return _vertexNode;
}
private GraphEdgeVertexNode AddEdge(VertexNode _from, VertexNode _to)
{
var _edge = new GraphEdgeVertexNode(_from, _to);
GraphEdges.Add(_edge);
return _edge;
}
private void MapGraph()
{
foreach (var _vertex in GridVertexData.GridNodes.Where(_vertex => _vertex != null))
{
var _vertices = GetVertexNeighbors(_vertex);
if (VertexToGirdNodeTable.TryGetValue(_vertex, out var _gridNode))
{
if (_gridNode.ConnectorCount == 0)
_gridNode.Connect4Directions();
var _validVertexConnects = _gridNode
.WorldNodeConnector
.Select(_connectPos => _connectPos + Vector3.up)
.Select(_vertexPos =>
{
var _vertexGridPos = new Vector3Int((int)_vertexPos.x, (int)_vertexPos.y, (int)_vertexPos.z);
return _vertexGridPos;
});
_vertices = _vertices.Where(_v => _validVertexConnects.Contains(_v.NodeGridPosition)).ToArray();
var _edges = new List<GraphEdgeVertexNode>();
foreach (var _toVertex in _vertices)
{
var _edge = AddEdge(_vertex, _toVertex);
_edges.Add(_edge);
}
_vertex.AddEdges(_edges);
}
}
}
public IEnumerable<VertexNode> GetVertexNeighbors(VertexNode _vertex)
{
return GridUtility
.GetNodesFrom3x3Cubes(_vertex, GridVertexData.GridSize, GridVertexData.GridNodes)
.Where(_v => _v != null);
}
public void CalculateFlowField()
{
StartCoroutine(BreadFirstSearch<VertexNode>.BreadthFirstSearch(SourceVertexNode.NodeGridPosition, GridVertexData));
}
public void ClearVertexData()
{
GridVertexData.ClearData();
GraphEdges .Clear();
}
private void OnDrawGizmos()
{
if (!IsDebug) return;
if(GraphEdges == null || GraphEdges.Count == 0) return;
foreach (var _edge in GraphEdges)
{
var _startPos = _edge.To.NodeWorldPosition;
var _targetPos = _edge.From.NodeWorldPosition;
var _dir = _targetPos - _startPos;
var _startTangent = _startPos + Vector3.up / 2;
var _endTangent = _startTangent + _dir;
if(DisplayBezier)
Handles.DrawBezier(_startPos, _targetPos, _startTangent, _endTangent, Color.red, null, EdgeWidth);
if(DisplayLine)
Gizmos.DrawLine(_startPos, _targetPos);
}
}
}
}
Utilizing the 'Breadth First Search' technique, I can create a behavior that mimics fire spreading across an adjacent field of grass.
