[C++/alghorithm concept] Infinite and multifloor pathfinder

[Noxitu]

Hello. I think that most bots still are missing the feature to find way to some place (like depot) no mater where they are. For now I wrote a simple map viewer that can search path without upper bound on distance and supporting multi floor walking.

New sources with binaries, dlls and libs at post #6

For now code is very messy and to work correctly it needs database of ladders, ramps, etc. Also important fact is that players are actually saved on minimap (for eg. on the minimap on WoT route passing The Big Old One is blocked ).

But what is good - if you have good data it works.

Code:

select c++

#include <iostream>
#include <cstdio>
#include <map>
#include <SDL/SDL.h>
 
#ifndef _DISABLE_PTHREAD_
#include <pthread.h>
#endif//_DISABLE_PTHREAD_
 
// enum sux
unsigned short int tmp;
typedef unsigned short int DATA_TYPE;
DATA_TYPE 
	DT_NONE = 0,
	DT_WEST = tmp=1,//0
	DT_EAST = tmp*=2,//1
	DT_NORTH = tmp*=2,//2
	DT_SOUTH = tmp*=2,//3
	DT_UP = tmp*=2, //4
	DT_DOWN = tmp*=2, //5
	DT_DIRECTIONS = tmp*2 - 1,
	DT_PATH = tmp*=2,//6
	DT_START = tmp*=2,//7
	DT_END = tmp*=2, //8
	DT_VISITED = tmp*2 - 1,
	DT_REQWEST = tmp*=2,//9
	DT_REQEAST = tmp*=2,//10
	DT_REQNORTH = tmp*=2,//11
	DT_REQSOUTH = tmp*=2,//12
	DT_ALLOWUP = tmp*=2, //13
	DT_ALLOWDOWN = tmp*=2,//14
	DT_BLOCKING = tmp*=2;//15
 
DATA_TYPE ReqToDir( DATA_TYPE val )
{
	return ( val/DT_REQWEST ) & DT_DIRECTIONS;
}
 
struct Loc{ int x,y,z; Loc(int a, int b, int c):x(a),y(b),z&copy;{ } };
 
bool operator<( const Loc& a, const Loc& b ){
if( a.x == b.x )
	if( a.y == b.y )
		return a.z<b.z;
	else
		return a.y<b.y;
else
	return a.x<b.x;
}
 
 
 
const int W = 7, H = 9, Z=16;
const int resW = 800, resH = 600;
unsigned char map[Z][W][H][2][256][256];
union WTF
{
DATA_TYPE i;
unsigned char c;
} none;
 
int X = 124 << 8, Y = 120 << 8;
DATA_TYPE data[Z][W][H][256][256];
 
inline unsigned char& Map( int x, int y, int z, bool walk )
{
	if( x < 0 || y < 0 || x >= W*256 || y >= H*256 || z < 0 || z > Z )
		return none.c;
	return map[z][x/256][y/256][walk?1:0][x%256][y%256]; 
}
 
inline DATA_TYPE& Data( int x, int y, int z )
{
	if( x < 0 || y < 0 || x >= W*256 || y >= H*256 || z < 0 || z > Z )
		return none.i;
	return data[z][x/256][y/256][x%256][y%256];
}
 
 
std::multimap< Loc, DATA_TYPE > stairs;
 
void AddStairs()
{
	for( std::multimap< Loc, DATA_TYPE >::iterator it = stairs.begin(); it != stairs.end(); it++ )
	{
		const Loc& loc = it->first;
		Data( loc.x, loc.y, loc.z ) = it->second;
	}
}
void LoadStairs(const char* line)
{
	int x,y,z,up,dirs;
	if( sscanf( line, "%d %d %d %d %d", &x, &y, &z, &up, &dirs ) != 5 )
		return;
//	printf("{%d:%d:%d}
	x-=X; y-=Y;
	bool b = Map( x, y, z, true ) == 255;
	if( b )
		Map( x, y, z, true ) = 254;
	Loc loc( x, y, z );
	DATA_TYPE dt = dirs*DT_REQWEST | ( up ? DT_ALLOWUP : DT_ALLOWDOWN ) | ( b ? DT_BLOCKING : 0 );
	stairs.insert( std::pair< Loc, DATA_TYPE>( loc, dt ) );
}
 
void LoadStairsFile(const char* path)
{
	FILE* file=fopen(path,"rt");
	char buf[256];
	while( ! feof( file ) )
	{
		if( fgets( buf, 256, file ) == 0 )
			continue;
		LoadStairs( buf );
	}
}
 
bool hideyellow = false;
 
