private void Relax(DirectedEdge edge)
{
int v = edge.From;
int w = edge.To;
if (_distanceTo[w] > _distanceTo[v] + edge.Weight)
{
_distanceTo[w] = _distanceTo[v] + edge.Weight;
_edgeTo[w] = edge;
if (_priorityQueue.Contains(w))
{
_priorityQueue.DecreaseKey(w, _distanceTo[w]);
}
else
{
_priorityQueue.Insert(w, _distanceTo[w]);
}
}
}
/// <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)
{
// 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 (neighbor.IsWalkable == false || 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);
}