//  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 [M, omega] = CL_rot_quat2matrix(q, qdot)
// Quaternion to transformation matrix
//
// Calling Sequence
// [M, omega] = CL_rot_quat2matrix(q, [, qdot])
//
// Description
// <itemizedlist><listitem>
// <p>Computes the transformation matrix that corresponds to a given quaternion.</p> 
// <p>Also computes the angular velocity vector from the quaternion derivative (if it 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
// q: Quaternion (dim N)
// qdot: Quaternion time derivative. Can be empty (dim N)
// M: Transformation matrix (3x3xN)
// omega: Angular velocity vector. Empty if not computed (3xN)
//
// Bibliography
// 1) Mecanique Spatiale - CNES Cepadues 1995, Tome I, 7.2.2.3 Les quaternions, eq.15
// 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
//
// Authors
// CNES - DCT/SB
//
// See also
// CL_rot_defQuat
// CL_rot_matrix2quat
//
// Examples
// ang = CL_deg2rad(10)
// M1 = CL_rot_angles2matrix(3,ang)
// q1 = CL_rot_matrix2quat(M1)
// M2 = CL_rot_quat2matrix(q1)    // same as M1
//

// Declarations:


// Code:
// qdot initialized to [], but would not be used
if (~exists("qdot", "local")); qdot=[]; 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 | size(qdot) == 0); cder = %f; end

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

// default outputs
M = []; 
omega = []; 

n = size(q);

// return [] if arguments are empty
if (n == 0)
  return; 
end

// q1, q2, q3, q4: not the usual CelestLab convention!
q4 = q.r;
q1 = q.i(1,:);
q2 = q.i(2,:);
q3 = q.i(3,:);

M = hypermat([3, 3, n], ...
       [ q1.^2 - q2.^2 - q3.^2 + q4.^2; 2*(q1.*q2 - q3.*q4); ...
         2*(q1.*q3 + q2.*q4); 2*(q1.*q2 + q3.*q4); ...
         (-q1.^2 + q2.^2 - q3.^2 + q4.^2); ...
         2*(q2.*q3 - q1.*q4); 2*(q1.*q3 - q2.*q4); ...
         2*(q2.*q3 + q1.*q4); ...
         (-q1.^2 - q2.^2 + q3.^2 + q4.^2)]);

if (n == 1) 
  M = M(:,:,1);  // avoids hypermat if only one quaternion
end;  

if (cder)
  if (typeof(qdot) <> "CLquat")
    CL__error("Wrong type for argument ''qdot''. Quaternion expected");
  end
  if (size(qdot) <> n)
    CL__error("Wrong size for argument ''qdot''");
  end

  q_omega = 2 * qdot * q';
  omega = q_omega.i; 
end

endfunction