astar.cpp

A*算法

#include <memory.h>
#include <stdio.h>
/****************************/
#include <conio.h>
#include <dos.h>
//#include <alloc.h>
#include <stdlib.h>
#include <math.h>
/***************************/
#include "Terrain.h"

Terrain::Terrain()
{

/*******设置禁飞区，这里只能只设置一个，需扩充结构************/

    abort_round.x_point=35;
	abort_round.z_point=50;
	abort_round.round_r=10;

    abort_round2.x_point=90;
	abort_round2.z_point=105;
	abort_round2.round_r=10;	

}

Terrain::~Terrain()
{
	if (Indices) delete Indices;
//	if (Vertices) delete [] Vertices;
//////////////////////////////////////////////
	if (Stack) delete Stack;
	if (OPEN) delete OPEN;
	if (CLOSED) delete CLOSED;

}
/**************************************************************************/
/***************************** A* Algorithm *******************************/
/**************************************************************************/
//找到路径，返回指针
struct NODE *Terrain::FindPath(long sx,float sy,long sz,long dx,float dy,long dz)
{
    struct NODE *Node, *BestNode;
    
	int TileNumDest;
    TileNumDest=(int)dx*Size+(int)dz;

	OPEN=new struct NODE;
	memset(OPEN,0,sizeof( struct NODE ));
	CLOSED=new struct NODE;
	memset(CLOSED,0,sizeof( struct NODE ));
	
	Node=new struct NODE;
    memset(Node,0,sizeof(struct NODE ));
    Node->g=1000000;
	Node->rhs=0;
    Node->h=sqrt((sx-dx)*(sx-dx)+(sz-dz)*(sz-dz)+canshu*(sy-dy)*(sy-dy));  // 开平方更好些
    
	if(Node->h<100)
		w=w2;
	else
		w=w1;

	if (Node->rhs<Node->g)
		Node->f=Node->rhs+w*Node->h;        //change this two lines for cost function
	else
		Node->f=Node->g+w*Node->h;
    Node->NodeNum=(int)sx*Size+(int)sz;
    Node->x=sx;
    Node->z=sz;
	Node->y=sy;

    OPEN->NextNode=Node;        /* make Open List point to first node */
    float time;
	time=GetTickCount();
	FILE *stream;		
	for (;;)
    {
		BestNode=(struct NODE *)ReturnBestNode();
		if (BestNode->NodeNum == TileNumDest)    /* if we've found the end, break and finish */
			{ 
				if( (stream = fopen( "time.txt", "w" )) != NULL )
				{
				 time = (GetTickCount()-time);		
				 fprintf(stream,"%f\n",time);
				 fclose(stream);
				}
				break;
			}
   		if (BestNode->g>BestNode->rhs)
		{
			BestNode->g=BestNode->rhs;
			GenerateSuccessors(BestNode,dx,dy,dz);
		}
		else if(BestNode->g<=BestNode->rhs)
		{
			BestNode->g=1000000;
			GenerateSuccessors(BestNode,dx,dy,dz);				
		}
	 }
  return (BestNode);
}

struct NODE *Terrain::ReturnBestNode()
{
    struct NODE *tmp;

    if (OPEN->NextNode == NULL)
	{
//	    printf("ERROR: no more nodes on OPEN\n");
//	    closegraph();
	    exit(0);
	}

/* Pick Node with lowest f, in this case it's the first node in list
   because we sort the OPEN list wrt lowest f. Call it BESTNODE. */

    tmp=OPEN->NextNode;   // point to first node on OPEN
    OPEN->NextNode=tmp->NextNode;    // Make OPEN point to nextnode or NULL.

/* Next take BESTNODE (or temp in this case) and put it on CLOSED */

    tmp->NextNode=CLOSED->NextNode;
    CLOSED->NextNode=tmp;

    return(tmp);
}

