//  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 [omega] = CL_rot_angVelocity(naxes,angles,angles_der)
// Angular velocity vector from rotation angles and derivatives
//
// Calling Sequence
// [omega] = CL_rot_angVelocity(naxes,angles,angles_der)
//
// Description
// <itemizedlist><listitem>
// <p>Computes the angular velocity vector resulting from a combination 
// of successive rotations.</p>
// <p>The rotation axes are defined by numbers: 1=x-axis, 2=y-axis, 3=z-axis.</p>
// </listitem>
// <listitem>
// <p><b>Important: </b></p>
// <p>Let's note: </p>
// <p>F1: the "initial" frame, </p>
// <p>F2: the image of F1 by the successive rotations, </p>
// <p>Then: </p>
// <p>the angular velocity vector as computed is omega(F2/F1), with coordinates given in F1.</p>
// </listitem>
// <listitem>
// <p>See <link linkend="Data types">Data types</link> for more details 
// on the definition of angular velocity vectors and frame
// transformation matrices.</p> 
// </listitem>
// </itemizedlist>
//
// Parameters
// naxes : Axes numbers: 1=x-axis, 2=y-axis or 3=z-axis (1xP or Px1)
// angles : Rotation angles around respective axes [rad] (PxN) 
// angles_der : Time derivatives of angles [rad/s] (PxN) 
// omega : Angular velocity vector (see above for definition) [rad/s] (3xN)
//
// Authors
// CNES - DCT/SB
//
// See also
// CL_rot_angles2matrix
//
// Examples
// // Example 1
// naxes = [3,1,3]; // Euler angles
// angles = [%pi/4; %pi/8; %pi/6];
// angles_der = [0.1; -0.2; 0.3];
// omega = CL_rot_angVelocity(naxes,angles,angles_der);
//
// // Example 2
// naxes = [1,3,1,3]; // XZXZ
// angles = [%pi/4; %pi/8; %pi/6; %pi/10];
// angles_der = [0.1; -0.2; 0.3; 0.5];
// omega = CL_rot_angVelocity(naxes,angles,angles_der);

// Declarations:


// Code:

// -------------------------------------------------
// Specific formulas for some combinations of 3 rotations
// (more efficient, less memory used) 
// naxes: (1x3)
// angles, angles_der: (3xN)
// omega: (3xN), [] if unrecognized sequence
// -------------------------------------------------
function [omega] = angvel_3angles(naxes, angles, angles_der)

  omega = []; 

  // recognised combinations of axes
  // NB: 1st axis: in the order: 1,2,3,1,2,3,... 
  rot_sequences = [[1,2,3]; [2,3,1]; [3,1,2]; 
                   [1,3,2]; [2,1,3]; [3,2,1];
                   [1,2,1]; [2,3,2]; [3,1,3]; 
                   [1,3,1]; [2,1,2]; [3,2,3]];

  // find sequence 
  I = vectorfind(rot_sequences, naxes);

  if (I == [])
    return; // <== RETURN
  end

  // note: formulas are computed for the 1st sequence of 
  // each row ([1, xxx, xxx]). 
  // Other cases => circular permutation

  a1dot = angles_der(1,:); 
  a2dot = angles_der(2,:);
  a3dot = angles_der(3,:);

  cos1 = cos(angles(1,:));
  sin1 = sin(angles(1,:)); 
  cos2 = cos(angles(2,:));
  sin2 = sin(angles(2,:)); 

  i1 = rot_sequences(I,1); 
  i2 = modulo(i1,3) + 1; 
  i3 = modulo(i2,3) + 1; 

  omega = zeros(3, size(angles,2));

  // Case [1,3,1], [2,1,2], [3,2,3]
  if (I >= 10) 
    omega(i1,:) = a1dot + cos2.*a3dot;
    omega(i2,:) = -sin1.*a2dot + cos1.*a3dot.*sin2;
    omega(i3,:) = cos1.*a2dot + sin1.*a3dot.*sin2;

  // Case [1,2,1], [2,3,2], [3,1,3]
  elseif (I >= 7)
    omega(i1,:) = a3dot.*cos2 + a1dot;
    omega(i2,:) = cos1.*a2dot + sin1.*a3dot.*sin2;
    omega(i3,:) = sin1.*a2dot - cos1.*a3dot.*sin2;

  // Case [1,3,2]; [2,1,3]; [3,2,1]
  elseif (I >= 4)
    omega(i1,:) = a1dot - sin2.*a3dot;
    omega(i2,:) = cos1.*a3dot.*cos2 - sin1.*a2dot;
    omega(i3,:) = sin1.*a3dot.*cos2 + cos1.*a2dot;

  // Case [1,2,3], [2,3,1], [3,1,2]
  else // (I >= 1)
    omega(i1,:) = a1dot + sin2.*a3dot;
    omega(i2,:) = -sin1.*a3dot.*cos2 + cos1.*a2dot;
    omega(i3,:) = cos1.*a3dot.*cos2 + sin1.*a2dot;
      
  end
  
