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;
}
PathNode* PathNodePool::GetPathNode( unsigned frame, void* _state, float _costFromStart, float _estToGoal, PathNode* _parent )
{
unsigned key = Hash( _state );
PathNode* root = hashTable[key];
while( root ) {
if ( root->state == _state ) {
if ( root->frame == frame ) // This is the correct state and correct frame.
break;
// Correct state, wrong frame.
root->Init( frame, _state, _costFromStart, _estToGoal, _parent );
break;
}
root = ( _state < root->state ) ? root->child[0] : root->child[1];
}
if ( !root ) {
// allocate new one
root = Alloc();
root->Clear();
root->Init( frame, _state, _costFromStart, _estToGoal, _parent );
AddPathNode( key, root );
}
return root;
}
int MicroPather::SolveForNearStates( void* startState, std::vector< StateCost >* near, float maxCost )
{
/* http://en.wikipedia.org/wiki/Dijkstra%27s_algorithm
1 function Dijkstra(Graph, source):
2 for each vertex v in Graph: // Initializations
3 dist[v] := infinity // Unknown distance function from source to v
4 previous[v] := undefined // Previous node in optimal path from source
5 dist[source] := 0 // Distance from source to source
6 Q := the set of all nodes in Graph
// All nodes in the graph are unoptimized - thus are in Q
7 while Q is not empty: // The main loop
8 u := vertex in Q with smallest dist[]
9 if dist[u] = infinity:
10 break // all remaining vertices are inaccessible from source
11 remove u from Q
12 for each neighbor v of u: // where v has not yet been removed from Q.
13 alt := dist[u] + dist_between(u, v)
14 if alt < dist[v]: // Relax (u,v,a)
15 dist[v] := alt
16 previous[v] := u
17 return dist[]
*/
++frame;
OpenQueue open( graph ); // nodes to look at
ClosedSet closed( graph );
nodeCostVec.resize(0);
stateCostVec.resize(0);
PathNode closedSentinel;
closedSentinel.Clear();
closedSentinel.Init( frame, 0, FLT_MAX, FLT_MAX, 0 );
closedSentinel.next = closedSentinel.prev = &closedSentinel;
PathNode* newPathNode = pathNodePool.GetPathNode( frame, startState, 0, 0, 0 );
open.Push( newPathNode );
while ( !open.Empty() )
{
PathNode* node = open.Pop(); // smallest dist
closed.Add( node ); // add to the things we've looked at
closedSentinel.AddBefore( node );
if ( node->totalCost > maxCost )
continue; // Too far away to ever get here.
GetNodeNeighbors( node, &nodeCostVec );
for( int i=0; i<node->numAdjacent; ++i )
{
MPASSERT( node->costFromStart < FLT_MAX );
float newCost = node->costFromStart + nodeCostVec[i].cost;
PathNode* inOpen = nodeCostVec[i].node->inOpen ? nodeCostVec[i].node : 0;
PathNode* inClosed = nodeCostVec[i].node->inClosed ? nodeCostVec[i].node : 0;
MPASSERT( !( inOpen && inClosed ) );
PathNode* inEither = inOpen ? inOpen : inClosed;
MPASSERT( inEither != node );
if ( inEither && inEither->costFromStart <= newCost ) {
continue; // Do nothing. This path is not better than existing.
}
// Groovy. We have new information or improved information.
PathNode* child = nodeCostVec[i].node;
MPASSERT( child->state != newPathNode->state ); // should never re-process the parent.
child->parent = node;
child->costFromStart = newCost;
child->estToGoal = 0;
child->totalCost = child->costFromStart;
if ( inOpen ) {
open.Update( inOpen );
}
else if ( !inClosed ) {
open.Push( child );
}
}
}
near->clear();
for( PathNode* pNode=closedSentinel.next; pNode != &closedSentinel; pNode=pNode->next ) {
if ( pNode->totalCost <= maxCost ) {
StateCost sc;
sc.cost = pNode->totalCost;
sc.state = pNode->state;
near->push_back( sc );
}
}
#ifdef DEBUG
for( unsigned i=0; i<near->size(); ++i ) {
for( unsigned k=i+1; k<near->size(); ++k ) {
MPASSERT( near->at(i).state != near->at(k).state );
}
}
#endif
return SOLVED;
}
void MicroPather::GetNodeNeighbors( PathNode* node, std::vector< NodeCost >* pNodeCost )
{
if ( node->numAdjacent == 0 ) {
// it has no neighbors.
pNodeCost->resize( 0 );
}
else if ( node->cacheIndex < 0 )
{
// Not in the cache. Either the first time or just didn't fit. We don't know
// the number of neighbors and need to call back to the client.
stateCostVec.resize( 0 );
graph->AdjacentCost( node->state, &stateCostVec );
#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<stateCostVec.size(); ++i )
MPASSERT( stateCostVec[i].state != node->state );
}
#endif
pNodeCost->resize( stateCostVec.size() );
node->numAdjacent = stateCostVec.size();
if ( node->numAdjacent > 0 ) {
// Now convert to pathNodes.
// Note that the microsoft std library is actually pretty slow.
// Move things to temp vars to help.
const unsigned stateCostVecSize = stateCostVec.size();
const StateCost* stateCostVecPtr = &stateCostVec.at(0);
NodeCost* pNodeCostPtr = &pNodeCost->at(0);
for( unsigned i=0; i<stateCostVecSize; ++i ) {
void* state = stateCostVecPtr[i].state;
pNodeCostPtr[i].cost = stateCostVecPtr[i].cost;
pNodeCostPtr[i].node = pathNodePool.GetPathNode( frame, state, FLT_MAX, FLT_MAX, 0 );
}
// Can this be cached?
int start = 0;
if ( pNodeCost->size() > 0 && pathNodePool.PushCache( pNodeCostPtr, pNodeCost->size(), &start ) ) {
node->cacheIndex = start;
}
}
}
else {
// In the cache!
pNodeCost->resize( node->numAdjacent );
NodeCost* pNodeCostPtr = &pNodeCost->at(0);
pathNodePool.GetCache( node->cacheIndex, node->numAdjacent, pNodeCostPtr );
// A node is uninitialized (even if memory is allocated) if it is from a previous frame.
// Check for that, and Init() as necessary.
for( int i=0; i<node->numAdjacent; ++i ) {
PathNode* pNode = pNodeCostPtr[i].node;
if ( pNode->frame != frame ) {
pNode->Init( frame, pNode->state, FLT_MAX, FLT_MAX, 0 );
}
}
}
}