public bool A_StarAlgorithm(Node Start, Node Target)
{
Node CurrentNode = Start;
ActualNodes.Add(CurrentNode);
for (; ActualNodes.Count > 0;)
{
CurrentNode = ActualNodes[0];
if (CurrentNode.Equals(Target))
{
VisitedNodes.Add(Target);
for (int Index = 0; Index < VisitedNodes.Count - 1; Index++)
{
ShortesPath.Add(InnerGraph.FindEdge(VisitedNodes[Index], VisitedNodes[Index + 1]));
}
return(true);
}
ActualNodes.Remove(CurrentNode);
ClosedNodes.Add(CurrentNode);
foreach (Node Incomer in CurrentNode.Incomers.Where(Node => Node != null && Node.Index != Node.Incomers.Length - 1))
{
if (!ClosedNodes.Contains(Incomer))
{
if (!ActualNodes.Contains(Incomer))
{
Incomer[Incomer.Index] = CurrentNode;
Incomer.HeuristicCost = HeruisticPath(Incomer, Target);
Incomer.PastWayCost = InnerGraph.FindEdge(CurrentNode, Incomer).Weight;
Incomer.TotalPathCost = Incomer.PastWayCost + Incomer.HeuristicCost;
ActualNodes.Add(Incomer);
ActualNodes = ActualNodes.OrderBy(Node => Node.TotalPathCost).ToList <Node>();
}
}
}
VisitedNodes.Add(CurrentNode);
}
return(true);
}
public Node SelectNextNode(Node currentNode)
{
if (!IsInitialized)
{
throw new Exception("Grid is not initialized");
}
if (FindNeighbours(currentNode).Count <= 0)
{
return(OpenNodes.LastOrDefault());
}
var neighboursList = currentNode.Neighbours.Where(x => !ClosedNodes.Contains(x, nodeComparer)).ToList();
OpenNodes.AddRange(neighboursList.Where(x => !OpenNodes.Contains(x, nodeComparer)));
return(OpenNodes.OrderBy(x => x.Fn).ThenBy(x => x.Id).FirstOrDefault());
}
public async Task FindPathAsync(CancellationToken cancellationToken)
{
await Task.Factory.StartNew(() =>
{
if (grid.Algorithm == null)
{
throw new NullReferenceException("Scenario grid does not have an assigned pathfinding algorithim!");
}
while (true)
{
if (OpenNodes.Count == 1 && OpenNodes[0] == grid.StartingNode)
{
CurrentNode = OpenNodes[0];
}
else
{
CurrentNode = GetNodeWithLowestFCost(OpenNodes);
}
Thread.Sleep(VisualizationTime);
OpenNodes.Remove(CurrentNode);
ClosedNodes.Add(CurrentNode);
Thread.Sleep(VisualizationTime);
if (CurrentNode == grid.TargetNode)
{
SetSolution(cancellationToken);
PathfindingFinished?.Invoke(this, null);
return;
}
neighbouringNodes = GetNeighbouringNodes(CurrentNode, allowDiagonals: AllowDiagonalMovement);
foreach (Node n in neighbouringNodes)
{
if (cancellationToken.IsCancellationRequested)
{
ClearPath();
PathfindingFinished?.Invoke(this, null);
return;
}
if (!n.IsTraversable || ClosedNodes.Contains(n))
{
continue;
}
if (IsNewPathShorter(CurrentNode, n) || !OpenNodes.Contains(n))
{
n.GCost = CurrentNode.GCost + GetDistance(CurrentNode, n);
n.HCost = GetDistance(n, grid.TargetNode);
n.Parent = CurrentNode;
if (!OpenNodes.Contains(n))
{
OpenNodes.Add(n);
}
}
Thread.Sleep(VisualizationTime / 3);
}
}
});
}