inline Uint32 Color(int x, int y, int z, bool walk=false)
{
	DATA_TYPE dt = Data( x, y, z );
	if( dt != DT_NONE )
	{
		if( dt & ( DT_ALLOWUP | DT_ALLOWDOWN ) )
		{
			if( dt & DT_PATH )
				return 0xaa00aa;
			if( ! hideyellow && dt & DT_VISITED )
				return 0x00aa00;
			return 0x00ff00;
		}
		if( dt & DT_START )
			return 0xff0000;
		if( dt & DT_END )
			return 0x0000ff;
		if( dt & DT_PATH )
			return 0xff00ff;
		if( ! hideyellow )
			return 0xaaaa00;
	}
	int val = Map( x, y, z, walk );
	if( walk )
		return (255-val) * (65536+256+1);
	return ( 0x330000 * ( val / 36 % 6 ) + 0x003300 * ( val / 6 % 6 ) + 0x000033 * ( val % 6 ) );
}
 
void LoadMap()
{
	none.i = DT_NONE;
	memset( data, 0, sizeof( data ) );
	const char *basepath = "/home/grzesiu/Tibia/Automap3";
	char path[80];
	for( int z = 0; z < Z; z++ )
	for( int w = 0; w < W; w++ )
		for( int h = 0; h < H; h++ )
		{
			sprintf( path, "%s/%.3d%.3d%.2d.map", basepath, ( X >> 8 ) + w, ( Y >> 8 ) + h,z );
			FILE* file = fopen( path, "rb" );
			if( ! file )
			{
				memset( &map[z][ w ][ h ][0][0][0], 0, 256*256 );
				memset( &map[z][ w ][ h ][1][0][0], 254, 256*256 );
				continue;
			}
			fread ( &map[z][ w ][ h ][0][0][0], 256*256*2, 1, file );
			fclose( file );
		}
}
 
SDL_Surface* InitSDL()
{
	SDL_Init( SDL_INIT_EVERYTHING );
	atexit(SDL_Quit);
	return SDL_SetVideoMode( resW, resH, 32, SDL_ANYFORMAT | /*SDL_FULLSCREEN |*/ SDL_DOUBLEBUF );
}
 
void Draw( Uint32 *pix,int x=0, int y=0, int z=7,int scale = 1, bool walk = false )
{
	int sx = x - resW/scale/2;
	int sy = y - resH/scale/2;
	for( int rly = 0; rly < resH; rly++)
		for( int rlx = 0; rlx < resW; rlx++ )
			pix[ rlx + rly * resW ] =  Color( sx + rlx/scale, sy + rly/scale,z, walk );
}
 
bool selected = false;
bool locked = false;
int sx, sy, sz;
int ex, ey, ez;
int cost = 95, anticost=1;
#ifndef _DISABLE_PTHREAD_
pthread_t T;
#endif//_DISABLE_PTHREAD_
 
#include <map>
#include <set>
 
inline int abs( int val ){ return val>=0 ? val : -val; }
 
struct pos
{
	int val,x,y,z;
	DATA_TYPE type;
	pos( int _val, int _x, int _y, int _z, DATA_TYPE _type ) : val(_val),x(_x),y(_y),z(_z), type(_type) { }
	operator int() const { return anticost*val + cost*( abs(x-ex) + abs(y-ey) + 5*abs(z-ez) ); }
};
std::multiset< pos > heap;
bool operator<(const pos& a, const pos& b){ return int(a)<int(b); }
 
inline void push( int x, int y, int z, int val, DATA_TYPE type, int costmul = 1 )
{
	if( &Data( x, y, z ) != &none.i && Map(x,y,z, true) != 255 )
		heap.insert( pos( val+costmul*Map(x,y,z, true), x, y, z, type ) ) ;
}
 
inline void pushfew( int x, int y, int z, int val = 0 )
{
	push( x-1, y, z, val, DT_WEST );
	push( x+1, y, z, val, DT_EAST );
	push( x, y-1, z, val, DT_NORTH );
	push( x, y+1, z, val, DT_SOUTH );
	push( x-1, y-1, z, val, DT_WEST | DT_NORTH, 3 );
	push( x+1, y-1, z, val, DT_EAST | DT_NORTH, 3 );
	push( x-1, y+1, z, val, DT_WEST | DT_SOUTH, 3 );
	push( x+1, y+1, z, val, DT_EAST | DT_SOUTH, 3 );
}
 
