astarlinkedlist.cpp

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

// AStarLinkedList.cpp: implementation of the AStarLinkedList class.
//
//////////////////////////////////////////////////////////////////////
/*
AStar Version 2

  Sorted Linked Lists
*/

#include "stdafx.h"
#include "AStarLinkedList.h"

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


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

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

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

AStarLinkedList::~AStarLinkedList()
{
	
}

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

	this->airoute=airoute;
	airoute->count=0;
	
	// opt me
	for(register int n=0;n<WIDTH*HEIGHT;n++)
	{
		world[n].state=(world[n].terrain_cost==IMPASSABLE_TERRAIN_COST);
	}
	
	//
	frame=0;
	
	path_found=false;
	no_path=false;
	
	//
	open_nodes=1;
	closed_nodes=0;
	
	//
	best_node=1;
	nodesdata[best_node].yx=startyx;
	nodesdata[best_node].g=0.0f;
	nodes[best_node].f=nodesdata[best_node].g+distance(startyx)*DXY[4].cost_multiplier;
	nodes[best_node].prev=best_node;
	nodes[best_node].next=best_node;
	nodesdata[best_node].ancestor=EMPTY_NODE;
	world[startyx].route=NO_ROUTE;
	
	//closed node seed
	closed_node=0;
	nodes[closed_node].prev=EMPTY_NODE;
	nodes[closed_node].next=EMPTY_NODE;
	nodesdata[closed_node].ancestor=EMPTY_NODE;

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


//
void AStarLinkedList::FindPath()
{
	if(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);
	
	//
	if(use_terrain)
		with_terrain(iterations_per_frame);
	else
		without_terrain(iterations_per_frame);
	
	//
	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;
}

//
void AStarLinkedList::with_terrain(int iterations_per_frame)
{
	int count=iterations_per_frame;
	do
	{
		WORD parent_node=best_node;
		add_nodes_to_open_terrain(parent_node); // open successor nodes
		move_node_to_closed(parent_node);// close spawning parent node
	} while(--count>0 && !path_found && !no_path);
	
	//
	frame+=(iterations_per_frame-count);
}

//
void AStarLinkedList::without_terrain(int iterations_per_frame)
{
	int count=iterations_per_frame;
	do
	{
		WORD parent_node=best_node;
		add_nodes_to_open(parent_node); // open successor nodes
		move_node_to_closed(parent_node);// close spawning parent node
	} while(--count>0 && !path_found && !no_path);
	
	//
	frame+=(iterations_per_frame-count);
}

// add open nodes that ignore terrain except for impassables
inline void AStarLinkedList::add_nodes_to_open(WORD parent_node)
{
	for(int d=0;d<directions;d++)
	{
		//
		WORD yx=nodesdata[parent_node].yx+DXY[d].yx;
		
		if(world[yx].state==UNKNOWN)
		{ //the following has been organized in a way that may not be as readable,
			//but suggests to the compiler better optimizations (by a few ms).
			free_node++;
			
			world[yx].state=OPEN;
			world[yx].route=d;
			
			nodesdata[free_node].yx=yx;
			
			nodesdata[free_node].g=nodesdata[parent_node].g + DXY[d].cost_multiplier;
			nodes[free_node].f=nodesdata[free_node].g+distance(yx);
			
			register WORD sorted_node=find_sorted_node_position(nodes[free_node].f);
			
			if(nodes[free_node].f<nodes[best_node].f)
				best_node=free_node;
			
			WORD next=nodes[sorted_node].next;
			nodes[sorted_node].next=free_node;
			nodes[free_node].prev=sorted_node;
			nodes[free_node].next=next;
			nodes[next].prev=free_node;
			
			nodesdata[parent_node].successor[d]=free_node;
			nodesdata[free_node].ancestor=parent_node;
			for(register int i=0;i<directions;i++) nodesdata[free_node].successor[i]=EMPTY_NODE;
			
			open_nodes++;
		}
	}
}


