with class

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

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