inline void odw( int x, int y, int z, int val, DATA_TYPE type )
{
	if( ( Data( x, y, z ) & DT_VISITED ) == 0 )
	{
		Data( x, y, z ) |= type;
		if( Data( x, y, z ) & ( DT_ALLOWUP | DT_ALLOWDOWN ) )
		{
			DATA_TYPE r = ReqToDir( Data( x, y, z ) );
			push(
				x + ( ( r & DT_WEST ) ? -1 : ( r & DT_EAST ) ? 1 : 0 ),
				y + ( ( r & DT_NORTH ) ? -1 : ( r & DT_SOUTH ) ? 1 : 0 ),
				z + ( ( r & DT_UP ) ? -1 : ( r & DT_DOWN ) ? 1 : 0 ),
				val, r , 5 );	
		}
		if( Data( x, y, z ) & DT_BLOCKING )
			return;
			pushfew( x, y, z, val );
	}
}
 
void* func( void* )
{
	while( ! heap.empty() )
		heap.clear();
 
	pushfew( sx, sy, sz );
	while( ! heap.empty() )
	{
		pos t = * heap.begin();
		heap.erase( heap.begin() );
		if( t.x==ex && t.y==ey && t.z==ez )
		{
			printf("Found path: %d\n", t.val );
			Data( t.x, t.y, t.z ) |= t.type;
			break;
		}
		odw( t.x, t.y, t.z, t.val, t.type );
	}
	if( Data(ex,ey,ez ) & 15 )
	{
		while( ex != sx || ey != sy || ez != sz )
		{
			int type = Data( ex, ey, ez );
			Data( ex, ey, ez ) |= DT_PATH;
			if( type & DT_NORTH )
				ey++;
			if( type & DT_SOUTH )
				ey--;
			if( type & DT_WEST )
				ex++;
			if( type & DT_EAST )
				ex--;
			if( type & DT_UP )
				ez++;
			if( type & DT_DOWN )
				ez--;
		}
	}
	locked = false;
}
 
void AStar( int x, int y, int z )
{
	if( ! selected )
	{
		memset( data, 0, sizeof( data ) );
		AddStairs();
 
		Data( x, y, z ) = DT_START;
		sx = x, sy = y, sz = z;
		selected = true;
	}else
	if( selected )
	{
		locked = true;
		Data( x, y, z ) = DT_END;
		ex = x, ey = y, ez = z;
		selected = false;	
#ifndef _DISABLE_PTHREAD_
		pthread_create( &T, NULL, &func, NULL );
#else
		func( NULL );
#endif//_DISABLE_PTHREAD_
	}
}
 
int main()
{
	LoadMap();
	LoadStairsFile("stairs.txt");
 
	SDL_Surface *screen = InitSDL();
	Uint32 *pixels = (Uint32*) screen->pixels;
 
	SDL_Event event;
	int x = resW/2, y=resH/2, z=7, scale = 1;
	bool walk = false;
	while( true )
	{
		Draw( pixels,x,y,z,scale,walk );		
 
		while (SDL_PollEvent(&event))
 
		{
 
 
 
			switch (event.type)
 
			{
 
 
 
				case SDL_QUIT:
 
					exit( 0 );
 
 
 
	            case SDL_KEYDOWN:
					switch( event.key.keysym.sym )
					{
 
						case SDLK_ESCAPE:
 
							exit( 0 );
						case 'q': scale*=2; break;
						case 'e': if( scale > 1 ) scale/=2; break;
						case 'w': y -= resH/scale/4; break;
						case 's': y += resH/scale/4; break;
						case 'a': x -= resW/scale/4; break;
						case 'd': x += resW/scale/4; break;
						case '`': walk = !walk; none.c = 255-none.c; break;
						case '1': hideyellow = !hideyellow; break;
						case '2': printf("A* heuristic = %d\n", cost-=5 ); anticost=1; break;
						case '3': printf("A* heuristic = %d\n", cost+=5 ); anticost=1; break;
						case '4': printf("Dijkstra algorithm\n" ); cost = 0; anticost=1; break;
						case '5': printf("A* heuristic  = %d \n", cost=100 ); anticost=1; break;
						case '6': printf("A* heuristic  = %d \n", cost=225 ); anticost=1; break;
						case '7': printf("Straight Line algorithm\n" ); if( ! cost ) cost = 1; anticost=0; break;
						case 'r': z--; break;
						case 'f': z++; break;
 
					}
 
					break;
				case SDL_MOUSEBUTTONDOWN:
				{
					if( locked )
						break;
					int px = event.button.x;
					int py = event.button.y;
 
					px = x - resW/scale/2 + px/scale;
					py = y - resH/scale/2 + py/scale;
					int pz = z;
					if( event.button.button == 1 )
						AStar( px, py, pz );
					if( event.button.button == 3 )
						printf("%d %d %d\n", px+X,py+Y,pz);
				}
					break;
 
	        }
		}
		timeval tv = { 0, 10000 };
		select( 0, 0, 0, 0, &tv );
		SDL_Flip( screen );
	}
}

