astarheapinteger.cpp

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

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

/*
AStarHeapInteger version 3i

*/

#include "stdafx.h"
#include "AStarHeapInteger.h"

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

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

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

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

AStarHeapInteger::AStarHeapInteger(Setup *vSetup, AIROUTE *airoute)
{
	this->vSetup=vSetup;
	this->airoute=airoute;
	
	GetOptions();
	UpdateSettings();
	UpdateWorld();
	Reset(airoute);
	
	// empty node
	nodes[0].yx=0;
	nodes[0].f=0;
	nodes[0].g=0;
	
	// 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;
}

AStarHeapInteger::~AStarHeapInteger()
{
	
}

void AStarHeapInteger::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->state=(pworld->terrain_cost==IMPASSABLE_TERRAIN_COST);
		pworld++;
	} while(--n>0);
	
	//
	frame=0;
	
	pathing_state=FINDING;
	path_found=false;
	no_path=false;
	
	//
	open_nodes=1;
	closed_nodes=0;
	
	// open node see
	best_f_node=1;
	nodes[best_f_node].yx=startyx;
	nodes[best_f_node].g=0;
	nodes[best_f_node].f=nodes[best_f_node].g+(distance(startyx)*DXY[4].cost_multiplier);
	world[startyx].route=NO_ROUTE;
	
	//closed node seed
	closed_node=0;
	
	free_node=1;
	
	//
	heap[0]=EMPTY_NODE;
	last_heap_leaf=1;
	heap[last_heap_leaf]=best_f_node;
	
	//
	LARGE_INTEGER tmp2;
	QueryPerformanceCounter(&tmp2);
	bigtick.QuadPart += tmp2.QuadPart - tmp1.QuadPart;
}






///////////////////////////////////////////////////////////////////////////
// heap priority queue code
// note: make non-recursive
inline int AStarHeapInteger::LEFT(int k)
{
	return k<<1; //k*2;
}
inline int AStarHeapInteger::RIGHT(int k)
{
	return (k<<1)+1; //k*2+1;
}
inline int AStarHeapInteger::PARENT(int k)
{
	return (k>>1); //k/2;
}
inline bool AStarHeapInteger::NOTEMPTY_UP(int k)
{
	return k!=0;
}
inline bool AStarHeapInteger::NOTEMPTY_DOWN(int k)
{
	return k<=last_heap_leaf;
}
inline void AStarHeapInteger::swap_heap(const int k1, const int k2)
{
	WORD tmp=heap[k1];
	heap[k1]=heap[k2];
	heap[k2]=tmp;
}
//
void AStarHeapInteger::remove_root_from_heap()
{
	// move last leaf to the root and delte 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) )
		{
			if(nodes[heap[leftk]].f < nodes[heap[rightk]].f)
				bestk=leftk;
			else
				bestk=rightk;
		}
		else if(NOTEMPTY_DOWN(leftk))
			bestk=leftk;
		else
			break;
		
		if(nodes[heap[bestk]].f < nodes[heap[k]].f)
		{
			swap_heap(k,bestk);
			k=bestk;
		}
		else
			break;
	}
}
//
void AStarHeapInteger::insert_node_to_heap(WORD node)
{
	// add new leaf unless the tree is full.
	// in that case prune worst leaf
	//(ie, remove the very last leaf before adding another).
	if(last_heap_leaf<MAX_HEAP_LEAFS)
		last_heap_leaf++;
	
	//
	heap[last_heap_leaf]=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(nodes[heap[k]].f < nodes[heap[parentk]].f)
			{
				swap_heap(k,parentk);
				k=parentk;
			}
			else
				break;
		}
		else
			break;
	}
}




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

//
void AStarHeapInteger::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].terrain_cost==IMPASSABLE_TERRAIN_COST ||
		world[endyx].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;
	open_nodes=last_heap_leaf;
	closed_nodes=(free_node-last_heap_leaf);
}

