/// <summary>
/// These nodes get used over and over again, so we need to make sure they
/// get completely reset between uses.
/// </summary>
public void Reset()
{
mGraphNode = null;
mPrevious = null;
mCostFromStart = 0;
mCostToDestination = 0;
mReached = false;
mPathSolved = false;
}
/// <summary>
/// Constructor.
/// </summary>
public Planner()
{
// mUnusedNodes is a static used by all instances of the behavior. We only want to allocate
// it once.
if (mUnusedNodes == null)
{
mUnusedNodes = new Stack<PathNode>(10000);
for (Int32 i = 0; i < 100000; i++)
{
PathNode temp = new PathNode();
mUnusedNodes.Push(temp);
}
}
mPathInvalidated = false;
mBestPathEnd = null;
mSolved = false;
}
/// <summary>
/// Call this to put a message back to its default state.
/// </summary>
public override void Reset()
{
mBest_Out = null;
}
/// <summary>
/// Helper function for doing the work needed to go from a position to a GraphNode, and then finally
/// sending that GraphNode to the Planner.
/// </summary>
/// <param name="pos">The position at which we wish to travel from.</param>
private void SetSource(Vector2 pos)
{
mGetTileAtPositionMsg.Reset();
mGetTileAtPositionMsg.mPosition_In = pos;
WorldManager.pInstance.pCurrentLevel.OnMessage(mGetTileAtPositionMsg);
GraphNode start = mPlannerNavMesh.pStart;
// Only set the new destination if it is different than the current one (or the current
// one does not exist).
if (start == null || mGetTileAtPositionMsg.mTile_Out != start.pData)
{
WorldManager.pInstance.pCurrentLevel.OnMessage(mGetNavMeshMsg);
// If the GraphNode was already created, remove it before adding a new one.
if (start != null)
{
//mGetNavMeshMsg.mNavMesh_Out.UnlinkGraphNodes(end, mPlannerNavMesh.pStart);
mGetNavMeshMsg.mNavMesh_Out.RemoveTempNode(start);
}
GraphNode node = mGetNavMeshMsg.mNavMesh_Out.InsertTempNode(pos);
// Attempt to set a new destination. Returns true in the case where the destination was
// different (and thus changed).
if (mPlannerNavMesh.SetSource(node))
{
mLowLevelBest = null;
mLastHighLevelSearched = null;
// Find the TileGraphNode that maps to the location of node.
mGetTileAtPositionMsg.Reset();
mGetTileAtPositionMsg.mPosition_In = pos;
WorldManager.pInstance.pCurrentLevel.OnMessage(mGetTileAtPositionMsg);
mPlannerTileMap.SetSource(mGetTileAtPositionMsg.mTile_Out.mGraphNode);
}
}
//mGetNavMeshMsg.mNavMesh_Out.DebugCheckNodes();
//MBHEngine.PathFind.HPAStar.NavMesh.DebugCheckNode(mPlannerNavMesh.pEnd);
//MBHEngine.PathFind.HPAStar.NavMesh.DebugCheckNode(mPlannerNavMesh.pStart);
}
/// <summary>
/// Sets the current location that we want to path search to.
/// </summary>
/// <param name="pos">The position to try and reach.</param>
private void SetDestination(Vector2 pos)
{
mGetTileAtPositionMsg.Reset();
mGetTileAtPositionMsg.mPosition_In = pos;
WorldManager.pInstance.pCurrentLevel.OnMessage(mGetTileAtPositionMsg);
GraphNode end = mPlannerNavMesh.pEnd;
// Only set the new destination if it is different than the current one (or the current
// one does not exist).
if (end == null || mGetTileAtPositionMsg.mTile_Out != end.pData)
{
WorldManager.pInstance.pCurrentLevel.OnMessage(mGetNavMeshMsg);
// If the GraphNode was already created, remove it before adding a new one.
if (end != null)
{
//mGetNavMeshMsg.mNavMesh_Out.UnlinkGraphNodes(end, mPlannerNavMesh.pStart);
mGetNavMeshMsg.mNavMesh_Out.RemoveTempNode(end);
}
GraphNode node = mGetNavMeshMsg.mNavMesh_Out.InsertTempNode(pos);
// Attempt to set a new destination. Returns true in the case where the destination was
// different (and thus changed).
if (mPlannerNavMesh.SetDestination(node))
{
mLowLevelBest = null;
mLastHighLevelSearched = null;
// If the destination changes, it means the low level search is no longer valid, and
// needs to wait for the high level search to complete first.
mPlannerTileMap.ClearDestination();
}
}
//mGetNavMeshMsg.mNavMesh_Out.DebugCheckNodes();
//MBHEngine.PathFind.HPAStar.NavMesh.DebugCheckNode(mPlannerNavMesh.pEnd);
//MBHEngine.PathFind.HPAStar.NavMesh.DebugCheckNode(mPlannerNavMesh.pStart);
}
/// <summary>
/// Clears the destination and all the the associated data.
/// </summary>
private void ClearDestination()
{
//MBHEngine.PathFind.HPAStar.NavMesh.DebugCheckNode(mPlannerNavMesh.pStart);
WorldManager.pInstance.pCurrentLevel.OnMessage(mGetNavMeshMsg);
// If mPlannerNavMesh currently has a destination, that means that we added a temp
// node to the nav mesh as that destination, and it needs to be removed now.
if (mPlannerNavMesh.pEnd != null)
{
System.Diagnostics.Debug.Assert(mPlannerNavMesh.pEnd != mPlannerNavMesh.pStart, "End and start are the same.");
mGetNavMeshMsg.mNavMesh_Out.RemoveTempNode(mPlannerNavMesh.pEnd);
}
// Members used to coordinate the low level search need to be reset since the
// search had been invalidated.
mLowLevelBest = null;
mLastHighLevelSearched = null;
mPlannerNavMesh.ClearDestination();
mPlannerTileMap.ClearDestination();
//mGetNavMeshMsg.mNavMesh_Out.DebugCheckNodes();
//MBHEngine.PathFind.HPAStar.NavMesh.DebugCheckNode(mPlannerNavMesh.pStart);
}
/// <summary>
/// Called once per frame by the game object.
/// </summary>
/// <param name="gameTime">The amount of time that has passed this frame.</param>
public override void Update(GameTime gameTime)
{
// Does this instance of the behaviour just want to automatically update the source
// based on our parents position?
if (mUpdateSourceAutomatically)
{
SetSource(mParentGOH.pPosition);
}
if (mGetNavMeshMsg.mNavMesh_Out != null)
{
//mGetNavMeshMsg.mNavMesh_Out.DebugCheckNodes();
}
// Plan the path at a high level.
MBHEngine.PathFind.GenericAStar.Planner.Result res = mPlannerNavMesh.PlanPath();
// If the search is anything but InProgress it is safe to research the pass counter.
if (res == Planner.Result.InProgress)
{
mSearchPassCount++;
}
else
{
mSearchPassCount = 0;
}
// If the planner failed to find the destination tell the other behaviours.
if (res == MBHEngine.PathFind.GenericAStar.Planner.Result.Failed ||
res == Planner.Result.InvalidLocation ||
mSearchPassCount > mSearchPassLimit)
{
if (res == Planner.Result.Failed)
{
mOnPathFindFailedMsg.mReason = OnPathFindFailedMessage.Reason.Failed;
}
else if (res == Planner.Result.InvalidLocation)
{
mOnPathFindFailedMsg.mReason = OnPathFindFailedMessage.Reason.InvalidLocation;
}
else if (mSearchPassCount > mSearchPassLimit)
{
mOnPathFindFailedMsg.mReason = OnPathFindFailedMessage.Reason.Timeout;
}
mParentGOH.OnMessage(mOnPathFindFailedMsg);
mSearchPassCount = 0;
}
else if (res == Planner.Result.Solved)// && InputManager.pInstance.CheckAction(InputManager.InputActions.B))
{
// When the high level path finding is solved, start path finding at a lower
// level betwen PathNodes within the higher level.
//
PathNode node = mPlannerNavMesh.pCurrentBest;
// We need to do more than just walk the list until we hit the mLastHighLevelSearched. There
// are cases (such as 2 nodes at the same position) where we don't want to use a Node.
// This tracks which was the last VALID node.
PathNode lastValid = node;
// Loop all the way back to one after the starting point avoiding the starting node, as
// well as previously searched nodes.
// We don't want the starting node because that is where we are likely already standing.
while (
node != mLastHighLevelSearched && // Was the first node actually the one we looked at last Update?
node.pPrevious != mLastHighLevelSearched && // Is the next one the Node looked at last Update?
node.pPrevious.pGraphNode.pPosition != mPlannerTileMap.pStart.pPosition && // Is this Node at the same position as the starting position? We are already there so trying to get there again would cause issues.
node.pPrevious.pPrevious != null)
{
node = node.pPrevious;
// Only choose nodes that are not at the same position as the last valid node. Trying to path find between
// 2 nodes at the TileMap level causes problems. It will solve the path fine, but the issue is when it goes
// back to the HPA level, since it hasn't moved the starting position, HPA will find the exact same path.
// This cycle repeats forever.
if (lastValid.pGraphNode.pPosition != node.pGraphNode.pPosition)
{
lastValid = node;
}
}
node = lastValid;
// The lower level search uses the Level.Tile Graph, not the NavMesh, so we need to
// use the node in NavMesh to find a node in the main tile map.
mGetTileAtPositionMsg.Reset();
mGetTileAtPositionMsg.mPosition_In = node.pGraphNode.pPosition;
WorldManager.pInstance.pCurrentLevel.OnMessage(mGetTileAtPositionMsg);
// If this is the first time we are searching at the low level (for this particular
// search) we want to SET the destination. If that isn't the case, we want to extend
// the search to continue to a new destination.
if (null == mLowLevelBest)
{
mPlannerTileMap.SetDestination(mGetTileAtPositionMsg.mTile_Out.mGraphNode);
}
else
{
mPlannerTileMap.ExtendDestination(mGetTileAtPositionMsg.mTile_Out.mGraphNode);
}
// Start/Continue planning the path.
Planner.Result tileRes = mPlannerTileMap.PlanPath();
// Once the path has been solved, store out that this node in the high level search has
// been completed, so the next time through this function, the next node in the path w
// will be the target.
if (tileRes == Planner.Result.Solved)
{
mLowLevelBest = mPlannerTileMap.pCurrentBest;
mLastHighLevelSearched = node;
}
}
}
/// <summary>
/// Cleans up all the open and closed nodes.
/// </summary>
private void ClearNodeLists()
{
// Clear all the open nodes, and remember to return them to the unused pool!
for (Int32 i = 0; i < mOpenNodes.Count; i++)
{
mOpenNodes[i].Reset();
mUnusedNodes.Push(mOpenNodes[i]);
}
mOpenNodes.Clear();
// Clear all the closed nodes and remember to return them to the unused pool!
for (Int32 i = 0; i < mClosedNodes.Count; i++)
{
mClosedNodes[i].Reset();
mUnusedNodes.Push(mClosedNodes[i]);
}
mClosedNodes.Clear();
// The nodes are gone, so it doesn't make sense that we would
// hold on to a reference to once of them.
mBestPathEnd = null;
}
/// <summary>
/// Perform the path finding. Call repeatedly to continue to try and find the path over a number
/// of frames.
/// </summary>
/// <param name="restrictedArea">Restrict choosen nodes to this area.</param>
/// <returns>The result of the path finding for this frame.</returns>
public Result PlanPath(MBHEngine.Math.Rectangle restrictedArea, Boolean drawDebug)
{
// If there is no tile at the destination then there is no path finding to do.
if (mEnd == null)
{
return Result.NotStarted;
}
if (drawDebug)
{
// If we have a destination draw it. Even if there isn't a source yet.
DebugShapeDisplay.pInstance.AddPoint(mEnd.pPosition, 2.0f, Color.Yellow);
}
// If the destination is a solid tile then we will never be able to solve the path.
if (null != mEnd && !mEnd.IsEmpty())
{
// We consider this a failure, similar to if a destination was surrounded by solid.
return Result.InvalidLocation;
}
// If our source position is not on a tile then there is no path finding to do.
if (mStart == null)
{
return Result.NotStarted;
}
// If our source position is not on a tile, or that tile is solid we cannot ever solve
// this path, so abort right away.
if (mStart != null && !mStart.IsEmpty())
{
// Trying to path find to a solid tile is considered a failure.
return Result.InvalidLocation;
}
if (drawDebug)
{
// If there is a source, draw it.
DebugShapeDisplay.pInstance.AddPoint(mStart.pPosition, 2.0f, Color.Orange);
}
// If the path hasn't already been invalidated this frame, we need to check that
// the path didn't get blocked from something like the Player placing blocks.
// TODO: This could be changed to only do this check when receiving specific events,
// such as the ObjectPlacement Behaviour telling it that a new block has been
// placed.
if (!mPathInvalidated)
{
// Loop through the current path and check for any tiles that are not
// empty. If they aren't empty this path is no longer valid as there is
// something now blocking it.
//
PathNode node = mBestPathEnd;
while (null != node)
{
if (!node.pGraphNode.IsEmpty())
{
// Setting this flag will force the path finder to start from
// the begining.
mPathInvalidated = true;
// No need to loop any further. One blockade is enough.
break;
}
node = node.pPrevious;
}
}
// If the path has become invalid, we need to restart the pathing algorithm.
if (mPathInvalidated)
{
ClearNodeLists();
// First thing we need to do is add the first node to the open list.
PathNode p = mUnusedNodes.Pop();
p.pGraphNode = mStart;
// There is no cost because it is the starting node.
p.pCostFromStart = 0;
// For H we use the actual distance to the destination. The Manhattan Heuristic method.
/*
p.pCostToEnd = System.Math.Max(System.Math.Abs(
p.pGraphNode.pPosition.X - mEnd.pPosition.X),
System.Math.Abs(p.pGraphNode.pPosition.Y - mEnd.pPosition.Y));
*/
Vector2 source = p.pGraphNode.pPosition;
Single h_diagonal = System.Math.Min(System.Math.Abs(source.X - mEnd.pPosition.X), System.Math.Abs(source.Y - mEnd.pPosition.Y));
Single h_straight = System.Math.Abs(source.X - mEnd.pPosition.X) + System.Math.Abs(source.Y - mEnd.pPosition.Y);
p.pCostToEnd = (11.314f) * h_diagonal + 8.0f * (h_straight - 2 * h_diagonal);
// Add it to the list, and start the search!
mOpenNodes.Add(p);
// If the path was invalidated that assume that it is not longer solved.
mSolved = false;
// The path is no longer invalid. It has begun.
mPathInvalidated = false;
}
// Track how many times this planner has looped this time.
Int32 count = 0;
const Int32 maxLoops = 30;
// Loop until all possibilities have been exhusted, the time slice is expired or the
// path is solved.
while (mOpenNodes.Count > 0 && count < maxLoops && !mSolved)// && (!drawDebug || Input.InputManager.pInstance.CheckAction(Input.InputManager.InputActions.B, true)))
{
count++;
mBestPathEnd = mOpenNodes[0];
//HPAStar.NavMesh.DebugCheckNode(mBestPathEnd.pGraphNode);
for (Int32 i = 0; i < mOpenNodes.Count; i++)
{
if (mOpenNodes[i].pFinalCost <= mBestPathEnd.pFinalCost)
{
mBestPathEnd = mOpenNodes[i];
}
}
mOpenNodes.Remove(mBestPathEnd);
mClosedNodes.Add(mBestPathEnd);
// End the search once the destination node is added to the closed list.
//
if (mBestPathEnd.pGraphNode == mEnd)
{
OnPathSolved(drawDebug);
mSolved = true;
break;
}
for (Int32 i = 0; i < mBestPathEnd.pGraphNode.pNeighbours.Count; i++)
{
GraphNode.Neighbour nextNode = mBestPathEnd.pGraphNode.pNeighbours[i];
if (nextNode.mGraphNode.IsPassable(mBestPathEnd.pGraphNode) && (restrictedArea == null || restrictedArea.Intersects(nextNode.mGraphNode.pPosition)))
{
Boolean found = false;
for (Int32 j = 0; j < mClosedNodes.Count; j++)
{
if (mClosedNodes[j].pGraphNode == nextNode.mGraphNode)
{
found = true;
break;
}
}
// This node is already in the closed list, so move on to the next node.
if (found)
{
continue;
}
// 3-b. If it isn’t on the open list, add it to the open list.
// Make the current square the parent of this square. Record the F, G, and H costs of the square.
// TODO: Get a better way to know if something is in the opened list already.
//
PathNode foundNode = null;
for (Int32 j = 0; j < mOpenNodes.Count; j++)
{
if (mOpenNodes[j].pGraphNode == nextNode.mGraphNode)
{
foundNode = mOpenNodes[j];
break;
}
}
// Calculate the cost of moving to this node. This is the distance between the two nodes.
// This will be needed in both the case where the node is in the open list already,
// and the case where it is not.
// The cost is the cost of the previous node plus the distance cost to this node.
Single costFromCurrentBest = mBestPathEnd.pCostFromStart + nextNode.mCostToTravel;
// If the node was not found it needs to be added to the open list.
if (foundNode == null)
{
// Create a new node and add it to the open list so it can been considered for pathing
// in the updates to follow.
PathNode p = mUnusedNodes.Pop();
p.pGraphNode = nextNode.mGraphNode;
// For now it points back to the current node. This can be overwritten if another node
// leads here with a lower cost (see else statement below).
p.pPrevious = mBestPathEnd;
// The cost to get to this node (G) is calculated above.
p.pCostFromStart = costFromCurrentBest;
// Combo
Vector2 source = p.pGraphNode.pPosition;
Single h_diagonal = System.Math.Min(System.Math.Abs(source.X - mEnd.pPosition.X), System.Math.Abs(source.Y - mEnd.pPosition.Y));
Single h_straight = System.Math.Abs(source.X - mEnd.pPosition.X) + System.Math.Abs(source.Y - mEnd.pPosition.Y);
p.pCostToEnd = (11.314f) * h_diagonal + 8.0f * (h_straight - 2 * h_diagonal);
//p.mCostToEnd *= (10.0f + (1.0f/1000.0f));
/*
p.pCostToEnd = System.Math.Max(System.Math.Abs(
p.pGraphNode.pPosition.X - mEnd.pPosition.X),
System.Math.Abs(p.pGraphNode.pPosition.Y - mEnd.pPosition.Y));
*/
mOpenNodes.Add(p);
// Ending the search now will alomost always result in the best path
// but it is possible for it to fail.
if (p.pGraphNode == mEnd)
{
// Since the path is now solved, update mCurBest so that it is used for
// tracing back through the path from now on.
mBestPathEnd = p;
OnPathSolved(drawDebug);
break;
}
}
else
{
// If it is on the open list already, check to see if this path to that square is better,
// using G cost as the measure. A lower G cost means that this is a better path. If so,
// change the parent of the square to the current square, and recalculate the G and F
// scores of the square. If you are keeping your open list sorted by F score, you may need
// to resort the list to account for the change.
if (foundNode.pCostFromStart > costFromCurrentBest)
{
foundNode.pPrevious = mBestPathEnd;
foundNode.pCostFromStart = costFromCurrentBest;
}
}
}
}
}
// Draw the path.
if (drawDebug && mBestPathEnd != null)
{
DebugDraw();
}
if (mSolved)
{
return Result.Solved;
}
else if (mOpenNodes.Count > 0)
{
return Result.InProgress;
}
else
{
return Result.Failed;
}
}
/// <summary>
/// Call this to put a message back to its default state.
/// </summary>
public override void Reset()
{
mBest_Out = null;
}
