astarheap.cpp

一个VC写A*寻路的程序库

	vSetup->options.search_directions=true;
}

// this transfers the world map from Setup.cpp to the internal state here.
void AStarHeap::UpdateWorld()
{
	LARGE_INTEGER tmp1;
	QueryPerformanceCounter(&tmp1);
	
	//
	for(int y=0;y<HEIGHT;y++)
	{
		for(int x=0;x<WIDTH;x++)
		{
			WORD yx=(y<<YSHIFT)+x;
			world[yx].terrain_cost=vSetup->world[y][x].terrain_cost;
			world[yx].state=(world[yx].terrain_cost==IMPASSABLE_TERRAIN_COST); //?IMPASSABLE:UNKNOWN;
		}
	}
	
	//
	LARGE_INTEGER tmp2;
	QueryPerformanceCounter(&tmp2);
	bigtick.QuadPart += tmp2.QuadPart - tmp1.QuadPart;
}

// this transfers the all the settings in Setup.cpp to here.
void AStarHeap::UpdateSettings()
{
	startyx=vSetup->starty*WIDTH+vSetup->startx;
	endyx=vSetup->endy*WIDTH+vSetup->endx;
	
	endx=endyx&XMASK;
	endy=(endyx>>YSHIFT);
	
	use_terrain=vSetup->use_terrain;
	distance_method=vSetup->distance_method;
	
	iterations_per_frame=vSetup->iterations_per_frame;
	
	directions=vSetup->directions;
	
	float cost=vSetup->cost;
	float diagonal_cost=vSetup->diagonal_cost;
	float median_terrain_cost=vSetup->median_terrain_cost;
	
	int n;
	
	if(use_terrain)
	{
		for(n=0;n<4;n++)
		{
			DXY[n].cost_multiplier=median_terrain_cost;
		}
			for(n=4;n<8;n++)
			{
				DXY[n].cost_multiplier=diagonal_cost*median_terrain_cost;
			}
	}
	else
	{
		for(n=0;n<4;n++)
		{
			DXY[n].cost_multiplier=1.0f*cost;
		}
			for(n=4;n<8;n++)
			{
				DXY[n].cost_multiplier=diagonal_cost*cost;
			}
	}
}



///////////////////////////////////////////////////////////////////////////
// TYPE B
void AStarHeap::PackRoute()
{
	if(pathing_state&NO_PATH)
	{
		airoute->count=0;
		return;
	}
	
	//
	memset(airoute->route,0,MAX_ROUTES); //clear routes
	
	//
	airoute->active=1;
	airoute->compression=0;
	
	//
	WORD yx=endyx;
	int start=MAX_ROUTES-1;
	BYTE route=NO_ROUTE;
	while(yx!=startyx)
	{
		//		route=DXY[world[yx].route].route;
		route=world[yx].route;
		yx+=DXY[DXY[route].route].yx;
		airoute->route[start]=route;
		if(--start<0) start=MAX_ROUTES-1;
	};
	
	airoute->start=start+1;
	airoute->count=MAX_ROUTES-airoute->start;
	
	//
	airoute->startyx=startyx;
	airoute->endyx=endyx;
	
	//
	airoute->walk_point=airoute->start;
	airoute->walk_runlength_step=0;
	
	//     
	if(airoute->start==0) airoute->count=0;
}







