Vector3[] RetracePath(PathRequest request, Node startNode, Node endNode)
{
List <Node> path = new List <Node>();
Node currentNode = endNode;
while (currentNode != startNode)
{
path.Add(currentNode);
currentNode = currentNode.Parent;
}
Vector3[] waypoints = SimplifyPath(path);
Array.Reverse(waypoints);
return(waypoints);
}
public void FindPath(PathRequest pathRequest, Action <PathResult> callback)
{
sw.Start();
Vector3[] waypoints = new Vector3[0];
bool success = false;
Node startNode = grid.NodeFromWorldPoint(pathRequest.pathStart);
Node endNode = grid.NodeFromWorldPoint(pathRequest.pathEnd);
if (startNode.isWalkable && endNode.isWalkable)
{
Heap <Node> openSets = new Heap <Node>(grid.MaxSize);
HashSet <Node> closedSet = new HashSet <Node>();
openSets.Add(startNode);
while (openSets.Count > 0)
{
Node currentNode = openSets.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == endNode)
{
sw.Stop();
print("Path found: " + sw.ElapsedMilliseconds + "ms");
success = true;
break;
}
foreach (var item in grid.GetNeighbours(currentNode))
{
if (closedSet.Contains(item) || !item.isWalkable || (pathRequest.needLand && !item.gotLand))
{
continue;
}
int newCost = currentNode.gCost + GetDistance(currentNode, item) + item.weights;
if (newCost < item.gCost || !openSets.Contains(item))
{
item.gCost = newCost;
item.hCost = GetDistance(item, endNode);
item.Parent = currentNode;
if (!openSets.Contains(item))
{
openSets.Add(item);
}
else
{
openSets.UpdateItem(item);
}
}
}
}
}
if (success)
{
waypoints = RetracePath(pathRequest, startNode, endNode);
success = waypoints.Length > 0;
}
callback(new PathResult(waypoints, success, pathRequest.callback));
}
