The Ecere SDK - sdk/blob - extras/tiles/astar.ec

   1 /****************************************************************************
   2    ECERE Tile Engine
   3
   4    Copyright (c) 1997-2001 Jerome Jacovella-St-Louis
   5    All Rights Reserved.
   6
   7    astar.ec - A* Path Finding Algorithm
   8 ****************************************************************************/
   9 import "ecere"
  10
  11 #define TILE(a,b,d) ((b)*d.x+(a))
  12 #define TILENUM(p,d) TILE((p).x,(p).y,d)
  13
  14 struct ASNode
  15 {
  16    ASNode *parent;
  17    ASNode *child[8];       // a node may have up to 8+(NULL) children
  18    ASNode *nextNode;       // for filing purposes
  19    int f,h;
  20    int g,tmpg;
  21    int nodeNum;
  22    Point position;
  23 };
  24
  25 struct AStar
  26 {
  27    void ** stack;
  28    int stackSize;
  29    int stackIndex;
  30    ASNode **openMap;
  31    ASNode **closedMap;
  32    ASNode *open;
  33    ASNode *closed;
  34 };
  35
  36 void AStarTerminate(AStar * aStar)
  37 {
  38    if(aStar)
  39    {
  40       if(aStar->stack) delete aStar->stack;
  41       if(aStar->openMap) delete aStar->openMap;
  42       if(aStar->closedMap) delete aStar->closedMap;
  43       delete aStar;
  44    }
  45 }
  46
  47 AStar * AStarInitialize(int width, int height, int stackSize)
  48 {
  49    AStar * aStar = new0 AStar[1];
  50    if(aStar)
  51    {
  52       aStar->stackSize = stackSize;
  53       aStar->stack = new0 void *[stackSize];
  54       aStar->stackIndex = 0;
  55       aStar->openMap = new0 ASNode *[width*height];
  56       aStar->closedMap=new0 ASNode *[width*height];
  57    }
  58    return aStar;
  59 }
  60
  61 static ASNode * AStarRBestNode(AStar * aStar)
  62 {
  63    ASNode *tmp;
  64
  65    if(!aStar->open->nextNode)
  66       return null;
  67
  68    tmp=aStar->open->nextNode;
  69    aStar->open->nextNode=tmp->nextNode;
  70
  71    tmp->nextNode=aStar->closed->nextNode;
  72    aStar->closed->nextNode=tmp;
  73    aStar->openMap[tmp->nodeNum]=null;
  74    aStar->closedMap[tmp->nodeNum]=tmp;
  75    return tmp;
  76 }
  77
  78 static void AStarPropagateDown(AStar * aStar, ASNode *old)
  79 {
  80    int c,g;
  81    ASNode *child, *father;
  82
  83    g=old->g;
  84    for(c=0;c<8;c++)
  85    {
  86       child=old->child[c];
  87       if(!child)break;
  88       if(g+1<child->g)
  89       {
  90               child->g=g+1;
  91               child->f=child->g+child->h;
  92               child->parent=old;
  93          if(aStar->stackIndex>=aStar->stackSize)
  94             break;
  95          aStar->stack[aStar->stackIndex++] = child;
  96       }
  97    }
  98
  99    while(aStar->stackIndex > 0)
 100    {
 101       father=aStar->stack[--aStar->stackIndex];
 102       for(c=0; c<8; c++)
 103       {
 104          child=father->child[c];
 105          if(!child) break;
 106          if(father->g+1<child->g)
 107          {
 108             child->g=father->g+1;
 109                  child->f=child->g+child->h;
 110             child->parent=father;
 111             if(aStar->stackIndex>=aStar->stackSize)
 112                break;
 113             aStar->stack[aStar->stackIndex++] = child;
 114          }
 115       }
 116    }
 117 }
 118
 119 static void AStarInsert(AStar * aStar, ASNode *successor)
 120 {
 121    ASNode *tmp1,*tmp2;
 122    long f;
 123
 124    aStar->openMap[successor->nodeNum]=successor;
 125    if(!aStar->open->nextNode)
 126    {
 127       aStar->open->nextNode=successor;
 128       return;
 129    }
 130
 131    f=successor->f;
 132    tmp1=aStar->open;
 133    tmp2=aStar->open->nextNode;
 134
 135    while(tmp2&&(tmp2->f<f))
 136    {
 137       tmp1=tmp2;
 138       tmp2=tmp2->nextNode;
 139    }
 140    successor->nextNode=tmp2;
 141    tmp1->nextNode=successor;
 142 }
 143
 144 static void AStarGenerateSucc(AStar * aStar, ASNode *bestNode, int positionX, int positionY, int tileNumS,Point destination)
 145 {
 146    int g,c=0;
 147    ASNode *old,*successor;
 148
 149    g=bestNode->g+1;
 150
 151    if((old=aStar->openMap[tileNumS]))
 152    {
 153       for(c=0;c<8;c++)
 154          if(!bestNode->child[c]) break;
 155       bestNode->child[c]=old;
 156
 157       if(g<old->g)
 158       {
 159          old->parent=bestNode;
 160          old->g=g;
 161          old->f=g+old->h;
 162       }
 163    }
 164    else if((old=aStar->closedMap[tileNumS]))
 165    {
 166       for(c=0;c<8;c++)
 167          if(!bestNode->child[c]) break;
 168       bestNode->child[c]=old;
 169
 170       if(g<old->g)
 171       {
 172          old->parent=bestNode;
 173          old->g=g;
 174          old->f=g+old->h;
 175          AStarPropagateDown(aStar, old);
 176       }
 177    }
 178    else
 179    {
 180       successor = new0 ASNode[1];
 181       successor->parent=bestNode;
 182       successor->g=g;
 183       successor->h=(positionX-destination.x)*(positionX-destination.x)+
 184                    (positionY-destination.y)*(positionY-destination.y);
 185       successor->f=g+successor->h;
 186       successor->position.x=positionX;
 187       successor->position.y=positionY;
 188       successor->nodeNum=tileNumS;
 189       AStarInsert(aStar, successor);
 190       for(c=0;c<8;c++)
 191          if(!bestNode->child[c]) break;
 192       bestNode->child[c]=successor;
 193    }
 194 }
 195
 196 static void AStarGenerateSuccessors(AStar * aStar, ASNode *bestNode, byte *map, Point dim, Point destination)
 197 {
 198    int x,y;
 199
 200    for(y = bestNode->position.y - 1; y < bestNode->position.y + 2; y++)
 201       for(x = bestNode->position.x - 1; x < bestNode->position.x + 2; x++)
 202          if((x >= 0) && (y >= 0) && (x < dim.x) && (y < dim.y))
 203             if(x < bestNode->position.x || y < bestNode->position.y || x >= bestNode->position.x + 1 || y >= bestNode->position.y + 1)
 204             {
 205                if(map[TILE(x,y,dim)])
 206                   AStarGenerateSucc(aStar, bestNode,x,y,TILE(x,y,dim),destination);
 207             }
 208 }
 209
 210 ASNode * AStarFindPath(AStar * aStar, byte * map,Point dim,Point start,Point destination, int iterations)
 211 {
 212    ASNode *node, *bestNode;
 213    int tileNumDest;
 214    uint16 c;
 215
 216    tileNumDest = TILENUM(destination,dim);
 217
 218    aStar->open = new0 ASNode[1];
 219    aStar->closed = new0 ASNode[1];
 220
 221    node = new0 ASNode[1];
 222    node->g=0;
 223    node->h=(start.x-destination.x)*(start.x-destination.x)+
 224            (start.y-destination.y)*(start.y-destination.y);
 225    node->f=node->g+node->h;
 226    node->nodeNum = TILENUM(start,dim);
 227    node->position = start;
 228
 229    aStar->open->nextNode=node;
 230    aStar->openMap[node->nodeNum]=node;
 231
 232    if(!map[TILENUM(destination,dim)])
 233       return null;
 234    for(c=0;c<iterations;c++)
 235    {
 236       bestNode=AStarRBestNode(aStar);
 237       if(!bestNode) return null;
 238       if(bestNode->nodeNum==tileNumDest)
 239          return bestNode;
 240       AStarGenerateSuccessors(aStar, bestNode,map,dim,destination);
 241    }
 242    return null;
 243 }
 244
 245 void AStarFreeNodes(AStar * aStar)
 246 {
 247    ASNode *node,*tmp;
 248    node=aStar->open;
 249    while(node)
 250    {
 251       aStar->openMap[node->nodeNum]=null;
 252       tmp=node;
 253       node=tmp->nextNode;
 254       delete tmp;
 255    }
 256    node=aStar->closed;
 257    while(node)
 258    {
 259       aStar->closedMap[node->nodeNum]=null;
 260       tmp=node;
 261       node=tmp->nextNode;
 262       delete tmp;
 263    }
 264 }