spath.cpp

原创

/*A-Start algrithom search path [e-mail]*/
//1252

#include "stdio.h"
#include "stdlib.h"
#include "malloc.h"
#include "string.h"
#include "vld.h"

#define MAXD 100
#define TILE(x,y)  ((x)+((y)<<8)) 
#define TILEx(x)   ((x)&0xff)
#define TILEy(x)   ((x>>8)&0xff)
#define ABS(x)     (((x)<0) ? (-(x)) : (x))

int StartPoint,EndPoint;

int TileMap[MAXD][MAXD];
struct NODE {
	int  tilenum;
	NODE *next;
	NODE *father;
	int f;
	int g;
	int h;
};

NODE *open,*close;
int  w,h;

void readmap()
{
	int i,j;
	FILE *fmap;
	char c;

	fmap = fopen("map.dat","r");
	fscanf(fmap,"%d,%d\n",&w,&h);

	for(i=0;i<h;i++){
		for(j=0;j<w;j++){
			fscanf(fmap,"%c",&c);
			if (c=='o') 
				TileMap[i][j] = 1;
			else
				TileMap[i][j] = 0;
			
			if (c=='s')
				StartPoint = TILE(j,i);
			if (c=='e') 
				EndPoint = TILE(j,i);
		}
		fscanf(fmap,"\n");
	}
	fclose(fmap);
}

void writemap()
{
	int i,j;
	FILE *fmap;

	fmap = fopen("map_path.dat","w");
	fscanf(fmap,"%d,%d\n",&w,&h);

	for(i=0;i<h;i++){
		for(j=0;j<w;j++){
			
			if (TileMap[i][j] == 2) {
				fprintf(fmap,"+");
			}
			else if (TileMap[i][j] == 1){
				fprintf(fmap,"o");
			}
			else
				fprintf(fmap," ");
		}
		fprintf(fmap,"\n");
	}
	fclose(fmap);
}

int CalcH(int tilenum)
{
	return( ABS((tilenum >> 8) - (EndPoint>>8))+
        	ABS((tilenum & 0xff) - (EndPoint & 0xff)));
}

void AddSuccessor(NODE *BestNode, NODE *close, int tilenum)
{
	NODE *tmp,*tmp1;
	int  f,g,h;

	if (TileMap[TILEy(tilenum)][TILEx(tilenum)]) {
		return;
	}

	g = BestNode->g + 1;
	h = CalcH(tilenum);
	f = g+h;
	/*check if it in open list*/
	tmp = BestNode;
	while (tmp->next) {
		tmp = tmp->next;
		if (tmp->tilenum == tilenum) {
			if (f < tmp->f) {
				tmp->f = f;
				tmp->father = BestNode;
			}
			return;
		}
	}
	/*check if it in close list*/
	tmp = close;
	while (tmp->next) {
		tmp = tmp->next;
		if (tmp->tilenum == tilenum) {
			if (f < tmp->f) {//need to del it from close list and insert it to open list
				tmp->f = f;  //but its seem to not goes here!
				tmp->father = BestNode;
			}
			return;
		}
	}

	//insert it to openlist and sort it
	NODE *node;
	node  = (NODE *)malloc(sizeof(NODE));
	node->father = BestNode;
	node->tilenum = tilenum;
	node->next = NULL;
	node->g = g;
	node->h = h;
	node->f = f;

	tmp = BestNode;
	while (tmp->next && tmp->next->f < f) {
		tmp = tmp->next;
	}
	tmp1 = tmp->next;
	tmp->next = node;
	node->next = tmp1;

	return;
}

void main()
{
	NODE *node,*tmp;
	NODE *BestNode;
	int  tilenum;

	readmap();

	open  = (NODE *)malloc(sizeof(NODE));
	close = (NODE *)malloc(sizeof(NODE));
	node  = (NODE *)malloc(sizeof(NODE));

	memset(open,0,sizeof(NODE));
	memset(close,0,sizeof(NODE));

	node->tilenum = StartPoint;
	node->g = 0;
	node->h = CalcH(node->tilenum);
	node->f = node->g + node->h;
	node->next = NULL;
	node->father = NULL;

	open->next = node;

	while (1) {
		if (open->next == NULL) {
			printf("No Path found!\n");
			return;
		}

		BestNode = open->next;
		if (BestNode->tilenum == EndPoint) {
			break;
		}
		else{
			if (TILEy(BestNode->tilenum) > 0)//up 
				AddSuccessor(BestNode,close,BestNode->tilenum - 256);
			if (TILEy(BestNode->tilenum) < h-1)//down 
				AddSuccessor(BestNode,close,BestNode->tilenum + 256);
			if (TILEx(BestNode->tilenum) > 0)//left 
				AddSuccessor(BestNode,close,BestNode->tilenum - 1);
			if (TILEx(BestNode->tilenum) < w-1)//right 
				AddSuccessor(BestNode,close,BestNode->tilenum + 1);

			open->next = open->next->next;
			tmp = close;
			while (tmp->next) {
				tmp = tmp->next;
			}
			BestNode->next = NULL;
			tmp->next = BestNode; //insert bestnode to close list
#if 0
{
		tmp = close;
		printf("** closelist: ");
		while (tmp->next) {
			tmp = tmp->next;
			printf("x:%d y:%d f:%d,  ", TILEx(tmp->tilenum), TILEy(tmp->tilenum),tmp->f);
		}
		printf("\n");

		tmp = open;
		printf("** openlist: ");
		while (tmp->next) {
			tmp = tmp->next;
			printf("x:%d y:%d,f:%d,  ", TILEx(tmp->tilenum), TILEy(tmp->tilenum),tmp->f);
		}
		printf("\n");
}
#endif
		}
	}

	printf("**** find path success! ****\n");

	tmp = BestNode;
	while (tmp->father) {
		tilenum = tmp->tilenum;
		TileMap[TILEy(tilenum)][TILEx(tilenum)] = 2;
		//printf("%3d,%3d\n",TILEx(tilenum), TILEy(tilenum));
		tmp = tmp->father;
	}

	writemap();

	//free memory here
	tilenum = 0;
	tmp = node = open;
	while (tmp) {
		node = tmp->next;
		free(tmp);
		tmp = node;
		tilenum++;
	}

	tmp = node = close;
	while (tmp) {
		node = tmp->next;
		free(tmp);
		tmp = node;
		tilenum++;
	}

	printf("memory usage : %d\n", tilenum);

	return;
}