game_machine 0.0.11 → 1.0.2

data/pathfinding/include/navmesh.h

@@ -0,0 +1,114 @@
+#define _USE_MATH_DEFINES
+#define DT_VIRTUAL_QUERYFILTER 1
+
+#include <cmath>
+#include <limits>
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <vector>
+#include <math.h>
+#include <map>
+#include <float.h>
+#include "Recast.h"
+#include "DetourCommon.h"
+#include "DetourNavMesh.h"
+#include "DetourNavMeshBuilder.h"
+#include "DetourNavMeshQuery.h"
+
+#include "common.h"
+#include "mesh_loader.h"
+
+#define VERTEX_SIZE       3
+#define INVALID_POLYREF   0
+
+using namespace std;
+
+class Navmesh {
+	int meshId;
+	dtNavMesh* mesh;
+	dtNavMeshQuery* query;
+
+public:
+	int gwidth;
+	int gheight;
+	vector<int> gmap;
+	
+
+	static const int P_FAILURE = -1;
+	static const int P_NO_START_POLY = -2;
+	static const int P_NO_END_POLY = -3;
+	static const int P_PATH_NOT_FOUND = -4;
+	static const int P_MESH_NOT_FOUND = -5;
+	static const int P_QUERY_NOT_FOUND = -6;
+
+
+	static const int MAX_POLYS = 512;
+	static const int MAX_STEER_POINTS = 10;
+	static const int MAX_SMOOTH = 2048;
+	static float const STEP_SIZE;
+	static float const SLOP;
+
+	enum SamplePolyFlags
+	{
+		SAMPLE_POLYFLAGS_WALK		= 0x01,		// Ability to walk (ground, grass, road)
+		SAMPLE_POLYFLAGS_SWIM		= 0x02,		// Ability to swim (water).
+		SAMPLE_POLYFLAGS_DOOR		= 0x04,		// Ability to move through doors.
+		SAMPLE_POLYFLAGS_JUMP		= 0x08,		// Ability to jump.
+		SAMPLE_POLYFLAGS_DISABLED	= 0x10,		// Disabled polygon
+		SAMPLE_POLYFLAGS_ALL		= 0xffff	// All abilities.
+	};
+
+	Navmesh(int id, const char *file);
+	void gmapSetPassable(int a, int b, int radius);
+	void gmapSetBlocked(int a, int b, int radius);
+	dtNavMeshQuery* getQuery();
+	void EXPORT_API freeQuery(dtNavMeshQuery* query);
+	int findPath(float startx,
+	    float starty, float startz, float endx, float endy, float endz,
+	    int find_straight_path, float* resultPath);
+	float* getPathPtr(int max_paths);
+	void freePath(float* path);
+	dtNavMesh* getMesh();
+	bool meshLoaded();
+
+	int fixupCorridor(dtPolyRef* path, const int npath, const int maxPath,
+             const dtPolyRef* visited, const int nvisited);
+
+	bool getSteerTarget(dtNavMeshQuery* navQuery, const float* startPos, const float* endPos,
+               const float minTargetDist,
+               const dtPolyRef* path, const int pathSize,
+               float* steerPos, unsigned char& steerPosFlag, dtPolyRef& steerPosRef,
+               float* outPoints = 0, int* outPointCount = 0);
+
+	int fixupShortcuts(dtPolyRef* path, int npath, dtNavMeshQuery* navQuery);
+	inline bool inRange(const float* v1, const float* v2, const float r, const float h);
+};
+
+class myQueryFilter : public dtQueryFilter {
+
+private:	
+	float m_areaCost[DT_MAX_AREAS];		///< Cost per area type. (Used by default implementation.)
+	unsigned short m_includeFlags;		///< Flags for polygons that can be visited. (Used by default implementation.)
+	unsigned short m_excludeFlags;		///< Flags for polygons that should not be visted. (Used by default implementation.)
+	Navmesh* navmesh;
+public:
+	myQueryFilter();
+	~myQueryFilter();
+
+	void setNavmesh(Navmesh* n);
+	bool isPassable(const float* pa, const float* pb) const;
+	bool passFilter(const dtPolyRef /*ref*/,
+							   const dtMeshTile* /*tile*/,
+							   const dtPoly* poly) const;
+	
+
+    float getCost(const float* pa, const float* pb,
+							 const dtPolyRef /*prevRef*/, const dtMeshTile* /*prevTile*/, const dtPoly* /*prevPoly*/,
+							 const dtPolyRef /*curRef*/, const dtMeshTile* /*curTile*/, const dtPoly* curPoly,
+							 const dtPolyRef /*nextRef*/, const dtMeshTile* /*nextTile*/, const dtPoly* /*nextPoly*/) const;
+	
+
+};
+

