//  Copyright (c) CNES  2008
//
//  This software is part of CelestLab, a CNES toolbox for Scilab
//
//  This software is governed by the CeCILL  license under French law and
//  abiding by the rules of distribution of free software.  You can  use,
//  modify and/ or redistribute the software under the terms of the CeCILL
//  license as circulated by CEA, CNRS and INRIA at the following URL
//  'http://www.cecill.info'.

function [angles1, angles2, Inok] = CL__rot_qM2angles(obj, naxes)
// Quaternion or frame transformation matrix to rotation angles
//
// Calling Sequence
// [angles1, angles2, Inok] = CL__rot_qM2angles(q, naxes)
// [angles1, angles2, Inok] = CL__rot_qM2angles(M, naxes)
//
// Description
// <itemizedlist><listitem>
// Computes the rotation angles corresponding to a quaternion or a frame transformation matrix. 
// Each axis is described by an axis number: 1=x-axis, 2=y-axis, 3=z-axis).
// <p>For Cardan angles, singularities occur when the second angle is close to 
// -pi/2 or +pi/2. <p>
// <p>For Euler angle singularities occur when the
// second angle is close to 0 or pi. This implies that the identity 
// rotation is always singular for Euler angles! </p>
// <p>When singularities occur, the 3rd angle is forced to 0 (for the 1st solution). 
// The result is then approximate, and the corresponding indices 
// are stored in Inok. </p>
// </listitem>
// </itemizedlist>
//
// Parameters
// naxes : Axis numbers: 1=x-axis, 2=y-axis or 3=z-axis (1x3 or 3x1)
// q : Quaternion (that defines the rotation) (4xN).
// M : Frame transformation matrix (3x3xN).
// angles1 : Rotation angles around respective axes - solution 1 [rad] (3xN) 
// angles2 : Rotation angles around respective axes - solution 2 [rad] (3xN) 
// Inok : Indices for which the results are approximate (1xN) 
//
// Authors
// CNES - DCT/SB
//
// See also
// CL_rot_angles2quat
// CL_rot_angles2matrix
// CL_rot_matrix2angles

// Declarations:

function [vectRot] = applyRot(obj, vect)
// applies rotation to vector "vect". 
// rotation given by quaternion or frame transformation matrix
  
  if(typeof(obj) == "CLquat")
    vectRot = CL_rot_rotVect(obj,vect);
  elseif (typeof(obj) == "constant" | typeof(obj) == "hypermat")
    vectRot = obj' * vect;
  else
    CL__error("Frame transformation matrix or quaternion expected");
  end
  
endfunction

// second solution for cardan angles
function [angles2] = solution2_cardan(angles)
  sgn = ones(angles); 
  I = find(angles < 0); 
  sgn(I) = -1; 
  angles2 = [ -sgn(1,:) .* (%pi - abs(angles(1,:))); 
               sgn(2,:) .* (%pi - abs(angles(2,:))); 
              -sgn(3,:) .* (%pi - abs(angles(3,:))) ]; 
endfunction

// second solution for euler angles
function [angles2] = solution2_euler(angles)
  sgn = ones(angles); 
  I = find(angles < 0); 
  sgn(I) = -1; 
  angles2 = [ -sgn(1,:) .* (%pi - abs(angles(1,:))); 
              -angles(2,:); 
              -sgn(3,:) .* (%pi - abs(angles(3,:)))]  
endfunction


// Code:

// validity checking
if(typeof(obj) == "CLquat")
  N = size(obj);

elseif(typeof(obj) == "constant" | typeof(obj) == "hypermat")
  sizeobj = size(obj);
  if(sizeobj(1) <> 3 | sizeobj(2) <> 3) 
    CL__error("Matrix should be (3x3xN)"); 
  end
  N = 1; 
  if (typeof(obj) == "hypermat"); N = sizeobj(3); end

else
  CL__error("Frame transformation or quaternion expected");
end 

if ( length(naxes) <> 3 | ..
     find(naxes <> 1 & naxes <> 2 & naxes <> 3) <> [] )
  CL__error("Invalid axes"); 
end

Id = eye(3,3); 
angles = %nan * ones(3,N);
Inok = ones(1,N); 
vmax = 1 - 1.e-10; // limit value for approximate results


