bestfirst.cpp

a星路径规划

void CBestFirst::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=Setup->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 CBestFirst::UpdateSettings()
{
	startyx=Setup->starty*WIDTH+Setup->startx;
	endyx=Setup->endy*WIDTH+Setup->endx;
	
	endx=endyx&XMASK;
	endy=(endyx>>YSHIFT);
	
	distance_method=Setup->distance_method;
	
	iterations_per_frame=Setup->iterations_per_frame;
	
	directions=Setup->directions;
}



///////////////////////////////////////////////////////////////////////////
// TYPE B
void CBestFirst::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 CBestFirst::Paint(LPBYTE pwBits, HDC hdc,HWND hWnd)
{
	if(pwBits)
	{
		//
		RECT rt;
		GetClientRect(hWnd, &rt);
		int clientwidth = (rt.right-rt.left);
		
		COLORREF background;
		COLORREF foreground;
		Setup->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_h = 1048576.0f;
		float largest_h = 0.0001f;
		for(node=1;node<=free_node;node++)
		{
			if(nodes[node].h>largest_h) largest_h=nodes[node].h;
			if(nodes[node].h<smallest_h) smallest_h=nodes[node].h;
			used_nodes++;
		}
		
		//
		int length=0;
		p=pbegin;
		yx=nodes[best_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,
			"-- Best-First --\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 %4d\nh %f\nClosed Node %4d\nh %f\n\npath found? %s\nlength= %d\n\nstart -> current\nh %f -> %f\n",
			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_node,
			nodes[best_node].h,
			closed_node,
			nodes[closed_node].h,
			pathing_state&NO_PATH?"NO PATH!":(pathing_state&PATH_FOUND?"YES!":"Not yet..."),
			length,
			nodes[1].h,nodes[closed_node].h
			);
		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=0;
				tmp->green=255;
				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=255;
				tmp->green=0;
				tmp->blue=0;
			}
		}
		
		p=pbegin;
		yx=nodes[best_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;
		}
		
		
		// h
		GetClientRect(hWnd, &rt);
		rt.top=HEIGHT;
		rt.left=WIDTH+4;
		rt.right=WIDTH+4+WIDTH;
		sprintf(szStatusLine,"h (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].h/largest_h)*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].h/largest_h)*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 h=(BYTE)((nodes[node].h/largest_h)*255.0f);
			tmp=(p+(nodes[node].yx>>YSHIFT)*clientwidth+(nodes[node].yx&XMASK));
			tmp->red=h;
			tmp->green=0;
			tmp->blue=0;
		}
		
		
		// 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 h=(BYTE)((nodes[node].h/largest_h)*255.0f);
			tmp=(p+(nodes[node].yx>>YSHIFT)*clientwidth+(nodes[node].yx&XMASK));
			tmp->red=h;
			tmp->green=0;
			tmp->blue=0;
		}
		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
				};
			}
		}
	}
}