data/pathfinding/include/pathfinder.h

@@ -0,0 +1,24 @@
+
+#include "crowd.h"
+#include "astar.h"
+
+extern "C" EXPORT_API void addPathfinder(int i, int width, int height);
+extern "C" EXPORT_API void setPassable(int pathfinder, int x, int y);
+extern "C" EXPORT_API void setBlocked(int pathfinder, int x, int y);
+extern "C" EXPORT_API int getXAt(int pathfinder, int index);
+extern "C" EXPORT_API int getYAt(int pathfinder, int index);
+extern "C" EXPORT_API int findpath2d(int pathfinder, int startx, int starty, int endx, int endy);
+
+extern "C" EXPORT_API void addCrowd(int id, int navmeshid);
+extern "C" EXPORT_API int addAgent(int crowdId, const float* p, float accel, float speed, float radius,
+ float height, int optflag, float sepWeight);
+extern "C" EXPORT_API void removeAgent(int crowdId, const int idx);
+extern "C" EXPORT_API void setMoveTarget(int crowdId, const float* p, bool adjust, int agentIdx);
+extern "C" EXPORT_API void updateTick(int crowdId, const float dt);
+extern "C" EXPORT_API void getAgentPosition(int crowdId, int idx, float* resultPath);
+
+extern "C" EXPORT_API void addNavmesh(int id, const char *file);
+extern "C" EXPORT_API void gmapSetPassable(int navmeshId, int a, int b, int radius);
+extern "C" EXPORT_API void gmapSetBlocked(int navmeshId, int a, int b, int radius);
+extern "C" EXPORT_API int findPath(int navmeshId, float startx, float starty,
+ float startz, float endx, float endy, float endz, int find_straight_path, float* resultPath);

data/pathfinding/main.cpp

@@ -1,39 +1,130 @@
-#include "pathfind.h"
+#if _MSC_VER
+#else
+#include "pathfinder.h"
+#include <cstdlib>
+#include <stdio.h>
+#include <float.h>
+#include <time.h>
+#include <stdint.h>
 
