astararray.cpp

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

// AStarArray.cpp: implementation of the AStarArray class.
//
//////////////////////////////////////////////////////////////////////
/*
AStar Version 1

  Static Arrays
*/

#include "stdafx.h"
#include "AStarArray.h"

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


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

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

AStarArray::AStarArray(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;
}

AStarArray::~AStarArray()
{
	
}

void AStarArray::Reset(AIROUTE *airoute)
{
	//
	bigtick.QuadPart=0;
	LARGE_INTEGER tmp1;
	QueryPerformanceCounter(&tmp1);

	this->airoute=airoute;
	airoute->count=0;

	//
	frame=0;
	
	path_found=false;
	no_path=false;
	
	open_nodes=1;
	closed_nodes=0;
	
	//
	for(int yx=0;yx<WIDTH*HEIGHT;yx++)
	{
		world[yx].state=UNKNOWN;
	}
	
	int n,i;
	for(n=0;n<=MAX_NODES;n++)
	{
		nodes[n].yx=0;
		nodes[n].f=0.0f;
		nodes[n].g=0.0f;
		nodes[n].h=0.0f;
		nodes[n].ancestor=EMPTY_NODE;
		nodes[n].open=true;
		for(i=0;i<8;i++) nodes[n].successor[i]=EMPTY_NODE;
		nodes[n].open_node_index=EMPTY_NODE;
	}
	
	for(n=0;n<MAX_OPEN_NODE_INDEXES;n++) //optimization c
		open_node_index[n]=EMPTY_NODE;

	//
	nodes[EMPTY_NODE].f=MAX_F; //fake high number to avoid special casing EMPTY_NODE
	nodes[EMPTY_NODE].open=false;

	//
	last_node=current_node=1;
	last_index=1;
	
	nodes[current_node].yx=startyx;
	nodes[current_node].g=0.0f;
	nodes[current_node].h=distance(startyx)*DXY[4].cost_multiplier;//*median_terrain_cost;
	nodes[current_node].f=nodes[1].g+nodes[current_node].h;
	nodes[current_node].open=true;
	nodes[current_node].open_node_index=last_index;
	open_node_index[last_index]=current_node;
	
	//
	open_node_index[0]=EMPTY_NODE;

	//
	LARGE_INTEGER tmp2;
	QueryPerformanceCounter(&tmp2);
	bigtick.QuadPart += tmp2.QuadPart - tmp1.QuadPart;
}


//
void AStarArray::FindPath()
{
	if(current_node==EMPTY_NODE || last_node<=0 || path_found || no_path)
		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
		)
	{
		no_path=true;
		return;
	}
	
	//
	LARGE_INTEGER tmp1;
	QueryPerformanceCounter(&tmp1);
	
	//
	int count=iterations_per_frame;
	do
	{
		//
		open_successor_nodes();
		WORD parent_node=current_node;
		keep_moving_forward_through_best_successor_node();
		move_node_to_closed(parent_node);
	} while(--count>0 && !path_found && !no_path);
	
	//
	frame+=(iterations_per_frame-count);
	
	//
	if(path_found || no_path) PackRoute();
	
	//
	LARGE_INTEGER tmp2;
	QueryPerformanceCounter(&tmp2);
	bigtick.QuadPart += tmp2.QuadPart - tmp1.QuadPart;
	
	vSetup->frame=frame;
	vSetup->bigtick.QuadPart=bigtick.QuadPart;
}

//
inline void AStarArray::keep_moving_forward_through_best_successor_node()
{
	WORD parent_node=current_node;
	for(register WORD n=0;n<directions;n++)
	{
		register WORD node=nodes[parent_node].successor[n];
		if(
			node!=EMPTY_NODE &&
			nodes[node].open &&
			nodes[node].f <= nodes[current_node].f
			)
			current_node=node;
	}
}


