# Thread: A star 3D

1. ## A star 3D

I'm trying to implement the a star algorithm, the 2D version works fine(found some implementations) , but then with 3 dimensions it doesn't, and I don't know why..sometimes I don't get any result, sometimes it gives an error: segmentation fault. I know what segmentation fault means but I don't know how to avoid it. If anyone can help me... The code:
Code:
```#include <iostream>
#include <iomanip>
#include <queue>
#include <string>
#include <math.h>
#include <ctime>
#include <stdio.h>
#include <stdlib.h>
using namespace std;

const int n=60; // horizontal size of the map
const int m=60; // vertical size size of the map
const int rot=8;

const double translationalIncrement = 0.1; // *meter
const double rotationalIncrement = 0.785; //*radian
const double rot2transConstant = 0.471; // meter/radian

static int map[n][m][rot];
static int closed_nodes_map[n][m][rot]; // map of closed (tried-out) nodes
static int open_nodes_map[n][m][rot]; // map of open (not-yet-tried) nodes
static int dir_map[n][m][rot]; // map of directions
const int dir=26; // number of possible directions to go at any position
// if dir==4
//static int dx[dir]={1, 0, -1, 0};
//static int dy[dir]={0, 1, 0, -1};
// if dir==8
static int dx[dir]={1, 1, 0, -1, -1, -1,  0,  1, 0, 1, 1, 0, -1, -1, -1,  0,  1,  0,  1,  1,  0, -1, -1, -1,  0,  1};
static int dy[dir]={0, 1, 1,  1,  0, -1, -1, -1, 0, 0, 1, 1,  1,  0, -1, -1, -1,  0,  0,  1,  1,  1,  0, -1, -1, -1};
static int aphi[dir]={0, 0, 0,  0,  0,  0,  0,  0, 1, 1, 1, 1,  1,  1,  1,  1,  1, -1, -1, -1, -1, -1, -1, -1, -1,  -1};

class node
{
// current position
int xPos;
int yPos;
int phiAngle;
// total distance already travelled to reach the node
int level;
// priority=level+remaining distance estimate
int priority;  // smaller: higher priority

public:
node(int xp, int yp, int phia, int d, int p)
{xPos=xp; yPos=yp; phiAngle=phia; level=d; priority=p;}

int getxPos() const {return xPos;}
int getyPos() const {return yPos;}
int getphiAngle() const {return phiAngle;}
int getLevel() const {return level;}
int getPriority() const {return priority;}

void updatePriority(const int & xDest, const int & yDest, const int & phiDest)
{
priority=level+estimate(xDest, yDest, phiDest)*10; //A*
}

// give better priority to going strait instead of diagonally
void nextLevel(const int & i) // i: direction
{
level += (int) sqrt( pow(dx[i]*translationalIncrement, 2.0) + pow(dy[i]*translationalIncrement, 2.0) + pow(aphi[i]*rotationalIncrement * rot2transConstant, 2.0));
}

// Estimation function for the remaining distance to the goal.
const int & estimate(const int & xDest, const int & yDest, const int & phiDest) const
{
static int xd, yd, phid, d;
xd=xDest-xPos;
yd=yDest-yPos;
phid=phiDest-phiAngle;

// Euclidian Distance
d=static_cast<int>(sqrt(xd*xd+yd*yd+phid*phid));

// Manhattan distance
//d=abs(xd)+abs(yd);

// Chebyshev distance
//d=max(abs(xd), abs(yd));

return(d);
}
};

// Determine priority (in the priority queue)
bool operator<(const node & a, const node & b)
{
return a.getPriority() > b.getPriority();
}

// A-star algorithm.
// The route returned is a string of direction digits.
string pathFind( const int & xStart, const int & yStart, const int & phiStart,
const int & xFinish, const int & yFinish, const int & phiFinish )
{
static priority_queue<node> pq[2]; // list of open (not-yet-tried) nodes
static int pqi; // pq index
static node* n0;
static node* m0;
static int i, j, x, y, phi, xdx, ydy, phidphi;
static char c;
pqi=0;

// reset the node maps
for(phi=0;phi<rot;phi++)
{
for(y=0;y<m;y++)
{
for(x=0;x<n;x++)
{
closed_nodes_map[x][y][phi]=0;
open_nodes_map[x][y][phi]=0;
}
}
}

// create the start node and push into list of open nodes
n0=new node(xStart, yStart, phiStart, 0, 0);
n0->updatePriority(xFinish, yFinish, phiFinish);
pq[pqi].push(*n0);
open_nodes_map[x][y][phi]=n0->getPriority(); // mark it on the open nodes map

// A* search
while(!pq[pqi].empty())
{
// get the current node w/ the highest priority
// from the list of open nodes
n0=new node( pq[pqi].top().getxPos(), pq[pqi].top().getyPos(), pq[pqi].top().getphiAngle(),
pq[pqi].top().getLevel(), pq[pqi].top().getPriority());

x=n0->getxPos();
y=n0->getyPos();
phi=n0->getphiAngle();

pq[pqi].pop(); // remove the node from the open list
open_nodes_map[x][y][phi]=0;
// mark it on the closed nodes map
closed_nodes_map[x][y][phi]=1;

// quit searching when the goal state is reached
//if((*n0).estimate(xFinish, yFinish) == 0)
if(x==xFinish && y==yFinish && phi==phiFinish)
{
// generate the path from finish to start
// by following the directions
string path="";
while(!(x==xStart && y==yStart && phi==phiStart))
{
j=dir_map[x][y][phi];
c='0'+(j+dir/2)%dir;
path=c+path;
x+=dx[j];
y+=dy[j];
phi+=aphi[j];
}

// garbage collection
delete n0;
// empty the leftover nodes
while(!pq[pqi].empty()) pq[pqi].pop();
return path;
}

// generate moves (child nodes) in all possible directions
for(i=0;i<dir;i++)
{
xdx=x+dx[i];
ydy=y+dy[i];
phidphi=phi+aphi[i];

if(!(xdx<0 || xdx>n-1 || ydy<0 || ydy>m-1 || phidphi<0 || phidphi>rot-1 || map[xdx][ydy][phidphi]==1
|| closed_nodes_map[xdx][ydy][phidphi]==1))
{
// generate a child node
m0=new node( xdx, ydy, phidphi, n0->getLevel(),
n0->getPriority());
m0->nextLevel(i);
m0->updatePriority(xFinish, yFinish, phiFinish);

// if it is not in the open list then add into that
if(open_nodes_map[xdx][ydy][phidphi]==0)
{
open_nodes_map[xdx][ydy][phidphi]=m0->getPriority();
pq[pqi].push(*m0);
// mark its parent node direction
dir_map[xdx][ydy][phidphi]=(i+dir/2)%dir;
}
else if(open_nodes_map[xdx][ydy][phidphi]>m0->getPriority())
{
// update the priority info
open_nodes_map[xdx][ydy][phidphi]=m0->getPriority();
// update the parent direction info
dir_map[xdx][ydy][phidphi]=(i+dir/2)%dir;

// replace the node
// by emptying one pq to the other one
// except the node to be replaced will be ignored
// and the new node will be pushed in instead
while(!(pq[pqi].top().getxPos()==xdx &&
pq[pqi].top().getyPos()==ydy && pq[pqi].top().getphiAngle()==phidphi))
{
pq[1-pqi].push(pq[pqi].top());
pq[pqi].pop();
}
pq[pqi].pop(); // remove the wanted node

// empty the larger size pq to the smaller one
if(pq[pqi].size()>pq[1-pqi].size()) pqi=1-pqi;
while(!pq[pqi].empty())
{
pq[1-pqi].push(pq[pqi].top());
pq[pqi].pop();
}
pqi=1-pqi;
pq[pqi].push(*m0); // add the better node instead
}
else delete m0; // garbage collection
}
}
delete n0; // garbage collection
}
return ""; // no route found
}

int main()
{
srand(time(NULL));

// create empty map
for(int phi=0;phi<rot;phi++)
{
for(int y=0;y<m;y++)
{
for(int x=0;x<n;x++)

map[x][y][phi]=0;
}
}

// fillout the map matrix with a '+' pattern
for(int phi=0;phi<rot;phi++)
{
for(int x=n/8;x<n*7/8;x++)
{
map[x][m/2][phi]=1;
}
for(int y=m/8;y<m*7/8;y++)
{
map[n/2][y][phi]=1;
}
}

// randomly select start and finish locations
int xA, yA, phiA, xB, yB, phiB;
switch(rand()%2)
{
case 0: xA=0;yA=0;phiA=0;xB=n-1;yB=m-1;phiB=rot-1; break;
case 1: xA=0;yA=m-1;xB=n-1;yB=0;phiA=0;phiB=rot-1; break;
/*     case 2: xA=n/2-1;yA=m/2-1;xB=n/2+1;yB=m/2+1; break;
case 3: xA=n/2-1;yA=m/2+1;xB=n/2+1;yB=m/2-1; break;
case 4: xA=n/2-1;yA=0;xB=n/2+1;yB=m-1; break;
case 5: xA=n/2+1;yA=m-1;xB=n/2-1;yB=0; break;
case 6: xA=0;yA=m/2-1;xB=n-1;yB=m/2+1; break;
case 7: xA=n-1;yA=m/2+1;xB=0;yB=m/2-1; break;  */
}

cout<<"Map Size (X,Y,Phi): "<<n<<","<<m<<","<<rot<<endl;
cout<<"Start: "<<xA<<","<<yA<<","<<phiA<<endl;
cout<<"Finish: "<<xB<<","<<yB<<","<<phiB<<endl;
// get the route
clock_t start = clock();
string route=pathFind(xA, yA, phiA, xB, yB, phiB);
if(route=="") cout<<"An empty route generated!"<<endl;
clock_t end = clock();
double time_elapsed = double(end - start);
cout<<"Time to calculate the route (ms): "<<time_elapsed<<endl;
cout<<"Route:"<<endl;
cout<<route<<endl<<endl;

// follow the route on the map and display it
if(route.length()>0)
{
int j; char c;
int x=xA;
int y=yA;
int phi=phiA;
map[x][y][phi]=2;
for(int i=0;i<route.length();i++)
{
c =route.at(i);
j=atoi(&c);
x=x+dx[j];
y=y+dy[j];
phi=phi+aphi[j];
map[x][y][phi]=3;
}
map[x][y][phi]=4;

// display the map with the route

for(phi=0;phi<rot;phi++)
{
for(int y=0;y<m;y++)
{
for(int x=0;x<n;x++)
if(map[x][y][phi]==0)
cout<<".";
else if(map[x][y][phi]==1)
cout<<"O"; //obstacle
else if(map[x][y][phi]==2)
cout<<"S"; //start
else if(map[x][y][phi]==3)
cout<<"R"; //route
else if(map[x][y][phi]==4)
cout<<"F"; //finish
cout<<endl;
}
}
}
getchar(); // wait for a (Enter) keypress
return(0);
}```

