Yo Fleyt / Mbed 2 deprecated VirtualForces_with_class

with class

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

Map.cpp@39:890439b495e3, 2017-06-16 (annotated)

Committer:
Ludwigfr
Date:
Fri Jun 16 10:40:53 2017 +0000
Revision:
39:890439b495e3
Parent:
35:68f9edbb3cff 
last version with the 4th lab;

Who changed what in which revision?

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