// optimize me!
inline void AStarArray::backtrack_to_find_best_node_to_explore_off_of()
{ //backtrack
	int node=last_node;
	int index=last_index;
	current_node=EMPTY_NODE;
	do
	{
		node=open_node_index[index];
		if(node==EMPTY_NODE) break;
		if(nodes[node].f<=nodes[current_node].f)
			current_node=node;
	} while(--index>0);
	if(current_node==EMPTY_NODE)
		no_path=true;
}

// most often called function -- must be as fast as possible
inline void AStarArray::open_successor_nodes()
{
	WORD parentyx=nodes[current_node].yx;
	
	for(int d=0;d<directions;d++)
	{
		WORD yx=parentyx+DXY[d].yx;
		
		if(
			(last_node>=MAX_NODES) || //if we've ran out of allocatd nodes ignore
			(world[yx].terrain_cost==IMPASSABLE_TERRAIN_COST) ||//if elevation is high mountains we can't travel there
			(world[yx].state!=UNKNOWN) //optimization
			)
			;
		else
		{
			//
			world[yx].state=OPEN; //optimization
			world[yx].route=d;
			
			//
			last_node++;
			
			//
			nodes[last_node].open=true;
			nodes[last_node].yx=yx;
			
			nodes[last_node].h=distance(yx);
			
			if(use_terrain)
				nodes[last_node].g=nodes[current_node].g + world[yx].terrain_cost*DXY[d].cost_multiplier;
			else
				nodes[last_node].g=nodes[current_node].g + DXY[d].cost_multiplier;
			
			nodes[last_node].ancestor=current_node;
			nodes[current_node].successor[d]=last_node;
			nodes[last_node].f=nodes[last_node].g+nodes[last_node].h;
			
			// for optimization c
			if(++last_index>MAX_OPEN_NODE_INDEXES)
				no_path=true;
			open_node_index[last_index]=last_node;
			nodes[last_node].open_node_index=last_index;
			
			//
			open_nodes++;
		}
	}
}


//
inline bool AStarArray::move_node_to_closed(WORD node)
{
		world[nodes[node].yx].state=CLOSED;
		nodes[node].open=false;
		
		// indexs
		WORD index=nodes[node].open_node_index;
		for(register i=index;i<last_index;i++)
		{
			nodes[open_node_index[i+1]].open_node_index--;
			open_node_index[i]=open_node_index[i+1];
		}
		if(--last_index<=0) no_path=true;

		//
		if(nodes[node].yx==endyx)
			path_found=true;
		else if(current_node==node) //closing the node we're on? dead end. pruning.
			backtrack_to_find_best_node_to_explore_off_of();
		
		//
		closed_nodes++;
		if(--open_nodes==0) no_path=true;
	
	return path_found;
}


//
inline float AStarArray::distance(WORD yx)
{
	int x=yx&XMASK;
	int y=(yx>>YSHIFT);
	int endx=endyx&XMASK;
	int endy=(endyx>>YSHIFT);
	
	switch(distance_method)
	{
	case PYTHAGORAS_DISTANCE: //aka STRAIGHT LINE, pythagoras
		{
			int xpart=x-endx;
			int ypart=y-endy;
			return (float)sqrt(xpart*xpart + ypart*ypart);
		}
		break;
		
	case MANHATTAN_DISTANCE:
		return (float)(abs(x-endx) + abs(y-endy));
		break;
		
	case DIAGONAL_DISTANCE:
		{
			WORD a=(WORD)abs(x-endx);
			WORD b=(WORD)abs(y-endy);
			return (a>b)?(float)a:(float)b;
		}
		break;
		
	case SIMPLE_PYTHAGORAS_DISTANCE:
		{
			int xpart=x-endx;
			int ypart=y-endy;
			return (float)(xpart*xpart + ypart*ypart);
		}
		break;
	default: return 0.0f;
	}
}









///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
// 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 AStarArray::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 AStarArray::UpdateWorld()
{