From 1dcbb82b72bc2fe706d1114247acc0913785467d Mon Sep 17 00:00:00 2001 From: Rtch90 Date: Sat, 4 Feb 2012 15:04:22 +0000 Subject: [PATCH] [Add] SearchStep(void) method now implemented. -- Yes, there is some code I have held back for now.. --- src/libUnuk/Engine/Pathfinding.cpp | 154 +++++++++++++++++++++++++++-- src/libUnuk/Engine/Pathfinding.h | 4 +- 2 files changed, 150 insertions(+), 8 deletions(-) diff --git a/src/libUnuk/Engine/Pathfinding.cpp b/src/libUnuk/Engine/Pathfinding.cpp index fc41176..60b54dd 100644 --- a/src/libUnuk/Engine/Pathfinding.cpp +++ b/src/libUnuk/Engine/Pathfinding.cpp @@ -2,7 +2,7 @@ template AStarSearch::AStarSearch(void) : - _state(SEARCH_STATE_NOT_INITIALISED), + _state(SEARCH_STATE_NOT_INITIALIZED), _currentSolutionNode(NULL), _allocateNodeCount(0), _cancelRequest(false) {} @@ -21,15 +21,15 @@ void AStarSearch::SetStartAndGoalStates(UserState& start, UserState& assert((_start != NULL && _goal != NULL)); - _start->_UserState = _start; - _goal->_UserState = _goal; + _start->_userState = _start; + _goal->_userState = _goal; _state = SEARCH_STATE_SEARCHING; - // Initialise the AStar specific parts of the start node. + // Initialize the AStar specific parts of the start node. // You only need to fill out the state information. _start->g = 0; - _start->h = _start->_UserState.GoalDistanceEstimate(_goal->_UserState); + _start->h = _start->_userState.GoalDistanceEstimate(_goal->_userState); _start->f = _start->g + _start->h; _start->parent = 0; @@ -39,13 +39,155 @@ void AStarSearch::SetStartAndGoalStates(UserState& start, UserState& // Sort back element into the heap. push_heap(_openList.begin(), _openList.end(), HeapCompare_f()); - // Initialise counter for the search steps. + // Initialize counter for the search steps. _steps = 0; } template unsigned int AStarSearch::SearchStep(void) { + // Break if the search has not been initialized. + assert((_state > SEARCH_STATE_NOT_INITIALIZED) && ( _state < SEARCH_STATE_INVALID)); + // Ensure it is safe to do a seach step once the seach has succeeded. + if((_state == SEARCH_STATE_SUCCEEDED) || (_state == SEARCH_STATE_FAILED)) { return false; } + + if(_openList.empty() || _cancelRequest) { + // Then there is nothing left to search, so fail. + FreeAllNodes(); + _state = SEARCH_STATE_FAILED; + return _state; + } + + _steps++; + + // Pop the best node. -- The one with the lowest f. + Node* n = _openList.front(); // Get pointer to the node. + pop_heap(_openList.begin(), _openList.end(), HeapCompare_f()); + _openList.pop_back(); + + // Check for the goal, once we pop that, we are done. + if(n->_userState.IsGoal(_goal->_userState)) { + // Copy the parent pointer of n, as we will use the passed in goal node. + _goal->parent = n->parent; + + // If the goal was passed in at the start.. + if(false == n->_userState.IsSameState(_start->_userState)) { + FreeNode(n); + + // Set the child pointers in each node, apart from goal, as it has no child. + Node* nodeChild = _goal; + Node* nodeParent = _goal->parent; + + while(nodeChild != _start) { + // Start is always the first node by definition. + nodeParent->child = nodeChild; + + nodeChild = nodeParent; + nodeParent = nodeParent->parent; + } + } + // Delete nodes that are not needed for the solution. + FreeUnusedNodes(); + _state = SEARCH_STATE_SUCCEEDED; + + return _state; + } else { + // Not goal. + + /* + * Generate the successors of this node. + * The user helps us to do this, and we keep + * the new nodes in _successors. + */ + _successors.clear(); // empty the vector of successor nodes to n. + + // The user provides this functions and uses AddSuccessor to add each + // successor of node 'n' to _successors. + bool ret = n->_userState.GetSuccessors(this, n->parent ? &n->parent->_userState : NULL); + + if(!ret) { + typename vector::iterator successor; + + // Free the nodes that may have previously been added. + for(successor = _successors.begin(); successor != _successors.end(); successor++) { + FreeNode((*successor)); + } + // Empty vector of successor nodes nodes to n. + _successors.clear(); + + // Free up everything else we allocated along the way. + FreeAllNodes(); + + _state = SEARCH_STATE_OUT_OF_MEMORY; + return _state; + } + // Now handle each successor to the current node.. + for(typename vector::iterator successor = _successors.begin(); successor != _successors.end(); successor++) { + // The g value for this successor. + float newg = n->g + n->_userState.GetCost((*successor)->_userState); + + /* + * We need to see whether the node is on the open or closed + * list. If it is, but the node that is already on them is better + * (lower g) then we can forget about this successor. + * + * First linear search of open list to find node. + */ + typename vector::iterator openlist_result; + for(openlist_result = _openList.begin(); openlist_result != _openList.end(); openlist_result++) { + if((*openlist_result)->_userState.IsSameState((*successor)->_userState)) { + break; + } + } + if(openlist_result != _openList.end()) { + // We found this state open. + if((*openlist_result)->g <= newg) { + FreeNode((*successor)); + // The one on the open list is cheaper than this one. + continue; + } + } + typename vector::iterator closedlist_result; + for(closedlist_result = _closedList.begin(); closedlist_result != _closedList.end(); closedlist_result++) { + if((*closedlist_result)->_userState.IsSameState((*successor)->_userState)) { + break; + } + } + if(closedlist_result != _closedList.end()) { + // We found this state closed. + if((*closedlist_result)->g <= newg) { + // The one on the closed list is cheaper than this one. + FreeNode((*successor)); + continue; + } + } + // This node is the best node so fat with this particular state. + // So lets keep it, and set up its AStar specific data.. + (*successor)->parent = n; + (*successor)->g = newg; + (*successor)->h = (*successor)->_userState.GoalDistanceEstimate(_goal->_userState); + (*successor)->f = (*successor)->g + (*successor)->h; + + // Remove successor from closed if it was on it. + if(closedlist_result != _closedList.end()) { + // Remove it from the closed list. + FreeNode((*closedlist_result)); + _closedList.erase(closedlist_result); + + // Now remake the heap!! + make_heap(_openList.begin(), _openList.end(), HeapCompare_f()); + } + + // The heap is now unsorted. + _openList.push_back((*successor)); + + // Sort back elements into the heap. + push_heap(_openList.begin(), _openList.end(), HeapCompare_f()); + } + // push n onto the closed list as it has now been expanded. + _closedList.push_back(n); + } // (Not goal, so expand) + return _state; // Succeeded bool should be false at this point. } template diff --git a/src/libUnuk/Engine/Pathfinding.h b/src/libUnuk/Engine/Pathfinding.h index 09761bb..50af163 100644 --- a/src/libUnuk/Engine/Pathfinding.h +++ b/src/libUnuk/Engine/Pathfinding.h @@ -16,7 +16,7 @@ using namespace std; template class AStarSearch { public: enum { - SEARCH_STATE_NOT_INITIALISED, + SEARCH_STATE_NOT_INITIALIZED, SEARCH_STATE_SEARCHING, SEARCH_STATE_SUCCEEDED, SEARCH_STATE_FAILED, @@ -39,7 +39,7 @@ public: Node(void) : parent(0), child(0), g(0.0f), h(0.0f), f(0.0) {} - UserState _UserState; + UserState _userState; }; // Compare the f values of the two nodes.