IEnumerator GetPathCoro(Coordinate start, Coordinate end)
{
List <Coordinate> visited = new List <Coordinate>();
if (Target)
{
Priority_Queue.SimplePriorityQueue <Coordinate> queue = new Priority_Queue.SimplePriorityQueue <Coordinate>();
if (start == null)
{
start = prev_start_coord;
}
if (start != null)
{
start.parent = null;
}
if (end == null)
{
end = prev_end_coord;
}
if (start == null || end == null)
{
yield break;
}
float tstart = Time.realtimeSinceStartup;
prev_start_coord = start;
prev_end_coord = end;
queue.Enqueue(start, GetCoordinateDistFromTarget(start));
while (Path == null)
{
start = queue.Dequeue();
if (start == end)
{
/* Debug.Log(Time.realtimeSinceStartup - tstart + " seconds : " +
* queue.Count + " end routes considered : " +
* start.GetNumParents() + " parents."); */
Path = start;
yield break;
}
if (Time.realtimeSinceStartup > tstart + .01f || index != go_index)
{
yield return(new WaitForEndOfFrame());
tstart = Time.realtimeSinceStartup;
}
int safe_layer = ptr.gameObject.layer;
foreach (Coordinate coord in start.GetChildren())
{
coord.traverse_cost = GetCoordinateDistFromTarget(coord);
if (!visited.Contains(coord))
{
coord.parent = start;
queue.Enqueue(coord, coord.GetTotalCost(safe_layer, can_dodge));
visited.Add(coord);
}
else if (queue.Contains(coord) && coord.GetTotalCost(safe_layer, can_dodge, 0, start) < queue.GetPriority(coord))
{
coord.parent = start;
queue.UpdatePriority(coord, coord.GetTotalCost(safe_layer, can_dodge));
}
}
if (queue.Count != 0)
{
start = queue.First;
}
else
{
break;
}
}
}
yield return(null);
}
public bool GetPath(Vector2Int start, Vector2Int destination, out Vector2Int[] path)
{
Priority_Queue.SimplePriorityQueue <Vector2Int> open = new Priority_Queue.SimplePriorityQueue <Vector2Int>();
Dictionary <Vector2Int, bool> closed = new Dictionary <Vector2Int, bool>();
Dictionary <Vector2Int, Step> allSteps = new Dictionary <Vector2Int, Step>();
Step startStep = new Step(start, 0, null);
allSteps.Add(startStep.Location, startStep);
open.Enqueue(startStep.Location, 0);
Step currentStep;
Vector2Int currentLocation;
Step neighborStep;
// While lowest rank in OPEN is not the GOAL:
while (open.Count > 0)
{
// Set current = remove lowest rank item from OPEN
currentLocation = open.Dequeue();
if (!allSteps.TryGetValue(currentLocation, out currentStep))
{
throw new Exception("Could not find expected step");
}
if (currentLocation == destination)
{
// Reconstruct reverse path from goal to start by following parent pointers
path = GetPoints(currentStep);
return(true);
}
// Add current to CLOSED
closed.Add(currentLocation, true);
// For neighbors of current:
foreach (Vector2Int m in movements)
{
bool inOpen;
bool inClosed;
int nextCost = currentStep.Cost + 1;
Vector2Int nextLocation = currentLocation + m;
Step nextStep = new Step(nextLocation, nextCost, currentStep);
if (!IsPossibleMove(currentLocation, nextLocation))
{
// Spot is bad.
continue;
}
// If neighbor in OPEN and cost less than g(neighbor):
// remove neighbor from OPEN, because new path is better
if (open.Contains(nextLocation))
{
if (!allSteps.TryGetValue(nextLocation, out neighborStep))
{
throw new Exception("Could not find expected step");
}
if (nextCost < neighborStep.Cost)
{
open.Remove(nextLocation);
allSteps.Remove(nextLocation);
inOpen = false;
}
else
{
inOpen = true;
}
}
else
{
inOpen = false;
}
// If neighbor in CLOSED and cost less than g(neighbor):
// remove neighbor from CLOSED
if (closed.ContainsKey(nextLocation))
{
if (!allSteps.TryGetValue(nextLocation, out neighborStep))
{
throw new Exception("Could not find expected step");
}
if (nextCost < neighborStep.Cost)
{
closed.Remove(nextLocation);
inClosed = false;
}
else
{
inClosed = true;
}
}
else
{
inClosed = false;
}
// If neighbor not in OPEN and neighbor not in CLOSED:
if (!inOpen && !inClosed)
{
int h = Heuristic(nextLocation, destination);
neighborStep = new Step(nextLocation, nextCost, currentStep);
allSteps.Add(nextLocation, neighborStep);
open.Enqueue(nextLocation, nextCost + h);
}
}
}
path = null;
return(false);
}
