File content as of revision 4:256f2cbe3fdd:

#include <math.h>
#include <cmath>

void velRobot2velWheels(float vRobot,float wRobot,float wheelsRadius,float wheelsDistance,float w[2])
{
    w[0]=(vRobot-(wheelsDistance/2)*wRobot)/wheelsRadius;
    w[1]=(vRobot+(wheelsDistance/2)*wRobot)/wheelsRadius;
}


void nextPose(float countsLeft, float countsRight, float wheelsRadius, float wheelsDistance, float pose[3])
{
    // Deslocamentos
    float d_l, d_r, desl, delta_ang, delta_x, delta_y;
    
    d_l = 2*3.1415926535 * wheelsRadius * ( countsLeft/1440.0f );
    d_r = 2*3.1415926535 * wheelsRadius * ( countsRight/1440.0f );
    
    desl = (d_l+d_r)/2.0f;
    
    
    delta_ang = (d_r-d_l)/wheelsDistance;
    
    delta_x = desl * cos(pose[2]+delta_ang/2.0f);
    delta_y = desl * sin(pose[2]+delta_ang/2.0f);
    
    
    pose[0] = pose[0] + delta_x;
    pose[1] = pose[1] + delta_y;
    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){
    
    
    float z_max = 200; // 2 m
    float alfa = 5; // 5 cm
    //float beta = 1; // 1 grau
    float L0 = 0.0;
    float Locc = 0.65;
    float Lfree = -0.65;
    float L;
    
    float r = sqrt( pow((xi-xt),2) + pow((yi-yt),2) );
    //phi = atan2( yi-yt, xi-xt ) - theta;
    
    //if (r > min(z_max, z+alfa/2)) || (abs(phi-theta) > beta/2)
        //L = L0;
    if ((z < z_max) && (abs(r-z_max) < alfa/2.0))
        L = Locc;
    else if (r <= z)
        L = Lfree;
    else
        L = L0;
    
    return L;
    
}

void Mapping(float MapaLog[40][40], float xi, float yi, int **pointsVec, float z, int dim){
    
    int x, y;
    float L;
    
    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;
    }
    
}