/// <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; } }