/micropather.cpp (8350a63e947e8a4a55608fd090d128fef7b969a1) (27581 bytes) (mode 100755) (type blob)

/*
Copyright (c) 2000-2009 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.
*/

// This source code had been altered from its original form,
// to make it compile under MinGW-w64-posix. (near renamed _near)

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

#include <memory.h>
#include <stdio.h>

//#define DEBUG_PATH
//#define DEBUG_PATH_DEEP
//#define TRACK_COLLISION
//#define DEBUG_CACHING

#ifdef DEBUG_CACHING
#include "../grinliz/gldebug.h"
#endif

#include "micropather.h"

using namespace micropather;

class OpenQueue
{
  public:
	OpenQueue( Graph* _graph )
	{ 
		graph = _graph; 
		sentinel = (PathNode*) sentinelMem;
		sentinel->InitSentinel();
		#ifdef DEBUG
			sentinel->CheckList();
		#endif
	}
	~OpenQueue()	{}

	void Push( PathNode* pNode );
	PathNode* Pop();
	void Update( PathNode* pNode );
    
	bool Empty()	{ return sentinel->next == sentinel; }

  private:
	OpenQueue( const OpenQueue& );	// undefined and unsupported
	void operator=( const OpenQueue& );
  
	PathNode* sentinel;
	int sentinelMem[ ( sizeof( PathNode ) + sizeof( int ) ) / sizeof( int ) ];
	Graph* graph;	// for debugging
};


void OpenQueue::Push( PathNode* pNode )
{
	
	MPASSERT( pNode->inOpen == 0 );
	MPASSERT( pNode->inClosed == 0 );
	
#ifdef DEBUG_PATH_DEEP
	printf( "Open Push: " );
	graph->PrintStateInfo( pNode->state );
	printf( " total=%.1f\n", pNode->totalCost );		
#endif
	
	// Add sorted. Lowest to highest cost path. Note that the sentinel has
	// a value of FLT_MAX, so it should always be sorted in.
	MPASSERT( 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;
	}
	MPASSERT( pNode->inOpen );	// make sure this was actually added.
#ifdef DEBUG
	sentinel->CheckList();
#endif
}

