micropather实现A*算法

小灰灰 2022-03-29 09:06 175阅读 0赞

MicroPather is a path finder and A\* solver (astar or a-star) written in platform independent C++ that can be easily integrated into existing code. MicroPather focuses on being a path finding engine for video games but is a generic A\* solver.

There is plenty of pathing code out there, but most if it seems to focus on teaching somewhat how to write an A\* solver, rather than being utility code for pathing. MicroPather is firmly aimed at providing functionality, not at being a tutorial.

MicroPather's primary goal is to be easy to use:

1.  An easy API
2.  No requirements on the host program to change its data structures or objects
3.  No library or 'make' - just add 1 .cpp and 1 .h file to your project.

Enjoy, and thanks for checking out MicroPather!

官方网站可以下载其sdk和demo。

demo1的代码如下：

dungeon.cpp

#define USE_PATHER
    
    #include <ctype.h>
    #include <stdio.h>
    #include <memory.h>
    #include <math.h>
    
    #include <vector>
    #include <iostream>
    
    #ifdef USE_PATHER
    
    #include "micropather.h"
    using namespace micropather;
    #endif
    
    
    const int MAPX = 30;
    const int MAPY = 10;
    const char gMap[MAPX*MAPY+1] =
    //"012345678901234567890123456789"
    "     |      |                |"
    "     |      |----+    |      +"
    "---+ +---DD-+      +--+--+    "
    "   |                     +-- +"
    "        +----+  +---+         "
    "---+ +  D    D            |   "
    "   | |  +----+    +----+  +--+"
    "   D |            |    |      "
    "   | +-------+  +-+    |--+   "
    "---+                   |     +";
    
    class Dungeon
    #ifdef USE_PATHER
    	: public Graph
    #endif
    {
    private:
    	Dungeon( const Dungeon& );
    	void operator=( const Dungeon& );
    
    	// 寻路的起始点位置
    	int playerX, playerY;
    
    	// 寻路的最终路径
    	std::vector<void*> path;
    
    	// 门是否打开
    	bool doorsOpen;
    	// 
    	bool showConsidered;
    
    	MicroPather* pather;
    
    public:
    	Dungeon() : playerX( 0 ), playerY( 0 ), doorsOpen( false ), showConsidered( false ), pather( 0 )
    	{
    		// this : The "map" that implements the Graph callbacks. 
    		// 20 : How many states should be internally allocated at a time. 
    		// This can be hard to get correct. The higher the value, 
    		// the more memory MicroPather will use.
    		pather = new MicroPather( this, 20 );	// Use a very small memory block to stress the pather
    	}
    
    	virtual ~Dungeon() 
    	{
    		delete pather;
    	}
    
    	int X()	{ return playerX; }
    	int Y() { return playerY; }
    
    	// 校验和，用于debug
    	unsigned Checksum() { return pather->Checksum(); }
    
    	void ClearPath()
    	{
    #ifdef USE_PATHER
    		path.resize( 0 );
    #endif
    	}
    
    	// 
    	void ToggleTouched() 
    	{ 	
    		showConsidered = !showConsidered; 
    		// Reset() Should be called whenever the cost between states or 
    		// the connection between states changes. 
    		// Also frees overhead memory used by MicroPather, 
    		// and calling will free excess memory. 
    		pather->Reset();					 
    	}
    
    	// 打开或者关闭Door
    	void ToggleDoor() 
    	{ 
    		doorsOpen = !doorsOpen; 
    #ifdef USE_PATHER
    		pather->Reset();
    #endif	
    	}
    
    	// 目标点(nx, ny)是否可到达（" "或者"D"）
    	int Passable( int nx, int ny ) 
    	{
    		if ( nx >= 0 && nx < MAPX 
    			&& ny >= 0 && ny < MAPY )
    		{
    			int index = ny * MAPX + nx;
    			char c = gMap[index];
    			if ( c == ' ' )
    				return 1;
    			else if ( c == 'D' )
    				return 2;
    		}		
    		return 0;
    	}
    
    	// 计算起始位置到(nx,ny)的路径及其代价
    	int SetPos( int nx, int ny ) 
    	{
    		int result = 0;
    		if ( Passable( nx, ny ) == 1 )
    		{
    #ifdef USE_PATHER
    			float totalCost;
    			if ( showConsidered )
    				pather->Reset();
    			/*
    			int micropather::MicroPather::Solve  
    			( void * startState,  
    			  void *  endState,  
    			  std::vector< void * > *  path,  
    			  float *  totalCost   
    			) 
    			Solve for the path from start to end.
    			Parameters:
    			startState:  Input, the starting state for the path.  
    			endState:  Input, the ending state for the path.  
    			path:  Output, a vector of states that define the path. Empty if not found.  
    			totalCost:  Output, the cost of the path, if found.  
    
    			Returns:
    			Success or failure, expressed as SOLVED, NO_SOLUTION, or START_END_SAME. 
    			*/
    			result = pather->Solve( XYToNode( playerX, playerY ), XYToNode( nx, ny ), &path, &totalCost );
    
    			if ( result == MicroPather::SOLVED ) 
    			{
    				// 将玩家位置设置为（nx, ny）
    				playerX = nx;
    				playerY = ny;
    			}
    			printf( "Pather returned %d\n", result );
    #else
    			playerX = nx;
    			playerY = ny;
    #endif
    		}
    		return result;
    	}
    
    	void Print() 
    	{
    		char buf[ MAPX + 1 ];
    		std::vector<void*> stateVec;
    
    		if ( showConsidered )
    			pather->StatesInPool(&stateVec);
    		printf(" doors %s\n", doorsOpen ? "open" : "closed");
    		printf(" 0         10        20\n");
    		printf(" 012345678901234567890123456789\n");
    		for( int j=0; j<MAPY; ++j ) 
    		{
    			// 按行复制, 并输出
    			memcpy(buf, &gMap[MAPX * j], MAPX + 1);
    			buf[MAPX] = 0;
    #ifdef USE_PATHER
    			unsigned k;
    			// Wildly inefficient demo code.
    			for( k=0; k<path.size(); ++k )
    			{
    				int x, y;
    				NodeToXY( path[k], &x, &y );
    				if ( y == j )
    					buf[x] = '0' + k % 10;
    			}
    			if ( showConsidered )
    			{
    				for( k=0; k<stateVec.size(); ++k ) 
    				{
    					int x, y;
    					NodeToXY( stateVec[k], &x, &y );
    					if ( y == j )
    						buf[x] = 'x';
    				}     
    			}  		
    #endif
    			// Insert the player
    			if ( j == playerY )
    				buf[playerX] = 'i';
    			printf( "%d%s\n", j % 10, buf );
    		}
    	}
    
    #ifdef USE_PATHER
    	void NodeToXY( void* node, int* x, int* y ) 
    	{
    		int index = (int)node;
    		*y = index / MAPX;
    		*x = index - *y * MAPX;
    	}
    
    	void* XYToNode( int x, int y )
    	{
    		return (void*) ( y*MAPX + x );
    	}
    
    	// 最小代价
    	virtual float LeastCostEstimate( void* nodeStart, void* nodeEnd ) 
    	{
    		int xStart, yStart, xEnd, yEnd;
    		NodeToXY( nodeStart, &xStart, &yStart );
    		NodeToXY( nodeEnd, &xEnd, &yEnd );
    
    		/* Compute the minimum path cost using distance measurement. It is possible
    		to compute the exact minimum path using the fact that you can move only 
    		on a straight line or on a diagonal, and this will yield a better result.
    		*/
    		int dx = xStart - xEnd;
    		int dy = yStart - yEnd;
    		return (float) sqrt( (double)(dx*dx) + (double)(dy*dy) );
    	}
    
    	// Return the exact cost from the given state to all its neighboring states. 
    	// This may be called multiple times, or cached by the solver.
    	virtual void AdjacentCost( void* node, std::vector< StateCost > *neighbors ) 
    	{
    		int x, y;
    		// 在X,Y轴上8个方向   E   SE  S   SW     W    NW  N   NE
    		const int dx[8] =     { 1,  1,  0, -1,    -1,  -1,  0,  1 };
    		const int dy[8] =     { 0,  1,  1,  1,     0,  -1, -1, -1 };
    		// X,Y轴上8个方向的代价
    		const float cost[8] = { 1.0f, 1.41f, 1.0f, 1.41f, 1.0f, 1.41f, 1.0f, 1.41f };
    
    		NodeToXY( node, &x, &y );
    
    		// 得到与该点相邻的8个方向的点的坐标;计算是否可通过;将代价push到vector中
    		for( int i=0; i<8; ++i ) 
    		{
    			int nx = x + dx[i];
    			int ny = y + dy[i];
    
    			int pass = Passable( nx, ny );
    			if ( pass > 0 ) 
    			{
    				if ( pass == 1 || doorsOpen ) 
    				{
    					// Normal floor
    					StateCost nodeCost = 
    					{ 
    						// (nx, ny)点的索引号
    						XYToNode( nx, ny ), 
    						// node到该点的代价
    						cost[i] 
    					};
    					neighbors->push_back( nodeCost );
    				}
    				else 
    				{
    					// 若不可pass则代价为FLT_MAX
    					StateCost nodeCost = { XYToNode( nx, ny ), FLT_MAX };
    					neighbors->push_back( nodeCost );
    				}
    			}
    		}
    	}
    
    	virtual void PrintStateInfo( void* node ) 
    	{
    		int x, y;
    		NodeToXY( node, &x, &y );
    		printf( "(%d,%d)", x, y );
    	}
    
    #endif
    };
    
    int main( int /*argc*/, const char** /*argv*/ )
    {
    	{
    		Dungeon test;
    		const int NUM_TEST = 5;
    		int tx[NUM_TEST]	= {	24,	25,	10,	6,	0	};	// x of test
    		int ty[NUM_TEST]	= {	9,	9,	5,	5,	0	};	// y of test
    		int door[NUM_TEST]	= {	0,	0,	0,	1,	0	};	// toggle door? (before move)
    		unsigned check[NUM_TEST] = { 139640, 884, 0, 129313, 2914 };
    		for( int i=0; i<NUM_TEST; ++i )
    		{
    			// (25,9)到(10, 5)时, (10, 5)正好被不通路包围的中心，这个不通路有2扇门，
    			// 此时2扇门没打开，所以(25,9)到(10, 5)不通！
    			// (10,5)到(6,5)之间正好有一扇门，若门关闭则此路不通，门打开则可以通
    			if ( door[i] )
    				test.ToggleDoor();
    			int _result = test.SetPos( tx[i], ty[i] );
    			if ( _result == MicroPather::SOLVED ) 
    			{
    				// Return the "checksum" of the last path returned by Solve(). 
    				// Useful for debugging, and a quick way to see if 2 paths are the same. 
    				unsigned checkNum = test.Checksum();
    
    				if ( checkNum == check[i] )
    					printf( "Test %d to (%d,%d) ok\n", i, tx[i], ty[i] );
    				else
    					printf( "Test %d to (%d,%d) BAD CHECKSUM\n", i, tx[i], ty[i] );
    			}
    			else if (_result == MicroPather::NO_SOLUTION)
    			{
    				printf( "Test %d to (%d,%d) no solution\n", i, tx[i], ty[i] );
    			}
    			else if (_result == MicroPather::START_END_SAME)
    			{
    				printf( "Test %d to (%d,%d) start end same\n", i, tx[i], ty[i] );
    			}
    		}
    	}
    
    	Dungeon dungeon;
    	bool done = false;
    	char buf[ 256 ];
    	while ( !done ) 
    	{
    		dungeon.Print();
    
    		printf( "\n# # to move, q to quit, r to redraw, d to toggle doors, t for touched\n" );
    		std::cin.getline( buf, 256 );
    		if ( *buf )
    		{
    			if ( buf[0] == 'q' ) 
    				done = true;
    			else if ( buf[0] == 'd' ) 
    			{
    				dungeon.ToggleDoor();
    				dungeon.ClearPath();
    			}
    			else if ( buf[0] == 't' ) 
    				dungeon.ToggleTouched();      
    			else if ( buf[0] == 'r' ) 
    				dungeon.ClearPath();
    			else if ( isdigit( buf[0] ) ) 
    			{
    				int x, y;
    				sscanf( buf, "%d %d", &x, &y );	// sleazy, I know
    				dungeon.SetPos( x, y );
    			} 
    		}
    		else			
    			dungeon.ClearPath();
    	}
    	return 0;
    }