with class
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Map.cpp@33:814bcd7d3cfe, 2017-06-09 (annotated)
- Committer:
- Ludwigfr
- Date:
- Fri Jun 09 00:28:32 2017 +0000
- Revision:
- 33:814bcd7d3cfe
- Child:
- 34:c208497dd079
version with class
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Ludwigfr | 33:814bcd7d3cfe | 1 | #include "Map.hpp" |
Ludwigfr | 33:814bcd7d3cfe | 2 | |
Ludwigfr | 33:814bcd7d3cfe | 3 | Map::Map(float widthRealMap, float HeightRealMap, int nbCellWidth, int nbCellHeight){ |
Ludwigfr | 33:814bcd7d3cfe | 4 | this->widthRealMap=widthRealMap; |
Ludwigfr | 33:814bcd7d3cfe | 5 | this->HeightRealMap=HeightRealMap; |
Ludwigfr | 33:814bcd7d3cfe | 6 | this->nbCellWidth=nbCellWidth; |
Ludwigfr | 33:814bcd7d3cfe | 7 | this->nbCellHeight=nbCellHeight; |
Ludwigfr | 33:814bcd7d3cfe | 8 | this->sizeCellWidth=widthRealMap/(float)nbCellWidth; |
Ludwigfr | 33:814bcd7d3cfe | 9 | this->sizeCellHeight=HeightRealMap/(float)nbCellHeight; |
Ludwigfr | 33:814bcd7d3cfe | 10 | |
Ludwigfr | 33:814bcd7d3cfe | 11 | this->cellsLogValues= new float*[nbCellWidth]; |
Ludwigfr | 33:814bcd7d3cfe | 12 | for(int i = 0; i < nbCellWidth; ++i) |
Ludwigfr | 33:814bcd7d3cfe | 13 | this->cellsLogValues[i] = new float[nbCellHeight]; |
Ludwigfr | 33:814bcd7d3cfe | 14 | |
Ludwigfr | 33:814bcd7d3cfe | 15 | this->initialLogValues= new float*[nbCellWidth]; |
Ludwigfr | 33:814bcd7d3cfe | 16 | for(int i = 0; i < nbCellWidth; ++i) |
Ludwigfr | 33:814bcd7d3cfe | 17 | this->initialLogValues[i] = new float[nbCellHeight]; |
Ludwigfr | 33:814bcd7d3cfe | 18 | |
Ludwigfr | 33:814bcd7d3cfe | 19 | this->fill_initialLogValues(); |
Ludwigfr | 33:814bcd7d3cfe | 20 | } |
Ludwigfr | 33:814bcd7d3cfe | 21 | |
Ludwigfr | 33:814bcd7d3cfe | 22 | //fill initialLogValues with the values we already know (here the bordurs) |
Ludwigfr | 33:814bcd7d3cfe | 23 | void Map::fill_initialLogValues(){ |
Ludwigfr | 33:814bcd7d3cfe | 24 | //Fill map, we know the border are occupied |
Ludwigfr | 33:814bcd7d3cfe | 25 | for (int i = 0; i<this->nbCellWidth; i++) { |
Ludwigfr | 33:814bcd7d3cfe | 26 | for (int j = 0; j<nbCellHeight; j++) { |
Ludwigfr | 33:814bcd7d3cfe | 27 | if(j==0 || j==nbCellHeight-1 || i==0 || i==nbCellWidth-1) |
Ludwigfr | 33:814bcd7d3cfe | 28 | this->initialLogValues[i][j] = proba_to_log(1); |
Ludwigfr | 33:814bcd7d3cfe | 29 | else |
Ludwigfr | 33:814bcd7d3cfe | 30 | initialLogValues[i][j] = proba_to_log(0.5); |
Ludwigfr | 33:814bcd7d3cfe | 31 | } |
Ludwigfr | 33:814bcd7d3cfe | 32 | } |
Ludwigfr | 33:814bcd7d3cfe | 33 | } |
Ludwigfr | 33:814bcd7d3cfe | 34 | |
Ludwigfr | 33:814bcd7d3cfe | 35 | //returns the probability [0,1] that the cell is occupied from the log valAue lt |
Ludwigfr | 33:814bcd7d3cfe | 36 | float Map::log_to_proba(float lt){ |
Ludwigfr | 33:814bcd7d3cfe | 37 | return 1-1/(1+exp(lt)); |
Ludwigfr | 33:814bcd7d3cfe | 38 | } |
Ludwigfr | 33:814bcd7d3cfe | 39 | |
Ludwigfr | 33:814bcd7d3cfe | 40 | |
Ludwigfr | 33:814bcd7d3cfe | 41 | //returns the log value that the cell is occupied from the probability value [0,1] |
Ludwigfr | 33:814bcd7d3cfe | 42 | float Map::proba_to_log(float p){ |
Ludwigfr | 33:814bcd7d3cfe | 43 | return log(p/(1-p)); |
Ludwigfr | 33:814bcd7d3cfe | 44 | } |
Ludwigfr | 33:814bcd7d3cfe | 45 | |
Ludwigfr | 33:814bcd7d3cfe | 46 | void Map::print_final_map() { |
Ludwigfr | 33:814bcd7d3cfe | 47 | float currProba; |
Ludwigfr | 33:814bcd7d3cfe | 48 | pc.printf("\n\r"); |
Ludwigfr | 33:814bcd7d3cfe | 49 | for (int y = this->nbCellHeight -1; y>-1; y--) { |
Ludwigfr | 33:814bcd7d3cfe | 50 | for (int x= 0; x<this->nbCellWidth; x++) { |
Ludwigfr | 33:814bcd7d3cfe | 51 | currProba=this->log_to_proba(this->cellsLogValues[x][y]); |
Ludwigfr | 33:814bcd7d3cfe | 52 | if ( currProba < 0.5) { |
Ludwigfr | 33:814bcd7d3cfe | 53 | pc.printf(" "); |
Ludwigfr | 33:814bcd7d3cfe | 54 | } else { |
Ludwigfr | 33:814bcd7d3cfe | 55 | if(currProba==0.5) |
Ludwigfr | 33:814bcd7d3cfe | 56 | pc.printf(" . "); |
Ludwigfr | 33:814bcd7d3cfe | 57 | else |
Ludwigfr | 33:814bcd7d3cfe | 58 | pc.printf(" X "); |
Ludwigfr | 33:814bcd7d3cfe | 59 | } |
Ludwigfr | 33:814bcd7d3cfe | 60 | } |
Ludwigfr | 33:814bcd7d3cfe | 61 | pc.printf("\n\r"); |
Ludwigfr | 33:814bcd7d3cfe | 62 | } |
Ludwigfr | 33:814bcd7d3cfe | 63 | } |
Ludwigfr | 33:814bcd7d3cfe | 64 | |
Ludwigfr | 33:814bcd7d3cfe | 65 | |
Ludwigfr | 33:814bcd7d3cfe | 66 | void Map::print_final_map_with_robot_position(float robot_x,float robot_y) { |
Ludwigfr | 33:814bcd7d3cfe | 67 | float currProba; |
Ludwigfr | 33:814bcd7d3cfe | 68 | float Xrobot=this->robot_x_coordinate_in_world(robot_x,robot_y); |
Ludwigfr | 33:814bcd7d3cfe | 69 | float Yrobot=this->robot_y_coordinate_in_world(robot_x,robot_y); |
Ludwigfr | 33:814bcd7d3cfe | 70 | |
Ludwigfr | 33:814bcd7d3cfe | 71 | float heightIndiceInOrthonormal; |
Ludwigfr | 33:814bcd7d3cfe | 72 | float widthIndiceInOrthonormal; |
Ludwigfr | 33:814bcd7d3cfe | 73 | |
Ludwigfr | 33:814bcd7d3cfe | 74 | float widthMalus=-(3*sizeCellWidth/2); |
Ludwigfr | 33:814bcd7d3cfe | 75 | float widthBonus=sizeCellWidth/2; |
Ludwigfr | 33:814bcd7d3cfe | 76 | |
Ludwigfr | 33:814bcd7d3cfe | 77 | float heightMalus=-(3*sizeCellHeight/2); |
Ludwigfr | 33:814bcd7d3cfe | 78 | float heightBonus=sizeCellHeight/2; |
Ludwigfr | 33:814bcd7d3cfe | 79 | |
Ludwigfr | 33:814bcd7d3cfe | 80 | pc.printf("\n\r"); |
Ludwigfr | 33:814bcd7d3cfe | 81 | for (int y = nbCellHeight -1; y>-1; y--) { |
Ludwigfr | 33:814bcd7d3cfe | 82 | for (int x= 0; x<nbCellWidth; x++) { |
Ludwigfr | 33:814bcd7d3cfe | 83 | heightIndiceInOrthonormal=this->cell_height_coordinate_to_world(y); |
Ludwigfr | 33:814bcd7d3cfe | 84 | widthIndiceInOrthonormal=this->cell_width_coordinate_to_world(x); |
Ludwigfr | 33:814bcd7d3cfe | 85 | if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus)) |
Ludwigfr | 33:814bcd7d3cfe | 86 | pc.printf(" R "); |
Ludwigfr | 33:814bcd7d3cfe | 87 | else{ |
Ludwigfr | 33:814bcd7d3cfe | 88 | currProba=this->log_to_proba(this->cellsLogValues[x][y]); |
Ludwigfr | 33:814bcd7d3cfe | 89 | if ( currProba < 0.5) |
Ludwigfr | 33:814bcd7d3cfe | 90 | pc.printf(" "); |
Ludwigfr | 33:814bcd7d3cfe | 91 | else{ |
Ludwigfr | 33:814bcd7d3cfe | 92 | if(currProba==0.5) |
Ludwigfr | 33:814bcd7d3cfe | 93 | pc.printf(" . "); |
Ludwigfr | 33:814bcd7d3cfe | 94 | else |
Ludwigfr | 33:814bcd7d3cfe | 95 | pc.printf(" X "); |
Ludwigfr | 33:814bcd7d3cfe | 96 | } |
Ludwigfr | 33:814bcd7d3cfe | 97 | } |
Ludwigfr | 33:814bcd7d3cfe | 98 | } |
Ludwigfr | 33:814bcd7d3cfe | 99 | pc.printf("\n\r"); |
Ludwigfr | 33:814bcd7d3cfe | 100 | } |
Ludwigfr | 33:814bcd7d3cfe | 101 | } |
Ludwigfr | 33:814bcd7d3cfe | 102 | |
Ludwigfr | 33:814bcd7d3cfe | 103 | void Map::print_final_map_with_robot_position_and_target(float robot_x,float robot_y,float targetXWorld, float targetYWorld) { |
Ludwigfr | 33:814bcd7d3cfe | 104 | float currProba; |
Ludwigfr | 33:814bcd7d3cfe | 105 | float Xrobot=this->robot_x_coordinate_in_world(robot_x,robot_y); |
Ludwigfr | 33:814bcd7d3cfe | 106 | float Yrobot=this->robot_y_coordinate_in_world(robot_x,robot_y); |
Ludwigfr | 33:814bcd7d3cfe | 107 | |
Ludwigfr | 33:814bcd7d3cfe | 108 | float heightIndiceInOrthonormal; |
Ludwigfr | 33:814bcd7d3cfe | 109 | float widthIndiceInOrthonormal; |
Ludwigfr | 33:814bcd7d3cfe | 110 | |
Ludwigfr | 33:814bcd7d3cfe | 111 | float widthMalus=-(3*sizeCellWidth/2); |
Ludwigfr | 33:814bcd7d3cfe | 112 | float widthBonus=sizeCellWidth/2; |
Ludwigfr | 33:814bcd7d3cfe | 113 | |
Ludwigfr | 33:814bcd7d3cfe | 114 | float heightMalus=-(3*sizeCellHeight/2); |
Ludwigfr | 33:814bcd7d3cfe | 115 | float heightBonus=sizeCellHeight/2; |
Ludwigfr | 33:814bcd7d3cfe | 116 | |
Ludwigfr | 33:814bcd7d3cfe | 117 | pc.printf("\n\r"); |
Ludwigfr | 33:814bcd7d3cfe | 118 | for (int y = this->nbCellHeight -1; y>-1; y--) { |
Ludwigfr | 33:814bcd7d3cfe | 119 | for (int x= 0; x<this->nbCellWidth; x++) { |
Ludwigfr | 33:814bcd7d3cfe | 120 | heightIndiceInOrthonormal=this->cell_height_coordinate_to_world(y); |
Ludwigfr | 33:814bcd7d3cfe | 121 | widthIndiceInOrthonormal=this->cell_width_coordinate_to_world(x); |
Ludwigfr | 33:814bcd7d3cfe | 122 | if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus)) |
Ludwigfr | 33:814bcd7d3cfe | 123 | pc.printf(" R "); |
Ludwigfr | 33:814bcd7d3cfe | 124 | else{ |
Ludwigfr | 33:814bcd7d3cfe | 125 | if(targetYWorld >= (heightIndiceInOrthonormal+heightMalus) && targetYWorld <= (heightIndiceInOrthonormal+heightBonus) && targetXWorld >= (widthIndiceInOrthonormal+widthMalus) && targetXWorld <= (widthIndiceInOrthonormal+widthBonus)) |
Ludwigfr | 33:814bcd7d3cfe | 126 | pc.printf(" T "); |
Ludwigfr | 33:814bcd7d3cfe | 127 | else{ |
Ludwigfr | 33:814bcd7d3cfe | 128 | currProba=this->log_to_proba(this->cellsLogValues[x][y]); |
Ludwigfr | 33:814bcd7d3cfe | 129 | if ( currProba < 0.5) |
Ludwigfr | 33:814bcd7d3cfe | 130 | pc.printf(" "); |
Ludwigfr | 33:814bcd7d3cfe | 131 | else{ |
Ludwigfr | 33:814bcd7d3cfe | 132 | if(currProba==0.5) |
Ludwigfr | 33:814bcd7d3cfe | 133 | pc.printf(" . "); |
Ludwigfr | 33:814bcd7d3cfe | 134 | else |
Ludwigfr | 33:814bcd7d3cfe | 135 | pc.printf(" X "); |
Ludwigfr | 33:814bcd7d3cfe | 136 | } |
Ludwigfr | 33:814bcd7d3cfe | 137 | } |
Ludwigfr | 33:814bcd7d3cfe | 138 | } |
Ludwigfr | 33:814bcd7d3cfe | 139 | } |
Ludwigfr | 33:814bcd7d3cfe | 140 | pc.printf("\n\r"); |
Ludwigfr | 33:814bcd7d3cfe | 141 | } |
Ludwigfr | 33:814bcd7d3cfe | 142 | } |
Ludwigfr | 33:814bcd7d3cfe | 143 | |
Ludwigfr | 33:814bcd7d3cfe | 144 | void Map::update_cell_value(int widthIndice,int heightIndice ,float proba){ |
Ludwigfr | 33:814bcd7d3cfe | 145 | 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 |
Ludwigfr | 33:814bcd7d3cfe | 146 | } |
Ludwigfr | 33:814bcd7d3cfe | 147 | |
Ludwigfr | 33:814bcd7d3cfe | 148 | float Map::robot_x_coordinate_in_world(float robot_x, float robot_y){ |
Ludwigfr | 33:814bcd7d3cfe | 149 | return this->nbCellWidth*this->sizeCellWidth-robot_y; |
Ludwigfr | 33:814bcd7d3cfe | 150 | } |
Ludwigfr | 33:814bcd7d3cfe | 151 | |
Ludwigfr | 33:814bcd7d3cfe | 152 | float Map::robot_y_coordinate_in_world(float robot_x, float robot_y){ |
Ludwigfr | 33:814bcd7d3cfe | 153 | return robot_x; |
Ludwigfr | 33:814bcd7d3cfe | 154 | } |
Ludwigfr | 33:814bcd7d3cfe | 155 | |
Ludwigfr | 33:814bcd7d3cfe | 156 | float Map::cell_width_coordinate_to_world(int i){ |
Ludwigfr | 33:814bcd7d3cfe | 157 | return this->sizeCellWidth/2+i*this->sizeCellWidth; |
Ludwigfr | 33:814bcd7d3cfe | 158 | } |
Ludwigfr | 33:814bcd7d3cfe | 159 | |
Ludwigfr | 33:814bcd7d3cfe | 160 | float Map::cell_height_coordinate_to_world(int j){ |
Ludwigfr | 33:814bcd7d3cfe | 161 | return this->sizeCellHeight/2+j*this->sizeCellHeight; |
Ludwigfr | 33:814bcd7d3cfe | 162 | } |
Ludwigfr | 33:814bcd7d3cfe | 163 | |
Ludwigfr | 33:814bcd7d3cfe | 164 | float Map::get_proba_cell(int widthIndice, int heightIndice){ |
Ludwigfr | 33:814bcd7d3cfe | 165 | return this->log_to_proba(this->cellsLogValues[widthIndice][heightIndice]); |
Ludwigfr | 33:814bcd7d3cfe | 166 | } |