/*===============================================================
  通用最短路徑A*算法優(yōu)化求解
  Common shortest path finding in A*

  作者 林偉 于2001年12月19日
  Written by Wei Lin On Dec.19 2001

  電子科技大學(xué)通訊工程學(xué)院20013080班
  Uestc Dept.1 - 20013080
 */

#ifndef _ASTAR_H_
#define _ASTAR_H_

// 從上面#ifndef開始到#endif結(jié)束是頭文件內(nèi)容
typedef unsigned long ADWORD;
class AstarPathFind
{
protected:
	struct TAstarNode
	{
		ADWORD Pos;			// 該點(diǎn)的坐標(biāo)(16,16)的模式保存(y,x)
		short ActualCost;	// 保存從起點(diǎn)到該節(jié)點(diǎn)的實(shí)際開銷
		short EstimateCost;	// 保存此點(diǎn)的估價(jià)開銷
		TAstarNode *Father;	// 此點(diǎn)的父節(jié)點(diǎn)
		TAstarNode *Next;	// 在Open或者Next鏈表中的下一個(gè)節(jié)點(diǎn)
		char Modified;		// 該節(jié)點(diǎn)是否被修改過，記錄而備清除1空,2 Open,4 Close
	};

	TAstarNode **Node;		// 對應(yīng)地圖中每個(gè)節(jié)點(diǎn)
	TAstarNode *Open;		// 保存沒有處理的按估計(jì)值排序的節(jié)點(diǎn)
	TAstarNode *Close;		// 保存處理過的節(jié)點(diǎn)
	TAstarNode **Modify;	// 保存修改過的節(jié)點(diǎn)
	int height;				// 地圖的高度
	int width;				// 地圖的寬度
	ADWORD MaxSize;			// 最大面積即height*width
	ADWORD ModifyPoint;		// Modify數(shù)組的指針
	ADWORD OpenCount;		// Open隊(duì)列中的節(jié)點(diǎn)數(shù)目
	ADWORD CloseCount;		// Close隊(duì)列里面的節(jié)點(diǎn)數(shù)目
	ADWORD OpenLimited;		// Open隊(duì)列最多節(jié)點(diǎn)數(shù)目
	ADWORD CloseLimited;	// Close隊(duì)列最多節(jié)點(diǎn)數(shù)目
	short DirMask;			// 要搜索方向的標(biāo)志，0-7位為從上開始順時(shí)針七個(gè)方向
	ADWORD MinPos;			// 終點(diǎn)或最接近終點(diǎn)的節(jié)點(diǎn)
	char (*MoveAble)(short x,short y);	// 檢查地圖是否可以移動(dòng)函數(shù)指針
	short (*JudgeCost)(short x,short y);	// 取得估計(jì)值的函數(shù)指針
	char AddToOpenQueue(TAstarNode *node);	// 將節(jié)點(diǎn)排序加入Open隊(duì)列
	char GetFromOpenQueue();				// 從Open隊(duì)列取出最小的并放入Close隊(duì)列
	char PopCloseStack();					// 取出Close隊(duì)列中的節(jié)點(diǎn)
	void ClearModified();					// 清除所有搜索記錄
	char TryTile(short x,short y,TAstarNode *Father);
public:
	int Create(int map_w,int map_h,char (*MoveCheck)(short x,short y),short (*JudgeFun)(short x,short y));
	int Release();
	int FindPath(short startx,short starty,short endx,short endy);
	int GetPosPath(short *PosXY,int maxpos);
	int GetDirPath(char *Dirs,int maxpos);

	void SetJudgeFun(short (*JudgeFun)(short,short));
	void SetMapCheck(char (*MapCheck)(short,short));
	void SetOpenLimited(long OpenLimited);
	void SetCloseLimited(long CloseLimited);
	void SetDirMask(long DirMask);
};

#endif


#include <stdio.h>

#define COST_MAX 30000
#define tile_pos(x,y) (((ADWORD)y<<16)|x)
#define tile_x(pos)   (pos&0xffff)
#define tile_y(pos)   (pos>>16)

