From: xmerm05 AT vse DOT cz (Michal Mertl)
Newsgroups: comp.os.msdos.djgpp
Subject: Re: Astar (A*)
Date: Mon, 25 Aug 1997 16:44:25
Organization: University of Economics - Prague, CZ
Lines: 422
Message-ID: <xmerm05.15.0010BD92@vse.cz>
NNTP-Posting-Host: s018h01.vse.cz
To: djgpp AT delorie DOT com
DJ-Gateway: from newsgroup comp.os.msdos.djgpp
Precedence: bulk

Someone asked here for A* algorythm for DJGPP. I have one, it's the same 
like found in distribution spath but with changes to work without graphics.

I'm not sure if I should post it here. The sources are rather long (about 
400 lines) and "binary files shouldn't be posted to this group" (I mean zip 
file'd be much smaller).

Comment to source: I wanted to separate astar alg implementation from other 
parts of program but it still uses one function from the other module. 
                   I think it can be easily extended to have more 
kinds of tiles (not just free/wall) but I haven't tried yet.

-----astar.c-----#include "astar.h"
/**************************************************************************/
/***************************** A* Algorithm *******************************/
/**************************************************************************/
struct STACK S_Stack,*Stack=&S_Stack;
struct NODE *OPEN=NULL;
struct NODE *CLOSED=NULL;

struct NODE *FindPath(long sx,long sy,long dx,long dy)
{
    struct NODE *Node, *BestNode;

    OPEN=(struct NODE *)calloc(1,sizeof( struct NODE ));
    CLOSED=(struct NODE *)calloc(1,sizeof( struct NODE ));

    Node=(struct NODE *)calloc(1,sizeof( struct NODE ));
    Node->g=0;
    Node->h=(sx-dx)*(sx-dx)+(sy-dy)*(sy-dy);  // should really use sqrt().
    Node->f=Node->g+Node->h;
    Node->x=sx;
    Node->y=sy;

    OPEN->NextNode=Node;        /* make Open List point to first node */
    for (;;)
    {
	if (!(BestNode=(struct NODE *)ReturnBestNode())) break; /* No more nodes on open (no path found) */
	if (BestNode->x==dx && BestNode->y==dy)    /* if we've found the end, break and finish */
	   break;
	GenerateSuccessors(BestNode,dx,dy);
    }
  return (BestNode);
}

struct NODE *ReturnBestNode(void)
{
    struct NODE *tmp;

    if (OPEN->NextNode == NULL) return NULL; /*  Error: no more nodes on OPEN */

/* 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 GenerateSuccessors(struct NODE *BestNode,long dx,long dy)
{
  long x,y;

		    /* Upper-Left  */
    if (FreeTile(x=(long)BestNode->x-1,y=(long)BestNode->y-1))
      GenerateSucc(BestNode,x,y,dx,dy);
		    /* Upper       */
    if (FreeTile(x=(long)BestNode->x,y=(long)BestNode->y-1))
      GenerateSucc(BestNode,x,y,dx,dy);
		    /* Upper-Right */
    if (FreeTile(x=(long)BestNode->x+1,y=(long)BestNode->y-1))
      GenerateSucc(BestNode,x,y,dx,dy);
		    /* Right       */
    if (FreeTile(x=(long)BestNode->x+1,y=(long)BestNode->y))
      GenerateSucc(BestNode,x,y,dx,dy);
		    /* Lower-Right */
    if (FreeTile(x=(long)BestNode->x+1,y=(long)BestNode->y+1))
      GenerateSucc(BestNode,x,y,dx,dy);
		    /* Lower       */
    if (FreeTile(x=(long)BestNode->x,y=(long)BestNode->y+1))
      GenerateSucc(BestNode,x,y,dx,dy);
		    /* Lower-Left  */
    if (FreeTile(x=(long)BestNode->x-1,y=(long)BestNode->y+1))
      GenerateSucc(BestNode,x,y,dx,dy);
		    /* Left        */
    if (FreeTile(x=(long)BestNode->x-1,y=(long)BestNode->y))
      GenerateSucc(BestNode,x,y,dx,dy);
}

void GenerateSucc(struct NODE *BestNode,long x, long y, long dx, long dy)
{
    int g,c=0;
    struct NODE *Old,*Successor;

    g=BestNode->g+1;	    /* g(Successor)=g(BestNode)+cost of getting from BestNode to Successor */

    if ((Old=CheckOPEN(x,y)) != NULL) /* if equal to NULL then not in OPEN list, else it returns the Node in Old */
    {
	for(c=0;c<8;c++)
	  if(BestNode->Child[c] == NULL) /* Add Old to the list of BestNode's Children (or Successors). */
	   break;
	BestNode->Child[c]=Old;

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

	if (g < Old->g)  /* if our new g value is < Old's then reset Old's parent to point to BestNode */
	{
	    Old->Parent=BestNode;
	    Old->g=g;
	    Old->f=g+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=(struct NODE *)calloc(1,sizeof( struct NODE ));
	Successor->Parent=BestNode;
	Successor->g=g;
	Successor->h=(x-dx)*(x-dx)+(y-dy)*(y-dy);  // should do sqrt(), but since we don't really
	Successor->f=g+Successor->h;     // care about the distance but just which branch looks
	Successor->x=x;                  // better this should suffice. Anyayz it's faster.
	Successor->y=y;
	Insert(Successor);     /* Insert Successor on OPEN list wrt f */
	for(c=0;c<8;c++)
	if (BestNode->Child[c] == NULL) /* Add Old to the list of BestNode's Children (or Successors). */
	  break;
	BestNode->Child[c]=Successor;
    }
}

struct NODE *CheckOPEN(int x, int y)
{
    struct NODE *tmp;

    tmp=OPEN->NextNode;
    while (tmp != NULL)
    {
	if (tmp->x==x && tmp->y==y)
	  return (tmp);
	else
	  tmp=tmp->NextNode;
    }
  return (NULL);
}

struct NODE *CheckCLOSED(int x, int y)
{
    struct NODE *tmp;

    tmp=CLOSED->NextNode;

    while (tmp != NULL)
    {
	if (tmp->x==x && tmp->y==y)
	  return (tmp);
	else
	  tmp=tmp->NextNode;
    }
  return (NULL);
}

void Insert(struct NODE *Successor)
{
    struct NODE *tmp1,*tmp2;
    long 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 PropagateDown(struct NODE *Old)
{
    int c,g;
    struct NODE *Child, *Father;

    g=Old->g;            // alias.
    for(c=0;c<8;c++)
    {
    if ((Child=Old->Child[c])==NULL)   // create alias for faster access.
      break;
	if (g+1 < Child->g)
	{
	  Child->g=g+1;
	  Child->f=Child->g+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<8;c++)
	{
	 if ((Child=Father->Child[c])==NULL)       /* we may stop the propagation 2 ways: either */
	  break;
	  if (Father->g+1 < Child->g) /* there are no children, or that the g value of */
	  {                           /* the child is equal or better than the cost we're propagating */
	    Child->g=Father->g+1;
	    Child->f=Child->g+Child->h;
	    Child->Parent=Father;
	    Push(Child);
	  }
	}
    }
}

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

void Push(struct NODE *Node)
{
    struct STACK *tmp;

    tmp=( struct STACK *)calloc(1,sizeof(struct STACK));
    tmp->NodePtr=Node;
    tmp->NextStackPtr=Stack->NextStackPtr;
    Stack->NextStackPtr=tmp;
}

struct NODE *Pop()
{
    struct NODE *tmp;
    struct STACK *tmpSTK;

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

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

-----------------end of astar.c-----------------
-----------------astar.h------------------------
#ifndef _ASTAR_H_
#include <unistd.h>
#include <stdlib.h>
#define _ASTAR_H_

struct NODE {
    long f,h;
    int g,tmpg;
    int x,y;
    struct NODE *Parent;
    struct NODE *Child[8];       /* a node may have upto 8+(NULL) children. */
    struct NODE *NextNode;       /* for filing purposes */
};
/**************************************************************************/
/*                                 STACK                                  */
/**************************************************************************/
struct STACK {
    struct NODE *NodePtr;
    struct STACK *NextStackPtr;
};


struct NODE *FindPath(long sx,long sy,long dx,long dy);
struct NODE *ReturnBestNode(void);
void GenerateSuccessors(struct NODE *BestNode,long dx,long dy);
void GenerateSucc(struct NODE *BestNode,long x, long y, long dx, long dy);
struct NODE *CheckOPEN(int x, int y);
struct NODE *CheckCLOSED(int x, int y);
void Insert(struct NODE *Successor);
void PropagateDown(struct NODE *Old);

void Push(struct NODE *Node);
struct NODE *Pop(void);

#endif
--------------------end of astar.h----------------
--------------------pathfind.c--------------------#include <stdio.h>
#include <conio.h>
#include <dos.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include "astar.h"

#define COLS 80        // note the cols should be 2 more than the screen size
#define ROWS 49        // because if we have a point at the extremes and we check to see
#define TOTAL_TILES COLS*ROWS

unsigned char *TileMap;

void DisplayPath(int x1, int y1, int x2, int y2)
{
  struct NODE *path;

  path=(struct NODE *)FindPath((long)x1,(long)y1,(long)x2,(long)y2);
  
  if (path==NULL) {
    gotoxy(30,25);
    cprintf("Path not found");
    return;
  }
  gotoxy(x2+1,y2+1);
  putch('*');
  while (path->Parent != NULL)
  {
    path=path->Parent;
    gotoxy(path->x+1,path->y+1);
    putch('*');
  }
}

int FreeTile(int x, int y)
{             //  returns 1 if tile is free(nothing on it).
	      //  Note: if TileMap[equation]==0 it means free so just !(not) it.
  return (!TileMap[y*COLS+x]);
}

void BoundaryTiles(void)     // This function places 1 around the boundary
{                            // so that we don't go there. Also we don't
  int c,index=0;          // have to treat the boundaries as a special case

  for(c=0;c<TOTAL_TILES;c++)      // zero out tiles.
      TileMap[c]=0;

  for(c=0;c<COLS;c++) {        // place 1's at boundaries.
    TileMap[c]=1;
    TileMap[c+(ROWS-1)*COLS]=1;
  }
  for(c=1;c<ROWS-1;c++)
  {
    index+=COLS;
    TileMap[index]=1;
    TileMap[index+COLS-1]=1;
  }
}

void DisplayMap(void)
{
    int x,y;

    for (y=0;y<ROWS;y++)
      for (x=0;x<COLS;x++)
        if (TileMap[y*COLS+x]==1) {
          gotoxy(x+1,y+1);
          putch('x');
        }
}

int main(void)
{
    struct text_info textinfo;
    int startmode;
    int x1,y1,x,y;

    startmode=LASTMODE;
    textmode(C4350);
    gettextinfo(&textinfo);
    TileMap=(unsigned char *)calloc(TOTAL_TILES,sizeof(unsigned char));
    BoundaryTiles();

    clrscr();
    srand(time(NULL));
    for (x1=0;x1<TOTAL_TILES/2;x1++) {
      x=rand()%COLS;
      y=rand()%ROWS;
      TileMap[y*COLS+x]=1;
    }
    DisplayMap();
    x=rand()%(COLS-1)+1;
    y=rand()%(ROWS-1)+1;
    x1=rand()%(COLS-1)+1;
    y1=rand()%(ROWS-1)+1;
    TileMap[y*COLS+x]=0;
    TileMap[y1*COLS+x1]=0;
    textcolor(RED);
    DisplayPath(x,y,x1,y1);
    gotoxy(x+1,y+1);
    putch('S');
    gotoxy(x1+1,y1+1);
    putch('G');
    getch();
    textattr(textinfo.normattr);
    textmode(startmode);
    clrscr();
    return 0;
}
---------------------end of pathfind.c------------------
Michal 'MiMe' Mertl
e-mail:XMERM05 AT vse DOT cz