development.cpp

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

// Development.cpp: implementation of the Development class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "Development.h"

#include "math.h" //sqrt
#include "stdio.h" //for sprintf

//#pragma optimize( "yawgt", on )

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

Development::Development()
{
	vSetup=NULL;
	airoute=NULL;
}

Development::Development(Setup *vSetup, AIROUTE *airoute)
{
	this->vSetup=vSetup;
	this->airoute=airoute;
	
	GetOptions();
	UpdateSettings();
	UpdateWorld();
	Reset(airoute);
	
	// make the routing lookup table
	DXY[0].yx=-WIDTH;	DXY[0].route=2;
	DXY[1].yx=1;		DXY[1].route=3;
	DXY[2].yx=WIDTH;	DXY[2].route=0;
	DXY[3].yx=-1;		DXY[3].route=1;
	
	DXY[4].yx=-WIDTH+1;	DXY[4].route=6;
	DXY[5].yx=WIDTH+1;	DXY[5].route=7;
	DXY[6].yx=WIDTH-1;	DXY[6].route=4;
	DXY[7].yx=-WIDTH-1;	DXY[7].route=5;
	
	//in case a NO_ROUTE accidentally is passed for lookup
	DXY[8].yx=0;		DXY[8].route=0;
}

Development::~Development()
{
	
}

void Development::Reset(AIROUTE *airoute)
{
	//
	bigtick.QuadPart=0;
	LARGE_INTEGER tmp1;
	QueryPerformanceCounter(&tmp1);
	
	this->airoute=airoute;
	airoute->count=0;
	
	// opt me
	_WORLD * pworld=world;
	int n=WIDTH*HEIGHT;
	do {
		pworld->u.raw&=0xf;
		pworld++;
	} while(--n>0);
	
	//
	frame=0;
	
	pathing_state=FINDING;
	path_found=false;
	no_path=false;
	
	//
	closed_yx=EMPTY_NODE;
	
	//
	heap[0].node=EMPTY_NODE;
	heap[0].f=0;
	heap[0].g=0;
	
	last_heap_leaf=1;
	heap[last_heap_leaf].node=startyx;
	
	heap[last_heap_leaf].g=0;
//	if(use_terrain)
		heap[last_heap_leaf].f=heap[last_heap_leaf].g+distance(startyx)*DXY[4].cost_multiplier;
//	else
//		heap[last_heap_leaf].f=heap[last_heap_leaf].g+distance(startyx);
	world[startyx].u.state.route=NO_ROUTE;
	
	//
	LARGE_INTEGER tmp2;
	QueryPerformanceCounter(&tmp2);
	bigtick.QuadPart += tmp2.QuadPart - tmp1.QuadPart;
}





///////////////////////////////////////////////////////////////////////////
// heap priority queue code
// note: make non-recursive
inline int Development::LEFT(int k)
{
	return k<<1; //k*2;
}
inline int Development::RIGHT(int k)
{
	return (k<<1)+1; //k*2+1;
}
inline int Development::PARENT(int k)
{
	return (k>>1); //k/2;
}
inline bool Development::NOTEMPTY_UP(int k)
{
	return k!=0;
}
inline bool Development::NOTEMPTY_DOWN(int k)
{
	return k<=last_heap_leaf;
}
inline void Development::swap_heap(const int k1, const int k2)
{ // swaps node,f and g as quickly as possible
	register _HEAP *heap1=&heap[k1];
	register _HEAP *heap2=&heap[k2];
	register WORD tmpnode;
	register DWORD tmp;
	
	tmpnode=heap1->node;
	tmp=*(DWORD *)heap1; //swap f & g at same time
	
	heap1->node=heap2->node;
	*heap1=*heap2;
	
	heap2->node=tmpnode;
	*(DWORD *)heap2=tmp;
}
// note: slowest function, speed up
inline void Development::remove_root_from_heap()
{
	// move last leaf to the root and delete the last leaf
	heap[ROOT_HEAP]=heap[last_heap_leaf--];
	
	//move transposed k back down level to appropriate place
	int k=ROOT_HEAP;
	while(NOTEMPTY_DOWN(k))
	{
		int leftk=LEFT(k);
		int rightk=RIGHT(k);
		int bestk;
		if(NOTEMPTY_DOWN(leftk) && NOTEMPTY_DOWN(rightk) )
		{
			bestk=(heap[leftk].f < heap[rightk].f)?leftk:rightk;
		}
		else if(NOTEMPTY_DOWN(leftk))
			bestk=leftk;
		else
			break;
		
		if(heap[bestk].f < heap[k].f)
		{
			swap_heap(k,bestk);
			k=bestk;
		}
		else
			break;
	}
}
//
// note: 2nd slowest function, speed up
inline void Development::insert_node_to_heap(const WORD node, const WORD g)
{
	if(++last_heap_leaf<MAX_HEAP_LEAFS)
	{
		//
		heap[last_heap_leaf].node=node;
		heap[last_heap_leaf].g=g;
		heap[last_heap_leaf].f=g+distance(node);
		
		// move new k up through heap to closest level
		int k=last_heap_leaf;
		while(NOTEMPTY_UP(k))
		{
			int parentk=PARENT(k);
			if(NOTEMPTY_UP(parentk))
			{
				if(heap[k].f < heap[parentk].f)
				{
					swap_heap(k,parentk);
					k=parentk;
				}
				else
					break;
			}
			else
				break;
		}
	}
}




