//  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 [u,alpha] = CL_rot_quat2axAng(q)
// Quaternion to rotation axis and angle
//
// Calling Sequence
// [u,alpha] = CL_rot_quat2axAng(q)
//
// Description
// <itemizedlist><listitem>
// <p>Converts a quaternion into rotation axis and angle.</p> 
// <p></p></listitem>
// <listitem>
// <p><b>Notes:</b></p>
// <p> - The returned angle (alpha) is in the range [-pi, pi] (miminal rotation angle). </p>
// <p> - The returned rotation axis either has a norm equal to 1 or is [0;0;0]. It can be [0;0;0] 
// if the input quaternion corresponds to Identity, in which case the rotation axis is undefined. </p>
// <p> - If q is close to the null quaternion, the rotation axis and angle are set to %nan. </p>
// <p> - See <link linkend="Data types">Data types</link> for more details on quaternions. </p>
// </listitem>
// </itemizedlist>
//
// Parameters
// q : Quaternion (dim N)
// u : Rotation axis [ux; uy; uz] (3xN)
// alpha : Rotation angle [rad] (1xN)
//
// Authors
// CNES - DCT/SB
//
// See also
// CL_rot_axAng2quat
//
// Examples
// q = CL_rot_angles2quat([3], %pi/2); 
// [u, alpha] = CL_rot_quat2axAng(q)

// Declarations:


// Code:

if (typeof(q) <> "CLquat")
  CL__error("Wrong type of input argument. Quaternion expected");
end

// Justification: 
// q can be written: (qr=q.r, V=q.i) = (qr, ||V|| * u)
// We define alpha such that: 
// ||V|| = sin(alpha/2) and qr = cos(alpha/2)

// note1: u = %nan if q.i == 0 (i.e. nqi == 0)
// note2: alpha is between 0 and 2*pi as nqi >= 0
[u, nqi] = CL_unitVector(q.i);
alpha = 2 * atan(nqi, q.r);

eps = 2*%eps; 


// null axis => axis = [0;0;0] and angle = 0 (Identity)
nu2 = CL_dot(u); 
I = find(isnan(nu2)); 
u(:,I) = 0; 
alpha(I) = 0; 

// normal situation
// minimize |alpha] => put alpha in [-pi, pi]  
I = find(alpha > %pi)
if (I <> [])
  alpha(I) = alpha(I) - 2*%pi; 
end


// null quaternion (in relation to 1) => %nan for axis and angle
I = find(CL_dot([q.r; q.i]) < eps^2); 
u(:,I) = %nan;
alpha(I) = %nan; 


// Make sure %nan in input are correctly propagated
I = find(isnan(CL_dot([q.r; q.i]))); 
if (I <> [])
  u(:,I) = %nan; 
  alpha(I) = %nan; 
end


endfunction