PathNode* OpenQueue::Pop()
{
	MPASSERT( sentinel->next != sentinel );
	PathNode* pNode = sentinel->next;
	pNode->Unlink();
#ifdef DEBUG
	sentinel->CheckList();
#endif
	
	MPASSERT( pNode->inClosed == 0 );
	MPASSERT( 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 OpenQueue::Update( PathNode* pNode )
{
#ifdef DEBUG_PATH_DEEP
	printf( "Open Update: " );		
	graph->PrintStateInfo( pNode->state );
	printf( " total=%.1f\n", pNode->totalCost );		
#endif
	
	MPASSERT( 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
	}
}


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
		MPASSERT( pNode->inClosed == 0 );
		MPASSERT( 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
		MPASSERT( pNode->inClosed == 1 );
		MPASSERT( pNode->inOpen == 0 );

		pNode->inClosed = 0;
	}

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


PathNodePool::PathNodePool( unsigned _allocate, unsigned _typicalAdjacent ) 
	: firstBlock( 0 ),
	  blocks( 0 ),
#if defined( MICROPATHER_STRESS )
	  allocate( 32 ),
#else
	  allocate( _allocate ),
#endif
	  nAllocated( 0 ),
	  nAvailable( 0 )
{
	freeMemSentinel.InitSentinel();

	cacheCap = allocate * _typicalAdjacent;
	cacheSize = 0;
	cache = (NodeCost*)malloc(cacheCap * sizeof(NodeCost));

	// Want the behavior that if the actual number of states is specified, the cache 
	// will be at least that big.
	hashShift = 3;	// 8 (only useful for stress testing) 
#if !defined( MICROPATHER_STRESS )
	while( HashSize() < allocate )
		++hashShift;
#endif
	hashTable = (PathNode**)calloc( HashSize(), sizeof(PathNode*) );

	blocks = firstBlock = NewBlock();
//	printf( "HashSize=%d allocate=%d\n", HashSize(), allocate );
	totalCollide = 0;
}


PathNodePool::~PathNodePool()
{
	Clear();
	free( firstBlock );
	free( cache );
	free( hashTable );
#ifdef TRACK_COLLISION
	printf( "Total collide=%d HashSize=%d HashShift=%d\n", totalCollide, HashSize(), hashShift );
#endif
}


bool PathNodePool::PushCache( const NodeCost* nodes, int nNodes, int* start ) {
	*start = -1;
	if ( nNodes+cacheSize <= cacheCap ) {
		for( int i=0; i<nNodes; ++i ) {
			cache[i+cacheSize] = nodes[i];
		}
		*start = cacheSize;
		cacheSize += nNodes;
		return true;
	}
	return false;
}


void PathNodePool::GetCache( int start, int nNodes, NodeCost* nodes ) {
	MPASSERT( start >= 0 && start < cacheCap );
	MPASSERT( nNodes > 0 );
	MPASSERT( start + nNodes <= cacheCap );
	memcpy( nodes, &cache[start], sizeof(NodeCost)*nNodes );
}


void PathNodePool::Clear()
{
#ifdef TRACK_COLLISION
	// Collision tracking code.
	int collide=0;
	for( unsigned i=0; i<HashSize(); ++i ) {
		if ( hashTable[i] && (hashTable[i]->child[0] || hashTable[i]->child[1]) )
			++collide;
	}
	//printf( "PathNodePool %d/%d collision=%d %.1f%%\n", nAllocated, HashSize(), collide, 100.0f*(float)collide/(float)HashSize() );
	totalCollide += collide;
#endif

	Block* b = blocks;
	while( b ) {
		Block* temp = b->nextBlock;
		if ( b != firstBlock ) {
			free( b );
		}
		b = temp;
	}
	blocks = firstBlock;	// Don't delete the first block (we always need at least that much memory.)

	// Set up for new allocations (but don't do work we don't need to. Reset/Clear can be called frequently.)
	if ( nAllocated > 0 ) {
		freeMemSentinel.next = &freeMemSentinel;
		freeMemSentinel.prev = &freeMemSentinel;
	
		memset( hashTable, 0, sizeof(PathNode*)*HashSize() );
		for( unsigned i=0; i<allocate; ++i ) {
			freeMemSentinel.AddBefore( &firstBlock->pathNode[i] );
		}
	}
	nAvailable = allocate;
	nAllocated = 0;
	cacheSize = 0;
}


PathNodePool::Block* PathNodePool::NewBlock()
{
	Block* block = (Block*) calloc( 1, sizeof(Block) + sizeof(PathNode)*(allocate-1) );
	block->nextBlock = 0;

	nAvailable += allocate;
	for( unsigned i=0; i<allocate; ++i ) {
		freeMemSentinel.AddBefore( &block->pathNode[i] );
	}
	return block;
}


unsigned PathNodePool::Hash( void* voidval ) 
{
	/*
		Spent quite some time on this, and the result isn't quite satifactory. The
		input set is the size of a void*, and is generally (x,y) pairs or memory pointers.

		FNV resulting in about 45k collisions in a (large) test and some other approaches
		about the same.

		Simple folding reduces collisions to about 38k - big improvement. However, that may
		be an artifact of the (x,y) pairs being well distributed. And for either the x,y case 
		or the pointer case, there are probably very poor hash table sizes that cause "overlaps"
		and grouping. (An x,y encoding with a hashShift of 8 is begging for trouble.)

		The best tested results are simple folding, but that seems to beg for a pathelogical case.
		FNV-1a was the next best choice, without obvious pathelogical holes.

		Finally settled on h%HashMask(). Simple, but doesn't have the obvious collision cases of folding.
	*/
	/*
	// Time: 567
	// FNV-1a
	// http://isthe.com/chongo/tech/comp/fnv/
	// public domain.
	MP_UPTR val = (MP_UPTR)(voidval);
	const unsigned char *p = (unsigned char *)(&val);
	unsigned int h = 2166136261;

	for( size_t i=0; i<sizeof(MP_UPTR); ++i, ++p ) {
		h ^= *p;
		h *= 16777619;
	}
	// Fold the high bits to the low bits. Doesn't (generally) use all
	// the bits since the shift is usually < 16, but better than not
	// using the high bits at all.
	return ( h ^ (h>>hashShift) ^ (h>>(hashShift*2)) ^ (h>>(hashShift*3)) ) & HashMask();
	*/
	/*
	// Time: 526
	MP_UPTR h = (MP_UPTR)(voidval);
	return ( h ^ (h>>hashShift) ^ (h>>(hashShift*2)) ^ (h>>(hashShift*3)) ) & HashMask();
	*/

	// Time: 512
	// The HashMask() is used as the divisor. h%1024 has lots of common
	// repetitions, but h%1023 will move things out more.
	MP_UPTR h = (MP_UPTR)(voidval);
	return h % HashMask();	
}



PathNode* PathNodePool::Alloc()
{
	if ( freeMemSentinel.next == &freeMemSentinel ) {
		MPASSERT( nAvailable == 0 );

		Block* b = NewBlock();
		b->nextBlock = blocks;
		blocks = b;
		MPASSERT( freeMemSentinel.next != &freeMemSentinel );
	}
	PathNode* pathNode = freeMemSentinel.next;
	pathNode->Unlink();

	++nAllocated;
	MPASSERT( nAvailable > 0 );
	--nAvailable;
	return pathNode;
}


void PathNodePool::AddPathNode( unsigned key, PathNode* root )
{
	if ( hashTable[key] ) {
		PathNode* p = hashTable[key];
		while( true ) {
			int dir = (root->state < p->state) ? 0 : 1;
			if ( p->child[dir] ) {
				p = p->child[dir];
			}
			else {
				p->child[dir] = root;
				break;
			}
		}
	}
	else {
		hashTable[key] = root;
	}
}


PathNode* PathNodePool::FetchPathNode( void* state )
{
	unsigned key = Hash( state );

	PathNode* root = hashTable[key];
	while( root ) {
		if ( root->state == state ) {
			break;
		}
		root = ( state < root->state ) ? root->child[0] : root->child[1];
	}
	MPASSERT( root );
	return root;
}


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


void PathNode::Init(	unsigned _frame,
						void* _state,
						float _costFromStart, 
						float _estToGoal, 
						PathNode* _parent )
{
	state = _state;
	costFromStart = _costFromStart;
	estToGoal = _estToGoal;
	CalcTotalCost();
	parent = _parent;
	frame = _frame;
	inOpen = 0;
	inClosed = 0;
}


void PathNode::Clear()
{
	memset( this, 0, sizeof( PathNode ) );
	numAdjacent = -1;
	cacheIndex  = -1;
}

MicroPather::MicroPather( Graph* _graph, unsigned allocate, unsigned typicalAdjacent, bool cache )
	:	pathNodePool( allocate, typicalAdjacent ),
		graph( _graph ),
		frame( 0 )
{
	MPASSERT( allocate );
	MPASSERT( typicalAdjacent );
	pathCache = 0;
	if ( cache ) {
		pathCache = new PathCache( allocate*4 );	// untuned arbitrary constant	
	}
}


MicroPather::~MicroPather()
{
	delete pathCache;
}


void MicroPather::Reset()
{
	pathNodePool.Clear();
	if ( pathCache ) {
		pathCache->Reset();
	}
	frame = 0;
}


void MicroPather::GoalReached( PathNode* node, void* start, void* end, MP_VECTOR< void* > *_path )
{
	MP_VECTOR< void* >& path = *_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;
		}
	}

	if ( pathCache ) {
		costVec.clear();

		PathNode* pn0 = pathNodePool.FetchPathNode( path[0] );
		PathNode* pn1 = 0;
		for( unsigned i=0; i<path.size()-1; ++i ) {
			pn1 = pathNodePool.FetchPathNode( path[i+1] );
			nodeCostVec.clear();
			GetNodeNeighbors( pn0, &nodeCostVec );
			for( unsigned j=0; j<nodeCostVec.size(); ++j ) {
				if ( nodeCostVec[j].node == pn1 ) {
					costVec.push_back( nodeCostVec[j].cost );
					break;
				}
			}
			MPASSERT( costVec.size() == i+1 );
			pn0 = pn1;
		}
		pathCache->Add( path, costVec );
	}

	#ifdef DEBUG_PATH
	printf( "Path: " );
	int counter=0;
	#endif
	for ( unsigned k=0; k<path.size(); ++k )
	{
		#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
}


void MicroPather::GetNodeNeighbors( PathNode* node, MP_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[0];
			NodeCost* pNodeCostPtr = &(*pNodeCost)[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)[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 );
			}
		}
	}
}


