server/common/micropather.C

/*
Copyright (c) 2000-2005 Lee Thomason (www.grinninglizard.com)

Grinning Lizard Utilities.

This software is provided 'as-is', without any express or implied 
warranty. In no event will the authors be held liable for any 
damages arising from the use of this software.

Permission is granted to anyone to use this software for any 
purpose, including commercial applications, and to alter it and 
redistribute it freely, subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must 
not claim that you wrote the original software. If you use this 
software in a product, an acknowledgment in the product documentation 
would be appreciated but is not required.

2. Altered source versions must be plainly marked as such, and 
must not be misrepresented as being the original software.

3. This notice may not be removed or altered from any source 
distribution.
*/

#ifdef _MSC_VER
#pragma warning( disable : 4786 )       // Debugger truncating names.
#pragma warning( disable : 4530 )       // Exception handler isn't used
#endif

#include <cstring>
#include <cstdio>

#include <vector>
#include <map>
#include <algorithm>
#include <functional>

//#define DEBUG_PATH
//#define DEBUG_PATH_DEEP

#include "micropather.h"

using namespace std;
using namespace micropather;

template<class _Ty>
        struct PathNodeCompareTotalCost : binary_function<_Ty, _Ty, bool> {
        bool operator()(const _Ty& _X, const _Ty& _Y) const
                {return (_X->totalCost > _Y->totalCost ); }
        };

class OpenQueue
{
  public:
        OpenQueue( Graph* _graph )
        { 
                graph = _graph; 
                #ifdef USE_LIST
                sentinel = (PathNode*) sentinelMem;
                sentinel->Init( 0, 0, FLT_MAX, FLT_MAX, 0 );
                sentinel->totalCost = FLT_MAX;
                sentinel->next = sentinel;
                sentinel->prev = sentinel;
                        #ifdef DEBUG
                                sentinel->CheckList();
                        #endif
                #endif
        }
        ~OpenQueue()    {}

        void Push( PathNode* pNode )
        {

                assert( pNode->inOpen == 0 );
                assert( pNode->inClosed == 0 );

                #ifdef DEBUG_PATH_DEEP
                        printf( "Open Push: " );
                        graph->PrintStateInfo( pNode->state );
                        printf( " total=%.1f\n", pNode->totalCost );            
                #endif

                #ifdef USE_LIST
                // Add sorted. Lowest to highest cost path. Note that the sentinel has
                // a value of FLT_MAX, so it should always be sorted in.
                assert( pNode->totalCost < FLT_MAX );
                PathNode* iter = sentinel->next;
                while ( true )
                {
                        if ( pNode->totalCost < iter->totalCost ) {
                                iter->AddBefore( pNode );
                                pNode->inOpen = 1;
                                break;
                        }
                        iter = iter->next;
                }
                assert( pNode->inOpen );        // make sure this was actually added.
                        #ifdef DEBUG
                                sentinel->CheckList();
                        #endif
                #else
                heapVector.push_back( pNode );
                #endif
        }

        PathNode* Pop()
        {
                #ifdef USE_LIST
                assert( sentinel->next != sentinel );
                PathNode* pNode = sentinel->next;
                pNode->Unlink();
                        #ifdef DEBUG
                                sentinel->CheckList();
                        #endif
                #else
                PathNode* pNode = heapVector[0];

                const int size = heapVector.size();
                int found = 0;
                for( int i=1; i<size; ++i ) {
                        if ( heapVector[i]->totalCost < pNode->totalCost ) {
                                pNode = heapVector[i];
                                found = i;
                        }                               
                }   
                if ( found < size-1 ) 
                        memcpy( &heapVector[found], &heapVector[found+1], sizeof( PathNode* ) * (size-found-1) );
                heapVector.pop_back();
                #endif

                assert( pNode->inClosed == 0 );
                assert( pNode->inOpen == 1 );
                pNode->inOpen = 0;

                #ifdef DEBUG_PATH_DEEP
                        printf( "Open Pop: " );
                        graph->PrintStateInfo( pNode->state );
                        printf( " total=%.1f\n", pNode->totalCost );            
                #endif

                return pNode;
        }

        void Update( PathNode* pNode )
        {
                #ifdef DEBUG_PATH_DEEP
                        printf( "Open Update: " );              
                        graph->PrintStateInfo( pNode->state );
                        printf( " total=%.1f\n", pNode->totalCost );            
                #endif
                
                #ifdef USE_LIST
                assert( pNode->inOpen );

                // If the node now cost less than the one before it,
                // move it to the front of the list.
                if ( pNode->prev != sentinel && pNode->totalCost < pNode->prev->totalCost ) {
                        pNode->Unlink();
                        sentinel->next->AddBefore( pNode );
                }

                // If the node is too high, move to the right.
                if ( pNode->totalCost > pNode->next->totalCost ) {
                        PathNode* it = pNode->next;
                        pNode->Unlink();

                        while ( pNode->totalCost > it->totalCost )
                                it = it->next;

                        it->AddBefore( pNode );
                                #ifdef DEBUG
                                        sentinel->CheckList();
                                #endif
                }
                #endif
        }
    
        #ifdef USE_LIST
        bool Empty()    { return sentinel->next == sentinel; }
        #else
        bool Empty()    { return heapVector.empty(); }
        #endif