// 待處理節(jié)點(diǎn)入隊(duì)列, 依靠對目的地估價(jià)距離插入排序
char AstarPathFind::AddToOpenQueue(TAstarNode *node)
{
	ADWORD i;
	short f=node->EstimateCost;
	TAstarNode *p,*b;
	node->Modified|=2;	// 記錄Open標(biāo)志
	for (b=NULL,p=Open,i=0;p&&i<=OpenCount;b=p,p=p->Next,i++) 
		if (f<=p->EstimateCost) break;
	if (i>OpenCount) return -2;
	node->Next=p;
	if (b) b->Next=node;
	else Open=node;
	OpenCount++;
	return 0;
}
// 將離目的地估計(jì)最近的方案出隊(duì)列
char AstarPathFind::GetFromOpenQueue()
{
	TAstarNode *BestChoice=Open;
	if (!Open) return -1;
	Open=Open->Next;
	if (BestChoice->Modified&4) return -2; // 已經(jīng)在Close中了
	BestChoice->Next=Close;
	BestChoice->Modified&=5;	// 清除Open標(biāo)志
	BestChoice->Modified|=4;	// 設(shè)置Close標(biāo)志
	Close=BestChoice;
	OpenCount--;
	CloseCount++;
	return 0;
}
// 釋放棧頂節(jié)點(diǎn)
char AstarPathFind::PopCloseStack()
{
	if (Close) { 
		Close->Modified&=3;		// 清除Close標(biāo)志
		Close=Close->Next;
		CloseCount--; 
		return 0; 
	}
	return -1;
}
// 還原修改過的所有節(jié)點(diǎn)
void AstarPathFind::ClearModified()
{
	ADWORD i;
	for (i=0;i<ModifyPoint;i++) {
		Modify[i]->Modified=0;
		Modify[i]->ActualCost=COST_MAX;
	}
	ModifyPoint=0;
	OpenCount=0;
	CloseCount=0;
	Open=NULL;
	Close=NULL;
}
// 嘗試下一步移動(dòng)到 x,y 可行否
char AstarPathFind::TryTile(short x,short y,TAstarNode *Father)
{
	int ActualCost;
	if (!MoveAble(x,y)) return 1;			// 如果地圖無法通過則退出
	if (Node[y][x].Modified&6) return 1;	// 如果已經(jīng)存在在Open,Close隊(duì)列中則退出
	ActualCost=Father->ActualCost+1;		// 實(shí)際值計(jì)算
	if (ActualCost>=Node[y][x].ActualCost) return 1;
	if (!Node[y][x].Modified) Modify[ModifyPoint++]=&Node[y][x]; // 記錄這個(gè)修改過的點(diǎn)以還原
	Node[y][x].ActualCost=ActualCost;
	Node[y][x].Modified=1;
	Node[y][x].Father=Father;
	Node[y][x].EstimateCost=ActualCost+JudgeCost(x,y);
	AddToOpenQueue(&Node[y][x]);			// 將節(jié)點(diǎn)加入Open隊(duì)列
	return 0;
}
// 路徑尋找主函數(shù)
int AstarPathFind::FindPath(short startx,short starty,short endx,short endy)
{
	TAstarNode *root;
	int i,j,x,y,ok,dirs,max=0;
	short MinJudge;
	static short incx[8]={ 0, 1, 1, 1, 0, -1, -1, -1 };
	static short incy[8]={ -1, -1, 0, 1, 1, 1, 0, -1 };

	if (!Node||!Modify) return -1;
	ClearModified();
	root=&Node[starty][startx];
	root->ActualCost=0;
	root->EstimateCost=JudgeCost(startx,starty);
	root->Father=NULL; 
	root->Modified=1;
	Modify[ModifyPoint++]=root;
	AddToOpenQueue(root);
	MinPos=tile_pos(startx,starty);
	MinJudge=JudgeCost(startx,starty);
	for (ok=0;ok==0;)
	{
		ok=GetFromOpenQueue();		// 取出Open隊(duì)列估價(jià)值最小的節(jié)點(diǎn)放入Close中
		root=Close;					// 得到剛才取出的節(jié)點(diǎn)
		if (ok||root==NULL) { ok=-1; break; }
		x=tile_x(root->Pos);
		y=tile_y(root->Pos);
		if (root->EstimateCost-root->ActualCost<MinJudge)	// 找到一個(gè)估計(jì)離終點(diǎn)最近的點(diǎn)
			MinJudge=root->EstimateCost-root->ActualCost, MinPos=root->Pos;
		if (CloseCount>max) max=CloseCount;
		if (x==endx&&y==endy) MinPos=root->Pos,ok=1;	// 如果走到終點(diǎn)了
		else {
			for (i=0,dirs=1,j=1;i<8;i++,dirs<<=1)		// 如果沒有走到終點(diǎn)
				if (DirMask&dirs) j&=TryTile(x+incx[i],y+incy[i],root);
			if (j) if (PopCloseStack()) { ok=-2; break; }	// 如果不是通路則從Close中取出
		}
		if (OpenCount>=OpenLimited||CloseCount>=CloseLimited) ok=2;
	}
	if (ok<0) return -2;
	return 0;
}
int AstarPathFind::GetPosPath(short *PosXY,int maxpos)
{
	short x,y;
	ADWORD Pos,j;
	TAstarNode *p,*s;
	int i;
	if (maxpos>1) maxpos--;
	for (p=&Node[tile_y(MinPos)][tile_x(MinPos)],s=p,j=0;p&&j<MaxSize;p=p->Father,j++)
	{
		x=tile_x(p->Pos);
		y=tile_y(p->Pos);
		i=(p->ActualCost<maxpos)?p->ActualCost:maxpos;
		PosXY[i<<1]=x;
		PosXY[(i<<1)+1]=y;
	}
	i=(s->ActualCost+1<maxpos)?(s->ActualCost+1):maxpos;
	PosXY[i*2]=-1;
	PosXY[i*2+1]=-1;
	return 0;
}
int AstarPathFind::GetDirPath(char *PosDir,int maxpos)
{
	static short incx[8]={ 0, 1, 1, 1, 0, -1, -1, -1 };
	static short incy[8]={ -1, -1, 0, 1, 1, 1, 0, -1 };
	static char inc2r[10]={ 0,0,0,0 };
	short i,x,y,nx,ny;
	ADWORD j;
	TAstarNode *p,*s;
	if (!inc2r[0])
	{
		for (i=0;i<8;i++) inc2r[(incy[i]+1)*3+incx[i]+1]=i;
		inc2r[4]=8;
	}
	x=tile_x(MinPos);
	y=tile_y(MinPos);
	if (maxpos>1) maxpos--;
	for (p=&Node[y][x],s=p,j=0;p&&j<MaxSize;p=p->Father,j++)
	{
		nx=tile_x(p->Pos);
		ny=tile_y(p->Pos);
		i=(p->ActualCost<maxpos)?(p->ActualCost):maxpos;
		PosDir[i]=inc2r[(y-ny+1)*3+x-nx+1];
		x=nx;
		y=ny;
	}
	i=(s->ActualCost+1<maxpos)?(s->ActualCost+1):maxpos;
	PosDir[i]=8;
	return 0;
}
int AstarPathFind::Create(int mapw,int maph,char (*MoveCheck)(short,short),short (*JudgeFun)(short x,short y))
{
	int i,j;

	height=maph;	width=mapw;
	MaxSize=maph;	MaxSize*=mapw;
	Modify=new TAstarNode*[MaxSize];
	if (!Modify) return -1;
	Node=new TAstarNode*[maph];
	if (!Node) { delete Modify; Modify=NULL; return -1; }
	for (i=0;i<maph;i++) Node[i]=new TAstarNode[mapw];
	for (i=0,j=1;i<maph;i++) if (!Node[i]) j=0;
	if (!j)
	{
		for (i=0;i<maph;i++) if (Node[i]) delete Node[i];
		delete Node;
		delete Modify;
		Node=NULL;
		Modify=NULL;
		return -2;
	}
	for (j=0;j<maph;j++) 
		for (i=0;i<mapw;i++) 
		{
			Node[j][i].Pos=tile_pos(i,j);
			Node[j][i].Modified=0;
			Node[j][i].ActualCost=COST_MAX;
		}
	ModifyPoint=0;
	SetMapCheck(MoveCheck);
	SetJudgeFun(JudgeFun);
	SetDirMask(0x55);
	SetOpenLimited(200);
	SetCloseLimited(MaxSize/20);
	return 0;
}
int AstarPathFind::Release()
{
	int i,j;
	if (Node) for (j=0;j<height;j++) if (Node[j]) delete Node[j];
	if (Node) delete Node;
	if (Modify) delete Modify;
	Node=NULL;
	Modify=NULL;
	return 0;
}
void AstarPathFind::SetJudgeFun(short (*JudgeFun)(short,short)) { JudgeCost=JudgeFun; }
void AstarPathFind::SetMapCheck(char (*MapCheck)(short,short)) { MoveAble=MapCheck; }
void AstarPathFind::SetOpenLimited(long OL) { OpenLimited=OL; }
void AstarPathFind::SetCloseLimited(long CL) { CloseLimited=CL; }
void AstarPathFind::SetDirMask(long DM) { DirMask=DM; }