/// <summary>
/// Searches for shortest path in the town using path graph. Uses Dijkstra 1-1 form (A*).
/// </summary>
/// <param name="from">Source position</param>
/// <param name="to">Target position</param>
/// <param name="keepInTheSameQuarter">Indicator whether the searching has to use only vertices from the same quarter as the from position is in</param>
/// <returns>Set of vertices forming result path</returns>
public static IEnumerable <PathGraphVertex> FindShortestPath(PositionInTown from, PositionInTown to, bool keepInTheSameQuarter)
{
PathGraphVertex start = from.Quarter.FindNearestPathGraphVertex(from.PositionInQuarter);
PathGraphVertex end = to.Quarter.FindNearestPathGraphVertex(to.PositionInQuarter);
return(FindShortestPath(start, end, keepInTheSameQuarter));
}
/// <summary>
/// Scans the graph whether it is connected.
/// </summary>
/// <param name="graph">The set of vertices that have pointer to their neighbor vertices</param>
/// <returns>True if the graph is connected</returns>
public static bool IsConnected(IEnumerable <PathGraphVertex> graph)
{
Dictionary <PathGraphVertex, bool> visited = new Dictionary <PathGraphVertex, bool>();
foreach (var v in graph)
{
if (!visited.ContainsKey(v))
{
visited.Add(v, false);
}
}
if (visited.Count == 0)
{
return(true);
}
int visitedCount = 0;
Queue <PathGraphVertex> queue = new Queue <PathGraphVertex>();
queue.Enqueue(graph.First());
while (queue.Count != 0)
{
PathGraphVertex current = queue.Dequeue();
if (!visited[current])
{
visited[current] = true;
visitedCount++;
foreach (var n in current.Neighbors)
{
queue.Enqueue(n);
}
}
}
return(visitedCount == visited.Count);
}
/// <summary>
/// Calculates length of specified path.
/// </summary>
/// <param name="path">Path vertices</param>
/// <returns>Absolute length in meters</returns>
public static float GetLengthOfPath(IEnumerable <PathGraphVertex> path)
{
float length = 0;
PathGraphVertex prev = null;
foreach (PathGraphVertex v in path)
{
if (prev != null)
{
length += prev.GetDistanceToNeighbor(v);
}
prev = v;
}
return(length);
}
/// <summary>
/// Gets distance to specified neighbor.
/// </summary>
/// <param name="neighbor">The tested vertex</param>
/// <returns>Distance in meters</returns>
public float GetDistanceToNeighbor(PathGraphVertex neighbor)
{
return(distances[neighbor]);
}
/// <summary>
/// Adds edges between this and the given neighbor vertex in both directions.
/// </summary>
/// <param name="neighbor">The neighbor vertex</param>
/// <param name="distance">The real distance between this vertex and the neighbor one</param>
public void AddNeighborBothDirection(PathGraphVertex neighbor, float distance)
{
AddNeighbor(neighbor, distance);
neighbor.AddNeighbor(this, distance);
}
/// <summary>
/// Adds a neighbor to this vertex.
/// </summary>
/// <param name="neighbor">The neighbor vertex</param>
/// <param name="distance">The real distance between this vertex and the neighbor one</param>
public void AddNeighbor(PathGraphVertex neighbor, float distance)
{
neightbors.Add(neighbor);
distances.Add(neighbor, distance);
}
/// <summary>
/// Searches for shortest path in the graph. Uses Dijkstra 1-1 form (A*).
/// </summary>
/// <param name="from">Source vertex</param>
/// <param name="to">Target vertex</param>
/// <param name="keepInTheSameQuarter">Indicator whether the searching has to use only vertices from the same quarter as the from position is in</param>
/// <returns>Set of vertices forming result path</returns>
public static IEnumerable <PathGraphVertex> FindShortestPath(PathGraphVertex from, PathGraphVertex to, bool keepInTheSameQuarter)
{
keepInTheSameQuarter = keepInTheSameQuarter && from.Position.Quarter == to.Position.Quarter;
TownQuarter fromQuarter = from.Position.Quarter;
LinkedList <PathGraphVertex> resultPath = new LinkedList <PathGraphVertex>();
HashSet <PathGraphVertex> closed = new HashSet <PathGraphVertex>();
Dictionary <PathGraphVertex, PathGraphVertex> cameFrom = new Dictionary <PathGraphVertex, PathGraphVertex>();
Dictionary <PathGraphVertex, float> gScore = new Dictionary <PathGraphVertex, float>();
gScore.Add(from, 0);
Dictionary <PathGraphVertex, float> fScore = new Dictionary <PathGraphVertex, float>();
fScore.Add(from, gScore[from] + HeuristicDistance(from, to));
HashSet <PathGraphVertex> open = new HashSet <PathGraphVertex>();
open.Add(from);
while (open.Count != 0)
{
float lowestScore = float.MaxValue;
PathGraphVertex lowestVertex = null;
foreach (PathGraphVertex openedOne in open)
{
float score = fScore[openedOne];
if (score < lowestScore)
{
lowestVertex = openedOne;
}
}
PathGraphVertex current = lowestVertex;//open.OrderBy(x => fScore[x]).First();
if (current == to)
{
PathGraphVertex t = to;
while (t != from)
{
resultPath.AddFirst(t);
t = cameFrom[t];
}
resultPath.AddFirst(from);
return(resultPath);
}
open.Remove(current);
closed.Add(current);
foreach (PathGraphVertex n in current.Neighbors)
{
if (closed.Contains(n) || (keepInTheSameQuarter && n.Position.Quarter != fromQuarter))
{
continue;
}
else
{
float tempGSore = gScore[current] + current.GetDistanceToNeighbor(n);
if (!open.Contains(n) || tempGSore <= gScore[n])
{
cameFrom.SetValue(n, current);
gScore.SetValue(n, tempGSore);
fScore.SetValue(n, gScore[n] + HeuristicDistance(current, n));
if (!open.Contains(n))
{
open.Add(n);
}
}
}
}
}
throw new PathNotFoundException("Source and target vertices aren't in the same component.");
}
static float HeuristicDistance(PathGraphVertex u, PathGraphVertex v)
{
return(u.Position.MinimalDistanceTo(v.Position));
}