public override List <Vertex> FindShortestPath(Vertex startNode, Vertex endNode, ref float pathLength)
{
List <Vertex> vertexList = this.graph.Verticies;
int size = vertexList.Count();
//Dictionary<Vertex, float> dist = new Dictionary<Vertex, float>();
Models.PriorityQueues.MinKHeap <Vertex> q = new Models.PriorityQueues.MinKHeap <Vertex>(this.graph.Verticies.Count);
for (int i = 0; i < size; i++)
{
Vertex node = vertexList[i];
float distance = 0;
if (!vertexList[i].Equals(startNode))
{
distance = Int32.MaxValue;
}
node.CostFromStart = distance;
vertexList[i].Previous = null;
q.Enqueue(node, distance);
}
while (q.Count > 0)
{
Vertex node = q.Dequeue();
if (node.Equals(endNode))
{
return(GetPathResult(endNode, ref pathLength));
}
foreach (var neighbor in node.Neighbors)
{
float newDistance = neighbor.Value.Weight + node.CostFromStart;
if (q.Contains(neighbor.Key) && newDistance < neighbor.Key.CostFromStart)
{
neighbor.Key.CostFromStart = newDistance;
neighbor.Key.Previous = node;
q.UpdatePriority(neighbor.Key, newDistance);
}
}
}
return(null);
}
public override List <Vertex> FindShortestPath(Vertex startNode, Vertex endNode, ref float pathLength)
{
Models.PriorityQueues.MinKHeap <Vertex> openList = new Models.PriorityQueues.MinKHeap <Vertex>(graph.Verticies.Count);
HashSet <Vertex> closedList = new HashSet <Vertex>();
startNode.Update(0.0f, Edge.GetMinimumDistance(startNode.Coordinates, endNode.Coordinates), null);
var currentVertex = startNode;
while (currentVertex != null)
{
closedList.Add(currentVertex); // Put it in "done" pile
//// If current vertex is the target then we are done
if (currentVertex.Equals(endNode))
{
return(GetPathResult(endNode, ref pathLength));
}
foreach (var exit in currentVertex.Neighbors) // For each node adjacent to the current node
{
Vertex reachableVertex = exit.Key;
// If the closed list already searched this vertex, skip it
if (!closedList.Contains(reachableVertex))
{
float edgeCost = exit.Value.Weight;
if (edgeCost <= 0.0) // Are positive values that are extremely close to 0 going to be a problem?
{
throw new ArgumentException("The A* algorithm is only valid for edge costs greater than 0");
}
float costFromStart = currentVertex.CostFromStart + edgeCost;
bool isShorterPath = costFromStart < reachableVertex.CostFromStart;
if (!openList.Contains(reachableVertex) || isShorterPath)
{
float estimatedCostFromEnd = exit.Key.Coordinates.Equals(endNode.Coordinates) ? 0.0f
: Edge.GetMinimumDistance(reachableVertex.Coordinates, endNode.Coordinates);
reachableVertex.Update(costFromStart, estimatedCostFromEnd, currentVertex);
if (!openList.Contains(reachableVertex))
{
openList.Enqueue(reachableVertex, estimatedCostFromEnd + costFromStart);
}
else
{
openList.UpdatePriority(reachableVertex, estimatedCostFromEnd + costFromStart);
}
}
}
}
if (openList.Count > 0)
{
currentVertex = openList.Dequeue();
}
else
{
currentVertex = null;
}
}
return(null); // No path between the start and end nodes
}