/// <summary>
/// Adds the specified node to the open nodes collection.
/// </summary>
/// <param name="node">Node to be added.</param>
/// <param name="gValue">Calculated distance from the start.</param>
/// <param name="predecessor">Predecessor node.</param>
/// <param name="appliedOperator">Predecessor operator.</param>
protected override void AddToOpenNodes(ISearchNode node, int gValue, ISearchNode predecessor, IOperator appliedOperator)
{
if (gValue > LimitValue)
{
return;
}
NodeInfo nodeInfo;
if (NodesInfo.TryGetValue(node, out nodeInfo))
{
if (nodeInfo.GValue > gValue)
{
nodeInfo.GValue = gValue;
nodeInfo.Predecessor = Tuple.Create(predecessor, appliedOperator);
NodesInfo[node] = nodeInfo;
double hValue = Heuristic.GetValue(node);
OpenNodes.Add(gValue + hValue, node);
}
}
else
{
double hValue = Heuristic.GetValue(node);
double fValue = gValue + hValue;
if (fValue > LimitValue)
{
LowestValueOverLimit = Math.Min(LowestValueOverLimit, fValue);
return;
}
NodesInfo.Add(node, new NodeInfo(gValue, Tuple.Create(predecessor, appliedOperator)));
OpenNodes.Add(fValue, node);
}
}
public async Task <INode <TState, TCost> > StepAsync()
{
if (Result.IsCompleted)
{
return(Result.Result);
}
try
{
//System.Console.WriteLine($"{Thread.CurrentThread.ManagedThreadId}: Step started.");
return(await OpenNodes.PerformWorkerOperationAsync(
async node =>
{
Diag_NodeTraversing?.Invoke(node);
if (Problem.Comparer.Compare(node.EstimatedDistance, Scenario.AcceptedDistance) <= 0) //Found a goal node
{
lock (ResultSetter)
{
ResultSetter.SetResult(node);
WorkTokenSource.Cancel();
return node;
}
}
var newnodes = (await Problem.ExpandNode(new NodeExpansionImplementation(this, node))).ToList();
foreach (var newnode in newnodes)
{
OpenNodes.Add(newnode);
}
Diag_NodeTraversed?.Invoke(node);
return node;
}));
}
catch (NodeRepository <TState, TCost> .QueueExhaustedException)
{
if (Result.IsCompleted)
{
return(Result.Result);
}
if (Result.IsFaulted)
{
throw Result.Exception;
}
var ex = new MissingSolutionException();
ResultSetter.TrySetException(ex);
throw ex;
}
catch (Exception e)
{
ResultSetter.TrySetException(e);
throw;
}
}
/// <summary>
/// Computes the heuristic value for the specified node and inserts both into open nodes. Should not be called separately, but only
/// within AddToOpenNodes() method containing necessary checks and additional operations.
/// </summary>
/// <param name="node">Node to be added.</param>
/// <param name="gValue">Calculated distance from the start.</param>
/// <param name="predecessor">Predecessor node.</param>
/// <param name="appliedOperator">Predecessor operator.</param>
protected virtual void ComputeHeuristicAndInsertNewOpenNode(ISearchNode node, int gValue, ISearchNode predecessor, IOperator appliedOperator)
{
double hValue = (IsComplexHeuristic) ? ((ComplexHeuristic)Heuristic).GetValue(node, predecessor, appliedOperator) : Heuristic.GetValue(node);
if (!double.IsInfinity(hValue)) // Infinity heuristic indicates dead-end
{
OpenNodes.Add(ComputeNewOpenNodeValueWithTieBreakingRule(gValue, hValue), node);
}
}
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);
}
/// <summary>
/// Computes the heuristic value for the specified node and inserts both into open nodes. Should not be called separately, but only
/// within AddToOpenNodes() method containing necessary checks and additional operations.
/// </summary>
/// <param name="node">Node to be added.</param>
/// <param name="gValue">Calculated distance from the start.</param>
/// <param name="predecessor">Predecessor node.</param>
/// <param name="appliedOperator">Predecessor operator.</param>
protected override void ComputeHeuristicAndInsertNewOpenNode(ISearchNode node, int gValue, ISearchNode predecessor, IOperator appliedOperator)
{
for (int i = 0; i < NumberOfOpenLists; ++i)
{
Heuristic = Heuristics[i];
OpenNodes = OpenLists[i];
double hValue = Heuristic.GetValue(node);
if (!double.IsInfinity(hValue)) // Infinity heuristic indicates dead-end
{
OpenNodes.Add(ComputeNewOpenNodeValueWithTieBreakingRule(gValue, hValue), node);
++AllOpenNodesCount;
}
}
}
/// <summary>
/// Adds the specified node to the open nodes collection.
/// </summary>
/// <param name="node">Node to be added.</param>
/// <param name="gValue">Calculated distance from the start.</param>
/// <param name="hValue">Heuristic distance to the goal.</param>
/// <param name="predecessor">Predecessor node.</param>
/// <param name="appliedOperator">Predecessor operator.</param>
protected void AddToOpenNodes(ISearchNode node, int gValue, double hValue, ISearchNode predecessor, IOperator appliedOperator)
{
NodeInfo nodeInfo;
if (NodesInfo.TryGetValue(node, out nodeInfo))
{
if (nodeInfo.GValue > gValue)
{
nodeInfo.GValue = gValue;
nodeInfo.Predecessor = Tuple.Create(predecessor, appliedOperator);
NodesInfo[node] = nodeInfo;
OpenNodes.Add(gValue + hValue, node);
}
}
else
{
NodesInfo.Add(node, new NodeInfo(gValue, Tuple.Create(predecessor, appliedOperator)));
OpenNodes.Add(gValue + hValue, node);
}
}
/// <summary>
/// Sets the initial search node of the search procedure.
/// </summary>
/// <param name="initialNode">Initial node of the search.</param>
protected virtual void SetInitialNode(ISearchNode initialNode)
{
OpenNodes.Add(0, initialNode);
NodesInfo.Add(initialNode, new NodeInfo(0, null));
}
public void OnEnterNode(Guid node)
{
NodeCount++;
OpenNodes.Add(node);
}
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);
}
}
});
}