astarcomplete.cpp

一个非常完美复杂的、效率极高寻路程序

// AStarComplete.cpp: implementation of the AStarComplete class.
//
//////////////////////////////////////////////////////////////////////
/*
AStar Version 2c w/complete path revisiting

Sorted Linked Lists
*/

#include "stdafx.h"
#include "AStarComplete.h"

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


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

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

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

AStarComplete::~AStarComplete()
{
	
}

void AStarComplete::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); //?IMPASSABLE:UNKNOWN;
		world[n].node=EMPTY_NODE;
	}
	
	//
	frame=0;
	
	path_found=false;
	no_path=false;
	
	//
	open_nodes=1;
	closed_nodes=0;
	reopened_nodes=0;
	
	//
	best_node=1;
	nodesdata[best_node].yx=startyx;
	nodesdata[best_node].g=0.0f;
	if(use_terrain)
		nodes[best_node].f=nodesdata[best_node].g+distance(startyx)*median_terrain_cost;
	else
		nodes[best_node].f=nodesdata[best_node].g+distance(startyx);
	nodes[best_node].prev=best_node;
	nodes[best_node].next=best_node;
	nodesdata[best_node].ancestor=EMPTY_NODE;

	world[startyx].route=NO_ROUTE;
	world[startyx].node=best_node;
	
	//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 AStarComplete::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 AStarComplete::without_terrain(int iterations_per_frame)
{
	int count=iterations_per_frame;
	do
	{
		WORD parent_node=best_node;
		expand_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);
}

void AStarComplete::with_terrain(int iterations_per_frame)
{
	int count=iterations_per_frame;
	do
	{
		WORD parent_node=best_node;
		expand_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);
}

// most often called function -- must be as fast as possible
inline void AStarComplete::expand_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)
		{
			//
			world[yx].state=OPEN;
			world[yx].route=d;
			
			free_node++;
			world[yx].node=free_node;
			
			nodesdata[free_node].yx=yx;
			
			nodesdata[free_node].g=nodesdata[parent_node].g + cost*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++;
		}
		
		else if(world[yx].state==CLOSED)
		{
			float g=nodesdata[parent_node].g + cost*DXY[d].cost_multiplier;
			float f=g+distance(yx);
			if(f<nodes[world[yx].node].f)
			{
				reopen_closed_node(f,g,world[yx].node,yx,d,parent_node);
			}
		}
		
	}
}


// most often called function -- must be as fast as possible
inline void AStarComplete::expand_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)
		{
			//
			world[yx].state=OPEN;
			world[yx].route=d;
			
			free_node++;
			world[yx].node=free_node;
			
			nodesdata[free_node].yx=yx;
			
			nodesdata[free_node].g=nodesdata[parent_node].g + (float)world[yx].terrain_cost*DXY[d].terrain_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++;
		}
		
		else if(world[yx].state==CLOSED)
		{
			float g=nodesdata[parent_node].g + (float)world[yx].terrain_cost*DXY[d].terrain_cost_multiplier;
			float f=g+distance(yx);
			if(f<nodes[world[yx].node].f)
			{
				reopen_closed_node(f,g,world[yx].node,yx,d,parent_node);
			}
		}
		
	}
}

//
inline void AStarComplete::reopen_closed_node(
											  float f,
											  float g,
											  WORD reopened_node,
											  WORD yx,
											  BYTE d,
											  WORD parent_node
											  )
{
	//
	world[yx].state=OPEN;
	world[yx].route=d;
	nodesdata[reopened_node].g=g;
	nodes[reopened_node].f=f;
	nodesdata[reopened_node].ancestor=parent_node;
	nodesdata[parent_node].successor[d]=reopened_node;
				
	//
	if(f<nodes[best_node].f)
		best_node=reopened_node;
				
	//unlink from closed
	WORD prev=nodes[reopened_node].prev;
	WORD next=nodes[reopened_node].next;
	if(prev==EMPTY_NODE)
		nodes[next].prev=EMPTY_NODE;
	else if(next==EMPTY_NODE)
		nodes[prev].next=EMPTY_NODE;
	else
	{
		nodes[prev].next=next;
		nodes[next].prev=prev;
	}
				
	// relink to open
	register WORD sorted_node=find_sorted_node_position(f);
	next=nodes[sorted_node].next;
	nodes[sorted_node].next=reopened_node;
	nodes[reopened_node].prev=sorted_node;
	nodes[reopened_node].next=next;
	nodes[next].prev=reopened_node;
				
	reopened_nodes++;

	//
	open_nodes++;
	closed_nodes--;
}


// we look for a new f we're going to place to the right/next of the node we select.
inline WORD AStarComplete::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 AStarComplete::distance(WORD yx) //BYTE y,BYTE x)
{
	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;
	}
}


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