// Cardan 
// Exemple: Ordre des rotation XYZ (phi,theta,psi)
// K = r(k) s'exprime dans la base (i,j,k) : 
// [ sin(theta) ; -sin(phi)*cos(theta) ; cos(phi)*cos(theta) ]
// donc phi = atan( -K(2) , K(3) )
//
// i s'exprime dans la base (I=R(i), J=R(j), K=R(k)) : 
// [ cos(theta)*cos(psi) ; -cos(theta)*sin(psi) ; sin(theta) ]
// donc theta = asin( i(3) ) 
// et theta sera compris entre -pi/2 et pi/2
// et psi = atan( -i(2) , i(1) )
//
// Il y a singularite si theta tend vers +/- pi/2 donc si abs(i(3)) tend vers 1
// Dans ce cas on pose theta = +/- pi/2 et psi = 0 

// Euler:
// Exemple: Ordre des rotation ZXZ (phi,theta,psi)
// R(k)=K s'exprime dans la base (i,j,k) : 
// [ sin(phi)*sin(theta), -cos(phi)*sin(theta) ; cos(theta) ]
// donc phi = atan( K(1) , -K(2) ) 
//
// k s'exprime dans la base (I=R(i), J=R(j), K=R(k)) : 
// [ sin(theta)*sin(psi) ; sin(theta)*cos(psi) ; cos(theta) ]
// donc theta = acos( K(3) ) 
// et theta est compris entre 0 et pi
// et psi = atan( K(1) , K(2) )
//
// Il y a singularite si theta tend vers 0 ou pi donc si abs(k(3)) tend vers 1
// Dans ce cas on pose theta = 0 ou pi, et psi = 0 


if (naxes(1) <> naxes(2) & naxes(1) <> naxes(3) & ..
    naxes(2) <> naxes(3))
  // CARDAN: all axes are different
  n1 = naxes(1);
  n2 = naxes(2);
  n3 = naxes(3);
  // sgn: 1 if (n1,n2,n3) direct, -1 otherwise. 
  sgn = sign(CL_rMod(n2 - n1, -1.5, 1.5)); 

  X3 = applyRot(obj, Id(:,n3));  // R(n3 axis)
  cond_ok = abs(X3(n1,:)) < vmax; 

  I = find(cond_ok); 
  if (I <> [])
    x1 = applyRot(obj', Id(:,n1)); // R-1(n1 axis)
    angles(1,I) = atan(-sgn * X3(n2,I), X3(n3,I));
    angles(2,I) = real(asin(sgn * X3(n1,I))); 
    angles(3,I) = atan(-sgn * x1(n2,I), x1(n1,I));  
  end

  I = find(~cond_ok); 
  if (I <> []) // approximate results
    X2 = applyRot(obj, Id(:,n2)); // R(n2 axis)
    angles(3,I) = 0 * CL_dot(X2(:,I)); // = 0 or %nan (%nan correctly propagated)
    angles(2,I) = real(asin(sgn * X3(n1,I))); // ~ +/- %pi/2
    angles(1,I) = atan(sgn * X2(n3,I), X2(n2,I)); 
  end

  Inok = find(~cond_ok); 

  // final results 
  angles1 = angles;
  angles2 = solution2_cardan(angles1);


elseif (naxes(3) == naxes(1) & naxes(1) <> naxes(2) )
  // EULER: axes 1 and 3 are the same
  n1 = naxes(1);
  n2 = naxes(2);
  n3 = pmodulo(n2+(n2-n1)-1,3) + 1; // missing axis
  // sgn: 1 if (n1,n2,n3) direct, -1 otherwise. 
  sgn = sign(CL_rMod(n2 - n1, -1.5, 1.5)); 

  X1 = applyRot(obj, Id(:,n1));  // R(n1 axis)
  cond_ok = abs(X1(n1,:)) < vmax;

  I = find(cond_ok);
  if (I <> [])
    x1 = applyRot(obj', Id(:,n1)); // R-1(n1 axis)
    angles(1,I) = atan(X1(n2,I), -sgn * X1(n3,I));
    angles(2,I) = real(acos(X1(n1,I))); 
    angles(3,I) = atan(x1(n2,I), sgn * x1(n3,I));
  end

  I = find(~cond_ok);
  if (I <> []) // approximate results
    X2 = applyRot(obj, Id(:,n2));  // R(n2 axis)
    angles(3,I) = 0 * CL_dot(X2(:,I)); // = 0 or %nan (%nan correctly propagated)
    angles(2,I) = real(acos(X1(n1,I))); // ~0 or ~pi
    angles(1,I) = atan(sgn * X2(n3,I), X2(n2,I)); 
  end
  
  Inok = find(~cond_ok); 

  // final results 
  angles1 = angles;
  angles2 = solution2_euler(angles1);


else
  CL__error("Invalid axis numbers");
end

endfunction