void Terrain::GenerateSuccessors(struct NODE *BestNode,long dx,float dy,long dz)
{
  long x,z;
  float y;

		    /* Upper-Left  */
   if (FreeTile(x=(long)BestNode->x,y=(float)BestNode->y,z=(long)BestNode->z+1))
      GenerateSucc(BestNode,x,y,z,dx,dy,dz);  //将x、z的值修改，用新值计算和终点的f值，来决定走的方向
		    /* Upper       */
    if (FreeTile(x=(long)BestNode->x+1,y=(float)BestNode->y,z=(long)BestNode->z+1))
      GenerateSucc(BestNode,x,y,z,dx,dy,dz);
		    /* Upper-Right */
    if (FreeTile(x=(long)BestNode->x-1,y=(float)BestNode->y,z=(long)BestNode->z+1))
      GenerateSucc(BestNode,x,y,z,dx,dy,dz);
		    /* Right       */
    if (FreeTile(x=(long)BestNode->x,y=(float)BestNode->y+1,z=(long)BestNode->z+1))
      GenerateSucc(BestNode,x,y,z,dx,dy,dz);
		    /* Lower-Right */
    if (FreeTile(x=(long)BestNode->x,y=(float)BestNode->y-1,z=(long)BestNode->z+1))
      GenerateSucc(BestNode,x,y,z,dx,dy,dz);
		    /* Lower       */
    if (FreeTile(x=(long)BestNode->x+1,y=(float)BestNode->y+1,z=(long)BestNode->z+1))
      GenerateSucc(BestNode,x,y,z,dx,dy,dz);

    if (FreeTile(x=(long)BestNode->x-1,y=(float)BestNode->y+1,z=(long)BestNode->z+1))
      GenerateSucc(BestNode,x,y,z,dx,dy,dz);		    
			/* Lower-Left  */
   if (FreeTile(x=(long)BestNode->x+1,y=(float)BestNode->y-1,z=(long)BestNode->z+1))
     GenerateSucc(BestNode,x,y,z,dx,dy,dz);
		    /* Left        */
   if (FreeTile(x=(long)BestNode->x-1,y=(float)BestNode->y-1,z=(long)BestNode->z+1))
    GenerateSucc(BestNode,x,y,z,dx,dy,dz);


	if (FreeTile(x=(long)BestNode->x,y=(float)BestNode->y,z=(long)BestNode->z))
    GenerateSucc(BestNode,x,y,z,dx,dy,dz);  //将x、z的值修改，用新值计算和终点的f值，来决定走的方向
		    /* Upper       */
    if (FreeTile(x=(long)BestNode->x+1,y=(float)BestNode->y,z=(long)BestNode->z))
      GenerateSucc(BestNode,x,y,z,dx,dy,dz);
		    /* Upper-Right */
    if (FreeTile(x=(long)BestNode->x-1,y=(float)BestNode->y,z=(long)BestNode->z))
      GenerateSucc(BestNode,x,y,z,dx,dy,dz);
		    /* Right       */
    if (FreeTile(x=(long)BestNode->x,y=(float)BestNode->y+1,z=(long)BestNode->z))
      GenerateSucc(BestNode,x,y,z,dx,dy,dz);
		    /* Lower-Right */
    if (FreeTile(x=(long)BestNode->x,y=(float)BestNode->y-1,z=(long)BestNode->z))
      GenerateSucc(BestNode,x,y,z,dx,dy,dz);
		    /* Lower       */
    if (FreeTile(x=(long)BestNode->x+1,y=(float)BestNode->y+1,z=(long)BestNode->z))
      GenerateSucc(BestNode,x,y,z,dx,dy,dz);

   if (FreeTile(x=(long)BestNode->x-1,y=(float)BestNode->y+1,z=(long)BestNode->z))
     GenerateSucc(BestNode,x,y,z,dx,dy,dz);		    
			/* Lower-Left  */
    if (FreeTile(x=(long)BestNode->x+1,y=(float)BestNode->y-1,z=(long)BestNode->z))
     GenerateSucc(BestNode,x,y,z,dx,dy,dz);
		    /* Left        */
    if (FreeTile(x=(long)BestNode->x-1,y=(float)BestNode->y-1,z=(long)BestNode->z))
     GenerateSucc(BestNode,x,y,z,dx,dy,dz);
}
boolean Terrain::FreeTile(long x, float y,long z)
{
	if( x < 0 || z < 0 || x > 127 || z > 127 )
	{	return (FALSE); }
//	else if( x > abort_zone.x_begin && x < abort_zone.x_end && z > abort_zone.y_begin && z < abort_zone.y_end )
//	{/*设置禁飞区  在(20,20)--(40,40)之间禁飞*/
//		return (FALSE);
//	}
	else if(((x-abort_round.x_point)*(x-abort_round.x_point)+(z-abort_round.z_point)*(z-abort_round.z_point))<abort_round.round_r*abort_round.round_r)
		return (FALSE);
   else if(((x-abort_round2.x_point)*(x-abort_round2.x_point)+(z-abort_round2.z_point)*(z-abort_round2.z_point))<abort_round2.round_r*abort_round2.round_r)
		return (FALSE);
	else
	{	return (TRUE);  }

}
void Terrain::GenerateSucc(struct NODE *BestNode,long x, float y,long z, long dx, float dy ,long dz)
{
    int TileNumS,c=0;
	double g,rhs;
    struct NODE *Old,*Successor;

    int temx,temz;
	float temy;
	temx=BestNode->x;
	temz=BestNode->z;
	temy=Vertices[temx*Size+temz].posy;
	y=Vertices[x*Size+z].posy;
	dy=Vertices[dx*Size+dz].posy;
    
	rhs=BestNode->g+sqrt((x-temx)*(x-temx)+(z-temz)*(z-temz)+canshu*(y-temy)*(y-temy));
//	g=BestNode->g+sqrt((x-temx)*(x-temx)+(z-temz)*(z-temz)+canshu*(y-temy)*(y-temy));	    /* g(Successor)=g(BestNode)+cost of getting from BestNode to Successor */
    TileNumS=(int)x*Size+(int)z;  /* identification purposes */
 
	if ((Old=CheckOPEN(TileNumS)) != NULL) /* if equal to NULL then not in OPEN list, else it returns the Node in Old */
	{
		for(c=0;c<18;c++)
	        if(BestNode->Child[c] == NULL) /* Add Old to the list of BestNode's Children (or Successors). */
	           break;
          	BestNode->Child[c]=Old;

	if (rhs < Old->rhs)  /* if our new g value is < Old's then reset Old's parent to point to BestNode */
	{
	    Old->Parent=BestNode;
	    Old->rhs=rhs;
		Old->g=rhs;
		if(Old->h<100)
			w=w2;
		else 
			w=w1;
	    Old->f=rhs+w*Old->h;
		 PropagateDown(Old); 
	}
   }
   else if ((Old=CheckCLOSED(TileNumS)) != NULL) /* if equal to NULL then not in OPEN list, else it returns the Node in Old */
   {
     for(c=0;c<18;c++)
	if (BestNode->Child[c] == NULL) /* Add Old to the list of BestNode's Children (or Successors). */
	  break;
	BestNode->Child[c]=Old;
	if (rhs < Old->rhs)  /* if our new g value is < Old's then reset Old's parent to point to BestNode */
	{
	    Old->Parent=BestNode;
	    Old->rhs=rhs;
		Old->g=rhs;

		if(Old->h<100)
			w=w2;
		else
			w=w1;
	    Old->f=rhs+w*Old->h;
	    PropagateDown(Old);       /* Since we changed the g value of Old, we need
					 to propagate this new value downwards, i.e.
					 do a Depth-First traversal of the tree! */
	}
   }

    else 
    {
	Successor=new struct NODE;
	memset(Successor,0,sizeof( struct NODE ));
	Successor->Parent=BestNode;
	Successor->g=1000000;
	Successor->rhs=rhs;
	Successor->h=sqrt((x-dx)*(x-dx)+(z-dz)*(z-dz)+canshu*(y-dy)*(y-dy));  // should do sqrt(), but since we don't really
	if(Successor->h<100)
		w=w2;
	else 
	    w=w1;
	Successor->f=rhs+w*Successor->h;     // care about the distance but just which branch looks
	Successor->x=x;                  // better this should suffice. Anyayz it's faster.
	Successor->z=z;
	Successor->NodeNum=TileNumS;
	Insert(Successor);     /* Insert Successor on OPEN list wrt f */
	for(c=0;c<18;c++)
	if (BestNode->Child[c] == NULL) /* Add Old to the list of BestNode's Children (or Successors). */
	  break;
	BestNode->Child[c]=Successor;
    }
}

