/// <summary>
/// Algorytm A* znajdowania najkrotszej sciezki na planszy.
/// </summary>
/// <param name="from">Pole startowe</param>
/// <param name="to">Pole, ktore chcemy osiagnac</param>
/// <param name="heuristic">Metryka w jakiej maja byc szacowana odleglosc do pola docelowego</param>
/// <param name="forbidenFields">Pola przez które nie wolno przejść przy danym poszukiwaniu ścieżki</param>
/// <returns>Lista pol skladajaca sie na sciezke</returns>
public static IList <MapField> AStar(MapField from, MapField to, heuristicFunc heuristic, ref AStarDiagnostic diagnostic, params MapField[] forbidenFields)
{
if ((from == null) || (to == null))
{
throw new NullReferenceException(); // ktores z pol nie istnieje
}
if (!to.IsAvaliable)
{
throw new FieldNotAvaliableException(); // pole jest niedostepne
}
if (from.Neighbour.Contains(to))
{
return new List <MapField>(2)
{
from, to,
}
}
; // pola from i to leza kolo siebie
//Lita pol do przeszukania
List <PathFindingNode> openList = new List <PathFindingNode> {
new PathFindingNode(from)
};
//Lista przeszukanych pol
List <PathFindingNode> closeList = new List <PathFindingNode>(forbidenFields.Select(forbidenField => new PathFindingNode(forbidenField)));
bool reachedGoal = false;
//dopoki jakiekolwiek pole moze zostac jeszcze przebadane
while (openList.Count != 0)
{
++diagnostic.Iterations;
PathFindingNode current = openList.RemoveAtAndGet(0);
closeList.Add(current);
if (heuristic == NullDistance && current.This == to)
{
return(ReconstructPath(current, ref diagnostic));
}
else if (current.This == to) // znaleziono pole do ktorego dazylismy
{
reachedGoal = true;
}
else if (reachedGoal)
{
PathFindingNode endNode = openList.Concat(closeList)
.First(node => node.This == to);
float minValInOpenList = openList.Select(node => node.CostFromStart)
.Min();
if (minValInOpenList < endNode.CostFromStart)
{
return(ReconstructPath(endNode, ref diagnostic));
}
}
foreach (PathFindingNode neighbourNode in current.This.Neighbour.Where(neighbour => neighbour.IsAvaliable)
.Select(neighbour => new PathFindingNode(neighbour)
{
Parent = current,
CostFromStart = current.CostFromStart +
EuclideanDistance(current.This, neighbour) * current.This.Cost,
CostToEnd = heuristic(neighbour, to)
}))
{
//Zmiana parametrow sciezki jesli droga do sasiada jest krotsza z obecnego pola niz ustalona wczesniej
if (heuristic != NullDistance && closeList.Contains(neighbourNode))
{
PathFindingNode oldNode = closeList[closeList.IndexOf(neighbourNode)];
if (neighbourNode.CostFromStart < oldNode.CostFromStart)
{
closeList.Remove(oldNode);
openList.Add(neighbourNode);
}
}
else if (openList.Contains(neighbourNode))
{
PathFindingNode oldNode = openList[openList.IndexOf(neighbourNode)];
if (neighbourNode.CostFromStart < oldNode.CostFromStart)
{
oldNode.CostFromStart = neighbourNode.CostFromStart;
oldNode.Parent = neighbourNode.Parent;
}
}
else
{
openList.Add(neighbourNode);
}
}
//openList.AsParallel().WithDegreeOfParallelism(4).OrderBy(field => field.AllCost);
openList.Sort();
}
//jesli niedotarto do zadanego celu
throw new FieldNotAvaliableException();
}
public static List <NVector2> CalculatePath(Vector3 startPos, Vector3 endPos, heuristicFunc heuristicFunc, int depth = 10)
{
return(new List <NVector2>());
}
