//////////////////////////////////////////////////////////////////////////
private void _ImplementPathfinding(int checks)
{
// do while has variants
while (checks-- > 0)
{
// not found
if (OpenSet.Count == 0)
{
Complete(FindPathResult.NotReachable);
return;
}
// limit achieved
if (ClosedSet.Count >= Options.CheckLimit)
{
Complete(FindPathResult.Overwhelm);
return;
}
// get next node
var currentNode = OpenSet.First();
// close current, move from open to close set
OpenSet.Remove(currentNode);
ClosedSet.Add(currentNode.Master);
ClosedNodes.AddLast(currentNode);
// goal check
if (Goal.Any(n => n.Equals(currentNode.Master)))
{
getPath(currentNode, Path);
Complete(FindPathResult.Found);
return;
}
// proceed connections
foreach (var neighborNode in Explorer.GetNeighbours(currentNode.Master))
{
// skip if not passable
if (neighborNode == null || Explorer.Passable(neighborNode) == false)
{
continue;
}
// IsClosed, skip if already checked
if (ClosedSet.Contains(neighborNode))
{
continue;
}
var pathCost = currentNode.PathCost + Explorer.GetPathCost(currentNode.Master, neighborNode);
var openNode = OpenSet.FirstOrDefault(pathNode => pathNode.Master.Equals(neighborNode));
if (openNode != null)
{
// if presented and part is shorter, then reset his parent and cost
if (openNode.PathCost > pathCost)
{
openNode.СameFrom = currentNode;
openNode.PathCost = pathCost;
// update priority
openNode.Cost = openNode.PathCostEstimated + openNode.PathCost;
OpenSet.UpdatePriority(openNode, openNode.Cost);
}
}
else
{
// if not presented, add as variant
var pathNode = new PathNode <T>(neighborNode);
pathNode.СameFrom = currentNode;
pathNode.PathCost = pathCost;
if (Options.Heuristic)
{
pathNode.PathCostEstimated = getShortestPath(Explorer, pathNode.Master, Goal);
pathNode.Cost = pathNode.PathCostEstimated + pathNode.PathCost;
}
else
{
pathNode.Cost = pathNode.PathCost;
}
OpenSet.Enqueue(pathNode, pathNode.Cost);
}
}
}
/////////////////////////////////////
float getShortestPath(iExplorer <T> explorer, T start, IEnumerable <T> goal)
{
var shortestPath = float.MaxValue;
// ReSharper disable once ForeachCanBeConvertedToQueryUsingAnotherGetEnumerator
foreach (var n in goal)
{
var currentShortestPath = explorer.GetShortestPath(start, n);
if (shortestPath > currentShortestPath)
{
shortestPath = currentShortestPath;
}
}
return(shortestPath);
}
void getPath(PathNode <T> pathNode, LinkedList <T> path)
{
for (var currentNode = pathNode; currentNode != null; currentNode = currentNode.СameFrom)
{
path.AddFirst(currentNode.Master);
}
}
}
public void FindPath(Object obj)
{
try
{
KeyValuePair <Point, Spot> FirstPair = Spots.First();
FirstPair.Value.GScore = 0;
FirstPair.Value.HScore = PointFunctions.DistanceTo(FirstPair.Value.Point, End);
OpenSet.Add(FirstPair);
while (OpenSet.Count > 0)
{
Spot Current = OpenSet.ToList().OrderBy(p => p.Value.FScore).First().Value;
Console.WriteLine("Current point is: " + Current.Point.ToString());
Invalidate();
if (Current.Point == End)
{
Current.Color = Color.Blue;
bool Searching = true;
while (Searching)
{
if (Current.CameFrom.Value != null)
{
Current.CameFrom.Value.Color = Color.Blue;
Current.CameFrom.Value.OnBestPath = true;
Current = Current.CameFrom.Value;
}
else
{
Searching = false;
}
}
Spots.ToList().ForEach(s =>
{
if (!s.Value.OnBestPath && !s.Value.IsObstacle)
{
s.Value.Color = Color.White;
Invalidate();
}
});
Invalidate();
Console.WriteLine("Done!");
break;
}
ClosedSet.Add(Current.Point, Current);
OpenSet.Remove(Current.Point);
int tempG = 0;
foreach (KeyValuePair <Point, Spot> neighbour in Current.Neibhours)
{
if (ClosedSet.Contains(neighbour) || neighbour.Value.IsObstacle)
{
continue;
}
tempG = Current.GScore + PointFunctions.DistanceTo(Current.Point, neighbour.Key);
if (!OpenSet.Contains(neighbour))
{
OpenSet.Add(neighbour);
}
else if (tempG >= neighbour.Value.GScore)
{
continue;
}
neighbour.Value.GScore = tempG;
neighbour.Value.HScore = PointFunctions.DistanceTo(neighbour.Key, End);
neighbour.Value.CameFrom = new KeyValuePair <Point, Spot>(Current.Point, Current);
}
Invalidate();
}
}
catch (Exception ex)
{
Console.WriteLine(ex.StackTrace);
System.Diagnostics.Debugger.Break();
throw;
}
}
