aalgorithm.c

Go to the documentation of this file.

#include "aalgorithm.h"
#include "pathqueue.h"
#include "map.h"
#include <math.h>
#include <stdlib.h>

#define SQRT2 1.41421356        
#define WALL_COST       1000    
GraphMapCell graph[MAP_HEIGHT][MAP_WIDTH];      
void pushNodeInOrder(NodeQueue * q, int x, int y);      
float cFunction(int x1, int y1, int x2, int y2);

int aAlgorithm(double xstart_real,double ystart_real, double xgoal_real, double ygoal_real, double ** original_path){
        NodeQueue * prioritySet = newQueue();
        Node nbest;
        int i, j;
        int xcontig, ycontig;
        int xstart, ystart, xgoal, ygoal;
        // Transform real data in cell data
        xstart = ShmapPoint2Cell_X(xstart_real); ystart = ShmapPoint2Cell_Y(ystart_real); 
        xgoal = ShmapPoint2Cell_X(xgoal_real); ygoal = ShmapPoint2Cell_Y(ygoal_real); 
        


        initGraphStructure();
        calculateMapHeuristic(xgoal, ygoal);

        pushNodeInOrder(prioritySet,xstart,ystart);
        graph[ystart][xstart].g = 0;
        graph[ystart][xstart].f = graph[ystart][xstart].g + graph[ystart][xstart].h ;
        graph[ystart][xstart].bx = xstart;
        graph[ystart][xstart].by = ystart;


        do {
                popNode(prioritySet, &nbest);

                graph[nbest.y][nbest.x].processed = 1;

                if((nbest.x==xgoal) && (nbest.y==ygoal)){
                        NodeQueue * list = newQueue();          // Auxiliary list
                        int nbpoints, count;
                        
                        // clear prioritySet list
                        drainQueue(prioritySet);
                        delQueue(prioritySet);
                        
                        /* Returns a pointer to the path in real coordonates*/
                        
                        /* Memory allocation for the original path */
                        nbpoints = getPathLength(xstart, ystart, xgoal, ygoal);
                        (*original_path) = (double *) malloc(2 * sizeof(double) * (nbpoints));
                
                        /*
                         * Put the path in a LIFO list. The A* algorithm "returns" the path from goal to start.
                         * The robot needs the path from start to goal.
                         */
                        i = xgoal;j = ygoal; 
                        pushNodeLast( list, xgoal, ygoal);
                        while ( ! ((i== xstart) && (j == ystart)) ) {
                                xcontig = graph[j][i].bx;
                                ycontig = graph[j][i].by;
                                pushNodeLast( list, xcontig, ycontig);
                                i=xcontig;j=ycontig;
                        }
                
                        // Convert from grid coordonates to real coordonates
                        // the first point of the path is the real point
                        *((*original_path))     = xstart_real;
                        *((*original_path) + 1) = ystart_real;
                        popNode( list, &nbest);
                        count = 0;
                        while(!queueIsEmpty( list)){
                                count++;
                                popNode( list, &nbest);
                                *((*original_path) + count*2)   = ShmapCell2Point_X(nbest.x);
                                *((*original_path) + count*2+1) = ShmapCell2Point_Y(nbest.y);
                        }
                        // The last point is the real goal
                        *((*original_path) + count*2)   = xgoal_real;
                        *((*original_path) + count*2+1) = ygoal_real;
                        drainQueue(list);
                        delQueue(list);
                        return nbpoints;
                }

                for (i=-1;i<=1;i++){
                        for (j=-1;j<=1;j++){
                                if (!((i== 0) && (j==0))){ // If we are not in the center cell
                                        xcontig=nbest.x+i;ycontig=nbest.y+j;
                                        if(ShmapIsCellInMap(xcontig, ycontig) && (graph[ycontig][xcontig].processed == 0)){
                                                if (!isInQueue(prioritySet,xcontig,ycontig)){
                                                        //  add to prioritySet.
                                                        graph[ycontig][xcontig].g=graph[nbest.y][nbest.x].g + cFunction(nbest.x, nbest.y, xcontig, ycontig);
                                                        graph[ycontig][xcontig].f = graph[ycontig][xcontig].g + graph[ycontig][xcontig].h;
                                                        graph[ycontig][xcontig].bx=nbest.x;
                                                        graph[ycontig][xcontig].by=nbest.y;
                                                        if(ShmapIsFreeCell(xcontig, ycontig)) pushNodeInOrder(prioritySet,xcontig,ycontig);
                                                } else if ((graph[nbest.y][nbest.x].g + cFunction(nbest.x, nbest.y, xcontig, ycontig)) < graph[ycontig][xcontig].g){
                                                        graph[ycontig][xcontig].bx=nbest.x;
                                                        graph[ycontig][xcontig].by=nbest.y;
                                                        graph[ycontig][xcontig].g = (graph[nbest.y][nbest.x].g + cFunction(nbest.x, nbest.y, xcontig, ycontig));
                                                }
                                        }
                                }
                        }
                }
        } while(!queueIsEmpty(prioritySet));
        delQueue(prioritySet);

        return -1;

}