  private:
        OpenQueue( const OpenQueue& );  // undefined and unsupported
        void operator=( const OpenQueue& );
  
        #ifdef USE_LIST
        PathNode* sentinel;
        int sentinelMem[ ( sizeof( PathNode ) + sizeof( int ) ) / sizeof( int ) ];
        #else
        vector< PathNode* > heapVector;
        #endif
        Graph* graph;   // for debugging
};


class ClosedSet
{
  public:
        ClosedSet( Graph* _graph )              { this->graph = _graph; }
        ~ClosedSet()    {}

        void Add( PathNode* pNode )
        {
                #ifdef DEBUG_PATH_DEEP
                        printf( "Closed add: " );               
                        graph->PrintStateInfo( pNode->state );
                        printf( " total=%.1f\n", pNode->totalCost );            
                #endif
                #ifdef DEBUG
                assert( pNode->inClosed == 0 );
                assert( pNode->inOpen == 0 );
                #endif
                pNode->inClosed = 1;
        }

        void Remove( PathNode* pNode )
        {
                #ifdef DEBUG_PATH_DEEP
                        printf( "Closed remove: " );            
                        graph->PrintStateInfo( pNode->state );
                        printf( " total=%.1f\n", pNode->totalCost );            
                #endif
                assert( pNode->inClosed == 1 );
                assert( pNode->inOpen == 0 );

                pNode->inClosed = 0;
        }

  private:
        ClosedSet( const ClosedSet& );
        void operator=( const ClosedSet& );
        Graph* graph;
};


MicroPather::MicroPather( Graph* _graph, unsigned allocate )
        : ALLOCATE( allocate ),
          BLOCKSIZE( allocate-1 ),
          graph( _graph ),
          pathNodeMem( 0 ),
          availMem( 0 ),
          pathNodeCount( 0 ),
          frame( 0 ),
          checksum( 0 )
{
        memset( hashTable, 0, sizeof( PathNode* )*HASH_SIZE );
}


MicroPather::~MicroPather()
{
        PathNode *temp;

        while( pathNodeMem ) {
                temp = pathNodeMem;
                pathNodeMem = pathNodeMem[ ALLOCATE-1 ].left;
                free( temp );   // c-allocation
        }
}


void MicroPather::DumpHashTable()
{
        for( int i=0; i<HASH_SIZE; ++i )
        {
        int count = 0;
        RecPathNodeWalk( hashTable[i], &count );
        if ( count != 0 )
           printf( "Hashtable[%3d] has %d items\n", i, count );
    }    
}    

void MicroPather::RecPathNodeWalk( PathNode* node, int* count )
{
    if ( node ) 
    {
        *count += 1;
        if ( node->left )
            RecPathNodeWalk( node->left, count );
                #ifdef USE_BINARY_HASH
        if ( node->right )
            RecPathNodeWalk( node->right, count );
                #endif
    }
}

    
void MicroPather::Reset()
{
        while( pathNodeMem ) {
                PathNode* temp = pathNodeMem;
                pathNodeMem = pathNodeMem[ ALLOCATE-1 ].left;
                free( temp );   // c-allocation
        }
        pathNodeMem = 0;
        memset( hashTable, 0, sizeof( PathNode* )*HASH_SIZE );
        pathNodeMem = 0;
        availMem = 0;
        pathNodeCount = 0;
        frame = 0;
        checksum = 0;
}


//void MicroPather::StateCostChange( void* state )
//{
//      PathNode* node = FindPathNode( state, true );
//      if ( node ) {
//              node->numAdjacent = -1; // throw away any cached data.
//      }
//}


PathNode* MicroPather::AllocatePathNode() 
{
        if ( availMem == 0 ) {
                PathNode* newBlock = (PathNode*) malloc( sizeof(PathNode) * ALLOCATE );
                // set up the "next" pointer
                newBlock[ALLOCATE-1].left = pathNodeMem;
                pathNodeMem = newBlock;
                availMem = BLOCKSIZE;
        }
        assert( availMem );

        PathNode* result = pathNodeMem + (BLOCKSIZE     - availMem );
        --availMem;
        ++pathNodeCount;
        return result;
}


PathNode* MicroPather::NewPathNode( void* state, float costFromStart, float estToEnd, PathNode* parent )
{
        // Try to find an existing node for this state.
        unsigned key = Hash( state );   //(HASH_SIZE-1) & ( (unsigned)state + (((unsigned)state)>>8) + (((unsigned)state)>>16) + (((unsigned)state)>>24) );

        if ( !hashTable[key] ) {
                // There isn't even a hashtable yet - create and initialize the PathNode.
                hashTable[key] = AllocatePathNode();
                hashTable[key]->Init( frame, state, costFromStart, estToEnd, parent );
                return hashTable[key];
        }

        PathNode* root = hashTable[key];
        PathNode* up = 0;
        while ( root ) {
                up = root;
                if ( root->state == state ) {
                        root->Reuse( frame, costFromStart, estToEnd, parent );
                        assert( root->state == state );
                        return root;
                }
                #ifdef USE_BINARY_HASH
                else if ( state > root->state ) {
                        root = root->right;
                }
                #endif
                else {
                        #ifdef USE_BINARY_HASH
                        assert( state < root->state );
                        #endif
                        root = root->left;
                }
        }

        assert( up );
        PathNode* pNode = AllocatePathNode();
        pNode->Init( frame, state, costFromStart, estToEnd, parent );
        #ifdef USE_BINARY_HASH
        if ( state > up->state ) {
                assert( up->right == 0 );
                up->right = pNode;
        }
        else {
                assert( up->left == 0 );
                up->left = pNode;
        }
        #else
        up->left = pNode;
        #endif
        return pNode;
}


