t2x.m

function x=t2x(T,str)

% x=t2x(T,str);
% 
% Converts transformation matrix T between B and A coordinate
% frames into a generalized position vector x, which contains
% position and orientation vectors of B with respect to A.
% Orientation can be expressed with quaternions, euler angles
% (xyz or zxz convention), unit vector and rotation angle.
% Also both orientation and position can be expressed with
% Denavitt-Hartemberg parameters.
% 
% ---------------------------------------------------------------------------
% 
% The transformation matrix T between B and A coordinate
% frames is a 4 by 4 matrix such that:
% T(1:3,1:3) = Orientation matrix between B and A = unit vectors
%              of x,y,z axes of B expressed in the A coordinates.
% T(1:3,4)   = Origin of B expressed in A coordinates.
% T(4,1:3)   = zeros(1,3)
% T(4,4)     = 1
%
% ---------------------------------------------------------------------------
% 
% The generalized position vector x contains the origin of B
% expressed in the A coordinates in the first four entries,
% and orientation of B with respect to A in the last four entries.
% In more detail, its shape depends on the value of str as 
% specified below :
% 
% ---------------------------------------------------------------------------
% 
% str='van' : UNIT VECTOR AND ROTATION ANGLE 
% 
%           [ Ox ]     origin of the B coordinate frame    
%  x(1:4) = [ Oy ]     with respect to A.
%           [ Oz ]     
%           [  1 ]     
% 
%           [ Vx ]     Vx,Vy,Vz = unit vector respect to A, 
%  x(5:8) = [ Vy ]     which B is rotated about.
%           [ Vz ]     
%           [ Th ]     Th = angle which B is rotated (-pi,pi].
% ---------------------------------------------------------------------------
% 
% str='qua' : UNIT QUATERNION 
% 
%           [ Ox ]     origin of the B coordinate frame    
%  x(1:4) = [ Oy ]     with respect to A.
%           [ Oz ]     
%           [  1 ]     
% 
%           [ q1 ]     q1,q2,q3 = V*sin(Th/2)
%  x(5:8) = [ q2 ]     q0 = cos(Th/2) where :
%           [ q3 ]     V = unit vector respect to A, which B is 
%           [ q0 ]     rotated about, Th = angle which B is rotated (-pi,pi].
% ---------------------------------------------------------------------------
% 
% str='erp' : EULER-RODRIGUEZ PARAMETERS
% 
%           [ Ox ]     origin of the B coordinate frame    
%  x(1:4) = [ Oy ]     with respect to A.
%           [ Oz ]     
%           [  1 ]     
% 
%           [ r1 ]     r1,r2,r3 = V*tan(Th/2), where :
%  x(5:8) = [ r2 ]     V = unit vector with respect to A, which B is 
%           [ r3 ]     rotated about.
%           [  0 ]     Th = angle which B is rotated (-pi,pi) (<> pi).
% ---------------------------------------------------------------------------
% 
% str='rpy' : ROLL, PITCH, YAW ANGLES (euler x-y-z convention) 
% 
%           [ Ox ]     origin of the B coordinate frame    
%  x(1:4) = [ Oy ]     with respect to A.
%           [ Oz ]     
%           [  1 ]     
% 
%            [ r ]     r = roll angle  ( fi    (-pi,pi], about x,          )
%  x(5:8) =  [ p ]     p = pitch angle ( theta (-pi,pi], about y, <> +-pi/2)
%            [ y ]     y = yaw angle   ( psi   (-pi,pi], about z,          )
%            [ 0 ]     
% ---------------------------------------------------------------------------
% 
% str='rpm' : ROTATION, PRECESSION, MUTATION ANGLES (euler z-x-z convention) 
% 
%           [ Ox ]     origin of the B coordinate frame    
%  x(1:4) = [ Oy ]     with respect to A.
%           [ Oz ]     
%           [  1 ]     
% 
%            [ r ]     r = rotation angle   ( (-pi,pi] ,about z           )
%  x(5:8) =  [ p ]     p = precession angle ( (-pi,pi] ,about x , <> 0,pi )
%            [ y ]     y = mutation angle   ( (-pi,pi] ,about z           )
%            [ 0 ]     
% ---------------------------------------------------------------------------
% 
% str='dht' : DENAVITT-HARTEMBERG PARAMETERS 
% 
%            [ b ]                [ a ]     this four-parameter 
%  x(1:4) =  [ d ] ,   x(5:8) =   [ t ] ,   description does not involve
%            [ 0 ]                [ 0 ]     a loss of information if and 
%            [ 0 ]                [ 0 ]     only if T has this shape:
% 
%           [    ct   -st     0     b ]     where : 
%  T =      [ ca*st ca*ct   -sa -d*sa ]     
%           [ sa*st sa*ct    ca  d*ca ]     sa = sin(a), ca = cos(a)
%           [     0     0     0     1 ]     st = sin(t), ct = cos(t)
% ---------------------------------------------------------------------------
% 
% Example (see also x2t):
% x=[rand(3,1);1;rand(3,1);0];x-t2x(x2t(x,'rpm'),'rpm')
% 
%
% Giampiero Campa 1/11/96
% 

