File content as of revision 8:ad8766cf2ec0:

#include <math.h>
#include <stdio.h>
#include "Functions.h"

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;
}


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 bresenham(float poseX, float poseY, float xf, float yf, float z){
    int T, E, A, B;
    int x = static_cast<int>(poseX);
    int y = static_cast<int>(poseY);
    int dx = static_cast<int>(abs(xf - poseX));
    int dy = static_cast<int>(abs(yf - poseY));
    
    int s1 = static_cast<int>((xf - poseX)/dx); // substitui o sign() do matlab
    int s2 = static_cast<int>((yf - poseY)/dy);
    
    int interchange = 0;
    
    if (dy > dx){
        T = dx;
        dx = dy;
        dy = T;
        interchange = 1;
    }
    
    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;
        }
        
        if (x >= 0 && y >= 0 && x < 40 && y < 40){
            // Mapear mapa do Logaritmo
            MapaLog[x][y] = MapaLog[x][y] + Algorith_Inverse(poseX, poseY, x, y, z);
            Mapa40[x][y] = 1 - 1/(1+exp(MapaLog[x][y]));
        }
        
        
                  
    }
}