private List <GridLocation> LazyRun(Bitmap bitmap, GridLocation start, GridLocation goal)
{
//Debug.LogWarning("TS: setup dictionaries");
Dictionary <GridLocation, GridLocation> cameFrom = new Dictionary <GridLocation, GridLocation>();
Dictionary <GridLocation, double> costSoFar = new Dictionary <GridLocation, double>();
//Debug.LogWarning("TS: setup interval heap");
var frontier = new C5.IntervalHeap <GridLocation>();
start.priority = 0;
frontier.Add(start);
cameFrom[start] = null;
costSoFar[start] = 0;
//Debug.LogWarning("TS: while loop BEGIN");
float exitLoopTime = Time.time + 5f;
while (!frontier.IsEmpty)
{
if (Time.time > exitLoopTime)
{
Debug.LogWarning("TS: theta star timeout");
return(null);
}
//Debug.LogWarning("TS: while loop entered");
GridLocation current = frontier.DeleteMin();
//Debug.LogWarning("TS: while loop SetVertex");
SetVertex(bitmap, cameFrom, costSoFar, current);
if (current.Equals(goal))
{
//Debug.LogWarning("TS: current == goal");
return(GridSearch.RebuildPath(goal, cameFrom));
}
closedSet.Add(current);
foreach (GridLocation next in bitmap.Neighbours(current))
{
double computedCost = lazyComputeCost(cameFrom, costSoFar, current, next);
if (!costSoFar.ContainsKey(next) || computedCost < costSoFar[next])
{
cameFrom[next] = cameFrom[current];
costSoFar[next] = computedCost;
double p = computedCost + next.distanceTo(goal);
next.priority = p;
frontier.Add(next);
}
}
}
Debug.LogWarning("TS: returning null");
return(null);
}
