/// <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;
}
//System.Console.WriteLine("x");
// Get successors to the current node
NodeCurrent.GetSuccessors(FSuccessors);
foreach (AStarNode NodeSuccessor in FSuccessors)
{
//HACK?!
AStarNode n = NodeCurrent;
bool b = false;
while (n != null)
{
if (NodeSuccessor.IsSameState(n))
{
b = true;
break;
}
n = n.Parent;
}
if (b)
{
continue;
}
//System.Console.WriteLine("1");
// 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;
}
//System.Console.WriteLine("2");
// 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;
}
//System.Console.WriteLine("3");
// 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);
}
//System.Console.WriteLine("4");
// Add the current node to the closed list
FClosedList.Add(NodeCurrent);
}
}
/// <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>
/// 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;
}
//System.Console.WriteLine("x");
// Get successors to the current node
NodeCurrent.GetSuccessors(FSuccessors);
foreach(AStarNode NodeSuccessor in FSuccessors)
{
//HACK?!
AStarNode n = NodeCurrent;
bool b = false;
while (n != null)
{
if (NodeSuccessor.IsSameState(n))
{
b = true;
break;
}
n = n.Parent;
}
if (b) continue;
//System.Console.WriteLine("1");
// 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;
//System.Console.WriteLine("2");
// 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;
//System.Console.WriteLine("3");
// 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);
}
//System.Console.WriteLine("4");
// 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>
/// 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 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,AStarMap map)
: base(AParent, AGoalNode, ACost)
{
FX = AX;
FY = AY;
Map = map;
}