with class
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: Map.cpp
- Revision:
- 33:814bcd7d3cfe
- Child:
- 34:c208497dd079
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Map.cpp Fri Jun 09 00:28:32 2017 +0000 @@ -0,0 +1,166 @@ +#include "Map.hpp" + +Map::Map(float widthRealMap, float HeightRealMap, int nbCellWidth, int nbCellHeight){ + this->widthRealMap=widthRealMap; + this->HeightRealMap=HeightRealMap; + this->nbCellWidth=nbCellWidth; + this->nbCellHeight=nbCellHeight; + this->sizeCellWidth=widthRealMap/(float)nbCellWidth; + this->sizeCellHeight=HeightRealMap/(float)nbCellHeight; + + this->cellsLogValues= new float*[nbCellWidth]; + for(int i = 0; i < nbCellWidth; ++i) + this->cellsLogValues[i] = new float[nbCellHeight]; + + this->initialLogValues= new float*[nbCellWidth]; + for(int i = 0; i < nbCellWidth; ++i) + this->initialLogValues[i] = new float[nbCellHeight]; + + this->fill_initialLogValues(); +} + +//fill initialLogValues with the values we already know (here the bordurs) +void Map::fill_initialLogValues(){ + //Fill map, we know the border are occupied + for (int i = 0; i<this->nbCellWidth; i++) { + for (int j = 0; j<nbCellHeight; j++) { + if(j==0 || j==nbCellHeight-1 || i==0 || i==nbCellWidth-1) + this->initialLogValues[i][j] = proba_to_log(1); + else + initialLogValues[i][j] = proba_to_log(0.5); + } + } +} + +//returns the probability [0,1] that the cell is occupied from the log valAue lt +float Map::log_to_proba(float lt){ + return 1-1/(1+exp(lt)); +} + + +//returns the log value that the cell is occupied from the probability value [0,1] +float Map::proba_to_log(float p){ + return log(p/(1-p)); +} + +void Map::print_final_map() { + float currProba; + pc.printf("\n\r"); + for (int y = this->nbCellHeight -1; y>-1; y--) { + for (int x= 0; x<this->nbCellWidth; x++) { + currProba=this->log_to_proba(this->cellsLogValues[x][y]); + if ( currProba < 0.5) { + pc.printf(" "); + } else { + if(currProba==0.5) + pc.printf(" . "); + else + pc.printf(" X "); + } + } + pc.printf("\n\r"); + } +} + + +void Map::print_final_map_with_robot_position(float robot_x,float robot_y) { + float currProba; + float Xrobot=this->robot_x_coordinate_in_world(robot_x,robot_y); + float Yrobot=this->robot_y_coordinate_in_world(robot_x,robot_y); + + float heightIndiceInOrthonormal; + float widthIndiceInOrthonormal; + + float widthMalus=-(3*sizeCellWidth/2); + float widthBonus=sizeCellWidth/2; + + float heightMalus=-(3*sizeCellHeight/2); + float heightBonus=sizeCellHeight/2; + + pc.printf("\n\r"); + for (int y = nbCellHeight -1; y>-1; y--) { + for (int x= 0; x<nbCellWidth; x++) { + heightIndiceInOrthonormal=this->cell_height_coordinate_to_world(y); + widthIndiceInOrthonormal=this->cell_width_coordinate_to_world(x); + if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus)) + pc.printf(" R "); + else{ + currProba=this->log_to_proba(this->cellsLogValues[x][y]); + if ( currProba < 0.5) + pc.printf(" "); + else{ + if(currProba==0.5) + pc.printf(" . "); + else + pc.printf(" X "); + } + } + } + pc.printf("\n\r"); + } +} + +void Map::print_final_map_with_robot_position_and_target(float robot_x,float robot_y,float targetXWorld, float targetYWorld) { + float currProba; + float Xrobot=this->robot_x_coordinate_in_world(robot_x,robot_y); + float Yrobot=this->robot_y_coordinate_in_world(robot_x,robot_y); + + float heightIndiceInOrthonormal; + float widthIndiceInOrthonormal; + + float widthMalus=-(3*sizeCellWidth/2); + float widthBonus=sizeCellWidth/2; + + float heightMalus=-(3*sizeCellHeight/2); + float heightBonus=sizeCellHeight/2; + + pc.printf("\n\r"); + for (int y = this->nbCellHeight -1; y>-1; y--) { + for (int x= 0; x<this->nbCellWidth; x++) { + heightIndiceInOrthonormal=this->cell_height_coordinate_to_world(y); + widthIndiceInOrthonormal=this->cell_width_coordinate_to_world(x); + if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus)) + pc.printf(" R "); + else{ + if(targetYWorld >= (heightIndiceInOrthonormal+heightMalus) && targetYWorld <= (heightIndiceInOrthonormal+heightBonus) && targetXWorld >= (widthIndiceInOrthonormal+widthMalus) && targetXWorld <= (widthIndiceInOrthonormal+widthBonus)) + pc.printf(" T "); + else{ + currProba=this->log_to_proba(this->cellsLogValues[x][y]); + if ( currProba < 0.5) + pc.printf(" "); + else{ + if(currProba==0.5) + pc.printf(" . "); + else + pc.printf(" X "); + } + } + } + } + pc.printf("\n\r"); + } +} + +void Map::update_cell_value(int widthIndice,int heightIndice ,float proba){ + 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 +} + +float Map::robot_x_coordinate_in_world(float robot_x, float robot_y){ + return this->nbCellWidth*this->sizeCellWidth-robot_y; +} + +float Map::robot_y_coordinate_in_world(float robot_x, float robot_y){ + return robot_x; +} + +float Map::cell_width_coordinate_to_world(int i){ + return this->sizeCellWidth/2+i*this->sizeCellWidth; +} + +float Map::cell_height_coordinate_to_world(int j){ + return this->sizeCellHeight/2+j*this->sizeCellHeight; +} + +float Map::get_proba_cell(int widthIndice, int heightIndice){ + return this->log_to_proba(this->cellsLogValues[widthIndice][heightIndice]); +}