OK here the code.
Hope it's helpful for u!
This is just a simple version, u can modify it for min,maxRange search, source Size, goal Size.... Check goal if goal is surrounded by obstacle (so no way reach it)...
int * sOpenSet[2];
int sOpenSetSize[2];
int * sMatrixLen = NULL;
int * sMatrixFrom = NULL; //direction
//check if we can move to pos x,y
//check with your map here!
bool canMoveHere(int x,int y){
if (x==6 && y==6) return false;
return true;
}
const int Heading2X[9] = { 0,+1,+1,+1, 0,-1,-1,-1, 0 };
const int Heading2Y[9] = { -1,-1, 0,+1,+1,+1, 0,-1, 0 };
int HeadingFull[8]; //use for fast track back found Path
int sourceIndex;
int goalIndex;
bool processPoint2(int index,int size,int gx,int gy,int &bestLen,int &bestIndex,int leftSet ){
bool foundBest=false;
int dy=index/size;
int dx=index%size;
int curLen=sMatrixLen[index];
int newLen=curLen+1;
//numDirection is 8, choose 4 if u want !
for (int k=0;k<8;k++){
int rx=dx+Heading2X[k];
int ry=dy+Heading2Y[k];
int len=(rx-gx)*(rx-gx)+(ry-gy)*(ry-gy);
int index2=ry*size+rx;
//found the path
//we can stop and get the best Path right here
if (curLen>=0 && index2==goalIndex){
sMatrixLen[index2]=newLen;
sMatrixFrom[index2]=k;
return true;
}
//if (mx>=0 && my>=0 && mx<Map.Info.MapWidth && my<Map.Info.MapHeight && !iMapUnit[my][mx] )
if (canMoveHere(rx,ry )){
if (rx>=0 && ry>=0 && rx<size && ry<size && index2!=sourceIndex){
if ( newLen < sMatrixLen[index2] )
{
sMatrixLen[index2] = newLen;
sMatrixFrom[index2]=k;
}
if (sMatrixLen[index2]==0){
if (len<bestLen){
bestIndex=index2;
foundBest = true;
bestLen = len;
}
sMatrixLen[index2] = newLen;
sMatrixFrom[index2] = k;
//add new node to OpenSet
sOpenSet[leftSet][sOpenSetSize[leftSet]]=index2;
sOpenSetSize[leftSet]++;
}
}
}
}
return foundBest;
}
//search from (sx,sy) to (gx,gy)
int mySuperSimpleSearch2(int sx, int sy, int gx, int gy){
//your map size
int size=32;
sOpenSetSize[0] = 0;
sOpenSetSize[1] = 0;
//this init only run once! can move to other place
if (sMatrixLen==NULL){
sMatrixLen = new int[size*size];
sMatrixFrom = new int[size*size];
sOpenSet[0] = new int[size*size];
sOpenSet[1] = new int[size*size];
for (int i=0;i<8;i++){
HeadingFull=Heading2Y*size+Heading2X;
}
}
//use memset for best speed init!
memset(sMatrixLen, 0, ( size*size*sizeof(int) ));
memset(sMatrixFrom, 0, (size*size*sizeof(int) ));
goalIndex=gy*size+gx;
sourceIndex = sy*size+sx;
int bestIndex = sourceIndex;
int bestLen=10000; //keep the bestLen to goal, so we always head for node that is close to goal!
int curOpenSet = 0;
bool haveBest = true; //if we found the node that closest to goal, we process it first!
while(1){
int leftSet=1-curOpenSet;
if (sMatrixLen[goalIndex]>0 ){
//found goal get the Path;
int track=goalIndex;
int pathTotal=0;
int savePath[200];
for (int i=0;i<size*size;i++){
int dir=sMatrixFrom[track];
savePath[pathTotal]=dir;
pathTotal++;
track=track-HeadingFull[dir];
if (track==sourceIndex)
break;
}
return pathTotal;
}
if (haveBest){
haveBest = processPoint2(bestIndex,size,gx,gy,bestLen,bestIndex, curOpenSet );
}else
{
int nSize= sOpenSetSize[curOpenSet];
if (nSize ==0){
//no path left, unReachable;
return -1;
}
for (int i=0;i< nSize ;i++){
int index=sOpenSet[curOpenSet];
bool f1 = processPoint2(index,size,gx,gy,bestLen,bestIndex,leftSet );
if (f1) haveBest = true;
}
sOpenSetSize[curOpenSet]=0;
curOpenSet=leftSet; //swap Openset
}
}
}