2. Do you know how to use a debugger? It can interrupt your code to wherever you want to check the values of variables, as well as tell you where the segfault occurred.

3. Program received signal SIGSEGV, Segmentation fault.
0x0805b776 in pathFind (xStart=@0xbffff180, yStart=@0xbffff17c,
phiStart=@0xbffff178, xFinish=@0xbffff174, yFinish=@0xbffff170,
phiFinish=@0xbffff16c)
at /home/youbot/applications/Path planning/src/main.cpp:157
157 x+=dx[j];

I don't see what would be the problem at that line

4. This means that "j" is becoming larger than the size of the array "dx" at some point. You'd have to check the algorithm which produces "j" for errors, or recalibrate your array's size.

5. Looking at this code, the names of variables would suggest that you are not using cartesian coordinates:
Code:
```        const int & estimate(const int & xDest, const int & yDest, const int & phiDest) const
{
static int xd, yd, phid, d;
xd=xDest-xPos;
yd=yDest-yPos;
phid=phiDest-phiAngle;

// Euclidian Distance
d=static_cast<int>(sqrt(xd*xd+yd*yd+phid*phid));```
If you aren't using cartesian coordinates then the euclidean distance formula wont work. If you are using cartesian coordinates, then please rename the variables for the z coordinate respectively.
There's no need for those variables to be static either.

6. Thank you for the reply, both of you. I need to use the algorithm for a mobile robot(kuka youbot) for path planning, using 2D it works but the rotation motion is not used, that is why I need the 3D to incorporate the rotation motion. What formula should I use then?

7. Okay so are your coordinates x, y, and rotation angle of the robot?
If so, what makes you think this is a 3D problem?
Sounds to me that it's a 2D problem, but you additionally want to find out the method of getting from A to B once you have solved what path to use, and that itself probably involves a bunch of move and turn commands. If so, then rotation doesn't even come into the pathfinding part of the problem. The rotation only comes into play once you begin to carry out the following of that path.

If the idea of first solving the path, and then moving the robot, does not make sense for your application, then I'm afraid that the A-star algorithm may not be what you need afterall.

8. iMalc has it spot on there - Your robot may as well be an animated sprite - After the path is found then the problem becomes mapping your animation to the following of the path, which is akin to dealing with the problem of rotating your robot, presumably to face the direction of the found path - This is done afterwards and is not part of the algorithm for pathfinding and is not in 3d. I do like the idea of A* as a 3d search though, had not considered it. To accomplish that then the same rules could apply, you would just have the extra work of searching in the z direction, still following orthogonal movement only.

Popular pages Recent additions