struct NODE *Terrain::CheckOPEN(int tilenum)
{
    struct NODE *tmp;

    tmp=OPEN->NextNode;
    while (tmp != NULL)
    {
	if (tmp->NodeNum == tilenum)
	  return (tmp);
	else
	  tmp=tmp->NextNode;
    }
  return (NULL);
}

struct NODE *Terrain::CheckCLOSED(int tilenum)
{
    struct NODE *tmp;

    tmp=CLOSED->NextNode;

    while (tmp != NULL)
    {
	if (tmp->NodeNum == tilenum)
	  return (tmp);
	else
	  tmp=tmp->NextNode;
    }
  return (NULL);
}

void Terrain::Insert(struct NODE *Successor)
{
    struct NODE *tmp1,*tmp2;
    double f;

    if (OPEN->NextNode == NULL)
      {
	OPEN->NextNode=Successor;
	return;
      }

       /* insert into OPEN successor wrt f */

    f=Successor->f;
    tmp1=OPEN;
    tmp2=OPEN->NextNode;

    while ((tmp2 != NULL) && (tmp2->f < f))
    {
      tmp1=tmp2;
      tmp2=tmp2->NextNode;
    }
	Successor->NextNode=tmp2;
	tmp1->NextNode=Successor;
}

void Terrain::PropagateDown(struct NODE *Old)
{
    double g,rhs;
	int c;
    struct NODE *Child, *Father;

    long x=Old->x,z=Old->z; 
	float y=Vertices[x*Size+z].posy;

	g=Old->g;            // alias.
	rhs=Old->rhs;
    for(c=0;c<18;c++)
    {
    if ((Child=Old->Child[c])==NULL)   // create alias for faster access.
      break;
	if (rhs+sqrt((x-Child->x)*(x-Child->x)+(z-Child->z)*(z-Child->z)+canshu*(y-Vertices[x*Size+z].posy)*(y-Vertices[x*Size+z].posy)) < Child->rhs)
	{
	  Child->rhs=rhs+sqrt((x-Child->x)*(x-Child->x)+(z-Child->z)*(z-Child->z)+canshu*(y-Vertices[x*Size+z].posy)*(y-Vertices[x*Size+z].posy)) ;
	  if(Child->h<100)
		  w=w2;
	  else 
		  w=w1;
	  Child->f=Child->rhs+w*Child->h;
	  Child->Parent=Old;           // reset parent to new path.
	  Push(Child);                 // Now the Child's branch need to be
	}     // checked out. Remember the new cost must be propagated down.
    }

    while (Stack->NextStackPtr != NULL)
    {
	Father=Pop();
	for(c=0;c<18;c++)
	{
	 if ((Child=Father->Child[c])==NULL)       /* we may stop the propagation 2 ways: either */
	  break;
	  if (Father->rhs+sqrt((x-Child->x)*(x-Child->x)+(z-Child->z)*(z-Child->z)+canshu*(y-Vertices[x*Size+z].posy)*(y-Vertices[x*Size+z].posy)) < Child->rhs) /* there are no children, or that the g value of */
	  {                           /* the child is equal or better than the cost we're propagating */
	    Child->rhs=Father->rhs+sqrt((x-Child->x)*(x-Child->x)+(z-Child->z)*(z-Child->z)+canshu*(y-Vertices[x*Size+z].posy)*(y-Vertices[x*Size+z].posy)) ;
	     if(Child->h<100)
		  w=w2;
	     else 
		  w=w1;
		Child->f=Child->rhs+w*Child->h;
	    Child->Parent=Father;
	    Push(Child);
	  }
	}
    }
}

/**************************************************************************/
/*                            STACK FUNCTIONS                             */
/**************************************************************************/

void Terrain::Push(struct NODE *Node)
{
	struct STACK *tmp=new struct STACK;
    memset(tmp,0,sizeof(struct STACK));
    tmp->NodePtr=Node;
    tmp->NextStackPtr=Stack->NextStackPtr;
    Stack->NextStackPtr=tmp;
}
NODE * Terrain::Pop()
{
    struct NODE *tmp;
    struct STACK *tmpSTK;

    tmpSTK=Stack->NextStackPtr;
    tmp=tmpSTK->NodePtr;

    Stack->NextStackPtr=tmpSTK->NextStackPtr;
    free(tmpSTK);
    return (tmp);
}