#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( MP_VECTOR< void* >* stateVec )
{
 	stateVec->clear();
	pathNodePool.AllStates( frame, stateVec );
}


void PathNodePool::AllStates( unsigned frame, MP_VECTOR< void* >* stateVec )
{	
    for ( Block* b=blocks; b; b=b->nextBlock )
    {
    	for( unsigned i=0; i<allocate; ++i )
    	{
    	    if ( b->pathNode[i].frame == frame )
	    	    stateVec->push_back( b->pathNode[i].state );
    	}    
	}           
}   


PathCache::PathCache( int _allocated )
{
	mem = new Item[_allocated];
	memset( mem, 0, sizeof(*mem)*_allocated );
	allocated = _allocated;
	nItems = 0;
	hit = 0;
	miss = 0;
}


PathCache::~PathCache()
{
	delete [] mem;
}


void PathCache::Reset()
{
	if ( nItems ) {
		memset( mem, 0, sizeof(*mem)*allocated );
		nItems = 0;
		hit = 0;
		miss = 0;
	}
}


void PathCache::Add( const MP_VECTOR< void* >& path, const MP_VECTOR< float >& cost )
{
	if ( nItems + (int)path.size() > allocated*3/4 ) {
		return;
	}

	for( unsigned i=0; i<path.size()-1; ++i ) {
		// example: a->b->c->d
		// Huge memory saving to only store 3 paths to 'd'
		// Can put more in cache with also adding path to b, c, & d
		// But uses much more memory. Experiment with this commented
		// in and out and how to set.

		void* end   = path[path.size()-1];
		Item item = { path[i], end, path[i+1], cost[i] };
		AddItem( item );
	}
}


