public bool ColorPath(Vector3 start, Vector3 end, out List <Vector3> path) //Dijkstra
{
C5.IntervalHeap <Vertex> openList = new C5.IntervalHeap <Vertex>();
path = new List <Vector3>(); //Possibly should be null when return false;
Vertex begin = Vertices[start];
Vertex target = Vertices[end];
begin.Predecessor = null;
graphSearch++;
openList.Add(begin);
bool pathFound = false;
int count = 0;
while (!openList.IsEmpty)
{
Vertex v = openList.DeleteMin();
count++;
if (v == target)
{
pathFound = true;
break;
}
v.searchEnd = graphSearch;
foreach (Vertex n in v.Neighbors)
{
if (n.searchEnd == graphSearch)
{
continue;
}
if (n.searchInit < graphSearch)
{
n.SearchVal = float.PositiveInfinity;
n.Heur = Vector3.Distance(n.Pos, end);
n.searchInit = graphSearch;
}
float pathVal = v.SearchVal + Vector3.Distance(v.Pos, n.Pos);
if (pathVal < n.SearchVal)
{
n.SearchVal = pathVal;
n.Predecessor = v;
openList.Add(n);
}
}
}
if (!pathFound)
{
return(false);
}
//Color - Collecting path
Vertex cur = target;
while (cur != null)
{
path.Add(cur.Pos);
//cur.Rep.GetComponent<Renderer>().material.color = Color.green;
cur = cur.Predecessor;
}
path.Reverse();
return(true);
}
public static List <GridLocation> run(Bitmap bitmap, GridLocation start, GridLocation goal,
ComputeCost computeCost)
{
//Debug.LogWarning("GS: setup dictionaries");
Dictionary <GridLocation, GridLocation> cameFrom = new Dictionary <GridLocation, GridLocation>();
Dictionary <GridLocation, double> costSoFar = new Dictionary <GridLocation, double>();
//Debug.LogWarning("GS: setup interval heap");
var frontier = new C5.IntervalHeap <GridLocation>();
start.priority = 0;
frontier.Add(start);
cameFrom[start] = null;
costSoFar[start] = 0;
//Debug.LogWarning("GS: while loop BEGIN");
float exitLoopTime = Time.time + 5f;
while (!frontier.IsEmpty)
{
if (Time.time > exitLoopTime)
{
Debug.LogWarning("GS: grid search timeout");
return(null);
}
//Debug.LogWarning("GS: while loop entered");
var current = frontier.DeleteMin();
if (current.Equals(goal))
{
//Debug.LogWarning("GS: current == goal");
return(RebuildPath(goal, cameFrom));
}
foreach (GridLocation next in bitmap.Neighbours(current))
{
bool usingLOS = false;
double computedCost = computeCost(current, next, cameFrom, costSoFar, ref usingLOS, bitmap);
if (!costSoFar.ContainsKey(next) || computedCost < costSoFar[next])
{
if (usingLOS)
{
cameFrom[next] = cameFrom[current];
}
else
{
cameFrom[next] = current;
}
costSoFar[next] = computedCost;
double p = computedCost + next.distanceTo(goal);
next.priority = p;
frontier.Add(next);
}
}
}
Debug.LogWarning("GS: returning null");
return(null);
}
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);
}