C++ PathNode::Init方法代码示例

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
//.........这里部分代码省略.........

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 );
			}
		}
	}
}