J
jesper
I would like some feedback on this.
A while back I was trying my hand at some pathfinding for a small game
I was making.
I did not know anything about it so I read some stuff and came up with
the code below.
I was at the time quite satisfied with it, but reading stuff here has
made me doubt it.
Is this implementation valid?
Is this implementation fast?
The member TNode * Spread[LevelSize][LevelSize]; I put it in there to
make accessing
the open and closed lists faster, doubles the memory requirement of the
object is there
another way that is better memory-wize without being slower? (faster is
ok though )
TBinaryHeap, can it be replaced with an stl container? Is that faster?
In general, oppinions and suggestions are welcome, infact they are the
entire point of this post.
BTW, I snipped this out of context a bit, so I'm afraid that the
main program isn't all that exciting.
//-- Begin Code --
#include <math>
#include <stdlib.h>
#include <iostream>
// I dont know if #include <string> (<strings>??)is needed. My compiler
finds all the stuff
// and compiles this just fine.
typedef int(*ASUtilFunction)(int, int, void *);
const int LevelSize=100;
struct tDelta
{
int dx,dy;
};
tDelta
Direction[]={{0,-1},{1,0},{0,1},{-1,0},{-1,1},{1,1},{1,-1},{-1,-1}};
class TNode
{
public:
float f;
float c;
int x;
int y;
unsigned int heappos;
bool open;
bool closed;
TNode* next;
TNode* parent;
inline bool operator<(TNode &a){return f<a.f;}
inline bool operator==(TNode &a){return (x==a.x)&&(y==a.y);}
inline TNode(float iF=10){f=iF;heappos=-1;}
};
class TBinaryHeap
{
private:
TNode * Heap[LevelSize*LevelSize];
unsigned int insertpoint;
bool valid;
public:
inline void Push(TNode* tmp)
{
unsigned int pos=insertpoint;
unsigned int parentpos;
TNode* swap;
Heap[pos]=tmp;
while (pos>1)
{
parentpos=pos>>1;
if(*(Heap[pos])<*(Heap[parentpos]))
{
swap=Heap[parentpos];
Heap[parentpos]=Heap[pos];
Heap[parentpos]->heappos=parentpos;
Heap[pos]=swap;
Heap[pos]->heappos=pos;
pos=parentpos;
}
else
break;
}
tmp->heappos=pos;
insertpoint++;
}
inline TNode* Pop()
{
if (insertpoint==1) return NULL;
unsigned int pos=insertpoint;
unsigned int child1;
unsigned int child2;
TNode* swap;
TNode* ret=Heap[1];
Heap[1]=Heap[insertpoint-1];
insertpoint--;
pos=1;
unsigned int newpos;
do
{
newpos=pos;
child1=pos<<1;
child2=child1+1;
if (child2<insertpoint-1)
{
if(*(Heap[child1])<*(Heap[pos])) {newpos=child1;}
if(*(Heap[child2])<*(Heap[newpos])) {newpos=child2;}
}
else
if (child1<insertpoint-1)
{
if(*(Heap[child1])<*(Heap[pos])) {newpos=child1;}
}
if(pos!=newpos)
{
swap=Heap[pos];
Heap[pos]=Heap[newpos];
Heap[pos]->heappos=pos;
Heap[newpos]=swap;
Heap[newpos]->heappos=newpos;
pos=newpos;
}
else
break;
}while(true);
return ret;
}
inline bool Validate(unsigned int pos)
{
if ((pos<<1)+1<insertpoint-1)
{
if (!Validate(pos<<1)) return false;
if (!Validate(1+(pos<<1))) return false;
return
((*(Heap[pos])<*(Heap[pos<<1]))&&(*(Heap[pos])<*(Heap[(pos<<1)+1])));
}else
if ((pos<<1)+1<insertpoint-1)
{
if (!Validate(pos<<1)) return false;
return *(Heap[pos])<*(Heap[pos<<1]);
}
return true;
}
inline TBinaryHeap(){insertpoint=1;}
inline void LowerPosition(int pos)
{
unsigned int parentpos;
TNode* swap;
while (pos>1)
{
parentpos=pos>>1;
if(*(Heap[pos])<*(Heap[parentpos]))
{
swap=Heap[parentpos];
Heap[parentpos]=Heap[pos];
Heap[parentpos]->heappos=parentpos;
Heap[pos]=swap;
Heap[pos]->heappos=pos;
pos=parentpos;
}
else
break;
}
}
inline void Empty()
{
insertpoint=1;
}
};
class TAStar
{
private:
TNode * Spread[LevelSize][LevelSize];
TNode * FirstNodeToClear;
TBinaryHeap Heap;
int GoalX;
int GoalY;
void * data;
TNode *Path;
ASUtilFunction UtilCost;
ASUtilFunction UtilValid;
inline bool isopen(int x,int y){return
(Spread[x][y]!=NULL)&&(Spread[x][y]->open);}
inline bool isclosed(int x,int y){return
(Spread[x][y]!=NULL)&&(Spread[x][y]->closed);}
inline TNode* getnode(int x,int y){return Spread[x][y];}
inline TNode* getbest(){return Heap.Pop();}
inline bool doopen(int x,int y,float c)
{
if(!UtilValid(x,y,data)) return false;
float
nf=c+UtilCost(x,y,data)*sqrt(((GoalX-x)*(GoalX-x))+((GoalY-y)*(GoalY-y)));
if(isopen(x,y))
{
if(Spread[x][y]->f<=nf)
return false;
Spread[x][y]->f=nf;
Spread[x][y]->c=c;
Heap.LowerPosition(Spread[x][y]->heappos);
return true;
}
if(isclosed(x,y))
{
if(Spread[x][y]->f<=nf)
return false;
Spread[x][y]->f=nf;
Spread[x][y]->c=c;
Spread[x][y]->closed=false;
Spread[x][y]->open=true;
Heap.Push(Spread[x][y]);
return true;
}
if(!Spread[x][y]) Spread[x][y]=new TNode;
Spread[x][y]->open=true;
Spread[x][y]->x=x;
Spread[x][y]->y=y;
Spread[x][y]->f=nf;
Spread[x][y]->c=c;
Heap.Push(Spread[x][y]);
Spread[x][y]->next=FirstNodeToClear;
FirstNodeToClear=Spread[x][y];
return true;
}
inline void expand(int x,int y,float c)
{
for (int d=0;d<4;d++)
{
if(doopen(x+Direction[d].dx,y+Direction[d].dy,c))
{
Spread[x+Direction[d].dx][y+Direction[d].dy]->parent=Spread[x][y];
}
}
for (int d=4;d<8;d++)
{
if(doopen(x+Direction[d].dx,y+Direction[d].dy,c+0.4))
{
Spread[x+Direction[d].dx][y+Direction[d].dy]->parent=Spread[x][y];
}
}
}
inline void doclose(int x,int y)
{
if(!isopen(x,y)) return;
Spread[x][y]->open=false;
Spread[x][y]->closed=true;
}
inline void ClearNodes()
{
while(FirstNodeToClear)
{
FirstNodeToClear->open=false;
FirstNodeToClear->closed=false;
FirstNodeToClear=FirstNodeToClear->next;
}
Heap.Empty();
}
public:
inline TAStar(){data=NULL;for(int x=0;x<LevelSize;x++)for(int
y=0;y<LevelSize;y++)Spread[x][y]=NULL;}
inline void SetData(void * d){data=d;}
inline void SetValid(ASUtilFunction d){UtilValid=d;}
inline void SetCost(ASUtilFunction d){UtilCost=d;}
inline bool FindPath(int sx,int sy,int gx,int gy)
{
if(!data) return false;
ClearNodes();
GoalX=gx;GoalY=gy;
doopen(sx,sy,0);
Spread[sx][sy]->parent=NULL;
while ((Path=Heap.Pop())!=0)
{
if((Path->x==gx)&&(Path->y==gy)) break;
expand(Path->x,Path->y,Path->c+1);
doclose(Path->x,Path->y);
}
if (!Path) return false;
return true;
}
inline TNode* GetPath(){return Path;}
inline TNode* GetSearchList(){return FirstNodeToClear;}
};
int dummyCost(int x,int y, void *data)
{
return 1;
}
int dummyValid(int x,int y, void *data)
{
bool Edge =(x<1)||(x>=LevelSize)||(y<1)||(y>=LevelSize);
if(!Edge)
return (((bool*)data)[x+y*LevelSize])?1:0;
return 0;
}
int main(int argc,char ** argv)
{
TAStar as;
int startx=0;
int starty=0;
int goalx=0;
int goaly=0;
bool * Map=new bool[LevelSize*LevelSize];
randomize();
for(int i=0;i<LevelSize*LevelSize;i++)
Map= (random(10)!=0);
as.SetData(Map);
as.SetValid(dummyValid);
as.SetCost(dummyCost);
do
{
while(!dummyValid(startx=random(LevelSize),starty=random(LevelSize),(void*)Map));
while(!dummyValid(goalx=random(LevelSize),goaly=random(LevelSize),(void*)Map));
as.FindPath(startx,starty,goalx,goaly);
TNode * BeginingOfPath=as.GetPath();
std::string PathString;
char itoa_Buf[4];
while(BeginingOfPath)
{
PathString+='(';
PathString+=itoa(BeginingOfPath->x,itoa_Buf,10);
PathString+=',';
PathString+=itoa(BeginingOfPath->y,itoa_Buf,10);
PathString+=')';
BeginingOfPath=BeginingOfPath->parent;
}
std::cout<<PathString<<"\r\n";
}while (true);
return 0;
}
A while back I was trying my hand at some pathfinding for a small game
I was making.
I did not know anything about it so I read some stuff and came up with
the code below.
I was at the time quite satisfied with it, but reading stuff here has
made me doubt it.
Is this implementation valid?
Is this implementation fast?
The member TNode * Spread[LevelSize][LevelSize]; I put it in there to
make accessing
the open and closed lists faster, doubles the memory requirement of the
object is there
another way that is better memory-wize without being slower? (faster is
ok though )
TBinaryHeap, can it be replaced with an stl container? Is that faster?
In general, oppinions and suggestions are welcome, infact they are the
entire point of this post.
BTW, I snipped this out of context a bit, so I'm afraid that the
main program isn't all that exciting.
//-- Begin Code --
#include <math>
#include <stdlib.h>
#include <iostream>
// I dont know if #include <string> (<strings>??)is needed. My compiler
finds all the stuff
// and compiles this just fine.
typedef int(*ASUtilFunction)(int, int, void *);
const int LevelSize=100;
struct tDelta
{
int dx,dy;
};
tDelta
Direction[]={{0,-1},{1,0},{0,1},{-1,0},{-1,1},{1,1},{1,-1},{-1,-1}};
class TNode
{
public:
float f;
float c;
int x;
int y;
unsigned int heappos;
bool open;
bool closed;
TNode* next;
TNode* parent;
inline bool operator<(TNode &a){return f<a.f;}
inline bool operator==(TNode &a){return (x==a.x)&&(y==a.y);}
inline TNode(float iF=10){f=iF;heappos=-1;}
};
class TBinaryHeap
{
private:
TNode * Heap[LevelSize*LevelSize];
unsigned int insertpoint;
bool valid;
public:
inline void Push(TNode* tmp)
{
unsigned int pos=insertpoint;
unsigned int parentpos;
TNode* swap;
Heap[pos]=tmp;
while (pos>1)
{
parentpos=pos>>1;
if(*(Heap[pos])<*(Heap[parentpos]))
{
swap=Heap[parentpos];
Heap[parentpos]=Heap[pos];
Heap[parentpos]->heappos=parentpos;
Heap[pos]=swap;
Heap[pos]->heappos=pos;
pos=parentpos;
}
else
break;
}
tmp->heappos=pos;
insertpoint++;
}
inline TNode* Pop()
{
if (insertpoint==1) return NULL;
unsigned int pos=insertpoint;
unsigned int child1;
unsigned int child2;
TNode* swap;
TNode* ret=Heap[1];
Heap[1]=Heap[insertpoint-1];
insertpoint--;
pos=1;
unsigned int newpos;
do
{
newpos=pos;
child1=pos<<1;
child2=child1+1;
if (child2<insertpoint-1)
{
if(*(Heap[child1])<*(Heap[pos])) {newpos=child1;}
if(*(Heap[child2])<*(Heap[newpos])) {newpos=child2;}
}
else
if (child1<insertpoint-1)
{
if(*(Heap[child1])<*(Heap[pos])) {newpos=child1;}
}
if(pos!=newpos)
{
swap=Heap[pos];
Heap[pos]=Heap[newpos];
Heap[pos]->heappos=pos;
Heap[newpos]=swap;
Heap[newpos]->heappos=newpos;
pos=newpos;
}
else
break;
}while(true);
return ret;
}
inline bool Validate(unsigned int pos)
{
if ((pos<<1)+1<insertpoint-1)
{
if (!Validate(pos<<1)) return false;
if (!Validate(1+(pos<<1))) return false;
return
((*(Heap[pos])<*(Heap[pos<<1]))&&(*(Heap[pos])<*(Heap[(pos<<1)+1])));
}else
if ((pos<<1)+1<insertpoint-1)
{
if (!Validate(pos<<1)) return false;
return *(Heap[pos])<*(Heap[pos<<1]);
}
return true;
}
inline TBinaryHeap(){insertpoint=1;}
inline void LowerPosition(int pos)
{
unsigned int parentpos;
TNode* swap;
while (pos>1)
{
parentpos=pos>>1;
if(*(Heap[pos])<*(Heap[parentpos]))
{
swap=Heap[parentpos];
Heap[parentpos]=Heap[pos];
Heap[parentpos]->heappos=parentpos;
Heap[pos]=swap;
Heap[pos]->heappos=pos;
pos=parentpos;
}
else
break;
}
}
inline void Empty()
{
insertpoint=1;
}
};
class TAStar
{
private:
TNode * Spread[LevelSize][LevelSize];
TNode * FirstNodeToClear;
TBinaryHeap Heap;
int GoalX;
int GoalY;
void * data;
TNode *Path;
ASUtilFunction UtilCost;
ASUtilFunction UtilValid;
inline bool isopen(int x,int y){return
(Spread[x][y]!=NULL)&&(Spread[x][y]->open);}
inline bool isclosed(int x,int y){return
(Spread[x][y]!=NULL)&&(Spread[x][y]->closed);}
inline TNode* getnode(int x,int y){return Spread[x][y];}
inline TNode* getbest(){return Heap.Pop();}
inline bool doopen(int x,int y,float c)
{
if(!UtilValid(x,y,data)) return false;
float
nf=c+UtilCost(x,y,data)*sqrt(((GoalX-x)*(GoalX-x))+((GoalY-y)*(GoalY-y)));
if(isopen(x,y))
{
if(Spread[x][y]->f<=nf)
return false;
Spread[x][y]->f=nf;
Spread[x][y]->c=c;
Heap.LowerPosition(Spread[x][y]->heappos);
return true;
}
if(isclosed(x,y))
{
if(Spread[x][y]->f<=nf)
return false;
Spread[x][y]->f=nf;
Spread[x][y]->c=c;
Spread[x][y]->closed=false;
Spread[x][y]->open=true;
Heap.Push(Spread[x][y]);
return true;
}
if(!Spread[x][y]) Spread[x][y]=new TNode;
Spread[x][y]->open=true;
Spread[x][y]->x=x;
Spread[x][y]->y=y;
Spread[x][y]->f=nf;
Spread[x][y]->c=c;
Heap.Push(Spread[x][y]);
Spread[x][y]->next=FirstNodeToClear;
FirstNodeToClear=Spread[x][y];
return true;
}
inline void expand(int x,int y,float c)
{
for (int d=0;d<4;d++)
{
if(doopen(x+Direction[d].dx,y+Direction[d].dy,c))
{
Spread[x+Direction[d].dx][y+Direction[d].dy]->parent=Spread[x][y];
}
}
for (int d=4;d<8;d++)
{
if(doopen(x+Direction[d].dx,y+Direction[d].dy,c+0.4))
{
Spread[x+Direction[d].dx][y+Direction[d].dy]->parent=Spread[x][y];
}
}
}
inline void doclose(int x,int y)
{
if(!isopen(x,y)) return;
Spread[x][y]->open=false;
Spread[x][y]->closed=true;
}
inline void ClearNodes()
{
while(FirstNodeToClear)
{
FirstNodeToClear->open=false;
FirstNodeToClear->closed=false;
FirstNodeToClear=FirstNodeToClear->next;
}
Heap.Empty();
}
public:
inline TAStar(){data=NULL;for(int x=0;x<LevelSize;x++)for(int
y=0;y<LevelSize;y++)Spread[x][y]=NULL;}
inline void SetData(void * d){data=d;}
inline void SetValid(ASUtilFunction d){UtilValid=d;}
inline void SetCost(ASUtilFunction d){UtilCost=d;}
inline bool FindPath(int sx,int sy,int gx,int gy)
{
if(!data) return false;
ClearNodes();
GoalX=gx;GoalY=gy;
doopen(sx,sy,0);
Spread[sx][sy]->parent=NULL;
while ((Path=Heap.Pop())!=0)
{
if((Path->x==gx)&&(Path->y==gy)) break;
expand(Path->x,Path->y,Path->c+1);
doclose(Path->x,Path->y);
}
if (!Path) return false;
return true;
}
inline TNode* GetPath(){return Path;}
inline TNode* GetSearchList(){return FirstNodeToClear;}
};
int dummyCost(int x,int y, void *data)
{
return 1;
}
int dummyValid(int x,int y, void *data)
{
bool Edge =(x<1)||(x>=LevelSize)||(y<1)||(y>=LevelSize);
if(!Edge)
return (((bool*)data)[x+y*LevelSize])?1:0;
return 0;
}
int main(int argc,char ** argv)
{
TAStar as;
int startx=0;
int starty=0;
int goalx=0;
int goaly=0;
bool * Map=new bool[LevelSize*LevelSize];
randomize();
for(int i=0;i<LevelSize*LevelSize;i++)
Map= (random(10)!=0);
as.SetData(Map);
as.SetValid(dummyValid);
as.SetCost(dummyCost);
do
{
while(!dummyValid(startx=random(LevelSize),starty=random(LevelSize),(void*)Map));
while(!dummyValid(goalx=random(LevelSize),goaly=random(LevelSize),(void*)Map));
as.FindPath(startx,starty,goalx,goaly);
TNode * BeginingOfPath=as.GetPath();
std::string PathString;
char itoa_Buf[4];
while(BeginingOfPath)
{
PathString+='(';
PathString+=itoa(BeginingOfPath->x,itoa_Buf,10);
PathString+=',';
PathString+=itoa(BeginingOfPath->y,itoa_Buf,10);
PathString+=')';
BeginingOfPath=BeginingOfPath->parent;
}
std::cout<<PathString<<"\r\n";
}while (true);
return 0;
}