endfunction


// -------------------------------------------------
// Specific formulas for axes: 3 1 3 1
// (more efficient, less memory used) 
// naxes: (1x4)
// angles, angles_der: (4xN)
// omega: (3xN)
// -------------------------------------------------
function [omega] = angvel_3131(naxes, angles, angles_der)

  a1dot = angles_der(1,:);
  a2dot = angles_der(2,:);
  a3dot = angles_der(3,:);  
  a4dot = angles_der(4,:); 

  cos1 = cos(angles(1,:));    
  sin1 = sin(angles(1,:)); 
  cos2 = cos(angles(2,:));    
  sin2 = sin(angles(2,:)); 
  cos3 = cos(angles(3,:));    
  sin3 = sin(angles(3,:)); 

  omega = zeros(3, size(angles,2)); 

  omega(1,:) = cos3.*a4dot.*cos1 - cos2.*a4dot.*sin3.*sin1 + sin2.*a3dot.*sin1 + a2dot.*cos1;
  omega(2,:) = cos3.*a4dot.*sin1 - sin2.*a3dot.*cos1 + a2dot.*sin1 + cos2.*a4dot.*sin3.*cos1;
  omega(3,:) = a1dot + a3dot.*cos2 + sin3.*a4dot.*sin2;
  
endfunction


// -------------------------------------------------
// General case (any not empty rotation sequence)
// naxes: (1xP)
// angles, angles_der: (PxN)
// omega: (3xN)
// -------------------------------------------------
function [omega] = angvel_gene(naxes, angles, angles_der) 

  n = length(naxes); 
  Id = eye(3,3); 

  u = Id(:,naxes(n)); 
  omega =  CL_dMult(angles_der(n,:), u); 
  
  for i = n-1 : -1 : 1
    M = CL_rot_angles2matrix(naxes(i), angles(i,:))'; 
    u = Id(:,naxes(i)); 
    omega = CL_dMult(angles_der(i,:), u) + M * omega; 
  end

endfunction


// -------------------------------------------------
// MAIN
// -------------------------------------------------

// Validity checking

naxes = matrix(naxes, 1, -1); // => row vector
Nrot = length(naxes);

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

N = size(angles,2);
if (size(angles_der,2) <> N)
  CL__error("Invalid size for angles or angles_der"); 
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 numbers");
end



// Note on algorithms :
// There are specific formulas to increase efficiency :
//  - every combination of 3 rotations.
//  - an additional case [3,1,3,1] used in CelestLab 
// For all the other cases, there is a generic formulation 
// which works for all cases (but is slightly less efficient)


omega = []; 

if (Nrot == 0) 
  return; 

elseif (Nrot == 1)
  // 1 rotation => specific formula
  omega = zeros(3,N);
  omega(naxes,:) = angles_der;

elseif (Nrot == 3)
  // 3 rotations => specific formulas
  // NB: omega may be []
  omega = angvel_3angles(naxes, angles, angles_der);

elseif (Nrot == 2)
  // 2 rotations => add 3rd rotation with 0 angle
  // NB: omega may be []
  naxes = [naxes, naxes(1)];
  angles = [angles; zeros(angles(1,:))];
  angles_der = [angles_der; zeros(angles_der(1,:))];
  omega = angvel_3angles(naxes, angles, angles_der);

elseif (isequal(naxes, [3,1,3,1]))
  // Case [3,1,3,1] => specific formula
  omega = angvel_3131(naxes, angles, angles_der); 

end  

// if omega == [] => not yet computed
if (omega == [])
  omega = angvel_gene(naxes, angles, angles_der);   
end
    
    
endfunction