IEnumerator FindPath(Vector3 startPosition, Vector3 targetPosition)
{
Vector3[] waypoints = new Vector3[0];
bool success = false;
Node startNode = grid.NodeFromWorldPoint(startPosition);
Node targetNode = grid.NodeFromWorldPoint(targetPosition);
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)
{
success = true;
break;
}
foreach (Node neighbor in grid.GetNeighbors(currentNode))
{
if (!neighbor.walkable && neighbor != targetNode || closedSet.Contains(neighbor))
{
continue;
}
int newMovementCostToNeighbor = currentNode.gCost + GetDistance(currentNode, neighbor);
if (newMovementCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor))
{
neighbor.gCost = newMovementCostToNeighbor;
neighbor.hCost = GetDistance(neighbor, targetNode);
neighbor.parent = currentNode;
if (!openSet.Contains(neighbor))
{
openSet.Add(neighbor);
}
else
{
openSet.UpdateItem(neighbor);
}
}
}
}
yield return(null);
if (success)
{
waypoints = TracePath(startNode, targetNode);
}
requestManager.FinishProcessing(waypoints, success);
}
public void FindPath(PathRequest request, Action <PathResult> callback)
{
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
Node startNode = this.nodeGrid.NodeFromWorldPoint(request.pathStart);
Node targetNode = this.nodeGrid.NodeFromWorldPoint(request.pathEnd);
if (!startNode.walkable)
{
List <Node> neighbors = nodeGrid.GetNeighbors(startNode);
if (neighbors.Count > 0)
{
startNode = neighbors[0];
}
}
if (!targetNode.walkable)
{
List <Node> neighbors = nodeGrid.GetNeighbors(targetNode);
if (neighbors.Count > 0)
{
targetNode = neighbors[0];
}
}
if (startNode.walkable && targetNode.walkable)
{
Heap <Node> openSet = new Heap <Node>(nodeGrid.MaxSize);
HashSet <Node> closedSet = new HashSet <Node>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (currentNode == targetNode)
{
pathSuccess = true;
break;
}
foreach (Node neighbor in nodeGrid.GetNeighbors(currentNode))
{
if (closedSet.Contains(neighbor))
{
continue;
}
int movementCost = currentNode.gCost + this.GetDistance(currentNode, neighbor) + neighbor.weight;
if (movementCost < neighbor.gCost || !openSet.Contains(neighbor))
{
neighbor.gCost = movementCost;
neighbor.hCost = this.GetDistance(neighbor, targetNode);
neighbor.parent = currentNode;
if (!openSet.Contains(neighbor))
{
openSet.Add(neighbor);
}
else
{
openSet.UpdateItem(neighbor);
}
}
}
}
}
if (pathSuccess)
{
waypoints = RetracePath(startNode, targetNode);
pathSuccess = waypoints.Length > 0;
}
callback(new PathResult(waypoints, pathSuccess, request.callback));
}
/// <summary>
/// Finds a path between two points on the grid.
/// </summary>
/// <param name="from">initial position</param>
/// <param name="to">target position</param>
/// <returns>path array</returns>
private IEnumerator FindPath(Vector3 from, Vector3 to, Action <Vector3[], bool> callback)
{
/* F cost = G cost + H cost
* G cost = distance from origin to node
* H cost = distance from node to target */
// Open set => Nodes that are yet to be evaluated.
BinaryHeap <Node> openSet = new BinaryHeap <Node>(Grid.NodeCount);
// Closes set => Nodes that have been claimed by another.
HashSet <Node> closedSet = new HashSet <Node>();
Node initialNode = Grid.GetNodeFromWorldPosition(from);
Node targetNode = Grid.GetNodeFromWorldPosition(to);
// The initial node also must pass through the first iteration.
//initialNode.HCost = GetDistance(initialNode, targetNode);
openSet.Add(initialNode);
while (openSet.Count > 0)
{
// Removing and getting the node with the lowest F cost (highest G cost in ties).
Node currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
// Returning the path if the node is the target.
if (currentNode.Equals(targetNode))
{
callback.Invoke(RetracePath(initialNode, targetNode), true);
yield break;
}
// Getting the surrounding nodes.
Node[] neighbors = Grid.GetNeighbors(currentNode);
// Deciding what to do with each neighbor.
foreach (var neighbor in neighbors)
{
if (!neighbor.Walkable)
{
continue;
}
if (closedSet.Contains(neighbor))
{
continue;
}
/* newGCost refers to the new hypothetical G cost of the neighbor
* (it could have been assigned by another node and now be lower by this path).*/
int newGCost = GetDistance(currentNode, neighbor) + currentNode.GCost +
neighbor.Weight;
bool openSetHasNotNeighbor = !openSet.Contains(neighbor);
if (openSetHasNotNeighbor || newGCost < neighbor.GCost)
{
// Parent node is used to retrace the path once it has been found.
neighbor.ParentNode = currentNode;
neighbor.GCost = newGCost;
neighbor.HCost = GetDistance(neighbor, targetNode);
if (openSetHasNotNeighbor)
{
openSet.Add(neighbor);
}
else
{
/* The reason for this line is that if the neighbor is already in the
* open set, it certainly got its G cost lowered and must me sorted
* up in the binary heap.*/
openSet.SortUp(neighbor);
}
}
}
}
yield return(null);
callback.Invoke(null, false);
}
public static Coord[] FindPath(Coord startPos, IList <Coord> goalPositions)
{
bool singleTarget = goalPositions.Count == 1;
bool success = false;
HashSet <Node> goals = new HashSet <Node>(goalPositions.Select(NodeGrid.GetNode));
Node startNode = NodeGrid.GetNode(startPos);
Node goalNode = NodeGrid.GetNode(goalPositions[0]);
if (startNode != null && goalNode != null && goalNode.IsWalkable && goalNode != startNode)
{
Heap <Node> open = new Heap <Node>(NodeGrid.GridCount);
HashSet <Node> closed = new HashSet <Node>();
open.Add(startNode);
while (open.Count > 0)
{
Node current = open.RemoveFirst();
closed.Add(current);
if (singleTarget && current == goalNode || !singleTarget && goals.Contains(current) /*current == goalNode*/)
{
goalNode = current;
success = true;
break;
}
foreach (Node neighbor in NodeGrid.GetNeighbors(current))
{
if (!neighbor.IsWalkable || closed.Contains(neighbor))
{
continue;
}
int newMovementCost = current.gCost + GetDistance(current, neighbor);
if (newMovementCost < neighbor.gCost || !open.Contains(neighbor))
{
neighbor.gCost = newMovementCost;
neighbor.hCost = GetDistance(neighbor, goalNode);
neighbor.parent = current;
if (!open.Contains(neighbor))
{
open.Add(neighbor);
}
else
{
open.UpdateItem(neighbor);
}
}
}
}
}
Coord[] waypoints = null;
if (success)
{
waypoints = RetracePath(startNode, goalNode);
}
return(waypoints == null || waypoints.Length == 0 ? null : waypoints);
}