void PathCache::AddNoSolution( void* end, void* states[], int count )
{
	if ( count + nItems > allocated*3/4 ) {
		return;
	}

	for( int i=0; i<count; ++i ) {
		Item item = { states[i], end, 0, FLT_MAX };
		AddItem( item );
	}
}


int PathCache::Solve( void* start, void* end, MP_VECTOR< void* >* path, float* totalCost )
{
	const Item* item = Find( start, end );
	if ( item ) {
		if ( item->cost == FLT_MAX ) {
			++hit;
			return MicroPather::NO_SOLUTION;
		}

		path->clear();
		path->push_back( start );
		*totalCost = 0;

		for ( ;start != end; start=item->next, item=Find(start, end) ) {
			MPASSERT( item );
			*totalCost += item->cost;
			path->push_back( item->next );
		}
		++hit;
		return MicroPather::SOLVED;
	}
	++miss;
	return MicroPather::NOT_CACHED;
}


void PathCache::AddItem( const Item& item )
{
	MPASSERT( allocated );
	unsigned index = item.Hash() % allocated;
	while( true ) {
		if ( mem[index].Empty() ) {
			mem[index] = item;
			++nItems;
#ifdef DEBUG_CACHING
			GLOUTPUT(( "Add: start=%x next=%x end=%x\n", item.start, item.next, item.end ));
#endif
			break;
		}
		else if ( mem[index].KeyEqual( item ) ) {
			MPASSERT( (mem[index].next && item.next) || (mem[index].next==0 && item.next == 0) );
			// do nothing; in cache
			break;
		}
		++index;
		if ( index == allocated )
			index = 0;
	}
}


const PathCache::Item* PathCache::Find( void* start, void* end )
{
	MPASSERT( allocated );
	Item fake = { start, end, 0, 0 };
	unsigned index = fake.Hash() % allocated;
	while( true ) {
		if ( mem[index].Empty() ) {
			return 0;
		}
		if ( mem[index].KeyEqual( fake )) {
			return mem + index;
		}
		++index;
		if ( index == allocated )
			index = 0;
	}
}


void MicroPather::GetCacheData( CacheData* data )
{
	memset( data, 0, sizeof(*data) );

	if ( pathCache ) {
		data->nBytesAllocated = pathCache->AllocatedBytes();
		data->nBytesUsed = pathCache->UsedBytes();
		data->memoryFraction = (float)( (double)data->nBytesUsed / (double)data->nBytesAllocated );

		data->hit = pathCache->hit;
		data->miss = pathCache->miss;
		if ( data->hit + data->miss ) {
		data->hitFraction = (float)( (double)(data->hit) / (double)(data->hit + data->miss) );
	}
		else {
			data->hitFraction = 0;
		}
	}
}