void MicroPather::GoalReached( PathNode* node, void* start, void* end, vector< void* > *path )
{
        path->clear();

        // We have reached the goal.
        // How long is the path? Used to allocate the vector which is returned.
        int count = 1;
        PathNode* it = node;
        while( it->parent )
        {
                ++count;
                it = it->parent;
        }                       

        // Now that the path has a known length, allocate
        // and fill the vector that will be returned.
        if ( count < 3 )
        {
                // Handle the short, special case.
                path->resize(2);
                (*path)[0] = start;
                (*path)[1] = end;
        }
        else
        {
                path->resize(count);

                (*path)[0] = start;
                (*path)[count-1] = end;

                count-=2;
                it = node->parent;

                while ( it->parent )
                {
                        (*path)[count] = it->state;
                        it = it->parent;
                        --count;
                }
        }
        checksum = 0;
        #ifdef DEBUG_PATH
        printf( "Path: " );
        int counter=0;
        #endif
        for ( unsigned k=0; k<path->size(); ++k )
        {
                checksum += ((UPTR)((*path)[k])) << (k%8);

                #ifdef DEBUG_PATH
                graph->PrintStateInfo( (*path)[k] );
                printf( " " );
                ++counter;
                if ( counter == 8 )
                {
                        printf( "\n" );
                        counter = 0;
                }
                #endif
        }
        #ifdef DEBUG_PATH
        printf( "Cost=%.1f Checksum %d\n", node->costFromStart, checksum );
        #endif
}


NodeCost* MicroPather::GetNodeNeighbors( PathNode* node, std::vector< StateCost >* neighbor, std::vector< NodeCost >* neighborNode )
{
        NodeCost* nodeCost = 0;

        if (    node->numAdjacent < 0 
                 || node->numAdjacent > PathNode::MAX_CACHE )
        {
                // It has not been computed yet (<0) or
                // can not be cached (>MAX)
                neighbor->resize( 0 );
                graph->AdjacentCost( node->state, neighbor );
                #ifdef DEBUG
                {
                        // If this assert fires, you have passed a state
                        // as its own neighbor state. This is impossible --
                        // bad things will happen.
                        for ( unsigned i=0; i<neighbor->size(); ++i )
                                assert( (*neighbor)[i].state != node->state );
                }
                #endif
                node->numAdjacent = (int) neighbor->size();

                // Now convert to pathNodes, and put in cache if possible.
                if ( node->numAdjacent <= PathNode::MAX_CACHE ) 
                {
                        // Can fit in the cache:
                        for( int i=0; i<node->numAdjacent; ++i ) 
                        {
                                node->adjacent[i].cost = (*neighbor)[i].cost;
                                node->adjacent[i].node = FindPathNode( (*neighbor)[i].state );
                                if ( !node->adjacent[i].node ) 
                                {
                                        node->adjacent[i].node = NewPathNode( (*neighbor)[i].state, FLT_BIG, FLT_BIG, 0 );
                                }
                        }
                        nodeCost = node->adjacent;

                        #ifdef DEBUG_PATH_DEEP
                                printf( "State " );
                                graph->PrintStateInfo( node->state );
                                printf( "--> cache\n" );
                        #endif
                }
                else {
                        // Too big for cache.
                        node->numAdjacent = (int) neighbor->size();

                        neighborNode->resize( neighbor->size() );

                        for( unsigned i=0; i<neighborNode->size(); ++i ) {
                                (*neighborNode)[i].cost = (*neighbor)[i].cost;
                                (*neighborNode)[i].node = FindPathNode( (*neighbor)[i].state );
                                if ( !(*neighborNode)[i].node ) 
                                {
                                        (*neighborNode)[i].node = NewPathNode( (*neighbor)[i].state, FLT_BIG, FLT_BIG, 0 );
                                }
                        }
                        nodeCost = &(*neighborNode)[0];
                        #ifdef DEBUG_PATH_DEEP
                                printf( "State " );
                                graph->PrintStateInfo( node->state );
                                printf( "no cache\n" );
                        #endif
                }
        }
        else {
                // In the cache!
                nodeCost = node->adjacent;

                for( int i=0; i<node->numAdjacent; ++i ) {
                        if ( node->adjacent[i].node->frame != frame )
                                node->adjacent[i].node->Reuse( frame, FLT_BIG, FLT_BIG, 0 );
                }
                #ifdef DEBUG_PATH_DEEP
                        printf( "State " );
                        graph->PrintStateInfo( node->state );
                        printf( "cache HIT\n" );
                #endif
        }
        assert( nodeCost );

        return nodeCost;
}


#ifdef DEBUG
void MicroPather::DumpStats()
{
        int hashTableEntries = 0;
        for( int i=0; i<HASH_SIZE; ++i )
                if ( hashTable[i] )
                        ++hashTableEntries;
        
        int pathNodeBlocks = 0;
        for( PathNode* node = pathNodeMem; node; node = node[ALLOCATE-1].left )
                ++pathNodeBlocks;

        printf( "HashTableEntries=%d/%d PathNodeBlocks=%d [%dk] PathNodes=%d SolverCalled=%d\n",
                          hashTableEntries, HASH_SIZE, pathNodeBlocks, 
                          pathNodeBlocks*ALLOCATE*sizeof(PathNode)/1024,
                          pathNodeCount,
                          frame );
}
#endif


