Map.cpp

00001 #include "Map.hpp"
00002 
00003 
00004 Map::Map(float widthRealMap, float heightRealMap, int nbCellWidth, int nbCellHeight){
00005     this->widthRealMap=widthRealMap;
00006     this->heightRealMap=heightRealMap;
00007     this->nbCellWidth=nbCellWidth;
00008     this->nbCellHeight=nbCellHeight;
00009     this->sizeCellWidth=widthRealMap/(float)nbCellWidth;
00010     this->sizeCellHeight=heightRealMap/(float)nbCellHeight;
00011     
00012     this->cellsLogValues= new float*[nbCellWidth];
00013     for(int i = 0; i < nbCellWidth; ++i)
00014         this->cellsLogValues[i] = new float[nbCellHeight];
00015         
00016     this->initialLogValues= new float*[nbCellWidth];
00017     for(int i = 0; i < nbCellWidth; ++i)
00018         this->initialLogValues[i] = new float[nbCellHeight];
00019         
00020     this->fill_initialLogValues();
00021 }
00022 
00023 //fill initialLogValues with the values we already know (here the bordurs)
00024 void Map::fill_initialLogValues(){
00025     //Fill map, we know the border are occupied
00026     for (int i = 0; i<this->nbCellWidth; i++) {
00027         for (int j = 0; j<this->nbCellHeight; j++) {
00028             if(j==0 || j==this->nbCellHeight-1 || i==0 || i==this->nbCellWidth-1)
00029                 this->initialLogValues[i][j] = this->proba_to_log(1);
00030             else
00031                 this->initialLogValues[i][j] = this->proba_to_log(0.5);
00032         }
00033     }
00034 }
00035 
00036 void Map::fill_map_with_initial(){
00037     for (int i = 0; i<this->nbCellWidth; i++) {
00038         for (int j = 0; j<this->nbCellHeight; j++) {
00039             this->cellsLogValues[i][j] = initialLogValues[i][j];
00040         }
00041     }
00042 }
00043 
00044 //returns the probability [0,1] that the cell is occupied from the log valAue lt
00045 float Map::log_to_proba(float lt){
00046     return 1-1/(1+exp(lt));
00047 }
00048 
00049 void Map::update_cell_value(int widthIndice,int heightIndice ,float proba){
00050     this->cellsLogValues[widthIndice][heightIndice]=this->cellsLogValues[widthIndice][heightIndice]+this->proba_to_log(proba)+this->initialLogValues[widthIndice][heightIndice];//map is filled as map[0][0] get the data for the point closest to the origin
00051 }
00052 
00053 float Map::cell_width_coordinate_to_world(int i){
00054     return this->sizeCellWidth/2+i*this->sizeCellWidth;
00055 }
00056 
00057 float Map::cell_height_coordinate_to_world(int j){
00058     return this->sizeCellHeight/2+j*this->sizeCellHeight;
00059 }
00060 
00061 float Map::get_proba_cell(int widthIndice, int heightIndice){
00062     return  this->log_to_proba(this->cellsLogValues[widthIndice][heightIndice]);
00063 }
00064 
00065 //returns the log value that the cell is occupied from the probability value [0,1]
00066 float Map::proba_to_log(float p){
00067     return log(p/(1-p));
00068 }
00069 
00070 /*
00071 
00072 float Map::robot_x_coordinate_in_world(float robot_x, float robot_y){
00073     return this->nbCellWidth*this->sizeCellWidth-robot_y;
00074 }
00075 
00076 float Map::robot_y_coordinate_in_world(float robot_x, float robot_y){
00077     return robot_x;
00078 }
00079 
00080 
00081 void MiniExplorerCoimbra::print_final_map() {
00082     float currProba;
00083     pc.printf("\n\r");
00084     for (int y = this->nbCellHeight -1; y>-1; y--) {
00085         for (int x= 0; x<this->nbCellWidth; x++) {
00086                 currProba=this->log_to_proba(this->cellsLogValues[x][y]);
00087             if ( currProba < 0.5) {
00088                 pc.printf("   ");
00089             } else {
00090                 if(currProba==0.5)
00091                     pc.printf(" . ");
00092                 else
00093                     pc.printf(" X ");
00094             }
00095         }
00096         pc.printf("\n\r");
00097     }
00098 }
00099 
00100 
00101 void Map::print_final_map_with_robot_position(float robot_x,float robot_y) {
00102     float currProba;
00103     float Xrobot=this->robot_x_coordinate_in_world(robot_x,robot_y);
00104     float Yrobot=this->robot_y_coordinate_in_world(robot_x,robot_y);
00105     
00106     float heightIndiceInOrthonormal;
00107     float widthIndiceInOrthonormal;
00108     
00109     float widthMalus=-(3*sizeCellWidth/2);
00110     float widthBonus=sizeCellWidth/2;
00111     
00112     float heightMalus=-(3*sizeCellHeight/2);
00113     float heightBonus=sizeCellHeight/2;
00114 
00115     pc.printf("\n\r");
00116     for (int y = this->nbCellHeight -1; y>-1; y--) {
00117         for (int x= 0; x<this->nbCellWidth; x++) {
00118             heightIndiceInOrthonormal=this->cell_height_coordinate_to_world(y);
00119             widthIndiceInOrthonormal=this->cell_width_coordinate_to_world(x);
00120             if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus))                    
00121                 pc.printf(" R ");
00122             else{
00123                 currProba=this->log_to_proba(this->cellsLogValues[x][y]);
00124                 if ( currProba < 0.5)
00125                     pc.printf("   ");
00126                 else{
00127                     if(currProba==0.5)
00128                         pc.printf(" . ");
00129                     else
00130                         pc.printf(" X ");
00131                 } 
00132             }
00133         }
00134         pc.printf("\n\r");
00135     }
00136 }
00137 
00138 void Map::print_final_map_with_robot_position_and_target(float robot_x,float robot_y,float targetXWorld, float targetYWorld) {
00139     float currProba;
00140     float Xrobot=this->robot_x_coordinate_in_world(robot_x,robot_y);
00141     float Yrobot=this->robot_y_coordinate_in_world(robot_x,robot_y);
00142     
00143     float heightIndiceInOrthonormal;
00144     float widthIndiceInOrthonormal;
00145     
00146     float widthMalus=-(3*sizeCellWidth/2);
00147     float widthBonus=sizeCellWidth/2;
00148     
00149     float heightMalus=-(3*sizeCellHeight/2);
00150     float heightBonus=sizeCellHeight/2;
00151 
00152     pc.printf("\n\r");
00153     for (int y = this->nbCellHeight -1; y>-1; y--) {
00154         for (int x= 0; x<this->nbCellWidth; x++) {
00155             heightIndiceInOrthonormal=this->cell_height_coordinate_to_world(y);
00156             widthIndiceInOrthonormal=this->cell_width_coordinate_to_world(x);
00157             if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus))
00158                 pc.printf(" R ");
00159             else{
00160                 if(targetYWorld >= (heightIndiceInOrthonormal+heightMalus) && targetYWorld <= (heightIndiceInOrthonormal+heightBonus) && targetXWorld >= (widthIndiceInOrthonormal+widthMalus) && targetXWorld <= (widthIndiceInOrthonormal+widthBonus))                    
00161                     pc.printf(" T ");
00162                 else{
00163                     currProba=this->log_to_proba(this->cellsLogValues[x][y]);
00164                     if ( currProba < 0.5)
00165                         pc.printf("   ");
00166                     else{
00167                         if(currProba==0.5)
00168                             pc.printf(" . ");
00169                         else
00170                             pc.printf(" X ");
00171                     } 
00172                 }
00173             }
00174         }
00175         pc.printf("\n\r");
00176     }
00177 }
00178 */