public Path NavigateTo(Vector3Int from, Vector3Int to)
{
if (!HasNodeAt(from) || !HasNodeAt(to))
{
return(null);
}
List <PathfindingNode> nodes = new List <PathfindingNode>();
PathfindingNode current = new PathfindingNode(from);
PathfindingNode toNode = new PathfindingNode(to);
for (int x = 0; x < Width; x += 1)
{
for (int y = 0; y < Height; y += 1)
{
if (!_nodes[x, y])
{
continue;
}
PathfindingNode node = new PathfindingNode(x, y);
nodes.Add(node);
node.FScore = node.Heuristic(toNode);
if (node.Equals(from))
{
current = node;
}
if (node.Equals(to))
{
toNode = node;
}
}
}
current.GScore = 0;
List <PathfindingNode> closedSet = new List <PathfindingNode>();
List <PathfindingNode> openSet = new List <PathfindingNode> {
current
};
while (openSet.Count > 0)
{
current = openSet.OrderBy(n => n.FScore).First();
if (current.Equals(to))
{
return(current.ConstructPath());
}
openSet.Remove(current);
closedSet.Add(current);
foreach (PathfindingNode node in nodes.Where(n => n.IsNeighboor(current)))
{
if (closedSet.Contains(node))
{
continue;
}
if (!openSet.Contains(node))
{
openSet.Add(node);
}
int tentativeGScore = current.GScore + 1;
if (tentativeGScore > AStarLimit)
{
return(null);
}
if (tentativeGScore >= node.GScore)
{
continue;
}
node.GScore = tentativeGScore;
node.FScore = node.GScore + node.Heuristic(toNode);
if (current.From != null)
{
node.FScore += current.To(current.From).Equals(node.To(current)) ? 0 : 4;
}
node.From = current;
}
}
return(null);
}