public Queue <ICell> GenerateOptimalSquarePath(ISquareMap map, IVehicle vehicle)
{
var currentPostion = vehicle.CurrentSquareCell;
var currentHeading = GlobalDirection.East;
var finished = false;
var path = new List <ICell>();
while (!finished)
{
//Heading East
if (currentHeading == GlobalDirection.East)
{
path.AddRange(
map.GetShortestPath(currentPostion, map.GetCell(map.Width - 1, currentPostion.Y)));
currentPostion = new Cell(map.Width - 1, currentPostion.Y);
}
//Heading West
else if (currentHeading == GlobalDirection.West)
{
path.AddRange(
map.GetShortestPath(currentPostion, map.GetCell(0, currentPostion.Y)));
currentPostion = new Cell(0, currentPostion.Y);
}
//Check for finish
if (currentPostion.Y + (vehicle.DetectorRadius * 2) >= map.Height)
{
finished = true;
continue;
}
//Move up X Axis
path.AddRange(
map.GetShortestPath(
currentPostion,
map.GetCell(currentPostion.X, currentPostion.Y + (vehicle.DetectorRadius * 2) - 1)));
currentPostion = new Cell(currentPostion.X, currentPostion.Y + (vehicle.DetectorRadius * 2) - 1);
if (currentHeading == GlobalDirection.East)
{
currentHeading = GlobalDirection.West;
}
else
{
currentHeading = GlobalDirection.East;
}
}
return(new Queue <ICell>(path));
}
public void GenerateReactiveSquarePath(ISquareMap squareMap, Queue <ICell> optimalPath,
ICell vehicleCurrentSquareCell)
{
//Clean out old path
_reactiveSquarePath.Clear();
//If the optimal path is empty well we are done, otherwise pop 1
if (!optimalPath.TryDequeue(out ICell convergentPoint))
{
return;
}
//Dequeue until the cell is not blocked
while (squareMap.GetCell(convergentPoint.X, convergentPoint.Y).Blocked)
{
//if optimal path is empty were done and we can not clear around this position, otherwise
//lest pop another.
if (!optimalPath.TryDequeue(out convergentPoint))
{
return;
}
}
var map = (SquareMap)squareMap;
var reactivePath = map.GetShortestPath(vehicleCurrentSquareCell, convergentPoint);
foreach (var node in reactivePath)
{
_reactiveSquarePath.Enqueue(node);
}
}
public static List <ICell> FindShortestPath(ISquareMap graph, ICell start, ICell goal)
{
var costSoFar = new Dictionary <ICell, int>();
var cameFrom = new Dictionary <ICell, ICell>();
var frontier = new PriorityQueue <ICell>();
frontier.Enqueue(start, 0);
cameFrom[start] = start;
costSoFar[start] = 0;
while (frontier.Count > 0)
{
var current = frontier.Dequeue();
if (current.Equals(goal))
{
break;
}
foreach (var next in graph.GetPassableNeighbors(current))
{
var newCost = costSoFar[current] + (int)graph.GetCell(next.X, next.Y).Coverage;
if (costSoFar.ContainsKey(next) && newCost >= costSoFar[next])
{
continue;
}
costSoFar[next] = newCost;
var priority = newCost + Heuristic(next, goal);
frontier.Enqueue(next, priority);
cameFrom[next] = current;
}
}
var path = new List <ICell>();
var pathWasNotFound = !cameFrom.ContainsKey(goal);
// Returning an empty list if the path wasn't found
if (pathWasNotFound)
{
return(path);
}
// Reconstructing path
var curr = goal;
while (!curr.Equals(start))
{
path.Add(curr);
curr = cameFrom[curr];
}
// Reverse it to start at actual start point
path.Reverse();
return(path);
}