// add open nodes w/ a g that uses the terrain/elevation weight
inline void AStarLinkedList::add_nodes_to_open_terrain(WORD parent_node)
{
	for(int d=0;d<directions;d++)
	{
		//
		WORD yx=nodesdata[parent_node].yx+DXY[d].yx;
		
		if(world[yx].state==UNKNOWN)
		{ //the following has been organized in a way that may not be as readable,
			//but suggests to the compiler better optimizations (by a few ms).
			free_node++;
			
			world[yx].state=OPEN;
			world[yx].route=d;
			
			nodesdata[free_node].yx=yx;

			nodesdata[free_node].g=nodesdata[parent_node].g + (float)world[yx].terrain_cost;
			nodes[free_node].f=nodesdata[free_node].g+distance(yx)*DXY[d].cost_multiplier;

			register WORD sorted_node=find_sorted_node_position(nodes[free_node].f);
			
			if(nodes[free_node].f<nodes[best_node].f)
				best_node=free_node;
			
			WORD next=nodes[sorted_node].next;
			nodes[sorted_node].next=free_node;
			nodes[free_node].prev=sorted_node;
			nodes[free_node].next=next;
			nodes[next].prev=free_node;
			
			nodesdata[parent_node].successor[d]=free_node;
			nodesdata[free_node].ancestor=parent_node;
			for(register int i=0;i<directions;i++) nodesdata[free_node].successor[i]=EMPTY_NODE;
			
			open_nodes++;
		}
	}

	
}

// we look for a new f we're going to place to the right/next of the node we select.
// note: this is the slow bottleneck for this algorithm
inline WORD AStarLinkedList::find_sorted_node_position(float f)
{
	WORD sorted_node=best_node;
	do
	{
		if(nodes[sorted_node].f>f) break;
		sorted_node=nodes[sorted_node].prev;
	} while(sorted_node!=best_node);
	return sorted_node;
}

//
inline float AStarLinkedList::distance(WORD yx)
{
	switch(distance_method)
	{
	case MANHATTAN_DISTANCE:
		return (float)(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 (float)sqrt(xpart*xpart + ypart*ypart);
		}
		break;
		
	case DIAGONAL_DISTANCE:
		{
			WORD a=(WORD)abs((yx&XMASK)-endx);
			WORD b=(WORD)abs((yx>>YSHIFT)-endy);
			return (a>b)?(float)a:(float)b;
		}
		break;
		
	case SIMPLE_PYTHAGORAS_DISTANCE:
		{
			int xpart=(yx&XMASK)-endx;
			int ypart=(yx>>YSHIFT)-endy;
			return (float)(xpart*xpart + ypart*ypart);
		}
		break;
	default: return 0.0f;
	}
}


//
void AStarLinkedList::move_node_to_closed(WORD parent_node)
{
	//
	if(best_node==parent_node)
		best_node=nodes[best_node].prev;
	
	//
	if(nodesdata[parent_node].yx==endyx)
		path_found=true;
	
	world[ nodesdata[parent_node].yx ].state=CLOSED;
	
	// cache prev/next link and f
	WORD prev=nodes[parent_node].prev;
	WORD next=nodes[parent_node].next;
	
	//remove node from open list
	nodes[prev].next=next;
	nodes[next].prev=prev;
	
	//insert node into closed linked-list
	nodes[closed_node].next=parent_node;
	nodes[parent_node].prev=closed_node;
	nodes[parent_node].next=EMPTY_NODE;
	closed_node=parent_node;
	
	//
	closed_nodes++;
	if(--open_nodes==0) no_path=true;
}






///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////
// 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 AStarLinkedList::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 AStarLinkedList::UpdateWorld()
{
	LARGE_INTEGER tmp1;
	QueryPerformanceCounter(&tmp1);
	
	//
	for(register int y=0;y<HEIGHT;y++)
	{
		for(register 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 AStarLinkedList::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++)
			{