public void AddNodes(List <Node> Nodes)
{
foreach (Node B in Nodes)
{
if (!AllNodes.Contains(B))
{
OpenNodes.Add(H.Evaluate(B), B);
AllNodes.Add(B);
}
else
{
int keyIndex;
if (ClosedNodes.ContainsValue(B))
{
keyIndex = ClosedNodes.IndexOfValue(B);
}
else
{
keyIndex = OpenNodes.IndexOfValue(B);
}
int key = ClosedNodes.Keys[keyIndex];
if (H.Evaluate(B) < key)
{
UpdateKey(B, key - H.Evaluate(B));
}
}
}
}
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 List <Node> FindPath(Node start, Node end)
{
if (!IsInitialized)
{
throw new Exception("First you need to Initialize the grid.");
}
if (start?.IsUnavailable == true || end?.IsUnavailable == true)
{
throw new Exception("Start currentNode or End currentNode is unavailable. Pick others.");
}
StartNode = start;
EndNode = end;
CalculateHeruisticFromNode(StartNode);
OpenNodes.Add(StartNode);
var currentNode = StartNode;
while (currentNode != null && !currentNode.Equals(EndNode))
{
ClosedNodes.Add(currentNode);
OpenNodes.Remove(currentNode);
currentNode = SelectNextNode(currentNode);
}
if (currentNode == null)
{
throw new Exception("Cannot reach the end.");
}
var pathList = new List <Node>();
var nod = currentNode;
nod.FindNeighbours(this);
while (nod != null && !nod.Equals(StartNode))
{
pathList.Add(nod);
if (!nod.Neighbours.Any())
{
nod.FindNeighbours(this);
}
// nod = nod.Neighbours.OrderBy(x => x.Fn).FirstOrDefault(x => !(x.IsUnavailable ?? false));
nod = nod.Parent;
}
if (nod == null)
{
throw new Exception("Something went wrong. !!!!");
}
pathList.Add(StartNode);
pathList.Reverse();
return(pathList);
}
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());
}
private void UpdateKey(Node B, int Difference)
{
int OldKey = OpenNodes.Keys[OpenNodes.IndexOfValue(B)];
OpenNodes.Remove(OldKey);
ClosedNodes.Remove(OldKey);
OpenNodes.Add(OldKey - Difference, B);
ClosedNodes.Add(OldKey - Difference, B);
foreach (Node Br in AllNodes)
{
if (Br.Parent.Equals(B))
{
UpdateKey(Br, Difference);
}
}
}
public Node Solve(Node Initial)
{
OpenNodes.Add(H.Evaluate(Initial), Initial);
AllNodes.Add(Initial);
Node Current = Initial;
while (!Current.IsValid())
{
OpenNodes.RemoveAt(0);
AddNodes(Current.Expand());
ClosedNodes.Add(H.Evaluate(Current), Current);
Current = OpenNodes.Values[0];
//Console.WriteLine(Current);
//Console.ReadLine();
}
return(Current);
}
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);
}
}
});
}