///////////////////////////////////////////////////////////////////////////

//
void Development::FindPath()
{
	if(pathing_state)
		return;
	if(
		(vSetup->presearch_toggle && vSetup->world[startyx>>YSHIFT][startyx&XMASK].group!=vSetup->world[endyx>>YSHIFT][endyx&XMASK].group) ||
		world[startyx].u.state.terrain_cost==IMPASSABLE_TERRAIN_COST ||
		world[endyx].u.state.terrain_cost==IMPASSABLE_TERRAIN_COST
		)
		pathing_state=NO_PATH;
	else
	{
		//
		LARGE_INTEGER tmp1;
		QueryPerformanceCounter(&tmp1);
		
		//
		if(use_terrain)
			with_terrain();
		else
			without_terrain();
		
		//
		if(pathing_state) PackRoute();
		
		//
		LARGE_INTEGER tmp2;
		QueryPerformanceCounter(&tmp2);
		bigtick.QuadPart += tmp2.QuadPart - tmp1.QuadPart;
		
		vSetup->frame=frame;
		vSetup->bigtick.QuadPart=bigtick.QuadPart;
	}
	
	//
	path_found=(pathing_state&PATH_FOUND)==PATH_FOUND;
	no_path=(pathing_state&NO_PATH)==NO_PATH;
}

//
void Development::without_terrain()
{
	int count=iterations_per_frame;
	do
	{
		closed_yx=heap[ROOT_HEAP].node;
		WORD best_g=heap[ROOT_HEAP].g;
		
		// PART A close node
		remove_root_from_heap(); //
		// did we find the goal?
		if(closed_yx==endyx) { pathing_state|=PATH_FOUND; break; }
		
		// PART B open world
		for(BYTE d=0;d<directions;d++)
		{
			// figure the [y][x] offset into the world map
			WORD yx=closed_yx+DXY[d].yx;
			_WORLD *pworld=world+yx;
			
			//if we've never visited this [y][x] before...
			if( (pworld->u.raw&0x0f)!=0x0f && (pworld->u.raw&0x10)!=0x10 )
			{
				//mark [y][x] of world map as part of an open node
				//store routing info for use later in packroute...
				pworld->u.raw|=16|(d<<5); 
				
				// figure what the new f and g will be and pass on to heap insert...
				// insert this new node into the binary heap
				insert_node_to_heap(yx,best_g + DXY[d].cost_multiplier);
			}
		}
		
		if(last_heap_leaf<=0) pathing_state|=NO_PATH;
	} while(--count>0 && !pathing_state);
	
	//
	frame+=(iterations_per_frame-count); //
}


//
void Development::with_terrain()
{
	int count=iterations_per_frame;
	do
	{
		closed_yx=heap[ROOT_HEAP].node;
		WORD best_g=heap[ROOT_HEAP].g;
		
		// PART A close node
		remove_root_from_heap(); //
		// did we find the goal?
		if(closed_yx==endyx) { pathing_state|=PATH_FOUND; break; }
		
		// PART B open world
		for(BYTE d=0;d<directions;d++)
		{
			// figure the [y][x] offset into the world map
			WORD yx=closed_yx+DXY[d].yx;
			_WORLD *pworld=world+yx;
			
			//if we've never visited this [y][x] before...
			if( (pworld->u.raw&0x0f)!=0x0f && (pworld->u.raw&0x10)!=0x10 )
			{
				//mark [y][x] of world map as part of an open node
				//store routing info for use later in packroute...
				pworld->u.raw|=16|(d<<5); 
				
				// figure what the new f and g will be and pass on to heap insert...
				// insert this new node into the binary heap
				insert_node_to_heap(yx,best_g + pworld->u.state.terrain_cost*DXY[d].cost_multiplier);
			}
		}
		
		if(last_heap_leaf<=0) pathing_state|=NO_PATH;
	} while(--count>0 && !pathing_state);
	
	//
	frame+=(iterations_per_frame-count); //
}


// manhattan
inline WORD Development::distance(const WORD yx)
{
	return (WORD)(abs((yx&XMASK)-endx) + abs((yx>>YSHIFT)-endy));
}




///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
// to make it easier to add many new Setups to patherfinder.cpp we use Setup.cpp 
// class as a holder for all general settings and world info.

// tells Setup.cpp what menu options should be enabled/disabled for this alg.
void Development::GetOptions()
{
	vSetup->options.uniform_cost=true;
	vSetup->options.terrain_cost=true;
	vSetup->options.distance=true;
	vSetup->options.search_directions=true;
}

// this transfers the world map from Setup.cpp to the internal state here.
void Development::UpdateWorld()
{