function c = geo(a,b)
%GEO(A,B) Geometric Operator
%
%        1 - Frame = geo(H)
%        2 - H = geo(Frame)
%        3 - H = geo(A, B)
%
%Frame = [X Y Z A B C]; H = Homogeneous Transformation; A = Frame or H
%                                                       B = Frame or H

%July ~ December 2005, by Reinaldo M. do Nascimento

n=length(a);
if n==6,a=H(a);end
if nargin==2;
         if length(b)==6,b=H(b);end
            ab=a*b;
            c=ab;
else
            c=a;
         if length(a)==4 & n==4,k=0;
                     if isa(a,'sym')
                         try,a1=double(a(1:3,1:3));catch,k=1;end
                         if k==1,c=vts(a);else,c=vt([a1 [0;0;0];0 0 0 1])+vts([eye(3) a(1:3,4);0 0 0 1]);end
                     else
                         c=vt(a);
                     end
         end
end

%==========================================================================
function c = vt (H)

d(1)=H(1,4);d(2)=H(2,4);d(3)=H(3,4);
if (H(1,1)==0) & (H(2,1)==0)
          d(4)=0;
          if H(3,1)==-1
                   d(5)=pi/2;d(6)=atan2(H(1,2),H(2,2));
          else
                   d(5)=-pi/2;d(6)=-atan2(H(1,2),H(2,2));
          end
else
          d(4)=atan2(H(2,1),H(1,1));
          d(5)=atan2(-H(3,1),H(1,1)*cos(d(4))+H(2,1)*sin(d(4)));
          d(6)=atan2(H(3,2),H(3,3));
end
c=d;

%==========================================================================
function c = vts (H)

d(1)=H(1,4);d(2)=H(2,4);d(3)=H(3,4);
d(4)=atan(H(2,1),H(1,1));
d(5)=atan(-H(3,1),H(1,1)*cos(d(4))+H(2,1)*sin(d(4)));
d(6)=atan(H(3,2),H(3,3));
c=d;

%==========================================================================
function c = H (a)

A=[cos(a(4)),-sin(a(4)),0;sin(a(4)),cos(a(4)),0;0,0,1];
B=[cos(a(5)),0,sin(a(5));0,1,0;-sin(a(5)),0,cos(a(5))];
C=[1,0,0;0,cos(a(6)),-sin(a(6));0,sin(a(6)),cos(a(6))];
XYZ=[a(1);a(2);a(3)];
c=[A*B*C,XYZ;0 0 0 1];