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]);
+}