diff --git a/source/glest_game/ai/fast_path_finder.cpp b/source/glest_game/ai/fast_path_finder.cpp new file mode 100644 index 00000000..4d6bcbe1 --- /dev/null +++ b/source/glest_game/ai/fast_path_finder.cpp @@ -0,0 +1,1132 @@ +/*! +// ============================================================== +// This file is part of Glest (www.glest.org) +// +// Copyright (C) 2012 Mark Vejvoda +// +// You can redistribute this code and/or modify it under +// the terms of the GNU General Public License as published +// by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version +// ============================================================== + +** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net +** +** Portions of this source has been released with the PhysXViewer application, as well as +** Rocket, CreateDynamics, ODF, and as a number of sample code snippets. +** +** If you find this code useful or you are feeling particularily generous I would +** ask that you please go to http://www.amillionpixels.us and make a donation +** to Troy DeMolay. +** +** DeMolay is a youth group for young men between the ages of 12 and 21. +** It teaches strong moral principles, as well as leadership skills and +** public speaking. The donations page uses the 'pay for pixels' paradigm +** where, in this case, a pixel is only a single penny. Donations can be +** made for as small as $4 or as high as a $100 block. Each person who donates +** will get a link to their own site as well as acknowledgement on the +** donations blog located here http://www.amillionpixels.blogspot.com/ +** +** If you wish to contact me you can use the following methods: +** +** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches) +** Skype ID: jratcliff63367 +** Yahoo: jratcliff63367 +** AOL: jratcliff1961 +** email: jratcliff@infiniplex.net + +A* Algorithm Implementation using STL is +Copyright (C)2001-2005 Justin Heyes-Jones + + FixedSizeAllocator class + Copyright 2001 Justin Heyes-Jones + + This class is a constant time O(1) memory manager for objects of + a specified type. The type is specified using a template class. + + Memory is allocated from a fixed size buffer which you can specify in the + class constructor or use the default. + + Using GetFirst and GetNext it is possible to iterate through the elements + one by one, and this would be the most common use for the class. + + I would suggest using this class when you want O(1) add and delete + and you don't do much searching, which would be O(n). Structures such as binary + trees can be used instead to get O(logn) access time. + +** +** The MIT license: +** +** Permission is hereby granted, free of charge, to any person obtaining a copy +** of this software and associated documentation files (the "Software"), to deal +** in the Software without restriction, including without limitation the rights +** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +** copies of the Software, and to permit persons to whom the Software is furnished +** to do so, subject to the following conditions: +** +** The above copyright notice and this permission notice shall be included in all +** copies or substantial portions of the Software. + +** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, +** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN +** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + +*/ + +#include +#include +#include +#include + +//*** IMPORTANT : READ ME FIRST !! +//*** +//*** This source code simply provides a C++ wrapper for the AStar Algorithm Implementation in STL written by Justin Heyes-Jones +//*** There is nothing wrong with Justin's code in any way, except that he uses templates. My personal programming style is +//*** to use virtual interfaces and the PIMPLE paradigm to hide the details of the implementation. +//*** +//*** To use my wrapper you simply have your own path node inherit the pure virtual interface 'AI_Node' and implement the +//*** following four methods. +//*** +//*** +//** virtual float getDistance(const AI_Node *node) = 0; // Return the distance between two nodes +//** virtual float getCost(void) = 0; // return the relative 'cost' of a node. Default should be 1. +//** virtual unsigned int getEdgeCount(void) const = 0; // Return the number of edges in a node. +//** virtual AI_Node * getEdge(int index) const = 0; // Return a pointer to the node a particular edge is connected to. +//** +//** That's all there is to it. +//** +//** Here is an example usage: +//** +//** FastAstar *fa = createFastAstar(); +//** astarStartSearch(fq,fromNode,toNode); +//** for (int i=0; i<10000; i++) +//** { +//** bool finished = astarSearchStep(fa); +//** if ( finished ) break; +//** } +//** +//** unsigned int count; +//** AI_Node **solution = getSolution(fa,count); +//** +//** ... do something you want with the answer +//** +//** releaseFastAstar(fa); +//** +//******************************* + +#include "fast_path_finder.h" + +template class FixedSizeAllocator { + +public: + // Constants + enum { + FSA_DEFAULT_SIZE = 3000 + }; + + // This class enables us to transparently manage the extra data + // needed to enable the user class to form part of the double-linked + // list class + struct FSA_ELEMENT { + USER_TYPE UserType; + + FSA_ELEMENT *pPrev; + FSA_ELEMENT *pNext; + }; + +public: // methods + FixedSizeAllocator( unsigned int MaxElements = FSA_DEFAULT_SIZE ) : + m_MaxElements( MaxElements ), + m_pFirstUsed( NULL ) { + // Allocate enough memory for the maximum number of elements + char *pMem = new char[ m_MaxElements * sizeof(FSA_ELEMENT) ]; + m_pMemory = (FSA_ELEMENT *) pMem; + // Set the free list first pointer + m_pFirstFree = m_pMemory; + // Clear the memory + memset( m_pMemory, 0, sizeof( FSA_ELEMENT ) * m_MaxElements ); + // Point at first element + FSA_ELEMENT *pElement = m_pFirstFree; + // Set the double linked free list + for( unsigned int i=0; ipPrev = pElement-1; + pElement->pNext = pElement+1; + + pElement++; + } + + // first element should have a null prev + m_pFirstFree->pPrev = NULL; + // last element should have a null next + (pElement-1)->pNext = NULL; + } + + ~FixedSizeAllocator() { + // Free up the memory + delete [] m_pMemory; + } + + void reset() { + m_pFirstUsed = NULL; + m_pFirstFree = m_pMemory; + // Point at first element + FSA_ELEMENT *pElement = m_pFirstFree; + // Set the double linked free list + for( unsigned int i=0; ipPrev = pElement-1; + pElement->pNext = pElement+1; + + pElement++; + } + + // first element should have a null prev + m_pFirstFree->pPrev = NULL; + // last element should have a null next + (pElement-1)->pNext = NULL; + } + + // Allocate a new USER_TYPE and return a pointer to it + USER_TYPE *alloc() { + FSA_ELEMENT *pNewNode = NULL; + if( !m_pFirstFree ) { + return NULL; + } + else { + pNewNode = m_pFirstFree; + m_pFirstFree = pNewNode->pNext; + + // if the new node points to another free node then + // change that nodes prev free pointer... + if( pNewNode->pNext ) { + pNewNode->pNext->pPrev = NULL; + } + + // node is now on the used list + pNewNode->pPrev = NULL; // the allocated node is always first in the list + if( m_pFirstUsed == NULL ) { + pNewNode->pNext = NULL; // no other nodes + } + else { + m_pFirstUsed->pPrev = pNewNode; // insert this at the head of the used list + pNewNode->pNext = m_pFirstUsed; + } + + m_pFirstUsed = pNewNode; + } + + return reinterpret_cast(pNewNode); + } + + // Free the given user type + // For efficiency I don't check whether the user_data is a valid + // pointer that was allocated. I may add some debug only checking + // (To add the debug check you'd need to make sure the pointer is in + // the m_pMemory area and is pointing at the start of a node) + void free( USER_TYPE *user_data ) { + FSA_ELEMENT *pNode = reinterpret_cast(user_data); + + // manage used list, remove this node from it + if( pNode->pPrev ) { + pNode->pPrev->pNext = pNode->pNext; + } + else { + // this handles the case that we delete the first node in the used list + m_pFirstUsed = pNode->pNext; + } + + if( pNode->pNext ) { + pNode->pNext->pPrev = pNode->pPrev; + } + + // add to free list + if( m_pFirstFree == NULL ) { + // free list was empty + m_pFirstFree = pNode; + pNode->pPrev = NULL; + pNode->pNext = NULL; + } + else { + // Add this node at the start of the free list + m_pFirstFree->pPrev = pNode; + pNode->pNext = m_pFirstFree; + m_pFirstFree = pNode; + } + } + + // For debugging this displays both lists (using the prev/next list pointers) + void Debug() { + printf( "free list " ); + + FSA_ELEMENT *p = m_pFirstFree; + while( p ) { + printf( "%x!%x ", p->pPrev, p->pNext ); + p = p->pNext; + } + printf( "\n" ); + + printf( "used list " ); + + p = m_pFirstUsed; + while( p ) { + printf( "%x!%x ", p->pPrev, p->pNext ); + p = p->pNext; + } + printf( "\n" ); + } + + // Iterators + + USER_TYPE *GetFirst() { + return reinterpret_cast(m_pFirstUsed); + } + + USER_TYPE *GetNext( USER_TYPE *node ) { + return reinterpret_cast + ( + (reinterpret_cast(node))->pNext + ); + } + +public: // data + +private: // methods + +private: // data + + FSA_ELEMENT *m_pFirstFree; + FSA_ELEMENT *m_pFirstUsed; + unsigned int m_MaxElements; + FSA_ELEMENT *m_pMemory; + +}; + +/* +A* Algorithm Implementation using STL is +Copyright (C)2001-2005 Justin Heyes-Jones + +Permission is given by the author to freely redistribute and +include this code in any program as long as this credit is +given where due. + + COVERED CODE IS PROVIDED UNDER THIS LICENSE ON AN "AS IS" BASIS, + WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, + INCLUDING, WITHOUT LIMITATION, WARRANTIES THAT THE COVERED CODE + IS FREE OF DEFECTS, MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE + OR NON-INFRINGING. THE ENTIRE RISK AS TO THE QUALITY AND + PERFORMANCE OF THE COVERED CODE IS WITH YOU. SHOULD ANY COVERED + CODE PROVE DEFECTIVE IN ANY RESPECT, YOU (NOT THE INITIAL + DEVELOPER OR ANY OTHER CONTRIBUTOR) ASSUME THE COST OF ANY + NECESSARY SERVICING, REPAIR OR CORRECTION. THIS DISCLAIMER OF + WARRANTY CONSTITUTES AN ESSENTIAL PART OF THIS LICENSE. NO USE + OF ANY COVERED CODE IS AUTHORIZED HEREUNDER EXCEPT UNDER + THIS DISCLAIMER. + + Use at your own risk! + +*/ + +// used for text debugging +#include +#include +//#include +#include + +// stl includes +#include +#include +#include + +using namespace std; + +// Fixed size memory allocator can be disabled to compare performance +// Uses std new and delete instead if you turn it off +#define USE_FSA_MEMORY 1 + +// disable warning that debugging information has lines that are truncated +// occurs in stl headers +#pragma warning( disable : 4786 ) + +// The AStar search class. UserState is the users state space type +template class AStarSearch { + +public: // data + // A node represents a possible state in the search + // The user provided state type is included inside this type + +public: + + class Node { + public: + Node *parent; // used during the search to record the parent of successor nodes + Node *child; // used after the search for the application to view the search in reverse + + float g; // cost of this node + it's predecessors + float h; // heuristic estimate of distance to goal + float f; // sum of cumulative cost of predecessors and self and heuristic + + Node() : + parent( 0 ), + child( 0 ), + g( 0.0f ), + h( 0.0f ), + f( 0.0f ) + { + } + + UserState m_UserState; + }; + + // For sorting the heap the STL needs compare function that lets us compare + // the f value of two nodes + class HeapCompare_f { + public: + bool operator() ( const Node *x, const Node *y ) const { + return x->f > y->f; + } + }; + +public: // methods + + // constructor just initialises private data + AStarSearch( int MaxNodes = 2000 ) : + m_AllocateNodeCount(0), + m_FixedSizeAllocator( MaxNodes ), + m_State( SEARCH_STATE_NOT_INITIALISED ), + m_CurrentSolutionNode( NULL ), + m_Start( NULL), + m_Goal( NULL), + m_CancelRequest( false ) + { + } + + // call at any time to cancel the search and free up all the memory + void CancelSearch() { + m_CancelRequest = true; + } + + // Set Start and goal states + void SetStartAndGoalStates( UserState &Start, UserState &Goal, void *userData ) { + this->userData = userData; + + m_FixedSizeAllocator.reset(); + m_AllocateNodeCount = 0; + m_OpenList.clear(); + m_ClosedList.clear(); + m_Successors.clear(); + //m_OpenList.reserve(m_FixedSizeAllocator); + //m_ClosedList.reserve(m_FixedSizeAllocator); + //m_Successors.reserve(m_FixedSizeAllocator); + + m_CurrentSolutionNode = NULL; + + m_CancelRequest = false; + + m_Start = AllocateNode(); + m_Goal = AllocateNode(); + + assert((m_Start != NULL && m_Goal != NULL)); + + m_Start->m_UserState = Start; + m_Goal->m_UserState = Goal; + + m_State = SEARCH_STATE_SEARCHING; + + // Initialise the AStar specific parts of the Start Node + // The user only needs fill out the state information + m_Start->g = 0; + m_Start->h = m_Start->m_UserState.GoalDistanceEstimate( m_Goal->m_UserState, userData ); + m_Start->f = m_Start->g + m_Start->h; + m_Start->parent = 0; + + // Push the start node on the Open list + m_OpenList.push_back( m_Start ); // heap now unsorted + + // Sort back element into heap + push_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() ); + + // Initialise counter for search steps + m_Steps = 0; + } + + // Advances search one step + SearchState SearchStep(unsigned int &searchCount) { + searchCount = 0; + // Firstly break if the user has not initialised the search + assert( (m_State > SEARCH_STATE_NOT_INITIALISED) && (m_State < SEARCH_STATE_INVALID) ); + + // Next I want it to be safe to do a searchstep once the search has succeeded... + if( (m_State == SEARCH_STATE_SUCCEEDED) || (m_State == SEARCH_STATE_FAILED) ) { + return m_State; + } + + // Failure is defined as emptying the open list as there is nothing left to + // search... + // New: Allow user abort + if( m_OpenList.empty() || m_CancelRequest ) { + FreeAllNodes(); + m_State = SEARCH_STATE_FAILED; + return m_State; + } + + // Incremement step count + m_Steps ++; + + // Pop the best node (the one with the lowest f) + Node *n = m_OpenList.front(); // get pointer to the node + pop_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() ); + m_OpenList.pop_back(); + + // Check for the goal, once we pop that we're done + if( n->m_UserState.IsGoal( m_Goal->m_UserState ) ) { + // The user is going to use the Goal Node he passed in + // so copy the parent pointer of n + m_Goal->parent = n->parent; + + // A special case is that the goal was passed in as the start state + // so handle that here + if( false == n->m_UserState.IsSameState( m_Start->m_UserState ) ) { + FreeNode( n ); + + // set the child pointers in each node (except Goal which has no child) + Node *nodeChild = m_Goal; + Node *nodeParent = m_Goal->parent; + + do { + nodeParent->child = nodeChild; + + nodeChild = nodeParent; + nodeParent = nodeParent->parent; + + } + while( nodeChild != m_Start ); // Start is always the first node by definition + } + + // delete nodes that aren't needed for the solution + FreeUnusedNodes(); + + m_State = SEARCH_STATE_SUCCEEDED; + + return m_State; + } + // not goal + else { + // We now need to generate the successors of this node + // The user helps us to do this, and we keep the new nodes in + // m_Successors ... + m_Successors.clear(); // empty vector of successor nodes to n + + // User provides this functions and uses AddSuccessor to add each successor of + // node 'n' to m_Successors + bool ret = n->m_UserState.GetSuccessors( this, n->parent ? &n->parent->m_UserState : NULL ); + + if( !ret ) { + typename vector< Node * >::iterator successor; + + // free the nodes that may previously have been added + for( successor = m_Successors.begin(); successor != m_Successors.end(); successor ++ ) { + FreeNode( (*successor) ); + } + + m_Successors.clear(); // empty vector of successor nodes to n + + // free up everything else we allocated + FreeAllNodes(); + + m_State = SEARCH_STATE_OUT_OF_MEMORY; + return m_State; + } + + // Now handle each successor to the current node ... + for( typename vector< Node * >::iterator successor = m_Successors.begin(); + successor != m_Successors.end(); successor ++ ) { + searchCount++; + + // The g value for this successor ... + float newg = n->g + n->m_UserState.GetCost( (*successor)->m_UserState, userData ); + + // Now we need to find whether the node is on the open or closed lists + // 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< Node * >::iterator openlist_result; + + for( openlist_result = m_OpenList.begin(); + openlist_result != m_OpenList.end(); openlist_result ++ ) { + if( (*openlist_result)->m_UserState.IsSameState( (*successor)->m_UserState ) ) { + break; + } + } + + if( openlist_result != m_OpenList.end() ) { + // we found this state on open + if( (*openlist_result)->g <= newg ) { + FreeNode( (*successor) ); + + // the one on Open is cheaper than this one + continue; + } + } + + typename vector< Node * >::iterator closedlist_result; + + for( closedlist_result = m_ClosedList.begin(); + closedlist_result != m_ClosedList.end(); closedlist_result ++ ) { + if( (*closedlist_result)->m_UserState.IsSameState( (*successor)->m_UserState ) ) { + break; + } + } + + if( closedlist_result != m_ClosedList.end() ) { + // we found this state on closed + if( (*closedlist_result)->g <= newg ) { + // the one on Closed is cheaper than this one + FreeNode( (*successor) ); + + continue; + } + } + + // This node is the best node so far with this particular state + // so lets keep it and set up its AStar specific data ... + + (*successor)->parent = n; + (*successor)->g = newg; + (*successor)->h = (*successor)->m_UserState.GoalDistanceEstimate( m_Goal->m_UserState, userData ); + (*successor)->f = (*successor)->g + (*successor)->h; + + // Remove successor from closed if it was on it + if( closedlist_result != m_ClosedList.end() ) { + // remove it from Closed + FreeNode( (*closedlist_result) ); + m_ClosedList.erase( closedlist_result ); + + // Fix thanks to ... + // Greg Douglas + // who noticed that this code path was incorrect + // Here we have found a new state which is already CLOSED + // anus + } + + // Update old version of this node + if( openlist_result != m_OpenList.end() ) { + FreeNode( (*openlist_result) ); + m_OpenList.erase( openlist_result ); + + // re-make the heap + // make_heap rather than sort_heap is an essential bug fix + // thanks to Mike Ryynanen for pointing this out and then explaining + // it in detail. sort_heap called on an invalid heap does not work + make_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() ); + } + + // heap now unsorted + m_OpenList.push_back( (*successor) ); + + if (m_OpenList[0]->f == m_OpenList[m_OpenList.size()-1]->f) { + Node *aux = m_OpenList[0]; + m_OpenList[0] = m_OpenList[m_OpenList.size()-1]; + m_OpenList[m_OpenList.size()-1] = aux; + } + // sort back element into heap + push_heap( m_OpenList.begin(), m_OpenList.end(), HeapCompare_f() ); + + } + + // push n onto Closed, as we have expanded it now + + m_ClosedList.push_back( n ); + + } // end else (not goal so expand) + + return m_State; // Succeeded bool is false at this point. + } + + // User calls this to add a successor to a list of successors + // when expanding the search frontier + bool AddSuccessor( UserState &State ) { + Node *node = AllocateNode(); + if( node ) { + node->m_UserState = State; + + m_Successors.push_back( node ); + + return true; + } + + return false; + } + + // Free the solution nodes + // This is done to clean up all used Node memory when you are done with the + // search + void FreeSolutionNodes() { + Node *n = m_Start; + if( m_Start && m_Start->child ) { + do { + Node *del = n; + n = n->child; + FreeNode( del ); + + del = NULL; + + } while( n != m_Goal ); + + FreeNode( n ); // Delete the goal + m_Start = NULL; + m_Goal = NULL; + } + else { + // if the start node is the solution we need to just delete the start and goal + // nodes + FreeNode( m_Start ); + m_Start = NULL; + FreeNode( m_Goal ); + m_Goal = NULL; + } + } + + // Functions for traversing the solution + + // Get start node + UserState *GetSolutionStart() { + m_CurrentSolutionNode = m_Start; + if( m_Start ) { + return &m_Start->m_UserState; + } + else { + return NULL; + } + } + + // Get next node + UserState *GetSolutionNext() { + if( m_CurrentSolutionNode ) { + if( m_CurrentSolutionNode->child ) { + Node *child = m_CurrentSolutionNode->child; + m_CurrentSolutionNode = m_CurrentSolutionNode->child; + return &child->m_UserState; + } + } + + return NULL; + } + + // Get end node + UserState *GetSolutionEnd() { + m_CurrentSolutionNode = m_Goal; + if( m_Goal ) { + return &m_Goal->m_UserState; + } + else { + return NULL; + } + } + + // Step solution iterator backwards + UserState *GetSolutionPrev() { + if( m_CurrentSolutionNode ) { + if( m_CurrentSolutionNode->parent ) { + Node *parent = m_CurrentSolutionNode->parent; + m_CurrentSolutionNode = m_CurrentSolutionNode->parent; + return &parent->m_UserState; + } + } + + return NULL; + } + + // For educational use and debugging it is useful to be able to view + // the open and closed list at each step, here are two functions to allow that. + UserState *GetOpenListStart() { + float f,g,h; + return GetOpenListStart( f,g,h ); + } + + UserState *GetOpenListStart( float &f, float &g, float &h ) { + iterDbgOpen = m_OpenList.begin(); + if( iterDbgOpen != m_OpenList.end() ) { + f = (*iterDbgOpen)->f; + g = (*iterDbgOpen)->g; + h = (*iterDbgOpen)->h; + return &(*iterDbgOpen)->m_UserState; + } + + return NULL; + } + + UserState *GetOpenListNext() { + float f,g,h; + return GetOpenListNext( f,g,h ); + } + + UserState *GetOpenListNext( float &f, float &g, float &h ) { + iterDbgOpen++; + if( iterDbgOpen != m_OpenList.end() ) { + f = (*iterDbgOpen)->f; + g = (*iterDbgOpen)->g; + h = (*iterDbgOpen)->h; + return &(*iterDbgOpen)->m_UserState; + } + + return NULL; + } + + UserState *GetClosedListStart() { + float f,g,h; + return GetClosedListStart( f,g,h ); + } + + UserState *GetClosedListStart( float &f, float &g, float &h ) { + iterDbgClosed = m_ClosedList.begin(); + if( iterDbgClosed != m_ClosedList.end() ) { + f = (*iterDbgClosed)->f; + g = (*iterDbgClosed)->g; + h = (*iterDbgClosed)->h; + + return &(*iterDbgClosed)->m_UserState; + } + + return NULL; + } + + UserState *GetClosedListNext() { + float f,g,h; + return GetClosedListNext( f,g,h ); + } + + UserState *GetClosedListNext( float &f, float &g, float &h ) { + iterDbgClosed++; + if( iterDbgClosed != m_ClosedList.end() ) { + f = (*iterDbgClosed)->f; + g = (*iterDbgClosed)->g; + h = (*iterDbgClosed)->h; + + return &(*iterDbgClosed)->m_UserState; + } + + return NULL; + } + + // Get the number of steps + + int GetStepCount() { return m_Steps; } + + void EnsureMemoryFreed() { +#if USE_FSA_MEMORY + assert(m_AllocateNodeCount == 0); +#endif + } + + void * getUserData() { return userData; } + + // This is called when a search fails or is cancelled to free all used + // memory + void FreeAllNodes() { + // iterate open list and delete all nodes + typename vector< Node * >::iterator iterOpen = m_OpenList.begin(); + + while( iterOpen != m_OpenList.end() ) { + Node *n = (*iterOpen); + FreeNode( n ); + + iterOpen ++; + } + + m_OpenList.clear(); + + // iterate closed list and delete unused nodes + typename vector< Node * >::iterator iterClosed; + + for( iterClosed = m_ClosedList.begin(); + iterClosed != m_ClosedList.end(); iterClosed ++ ) { + Node *n = (*iterClosed); + FreeNode( n ); + } + + m_ClosedList.clear(); + + // delete the goal + FreeNode(m_Goal); + m_Goal = NULL; + } + +private: // methods + + // This call is made by the search class when the search ends. A lot of nodes may be + // created that are still present when the search ends. They will be deleted by this + // routine once the search ends + void FreeUnusedNodes() { + // iterate open list and delete unused nodes + typename vector< Node * >::iterator iterOpen = m_OpenList.begin(); + + while( iterOpen != m_OpenList.end() ) { + Node *n = (*iterOpen); + if( !n->child ) { + FreeNode( n ); + n = NULL; + } + + iterOpen ++; + } + + m_OpenList.clear(); + + // iterate closed list and delete unused nodes + typename vector< Node * >::iterator iterClosed; + + for( iterClosed = m_ClosedList.begin(); + iterClosed != m_ClosedList.end(); iterClosed ++ ) { + Node *n = (*iterClosed); + if( !n->child ) { + FreeNode( n ); + n = NULL; + } + } + + m_ClosedList.clear(); + } + + // Node memory management + Node *AllocateNode() { +#if !USE_FSA_MEMORY + Node *p = new Node; + return p; +#else + Node *address = m_FixedSizeAllocator.alloc(); + if( !address ) { + return NULL; + } + m_AllocateNodeCount ++; + Node *p = new (address) Node; + return p; +#endif + } + + void FreeNode( Node *node ) { + m_AllocateNodeCount --; + +#if !USE_FSA_MEMORY + delete node; +#else + m_FixedSizeAllocator.free( node ); +#endif + } + +private: // data + + // Heap (simple vector but used as a heap, cf. Steve Rabin's game gems article) + vector< Node *> m_OpenList; + + // Closed list is a vector. + vector< Node * > m_ClosedList; + + // Successors is a vector filled out by the user each type successors to a node + // are generated + vector< Node * > m_Successors; + + // State + SearchState m_State; + + // Counts steps + int m_Steps; + + // Start and goal state pointers + Node *m_Start; + Node *m_Goal; + + Node *m_CurrentSolutionNode; + + // Memory + FixedSizeAllocator m_FixedSizeAllocator; + + //Debug : need to keep these two iterators around + // for the user Dbg functions + typename vector< Node * >::iterator iterDbgOpen; + typename vector< Node * >::iterator iterDbgClosed; + + // debugging : count memory allocation and free's + int m_AllocateNodeCount; + + bool m_CancelRequest; + + void *userData; +}; + +//******************************************************** +//********** C++ wrapper written by John W. Ratcliff *** +//******************************************************** + +class MapSearchNode { +public: + MapSearchNode(void) { + mNode = 0; + } + + MapSearchNode(AI_Node *point) { + mNode = point; + } + + float GoalDistanceEstimate( MapSearchNode &nodeGoal, void *userData ); + bool IsGoal( MapSearchNode &nodeGoal ); + bool GetSuccessors( AStarSearch *astarsearch, MapSearchNode *parent_node ); + float GetCost( MapSearchNode &successor, void *userData ); + bool IsSameState( MapSearchNode &rhs ); + + void PrintNodeInfo(); + + AI_Node * getNode(void) const { return mNode; }; + +private: + + AI_Node *mNode; +}; + +bool MapSearchNode::IsSameState( MapSearchNode &rhs ) { + bool ret = false; + if ( mNode == rhs.mNode ) ret = true; + + return ret; +} + +void MapSearchNode::PrintNodeInfo() { +} + +float MapSearchNode::GoalDistanceEstimate( MapSearchNode &nodeGoal, void *userData ) { + return mNode->getDistance( nodeGoal.mNode, userData ); +} + +bool MapSearchNode::IsGoal( MapSearchNode &nodeGoal ) { + bool ret = false; + if ( mNode == nodeGoal.mNode ) ret = true; + return ret; +} + +bool MapSearchNode::GetSuccessors( AStarSearch *astarsearch, MapSearchNode *parent_node ) { + unsigned int count = mNode->getEdgeCount(astarsearch->getUserData()); + for (unsigned int i = 0; i < count; ++i) { + AI_Node *node = mNode->getEdge(i,astarsearch->getUserData()); + MapSearchNode newNode(node); + astarsearch->AddSuccessor( newNode ); + } + + return true; +} + +float MapSearchNode::GetCost( MapSearchNode &successor, void *userData ) { + return mNode->getCost(userData); +} + +class FastAstar { +public: + + FastAstar(void) { + } + + ~FastAstar(void) { + } + + void startSearch(AI_Node *from,AI_Node *to,void *userData) { + mSolution.clear(); + MapSearchNode start(from); + MapSearchNode end(to); + + mAstarSearch.SetStartAndGoalStates(start,end,userData); + } + + SearchState getLastSearchState() { + return lastSearchState; + } + + bool searchStep(unsigned int &searchCount) { + bool ret = false; + + SearchState state = mAstarSearch.SearchStep(searchCount); + lastSearchState = state; + + switch ( state ) { + case SEARCH_STATE_NOT_INITIALISED: + ret = true; + break; + case SEARCH_STATE_SEARCHING: + ret = false; + break; + case SEARCH_STATE_SUCCEEDED: + if ( 1 ) { + MapSearchNode *node = mAstarSearch.GetSolutionStart(); + while ( node ) { + AI_Node *ai = node->getNode(); + mSolution.push_back(ai); + node = mAstarSearch.GetSolutionNext(); + } + mAstarSearch.FreeSolutionNodes(); + } + ret = true; + break; + case SEARCH_STATE_FAILED: + ret = true; + break; + case SEARCH_STATE_OUT_OF_MEMORY: + assert(0); + break; + case SEARCH_STATE_INVALID: + ret = true; + break; + } + + return ret; + } + + AI_Node ** getSolution(unsigned int &count) { + AI_Node **ret = 0; + count = 0; + if ( !mSolution.empty() ) { + count = mSolution.size(); + ret = &mSolution[0]; + } + return ret; + } + +private: + AStarSearch< MapSearchNode > mAstarSearch; + std::vector< AI_Node * > mSolution; + SearchState lastSearchState; +}; + +FastAstar * createFastAstar(void) { + FastAstar *ret = new FastAstar; + return ret; +} + +void astarStartSearch(FastAstar *astar,AI_Node *from,AI_Node *to,void *userData) { + if ( astar ) astar->startSearch(from,to,userData); +} + +bool astarSearchStep(FastAstar *astar,unsigned int &searchCount) { + bool ret = true; + if ( astar ) + ret =astar->searchStep(searchCount); + + return ret; +} + +SearchState getLastSearchState(FastAstar *astar) { + return astar->getLastSearchState(); +} + +void releaseFastAstar(FastAstar *astar) { + delete astar; +} + +AI_Node ** getSolution(FastAstar *astar,unsigned int &count) { + AI_Node **ret = 0; + if ( astar ) { + ret = astar->getSolution(count); + } + + return ret; +} diff --git a/source/glest_game/ai/fast_path_finder.h b/source/glest_game/ai/fast_path_finder.h new file mode 100644 index 00000000..5f118525 --- /dev/null +++ b/source/glest_game/ai/fast_path_finder.h @@ -0,0 +1,150 @@ +/*! +// ============================================================== +// This file is part of Glest (www.glest.org) +// +// Copyright (C) 2012 Mark Vejvoda +// +// You can redistribute this code and/or modify it under +// the terms of the GNU General Public License as published +// by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version +// ============================================================== + +** Copyright (c) 2007 by John W. Ratcliff mailto:jratcliff@infiniplex.net +** +** Portions of this source has been released with the PhysXViewer application, as well as +** Rocket, CreateDynamics, ODF, and as a number of sample code snippets. +** +** If you find this code useful or you are feeling particularily generous I would +** ask that you please go to http://www.amillionpixels.us and make a donation +** to Troy DeMolay. +** +** DeMolay is a youth group for young men between the ages of 12 and 21. +** It teaches strong moral principles, as well as leadership skills and +** public speaking. The donations page uses the 'pay for pixels' paradigm +** where, in this case, a pixel is only a single penny. Donations can be +** made for as small as $4 or as high as a $100 block. Each person who donates +** will get a link to their own site as well as acknowledgement on the +** donations blog located here http://www.amillionpixels.blogspot.com/ +** +** If you wish to contact me you can use the following methods: +** +** Skype Phone: 636-486-4040 (let it ring a long time while it goes through switches) +** Skype ID: jratcliff63367 +** Yahoo: jratcliff63367 +** AOL: jratcliff1961 +** email: jratcliff@infiniplex.net + +A* Algorithm Implementation using STL is +Copyright (C)2001-2005 Justin Heyes-Jones + + FixedSizeAllocator class + Copyright 2001 Justin Heyes-Jones + + This class is a constant time O(1) memory manager for objects of + a specified type. The type is specified using a template class. + + Memory is allocated from a fixed size buffer which you can specify in the + class constructor or use the default. + + Using GetFirst and GetNext it is possible to iterate through the elements + one by one, and this would be the most common use for the class. + + I would suggest using this class when you want O(1) add and delete + and you don't do much searching, which would be O(n). Structures such as binary + trees can be used instead to get O(logn) access time. + +** +** The MIT license: +** +** Permission is hereby granted, free of charge, to any person obtaining a copy +** of this software and associated documentation files (the "Software"), to deal +** in the Software without restriction, including without limitation the rights +** to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +** copies of the Software, and to permit persons to whom the Software is furnished +** to do so, subject to the following conditions: +** +** The above copyright notice and this permission notice shall be included in all +** copies or substantial portions of the Software. + +** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +** IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +** FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +** AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, +** WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN +** CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + +*/ + +#ifndef FAST_ASTAR_H + +#define FAST_ASTAR_H + +//*** IMPORTANT : READ ME FIRST !! +//*** +//*** This source code simply provides a C++ wrapper for the AStar Algorithm Implementation in STL written by Justin Heyes-Jones +//*** There is nothing wrong with Justin's code in any way, except that he uses templates. My personal programming style is +//*** to use virtual interfaces and the PIMPLE paradigm to hide the details of the implementation. +//*** +//*** To use my wrapper you simply have your own path node inherit the pure virtual interface 'AI_Node' and implement the +//*** following four methods. +//*** +//*** +//** virtual float getDistance(const AI_Node *node) = 0; // Return the distance between two nodes +//** virtual float getCost(void) = 0; // return the relative 'cost' of a node. Default should be 1. +//** virtual unsigned int getEdgeCount(void) const = 0; // Return the number of edges in a node. +//** virtual AI_Node * getEdge(int index) const = 0; // Return a pointer to the node a particular edge is connected to. +//** +//** That's all there is to it. +//** +//** Here is an example usage: +//** +//** FastAstar *fa = createFastAstar(); +//** astarStartSearch(fq,fromNode,toNode); +//** for (int i=0; i<10000; i++) +//** { +//** bool finished = astarSearchStep(fa); +//** if ( finished ) break; +//** } +//** +//** unsigned int count; +//** AI_Node **solution = getSolution(fa,count); +//** +//** ... do something you want with the answer +//** +//** releaseFastAstar(fa); +//** +//******************************* + + +class AI_Node +{ +public: + virtual float getDistance(const AI_Node *node,void *userData) = 0; + virtual float getCost(void *userData) = 0; + virtual unsigned int getEdgeCount(void *userData) const = 0; + virtual AI_Node * getEdge(int index,void *userData) const = 0; +}; + +enum SearchState { + SEARCH_STATE_NOT_INITIALISED, + SEARCH_STATE_SEARCHING, + SEARCH_STATE_SUCCEEDED, + SEARCH_STATE_FAILED, + SEARCH_STATE_OUT_OF_MEMORY, + SEARCH_STATE_INVALID +}; + +class FastAstar; + +FastAstar * createFastAstar(void); // Create an instance of the FastAstar utility. +void astarStartSearch(FastAstar *astar,AI_Node *from,AI_Node *to, void *userData); // start a search. + +bool astarSearchStep(FastAstar *astar,unsigned int &searchCount); // step the A star algorithm one time. Return true if the search is completed. +SearchState getLastSearchState(FastAstar *astar); + +AI_Node ** getSolution(FastAstar *astar,unsigned int &count); // retrieve the solution. If this returns a null pointer and count of zero, it means no solution could be found. +void releaseFastAstar(FastAstar *astar); // Release the intance of the FastAstar utility. + + +#endif diff --git a/source/glest_game/ai/path_finder.cpp b/source/glest_game/ai/path_finder.cpp index 479fcec4..fe7bcd19 100644 --- a/source/glest_game/ai/path_finder.cpp +++ b/source/glest_game/ai/path_finder.cpp @@ -21,6 +21,7 @@ #include "command.h" #include "faction.h" #include "randomgen.h" +#include "fast_path_finder.h" #include "leak_dumper.h" using namespace std; @@ -49,8 +50,10 @@ const int PathFinder::pathFindExtendRefreshNodeCountMin = 40; const int PathFinder::pathFindExtendRefreshNodeCountMax = 40; PathFinder::PathFinder() { + minorDebugPathfinder = false; for(int i = 0; i < GameConstants::maxPlayers; ++i) { factions.push_back(FactionState()); + //factions.resize(GameConstants::maxPlayers); } map=NULL; } @@ -61,8 +64,10 @@ int PathFinder::getPathFindExtendRefreshNodeCount(int factionIndex) { } PathFinder::PathFinder(const Map *map) { + minorDebugPathfinder = false; for(int i = 0; i < GameConstants::maxPlayers; ++i) { factions.push_back(FactionState()); + //factions.resize(GameConstants::maxPlayers); } map=NULL; @@ -82,6 +87,9 @@ void PathFinder::init(const Map *map) { PathFinder::~PathFinder() { for(int i = 0; i < GameConstants::maxPlayers; ++i) { factions[i].nodePool.clear(); + + releaseFastAstar(factions[i].fa); + factions[i].fa = NULL; } factions.clear(); map=NULL; @@ -110,6 +118,8 @@ TravelState PathFinder::findPath(Unit *unit, const Vec2i &finalPos, bool *wasStu throw megaglest_runtime_error("map == NULL"); } + unit->setCurrentPathFinderDesiredFinalPos(finalPos); + if(frameIndex >= 0) { clearUnitPrecache(unit); } @@ -208,207 +218,639 @@ TravelState PathFinder::findPath(Unit *unit, const Vec2i &finalPos, bool *wasStu maxNodeCount= PathFinder::pathFindNodesAbsoluteMax; } - ts = aStar(unit, finalPos, false, frameIndex, maxNodeCount); + int unitFactionIndex = unit->getFactionIndex(); - //post actions - switch(ts) { - case tsBlocked: - case tsArrived: + minorDebugPathfinder = false; + bool enableFastPathfinder = true; + if(enableFastPathfinder == true) { + if(minorDebugPathfinder) printf("Fast Pathfind Unit [%d - %s] from = %s to = %s frameIndex = %d\n",unit->getId(),unit->getType()->getName().c_str(),unit->getPos().getString().c_str(),finalPos.getString().c_str(),frameIndex); - // The unit is stuck (not only blocked but unable to go anywhere for a while) - // We will try to bail out of the immediate area - if( ts == tsBlocked && unit->getInBailOutAttempt() == false && - path->isStuck() == true) { + ts = aStarFast(unit, finalPos, false, frameIndex, maxNodeCount); - //printf("$$$$ Unit START BAILOUT ATTEMPT for [%d - %s]\n",unit->getId(),unit->getFullName().c_str()); + //post actions + switch(ts) { + case tsBlocked: + case tsArrived: - if(SystemFlags::getSystemSettingType(SystemFlags::debugWorldSynch).enabled == true && frameIndex < 0) { - char szBuf[4096]=""; - sprintf(szBuf,"[attempting to BAIL OUT] finalPos [%s] ts [%d]", - finalPos.getString().c_str(),ts); - unit->logSynchData(__FILE__,__LINE__,szBuf); - } + // The unit is stuck (not only blocked but unable to go anywhere for a while) + // We will try to bail out of the immediate area + if( ts == tsBlocked && unit->getInBailOutAttempt() == false && + path->isStuck() == true) { - if(wasStuck != NULL) { - *wasStuck = true; - } - unit->setInBailOutAttempt(true); + //printf("$$$$ Unit START BAILOUT ATTEMPT for [%d - %s]\n",unit->getId(),unit->getFullName().c_str()); + if(minorDebugPathfinder) printf("Fast Pathfind Unit [%d - %s] START BAILOUT ATTEMPT frameIndex = %d\n",unit->getId(),unit->getType()->getName().c_str(),frameIndex); - bool useBailoutRadius = Config::getInstance().getBool("EnableBailoutPathfinding","true"); - if(useBailoutRadius == true) { + if(SystemFlags::getSystemSettingType(SystemFlags::debugWorldSynch).enabled == true && frameIndex < 0) { + char szBuf[4096]=""; + sprintf(szBuf,"[attempting to BAIL OUT] finalPos [%s] ts [%d]", + finalPos.getString().c_str(),ts); + unit->logSynchData(__FILE__,__LINE__,szBuf); + } - //!!! - bool unitImmediatelyBlocked = false; + if(wasStuck != NULL) { + *wasStuck = true; + } + unit->setInBailOutAttempt(true); - // First check if unit currently blocked all around them, if so don't try to pathfind - const bool showConsoleDebugInfo = Config::getInstance().getBool("EnablePathfinderDistanceOutput","false"); - const Vec2i unitPos = unit->getPos(); - int failureCount = 0; - int cellCount = 0; + bool useBailoutRadius = Config::getInstance().getBool("EnableBailoutPathfinding","true"); + if(useBailoutRadius == true) { - for(int i = -1; i <= 1; ++i) { - for(int j = -1; j <= 1; ++j) { - Vec2i pos = unitPos + Vec2i(i, j); - if(pos != unitPos) { - bool canUnitMoveToCell = map->aproxCanMove(unit, unitPos, pos); - if(canUnitMoveToCell == false) { - failureCount++; + //!!! + bool unitImmediatelyBlocked = false; + + // First check if unit currently blocked all around them, if so don't try to pathfind + const bool showConsoleDebugInfo = Config::getInstance().getBool("EnablePathfinderDistanceOutput","false"); + const Vec2i unitPos = unit->getPos(); + int failureCount = 0; + int cellCount = 0; + + for(int i = -1; i <= 1; ++i) { + for(int j = -1; j <= 1; ++j) { + Vec2i pos = unitPos + Vec2i(i, j); + if(pos != unitPos) { + bool canUnitMoveToCell = map->aproxCanMove(unit, unitPos, pos); + if(canUnitMoveToCell == false) { + failureCount++; + } + cellCount++; + } + } + } + unitImmediatelyBlocked = (failureCount == cellCount); + + //if(SystemFlags::getSystemSettingType(SystemFlags::debugPerformance).enabled == true && chrono.getMillis() > 1) SystemFlags::OutputDebug(SystemFlags::debugPerformance,"In [%s::%s Line: %d] **Check if dest blocked, distance for unit [%d - %s] from [%s] to [%s] is %.2f took msecs: %lld unitImmediatelyBlocked = %d, failureCount = %d\n",__FILE__,__FUNCTION__,__LINE__,unit->getId(),unit->getFullName().c_str(), unitPos.getString().c_str(), finalPos.getString().c_str(), dist,(long long int)chrono.getMillis(),unitImmediatelyBlocked,failureCount); + if(showConsoleDebugInfo && unitImmediatelyBlocked) { + printf("**Check if src blocked [%d], unit [%d - %s] from [%s] to [%s] unitImmediatelyBlocked = %d, failureCount = %d [%d]\n", + unitImmediatelyBlocked, unit->getId(),unit->getFullName().c_str(), unitPos.getString().c_str(), finalPos.getString().c_str(), unitImmediatelyBlocked,failureCount,cellCount); + } + + // if(unitImmediatelyBlocked == false) { + // // First check if final destination blocked + // failureCount = 0; + // cellCount = 0; + // + // for(int i = -1; i <= 1; ++i) { + // for(int j = -1; j <= 1; ++j) { + // Vec2i pos = finalPos + Vec2i(i, j); + // if(pos != finalPos) { + // bool canUnitMoveToCell = map->aproxCanMove(unit, pos, finalPos); + // if(canUnitMoveToCell == false) { + // failureCount++; + // } + // cellCount++; + // } + // } + // } + // unitImmediatelyBlocked = (failureCount == cellCount); + // + // if(SystemFlags::getSystemSettingType(SystemFlags::debugPerformance).enabled == true && chrono.getMillis() > 1) SystemFlags::OutputDebug(SystemFlags::debugPerformance,"In [%s::%s Line: %d] **Check if dest blocked, distance for unit [%d - %s] from [%s] to [%s] is %.2f took msecs: %lld unitImmediatelyBlocked = %d, failureCount = %d\n",__FILE__,__FUNCTION__,__LINE__,unit->getId(),unit->getFullName().c_str(), unitPos.getString().c_str(), finalPos.getString().c_str(), dist,(long long int)chrono.getMillis(),unitImmediatelyBlocked,failureCount); + // if(showConsoleDebugInfo && nodeLimitReached) { + // printf("**Check if dest blocked [%d - %d], unit [%d - %s] from [%s] to [%s] distance %.2f took msecs: %lld unitImmediatelyBlocked = %d, failureCount = %d [%d]\n", + // nodeLimitReached, inBailout, unit->getId(),unit->getFullName().c_str(), unitPos.getString().c_str(), finalPos.getString().c_str(), dist,(long long int)chrono.getMillis(),unitImmediatelyBlocked,failureCount,cellCount); + // } + // } + // + //!!! + + if(unitImmediatelyBlocked == false) { + int tryRadius = factions[unit->getFactionIndex()].random.randRange(0,1); + + // Try to bail out up to PathFinder::pathFindBailoutRadius cells away + if(tryRadius > 0) { + for(int bailoutX = -PathFinder::pathFindBailoutRadius; bailoutX <= PathFinder::pathFindBailoutRadius && ts == tsBlocked; ++bailoutX) { + for(int bailoutY = -PathFinder::pathFindBailoutRadius; bailoutY <= PathFinder::pathFindBailoutRadius && ts == tsBlocked; ++bailoutY) { + const Vec2i newFinalPos = finalPos + Vec2i(bailoutX,bailoutY); + bool canUnitMove = map->canMove(unit, unit->getPos(), newFinalPos); + + if(SystemFlags::getSystemSettingType(SystemFlags::debugWorldSynch).enabled == true && frameIndex < 0) { + char szBuf[4096]=""; + sprintf(szBuf,"[attempting to BAIL OUT] finalPos [%s] newFinalPos [%s] ts [%d] canUnitMove [%d]", + finalPos.getString().c_str(),newFinalPos.getString().c_str(),ts,canUnitMove); + unit->logSynchData(__FILE__,__LINE__,szBuf); + } + + if(canUnitMove) { + //printf("$$$$ Unit BAILOUT(1) ASTAR ATTEMPT for [%d - %s] newFinalPos = [%s]\n",unit->getId(),unit->getFullName().c_str(),newFinalPos.getString().c_str()); + + int maxBailoutNodeCount = (PathFinder::pathFindBailoutRadius * 2); + ts= aStarFast(unit, newFinalPos, true, frameIndex, maxBailoutNodeCount); + } + } + } + } + else { + for(int bailoutX = PathFinder::pathFindBailoutRadius; bailoutX >= -PathFinder::pathFindBailoutRadius && ts == tsBlocked; --bailoutX) { + for(int bailoutY = PathFinder::pathFindBailoutRadius; bailoutY >= -PathFinder::pathFindBailoutRadius && ts == tsBlocked; --bailoutY) { + const Vec2i newFinalPos = finalPos + Vec2i(bailoutX,bailoutY); + bool canUnitMove = map->canMove(unit, unit->getPos(), newFinalPos); + + if(SystemFlags::getSystemSettingType(SystemFlags::debugWorldSynch).enabled == true && frameIndex < 0) { + char szBuf[4096]=""; + sprintf(szBuf,"[attempting to BAIL OUT] finalPos [%s] newFinalPos [%s] ts [%d] canUnitMove [%d]", + finalPos.getString().c_str(),newFinalPos.getString().c_str(),ts,canUnitMove); + unit->logSynchData(__FILE__,__LINE__,szBuf); + } + + if(canUnitMove) { + //printf("$$$$ Unit BAILOUT(1) ASTAR ATTEMPT for [%d - %s] newFinalPos = [%s]\n",unit->getId(),unit->getFullName().c_str(),newFinalPos.getString().c_str()); + int maxBailoutNodeCount = (PathFinder::pathFindBailoutRadius * 2); + ts= aStarFast(unit, newFinalPos, true, frameIndex, maxBailoutNodeCount); + } + } + } } - cellCount++; } } - } - unitImmediatelyBlocked = (failureCount == cellCount); + unit->setInBailOutAttempt(false); - //if(SystemFlags::getSystemSettingType(SystemFlags::debugPerformance).enabled == true && chrono.getMillis() > 1) SystemFlags::OutputDebug(SystemFlags::debugPerformance,"In [%s::%s Line: %d] **Check if dest blocked, distance for unit [%d - %s] from [%s] to [%s] is %.2f took msecs: %lld unitImmediatelyBlocked = %d, failureCount = %d\n",__FILE__,__FUNCTION__,__LINE__,unit->getId(),unit->getFullName().c_str(), unitPos.getString().c_str(), finalPos.getString().c_str(), dist,(long long int)chrono.getMillis(),unitImmediatelyBlocked,failureCount); - if(showConsoleDebugInfo && unitImmediatelyBlocked) { - printf("**Check if src blocked [%d], unit [%d - %s] from [%s] to [%s] unitImmediatelyBlocked = %d, failureCount = %d [%d]\n", - unitImmediatelyBlocked, unit->getId(),unit->getFullName().c_str(), unitPos.getString().c_str(), finalPos.getString().c_str(), unitImmediatelyBlocked,failureCount,cellCount); + //printf("$$$$ Unit END BAILOUT ATTEMPT for [%d - %s] ts = %d\n",unit->getId(),unit->getFullName().c_str(),ts); + + if(ts == tsBlocked) { + unit->setLastStuckFrameToCurrentFrame(); + unit->setLastStuckPos(finalPos); + } } -// if(unitImmediatelyBlocked == false) { -// // First check if final destination blocked -// failureCount = 0; -// cellCount = 0; -// -// for(int i = -1; i <= 1; ++i) { -// for(int j = -1; j <= 1; ++j) { -// Vec2i pos = finalPos + Vec2i(i, j); -// if(pos != finalPos) { -// bool canUnitMoveToCell = map->aproxCanMove(unit, pos, finalPos); -// if(canUnitMoveToCell == false) { -// failureCount++; -// } -// cellCount++; -// } -// } -// } -// unitImmediatelyBlocked = (failureCount == cellCount); -// -// if(SystemFlags::getSystemSettingType(SystemFlags::debugPerformance).enabled == true && chrono.getMillis() > 1) SystemFlags::OutputDebug(SystemFlags::debugPerformance,"In [%s::%s Line: %d] **Check if dest blocked, distance for unit [%d - %s] from [%s] to [%s] is %.2f took msecs: %lld unitImmediatelyBlocked = %d, failureCount = %d\n",__FILE__,__FUNCTION__,__LINE__,unit->getId(),unit->getFullName().c_str(), unitPos.getString().c_str(), finalPos.getString().c_str(), dist,(long long int)chrono.getMillis(),unitImmediatelyBlocked,failureCount); -// if(showConsoleDebugInfo && nodeLimitReached) { -// printf("**Check if dest blocked [%d - %d], unit [%d - %s] from [%s] to [%s] distance %.2f took msecs: %lld unitImmediatelyBlocked = %d, failureCount = %d [%d]\n", -// nodeLimitReached, inBailout, unit->getId(),unit->getFullName().c_str(), unitPos.getString().c_str(), finalPos.getString().c_str(), dist,(long long int)chrono.getMillis(),unitImmediatelyBlocked,failureCount,cellCount); -// } -// } - // - //!!! - - if(unitImmediatelyBlocked == false) { - int tryRadius = factions[unit->getFactionIndex()].random.randRange(0,1); - - // Try to bail out up to PathFinder::pathFindBailoutRadius cells away - if(tryRadius > 0) { - for(int bailoutX = -PathFinder::pathFindBailoutRadius; bailoutX <= PathFinder::pathFindBailoutRadius && ts == tsBlocked; ++bailoutX) { - for(int bailoutY = -PathFinder::pathFindBailoutRadius; bailoutY <= PathFinder::pathFindBailoutRadius && ts == tsBlocked; ++bailoutY) { - const Vec2i newFinalPos = finalPos + Vec2i(bailoutX,bailoutY); - bool canUnitMove = map->canMove(unit, unit->getPos(), newFinalPos); - - if(SystemFlags::getSystemSettingType(SystemFlags::debugWorldSynch).enabled == true && frameIndex < 0) { - char szBuf[4096]=""; - sprintf(szBuf,"[attempting to BAIL OUT] finalPos [%s] newFinalPos [%s] ts [%d] canUnitMove [%d]", - finalPos.getString().c_str(),newFinalPos.getString().c_str(),ts,canUnitMove); - unit->logSynchData(__FILE__,__LINE__,szBuf); - } - - if(canUnitMove) { - //printf("$$$$ Unit BAILOUT(1) ASTAR ATTEMPT for [%d - %s] newFinalPos = [%s]\n",unit->getId(),unit->getFullName().c_str(),newFinalPos.getString().c_str()); - - int maxBailoutNodeCount = (PathFinder::pathFindBailoutRadius * 2); - ts= aStar(unit, newFinalPos, true, frameIndex, maxBailoutNodeCount); - } + if(ts == tsArrived || ts == tsBlocked) { + if(frameIndex < 0) { + unit->setCurrSkill(scStop); + } + } + break; + case tsMoving: + { + if(dynamic_cast(path) != NULL) { + UnitPathBasic *basicPath = dynamic_cast(path); + Vec2i pos; + if(frameIndex < 0) { + pos = basicPath->pop(frameIndex < 0); + } + else { + if(factions[unit->getFactionIndex()].precachedPath[unit->getId()].size() <= 0) { + throw megaglest_runtime_error("factions[unit->getFactionIndex()].precachedPath[unit->getId()].size() <= 0!"); } + pos = factions[unit->getFactionIndex()].precachedPath[unit->getId()][0]; + } + + if(map->canMove(unit, unit->getPos(), pos)) { + if(frameIndex < 0) { + unit->setTargetPos(pos); + } + } + else { + if(frameIndex < 0) { + unit->setCurrSkill(scStop); + } + + if(minorDebugPathfinder) printf("Fast Pathfind Unit [%d - %s] INT BAILOUT ATTEMPT BLOCKED frameIndex = %d\n",unit->getId(),unit->getType()->getName().c_str(),frameIndex); + return tsBlocked; + } + } + else if(dynamic_cast(path) != NULL) { + UnitPath *advPath = dynamic_cast(path); + Vec2i pos= advPath->peek(); + if(map->canMove(unit, unit->getPos(), pos)) { + if(frameIndex < 0) { + advPath->pop(); + unit->setTargetPos(pos); + } + } + else { + if(frameIndex < 0) { + unit->setCurrSkill(scStop); + } + return tsBlocked; } } else { - for(int bailoutX = PathFinder::pathFindBailoutRadius; bailoutX >= -PathFinder::pathFindBailoutRadius && ts == tsBlocked; --bailoutX) { - for(int bailoutY = PathFinder::pathFindBailoutRadius; bailoutY >= -PathFinder::pathFindBailoutRadius && ts == tsBlocked; --bailoutY) { - const Vec2i newFinalPos = finalPos + Vec2i(bailoutX,bailoutY); - bool canUnitMove = map->canMove(unit, unit->getPos(), newFinalPos); + throw megaglest_runtime_error("unsupported or missing path finder detected!"); + } + } + break; + } - if(SystemFlags::getSystemSettingType(SystemFlags::debugWorldSynch).enabled == true && frameIndex < 0) { - char szBuf[4096]=""; - sprintf(szBuf,"[attempting to BAIL OUT] finalPos [%s] newFinalPos [%s] ts [%d] canUnitMove [%d]", - finalPos.getString().c_str(),newFinalPos.getString().c_str(),ts,canUnitMove); - unit->logSynchData(__FILE__,__LINE__,szBuf); + return ts; + } + else { + if(minorDebugPathfinder) printf("Legacy Pathfind Unit [%d - %s] from = %s to = %s frameIndex = %d\n",unit->getId(),unit->getType()->getName().c_str(),unit->getPos().getString().c_str(),finalPos.getString().c_str(),frameIndex); + + ts = aStar(unit, finalPos, false, frameIndex, maxNodeCount); + + //post actions + switch(ts) { + case tsBlocked: + case tsArrived: + + // The unit is stuck (not only blocked but unable to go anywhere for a while) + // We will try to bail out of the immediate area + if( ts == tsBlocked && unit->getInBailOutAttempt() == false && + path->isStuck() == true) { + + //printf("$$$$ Unit START BAILOUT ATTEMPT for [%d - %s]\n",unit->getId(),unit->getFullName().c_str()); + if(minorDebugPathfinder) printf("Legacy Pathfind Unit [%d - %s] START BAILOUT ATTEMPT frameIndex = %d\n",unit->getId(),unit->getType()->getName().c_str(),frameIndex); + + if(SystemFlags::getSystemSettingType(SystemFlags::debugWorldSynch).enabled == true && frameIndex < 0) { + char szBuf[4096]=""; + sprintf(szBuf,"[attempting to BAIL OUT] finalPos [%s] ts [%d]", + finalPos.getString().c_str(),ts); + unit->logSynchData(__FILE__,__LINE__,szBuf); + } + + if(wasStuck != NULL) { + *wasStuck = true; + } + unit->setInBailOutAttempt(true); + + bool useBailoutRadius = Config::getInstance().getBool("EnableBailoutPathfinding","true"); + if(useBailoutRadius == true) { + + //!!! + bool unitImmediatelyBlocked = false; + + // First check if unit currently blocked all around them, if so don't try to pathfind + const bool showConsoleDebugInfo = Config::getInstance().getBool("EnablePathfinderDistanceOutput","false"); + const Vec2i unitPos = unit->getPos(); + int failureCount = 0; + int cellCount = 0; + + for(int i = -1; i <= 1; ++i) { + for(int j = -1; j <= 1; ++j) { + Vec2i pos = unitPos + Vec2i(i, j); + if(pos != unitPos) { + bool canUnitMoveToCell = map->aproxCanMove(unit, unitPos, pos); + if(canUnitMoveToCell == false) { + failureCount++; + } + cellCount++; } + } + } + unitImmediatelyBlocked = (failureCount == cellCount); - if(canUnitMove) { - //printf("$$$$ Unit BAILOUT(1) ASTAR ATTEMPT for [%d - %s] newFinalPos = [%s]\n",unit->getId(),unit->getFullName().c_str(),newFinalPos.getString().c_str()); - int maxBailoutNodeCount = (PathFinder::pathFindBailoutRadius * 2); - ts= aStar(unit, newFinalPos, true, frameIndex, maxBailoutNodeCount); + //if(SystemFlags::getSystemSettingType(SystemFlags::debugPerformance).enabled == true && chrono.getMillis() > 1) SystemFlags::OutputDebug(SystemFlags::debugPerformance,"In [%s::%s Line: %d] **Check if dest blocked, distance for unit [%d - %s] from [%s] to [%s] is %.2f took msecs: %lld unitImmediatelyBlocked = %d, failureCount = %d\n",__FILE__,__FUNCTION__,__LINE__,unit->getId(),unit->getFullName().c_str(), unitPos.getString().c_str(), finalPos.getString().c_str(), dist,(long long int)chrono.getMillis(),unitImmediatelyBlocked,failureCount); + if(showConsoleDebugInfo && unitImmediatelyBlocked) { + printf("**Check if src blocked [%d], unit [%d - %s] from [%s] to [%s] unitImmediatelyBlocked = %d, failureCount = %d [%d]\n", + unitImmediatelyBlocked, unit->getId(),unit->getFullName().c_str(), unitPos.getString().c_str(), finalPos.getString().c_str(), unitImmediatelyBlocked,failureCount,cellCount); + } + + // if(unitImmediatelyBlocked == false) { + // // First check if final destination blocked + // failureCount = 0; + // cellCount = 0; + // + // for(int i = -1; i <= 1; ++i) { + // for(int j = -1; j <= 1; ++j) { + // Vec2i pos = finalPos + Vec2i(i, j); + // if(pos != finalPos) { + // bool canUnitMoveToCell = map->aproxCanMove(unit, pos, finalPos); + // if(canUnitMoveToCell == false) { + // failureCount++; + // } + // cellCount++; + // } + // } + // } + // unitImmediatelyBlocked = (failureCount == cellCount); + // + // if(SystemFlags::getSystemSettingType(SystemFlags::debugPerformance).enabled == true && chrono.getMillis() > 1) SystemFlags::OutputDebug(SystemFlags::debugPerformance,"In [%s::%s Line: %d] **Check if dest blocked, distance for unit [%d - %s] from [%s] to [%s] is %.2f took msecs: %lld unitImmediatelyBlocked = %d, failureCount = %d\n",__FILE__,__FUNCTION__,__LINE__,unit->getId(),unit->getFullName().c_str(), unitPos.getString().c_str(), finalPos.getString().c_str(), dist,(long long int)chrono.getMillis(),unitImmediatelyBlocked,failureCount); + // if(showConsoleDebugInfo && nodeLimitReached) { + // printf("**Check if dest blocked [%d - %d], unit [%d - %s] from [%s] to [%s] distance %.2f took msecs: %lld unitImmediatelyBlocked = %d, failureCount = %d [%d]\n", + // nodeLimitReached, inBailout, unit->getId(),unit->getFullName().c_str(), unitPos.getString().c_str(), finalPos.getString().c_str(), dist,(long long int)chrono.getMillis(),unitImmediatelyBlocked,failureCount,cellCount); + // } + // } + // + //!!! + + if(unitImmediatelyBlocked == false) { + int tryRadius = factions[unit->getFactionIndex()].random.randRange(0,1); + + // Try to bail out up to PathFinder::pathFindBailoutRadius cells away + if(tryRadius > 0) { + for(int bailoutX = -PathFinder::pathFindBailoutRadius; bailoutX <= PathFinder::pathFindBailoutRadius && ts == tsBlocked; ++bailoutX) { + for(int bailoutY = -PathFinder::pathFindBailoutRadius; bailoutY <= PathFinder::pathFindBailoutRadius && ts == tsBlocked; ++bailoutY) { + const Vec2i newFinalPos = finalPos + Vec2i(bailoutX,bailoutY); + bool canUnitMove = map->canMove(unit, unit->getPos(), newFinalPos); + + if(SystemFlags::getSystemSettingType(SystemFlags::debugWorldSynch).enabled == true && frameIndex < 0) { + char szBuf[4096]=""; + sprintf(szBuf,"[attempting to BAIL OUT] finalPos [%s] newFinalPos [%s] ts [%d] canUnitMove [%d]", + finalPos.getString().c_str(),newFinalPos.getString().c_str(),ts,canUnitMove); + unit->logSynchData(__FILE__,__LINE__,szBuf); + } + + if(canUnitMove) { + //printf("$$$$ Unit BAILOUT(1) ASTAR ATTEMPT for [%d - %s] newFinalPos = [%s]\n",unit->getId(),unit->getFullName().c_str(),newFinalPos.getString().c_str()); + + int maxBailoutNodeCount = (PathFinder::pathFindBailoutRadius * 2); + ts= aStar(unit, newFinalPos, true, frameIndex, maxBailoutNodeCount); + } + } + } + } + else { + for(int bailoutX = PathFinder::pathFindBailoutRadius; bailoutX >= -PathFinder::pathFindBailoutRadius && ts == tsBlocked; --bailoutX) { + for(int bailoutY = PathFinder::pathFindBailoutRadius; bailoutY >= -PathFinder::pathFindBailoutRadius && ts == tsBlocked; --bailoutY) { + const Vec2i newFinalPos = finalPos + Vec2i(bailoutX,bailoutY); + bool canUnitMove = map->canMove(unit, unit->getPos(), newFinalPos); + + if(SystemFlags::getSystemSettingType(SystemFlags::debugWorldSynch).enabled == true && frameIndex < 0) { + char szBuf[4096]=""; + sprintf(szBuf,"[attempting to BAIL OUT] finalPos [%s] newFinalPos [%s] ts [%d] canUnitMove [%d]", + finalPos.getString().c_str(),newFinalPos.getString().c_str(),ts,canUnitMove); + unit->logSynchData(__FILE__,__LINE__,szBuf); + } + + if(canUnitMove) { + //printf("$$$$ Unit BAILOUT(1) ASTAR ATTEMPT for [%d - %s] newFinalPos = [%s]\n",unit->getId(),unit->getFullName().c_str(),newFinalPos.getString().c_str()); + int maxBailoutNodeCount = (PathFinder::pathFindBailoutRadius * 2); + ts= aStar(unit, newFinalPos, true, frameIndex, maxBailoutNodeCount); + } + } } } } } - } - } - unit->setInBailOutAttempt(false); + unit->setInBailOutAttempt(false); - //printf("$$$$ Unit END BAILOUT ATTEMPT for [%d - %s] ts = %d\n",unit->getId(),unit->getFullName().c_str(),ts); + //printf("$$$$ Unit END BAILOUT ATTEMPT for [%d - %s] ts = %d\n",unit->getId(),unit->getFullName().c_str(),ts); - if(ts == tsBlocked) { - unit->setLastStuckFrameToCurrentFrame(); - unit->setLastStuckPos(finalPos); - } - } - if(ts == tsArrived || ts == tsBlocked) { - if(frameIndex < 0) { - unit->setCurrSkill(scStop); - } - } - break; - case tsMoving: - { - if(dynamic_cast(path) != NULL) { - UnitPathBasic *basicPath = dynamic_cast(path); - Vec2i pos; - if(frameIndex < 0) { - pos = basicPath->pop(frameIndex < 0); - } - else { - pos = factions[unit->getFactionIndex()].precachedPath[unit->getId()][0]; - } - - if(map->canMove(unit, unit->getPos(), pos)) { - if(frameIndex < 0) { - unit->setTargetPos(pos); + if(ts == tsBlocked) { + unit->setLastStuckFrameToCurrentFrame(); + unit->setLastStuckPos(finalPos); } } - else { + if(ts == tsArrived || ts == tsBlocked) { if(frameIndex < 0) { unit->setCurrSkill(scStop); } - return tsBlocked; } - } - else if(dynamic_cast(path) != NULL) { - UnitPath *advPath = dynamic_cast(path); - Vec2i pos= advPath->peek(); - if(map->canMove(unit, unit->getPos(), pos)) { - if(frameIndex < 0) { - advPath->pop(); - unit->setTargetPos(pos); + break; + case tsMoving: + { + if(dynamic_cast(path) != NULL) { + UnitPathBasic *basicPath = dynamic_cast(path); + Vec2i pos; + if(frameIndex < 0) { + pos = basicPath->pop(frameIndex < 0); + } + else { + if(factions[unit->getFactionIndex()].precachedPath[unit->getId()].size() <= 0) { + throw megaglest_runtime_error("factions[unit->getFactionIndex()].precachedPath[unit->getId()].size() <= 0!"); + } + + pos = factions[unit->getFactionIndex()].precachedPath[unit->getId()][0]; + } + + if(map->canMove(unit, unit->getPos(), pos)) { + if(frameIndex < 0) { + unit->setTargetPos(pos); + } + } + else { + if(frameIndex < 0) { + unit->setCurrSkill(scStop); + } + return tsBlocked; + } + } + else if(dynamic_cast(path) != NULL) { + UnitPath *advPath = dynamic_cast(path); + Vec2i pos= advPath->peek(); + if(map->canMove(unit, unit->getPos(), pos)) { + if(frameIndex < 0) { + advPath->pop(); + unit->setTargetPos(pos); + } + } + else { + if(frameIndex < 0) { + unit->setCurrSkill(scStop); + } + + if(minorDebugPathfinder) printf("Legacy Pathfind Unit [%d - %s] INT BAILOUT ATTEMPT BLOCKED frameIndex = %d\n",unit->getId(),unit->getType()->getName().c_str(),frameIndex); + return tsBlocked; + } + } + else { + throw megaglest_runtime_error("unsupported or missing path finder detected!"); } } - else { - if(frameIndex < 0) { - unit->setCurrSkill(scStop); - } - return tsBlocked; - } - } - else { - throw megaglest_runtime_error("unsupported or missing path finder detected!"); - } + break; } - break; + return ts; } - return ts; } // ==================== PRIVATE ==================== +TravelState PathFinder::aStarFast(Unit *unit, Vec2i finalPos, bool inBailout, int frameIndex, int maxNodeCount) { + TravelState ts = tsImpossible; + + Chrono chrono; + if(SystemFlags::getSystemSettingType(SystemFlags::debugPerformance).enabled) chrono.start(); + + if(maxNodeCount < 0) { + maxNodeCount = factions[unit->getFactionIndex()].useMaxNodeCount; + + //printf("AStar set maxNodeCount = %d\n",maxNodeCount); + } + + if(maxNodeCount >= 1 && unit->getPathfindFailedConsecutiveFrameCount() >= 3) { + int orgmaxNodeCount = maxNodeCount; + maxNodeCount = 200; + //printf("AStar maxpath cut for unit [%d - %s] to %d [orig: %d] [unit->getPathfindFailedConsecutiveFrameCount(): %d]\n",unit->getId(),unit->getFullName().c_str(), maxNodeCount,orgmaxNodeCount,unit->getPathfindFailedConsecutiveFrameCount()); + } + + UnitPathInterface *path= unit->getPath(); + int unitFactionIndex = unit->getFactionIndex(); + factions[unitFactionIndex].nodePoolCount= 0; + factions[unitFactionIndex].openNodesList.clear(); + factions[unitFactionIndex].openPosList.clear(); + factions[unitFactionIndex].closedNodesList.clear(); + + if(frameIndex >= 0) { + clearUnitPrecache(unit); + } + + finalPos= computeNearestFreePos(unit, finalPos); + + // Start of New Fast AStar + FastAINode *fromNode = map->getCellNode(unit->getPos()); + FastAINode *toNode = map->getCellNode(finalPos); + //FastAstar *fa = createFastAstar(); + FastAstar *fa = factions[unitFactionIndex].fa; + astarStartSearch(fa,fromNode,toNode, unit); + + bool pathFound = false; + unsigned int nodeSearchCount=0; + for(nodeSearchCount=0; pathFound == false && nodeSearchCount < maxNodeCount; ++nodeSearchCount) { + unsigned int search_count=0; + pathFound = astarSearchStep(fa,search_count); + //printf("Fast Pathfind Unit [%d - %s] finished = %d search_count = %d i = %d\n",unit->getId(),unit->getType()->getName().c_str(),finished,search_count,i); + } + + unsigned int count=0; + AI_Node **solution = getSolution(fa,count); + + if(count <= 1 || getLastSearchState(fa) != SEARCH_STATE_SUCCEEDED) { + if(minorDebugPathfinder) printf("Fast Pathfind Unit [%d - %s] NOT FOUND PATH frameIndex = %d nodeSearchCount = %d\n",unit->getId(),unit->getType()->getName().c_str(),frameIndex,nodeSearchCount); + + //blocked + if(SystemFlags::getSystemSettingType(SystemFlags::debugPathFinder).enabled == true) { + string commandDesc = "none"; + Command *command= unit->getCurrCommand(); + if(command != NULL && command->getCommandType() != NULL) { + commandDesc = command->getCommandType()->toString(); + } + + std::pair lastHarvest = unit->getLastHarvestResourceTarget(); + + char szBuf[4096]=""; + sprintf(szBuf,"State: blocked, cmd [%s] pos: [%s], lastHarvest = [%s - %d], reason C= %d, D= %d, F = %d", + commandDesc.c_str(),unit->getPos().getString().c_str(), lastHarvest.first.getString().c_str(),lastHarvest.second, path->getBlockCount(), path->isBlocked(), path->isStuck()); + unit->setCurrentUnitTitle(szBuf); + } + + if(frameIndex < 0) { + unit->setUsePathfinderExtendedMaxNodes(false); + } + + ts= tsBlocked; + if(frameIndex < 0) { + path->incBlockCount(); + } + + if(SystemFlags::getSystemSettingType(SystemFlags::debugWorldSynch).enabled == true && frameIndex < 0) { + char szBuf[4096]=""; + sprintf(szBuf,"[path for unit BLOCKED] openNodesList.size() [%lu] openPosList.size() [%lu] finalPos [%s] ts [%d]", + factions[unitFactionIndex].openNodesList.size(),factions[unitFactionIndex].openPosList.size(),finalPos.getString().c_str(),ts); + unit->logSynchData(__FILE__,__LINE__,szBuf); + } + + //if(SystemFlags::getSystemSettingType(SystemFlags::debugPerformance).enabled == true && chrono.getMillis() > 4) SystemFlags::OutputDebug(SystemFlags::debugPerformance,"In [%s::%s Line: %d] took msecs: %lld\n",__FILE__,__FUNCTION__,__LINE__,chrono.getMillis()); + + unit->setLastStuckFrameToCurrentFrame(); + unit->setLastStuckPos(finalPos); + + // Now see if the unit is eligble for pathfind max nodes boost? + if(nodeSearchCount >= maxNodeCount) { + unit->incrementPathfindFailedConsecutiveFrameCount(); + } + else { + unit->resetPathfindFailedConsecutiveFrameCount(); + } + + } + else { + if(minorDebugPathfinder) printf("Fast Pathfind Unit [%d - %s] FOUND PATH count = %d frameIndex = %d nodeSearchCount = %d\n",unit->getId(),unit->getType()->getName().c_str(),count,frameIndex,nodeSearchCount); + + ts= tsMoving; + + UnitPathInterface *path= unit->getPath(); + + //if(SystemFlags::getSystemSettingType(SystemFlags::debugPerformance).enabled == true && chrono.getMillis() > 4) SystemFlags::OutputDebug(SystemFlags::debugPerformance,"In [%s::%s Line: %d] took msecs: %lld\n",__FILE__,__FUNCTION__,__LINE__,chrono.getMillis()); + if(frameIndex < 0) { + if(maxNodeCount == pathFindNodesAbsoluteMax) { + unit->setUsePathfinderExtendedMaxNodes(true); + } + else { + unit->setUsePathfinderExtendedMaxNodes(false); + } + } + //store path + if(frameIndex < 0) { + path->clear(); + } + + UnitPathBasic *basicPathFinder = dynamic_cast(path); + + for(int i=0; i < count; ++i) { + FastAINode *node = dynamic_cast(solution[i]); + const Vec2i &nodePos = node->getPos(); + if(nodePos != unit->getPos()) { + if(map->isInside(nodePos) == false || map->isInsideSurface(map->toSurfCoords(nodePos)) == false) { + throw megaglest_runtime_error("Pathfinder invalid node path position = " + nodePos.getString() + " i = " + intToStr(i)); + } + + //if(minorDebugPathfinder) printf("nodePos [%s]\n",nodePos.getString().c_str()); + + if(frameIndex >= 0) { + factions[unitFactionIndex].precachedPath[unit->getId()].push_back(nodePos); + } + else { + if(i < pathFindRefresh) { + //if(i < pathFindRefresh || + // (whileLoopCount >= pathFindExtendRefreshForNodeCount && + // i < getPathFindExtendRefreshNodeCount(unitFactionIndex))) { + path->add(nodePos); + } + //else if(tryLastPathCache == false) { + // break; + //} + + //if(tryLastPathCache == true && basicPathFinder) { + if(basicPathFinder) { + basicPathFinder->addToLastPathCache(nodePos); + } + } + } + } + + //if(SystemFlags::getSystemSettingType(SystemFlags::debugPerformance).enabled == true && chrono.getMillis() > 4) SystemFlags::OutputDebug(SystemFlags::debugPerformance,"In [%s::%s Line: %d] took msecs: %lld\n",__FILE__,__FUNCTION__,__LINE__,chrono.getMillis()); + + if(SystemFlags::getSystemSettingType(SystemFlags::debugWorldSynch).enabled == true && frameIndex < 0) { + char szBuf[4096]=""; + sprintf(szBuf,"[Setting new path for unit] openNodesList.size() [%lu] openPosList.size() [%lu] finalPos [%s] ts [%d]", + factions[unitFactionIndex].openNodesList.size(),factions[unitFactionIndex].openPosList.size(),finalPos.getString().c_str(),ts); + unit->logSynchData(__FILE__,__LINE__,szBuf); + + string pathToTake = ""; + for(int i = 0; i < path->getQueueCount(); ++i) { + Vec2i &pos = path->getQueue()[i]; + if(pathToTake != "") { + pathToTake += ", "; + } + pathToTake += pos.getString(); + } + unit->logSynchData(__FILE__,__LINE__,szBuf); + sprintf(szBuf,"Path for unit to take = %s",pathToTake.c_str()); + } + + if(SystemFlags::getSystemSettingType(SystemFlags::debugPathFinder).enabled == true) { + string commandDesc = "none"; + Command *command= unit->getCurrCommand(); + if(command != NULL && command->getCommandType() != NULL) { + commandDesc = command->getCommandType()->toString(); + } + + char szBuf[1024]=""; + sprintf(szBuf,"State: moving, cmd [%s] pos: %s, Queue= %d",commandDesc.c_str(),unit->getPos().getString().c_str(), path->getQueueCount()); + unit->setCurrentUnitTitle(szBuf); + } + + //if(SystemFlags::getSystemSettingType(SystemFlags::debugPerformance).enabled == true && chrono.getMillis() > 4) SystemFlags::OutputDebug(SystemFlags::debugPerformance,"In [%s::%s Line: %d] took msecs: %lld\n",__FILE__,__FUNCTION__,__LINE__,chrono.getMillis()); + } + //releaseFastAstar(fa); + + // End of New Fast AStar + + factions[unitFactionIndex].openNodesList.clear(); + factions[unitFactionIndex].openPosList.clear(); + factions[unitFactionIndex].closedNodesList.clear(); + + //if(SystemFlags::getSystemSettingType(SystemFlags::debugPerformance).enabled == true && chrono.getMillis() > 4) SystemFlags::OutputDebug(SystemFlags::debugPerformance,"In [%s::%s] Line: %d took msecs: %lld --------------------------- [END OF METHOD] ---------------------------\n",__FILE__,__FUNCTION__,__LINE__,chrono.getMillis()); + + if(frameIndex >= 0) { + factions[unitFactionIndex].precachedTravelState[unit->getId()] = ts; + } + //else { + if(SystemFlags::getSystemSettingType(SystemFlags::debugPerformance).enabled && chrono.getMillis() >= 1) printf("In [%s::%s Line: %d] fastastar took [%lld] msecs, ts = %d nodeSearchCount = %d.\n",__FILE__,__FUNCTION__,__LINE__,(long long int)chrono.getMillis(),ts,nodeSearchCount); + //if(chrono.getMillis() >= 1) printf("In [%s::%s Line: %d] fastastar took [%lld] msecs, ts = %d nodeSearchCount = %d.\n",__FILE__,__FUNCTION__,__LINE__,(long long int)chrono.getMillis(),ts,nodeSearchCount); + //} + + return ts; +} + bool PathFinder::addToOpenSet(Unit *unit, Node *node,const Vec2i finalPos, Vec2i sucPos, bool &nodeLimitReached,int maxNodeCount,Node **newNodeAdded, bool bypassChecks) { bool result = false; @@ -1152,6 +1594,8 @@ TravelState PathFinder::aStar(Unit *unit, const Vec2i &targetPos, bool inBailout //check results of path finding ts = tsImpossible; if(pathFound == false || lastNode == firstNode) { + if(minorDebugPathfinder) printf("Legacy Pathfind Unit [%d - %s] NOT FOUND PATH count = %d frameIndex = %d\n",unit->getId(),unit->getType()->getName().c_str(),whileLoopCount,frameIndex); + //blocked if(SystemFlags::getSystemSettingType(SystemFlags::debugPathFinder).enabled == true) { string commandDesc = "none"; @@ -1186,6 +1630,7 @@ TravelState PathFinder::aStar(Unit *unit, const Vec2i &targetPos, bool inBailout if(SystemFlags::getSystemSettingType(SystemFlags::debugPerformance).enabled == true && chrono.getMillis() > 4) SystemFlags::OutputDebug(SystemFlags::debugPerformance,"In [%s::%s Line: %d] took msecs: %lld\n",__FILE__,__FUNCTION__,__LINE__,chrono.getMillis()); } else { + if(minorDebugPathfinder) printf("Legacy Pathfind Unit [%d - %s] FOUND PATH count = %d frameIndex = %d\n",unit->getId(),unit->getType()->getName().c_str(),whileLoopCount,frameIndex); //on the way ts= tsMoving; @@ -1226,6 +1671,7 @@ TravelState PathFinder::aStar(Unit *unit, const Vec2i &targetPos, bool inBailout } //printf("nodePos [%s]\n",nodePos.getString().c_str()); + if(minorDebugPathfinder) printf("nodePos [%s]\n",nodePos.getString().c_str()); if(frameIndex >= 0) { factions[unitFactionIndex].precachedPath[unit->getId()].push_back(nodePos); diff --git a/source/glest_game/ai/path_finder.h b/source/glest_game/ai/path_finder.h index 0417cf61..1fa8efae 100644 --- a/source/glest_game/ai/path_finder.h +++ b/source/glest_game/ai/path_finder.h @@ -24,7 +24,7 @@ #include "skill_type.h" #include "map.h" #include "unit.h" - +#include "fast_path_finder.h" #include "leak_dumper.h" using std::vector; @@ -111,6 +111,11 @@ public: //mapFromToNodeList.clear(); //lastFromToNodeListFrame = -100; badCellList.clear(); + + fa = createFastAstar(); + } + ~FactionState() { + //fa = NULL; } std::map openPosList; std::map openNodesList; @@ -127,6 +132,8 @@ public: //std::map > > mapFromToNodeList; std::map > badCellList; + + FastAstar *fa; }; typedef vector FactionStateList; @@ -145,6 +152,7 @@ private: FactionStateList factions; const Map *map; + bool minorDebugPathfinder; public: PathFinder(); @@ -162,6 +170,7 @@ public: void loadGame(const XmlNode *rootNode); private: + TravelState aStarFast(Unit *unit, Vec2i finalPos, bool inBailout, int frameIndex, int maxNodeCount=-1); TravelState aStar(Unit *unit, const Vec2i &finalPos, bool inBailout, int frameIndex, int maxNodeCount=-1); //Node *newNode(FactionState &faction,int maxNodeCount); inline static Node *newNode(FactionState &faction, int maxNodeCount) { diff --git a/source/glest_game/type_instances/unit.cpp b/source/glest_game/type_instances/unit.cpp index 88dba8d5..bdf41369 100644 --- a/source/glest_game/type_instances/unit.cpp +++ b/source/glest_game/type_instances/unit.cpp @@ -3776,6 +3776,8 @@ void Unit::saveGame(XmlNode *rootNode) { //pathfindFailedConsecutiveFrameCount unitNode->addAttribute("pathfindFailedConsecutiveFrameCount",intToStr(pathfindFailedConsecutiveFrameCount), mapTagReplacements); + + unitNode->addAttribute("currentPathFinderDesiredFinalPos",currentPathFinderDesiredFinalPos.getString(), mapTagReplacements); } Unit * Unit::loadGame(const XmlNode *rootNode, GameSettings *settings, Faction *faction, World *world) { @@ -4189,6 +4191,10 @@ Unit * Unit::loadGame(const XmlNode *rootNode, GameSettings *settings, Faction * } result->meetingPos = Vec2i::strToVec2(unitNode->getAttribute("meetingPos")->getValue()); + + if(unitNode->hasAttribute("currentPathFinderDesiredFinalPos")) { + result->currentPathFinderDesiredFinalPos = Vec2i::strToVec2(unitNode->getAttribute("currentPathFinderDesiredFinalPos")->getValue()); + } return result; } diff --git a/source/glest_game/type_instances/unit.h b/source/glest_game/type_instances/unit.h index 3f56cae5..b60464d5 100644 --- a/source/glest_game/type_instances/unit.h +++ b/source/glest_game/type_instances/unit.h @@ -437,6 +437,7 @@ private: CauseOfDeathType causeOfDeath; uint32 pathfindFailedConsecutiveFrameCount; + Vec2i currentPathFinderDesiredFinalPos; public: Unit(int id, UnitPathInterface *path, const Vec2i &pos, const UnitType *type, Faction *faction, Map *map, CardinalDir placeFacing); @@ -446,6 +447,9 @@ public: static void setGame(Game *value) { game=value;} + void setCurrentPathFinderDesiredFinalPos(const Vec2i &finalPos) { currentPathFinderDesiredFinalPos = finalPos; } + Vec2i getCurrentPathFinderDesiredFinalPos() const { return currentPathFinderDesiredFinalPos; } + //const std::pair > & getCurrentAttackBoostUnits() const { return currentAttackBoostUnits; } const UnitAttackBoostEffectOriginator & getAttackBoostOriginatorEffect() const { return currentAttackBoostOriginatorEffect; } bool unitHasAttackBoost(const AttackBoost *boost, const Unit *source) const; diff --git a/source/glest_game/world/map.cpp b/source/glest_game/world/map.cpp index ddf1dca7..33d61316 100644 --- a/source/glest_game/world/map.cpp +++ b/source/glest_game/world/map.cpp @@ -279,6 +279,88 @@ void SurfaceCell::loadGame(const XmlNode *rootNode, int index, World *world) { // class Map // ===================================================== +FastAINode * FastAINode::getNodeForEdgeIndex(int index,void *userData) const { + FastAINode *resultNode = NULL; + switch(index) { + case 0: // north + resultNode = map->getCellNode(pos.x,pos.y-1,false); + break; + case 1: // north east + resultNode = map->getCellNode(pos.x+1,pos.y-1,false); + break; + case 2: // east + resultNode = map->getCellNode(pos.x+1,pos.y,false); + break; + case 3: // south east + resultNode = map->getCellNode(pos.x+1,pos.y+1,false); + break; + case 4: // south + resultNode = map->getCellNode(pos.x,pos.y+1,false); + break; + case 5: // south west + resultNode = map->getCellNode(pos.x-1,pos.y+1,false); + break; + case 6: // west + resultNode = map->getCellNode(pos.x-1,pos.y,false); + break; + case 7: // north west + resultNode = map->getCellNode(pos.x-1,pos.y-1,false); + break; + } + if(resultNode != NULL) { + Unit *unit = (Unit *)userData; + if(resultNode->getPos() != unit->getCurrentPathFinderDesiredFinalPos()) { + if(map->aproxCanMoveSoon(unit, pos, resultNode->getPos()) == false) { + resultNode = NULL; + } + } + } + + return resultNode; +} + +float FastAINode::getDistance(const AI_Node *node,void *userData) { + return pos.dist(dynamic_cast(node)->pos); +} +float FastAINode::getCost(void *userData) { + return 1.0f; +} + +unsigned int FastAINode::getEdgeCount(void *userData) const { + unsigned int result = 0; + for(unsigned int index = 0; index < NODE_EDGE_COUNT; ++index) { + FastAINode *resultNode = getNodeForEdgeIndex(index,userData); + if(resultNode != NULL) { + result++; + } + } + return result; +} + +AI_Node * FastAINode::getEdge(int index,void *userData) const { + FastAINode *result = NULL; + + unsigned int edgeCount = getEdgeCount(userData); + if(edgeCount < NODE_EDGE_COUNT) { + int edgeIndex = -1; + + for(unsigned int index2 = 0; index2 < NODE_EDGE_COUNT; ++index2) { + result = getNodeForEdgeIndex(index2,userData); + if(result != NULL) { + edgeIndex++; + + if(edgeIndex == index) { + return result; + } + } + result = NULL; + } + return result; + } + result = getNodeForEdgeIndex(index,userData); + return result; +}; + // ===================== PUBLIC ======================== const int Map::cellScale= 2; @@ -287,6 +369,7 @@ const int Map::mapScale= 2; Map::Map() { cells= NULL; surfaceCells= NULL; + cellNodes=NULL; startLocations= NULL; title=""; @@ -309,6 +392,8 @@ Map::~Map() { cells = NULL; delete [] surfaceCells; surfaceCells = NULL; + delete [] cellNodes; + cellNodes = NULL; delete [] startLocations; startLocations = NULL; } @@ -406,6 +491,14 @@ Checksum Map::load(const string &path, TechTree *techTree, Tileset *tileset) { //cells cells= new Cell[getCellArraySize()]; + cellNodes = new FastAINode[getCellArraySize()]; + for(unsigned int x = 0; x < w; ++x) { + for(unsigned int y = 0; y < h; ++y) { + int arrayIndex = y * w + x; + FastAINode &cellNode = cellNodes[arrayIndex]; + cellNode.setData(Vec2i(x,y),this); + } + } surfaceCells= new SurfaceCell[getSurfaceCellArraySize()]; //read heightmap diff --git a/source/glest_game/world/map.h b/source/glest_game/world/map.h index 4c052315..ad764d14 100644 --- a/source/glest_game/world/map.h +++ b/source/glest_game/world/map.h @@ -26,6 +26,7 @@ #include "selection.h" #include #include "unit_type.h" +#include "fast_path_finder.h" #include "leak_dumper.h" @@ -192,6 +193,35 @@ public: /// Represents the game map (and loads it from a gbm file) // ===================================================== +class FastAINode : public AI_Node { +protected: + Vec2i pos; + const Map *map; + static const int NODE_EDGE_COUNT = 8; + + FastAINode * getNodeForEdgeIndex(int index,void *userData) const; + +public: + + FastAINode() { + this->map = NULL; + } + FastAINode(Vec2i &pos,const Map *map) { + this->pos = pos; + this->map = map; + } + void setData(Vec2i pos, const Map *map) { + this->pos = pos; + this->map = map; + } + inline const Vec2i & getPos() const { return pos; } + + virtual float getDistance(const AI_Node *node, void *userData); + virtual float getCost(void *userData); + virtual unsigned int getEdgeCount(void *userData) const; + virtual AI_Node * getEdge(int index, void *userData) const; +}; + class Map { public: static const int cellScale; //number of cells per surfaceCell @@ -210,6 +240,7 @@ private: int maxPlayers; Cell *cells; SurfaceCell *surfaceCells; + FastAINode *cellNodes; Vec2i *startLocations; Checksum checksumValue; float maxMapHeight; @@ -229,13 +260,20 @@ public: Checksum load(const string &path, TechTree *techTree, Tileset *tileset); //get - inline Cell *getCell(int x, int y) const { + inline Cell *getCell(int x, int y, bool errorOnInvalid=true) const { int arrayIndex = y * w + x; if(arrayIndex < 0 || arrayIndex >= getCellArraySize()) { + if(errorOnInvalid == false) { + return NULL; + } //abort(); throw megaglest_runtime_error("arrayIndex >= getCellArraySize(), arrayIndex = " + intToStr(arrayIndex) + " w = " + intToStr(w) + " h = " + intToStr(h)); } else if(cells == NULL) { + if(errorOnInvalid == false) { + return NULL; + } + throw megaglest_runtime_error("cells == NULL"); } @@ -245,6 +283,30 @@ public: return getCell(pos.x, pos.y); } + //get + inline FastAINode *getCellNode(Vec2i pos, bool errorOnInvalid=true) const { + return getCellNode(pos.x, pos.y, errorOnInvalid); + } + inline FastAINode *getCellNode(int x, int y, bool errorOnInvalid=true) const { + int arrayIndex = y * w + x; + if(arrayIndex < 0 || arrayIndex >= getCellArraySize()) { + if(errorOnInvalid == false) { + return NULL; + } + //abort(); + throw megaglest_runtime_error("arrayIndex >= getCellArraySize(), arrayIndex = " + intToStr(arrayIndex) + " w = " + intToStr(w) + " h = " + intToStr(h)); + } + else if(cellNodes == NULL) { + if(errorOnInvalid == false) { + return NULL; + } + + throw megaglest_runtime_error("cellNodes == NULL"); + } + + return &cellNodes[arrayIndex]; + } + inline int getCellArraySize() const { return (w * h); }