clear all

R = 1;
ro = 30*pi/180;
e = 0*pi/180;

a = -pi/2:pi/60:pi/2;
X0 = [pi; pi]; 
nmax = 20;

for i=1:length(a);

n = 0;
erro = 1000*ones(2,1);
delta = 1e-6*ones(2,1);
    
  while isempty(find(erro < delta,1)),
    
  b = X0(1);
  g = X0(2);

  f(1,1) = cos(a(i))*cos(b) + sin(g)*sin(ro) - cos(g)*sin(e);
  f(2,1) = sin(a(i))*cos(b) + cos(g)*cos(e) + sin(e);

  J = [-cos(a(i))*sin(b) (cos(g)*sin(ro)+sin(g)*sin(e));
       -sin(a(i))*sin(b)         -sin(g)*cos(e)        ];
  
  X = X0 - J\f;

  erro = abs(X-X0);

  if n==nmax,
      erro = zeros(2,1);
  end

  X0 = X;
  n = n+1

  end

gama(i) = X0(2);
  
xA = R;
yA = 0;
zA = 0;
%xB =  0;
%yB =  R*sin(a(i));
%zB = -R*cos(a(i));
xB =  R*cos(0:pi/20:pi/2);
yB =  R*sin(0:pi/20:pi/2).*sin(a(i));
zB = -R*sin(0:pi/20:pi/2).*cos(a(i));
yy =  R*cos(0:pi/20:pi/2);
zz =  R*sin(0:pi/20:pi/2);
for j=1:length(yy),
  C3 = [0; yy(j); -zz(j)];
  C2 = [cos(g) 0 sin(g); 0 1 0; -sin(g) 0 cos(g)]*C3;
  C1 = [1 0 0; 0 cos(e) -sin(e); 0 sin(e) cos(e)]*C2;
  C0 = [cos(ro) -sin(ro) 0; sin(ro) cos(ro) 0; 0 0 1]*C1;
  xC(j) = C0(1);
  yC(j) = C0(2);
  zC(j) = C0(3);
  
  G2 = [0; zz(j); -yy(j)];
  G1 = [cos(b) -sin(b) 0; sin(b) cos(b) 0; 0 0 1]*G2;
  G0 = [1 0 0; 0 cos(a(i)) -sin(a(i)); 0 sin(a(i)) cos(a(i))]*G1;
  xC3(j) = G0(1);
  yC3(j) = G0(2);
  zC3(j) = G0(3);
end
xD = -R*cos(e)*sin(ro);
yD =  R*cos(e)*cos(ro);
zD =  R*sin(e);
xC0 = xD -R*cos(ro)*sin(g)-R*cos(g)*sin(e)*sin(ro)+R*sin(ro)*cos(e);
yC0 = yD -R*sin(ro)*sin(g)+R*cos(g)*sin(e)*cos(ro)-R*cos(e)*cos(ro);
zC0 = zD -R*cos(g)*cos(e)-R*sin(e);
xC2 = xB(11) -R*sin(b);
yC2 = yB(11) +R*cos(a(i))*cos(b)-R*sin(a(i));
zC2 = zB(11) +R*sin(a(i))*cos(b)+R*cos(a(i));

xD2 = xD -0.1*cos(e)*sin(ro);
yD2 = yD +0.1*cos(e)*cos(ro);
zD2 = zD +0.1*sin(e);

figure(1)
plot3([-R R],[0 0],[0 0],'k--','linewidth',2)
hold on
plot3([0 0],[-R R],[0 0],'k--','linewidth',2)
plot3([0 0],[0 0],[-R R],'k--','linewidth',2)
plot3([xA xA+0.1],[yA yA],[zA zA],'k-','linewidth',5)
plot3([xD xD2],[yD yD2],[zD zD2],'k-','linewidth',5)
plot3(xB,yB,zB,'r-','linewidth',3)
plot3([xA xB(11)],[yA yB(11)],[zA zB(11)],'r-')
plot3(xC,yC,zC,'b-','linewidth',3)
plot3([xD xC0],[yD yC0],[zD zC0],'b-')
plot3(xC3,yC3,zC3,'g-','linewidth',3)
plot3([xB(11) xC2],[yB(11) yC2],[zB(11) zC2],'g-')
hold off
grid on
axis([-1.3 1.3 -1.3 1.3 -1.3 1.3])
axis square
view(-50,30)
pause(0.1)

end

figure(2)
plot(a*180/pi,gama*180/pi)
xlabel('\alpha entrada (rad)')
ylabel('\beta saída (rad)')