path_simplifier.c

Go to the documentation of this file.

#include "path_simplifier.h"
//for DBG function
#define DBG printf
#include <math.h>
#include <stdlib.h>
#define ABS_VAL(x)      ((x > 0.0)?(x):(-x))    
#define POW(x)          (x*x)

#define PATH_ERROR 0.09         
PathPoint * newPathPoint(PathPoint * last ,  float x, float y){
        PathPoint *p = malloc(sizeof(PathPoint));
        p->x = x;
        p->y = y;
        p->next = NULL;
        last->next = p;
        return p;
}
void calc_line(float x1,float y1, float x2, float y2, Line * l ){
        if (x1 == x2){
                // It is a vertical line
                l->a1 = 1.0;
                l->a2 = 0.0;
                l->b = x1;
        }else if (y1 == y2){
                // It is a horizontal line
                l->a1 = 0.0;
                l->a2 = 1.0;
                l->b = y1;
        }else{ 
                // It is a normal line
                l->a1 = -(y2-y1)/(x2-x1);
                l->a2 = 1.0;
                l->b = (y1*x2-y2*x1)/(x2-x1);
        }
}
float calc_dist(Line * l, float x,float y){
        float dist, abs_value, sqrt_value;
        if (l->a1 == 0){
                dist = y - (l->b);
        }else if(l->a2 == 0){
                dist = x-(l->b);
        }else{
                abs_value = (l->a1) * x + (l->a2) * y - (l->b);
                sqrt_value = sqrt( POW(l->a1) + POW(l->a2) );
                dist = abs_value/ sqrt_value;
        }
        if (dist< 0.0 ) dist = -dist;
        return dist;
}


int path_simplifier (double * path, int nbpoints, PathPoint * first_point, double * angle){
        int first_index     = 0;
        int last_index      = nbpoints - 1; 
        int step            = floor ((last_index - first_index)/2);
        float max_err;                          // Max error
        float x1,y1,x2,y2;                      // Limits of the line
        int i;                                  // Auxiliar value
        float dist;                             // Distance betwen actual point and the line
        Line line;                              // Parameters of the line (x1,y1), (x2,y2)
        int points_simplify = 1;
        PathPoint *last_point;                  // A pointer to the last point of the list
        
        
        first_point->x = *(path);
        first_point->y = *(path+1);
        first_point->next = NULL;
        last_point = first_point;               // In the begining, the last point of the list is the first one (there are only one)
        
        
        //DBG("path_simplifier: Nb of points %d :",nbpoints);
        //for(i=0;i<nbpoints;i++) printf("(%f,%f)", (*(path+i*2)), (*(path + i*2 + 1)));        printf("\n");
        
        while ((first_index < (nbpoints-1)) && (last_index <= (nbpoints-1)) ){
                x1=*(path + first_index*2);     y1=*(path + first_index*2 +1);
                x2=*(path + last_index*2);      y2=*(path + last_index*2 +1);
                
                //DBG("path_simplifier: Taking indexes %d to %d\n",first_index, last_index);
                //DBG("path_simplifier: Points : (%f,%f),(%f,%f)\n", x1,y1,x2,y2);
                
                calc_line(x1,y1, x2, y2, &line);
                //DBG("path_simplifier: Line : %f * x + %f * y = %f\n", line.a1, line.a2, line.b);

                // Error Calculus
                max_err = 0;
                for (i=(first_index+1);i<last_index;i++){
                        dist= calc_dist(&line ,(*(path + i*2)) ,(*(path + i*2 +1)));
                        //DBG("path_simplifier: Distance from [%d](%f,%f) to Line : %f\n",i,(*(path + i*2)) ,(*(path + i*2 +1)), dist);
                        if (dist > max_err) max_err = dist;
                }

                // Error decision
                if (max_err < PATH_ERROR ){
                        step = floor ((last_index - first_index)*2);
                        first_index = last_index;
                        last_index = first_index + step;
                        //DBG("path_simplifier: Adding Point : (%f,%f)\n", x2,y2);
                        last_point->next = newPathPoint(last_point, x2, y2);
                        last_point = last_point->next;
                        points_simplify++;
                        //DBG("path_simplifier: error ok\n");
                } else { 
                        step = ceil ((last_index - first_index)/2);
                        last_index = first_index + step;
                        //DBG("path_simplifier: error ko\n");
                }
                // last index correction
                if (last_index > (nbpoints -1)) last_index = nbpoints -1;
        }
        //The final angle must be the angle of the slope of the last line
        (*angle) = atan2((y2-y1),(x2-x1)); 
        
        // Return the nb of points in the path
        return points_simplify;
}
void freePathMemory(PathPoint * simple_path){
        PathPoint * tmp;
        PathPoint * toFree;
        // Add this simple path to the trajectory
        tmp = simple_path;
        while(tmp!= NULL){
                toFree = tmp;
                tmp = tmp->next;
                //Free allocated memory
                free(toFree);
        }
}

---