Diff: Functions.cpp

Revision:

5:bc42c03f2a23

Parent:

4:256f2cbe3fdd

Child:

6:59fbbeaac2af

--- a/Functions.cpp	Thu May 06 16:03:09 2021 +0000
+++ b/Functions.cpp	Fri May 07 14:58:46 2021 +0000
@@ -1,6 +1,7 @@
 
 #include <math.h>
-#include <cmath>
+//#include <cmath>
+#include <stdio.h>
 
 void velRobot2velWheels(float vRobot,float wRobot,float wheelsRadius,float wheelsDistance,float w[2])
 {
@@ -31,72 +32,6 @@
     pose[2] = pose[2] + delta_ang;
 }
 
-int** bresenham(float poseX, float poseY, float x1, float y1, int *dim){
-    
-    float T, E, A, B;
-    float x = poseX;
-    float y = poseY;
-    float dx = abs(x1 - poseX);
-    float dy = abs(y1 - poseY);
-    
-    float s1 = (x1 - poseX)/dx; // substitui o sign() do matlab
-    float s2 = (y1 - poseY)/dy;
-    
-    int interchange = 0;
-
-    if (dy > dx){
-        T = dx;
-        dx = dy;
-        dy = T;
-        interchange = 1;
-    }
-    
-    E = 2.0f*dy - dx;
-    A = 2.0f*dy;
-    B = 2.0f*dy - 2.0f*dx;
-    
-    // =========================================
-    //  Inicializar tabela bidimensional a zero
-    // =========================================
-    int width = 2;
-    int height = (int)(double)(dx+0.5);
-    *dim = height;
-    
-    int** pointsVec = 0;
-    pointsVec = new int*[height];
-    
-    for (int h = 0; h < height; h++){
-        pointsVec[h] = new int[width];
-        for (int w = 0; w < width; w++){
-              pointsVec[h][w] = 0;
-        }
-    }
-    // =========================================
-    
-    for (int i = 0; i<dx; i++){
-        if (E < 0){
-            if (interchange == 1){
-                y = y + s2;
-            }
-            else{
-                x = x + s1;
-            }
-            E = E + A;
-        }
-        
-        else{
-            y = y + s2;
-            x = x + s1;
-            E = E + B;
-        }
-        
-        pointsVec[i][0] = static_cast<int>(x); // converte de float para int (confirmar)
-        pointsVec[i][1] = static_cast<int>(y);
-    }
-    
-    return pointsVec;
-}
-
 
 float Algorith_Inverse(float xi, float yi, float xt, float yt, float z){
     
@@ -125,18 +60,50 @@
     
 }
 
-void Mapping(float MapaLog[40][40], float xi, float yi, int **pointsVec, float z, int dim){
+void bresenham(float poseX, float poseY, float x1, float y1, float MapaLog[40][40], float z){
     
-    int x, y;
-    float L;
+    int T, E, A, B;
+    int x = static_cast<int>(poseX);
+    int y = static_cast<int>(poseY);
+    int dx = static_cast<int>(abs(x1 - poseX));
+    int dy = static_cast<int>(abs(y1 - poseY));
     
-    for(int i=0; i<dim; i++){
-        x = pointsVec[i][0];
-        y = pointsVec[i][1];
-        
-        L = Algorith_Inverse(xi, yi, x, y, z);
-        
-        MapaLog[x][y] = MapaLog[x][y] + L;
+    int s1 = static_cast<int>((x1 - poseX)/dx); // substitui o sign() do matlab
+    int s2 = static_cast<int>((y1 - poseY)/dy);
+    
+    int interchange = 0;
+    
+    if (dy > dx){
+        T = dx;
+        dx = dy;
+        dy = T;
+        interchange = 1;
     }
     
-}
\ No newline at end of file
+    E = 2*dy - dx;
+    A = 2*dy;
+    B = 2*dy - 2*dx;
+    
+    for (int i = 0; i<dx; i++){
+        if (E < 0){
+            if (interchange == 1){
+                y = y + s2;
+            }
+            else{
+                x = x + s1;
+            }
+            E = E + A;
+        }
+        
+        else{
+            y = y + s2;
+            x = x + s1;
+            E = E + B;
+        }
+        
+        // Mapear mapa do Logaritmo
+        MapaLog[x][y] = MapaLog[x][y] + Algorith_Inverse(poseX, poseY, x, y, z);        
+        
+    }
+
+}