float cFunction(int x1, int y1, int x2, int y2)
{
        float c;
        //if(GETMAPPOS(x2, y2)== MAP_WALL) c = WALL_COST;
        if(!ShmapIsFreeCell(x2,y2)) c = WALL_COST;
        else if( (x1==x2) || (y1 == y2) ) c=1.0;
        else c=SQRT2;
        
        return c;

}

void calculateMapHeuristic(int xgoal, int ygoal){
        int i, j;
        for(i=0;i<MAP_HEIGHT;i++){
                for (j=0;j<MAP_WIDTH;j++){
                        graph[i][j].h=sqrt(abs(xgoal-j)*abs(xgoal-j)+abs(ygoal-i)*abs(ygoal-i));
                }
        }
#if 0
/*      This is another way to compute heuristic without SQRT but it don't work properly */

        Node n;
        NodeQueue * toProcess = malloc(sizeof(NodeQueue));
        int xcontig, ycontig;
        float h; // Heuristic distance of cell
        float newh;                             // New Heuristic distance of cell
        newQueue(toProcess);
        graph[ygoal][xgoal].h=0;
        graph[ygoal][xgoal].processed=1;
        
        pushNode(toProcess, xgoal, ygoal);
        while(!queueIsEmpty(toProcess)){
        //printPrioritySet(toProcess);
                // Pop the first node in the queue
        popNode(toProcess,&n);
        graph[n.y][n.x].processed=1;
                // Take node's heuristic distance
        h = graph[n.y][n.x].h;
                
                // Cross contiguous cells
        for (xcontig = n.x-1 ; xcontig <= n.x + 1 ; xcontig++){
        for (ycontig = n.y - 1 ; ycontig <= n.y + 1 ; ycontig++){
        if(ShmapIsCellInMap(xcontig, ycontig) && (graph[ycontig][xcontig].processed==0)) {
                                                //Add node to priority queue
        if ((!isInQueue(toProcess,xcontig,ycontig))) pushNode(toProcess,xcontig,ycontig);
                                        // Update heuristic if  [ (Heuristic not set) OR [oldHeuristic is higher than newer)]
        newh= h + calculateCost(n.x, n.y, xcontig, ycontig);
        if( ( (graph[ycontig][xcontig].h < 0) || (graph[ycontig][xcontig].h > newh )) ) 
        graph[ycontig][xcontig].h=newh ;
}
                                
}
}
}

        // Clear proccessed flag        
        for(i=0;i<MAP_HEIGHT;i++){
        for (j=0;j<MAP_WIDTH;j++){
        graph[i][j].processed=0;
}
}
#endif

}

float calculateCost(int x1, int y1, int x2, int y2){

        if( (x1==x2) || (y1 == y2) ) return 1.0;
        else return SQRT2;

}

void initGraphStructure(void){
        int i,j;

        //Initialisation of graph structure
        for(i=0;i<MAP_HEIGHT;i++){
                for (j=0;j<MAP_WIDTH;j++){
                        graph[i][j].h=-1;
                        graph[i][j].f=-1;
                        graph[i][j].g=0xFFFF;
                        graph[i][j].bx=-1;
                        graph[i][j].by=-1;
                        graph[i][j].processed=0;
                }
        }
}
void pushNodeInOrder(NodeQueue * q, int x, int y){
        Node *current;
        Node *last;
        Node *n= malloc(sizeof(Node));

        if (n == NULL) {
                // Memory could not be allocated, so print an error and exit. 
                fprintf(stderr, "Couldn't allocate memory\n");
                exit(EXIT_FAILURE);
        }

        n->x=x;
        n->y=y;
        n->next=NULL;

        if (queueIsEmpty(q)) {
                q->first=n;
                q->last=n;
        } else {
                // Put the node in right position
                current=q->first;
                last=NULL;
                while (current!=NULL){
                        if ( graph[y][x].f < graph[current->y][current->x].f ){
                                if(last==NULL) q->first = n;
                                else last->next = n;
                                n->next=current;
                                current=NULL;
                        } else {
                                if (current->next==NULL){
                                        // the node goes in the last position
                                        (q->last)->next=n;
                                        q->last=n;
                                        current = NULL;
                                } else {
                                        last=current;
                                        current=current->next;
                                }
                        }
                }
        }
}
int getPathLength(int xstart, int ystart, int xgoal, int ygoal){
        int i,j,tmpx, tmpy, nbpoints;
        // Count original path length 
        i = xgoal;j = ygoal; nbpoints =1;
        while ( ! ((i== xstart) && (j == ystart)) ) {
                tmpx = graph[j][i].bx;
                tmpy = graph[j][i].by;
                nbpoints++;
                i=tmpx;j=tmpy;
                ShmapSetCellValue(i,j, MAP_PATH);
        }
        ShmapSetCellValue(xgoal,ygoal, MAP_GOAL);
        ShmapSetCellValue(xstart,ystart, MAP_START);
        return nbpoints;
}

---