/// <summary>
/// initialised the variables for a new search
/// </summary>
/// <param name="start">the waypoint to start from</param>
/// <param name="goal">the destination waypoint</param>
/// <param name="agent">the agent to use for path</param>
private void Init(IWaypoint start, IWaypoint goal, IAgent agent)
{
open = new PriorityQueue <float, Node>(256);
closed = new Hashtable();
existing = new Hashtable();
Node startNode = new Node(start);
startNode.H = start.EstimateCost(goal, agent);
open.Add(0, startNode);
existing.Add(start, startNode);
}
//---------------------------------------------------------------
#endregion
//---------------------------------------------------------------
//---------------------------------------------------------------
#region Methods
//---------------------------------------------------------------
/// <summary>
/// finds a path between two waypoints
/// </summary>
/// <param name="start">the start waypoint</param>
/// <param name="goal">the destination waypoint</param>
/// <param name="agent">the agent to find path for</param>
/// <returns>the path in form of an array of moves
/// or null if there is no connection between from and to</returns>
public Move[] FindPath(IWaypoint start, IWaypoint goal, IAgent agent)
{
// init variables, and add the startNode to the openList
Init(start, goal, agent);
// read out the potentially best node, put it to the closed list and calculate moves
while (open.Count != 0)
{
Node parentNode = open.DequeueValue();
// do we have already reached the goal?
if (parentNode.Waypoint.Equals(goal))
{
return(CreatePath(parentNode));
}
Move[] moves = parentNode.Waypoint.GetMoves(agent);
// traverse all neighbours of the current waypoint
for (int i = 0; i < moves.Length; i++)
{
IWaypoint currentWaypoint = moves[i].Waypoint;
// if this isn't a valid waypoint move on to next
if (currentWaypoint.IsValid() && !closed.Contains(currentWaypoint))
{
// calculate new cost
float g = parentNode.G + moves[i].Cost;
// if node for current waypoint is already existing => update node if cost is lower
if (existing.Contains(currentWaypoint))
{
Node node = (Node)existing[currentWaypoint];
if (g < node.G)
{
node.G = g;
node.H = currentWaypoint.EstimateCost(goal, agent);
node.Parent = parentNode;
}
// if it was already in closed list => remove it
// actualy this can only happen if our function overestimates the distance
// which may be used to speed up the pathfinding, but may result into a suboptimal path
if (closed.Contains(currentWaypoint))
{
closed.Remove(currentWaypoint);
open.Add(node.F, node);
Root.instance.logging.debug("This shouldn't happen for underestimating functions!");
}
}
else
{
Node node = new Node(currentWaypoint);
node.G = g;
node.H = currentWaypoint.EstimateCost(goal, agent);
node.Parent = parentNode;
node.Waypoint = currentWaypoint;
open.Add(node.F, node);
existing.Add(currentWaypoint, node);
}
}
}
}
return(null);
}