void MicroPather::StatesInPool( std::vector< void* >* stateVec )
{
        stateVec->clear();
        
    for ( PathNode* mem = pathNodeMem; mem; mem = mem[ALLOCATE-1].left )
    {
        unsigned count = BLOCKSIZE;
        if ( mem == pathNodeMem )
                count = BLOCKSIZE - availMem;
        
        for( unsigned i=0; i<count; ++i )
        {
            if ( mem[i].frame == frame )
                    stateVec->push_back( mem[i].state );
        }    
        }           
}    


int MicroPather::Solve( void* startNode, void* endNode, vector< void* >* path, float* cost )
{
        #ifdef DEBUG_PATH
        printf( "Path: " );
        graph->PrintStateInfo( startNode );
        printf( " --> " );
        graph->PrintStateInfo( endNode );
        printf( " min cost=%f\n", graph->LeastCostEstimate( startNode, endNode ) );
        #endif

        *cost = 0.0f;

        if ( startNode == endNode )
                return START_END_SAME;

        ++frame;
//      // In "reuse" mode, reset the costs.
//      if ( pathNodeMem )
//              ReuseAll();

        OpenQueue open( graph );
        ClosedSet closed( graph );
        
        open.Push( NewPathNode( startNode,                                                                              // node
                                                        0,                                                                                              // cost from start
                                                        graph->LeastCostEstimate( startNode, endNode ),
                                                        0 ) );
        
        //vector< StateCost > neighbor;
        //vector< NodeCost > neighborNode;
        neighborVec.resize(0);
        neighborNodeVec.resize(0);

        while ( !open.Empty() )
        {
                PathNode* node = open.Pop();
                
                if ( node->state == endNode )
                {
                        GoalReached( node, startNode, endNode, path );
                        *cost = node->costFromStart;
                        #ifdef DEBUG_PATH
                        DumpStats();
                        #endif
                        return SOLVED;
                }
                else
                {
                        // We have not reached the goal - add the neighbors.
                        NodeCost* nodeCost = GetNodeNeighbors( node, &neighborVec, &neighborNodeVec );

                        for( int i=0; i<node->numAdjacent; ++i )
                        {
                                if ( nodeCost[i].cost == FLT_MAX ) {
                                        continue;
                                }

                                float newCost = node->costFromStart + nodeCost[i].cost;

                                PathNode* inOpen   = nodeCost[i].node->inOpen ? nodeCost[i].node : 0;
                                PathNode* inClosed = nodeCost[i].node->inClosed ? nodeCost[i].node : 0;
                                assert( !( inOpen && inClosed ) );
                                PathNode* inEither = inOpen ? inOpen : inClosed;

                                assert( inEither != node );

                                if ( inEither )
                                {
                                // Is this node is in use, and the cost is not an improvement,
                                // continue on.
                                        if ( inEither->costFromStart <= newCost )
                                                continue;       // Do nothing. This path is not better than existing.

                                        // Groovy. We have new information or improved information.
                                        inEither->parent = node;
                                        inEither->costFromStart = newCost;
                                        inEither->estToGoal = graph->LeastCostEstimate( inEither->state, endNode );
                                        inEither->totalCost = inEither->costFromStart + inEither->estToGoal;
                                }

                                if ( inClosed )
                                {
                                        closed.Remove( inClosed );
                                        open.Push( inClosed );
                                }       
                                else if ( inOpen )
                                {
                                        // Need to update the sort!
                                        open.Update( inOpen );
                                }
                                else if (!inEither)
                                {
                                        assert( !inEither );
                                        assert( nodeCost[i].node );

                                        PathNode* pNode = nodeCost[i].node;
                                        pNode->parent = node;
                                        pNode->costFromStart = newCost;
                                        pNode->estToGoal = graph->LeastCostEstimate( pNode->state, endNode ),
                                        pNode->totalCost = pNode->costFromStart + pNode->estToGoal;
                                        
                                        open.Push( pNode );
                                }
                        }
                }                                       
                closed.Add( node );
        }
        #ifdef DEBUG_PATH
        DumpStats();
        #endif
        return NO_SOLUTION;             
}       


