AstarPathFind PathFind;
int startx,starty,endx,endy;
short Judge(short x,short y,short endx,short endy)
{
short dx=abs(x-endx);
short dy=abs(y-endy);
return (dx+dy)<<3;
}
char Possible(short x,short y)
{
if (x<0||y<0||x>=mapw||y>=maph) return 0;
return (map[y][x]==' ')?1:0;
}
int readmap()
{
int i,j;
FILE *f;
if ((f=fopen("map.dat","r"))==NULL) return -1;
fscanf(f,"%d,%d\n",&mapw,&maph);
for (j=0;j<maph;j++) fgets(map[j],mapw+2,f);
fclose(f);
for (j=0;j<maph;j++) for (i=0;i<mapw;i++)
{
if (map[j][i]=='s'||map[j][i]=='S') startx=i,starty=j,map[j][i]=' ';
if (map[j][i]=='e'||map[j][i]=='E') endx=i,endy=j,map[j][i]=' ';
}
return 0;
}
#include <time.h>
#define TIMES 512
int main(void)
{
int i,j;
int x,y;
long t;
double f;
if (readmap()) { printf("no map file\n"); return -1; }
short PosXY[500];
char PosR[500];
for (j=0;j<maph;j++) {
for (i=0;i<mapw;i++) putchar(map[j][i]);
putchar('\n');
}
i=PathFind.Create(mapw,maph,Possible,Judge);
if (i) {
printf("创建失败\n");
return -1;
}
PathFind.SetCloseLimited(79*24L);
PathFind.SetDirMask(0xff);
PathFind.FindPath(startx,starty,endx,endy);
PathFind.GetPosPath(PosXY,250);
PathFind.GetDirPath(PosR,500);
getch();
for (i=0;PosXY[i*2]>=0;i++)
{
x=PosXY[i*2];
y=PosXY[i*2+1];
map[y][x]='.';
}
for (j=0;j<maph;j++) {
for (i=0;i<mapw;i++)
{
putchar(map[j][i]);
if (map[j][i]=='.') map[j][i]=' ';
}
putchar('\n');
}
printf("Any key to Testing speed, finding 512 paths...\n");
getch();
for (t=clock();t==clock(););
for (i=0,t=clock();i<TIMES;i++)
{
PathFind.FindPath(startx,starty,endx,endy);
PathFind.GetPosPath(PosXY,250);
}
t=clock()-t;
f=TIMES*CLK_TCK;
f/=t;
PathFind.Release();
printf("Time to find %d paths is %ld TCK use! freq=%f \n",TIMES,t,f);
getch();
return 0;
}
/*===============================================================================
原始算法 98.088421次每秒
算法记录一 152.760656次每妙
算法记录二 157.938983次每妙
算法记录三 179.200000次每秒
用VC优化 190.334572次每妙
地图文件:map.dat
79,24
ooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo
o o o
o ooooooo o o
o o o oooooooooooooo o
o s o o o
oooooooooooooooooo o o
o oooooooooooooooooooooooooo oooooooo o
o oooooo o oooo o o o
o o o ooo ooo o
o oooo oooo o
o o oooooooooooooooooooooooooooooooooooooooooooooo
o oooooooooooooooooooooooo o
o o
o o
ooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo o
o o ooooooooooo
o o ooooooo o o
o o o o o
o ooooooooooo oooooooooo o o
o o e ooo o o
o ooooo o o o
o o oooooooooooooooooo oooooooo o
o o o
ooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo
---------------------------------------------------------------------------------*/
// 路径寻找主函数
int AstarPathFind::FindPath(short startx,short starty,short endx,short endy)
{
TAstarNode *root;
int j,ok;
short x,y;
ADWORD max=0;
short MinJudge;
if (!Node||!Modify) return -1;
ClearModified();
root=&Node[starty][startx];
root->ActualCost=0;
root->EstimateCost=JudgeCost(startx,starty,endx,endy);
root->SumCost=root->EstimateCost;
root->Father=NULL;
root->Modified=1;
Modify[ModifyPoint++]=root;
AddToOpenQueue(root);
MinPos=tile_pos(startx,starty);
MinJudge=root->EstimateCost;
TargetX=endx;
TargetY=endy;
for (ok=0;ok==0;)
{
ok=GetFromOpenQueue(); // 取出Open队列估价值最小的节点放入Close中
root=Close; // 得到刚才取出的节点
if (ok||root==NULL) { ok=-1; break; }
x=(short)tile_x(root->Pos);
y=(short)tile_y(root->Pos);
if (root->EstimateCost<MinJudge) // 找到一个估计离终点最近的点
MinJudge=root->EstimateCost, MinPos=root->Pos;
if (CloseCount>max) max=CloseCount;
if (x==endx&&y==endy) MinPos=root->Pos,ok=1; // 如果走到终点了
else {
j=1;
if (DirMask&1) j&=TryTile(x,y-1,root,0);
if (DirMask&2) j&=TryTile(x+1,y-1,root,0);
if (DirMask&4) j&=TryTile(x+1,y,root,0);
if (DirMask&8) j&=TryTile(x+1,y+1,root,0);
if (DirMask&16) j&=TryTile(x,y+1,root,0);
if (DirMask&32) j&=TryTile(x-1,y+1,root,0);
if (DirMask&64) j&=TryTile(x-1,y,root,0);
if (DirMask&128) j&=TryTile(x-1,y-1,root,0);
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 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=(short)tile_x(p->Pos);
y=(short)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 char inc2r[10]={ 7,0,1,6,8,2,5,4,3,0 };
short i,x,y,nx,ny;
ADWORD j;
TAstarNode *p,*s;
x=(short)tile_x(MinPos);
y=(short)tile_y(MinPos);
if (maxpos>1) maxpos--;
for (p=&Node[y][x],s=p,j=0;p&&j<MaxSize;p=p->Father,j++)
{
nx=(short)tile_x(p->Pos);
ny=(short)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,short,short))
{
int i,j;
height=maph; width=mapw;
MaxSize=maph; MaxSize*=mapw;
Modify=new TAstarNode*[MaxSize*2];
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(JudgeCost=JudgeFun);
SetDirMask(0x55);
SetOpenLimited(200);
SetCloseLimited(MaxSize/20);
return 0;
}
int AstarPathFind::Release()
{
int 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,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=(short)DM; }