public bool Contains(WayPoint wp)
{
return(route.Contains(wp));
}
public int IndexOf(WayPoint wp)
{
return(route.IndexOf(wp));
}
/// <summary>
/// Возвращает последовательность <typeparamref name="WayPoint"/>'ов, являющихся кратчайшим путём от <typeparamref name="WayPoint"/>'а <paramref name="from"/> к <typeparamref name="WayPoint"/>'у <paramref name="to"/>.
/// </summary>
/// <param name="from"><typeparamref name="WayPoint"/>, от которого строится путь.</param>
/// <param name="to"><typeparamref name="WayPoint"/>, к которому строится путь.</param>
/// <param name="back">Если <typeparamref name="true"/>, то построенный путь будет замкнутым, иначе - нет.</param>
/// <returns>Возвращает последовательность <typeparamref name="WayPoint"/>'ов, являющихся кратчайшим путём.</returns>
private static List <WayPoint> BFS(WayPoint from, WayPoint to, bool back)
{
Queue <WayPoint> queue = new Queue <WayPoint>();
HashSet <WayPoint> explored = new HashSet <WayPoint>();
Dictionary <WayPoint, WayPoint> parentsWP = new Dictionary <WayPoint, WayPoint>(64);
queue.Enqueue(from);
while (queue.Count > 0)
{
WayPoint current = queue.Dequeue();
if (current == to)
{
break;
}
WayPoint[] links = current.neighbours.Where(wp => wp != null).ToArray();
for (int i = 0; i < links.Length; i++)
{
WayPoint link = links[i];
if (!explored.Contains(link))
{
explored.Add(link);
parentsWP.Add(link, current);
queue.Enqueue(link);
}
}
if (queue.Count == 0)
{
return(null);
}
}
List <WayPoint> path = new List <WayPoint>();
WayPoint curr = to;
try
{
while (curr != from)
{
path.Add(curr);
curr = parentsWP[curr];
}
path.Add(from);
path.Reverse();
if (back)
{
//parentsWP.Clear();
WayPoint last = path[path.Count - 1];
WayPoint _from = last.neighbours[0] ?? last.neighbours[1];
List <WayPoint> backPath = BFS(_from, path[0], false);
backPath.Remove(backPath[backPath.Count - 1]);
path.AddRange(backPath);
}
//parentsWP.Clear();
return(path);
}
catch (KeyNotFoundException)
{
return(null);
}
}