///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
/*
This is the function that paints the graphics to the screen.
Called by PathFinder.
Localized to respective Setup classes v1.6.
*/
//
void AStarHeap::Paint(LPBYTE pwBits, HDC hdc,HWND hWnd)
{
	if(pwBits)
	{
		//
		RECT rt;
		GetClientRect(hWnd, &rt);
		int clientwidth = (rt.right-rt.left);
		//		int clientheight = (rt.bottom-rt.top);
		
		COLORREF background;
		COLORREF foreground;
		vSetup->get_colorscheme_colors(background,foreground);
		HBRUSH hbrBkGnd = CreateSolidBrush(background);
		FillRect(hdc, &rt, hbrBkGnd);
		DeleteObject(hbrBkGnd);
		SetBkColor(hdc,background);
		SetTextColor(hdc,foreground);
		
		//
		int starty=startyx>>YSHIFT;
		int startx=startyx&XMASK;
		int endy=endyx>>YSHIFT;
		int endx=endyx&XMASK;
		
		TCHAR szStatusLine[1024];
		_RGBA *p,*ppush;
		_RGBA *pbegin=(_RGBA *)pwBits;
		const int yxmax=WIDTH*HEIGHT;
		int y,x;
		int yx;
		int node;
		int count;
		int n;
		register _RGBA *tmp;
		
		//
		int used_nodes=0;
		float smallest_f, smallest_g, smallest_h;
		float largest_f, largest_g, largest_h;
		smallest_f = smallest_g = smallest_h = 1048576.0f;
		largest_f = largest_g = largest_h = 0.0001f;
		for(node=1;node<=free_node;node++)
		{
			if(nodes[node].f>largest_f) largest_f=nodes[node].f;
			if(nodes[node].f<smallest_f) smallest_f=nodes[node].f;
			if(nodes[node].g>largest_g) largest_g=nodes[node].g;
			if(nodes[node].g<smallest_g) smallest_g=nodes[node].g;
			float h=nodes[node].f-nodes[node].g;
			if(h>largest_h) largest_h=h;
			if(h<smallest_h) smallest_h=h;
			used_nodes++;
		}
		
		//
		int length=0;
		p=pbegin;
		yx=nodes[best_f_node].yx;
		while(yx!=startyx && yx>0 && yx<yxmax)
		{
			yx+=DXY[DXY[world[yx].route].route].yx;
			length++;
			if(length>MAX_NODE) break;
		}
		
		
		// stats
		GetClientRect(hWnd, &rt);
		sprintf(szStatusLine,
			"-- A* Heap (v3) --\nf %f ... %f\ng %f ... %f\nh %f ... %f\n\nstruct size %d bytes\n(nodes %d B)\n(heap %d B)\n(world %d B)\n[used node %d B]\n[used heap %d B]\n\nMAX NODES %d\nBest Node\n%4d\nf %f\ng %f\nh %f\nClosed Node\n%4d\nf %f\ng %f\nh %f\n\npath found? %s\nlength= %d\n\nstart -> current\nf %f -> %f\ng %f -> %f\nh %f -> %f\n%s\n",
			smallest_f, largest_f,
			smallest_g, largest_g,
			smallest_h, largest_h,
			sizeof(nodes) + sizeof(world) + sizeof(heap),
			sizeof(nodes),
			sizeof(heap),
			sizeof(world),
			used_nodes*(sizeof(_NODES)),
			last_heap_leaf*(sizeof(WORD)),
			MAX_NODE,
			best_f_node,
			nodes[best_f_node].f,
			nodes[best_f_node].g,
			nodes[best_f_node].f-nodes[best_f_node].g,
			closed_node,
			nodes[closed_node].f,
			nodes[closed_node].g,
			nodes[closed_node].f-nodes[closed_node].g,
			pathing_state&NO_PATH?"NO PATH!":(pathing_state&PATH_FOUND?"YES!":"Not yet..."),
			length,
			nodes[1].f,nodes[closed_node].f,
			nodes[1].g,nodes[closed_node].g,
			nodes[1].f-nodes[1].g,nodes[closed_node].f-nodes[closed_node].g,
			(nodes[closed_node].f<=nodes[1].f)?"ADMISSIBLE":"not admissible"
			);
		DrawText(hdc, szStatusLine, strlen(szStatusLine), &rt, DT_RIGHT);
		
		
		//shortest-path w/ terrain
		GetClientRect(hWnd, &rt);
		rt.top=HEIGHT;
		rt.right=WIDTH;
		sprintf(szStatusLine,"shortest path w/terrain");
		DrawText(hdc, szStatusLine, strlen(szStatusLine), &rt, DT_CENTER);
		p=ppush=pbegin;
		for(y=0;y<HEIGHT;y++)
		{
			for(x=0;x<WIDTH;x++)
			{
				BYTE b=(BYTE)(255-(world[y*WIDTH+x].terrain_cost<<4));
				p->red=b; p->green=b; p->blue=b;
				p++;
			}
			ppush+=clientwidth;
			p=ppush;
		}
		for(y=-1;y<=1;y++) //starting point
		{
			for(x=-1;x<=1;x++)
			{
				tmp=(pbegin+(starty+y)*clientwidth+startx+x);
				tmp->red=255;
				tmp->green=0;
				tmp->blue=0;
			}
		}
		for(y=-1;y<=1;y++) //end/goal point
		{
			for(x=-1;x<=1;x++)
			{
				tmp=(pbegin+(endy+y)*clientwidth+endx+x);
				tmp->red=0;
				tmp->green=255;
				tmp->blue=0;
			}
		}
		p=pbegin;
		yx=nodes[best_f_node].yx;
		while(yx!=startyx && yx>0 && yx<yxmax)
		{
			tmp=(p+(yx>>YSHIFT)*clientwidth+(yx&XMASK));
			tmp->red=255;
			tmp->green=0;
			tmp->blue=0;
			yx+=DXY[DXY[world[yx].route].route].yx;
			if(length>MAX_NODE) break;
		}
		
		
		// f
		GetClientRect(hWnd, &rt);
		rt.top=HEIGHT;
		rt.left=WIDTH+4;
		rt.right=WIDTH+4+WIDTH;
		sprintf(szStatusLine,"f (red closed/blue open)");
		DrawText(hdc, szStatusLine, strlen(szStatusLine), &rt, DT_CENTER);
		p=pbegin+(WIDTH+4);
		count=MAX_NODE;
		for(node=1;node<=free_node;node++)
		{
			tmp=(p+(nodes[node].yx>>YSHIFT)*clientwidth+(nodes[node].yx&XMASK));
			tmp->red=(BYTE)((nodes[node].f/largest_f)*255.0f);
			tmp->green=0;
			tmp->blue=0;
		};
		for(n=1;n<=last_heap_leaf;n++)
		{
			node=heap[n];
			tmp=(p+(nodes[node].yx>>YSHIFT)*clientwidth+(nodes[node].yx&XMASK));
			tmp->red=0;
			tmp->green=0;
			tmp->blue=(BYTE)((nodes[node].f/largest_f)*255.0f);
		}
		
		
		// open
		int levels=0;
		{
			int b=1;
			int sum=b;
			while(b<=last_heap_leaf)
			{
				b=b<<1;
				sum+=b;
				levels++;
			}
		}
		GetClientRect(hWnd, &rt);
		rt.top=HEIGHT*2+16;
		rt.left=0;
		rt.right=WIDTH;
		sprintf(szStatusLine,"open nodes %d (%d heap levels)",last_heap_leaf,levels);
		DrawText(hdc, szStatusLine, strlen(szStatusLine), &rt, DT_CENTER);
		p=pbegin+(HEIGHT+16)*clientwidth;
		for(n=1;n<=last_heap_leaf;n++)
		{
			node=heap[n];
			
			BYTE f=(BYTE)((nodes[node].f/largest_f)*255.0f);
			BYTE g=(BYTE)((nodes[node].g/largest_g)*255.0f);
			BYTE h=(BYTE)(((nodes[node].f-nodes[node].g)/largest_h)*255.0f);
			tmp=(p+(nodes[node].yx>>YSHIFT)*clientwidth+(nodes[node].yx&XMASK));
			tmp->red=h;
			tmp->green=g;
			tmp->blue=f;
		}
		
		
		// closed
		GetClientRect(hWnd, &rt);
		rt.top=HEIGHT*2+16;
		rt.left=WIDTH+4;
		rt.right=WIDTH+4+WIDTH;
		sprintf(szStatusLine,"closed nodes %d",closed_nodes);
		DrawText(hdc, szStatusLine, strlen(szStatusLine), &rt, DT_CENTER);
		p=pbegin+WIDTH+4+(HEIGHT+16)*clientwidth;
		for(node=1;node<=free_node;node++)
		{
			BYTE f=(BYTE)((nodes[node].f/largest_f)*255.0f);
			BYTE g=(BYTE)((nodes[node].g/largest_g)*255.0f);
			BYTE h=(BYTE)(((nodes[node].f-nodes[node].g)/largest_h)*239.0f);
			tmp=(p+(nodes[node].yx>>YSHIFT)*clientwidth+(nodes[node].yx&XMASK));
			tmp->red=h;
			tmp->green=g;
			tmp->blue=f;
		}
		for(n=1;n<=last_heap_leaf;n++) //erase
		{
			node=heap[n];
			DWORD *tmp=((DWORD *)p+(nodes[node].yx>>YSHIFT)*clientwidth+(nodes[node].yx&XMASK));
			*tmp=(DWORD)background;
		}
		
		
		// state
		GetClientRect(hWnd, &rt);
		rt.top=HEIGHT;
		rt.left=(WIDTH+4)*2;
		rt.right=(WIDTH+4)*2+WIDTH;
		sprintf(szStatusLine,"state (g=open b=closed r=impass)");
		DrawText(hdc, szStatusLine, strlen(szStatusLine), &rt, DT_CENTER);
		p=pbegin+(WIDTH+4)*2;
		for(y=0;y<HEIGHT;y++)
		{
			for(x=0;x<WIDTH;x++)
			{
				yx=y*WIDTH+x;
				tmp=(p+y*clientwidth+x);
				tmp->green=(BYTE)((world[yx].state==OPEN)?255:0);
				tmp->blue=(BYTE)((world[yx].state==CLOSED)?255:0);
				tmp->red=(BYTE)((world[yx].state==IMPASSABLE)?255:0);
			}
		}
		
		
		//routing
		GetClientRect(hWnd, &rt);
		rt.top=HEIGHT*2+16;
		rt.left=(WIDTH+4)*2;
		rt.right=(WIDTH+4)*2+WIDTH;
		sprintf(szStatusLine,"routes");
		DrawText(hdc, szStatusLine, strlen(szStatusLine), &rt, DT_CENTER);
		p=pbegin+(WIDTH+4)*2+(HEIGHT+16)*clientwidth;
		for(y=0;y<HEIGHT;y++)
		{
			for(x=0;x<WIDTH;x++)
			{
				tmp=(p+y*clientwidth+x);
				switch(world[(y<<YSHIFT)+x].route)
				{
				case 0: tmp->red=0;		tmp->green=0;	tmp->blue=255;	break; // n
				case 1: tmp->red=127;	tmp->green=0;	tmp->blue=255;	break; // e
				case 2: tmp->red=255;	tmp->green=0;	tmp->blue=0;	break; // s
				case 3: tmp->red=255;	tmp->green=0;	tmp->blue=127;	break; // w pink
				case 4: tmp->red=0;		tmp->green=255;	tmp->blue=255;	break; // ne
				case 5: tmp->red=127;	tmp->green=255;	tmp->blue=255;	break; // se
				case 6: tmp->red=255;	tmp->green=255;	tmp->blue=0;	break; // sw
				case 7: tmp->red=255;	tmp->green=255;	tmp->blue=127;	break; // nw
				};
			}
		}
	}
}