Yo Fleyt / Mbed 2 deprecated VirtualForces_with_class

with class

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

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?

UserRevisionLine numberNew 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 }