-int main (int argc, char* argv[]) {
 
-  int find_straight_path = 1;
-  float *newPath;
-  newPath = getPathPtr(MAX_SMOOTH);
+#include <sys/time.h>
+#include <ctime>
+using namespace std;
+
+ typedef unsigned long long timestamp_t;
 
-  //const char *file = "/home2/chris/game_machine/server/detour/meshes/terrain.bin";
-  const char *file = "/home2/chris/game_machine/data/meshes/all_tiles_navmesh.bin";
+    static timestamp_t  get_timestamp ()
+    {
+      struct timeval now;
+      gettimeofday (&now, NULL);
+      return  now.tv_usec + (timestamp_t)now.tv_sec * 1000000;
+    }
 
+void testnavmesh() {
+  const char *file = "/home/chris/game_machine/server/pathfinding/all_tiles_navmesh.bin";
+  Navmesh* navmesh = new Navmesh(1,file);
 
-  int loadRes = loadNavMesh(1,file);
-  fprintf (stderr, "loadNavMesh returned %d\n", loadRes);
-  dtNavMeshQuery* query = getQuery(1);
+  int find_straight_path = 1;
+  float *newPath;
+  newPath = navmesh->getPathPtr(Navmesh::MAX_SMOOTH);
+  timestamp_t t0;
+  timestamp_t t1;
+  double secs;
 
-  if (loadRes == 1) {
+  t0 = get_timestamp();
+  for (int j = 0; j < 15; ++j) {
+    navmesh->gmapSetPassable(200,200,3);
+    navmesh->gmapSetBlocked(130,95,3);
+  }
+  t1 = get_timestamp();
+  secs = (t1 - t0) / 1000000.0L;
+  fprintf (stderr, "setPassable %f\n", secs);
+
+  if (navmesh->meshLoaded()) {
     for (int j = 0; j < 1; ++j) {
       for (int i = 0; i < 1; ++i) {
-        //int res = findPath(query, 563.0, 0.2, 504.0, 509.0, 0.2, 528.0,
-        int res = findPath(query, 563.0f, 0.2f, 504.0f, 563.0f, 0.2f, 541.0f,
+        t0 = get_timestamp();
+        int res = navmesh->findPath(10.0f, 0.2f, 10.0f, 300.0f, 0.2f, 300.0f,
             find_straight_path, newPath);
+        t1 = get_timestamp();
+        secs = (t1 - t0) / 1000000.0L;
+        fprintf (stderr, "end time %f\n", secs);
+
         fprintf (stderr, "findPath returned %d\n", res);
         for (int i = 0; i < res; ++i) {
           //fprintf (stderr, "%d\n", i);
           const float* v = &newPath[i*3];
-          fprintf (stderr, "%f.%f.%f\n", v[0], v[1], v[2]);
+          fprintf (stderr, "%f %f %f\n", v[0], v[1], v[2]);
         }
       }
     }
   }
+  
+  
 
   fprintf (stderr, "endLoop\n");
-  freePath(newPath);
+  navmesh->freePath(newPath);
   fprintf (stderr, "freePath\n");
-  freeQuery(query);
-  fprintf (stderr, "freeQuery\n");
+  
+
+  
+
+  Crowd* crowd = new Crowd(navmesh);
+  float delta = 1.01f;
+  float agentPos[3] = {10.0f,0.0f,10.0f};
+  float targetPos[3] = {300.0f,0.0f,300.0f};
+  const float* ap = &agentPos[0];
+  const float* tp = &targetPos[0];
+  fprintf (stderr, "createCrowd\n");
+
+  
+  crowd->setMoveTarget(tp, false, -1);
+  fprintf (stderr, "setMoveTarget\n");
+
+  for (int j = 0; j < 100; ++j) {
+    crowd->addAgent(ap, 8.0f, 3.5f, 0.6f, 2.0f,5,2.0f);
+  }
+  
+  fprintf (stderr, "addAgent\n");
+
+  for (int j = 0; j < 900; ++j) {
+    crowd->updateTick(delta);
+    for (int i = 0; i < crowd->getCrowd()->getAgentCount(); ++i)
+    {
+      const dtCrowdAgent* ag = crowd->getCrowd()->getAgent(i);
+      if (!ag->active)
+        continue;
+      const float* v = ag->npos;
+      fprintf (stderr, "%d = %f %f %f\n", i, v[0], v[1], v[2]);
+    }
+
+  }
+  fprintf (stderr, "updateTick\n");
+
+  
+}
+
+#endif
+
+int main (int argc, char* argv[]) {
+  #if _MSC_VER
+  #else
+  testnavmesh();
+  return 1;
+
+  addPathfinder(1,500,500);
+  for (int j = 0; j < 500; ++j) {
+      for (int i = 0; i < 500; ++i) {
+        //setPassable(1,j,i);
+      }
+  }
+  fprintf (stderr, "grid created\n");
+
+  int res = findpath2d(1,0,0,100,100);
+  fprintf (stderr, "findpath2d returned %d\n", res);
+  
+  #endif
+
   return 1;
 }

data/pathfinding/micropather.cpp

@@ -0,0 +1,1062 @@
+/*
+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.
+*/
+
+#ifdef _MSC_VER
+#pragma warning( disable : 4786 )	// Debugger truncating names.
+#pragma warning( disable : 4530 )	// Exception handler isn't used
+#endif
+
+//#include <vector>
+#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 std;
+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::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;
+}
+
+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;
+}
+
+
+