From c30739a610b760a09d85397eed13343e7e60c330 Mon Sep 17 00:00:00 2001 From: Rtch90 Date: Sat, 4 Feb 2012 18:27:29 +0000 Subject: [PATCH] [Add] Some more work on pathfinding. -- Decided to stick with ANSI/ISO standards by keeping template implementation inside the header it came from as apposed to exporting it into another compilation unit. This stops gcc from bitching at me! --- .gitignore | 1 + Unuk-QT/Unuk-QT.pro | 1 - src/libUnuk/Engine/Pathfinding.cpp | 282 ----------------------------- src/libUnuk/Engine/Pathfinding.h | 216 +++++++++++++++++++++- 4 files changed, 211 insertions(+), 289 deletions(-) delete mode 100644 src/libUnuk/Engine/Pathfinding.cpp diff --git a/.gitignore b/.gitignore index 3f9396c..f885ed8 100644 --- a/.gitignore +++ b/.gitignore @@ -12,6 +12,7 @@ Win32/Unuk/Unuk.ncb Win32/Unuk/Unuk.suo Bin/Unuk Bin/*.dll +Save/save* Save/save_* *.swp *.o diff --git a/Unuk-QT/Unuk-QT.pro b/Unuk-QT/Unuk-QT.pro index 09248e4..c10058c 100644 --- a/Unuk-QT/Unuk-QT.pro +++ b/Unuk-QT/Unuk-QT.pro @@ -76,6 +76,5 @@ SOURCES += ../src/libUnuk/Engine/WorldManager.cpp \ ../src/libUnuk/UI/EventHistory.cpp \ ../src/libUnuk/UI/Bar.cpp \ ../src/libUnuk/System/Vec2.cpp \ - ../src/libUnuk/Engine/Pathfinding.cpp \ ../src/libUnuk/UI/SavegameMenu.cpp OTHER_FILES += diff --git a/src/libUnuk/Engine/Pathfinding.cpp b/src/libUnuk/Engine/Pathfinding.cpp deleted file mode 100644 index 60b54dd..0000000 --- a/src/libUnuk/Engine/Pathfinding.cpp +++ /dev/null @@ -1,282 +0,0 @@ -#include "Pathfinding.h" - -template -AStarSearch::AStarSearch(void) : - _state(SEARCH_STATE_NOT_INITIALIZED), - _currentSolutionNode(NULL), - _allocateNodeCount(0), - _cancelRequest(false) {} - -template -AStarSearch::~AStarSearch(void) { - -} - -template -void AStarSearch::SetStartAndGoalStates(UserState& start, UserState& goal) { - _cancelRequest= false; - - _start = AllocateNode(); - _goal = AllocateNode(); - - assert((_start != NULL && _goal != NULL)); - - _start->_userState = _start; - _goal->_userState = _goal; - - _state = SEARCH_STATE_SEARCHING; - - // 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->f = _start->g + _start->h; - _start->parent = 0; - - // Push the start node onto the open list. - _openList.push_back(_start); // Heap is now unsorted. - - // Sort back element into the heap. - push_heap(_openList.begin(), _openList.end(), HeapCompare_f()); - - // 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 -bool AStarSearch::AddSuccessor(UserState& state) { - -} - -template -void AStarSearch::FreeSolutionNodes(void) { - -} - -template -UserState* AStarSearch::GetSolutionStart(void) { - -} - -template -UserState* AStarSearch::GetSolutionNext(void) { - -} - -template -UserState* AStarSearch::GetSolutionEnd(void) { - -} - -template -UserState* AStarSearch::GetSolutionPrev(void) { - -} - -template -UserState* AStarSearch::GetOpenListStart(void) { - -} - -template -UserState* AStarSearch::GetOpenListStart(float& f, float& g, float& h) { - -} - -template -UserState* AStarSearch::GetOpenListNext(void) { - -} - -template -UserState* AStarSearch::GetOpenListNext(float& f, float& g, float& h) { - -} - -template -UserState* AStarSearch::GetClosedListStart(void) { - -} - -template -UserState* AStarSearch::GetClosedListStart(float& f, float& g, float& h) { - -} - -template -UserState* AStarSearch::GetClosedListNext(void) { - -} - -template -UserState* AStarSearch::GetClosedListNext(float& f, float& g, float& h) { - -} - -// Private. -template -void AStarSearch::FreeAllNodes(void) { - -} - -template -void AStarSearch::FreeUnusedNodes(void) { - -} - -template -typename AStarSearch::Node* AStarSearch::AllocateNode(void) { - -} - -template -void AStarSearch::FreeNode(Node* node) { - -} diff --git a/src/libUnuk/Engine/Pathfinding.h b/src/libUnuk/Engine/Pathfinding.h index 50af163..45986d4 100644 --- a/src/libUnuk/Engine/Pathfinding.h +++ b/src/libUnuk/Engine/Pathfinding.h @@ -51,8 +51,13 @@ public: }; public: - AStarSearch(void); - ~AStarSearch(void); + AStarSearch(void) : + _state(SEARCH_STATE_NOT_INITIALIZED), + _currentSolutionNode(NULL), + _allocateNodeCount(0), + _cancelRequest(false) {} + + ~AStarSearch(void) {} int GetState(void) { return _state; } @@ -60,19 +65,218 @@ public: void CancelSearch(void) { _cancelRequest = true; } // Set the start/goal state. - void SetStartAndGoalStates(UserState& start, UserState& goal); + void SetStartAndGoalStates(UserState& start, UserState& goal) { + _cancelRequest= false; + + _start = AllocateNode(); + _goal = AllocateNode(); + + assert((_start != NULL && _goal != NULL)); + + _start->_userState = _start; + _goal->_userState = _goal; + + _state = SEARCH_STATE_SEARCHING; + + // 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->f = _start->g + _start->h; + _start->parent = 0; + + // Push the start node onto the open list. + _openList.push_back(_start); // Heap is now unsorted. + + // Sort back element into the heap. + push_heap(_openList.begin(), _openList.end(), HeapCompare_f()); + + // Initialize counter for the search steps. + _steps = 0; + } // Search one step. - unsigned int SearchStep(void); + unsigned int 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. + } // Call this to add a successor to a list of // successors when expanding the seach frontier. - bool AddSuccessor(UserState& state); + bool AddSuccessor(UserState& state) { + Node* node = Allocate(); + + if(node) { + node->_userState = state; + _successors.push_back(node); + return true; + } + return false; + } // Free the solution nodes. // This is done to clean up all used nodes in memory when you are // done with the search. - void FreeSolutionNodes(void); + void FreeSolutionNodes(void) { + Node* n = _start; + + if(_start->child) { + while(n != _goal) { + Node* del = n; + n = n->child; + FreeNode(del); + + del = NULL; + } + FreeNode(n); // Delete the goal. + } else { + // If the start node is the solution, we need to just + // delete the start goal nodes. + FreeNode(_start); + FreeNode(_goal); + } + } // -- Some methods for travelling through the solution. --