//  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] = CL_rot_angles2matrix(naxes,angles)
// Rotation angles to transformation matrix
// 
// Calling Sequence
// M = CL_rot_angles2matrix(naxes,angles)
//
// Description
// <itemizedlist><listitem>
// <p>Computes the transformation matrix <b>M</b> that results 
// from the combination of successive elementary rotations, each elementary rotation 
// being described by an axis number (1=x-axis, 2=y-axis, 3=z-axis) and an angle.</p> 
// <p></p></listitem>
// <listitem>
// <p><b>Notes:</b></p>
// <p> - M is the <b>frame transformation matrix</b> and its 
// definition is consistent with conventions on reference frame transformations.  </p>
// <p> - Let u be the coordinates of some vector in some reference frame. The coordinates of
// the rotated vector are given by: </p>
// <p> R(u) = M' * u (M transposed) </p>
// <p> - Conversely, let F1 be some reference frame, F2 be the reference frame obtained 
// after applying the successive rotations to F1, u1 the coordinates relative to F1 
// of some vector, then the coordinates relative to F2 of the same vector are 
// given by:  </p>
// <p> u2 = M * u1 </p>
// </listitem>
// </itemizedlist>
//
// Parameters
// naxes : Axis numbers: 1=x-axis, 2=y-axis or 3=z-axis (1xP or Px1)
// angles : Rotation angles around respective axes [rad] (PxN) 
// M : Transformation matrix (3x3xN).
//
// Authors
// CNES - DCT/SB
//
// See also
// CL_rot_quat2matrix
// CL_rot_matrix2quat
//
// Examples
// // 60deg rotation around X: 
// M = CL_rot_angles2matrix([1],[%pi/3])
//
// // 60deg rotation around X followed by 90deg around Y: 
// M = CL_rot_angles2matrix([1,2],[%pi/3;%pi/2])
//
// // Meaning of M:   
// M = CL_rot_angles2matrix(3, 10*%pi/180)
// u = [1;0;0]  // coordinates of vector u in initial frame
// v = M*u      // coordinates of vector u in final frame
// urot = M'*u  // image of rotation of u in initial frame
//

// Declarations:


// Code:

// computes the matrix for 1 pair angle/axis
function [M] = angle2matrix(naxis, ang)

  // NB: 'hypermat': elements must be given column by column

  N = size(ang,2); 
  cosa = cos(ang);
  sina = sin(ang);
  
  select naxis
    case 1
      M = hypermat([3,3,N], ...
          [ ones(ang); zeros(ang); zeros(ang); 
            zeros(ang); cosa; -sina; 
            zeros(ang); sina; cosa ]);
    case 2
      M = hypermat([3,3,N], ...
          [ cosa; zeros(ang); sina; 
            zeros(ang); ones(ang); zeros(ang); 
            -sina; zeros(ang); cosa ]);
    case 3
      M = hypermat([3,3,N], ...
          [ cosa; -sina; zeros(ang); 
            sina; cosa; zeros(ang); 
            zeros(ang); zeros(ang); ones(ang) ]);
  end

endfunction


// validity checking
Nrot = length(naxes);
N = size(angles,2); 

// same number of naxes as angles
if (size(angles,1) <> Nrot)
  CL__error("Invalid size for naxes or angles"); 
end

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

// check that axis numbers are equal to 1, 2 or 3
I = find(naxes <> 1 & naxes <> 2 & naxes <> 3); 
if (~isempty(I))
  CL__error("Invalid axis number");
end


M = angle2matrix(naxes(1),angles(1,:));

for k = 2 : Nrot
  M = angle2matrix(naxes(k),angles(k,:)) * M;
end


if (N == 1) 
  M = M(:,:,1); 
end

endfunction