static private int GetDistance(GridPathfinderNode nodeA, GridPathfinderNode nodeB)
{
int dstX = Mathf.Abs(nodeA.x - nodeB.x);
int dstY = Mathf.Abs(nodeA.y - nodeB.y);
return(dstX + dstY);
}
static private List <GridPathfinderNode> FindNeighbours(GridPathfinderNode node)
{
List <GridPathfinderNode> neighbours = new List <GridPathfinderNode>();
GridPathfinderNode? n = null;
n = CreatePathfinderNode(node.node.GetNeighbour(1, 0));
if (n != null)
{
neighbours.Add((GridPathfinderNode)n);
}
n = CreatePathfinderNode(node.node.GetNeighbour(-1, 0));
if (n != null)
{
neighbours.Add((GridPathfinderNode)n);
}
n = CreatePathfinderNode(node.node.GetNeighbour(0, 1));
if (n != null)
{
neighbours.Add((GridPathfinderNode)n);
}
n = CreatePathfinderNode(node.node.GetNeighbour(0, -1));
if (n != null)
{
neighbours.Add((GridPathfinderNode)n);
}
return(neighbours);
}
static private void UpdateItemInList(GridPathfinderNode node, List <GridPathfinderNode> list)
{
for (int i = 0; i < list.Count; i++)
{
if (list[i].x == node.x && list[i].y == node.y)
{
list[i] = node;
}
}
return;
}
static private bool ListContainsNode(List <GridPathfinderNode> path, GridPathfinderNode node)
{
foreach (GridPathfinderNode pnode in path)
{
if (pnode.x == node.x && pnode.y == node.y)
{
return(true);
}
}
return(false);
}
#pragma warning disable CS8632 // The annotation for nullable reference types should only be used in code within a '#nullable' annotations context.
static private GridPathfinderNode?CreatePathfinderNode(GridNode inNode)
#pragma warning restore CS8632 // The annotation for nullable reference types should only be used in code within a '#nullable' annotations context.
{
if (inNode == null)
{
return(null);
}
GridPathfinderNode outNode = new GridPathfinderNode();
outNode.x = inNode.x;
outNode.y = inNode.y;
outNode.IsTraversable = inNode.isTraversable;
outNode.node = inNode;
return(outNode);
}
static private List <GridNode> RetracePath(GridPathfinderNode startNode, GridPathfinderNode endNode)
{
List <GridNode> path = new List <GridNode>();
GridPathfinderNode currentNode = endNode;
while (currentNode != null)
{
if (currentNode.node == null)
{
break;
}
path.Add(currentNode.node);
currentNode = currentNode.parent;
}
path.Reverse();
return(path);
}
// ported from lil-guy-big-adventure : PathFinder.cs written by @jay
static public List <GridNode> FindPath(GridNode start, GridNode end)
{
bool pathSuccess = false;
List <GridPathfinderNode> openSet = new List <GridPathfinderNode>();
List <GridPathfinderNode> closedSet = new List <GridPathfinderNode>();
// TODO - nullcheck
GridPathfinderNode startNode = (GridPathfinderNode)CreatePathfinderNode(start);
GridPathfinderNode targetNode = (GridPathfinderNode)CreatePathfinderNode(end);
if (startNode.IsTraversable && targetNode.IsTraversable)
{
openSet.Add(startNode);
while (openSet.Count > 0)
{
GridPathfinderNode currentNode = openSet[0];
openSet.RemoveAt(0);
closedSet.Add(currentNode);
if (currentNode.x == targetNode.x && currentNode.y == targetNode.y)
{
pathSuccess = true;
break;
}
List <GridPathfinderNode> neighbourList = FindNeighbours(currentNode);
for (int i = 0; i < neighbourList.Count; i++)
{
GridPathfinderNode neighbour = neighbourList[i];
if (!neighbour.IsTraversable || ListContainsNode(closedSet, neighbour))
{
continue;
}
int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour);
if (newMovementCostToNeighbour < neighbour.gCost || !ListContainsNode(openSet, neighbour))
{
neighbour.gCost = newMovementCostToNeighbour;
neighbour.hCost = GetDistance(neighbour, targetNode);
neighbour.parent = currentNode;
if (!ListContainsNode(openSet, neighbour))
{
openSet.Add(neighbour);
}
else
{
UpdateItemInList(neighbour, openSet);
}
}
}
}
}
if (pathSuccess)
{
return(RetracePath(openSet[0], openSet[openSet.Count - 1]));
}
return(null);
}
