/// <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 current 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 current 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)
{
Parent = aParent;
Cost = aCost;
GoalNode = aGoalNode;
}
/// <summary>
/// Determines whether 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 for a node in a 2-dimensional map
/// </summary>
/// <param name="aParent">Parent of the node</param>
/// <param name="aGoalNode">Goal node</param>
/// <param name="aCost">Accumulative cost</param>
/// <param name="aX">X-coordinate</param>
/// <param name="aY">Y-coordinate</param>
public AStarNode2D(AStarNode aParent, AStarNode aGoalNode, double aCost, int aX, int aY)
: base(aParent, aGoalNode, aCost)
{
FX = aX;
FY = aY;
}
/// <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>
/// Constructor for a node in a 2-dimensional map
/// </summary>
/// <param name="AParent">Parent of the node</param>
/// <param name="AGoalNode">Goal node</param>
/// <param name="ACost">Accumulative cost</param>
/// <param name="AX">X-coordinate</param>
/// <param name="AY">Y-coordinate</param>
public AStarNode2D(AStarNode AParent, AStarNode AGoalNode, double ACost, int AX, int AY)
: base(AParent, AGoalNode, ACost)
{
FX = AX;
FY = AY;
}
/// <summary>
/// Determines whether 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 override bool IsSameState(AStarNode aNode)
{
if (aNode == null)
{
return false;
}
return ((((AStarNode2D) aNode).X == FX) &&
(((AStarNode2D) aNode).Y == FY));
}
/// <summary>
/// Constructor for a node in a 2-dimensional map
/// </summary>
/// <param name="aParent">Parent of the node</param>
/// <param name="aGoalNode">Goal node</param>
/// <param name="aCost">Accumulative cost</param>
/// <param name="aX">X-coordinate</param>
/// <param name="aY">Y-coordinate</param>
public AStarNode2D(AStarNode aParent, AStarNode aGoalNode, double aCost, int aX, int aY)
: base(aParent, aGoalNode, aCost)
{
FX = aX;
FY = aY;
}
/// <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 current 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 current 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>
/// Determines whether 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>
/// Constructor for a node in a 2-dimensional map
/// </summary>
/// <param name="AParent">Parent of the node</param>
/// <param name="AGoalNode">Goal node</param>
/// <param name="ACost">Accumulative cost</param>
/// <param name="AX">X-coordinate</param>
/// <param name="AY">Y-coordinate</param>
public AStarNode2D(AStarNode AParent, AStarNode AGoalNode, double ACost, int AX, int AY)
: base(AParent, AGoalNode, ACost)
{
FX = AX;
FY = AY;
}