Revision:	1.2
Committed:	Sat Apr 23 04:50:23 2011 UTC (13 years, 1 month ago) by root
Content type:	text/plain
Branch:	MAIN
CVS Tags:	HEAD
Changes since 1.1:	+0 -0 lines
State:	*FILE REMOVED*
Log Message:	removed micropather
#	User	Rev	Content
1	root	1.1	/*
2			Copyright (c) 2000-2005 Lee Thomason (www.grinninglizard.com)
3
4			Grinning Lizard Utilities.
5
6			This software is provided 'as-is', without any express or implied
7			warranty. In no event will the authors be held liable for any
8			damages arising from the use of this software.
9
10			Permission is granted to anyone to use this software for any
11			purpose, including commercial applications, and to alter it and
12			redistribute it freely, subject to the following restrictions:
13
14			1. The origin of this software must not be misrepresented; you must
15			not claim that you wrote the original software. If you use this
16			software in a product, an acknowledgment in the product documentation
17			would be appreciated but is not required.
18
19			2. Altered source versions must be plainly marked as such, and
20			must not be misrepresented as being the original software.
21
22			3. This notice may not be removed or altered from any source
23			distribution.
24			*/
25
26			#ifdef _MSC_VER
27			#pragma warning( disable : 4786 ) // Debugger truncating names.
28			#pragma warning( disable : 4530 ) // Exception handler isn't used
29			#endif
30
31			#include <cstring>
32			#include <cstdio>
33
34			#include <vector>
35			#include <map>
36			#include <algorithm>
37			#include <functional>
38
39			//#define DEBUG_PATH
40			//#define DEBUG_PATH_DEEP
41
42			#include "micropather.h"
43
44			using namespace std;
45			using namespace micropather;
46
47			template<class _Ty>
48			struct PathNodeCompareTotalCost : binary_function<_Ty, _Ty, bool> {
49			bool operator()(const _Ty& _X, const _Ty& _Y) const
50			{return (_X->totalCost > _Y->totalCost ); }
51			};
52
53			class OpenQueue
54			{
55			public:
56			OpenQueue( Graph* _graph )
57			{
58			graph = _graph;
59			#ifdef USE_LIST
60			sentinel = (PathNode*) sentinelMem;
61			sentinel->Init( 0, 0, FLT_MAX, FLT_MAX, 0 );
62			sentinel->totalCost = FLT_MAX;
63			sentinel->next = sentinel;
64			sentinel->prev = sentinel;
65			#ifdef DEBUG
66			sentinel->CheckList();
67			#endif
68			#endif
69			}
70			~OpenQueue() {}
71
72			void Push( PathNode* pNode )
73			{
74
75			assert( pNode->inOpen == 0 );
76			assert( pNode->inClosed == 0 );
77
78			#ifdef DEBUG_PATH_DEEP
79			printf( "Open Push: " );
80			graph->PrintStateInfo( pNode->state );
81			printf( " total=%.1f\n", pNode->totalCost );
82			#endif
83
84			#ifdef USE_LIST
85			// Add sorted. Lowest to highest cost path. Note that the sentinel has
86			// a value of FLT_MAX, so it should always be sorted in.
87			assert( pNode->totalCost < FLT_MAX );
88			PathNode* iter = sentinel->next;
89			while ( true )
90			{
91			if ( pNode->totalCost < iter->totalCost ) {
92			iter->AddBefore( pNode );
93			pNode->inOpen = 1;
94			break;
95			}
96			iter = iter->next;
97			}
98			assert( pNode->inOpen ); // make sure this was actually added.
99			#ifdef DEBUG
100			sentinel->CheckList();
101			#endif
102			#else
103			heapVector.push_back( pNode );
104			#endif
105			}
106
107			PathNode* Pop()
108			{
109			#ifdef USE_LIST
110			assert( sentinel->next != sentinel );
111			PathNode* pNode = sentinel->next;
112			pNode->Unlink();
113			#ifdef DEBUG
114			sentinel->CheckList();
115			#endif
116			#else
117			PathNode* pNode = heapVector[0];
118
119			const int size = heapVector.size();
120			int found = 0;
121			for( int i=1; i<size; ++i ) {
122			if ( heapVector[i]->totalCost < pNode->totalCost ) {
123			pNode = heapVector[i];
124			found = i;
125			}
126			}
127			if ( found < size-1 )
128			memcpy( &heapVector[found], &heapVector[found+1], sizeof( PathNode* ) * (size-found-1) );
129			heapVector.pop_back();
130			#endif
131
132			assert( pNode->inClosed == 0 );
133			assert( pNode->inOpen == 1 );
134			pNode->inOpen = 0;
135
136			#ifdef DEBUG_PATH_DEEP
137			printf( "Open Pop: " );
138			graph->PrintStateInfo( pNode->state );
139			printf( " total=%.1f\n", pNode->totalCost );
140			#endif
141
142			return pNode;
143			}
144
145			void Update( PathNode* pNode )
146			{
147			#ifdef DEBUG_PATH_DEEP
148			printf( "Open Update: " );
149			graph->PrintStateInfo( pNode->state );
150			printf( " total=%.1f\n", pNode->totalCost );
151			#endif
152
153			#ifdef USE_LIST
154			assert( pNode->inOpen );
155
156			// If the node now cost less than the one before it,
157			// move it to the front of the list.
158			if ( pNode->prev != sentinel && pNode->totalCost < pNode->prev->totalCost ) {
159			pNode->Unlink();
160			sentinel->next->AddBefore( pNode );
161			}
162
163			// If the node is too high, move to the right.
164			if ( pNode->totalCost > pNode->next->totalCost ) {
165			PathNode* it = pNode->next;
166			pNode->Unlink();
167
168			while ( pNode->totalCost > it->totalCost )
169			it = it->next;
170
171			it->AddBefore( pNode );
172			#ifdef DEBUG
173			sentinel->CheckList();
174			#endif
175			}
176			#endif
177			}
178
179			#ifdef USE_LIST
180			bool Empty() { return sentinel->next == sentinel; }
181			#else
182			bool Empty() { return heapVector.empty(); }
183			#endif
184
185			private:
186			OpenQueue( const OpenQueue& ); // undefined and unsupported
187			void operator=( const OpenQueue& );
188
189			#ifdef USE_LIST
190			PathNode* sentinel;
191			int sentinelMem[ ( sizeof( PathNode ) + sizeof( int ) ) / sizeof( int ) ];
192			#else
193			vector< PathNode* > heapVector;
194			#endif
195			Graph* graph; // for debugging
196			};
197
198
199			class ClosedSet
200			{
201			public:
202			ClosedSet( Graph* _graph ) { this->graph = _graph; }
203			~ClosedSet() {}
204
205			void Add( PathNode* pNode )
206			{
207			#ifdef DEBUG_PATH_DEEP
208			printf( "Closed add: " );
209			graph->PrintStateInfo( pNode->state );
210			printf( " total=%.1f\n", pNode->totalCost );
211			#endif
212			#ifdef DEBUG
213			assert( pNode->inClosed == 0 );
214			assert( pNode->inOpen == 0 );
215			#endif
216			pNode->inClosed = 1;
217			}
218
219			void Remove( PathNode* pNode )
220			{
221			#ifdef DEBUG_PATH_DEEP
222			printf( "Closed remove: " );
223			graph->PrintStateInfo( pNode->state );
224			printf( " total=%.1f\n", pNode->totalCost );
225			#endif
226			assert( pNode->inClosed == 1 );
227			assert( pNode->inOpen == 0 );
228
229			pNode->inClosed = 0;
230			}
231
232			private:
233			ClosedSet( const ClosedSet& );
234			void operator=( const ClosedSet& );
235			Graph* graph;
236			};
237
238
239			MicroPather::MicroPather( Graph* _graph, unsigned allocate )
240			: ALLOCATE( allocate ),
241			BLOCKSIZE( allocate-1 ),
242			graph( _graph ),
243			pathNodeMem( 0 ),
244			availMem( 0 ),
245			pathNodeCount( 0 ),
246			frame( 0 ),
247			checksum( 0 )
248			{
249			memset( hashTable, 0, sizeof( PathNode* )*HASH_SIZE );
250			}
251
252
253			MicroPather::~MicroPather()
254			{
255			PathNode *temp;
256
257			while( pathNodeMem ) {
258			temp = pathNodeMem;
259			pathNodeMem = pathNodeMem[ ALLOCATE-1 ].left;
260			free( temp ); // c-allocation
261			}
262			}
263
264
265			void MicroPather::DumpHashTable()
266			{
267			for( int i=0; i<HASH_SIZE; ++i )
268			{
269			int count = 0;
270			RecPathNodeWalk( hashTable[i], &count );
271			if ( count != 0 )
272			printf( "Hashtable[%3d] has %d items\n", i, count );
273			}
274			}
275
276			void MicroPather::RecPathNodeWalk( PathNode* node, int* count )
277			{
278			if ( node )
279			{
280			*count += 1;
281			if ( node->left )
282			RecPathNodeWalk( node->left, count );
283			#ifdef USE_BINARY_HASH
284			if ( node->right )
285			RecPathNodeWalk( node->right, count );
286			#endif
287			}
288			}
289
290
291			void MicroPather::Reset()
292			{
293			while( pathNodeMem ) {
294			PathNode* temp = pathNodeMem;
295			pathNodeMem = pathNodeMem[ ALLOCATE-1 ].left;
296			free( temp ); // c-allocation
297			}
298			pathNodeMem = 0;
299			memset( hashTable, 0, sizeof( PathNode* )*HASH_SIZE );
300			pathNodeMem = 0;
301			availMem = 0;
302			pathNodeCount = 0;
303			frame = 0;
304			checksum = 0;
305			}
306
307
308			//void MicroPather::StateCostChange( void* state )
309			//{
310			// PathNode* node = FindPathNode( state, true );
311			// if ( node ) {
312			// node->numAdjacent = -1; // throw away any cached data.
313			// }
314			//}
315
316
317			PathNode* MicroPather::AllocatePathNode()
318			{
319			if ( availMem == 0 ) {
320			PathNode* newBlock = (PathNode) malloc( sizeof(PathNode) ALLOCATE );
321			// set up the "next" pointer
322			newBlock[ALLOCATE-1].left = pathNodeMem;
323			pathNodeMem = newBlock;
324			availMem = BLOCKSIZE;
325			}
326			assert( availMem );
327
328			PathNode* result = pathNodeMem + (BLOCKSIZE - availMem );
329			--availMem;
330			++pathNodeCount;
331			return result;
332			}
333
334
335			PathNode* MicroPather::NewPathNode( void* state, float costFromStart, float estToEnd, PathNode* parent )
336			{
337			// Try to find an existing node for this state.
338			unsigned key = Hash( state ); //(HASH_SIZE-1) & ( (unsigned)state + (((unsigned)state)>>8) + (((unsigned)state)>>16) + (((unsigned)state)>>24) );
339
340			if ( !hashTable[key] ) {
341			// There isn't even a hashtable yet - create and initialize the PathNode.
342			hashTable[key] = AllocatePathNode();
343			hashTable[key]->Init( frame, state, costFromStart, estToEnd, parent );
344			return hashTable[key];
345			}
346
347			PathNode* root = hashTable[key];
348			PathNode* up = 0;
349			while ( root ) {
350			up = root;
351			if ( root->state == state ) {
352			root->Reuse( frame, costFromStart, estToEnd, parent );
353			assert( root->state == state );
354			return root;
355			}
356			#ifdef USE_BINARY_HASH
357			else if ( state > root->state ) {
358			root = root->right;
359			}
360			#endif
361			else {
362			#ifdef USE_BINARY_HASH
363			assert( state < root->state );
364			#endif
365			root = root->left;
366			}
367			}
368
369			assert( up );
370			PathNode* pNode = AllocatePathNode();
371			pNode->Init( frame, state, costFromStart, estToEnd, parent );
372			#ifdef USE_BINARY_HASH
373			if ( state > up->state ) {
374			assert( up->right == 0 );
375			up->right = pNode;
376			}
377			else {
378			assert( up->left == 0 );
379			up->left = pNode;
380			}
381			#else
382			up->left = pNode;
383			#endif
384			return pNode;
385			}
386
387
388			void MicroPather::GoalReached( PathNode* node, void* start, void* end, vector< void* > *path )
389			{
390			path->clear();
391
392			// We have reached the goal.
393			// How long is the path? Used to allocate the vector which is returned.
394			int count = 1;
395			PathNode* it = node;
396			while( it->parent )
397			{
398			++count;
399			it = it->parent;
400			}
401
402			// Now that the path has a known length, allocate
403			// and fill the vector that will be returned.
404			if ( count < 3 )
405			{
406			// Handle the short, special case.
407			path->resize(2);
408			(*path)[0] = start;
409			(*path)[1] = end;
410			}
411			else
412			{
413			path->resize(count);
414
415			(*path)[0] = start;
416			(*path)[count-1] = end;
417
418			count-=2;
419			it = node->parent;
420
421			while ( it->parent )
422			{
423			(*path)[count] = it->state;
424			it = it->parent;
425			--count;
426			}
427			}
428			checksum = 0;
429			#ifdef DEBUG_PATH
430			printf( "Path: " );
431			int counter=0;
432			#endif
433			for ( unsigned k=0; k<path->size(); ++k )
434			{
435			checksum += ((UPTR)((*path)[k])) << (k%8);
436
437			#ifdef DEBUG_PATH
438			graph->PrintStateInfo( (*path)[k] );
439			printf( " " );
440			++counter;
441			if ( counter == 8 )
442			{
443			printf( "\n" );
444			counter = 0;
445			}
446			#endif
447			}
448			#ifdef DEBUG_PATH
449			printf( "Cost=%.1f Checksum %d\n", node->costFromStart, checksum );
450			#endif
451			}
452
453
454			NodeCost* MicroPather::GetNodeNeighbors( PathNode* node, std::vector< StateCost >* neighbor, std::vector< NodeCost >* neighborNode )
455			{
456			NodeCost* nodeCost = 0;
457
458			if ( node->numAdjacent < 0
459			\|\| node->numAdjacent > PathNode::MAX_CACHE )
460			{
461			// It has not been computed yet (<0) or
462			// can not be cached (>MAX)
463			neighbor->resize( 0 );
464			graph->AdjacentCost( node->state, neighbor );
465			#ifdef DEBUG
466			{
467			// If this assert fires, you have passed a state
468			// as its own neighbor state. This is impossible --
469			// bad things will happen.
470			for ( unsigned i=0; i<neighbor->size(); ++i )
471			assert( (*neighbor)[i].state != node->state );
472			}
473			#endif
474			node->numAdjacent = (int) neighbor->size();
475
476			// Now convert to pathNodes, and put in cache if possible.
477			if ( node->numAdjacent <= PathNode::MAX_CACHE )
478			{
479			// Can fit in the cache:
480			for( int i=0; i<node->numAdjacent; ++i )
481			{
482			node->adjacent[i].cost = (*neighbor)[i].cost;
483			node->adjacent[i].node = FindPathNode( (*neighbor)[i].state );
484			if ( !node->adjacent[i].node )
485			{
486			node->adjacent[i].node = NewPathNode( (*neighbor)[i].state, FLT_BIG, FLT_BIG, 0 );
487			}
488			}
489			nodeCost = node->adjacent;
490
491			#ifdef DEBUG_PATH_DEEP
492			printf( "State " );
493			graph->PrintStateInfo( node->state );
494			printf( "--> cache\n" );
495			#endif
496			}
497			else {
498			// Too big for cache.
499			node->numAdjacent = (int) neighbor->size();
500
501			neighborNode->resize( neighbor->size() );
502
503			for( unsigned i=0; i<neighborNode->size(); ++i ) {
504			(neighborNode)[i].cost = (neighbor)[i].cost;
505			(neighborNode)[i].node = FindPathNode( (neighbor)[i].state );
506			if ( !(*neighborNode)[i].node )
507			{
508			(neighborNode)[i].node = NewPathNode( (neighbor)[i].state, FLT_BIG, FLT_BIG, 0 );
509			}
510			}
511			nodeCost = &(*neighborNode)[0];
512			#ifdef DEBUG_PATH_DEEP
513			printf( "State " );
514			graph->PrintStateInfo( node->state );
515			printf( "no cache\n" );
516			#endif
517			}
518			}
519			else {
520			// In the cache!
521			nodeCost = node->adjacent;
522
523			for( int i=0; i<node->numAdjacent; ++i ) {
524			if ( node->adjacent[i].node->frame != frame )
525			node->adjacent[i].node->Reuse( frame, FLT_BIG, FLT_BIG, 0 );
526			}
527			#ifdef DEBUG_PATH_DEEP
528			printf( "State " );
529			graph->PrintStateInfo( node->state );
530			printf( "cache HIT\n" );
531			#endif
532			}
533			assert( nodeCost );
534
535			return nodeCost;
536			}
537
538
539			#ifdef DEBUG
540			void MicroPather::DumpStats()
541			{
542			int hashTableEntries = 0;
543			for( int i=0; i<HASH_SIZE; ++i )
544			if ( hashTable[i] )
545			++hashTableEntries;
546
547			int pathNodeBlocks = 0;
548			for( PathNode* node = pathNodeMem; node; node = node[ALLOCATE-1].left )
549			++pathNodeBlocks;
550
551			printf( "HashTableEntries=%d/%d PathNodeBlocks=%d [%dk] PathNodes=%d SolverCalled=%d\n",
552			hashTableEntries, HASH_SIZE, pathNodeBlocks,
553			pathNodeBlocksALLOCATEsizeof(PathNode)/1024,
554			pathNodeCount,
555			frame );
556			}
557			#endif
558
559
560			void MicroPather::StatesInPool( std::vector< void* >* stateVec )
561			{
562			stateVec->clear();
563
564			for ( PathNode* mem = pathNodeMem; mem; mem = mem[ALLOCATE-1].left )
565			{
566			unsigned count = BLOCKSIZE;
567			if ( mem == pathNodeMem )
568			count = BLOCKSIZE - availMem;
569
570			for( unsigned i=0; i<count; ++i )
571			{
572			if ( mem[i].frame == frame )
573			stateVec->push_back( mem[i].state );
574			}
575			}
576			}
577
578
579			int MicroPather::Solve( void* startNode, void* endNode, vector< void* >* path, float* cost )
580			{
581			#ifdef DEBUG_PATH
582			printf( "Path: " );
583			graph->PrintStateInfo( startNode );
584			printf( " --> " );
585			graph->PrintStateInfo( endNode );
586			printf( " min cost=%f\n", graph->LeastCostEstimate( startNode, endNode ) );
587			#endif
588
589			*cost = 0.0f;
590
591			if ( startNode == endNode )
592			return START_END_SAME;
593
594			++frame;
595			// // In "reuse" mode, reset the costs.
596			// if ( pathNodeMem )
597			// ReuseAll();
598
599			OpenQueue open( graph );
600			ClosedSet closed( graph );
601
602			open.Push( NewPathNode( startNode, // node
603			0, // cost from start
604			graph->LeastCostEstimate( startNode, endNode ),
605			0 ) );
606
607			//vector< StateCost > neighbor;
608			//vector< NodeCost > neighborNode;
609			neighborVec.resize(0);
610			neighborNodeVec.resize(0);
611
612			while ( !open.Empty() )
613			{
614			PathNode* node = open.Pop();
615
616			if ( node->state == endNode )
617			{
618			GoalReached( node, startNode, endNode, path );
619			*cost = node->costFromStart;
620			#ifdef DEBUG_PATH
621			DumpStats();
622			#endif
623			return SOLVED;
624			}
625			else
626			{
627			// We have not reached the goal - add the neighbors.
628			NodeCost* nodeCost = GetNodeNeighbors( node, &neighborVec, &neighborNodeVec );
629
630			for( int i=0; i<node->numAdjacent; ++i )
631			{
632			if ( nodeCost[i].cost == FLT_MAX ) {
633			continue;
634			}
635
636			float newCost = node->costFromStart + nodeCost[i].cost;
637
638			PathNode* inOpen = nodeCost[i].node->inOpen ? nodeCost[i].node : 0;
639			PathNode* inClosed = nodeCost[i].node->inClosed ? nodeCost[i].node : 0;
640			assert( !( inOpen && inClosed ) );
641			PathNode* inEither = inOpen ? inOpen : inClosed;
642
643			assert( inEither != node );
644
645			if ( inEither )
646			{
647			// Is this node is in use, and the cost is not an improvement,
648			// continue on.
649			if ( inEither->costFromStart <= newCost )
650			continue; // Do nothing. This path is not better than existing.
651
652			// Groovy. We have new information or improved information.
653			inEither->parent = node;
654			inEither->costFromStart = newCost;
655			inEither->estToGoal = graph->LeastCostEstimate( inEither->state, endNode );
656			inEither->totalCost = inEither->costFromStart + inEither->estToGoal;
657			}
658
659			if ( inClosed )
660			{
661			closed.Remove( inClosed );
662			open.Push( inClosed );
663			}
664			else if ( inOpen )
665			{
666			// Need to update the sort!
667			open.Update( inOpen );
668			}
669			else if (!inEither)
670			{
671			assert( !inEither );
672			assert( nodeCost[i].node );
673
674			PathNode* pNode = nodeCost[i].node;
675			pNode->parent = node;
676			pNode->costFromStart = newCost;
677			pNode->estToGoal = graph->LeastCostEstimate( pNode->state, endNode ),
678			pNode->totalCost = pNode->costFromStart + pNode->estToGoal;
679
680			open.Push( pNode );
681			}
682			}
683			}
684			closed.Add( node );
685			}
686			#ifdef DEBUG_PATH
687			DumpStats();
688			#endif
689			return NO_SOLUTION;
690			}
691
692