int MicroPather::Solve( void* startNode, void* endNode, MP_VECTOR< void* >* path, float* cost )
{
	// Important to clear() in case the caller doesn't check the return code. There
	// can easily be a left over path  from a previous call.
	path->clear();

	#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;

	if ( pathCache ) {
		int cacheResult = pathCache->Solve( startNode, endNode, path, cost );
		if ( cacheResult == SOLVED || cacheResult == NO_SOLUTION ) {
		#ifdef DEBUG_CACHING
			GLOUTPUT(( "PathCache hit. result=%s\n", cacheResult == SOLVED ? "solved" : "no_solution" ));
		#endif
			return cacheResult;
		}
		#ifdef DEBUG_CACHING
		GLOUTPUT(( "PathCache miss\n" ));
		#endif
	}

	++frame;

	OpenQueue open( graph );
	ClosedSet closed( graph );
	
	PathNode* newPathNode = pathNodePool.GetPathNode(	frame, 
														startNode, 
														0, 
														graph->LeastCostEstimate( startNode, endNode ), 
														0 );

	open.Push( newPathNode );	
	stateCostVec.resize(0);
	nodeCostVec.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
		{
			closed.Add( node );

			// We have not reached the goal - add the neighbors.
			GetNodeNeighbors( node, &nodeCostVec );

			for( int i=0; i<node->numAdjacent; ++i )
			{
				// Not actually a neighbor, but useful. Filter out infinite cost.
				if ( nodeCostVec[i].cost == FLT_MAX ) {
					continue;
				}
				PathNode* child = nodeCostVec[i].node;
				float newCost = node->costFromStart + nodeCostVec[i].cost;

				PathNode* inOpen   = child->inOpen ? child : 0;
				PathNode* inClosed = child->inClosed ? child : 0;
				PathNode* inEither = (PathNode*)( ((MP_UPTR)inOpen) | ((MP_UPTR)inClosed) );

				MPASSERT( inEither != node );
				MPASSERT( !( inOpen && inClosed ) );

				if ( inEither ) {
					if ( newCost < child->costFromStart ) {
						child->parent = node;
						child->costFromStart = newCost;
						child->estToGoal = graph->LeastCostEstimate( child->state, endNode );
						child->CalcTotalCost();
						if ( inOpen ) {
							open.Update( child );
						}
					}
				}
				else {
					child->parent = node;
					child->costFromStart = newCost;
					child->estToGoal = graph->LeastCostEstimate( child->state, endNode ),
					child->CalcTotalCost();
					
					MPASSERT( !child->inOpen && !child->inClosed );
					open.Push( child );
				}
			}
		}					
	}
	#ifdef DEBUG_PATH
	DumpStats();
	#endif
	if ( pathCache ) {
		// Could add a bunch more with a little tracking.
		pathCache->AddNoSolution( endNode, &startNode, 1 );
	}
	return NO_SOLUTION;		
}	


int MicroPather::SolveForNearStates( void* startState, MP_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)[i].state != (*_near)[k].state );
		}
	}
#endif

	return SOLVED;
}






Mode Type Size Ref File
100644 blob 98 227abf3bfa53b2530dcc74495da7bd0ccdcb0775 .gitignore
100644 blob 225 9b00c2c2e7b4f0c1e338fdead65f17ba0af089c1 COPYING
100755 blob 43 45aea818a4a3202b2467509f28a481cce08834d2 Confectioner.command
100644 blob 14281 269812932bd49b9542e969a1d70a9b1a786da7d1 Makefile
100644 blob 2723 b5a3f573f076ef740ca742ec9598043732e10c0e README.md
040000 tree - f18894a3a50f88457b898a360b553b9d9ba6f856 base
100755 blob 156 84cb1387849f2ca98e53e43536d00af2dfabf7d3 caveconfec
100755 blob 28 41b0ef285892c86306eaa269f366dd04cb633d21 caveconfec.bat
100644 blob 196408 d38c9221e50e1a97d8e2ae6678ed61f34e788d97 confec.cpp
100644 blob 394069 50803f515c89046dd7a753514bbfc242a0015b55 gamecontrollerdb.txt
040000 tree - 0f0978f80e4982d0c776b5bfed5fb8830dfcfecb include
100755 blob 1035 2300dbb05e2b8d1b6decf6a6b9d4d208185370c8 libs.cpp
100755 blob 27581 8350a63e947e8a4a55608fd090d128fef7b969a1 micropather.cpp
100644 blob 141235 f54e2d2631a628876a631456c043b77da5db78bd openjdk.pem
100755 blob 8 e9a74187b02a27b165dfa4f93bf6f060376d0ee6 steam_appid.txt
Hints:
Before first commit, do not forget to setup your git environment:
git config --global user.name "your_name_here"
git config --global user.email "your@email_here"

Clone this repository using HTTP(S):
git clone https://rocketgit.com/user/mse/ConfectionerEngine

Clone this repository using ssh (do not forget to upload a key first):
git clone ssh://rocketgit@ssh.rocketgit.com/user/mse/ConfectionerEngine

Clone this repository using git:
git clone git://git.rocketgit.com/user/mse/ConfectionerEngine

You are allowed to anonymously push to this repository.
This means that your pushed commits will automatically be transformed into a merge request:
... clone the repository ...
... make some changes and some commits ...
git push origin master