public void MinKey_WhenMultipleKeysAreInQueue_WillReturnMinimumKey()
{
IndexMinPriorityQueue <double> queue = new IndexMinPriorityQueue <double>(10);
queue.Insert(4, 12.5);
queue.Insert(3, 40.12);
queue.Insert(7, 4.3);
queue.Insert(2, 162.75);
double minKey = queue.MinKey();
Assert.AreEqual(4.3, minKey);
}
/// <summary>
/// Returns an List of Cells representing a shortest path from the specified source to the specified destination
/// </summary>
/// <param name="source">The source Cell to find a shortest path from</param>
/// <param name="destination">The destination Cell to find a shortest path to</param>
/// <param name="map">The Map on which to find the shortest path between Cells</param>
/// <returns>List of Cells representing a shortest path from the specified source to the specified destination</returns>
public List <TCell> FindPath(TCell source, TCell destination, IMap <TCell> map, Func <TCell, TCell, bool> ValidStep)
{
// OPEN = the set of nodes to be evaluated
IndexMinPriorityQueue <PathNode> openNodes = new IndexMinPriorityQueue <PathNode>(map.Height * map.Width);
// CLOSED = the set of nodes already evaluated
bool[] isNodeClosed = new bool[map.Height * map.Width];
// add the start node to OPEN
openNodes.Insert(map.IndexFor(source), new PathNode {
DistanceFromStart = 0,
HeuristicDistanceFromEnd = CalculateDistance(source, destination, _diagonalCost),
X = source.X,
Y = source.Y,
Parent = null
});
PathNode currentNode;
// loop
while (true)
{
// current = node in OPEN with the lowest f_cost
if (openNodes.Size < 1)
{
return(null);
}
currentNode = openNodes.MinKey();
// remove current from OPEN
int currentIndex = openNodes.DeleteMin();
// add current to CLOSED
isNodeClosed[currentIndex] = true;
ICell currentCell = map.CellFor(currentIndex);
// if current is the target node the path has been found
if (currentCell.Equals(destination))
{
break;
}
// foreach neighbor of the current node
bool includeDiagonals = _diagonalCost.HasValue;
foreach (TCell neighbor in map.GetAdjacentCells(currentCell.X, currentCell.Y, includeDiagonals))
{
int neighborIndex = map.IndexFor(neighbor);
// if neighbor is not walkable or neighbor is in CLOSED
if (!ValidStep(neighbor, destination) || isNodeClosed[neighborIndex])
{
// skip to the next neighbor
continue;
}
bool isNeighborInOpen = openNodes.Contains(neighborIndex);
// if neighbor is in OPEN
if (isNeighborInOpen)
{
// if new path to neighbor is shorter
PathNode neighborNode = openNodes.KeyAt(neighborIndex);
double newDistance = currentNode.DistanceFromStart + 1;
if (newDistance < neighborNode.DistanceFromStart)
{
// update neighbor distance
neighborNode.DistanceFromStart = newDistance;
// set parent of neighbor to current
neighborNode.Parent = currentNode;
}
}
else // if neighbor is not in OPEN
{
// set f_cost of neighbor
// set parent of neighbor to current
PathNode neighborNode = new PathNode {
DistanceFromStart = currentNode.DistanceFromStart + 1,
HeuristicDistanceFromEnd = CalculateDistance(source, destination, _diagonalCost),
X = neighbor.X,
Y = neighbor.Y,
Parent = currentNode
};
// add neighbor to OPEN
openNodes.Insert(neighborIndex, neighborNode);
}
}
}
List <TCell> path = new List <TCell>();
path.Add(map.GetCell(currentNode.X, currentNode.Y));
while (currentNode.Parent != null)
{
currentNode = currentNode.Parent;
path.Add(map.GetCell(currentNode.X, currentNode.Y));
}
path.Reverse();
return(path);
}
