//  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 [q, qdot] = CL_rot_matrix2quat(M, omega)
// Transformation matrix to quaternion
//
// Calling Sequence
// [q, qdot] = CL_rot_matrix2quat(M [, omega])
//
// Description
// <itemizedlist><listitem>
// <p>Computes the quaternion corresponding to a given transformation matrix.</p> 
// <p>Also computes the quaternion time derivative from the angular velocity vector (if is is present and not empty).</p> 
// <p></p></listitem>
// <listitem>
// <p><b>Notes:</b></p>
// <p> - See <link linkend="CL_rot_angles2matrix">CL_rot_angles2matrix</link> for 
// conventions regarding the definition of the transformation matrix. </p>
// <p> - See <link linkend="Data types">Data types</link> or <link linkend="CL_rot_defQuat">CL_rot_defQuat</link> for more details on quaternions. </p>
// </listitem>
// </itemizedlist>
//
// Parameters
// M: Transformation matrix (3x3xN)
// omega: Angular velocity vector. Can be empty. [rad/s] (3xN)
// q: Quaternion (dim N)
// qdot: Quaternion time derivative. Empty quaternion if not computed [-/s] (dim N or 0)
//
// Authors
// CNES - DCT/SB
//
// Bibliography
// 1) Mecanique Spatiale - CNES Cepadues 1995, Tome I, 7.2.2.3 Les quaternions
// 2) James R. Wertz, Spacecraft attitude determination and control (volume 73 of Astrophyisics and Space Science Library), D. Reidel Publishing Company, 1980, appendix D-E
//
// See also
// CL_rot_defQuat
// CL_rot_quat2matrix
//
// Examples
// ang = CL_deg2rad(10);
// M = CL_rot_angles2matrix(3,ang) // rotation around Z axis
// q = CL_rot_matrix2quat(M)
// M2 = CL_rot_quat2matrix(q)    //same as M1

// Declarations:


// Code:
if (~exists("omega", "local")); omega=[]; end

// cder: %t if derivative is computed
cder = %t;
// number of arguments (left, right)
[lhs, rhs] = argn();
if (lhs <= 1); cder = %f; end
if (rhs <= 1 | omega == []); cder = %f; end

// default outputs
q = CL__defQuat([], []); 
qdot = q; 

// return [] if arguments are empty
if (M == [] & omega == [])
  return; 
end

if (typeof(M) == "constant")
  // matrix
  [nr, nc] = size(M);
  n = 1;
else 
  // hypermatrix
  [nr, nc, n] = size(M);
end

if (nr <> 3 | nc <> 3)
  CL__error("Invalid matrix size");
end

if (omega <> [])
  [nr, nc] = size(omega); 
  if (nr <> 3 | nc <> n)
     CL__error("Invalid size for omega");
  end
end


// aij: column vectors 
a11 = matrix(M(1,1,:),-1,1);
a12 = matrix(M(1,2,:),-1,1);
a13 = matrix(M(1,3,:),-1,1);
a21 = matrix(M(2,1,:),-1,1);
a22 = matrix(M(2,2,:),-1,1);
a23 = matrix(M(2,3,:),-1,1);
a31 = matrix(M(3,1,:),-1,1);
a32 = matrix(M(3,2,:),-1,1);
a33 = matrix(M(3,3,:),-1,1);

// q1, q2, q3, q4: not the usual CelestLab convention!
// Initialized to %nan in case ...
q1 = %nan * ones(n,1);
q2 = %nan * ones(n,1);
q3 = %nan * ones(n,1);
q4 = %nan * ones(n,1);

// V: size = nx4
V = real(sqrt(1 + [a11-a22-a33, a22-a33-a11, a33-a11-a22, a11+a22+a33]));

// jmax: column number (1,2,3,4) for which V is maximum
[Vmax, jmax] = max(V, "c");

// Vmax == 0 => undefined
I = find(Vmax == 0); 
Vmax(I) = %nan; 

I = find(jmax == 1); 
if (I <> [])
  q1(I) = 0.5 * Vmax(I);
  q2(I) = (a12(I)+a21(I)) ./ (2*Vmax(I));
  q3(I) = (a13(I)+a31(I)) ./ (2*Vmax(I));
  q4(I) = (a23(I)-a32(I)) ./ (2*Vmax(I));
end

I = find(jmax == 2); 
if (I <> [])
  q2(I) = 0.5 * Vmax(I);
  q1(I) = (a12(I)+a21(I)) ./ (2*Vmax(I));
  q3(I) = (a23(I)+a32(I)) ./ (2*Vmax(I));
  q4(I) = (a31(I)-a13(I)) ./ (2*Vmax(I));
end

I = find(jmax == 3); 
if (I <> [])
  q3(I) = 0.5 * Vmax(I);
  q1(I) = (a13(I)+a31(I)) ./ (2*Vmax(I));
  q2(I) = (a23(I)+a32(I)) ./ (2*Vmax(I));
  q4(I) = (a12(I)-a21(I)) ./ (2*Vmax(I));
end

I = find(jmax == 4); 
if (I <> [])
  q4(I) = 0.5 * Vmax(I);
  q1(I) = (a23(I)-a32(I)) ./ (2*Vmax(I));
  q2(I) = (a31(I)-a13(I)) ./ (2*Vmax(I));
  q3(I) = (a12(I)-a21(I)) ./ (2*Vmax(I));
end

q = CL__defQuat(q4', [q1, q2, q3]');

if (cder)
  q_omega = CL__defQuat(zeros(omega(1,:)), omega); 
  qdot = 0.5 * q_omega * q; 
end


endfunction