public List <Node> FindRange(Vector3 startPosition, int range)
{
Node startingNode = grid.GetNodeFromWorldPosition(startPosition);
List <Node> inRange = new List <Node>();
Queue <Node> BFSQueue = new Queue <Node>();
bool[,,] searched = new bool[grid.maxX, grid.maxY, grid.maxZ];
BFSQueue.Enqueue(startingNode);
while (BFSQueue.Count > 0)
{
Node current = BFSQueue.Dequeue();
inRange.Add(current);
List <Node> neighbors = grid.GetNodeNeighbors(current);
foreach (Node neighbor in neighbors)
{
if (range - GetDistance(startingNode, neighbor) > 0 && !searched[neighbor.x, neighbor.y, neighbor.z])
{
BFSQueue.Enqueue(neighbor);
searched[neighbor.x, neighbor.y, neighbor.z] = true;
}
}
}
return(inRange);
}
/// <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);
}