Code uses my /home directory, you must change it. Also it requires SDL and pthread(i am not sure if it exists for windows, if not use -D_DISABLE_PTHREAD_ and get eady for hangouts ). I am also unsure about select on windows (maybe it is in winsock32.h), but it is used only as a sleep function with usec's as parameter.

Then you need stairs.txt file:

Code:

//Dwarven Bridge
32609 31963 7 1 2 ramp
32614 31976 7 1 2 ramp
32624 31974 7 1 1 ramp
32630 31962 7 1 1 ramp
32609 31963 6 0 1 ramp
32614 31976 6 0 1 ramp
32624 31974 6 0 2 ramp
32630 31962 6 0 2 ramp

//Cyclops-Thais Bridge
32548 32076 7 1 2 ramp
32548 32077 7 1 2 ramp
32554 32076 7 1 1 ramp
32554 32077 7 1 1 ramp
32548 32076 6 0 1 ramp
32548 32077 6 0 1 ramp
32554 32076 6 0 2 ramp
32554 32077 6 0 2 ramp

And if you run it here is manual:

Code:

case ESC: exit
                        case 'q': zoom in
                        case 'e': zoom out
                        case 'w': go north
                        case 's': go south
                        case 'a': go west
                        case 'd': go east
                        case '`': switch between minimap and walkmap (speed for every tile, 0xff if blocable - i inverted colors: blockable is black)
                        case '1': hide yellow color (tiles that was checked in algorithm)
                        case '2': decrase A* heuristic by 5
                        case '3': increase ... by 5
                        case '4': use Dijkstra Alghorithm
                        case '5': use A* with heuristic 100 (almost under estimated)
                        case '6': use A* with heuristic 225 (probably overestimated)
                        case '7': some weird modification that tires to go in right way, even if it means turning back :P
                        case 'r': floor up
                        case 'f': floor down

And again, code is very messy - I'll probably clean it some day (making it object-oriented probably).

[Dored]

wtf you do know c++ alot!
Really good this!

[DarkstaR]

Simply amazing.

[Ian]

Very cool. I really like the idea of the heat-map for walk speed!

Could you post a compiled executable so we can run it easier? It isn't that I can't compile it, but it is nice if you don't have to. You can just ask the user for the maps directory. Also, please attach the code also, since it looks like ©'s get turned into ©'s...

So far though, it looks awesome!

[Noxitu]

I will try to post new version (fully object made - as you prefer C# it will be easy to translate) in a few days. But i still didn't manage to compile it on windows - it looks like SDL dislikes console applications (but maybe it is just something wrong with my Dev-cpp or some other VM setup). But expect some code update within 10-15 hours from now.

[Noxitu]

It works on windows now, but threads looks quite slow there.

Also there are still few global values - X, Y, Z, W, H that describes minimap position and size for rl tibia. Probably still some positions are stored only as offset from the begining of map. But rest looks quite good for now.

I also included lib and include directories for pthread, .devpak for sdl and .dev project, so it can be easly compiled on windows.

Ah, and now to change path you simply run it like: map.exe "c:\mypath"

[Ian]

Very nice job, the new code looks great. The separation of data/logic and display makes it easier to use the code with other front ends.

Could you explain the algorithms used and how you incorporated level changing?

[Noxitu]

(02-27-2010 01:46 AM)Ian Wrote:  Very nice job, the new code looks great. The separation of data/logic and display makes it easier to use the code with other front ends.

Could you explain the algorithms used and how you incorporated level changing?

Algorithms are quite simple. I used A* ( I am using binary tree(map) instead of heap, but it is still log n ). Its implementation should look quite similar to BFS algorithm. Many functions are made only to shorten the code.

Each tile stores it's moving speed. 0 is no cost, 255 is blockable.

Then I am loading stairs from external file - for now it is simply text file, with coords, type (direction we must move after moving) and up/down flag (for teleports there is other syntax). Also in few places I included description of stairs - they could be helpful if we don't have rope or implement ships into pathfinding.

This file probably should be stored as xml, but I used plain format because it is easy to load and easiest to convert into any other format.

There are also some things that can look weird:

I tried to make the code fast and short - for example if stairs is blockable (like stairs, holes, ramps ) I change it cost into 254 and add flag into other var.

Also I am loading map not into simple array [x][y][z], but something like [x%256][x/256][y%256][y/256][z]. In case of c++ operations %256 and /256 are translated into binary operators, so they are fast and loading map from files is made by memcpy ( order of table dimensions is selected, that each 256*256 is continuous ).


If you wish I could try to comment this code.

[Ian]

(03-01-2010 03:44 PM)Noxitu Wrote:  If you wish I could try to comment this code.

Thanks for the explanation, it would certainly be helpful if you did comment the code .