%---------------------------------------------------------------------
% Carlos Cabral 2003 - Dep.UA

% Modelo 1  - Caso 2    ( o o ) distanciados 3600
%---------------------------------------------------------------------
function [coord_loc_robot] = LOCALIZACAO_CASO1_2(coord_pilares,posicao_pilares)

[r2, r1] = DIST_ROBOT2PILAR(coord_pilares); % calcula da distancia do robot aos pilares
angulo_1=atan2(coord_pilares(2,2),coord_pilares(2,1));
angulo_2=atan2(coord_pilares(1,2),coord_pilares(1,1));

if r1 > r2
    
    r_temp=r2;
    r2=r1;
    r1=r_temp;
    angulo_temp=angulo_2;
    angulo_2=angulo_1;
    angulo_1=angulo_temp;
    
end

% x2_coord=0;
% y2_coord=14400;
%     
% x1_coord=0;
% y1_coord=10800;
      
if angulo_2 > angulo_1
    coord_loc_robot(1,1) =[ -1/7200*(-r1^4-167961600000000+25920000*r1^2+25920000*r2^2+2*r1^2*r2^2-r2^4)^(1/2)];
else
    coord_loc_robot(1,1) =[  1/7200*(-r1^4-167961600000000+25920000*r1^2+25920000*r2^2+2*r1^2*r2^2-r2^4)^(1/2)];
end
    
coord_loc_robot(1,2) =[ 12600+1/7200*r1^2-1/7200*r2^2];

   
