/// <summary>
/// Do the work
/// </summary>
protected internal virtual void determine_shortest_paths(BaseVertex source_vertex, BaseVertex sink_vertex, bool is_source2sink)
{
// 0. clean up variables
clear();
// 1. initialize members
BaseVertex end_vertex = is_source2sink ? sink_vertex : source_vertex;
BaseVertex start_vertex = is_source2sink ? source_vertex : sink_vertex;
_start_vertex_distance_index[start_vertex] = 0d;
start_vertex.set_weight(0d);
_vertex_candidate_queue.Enqueue(start_vertex, 0);
// 2. start searching for the shortest path
while (_vertex_candidate_queue.Count() != 0)
{
BaseVertex cur_candidate = _vertex_candidate_queue.Dequeue();
if (cur_candidate.Equals(end_vertex))
{
break;
}
_determined_vertex_set.Add(cur_candidate);
_improve_to_vertex(cur_candidate, is_source2sink);
}
}
public void Reschedule(Event e)
{
if (queue.Count(ev => ev == e) != 1)
{
throw new Exception("Double event?");
}
queue.UpdatePriority(e, e.time);
}
public int Count(Predicate <T> predicate)
{
return(_queue.Count(predicate.Invoke));
}
/*****************************************************************/
// Methods
/*****************************************************************/
#region Methods
/// <summary>
/// Finds a path on the given grid and returns the path, beginning with the given start cell.
/// </summary>
/// <param name="start">A vertex to begin the search at. </param>
/// <param name="goal">A vertex to end the search at. </param>
/// <param name="grid">The grid to search on. </param>
/// <returns></returns>
public static IEnumerable <T> GetPath <T>(T start, T goal, IGraph <T> grid, float costDiagonal = 1.4F) where T : Vertex
{
SimplePriorityQueue <T> frontier = new SimplePriorityQueue <T>();
List <T> lPath = new List <T>();
Dictionary <T, T> cameFrom = new Dictionary <T, T>();
Dictionary <T, float> costSoFar = new Dictionary <T, float>();
costSoFar.Add(start, 0);
frontier.Enqueue(start, 0);
cameFrom.Add(start, null);
T current = null;
// Traverse map.
while (frontier.Count() != 0)
{
current = frontier.Dequeue();
if (current == goal) // Reached goal destination.
{
break;
}
IEnumerable <T> neighbors = grid.GetNeighbors(current);
for (int next = 0; next < neighbors.Count(); next++)
{
T neighbor = neighbors.ElementAt(next);
if (neighbor.impassable) // Looking at impassable tile.
{
continue;
}
// Get cost.
float newCost = 0.0F;
costSoFar.TryGetValue(current, out newCost);
newCost += grid.GetCost(current, neighbor);
if (!costSoFar.ContainsKey(neighbor) || newCost < costSoFar[neighbor])
{
if (costSoFar.ContainsKey(neighbor))
{
costSoFar[neighbor] = newCost;
}
else
{
costSoFar.Add(neighbor, newCost);
}
float priority = newCost + grid.GetHeuristic(goal, neighbor);
frontier.Enqueue(neighbor, priority);
if (cameFrom.ContainsKey(neighbor))
{
cameFrom[neighbor] = current;
}
else
{
cameFrom.Add(neighbor, current);
}
}
}
}
return(GraphUtility.ConstructPath(cameFrom, goal));
}
/// <summary>
/// Finds a path on the given grid and returns the path, beginning with the given start cell.
/// </summary>
/// <typeparam name="T">The "Vertex" class or a class inheriting from the "Vertex" class. </typeparam>
/// <param name="start">A vertex to begin the search at. </param>
/// <param name="goal">A vertex to end the search at. Will be ignored, if null. </param>
/// <param name="grid">The grid to search on. </param>
/// <param name="breakEarly">If true, will stop searching after reaching the goal. </param>
/// <returns></returns>
internal static Dictionary <T, T> GetPath <T>(T start, T goal, IGraph <T> grid, bool breakEarly) where T : Vertex
{
SimplePriorityQueue <T> frontier = new SimplePriorityQueue <T>();
List <T> lPath = new List <T>();
Dictionary <T, T> cameFrom = new Dictionary <T, T>();
Dictionary <T, float> costSoFar = new Dictionary <T, float>();
costSoFar.Add(start, 0);
frontier.Enqueue(start, 0);
cameFrom.Add(start, null);
T current = null;
// Traverse map.
while (frontier.Count() != 0)
{
current = frontier.Dequeue();
if (goal != null && current == goal && breakEarly) // Reached goal destination.
{
break;
}
IEnumerable <T> neighbors = grid.GetNeighbors(current);
for (int next = 0; next < neighbors.Count(); next++)
{
T neighborNext = neighbors.ElementAt(next);
if (neighborNext.impassable) // Looking at impassable tile.
{
continue;
}
float newCost = 0.0F;
costSoFar.TryGetValue(current, out newCost);
newCost += grid.GetCost(current, neighborNext);
if (!costSoFar.ContainsKey(neighborNext) || newCost < costSoFar[neighborNext])
{
if (costSoFar.ContainsKey(neighborNext))
{
costSoFar[neighborNext] = newCost;
}
else
{
costSoFar.Add(neighborNext, newCost);
}
float priority = newCost;
frontier.Enqueue(neighborNext, priority);
if (cameFrom.ContainsKey(neighborNext))
{
cameFrom[neighborNext] = current;
}
else
{
cameFrom.Add(neighborNext, current);
}
}
}
}
return(cameFrom);
}