//
void AStarHeapInteger::without_terrain()
{
	int count=iterations_per_frame;
	do
	{
		closed_node=best_f_node=heap[ROOT_HEAP]; //parent_node
		
		// PART A close node
		_NODES *pparent_node=nodes+best_f_node;
		if(pparent_node->yx==endyx) pathing_state|=PATH_FOUND; // did we find the goal?
		world[ pparent_node->yx ].state=CLOSED; // map the world map with closed state
		remove_root_from_heap(); //
		
		// PART B open nodes
		for(BYTE d=0;d<directions;d++)
		{
			// figure the [y][x] offset into the world map
			WORD yx=pparent_node->yx+DXY[d].yx;
			_WORLD *pworld=world+yx;
			
			if(pworld->state==UNKNOWN) //if we've never visited this [y][x] before...
			{
				pworld->state=OPEN; //mark [y][x] of world map as part of an open node
				pworld->route=d; //store routing info for use later in packroute...
				
				free_node++; //cycle up to the next unused slot for a node
				// since we don't actually delete nodes when we close them there's no need to
				// search unused nodes out.
				
				_NODES *pfree_node=nodes+free_node;
				
				pfree_node->yx=yx; // mark the [y][x] position of this node for the world map
				
				// figure what the new f and g will be and store directly to [].f and [].g
				pfree_node->g=pparent_node->g + DXY[d].cost_multiplier;
				pfree_node->f=pfree_node->g+distance(yx);
				
				// insert this new node into the binary heap
				insert_node_to_heap(free_node);
			}
		}
		
		if(last_heap_leaf<=0) pathing_state|=NO_PATH;
	} while(--count>0 && !pathing_state);
	
	//
	frame+=(iterations_per_frame-count); //
}


//
void AStarHeapInteger::with_terrain()
{
	int count=iterations_per_frame;
	do
	{
		closed_node=best_f_node=heap[ROOT_HEAP]; //parent_node
		
		// PART A close node
		_NODES *pparent_node=nodes+best_f_node;
		if(pparent_node->yx==endyx) pathing_state|=PATH_FOUND; // did we find the goal?
		world[ pparent_node->yx ].state=CLOSED; // map the world map with closed state
		remove_root_from_heap(); //
		
		// PART B open nodes
		for(BYTE d=0;d<directions;d++)
		{
			// figure the [y][x] offset into the world map
			WORD yx=pparent_node->yx+DXY[d].yx;
			_WORLD *pworld=world+yx;
			
			if(pworld->state==UNKNOWN) //if we've never visited this [y][x] before...
			{
				pworld->state=OPEN; //mark [y][x] of world map as part of an open node
				pworld->route=d; //store routing info for use later in packroute...
				
				free_node++; //cycle up to the next unused slot for a node
				// since we don't actually delete nodes when we close them there's no need to
				// search unused nodes out.
				
				_NODES *pfree_node=nodes+free_node;
				
				pfree_node->yx=yx; // mark the [y][x] position of this node for the world map
				
				// figure what the new f and g will be and store directly to [].f and [].g
				pfree_node->g=pparent_node->g + (pworld->terrain_cost*DXY[d].cost_multiplier);
				pfree_node->f=pfree_node->g+distance(yx);
				
				// insert this new node into the binary heap
				insert_node_to_heap(free_node);
			}
		}
		
		if(last_heap_leaf<=0) pathing_state|=NO_PATH;
	} while(--count>0 && !pathing_state);
	
	//
	frame+=(iterations_per_frame-count); //
}


// note: removing switchs and just using manhattan saves a grand total of .5ms on
// the test machine. :)
inline WORD AStarHeapInteger::distance(const WORD yx)
{
	switch(distance_method)
	{
	case MANHATTAN_DISTANCE:
		{
			return (WORD)(abs((yx&XMASK)-endx) + abs((yx>>YSHIFT)-endy));
		}
		break;
		
	case PYTHAGORAS_DISTANCE: //aka STRAIGHT LINE, pythagoras
		{
			int xpart=(yx&XMASK)-endx;
			int ypart=(yx>>YSHIFT)-endy;
			return (WORD)sqrt(xpart*xpart + ypart*ypart);
		}
		break;
		
	case DIAGONAL_DISTANCE:
		{
			WORD a=abs((yx&XMASK)-endx);
			WORD b=abs((yx>>YSHIFT)-endy);
			return (a>b)?a:b;
		}
		break;
		
	case SIMPLE_PYTHAGORAS_DISTANCE:
		{
			int xpart=(yx&XMASK)-endx;
			int ypart=(yx>>YSHIFT)-endy;
			return (WORD)(xpart*xpart + ypart*ypart);
		}
		break;
	default: return 0;
	}
}







///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
// 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 AStarHeapInteger::GetOptions()
{
	vSetup->options.uniform_cost=true;
	vSetup->options.terrain_cost=true;
	vSetup->options.distance=true;