A star 3D

This is a discussion on A star 3D within the C++ Programming forums, part of the General Programming Boards category; I'm trying to implement the a star algorithm, the 2D version works fine(found some implementations) , but then with 3 ...

  1. #1
    Registered User
    Join Date
    Aug 2013
    Posts
    3

    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. #2
    Programming Wraith GReaper's Avatar
    Join Date
    Apr 2009
    Location
    Greece
    Posts
    1,616
    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.
    Devoted my life to programming...

  3. #3
    Registered User
    Join Date
    Aug 2013
    Posts
    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. #4
    Programming Wraith GReaper's Avatar
    Join Date
    Apr 2009
    Location
    Greece
    Posts
    1,616
    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.
    Devoted my life to programming...

  5. #5
    Algorithm Dissector iMalc's Avatar
    Join Date
    Dec 2005
    Location
    New Zealand
    Posts
    6,292
    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.
    My homepage
    Advice: Take only as directed - If symptoms persist, please see your debugger

    Linus Torvalds: "But it clearly is the only right way. The fact that everybody else does it some other way only means that they are wrong"

  6. #6
    Registered User
    Join Date
    Aug 2013
    Posts
    3
    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. #7
    Algorithm Dissector iMalc's Avatar
    Join Date
    Dec 2005
    Location
    New Zealand
    Posts
    6,292
    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.
    My homepage
    Advice: Take only as directed - If symptoms persist, please see your debugger

    Linus Torvalds: "But it clearly is the only right way. The fact that everybody else does it some other way only means that they are wrong"

  8. #8
    Registered User rogster001's Avatar
    Join Date
    Aug 2006
    Location
    Liverpool UK
    Posts
    1,407
    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.
    Thought for the day:
    "Are you sure your sanity chip is fully screwed in sir?" (Kryten)
    FLTK: "The most fun you can have with your clothes on."

    Stroustrup:
    "If I had thought of it and had some marketing sense every computer and just about any gadget would have had a little 'C++ Inside' sticker on it'"

Popular pages Recent additions subscribe to a feed

Similar Threads

  1. A-Star Pathfinding
    By mike_g in forum General AI Programming
    Replies: 1
    Last Post: 08-05-2007, 04:18 PM
  2. star trekked
    By kryptkat in forum A Brief History of Cprogramming.com
    Replies: 6
    Last Post: 05-16-2007, 11:08 PM
  3. star wars
    By Doxygen in forum A Brief History of Cprogramming.com
    Replies: 7
    Last Post: 11-25-2006, 10:14 AM
  4. star wars vs star trek
    By psychopath in forum A Brief History of Cprogramming.com
    Replies: 28
    Last Post: 06-14-2005, 10:28 PM
  5. Star Wars or Star Trek
    By Garfield in forum A Brief History of Cprogramming.com
    Replies: 32
    Last Post: 09-28-2001, 08:33 AM

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21