IEnumerator FindPath(Vector3 startPos, Vector3 targetPos)
{
Stopwatch sw = new Stopwatch();
sw.Start();
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
Node startNode = grid.NodeFromWorldPoint(startPos);
Node targetNode = grid.NodeFromWorldPoint(targetPos);
startNode.parent = startNode;
if (startNode.walkable && targetNode.walkable)
{
Heap <Node> openSet = new Heap <Node>(grid.MaxSize);
HashSet <Node> closedSet = new HashSet <Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
sw.Stop();
//Debug.Log("Path found: " + sw.ElapsedMilliseconds + " ms");
pathSuccess = true;
break;
}
foreach (Node neighbour in grid.GetNeighbours(currentNode))
{
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty;
if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = newMovementCostToNeighbour;
neighbour.hCost = GetDistance(neighbour, targetNode);
neighbour.parent = currentNode;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
else
{
openSet.UpdateItem(neighbour);
}
}
}
}
}
yield return(null);
if (pathSuccess)
{
waypoints = RetracePath(startNode, targetNode);
}
requestManager.FinishedProcessingPath(waypoints, pathSuccess);
}
public void FindPath(PathRequest request, Action <PathResult> callback)
{
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
Node startNode = worldGrid.NodeFromWorldPoint(request.pathStart);
Node targetNode = worldGrid.NodeFromWorldPoint(request.pathEnd);
startNode.parent = startNode;
int dist = GetDistance(startNode, targetNode);
if (startNode.walkable && targetNode.walkable && dist > 40)
{
Heap <Node> openSet = new Heap <Node> (worldGrid.MaxSize);
HashSet <Node> closedSet = new HashSet <Node> ();
openSet.Add(startNode);
while (openSet.Count > 0 && closedSet.Count < 350)
{
Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
pathSuccess = true;
break;
}
foreach (Node neighbour in worldGrid.GetNeighbours(currentNode, request.unitCollider))
{
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour);
if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = newMovementCostToNeighbour;
neighbour.hCost = GetDistance(neighbour, targetNode);
neighbour.parent = currentNode;
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
else
{
openSet.UpdateItem(neighbour);
}
}
}
}
}
if (pathSuccess)
{
waypoints = RetracePath(startNode, targetNode, request.unitCollider);
pathSuccess = waypoints.Length > 0;
}
callback(new PathResult(waypoints, pathSuccess, request.callback));
}
public void GetPath(Cell start, Cell end)
{
//Closed and open sets
closedSet.Clear();
open.Clear();
//Holds result
trajectory.Clear();
currentDestination = end;
ACell first = new ACell(start);
open.Add(first);
List <Cell> result = new List <Cell>();
while (open.Count > 0)
{
//Get first node by sorting by smallest f cost and picking that
ACell current = open[0];
for (int i = 1; i < open.Count; i++)
{
if (open[i].f < current.f || (open[i].f == current.f && open[i].h < current.h))
{
current = open[i];
}
}
open.Remove(current);
if (current.main == null || current == null)
{
continue;
}
closedSet.Add(current.main.worldPosition);
if (current.main.worldPosition == end.worldPosition)
{
while (current != null)
{
result.Insert(0, current.main);
current = current.parent;
}
trajectory = result;
moveList = trajectory;
return;
}
List <Cell> neighbours = grid.GetNeighbours(current.main);
foreach (Cell n in neighbours)
{
ACell neighbour = new ACell(n);
if (closedSet.Contains(n.worldPosition))
{
continue;
}
float newDistFromStart = current.g + GetDistance(current.main, n);
if (newDistFromStart < neighbour.g || !open.Contains(neighbour))
{
open.Remove(neighbour);
neighbour.g = newDistFromStart;
neighbour.h = GetDistance(neighbour.main, end);
neighbour.parent = current;
//Remove if it exists so we can store the new updated value
open.Add(neighbour);
}
}
}
}
private IEnumerator FindPathEnum(Node startNode, Node targetNode, Action <List <Node>, bool> callback)
{
var openSet = new Heap <Node>(grid.MaxSize);
var closedSet = new HashSet <Node>();
startNode.parent = startNode;
openSet.Enqueue(startNode);
Node currentNode;
for (int i = 0; openSet.Count > 0; i++)
{
currentNode = openSet.Dequeue();
if (currentNode == targetNode)
{
List <Node> path = RetracePath(startNode, targetNode);
//draw path from targetPoint back to target
foreach (Node n in path)
{
n.ChooseAsPath();
toReset.Add(n);
yield return(new WaitForSeconds(0.1f));
}
path.Reverse();
callback(path, path.Count > 0);
yield break;
}
closedSet.Add(currentNode);
foreach (Node neighbour in grid.GetNeighbours(currentNode))
{
if (neighbour.walkable && !closedSet.Contains(neighbour))
{
int tentativeGCost = gCost(currentNode, neighbour);
if (tentativeGCost < neighbour.gCost || !openSet.Contains(neighbour))
{
neighbour.gCost = tentativeGCost;
neighbour.hCost = hCost(neighbour, targetNode);
neighbour.parent = currentNode;
if (!openSet.Contains(neighbour))
{
openSet.Enqueue(neighbour);
}
else
{
openSet.UpdateItem(neighbour);
}
neighbour.Explore();
toReset.Add(neighbour);
}
}
}
//speed things up a bit
if (i % 10 == 0)
{
yield return(null);
}
}
foreach (Node n in toReset)
{
n.SetInvalid();
}
}
