public Queue <PathfindingNode> CalculatePath(PathfindingNode origin, PathfindingNode target)
{
Open = new List <ConsideredNode>();
Closed = new List <ConsideredNode>();
Open.Add(new ConsideredNode(origin, null, CalculateDirectTravelCost(origin, target)));
while (true)
{
ConsideredNode currentNode = GetLowestCostNode(Open);
if (currentNode == null)
{
Debug.LogError("Could not find path");
return(new Queue <PathfindingNode>());
}
Open.Remove(currentNode);
Closed.Add(currentNode);
if (currentNode.Info == target)
{
List <PathfindingNode> reversedPath = new List <PathfindingNode>();
while (currentNode != null)
{
reversedPath.Add(currentNode.Info);
currentNode = currentNode.Parent;
}
Queue <PathfindingNode> path = new Queue <PathfindingNode>();
while (reversedPath.Count > 1)
{
path.Enqueue(reversedPath[reversedPath.Count - 2]);
reversedPath.RemoveAt(reversedPath.Count - 2);
}
return(path);
}
foreach (PathfindingNode neighbour in Grid.GetNeighbourNodes(currentNode.Info))
{
if (neighbour.IsWalkable == false || Closed.Find(n => n.Info == neighbour) != null)
{
continue;
}
ConsideredNode neighbourNode = Open.Find(n => n.Info == neighbour);
if (neighbourNode == null)
{
neighbourNode = new ConsideredNode(neighbour, currentNode, CalculateDirectTravelCost(neighbour, target));
Open.Add(neighbourNode);
}
else
{
float newTravelCost = currentNode.TravelCost + CalculateDirectTravelCost(currentNode.Info, neighbour);
if (neighbourNode.TravelCost > newTravelCost)
{
neighbourNode.SetParent(currentNode);
}
}
}
}
}
private ConsideredNode GetLowestCostNode(List <ConsideredNode> List)
{
int currentNodeCost = int.MaxValue;
ConsideredNode currentNode = null;
foreach (ConsideredNode node in List)
{
if (node.TotalCost < currentNodeCost)
{
currentNodeCost = node.TotalCost;
currentNode = node;
}
}
return(currentNode);
}
public void SetParent(ConsideredNode parent)
{
this.Parent = parent;
}
public ConsideredNode(PathfindingNode nodeInfo, ConsideredNode parent, int directTravelCostToTarget)
{
this.Info = nodeInfo;
this.DirectTravelCostToTarget = directTravelCostToTarget;
this.SetParent(parent);
}
