/// <summary>
/// Finds the shortest path from the start node to the goal node
/// </summary>
/// <param name="AStartNode">Start node</param>
/// <param name="AGoalNode">Goal node</param>
public void FindPath(AStarNode AStartNode,AStarNode AGoalNode)
{
FStartNode = AStartNode;
FGoalNode = AGoalNode;
FOpenList.Add(FStartNode);
while(FOpenList.Count > 0)
{
// Get the node with the lowest TotalCost
AStarNode NodeCurrent = (AStarNode)FOpenList.Pop();
// If the node is the goal copy the path to the solution array
if(NodeCurrent.IsGoal()) {
while(NodeCurrent != null) {
FSolution.Insert(0,NodeCurrent);
NodeCurrent = NodeCurrent.Parent;
}
break;
}
// Get successors to the current node
NodeCurrent.GetSuccessors(FSuccessors);
foreach(AStarNode NodeSuccessor in FSuccessors)
{
// Test if the currect successor node is on the open list, if it is and
// the TotalCost is higher, we will throw away the current successor.
AStarNode NodeOpen = null;
if(FOpenList.Contains(NodeSuccessor))
NodeOpen = (AStarNode)FOpenList[FOpenList.IndexOf(NodeSuccessor)];
if((NodeOpen != null) && (NodeSuccessor.TotalCost > NodeOpen.TotalCost))
continue;
// Test if the currect successor node is on the closed list, if it is and
// the TotalCost is higher, we will throw away the current successor.
AStarNode NodeClosed = null;
if(FClosedList.Contains(NodeSuccessor))
NodeClosed = (AStarNode)FClosedList[FClosedList.IndexOf(NodeSuccessor)];
if((NodeClosed != null) && (NodeSuccessor.TotalCost > NodeClosed.TotalCost))
continue;
// Remove the old successor from the open list
FOpenList.Remove(NodeOpen);
// Remove the old successor from the closed list
FClosedList.Remove(NodeClosed);
// Add the current successor to the open list
FOpenList.Push(NodeSuccessor);
}
// Add the current node to the closed list
FClosedList.Add(NodeCurrent);
}
}
/// <summary>
/// Determines wheather the current node is the same state as the on passed.
/// </summary>
/// <param name="ANode">AStarNode to compare the current node to</param>
/// <returns>Returns true if they are the same state</returns>
public virtual bool IsSameState(AStarNode ANode)
{
return false;
}
/// <summary>
/// Finds the shortest path from the start node to the goal node
/// </summary>
/// <param name = "AStartNode">Start node</param>
/// <param name = "AGoalNode">Goal node</param>
public void FindPath(AStarNode AStartNode, AStarNode AGoalNode)
{
FStartNode = AStartNode;
FGoalNode = AGoalNode;
FOpenList.Add(FStartNode);
int i = 0;
while (FOpenList.Count > 0 && i < 2000)
{
// Get the node with the lowest TotalCost
AStarNode NodeCurrent = (AStarNode)FOpenList.Pop();
// If the node is the goal copy the path to the solution array
if (NodeCurrent.IsGoal())
{
while (NodeCurrent != null)
{
FSolution.Insert(0, NodeCurrent);
NodeCurrent = NodeCurrent.Parent;
}
break;
}
// Get successors to the current node
NodeCurrent.GetSuccessors(FSuccessors);
foreach (AStarNode NodeSuccessor in FSuccessors)
{
// Test if the currect successor node is on the open list, if it is and
// the TotalCost is higher, we will throw away the current successor.
AStarNode NodeOpen = null;
if (FOpenList.Contains(NodeSuccessor))
{
NodeOpen = (AStarNode)FOpenList[FOpenList.IndexOf(NodeSuccessor)];
}
if ((NodeOpen != null) && (NodeSuccessor.TotalCost > NodeOpen.TotalCost))
{
continue;
}
// Test if the currect successor node is on the closed list, if it is and
// the TotalCost is higher, we will throw away the current successor.
AStarNode NodeClosed = null;
if (FClosedList.Contains(NodeSuccessor))
{
NodeClosed = (AStarNode)FClosedList[FClosedList.IndexOf(NodeSuccessor)];
}
if ((NodeClosed != null) && (NodeSuccessor.TotalCost > NodeClosed.TotalCost))
{
continue;
}
// Remove the old successor from the open list
FOpenList.Remove(NodeOpen);
// Remove the old successor from the closed list
FClosedList.Remove(NodeClosed);
// Add the current successor to the open list
FOpenList.Push(NodeSuccessor);
}
// Add the current node to the closed list
FClosedList.Add(NodeCurrent);
i++;
}
if (i == 2000)
{
m_pathPossible = false;
}
}
/// <summary>
/// Constructor.
/// </summary>
/// <param name="AParent">The node's parent</param>
/// <param name="AGoalNode">The goal node</param>
/// <param name="ACost">The accumulative cost until now</param>
public AStarNode(AStarNode AParent,AStarNode AGoalNode,double ACost)
{
FParent = AParent;
FCost = ACost;
GoalNode = AGoalNode;
}
/// <summary>
/// Determines wheather the current node is the same state as the on passed.
/// </summary>
/// <param name = "ANode">AStarNode to compare the current node to</param>
/// <returns>Returns true if they are the same state</returns>
public virtual bool IsSameState(AStarNode ANode)
{
return(false);
}
/// <summary>
/// Constructor.
/// </summary>
/// <param name = "AParent">The node's parent</param>
/// <param name = "AGoalNode">The goal node</param>
/// <param name = "ACost">The accumulative cost until now</param>
public AStarNode(AStarNode AParent, AStarNode AGoalNode, double ACost)
{
Parent = AParent;
Cost = ACost;
GoalNode = AGoalNode;
}
/// <summary>
/// Finds the shortest path from the start node to the goal node
/// </summary>
/// <param name="AStartNode">Start node</param>
/// <param name="AGoalNode">Goal node</param>
public void FindPath(AStarNode AStartNode, AStarNode AGoalNode, System.Threading.CancellationTokenSource cancel, int maxIter = int.MaxValue)
{
FStartNode = AStartNode;
// FGoalNode = AGoalNode;
int iter = 0;
FOpenList.Add(FStartNode);
while (FOpenList.Count > 0)
{
if (cancel.IsCancellationRequested || (iter++ > maxIter))
{
break;
}
// Get the node with the lowest TotalCost
AStarNode NodeCurrent = (AStarNode)FOpenList.Pop();
// If the node is the goal copy the path to the solution array
if (NodeCurrent.IsGoal())
{
while (NodeCurrent != null)
{
FSolution.Insert(0, NodeCurrent);
NodeCurrent = NodeCurrent.Parent;
}
break;
}
// Get successors to the current node
NodeCurrent.GetSuccessors(FSuccessors);
foreach (AStarNode NodeSuccessor in FSuccessors)
{
// Test if the currect successor node is on the open list, if it is and
// the TotalCost is higher, we will throw away the current successor.
AStarNode NodeOpen = null;
if (FOpenList.Contains(NodeSuccessor))
{
NodeOpen = (AStarNode)FOpenList[FOpenList.IndexOf(NodeSuccessor)];
}
if ((NodeOpen != null) && (NodeSuccessor.TotalCost > NodeOpen.TotalCost))
{
continue;
}
// Test if the currect successor node is on the closed list, if it is and
// the TotalCost is higher, we will throw away the current successor.
AStarNode NodeClosed = null;
if (FClosedList.Contains(NodeSuccessor))
{
NodeClosed = (AStarNode)FClosedList[FClosedList.IndexOf(NodeSuccessor)];
}
if ((NodeClosed != null) && (NodeSuccessor.TotalCost > NodeClosed.TotalCost))
{
continue;
}
// Remove the old successor from the open list
FOpenList.Remove(NodeOpen);
// Remove the old successor from the closed list
FClosedList.Remove(NodeClosed);
// Add the current successor to the open list
FOpenList.Push(NodeSuccessor);
}
// Add the current node to the closed list
FClosedList.Add(NodeCurrent);
}
}
public override bool IsSameState(AStarNode aNode)
{
if (aNode == null)
{
return false;
}
return ((((AStarNode2D)aNode).X == _fx) &&
(((AStarNode2D)aNode).Y == Y));
}
public AStarNode2D(AStarNode aParent, AStarNode aGoalNode, double aCost, int ax, int ay)
: base(aParent, aGoalNode, aCost)
{
_fx = ax;
Y = ay;
}
