public static void GetPath(Vector2 start, Vector2 target, List <Node> pp, Resolution resolution)
{
#region INIT
var grid = NodeGridManager.GetGrid(resolution);
var nodeGrid = grid.nodeGrid;
Node startNode = grid.NodeFromWorldPoint(start);
Node targetNode = grid.NodeFromWorldPoint(target);
if (!startNode.walkable)
{
startNode = NodeHelper.ClosestWalkable(startNode, resolution);
}
if (!targetNode.walkable)
{
targetNode = NodeHelper.ClosestWalkable(targetNode, resolution);
}
Node node, nbr;
openSet.Clear();
closedSet.Clear();
openSet.Add(startNode);
startNode.hCost = startNode.gCost = 0;
#endregion
while (openSet.Count > 0)
{
node = openSet.RemoveFirst();
closedSet.Add(node);
if (node == targetNode)
{
pp.Clear();
Node currentNode = targetNode;
while (currentNode != startNode)
{
pp.Add(currentNode);
currentNode = nodeGrid[currentNode.parent.x, currentNode.parent.y];
}
pp.Reverse();
return;
}
bool skipD = false;
var nbsA = node.neighbours.directionNeighbours;
int scanA = node.neighbours.DirectionCount;
for (int i = 0; i < scanA; i++)
{
nbr = nodeGrid[nbsA[i].x, nbsA[i].y];
if (!nbr.walkable)
{
skipD = true; continue;
}
if (closedSet.Contains(nbr))
{
continue;
}
int cost = node.gCost + heuristic(node, nbr);
if (!openSet.Contains(nbr))
{
nbr.gCost = cost;
nbr.hCost = heuristic(nbr, targetNode);
nbr.parent = new BaseNode(node.X, node.Y);
openSet.Add(nbr);
}
else if (cost < nbr.gCost)
{
nbr.gCost = cost;
nbr.hCost = heuristic(nbr, targetNode);
nbr.parent = new BaseNode(node.X, node.Y);
}
}
if (skipD)
{
continue;
}
var nbsB = node.neighbours.diagonalNeighbours;
int scanB = node.neighbours.DiagonalCount;
for (int i = 0; i < scanB; i++)
{
nbr = nodeGrid[nbsB[i].x, nbsB[i].y];
if (!nbr.walkable)
{
continue;
}
if (closedSet.Contains(nbr))
{
continue;
}
for (int j = 0; j < nbr.neighbours.DiagonalCount; j++)
{
var nbr_D = nbr.neighbours.diagonalNeighbours[j];
var dNode = nodeGrid[nbr_D.x, nbr_D.y];
if (!dNode.walkable)
{
break;
}
}
int cost = node.gCost + heuristic(node, nbr);
if (!openSet.Contains(nbr))
{
nbr.gCost = cost;
nbr.hCost = heuristic(nbr, targetNode);
nbr.parent = new BaseNode(node.X, node.Y);
openSet.Add(nbr);
}
else if (cost < nbr.gCost)
{
nbr.gCost = cost;
nbr.hCost = heuristic(nbr, targetNode);
nbr.parent = new BaseNode(node.X, node.Y);
}
}
}
// If the code escapes the while loop, it means no valid path could be found.
Debug.LogWarning($"Path not found (Start: {start} -- Target: {target})");
}