% ---------------------------------------------------------------------------
% UNIT VECTOR AND ROTATION ANGLE 

if [ str=='van' size(T)==[4 4] ],

O=T(1:3,4);
R=T(1:3,1:3);
d=round(.5*(trace(R)-1)*1e12)/1e12;

if d==1,
   v=[0 0 1]';
   th=0;

elseif d==-1,
    v0=sum(R'+eye(3,3))';

    if v0 == 0 
	 v0=R(:,2)+R(:,3)+[0 1 1]';
    end;

    if v0 == 0 
	 v0=R(:,3)+[0 0 1]';
    end;

    v=v0/norm(v0);
    th=pi;

else 

   sg=(vp([1 0 0]',R(:,1))+vp([0 1 0]',R(:,2))+vp([0 0 1]',R(:,3)));

    if norm(sg) < 1e-12
        disp(' ');
        disp('T2x warning: det(R)<>1, unit vector assumed to be [0 0 1]''.');
        disp(' ');
        sg=[0 0 1]';
    end

   v=sg/norm(sg);
   th=atan2(norm(sg)/2,d);

end

x=[O;1;v;th];

% ---------------------------------------------------------------------------
% UNIT QUATERNION 

elseif [ str=='qua' size(T)==[4 4] ],

O=T(1:3,4);
R=T(1:3,1:3);
d=round(.5*(trace(R)-1)*1e12)/1e12;

if d==1,
   v=[0 0 1]';
   th=0;

elseif d==-1,
    v0=sum(R'+eye(3,3))';

    if v0 == 0 
	v0=R(:,2)+R(:,3)+[0 1 1]';
    end;

    if v0 == 0 
	v0=R(:,3)+[0 0 1]';
    end;

    v=v0/norm(v0);
    th=pi;

else 

   sg=(vp([1 0 0]',R(:,1))+vp([0 1 0]',R(:,2))+vp([0 0 1]',R(:,3)));

    if norm(sg) < 1e-12
        disp(' ');
        disp('T2x warning: det(R)<>1, unit vector assumed to be [0 0 1]''.');
        disp(' ');
        sg=[0 0 1]';
    end

   v=sg/norm(sg);
   th=atan2(norm(sg)/2,d);
   
end

q=v*sin(th/2);
q0=cos(th/2);

x=[O;1;q;q0];

% ---------------------------------------------------------------------------
% EULER-RODRIGUEZ PARAMETERS

elseif [ str=='erp' size(T)==[4 4] ],

O=T(1:3,4);
R=T(1:3,1:3);
d=round(.5*(trace(R)-1)*1e12)/1e12;

if d==1,
   v=[0 0 1]';
   th=0;

elseif d==-1,
    v0=sum(R'+eye(3,3))';

    if v0 == 0 
	v0=R(:,2)+R(:,3)+[0 1 1]';
    end;

    if v0 == 0 
	v0=R(:,3)+[0 0 1]';
    end;

    v=v0/norm(v0);
    th=pi;

else 

   sg=(vp([1 0 0]',R(:,1))+vp([0 1 0]',R(:,2))+vp([0 0 1]',R(:,3)));

    if norm(sg) < 1e-12
        disp(' ');
        disp('T2x warning: det(R)<>1, unit vector assumed to be [0 0 1]''.');
        disp(' ');
        sg=[0 0 1]';
    end

   v=sg/norm(sg);
   th=atan2(norm(sg)/2,d);
   
end

p=v*tan(th/2);
x=[O;1;p;0];

% ---------------------------------------------------------------------------
% ROLL, PITCH, YAW ANGLES (euler x-y-z convention) 

elseif [ str=='rpy' size(T)==[4 4] ],

O=T(1:3,4);
R=T(1:3,1:3);
d=round([0 0 1]*R(:,1)*1e12)/1e12;

if d==1,
   y=atan2([0 1 0]*R(:,2),[1 0 0]*R(:,2));
   p=-pi/2;
   r=-pi/2;

elseif d==-1
   y=atan2([0 1 0]*R(:,2),[1 0 0]*R(:,2));
   p=pi/2;
   r=pi/2;

else 
   sg=vp([0 0 1]',R(:,1));
   j2=sg/sqrt(sg'*sg);
   k2=vp(R(:,1),j2);

   r=atan2(k2'*R(:,2),j2'*R(:,2));
   p=atan2(-[0 0 1]*R(:,1),[0 0 1]*k2);
   y=atan2(-[1 0 0]*j2,[0 1 0]*j2);
end

y1=y+(1-sign(y)-sign(y)^2)*pi;
p1=p+(1-sign(p)-sign(p)^2)*pi;
r1=r+(1-sign(r)-sign(r)^2)*pi;

% takes smaller values of angles

if norm([y1 p1 r1]) < norm([y p r])
    x=[O;1;r1;-p1;y1;0];
else
    x=[O;1;r;p;y;0];
end

% ---------------------------------------------------------------------------
% ROTATION, PRECESSION, MUTATION ANGLES (euler z-x-z convention) 

elseif [ str=='rpm' size(T)==[4 4] ],

O=T(1:3,4);
R=T(1:3,1:3);
d=round([0 0 1]*R(:,3)*1e12)/1e12;

if d==1,
   m=0;
   p=0;
   r=atan2([0 1 0]*R(:,1),[1 0 0]*R(:,1));

elseif d==-1
   m=0;
   p=pi;
   r=atan2([0 1 0]*R(:,1),[1 0 0]*R(:,1));

else 
   sg=vp([0 0 1]',R(:,3));
   i2=sg/norm(sg);
   j2=vp(R(:,3),i2);

   m=atan2(j2'*R(:,1),i2'*R(:,1));
   p=atan2([0 0 1]*j2,[0 0 1]*R(:,3));
   r=atan2([0 1 0]*i2,[1 0 0]*i2);
   
end

r1=r+(1-sign(r)-sign(r)^2)*pi;
p1=p;
m1=m+(1-sign(m)-sign(m)^2)*pi;

% takes the smaller values of angles

if norm([r1 p1 m1]) < norm([r p m])
    x=[O;1;r1;-p1;m1;0];
else
    x=[O;1;r;p;m;0];
end

% ---------------------------------------------------------------------------
% DENAVITT-HARTEMBERG PARAMETERS 

elseif [ str=='dht' size(T)==[4 4] ],

% b calculation
b=T(1,4);

% d calculation
if norm(T(3,3)) > norm(T(2,3))
    d=T(3,4)/T(3,3);
else
    d=T(2,4)/T(2,3);
end

% alfa and theta computation by mean of euler-zxz angles
R=T(1:3,1:3);
dl=round([0 0 1]*R(:,3)*1e12)/1e12;

if dl==1,
   th=0;
   alfa=0;
   n=atan2([0 1 0]*R(:,1),[1 0 0]*R(:,1));
elseif dl==-1
   th=0;
   alfa=pi;
   n=atan2([0 1 0]*R(:,1),[1 0 0]*R(:,1));
else 
   sg=vp([0 0 1]',R(:,3));
   i2=sg/norm(sg);
   j2=vp(R(:,3),i2);

   th=atan2(j2'*R(:,1),i2'*R(:,1));
   alfa=atan2([0 0 1]*j2,[0 0 1]*R(:,3));
   n=atan2([0 1 0]*i2,[1 0 0]*i2);
end

th1=th+(1-sign(th)-sign(th)^2)*pi;
alfa1=alfa;
n1=n+(1-sign(n)-sign(n)^2)*pi;

% takes the smaller values of angles

if norm([th1 alfa1 n1]) < norm([th alfa n])
   x=[b;d;0;0;-alfa1;th1;0;0];
else
   x=[b;d;0;0;alfa;th;0;0];
end

% ---------------------------------------------------------------------------
% OTHER STRING

else

disp('   ');
disp('   x=T2x(T,str)');
disp('   where T is a 4 by 4 matrix (see help for details)');
disp('   and str can be : ''van'',''qua'',''erp'',''rpy'',''rpm'',''dht''. ');
disp('   ');

end


function z=vp(x,y)

% z=vp(x,y); z = 3d cross product of x and y
% vp(x) is the 3d cross product matrix : vp(x)*y=vp(x,y).
%
% by Giampiero Campa.  

z=[  0    -x(3)   x(2);
    x(3)    0    -x(1);
   -x(2)   x(1)    0   ];

if nargin>1, z=z*y; end