private void Visit(EdgeWeightedGraph g, int v)
{
marked[v] = true;
foreach (var e in g.Adj(v))
{
int w = e.Other(v);
if (marked[w])
{
continue;
}
if (e.Weight < distTo[w])
{
edgeTo[w] = e;
distTo[w] = e.Weight;
if (pq.Contains(w))
{
pq.ChangeKey(w, distTo[w]);
}
else
{
pq.Insert(w, distTo[w]);
}
}
}
}
private void Scan(EdgeWeightedGraph G, int v)
{
_marked[v] = true;
foreach (Edge e in G.Adj(v))
{
int w = e.Target(v);
if (_marked[w])
{
continue;
}
if (e.Weight() < _distTo[w])
{
_distTo[w] = e.Weight();
_edgeTo[w] = e;
if (_pq.Contains(w))
{
_pq.ChangeKey(w, _distTo[w]);
}
else
{
_pq.Insert(w, _distTo[w]);
}
}
}
}
public void Contains_Index4AfterInsertingKeyAtIndex4_WillBeTrue()
{
IndexMinPriorityQueue <double> queue = new IndexMinPriorityQueue <double>(10);
queue.Insert(4, 12.5);
Assert.IsTrue(queue.Contains(4));
}
List <List <int> > GetMST()
{
var size = nodes.NodeList.Count;
var parents = new int[size];
var weights = new float[size];
var marked = new bool[size];
var pq = new IndexMinPriorityQueue <float>(size);
for (var x = 1; x < size; x++)
{
weights[x] = float.MaxValue;
}
weights[0] = 0;
pq.Insert(0, 0);
while (!pq.IsEmpty())
{
int v = pq.DeleteMin();
marked[v] = true;
for (var x = 0; x < size; x++)
{
if (x != v && !marked[x])
{
var distance = Vector2.Distance(nodes.NodeList[v], nodes.NodeList[x]);
if (distance < weights[x])
{
weights[x] = distance;
parents[x] = v;
if (pq.Contains(x))
{
pq.ChangeKey(x, distance);
}
else
{
pq.Insert(x, distance);
}
}
}
}
}
var result = GetAdjacencyLists(size);
for (var x = 1; x < parents.Length; x++)
{
var target = parents[x];
result[x].Add(target);
result[target].Add(x);
}
return(result);
}
private void Relax(DirectedEdge edge)
{
uint v = edge.From;
uint w = edge.To;
if (_distanceTo[w] > _distanceTo[v] + edge.Weight)
{
_distanceTo[w] = _distanceTo[v] + edge.Weight;
_edgeTo[w] = edge;
if (_priorityQueue.Contains((int)w))
{
_priorityQueue.DecreaseKey((int)w, _distanceTo[w]);
}
else
{
_priorityQueue.Insert((int)w, _distanceTo[w]);
}
}
}
private void Relax(EdgeWeightedDigraph g, int v)
{
foreach (var e in g.Adj(v))
{
int w = e.To;
if (distTo[w] > distTo[v] + e.Weight)
{
distTo[w] = distTo[v] + e.Weight;
edgeTo[w] = e;
if (pq.Contains(w))
{
pq.ChangeKey(w, distTo[w]);
}
else
{
pq.Insert(w, distTo[w]);
}
}
}
}
private void Visit(EdgeWeightedGraph graph, int vertex, ISet <int> excludes)
{
Marked[vertex] = true;
foreach (var edge in graph.Edges[vertex].Where(edge => !excludes.Contains(edge.Id)))
{
var other = edge.OtherVertex(vertex);
if (Marked[other] || edge.Weight >= DistTo[other])
{
continue;
}
EdgeTo[other] = edge;
DistTo[other] = edge.Weight;
if (_priorityQueue.Contains(other))
{
_priorityQueue.Change(other, DistTo[other]);
}
else
{
_priorityQueue.Insert(other, DistTo[other]);
}
}
}
/// <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);
}
