//  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 [jacob] = CL_rot_pvJacobian(M, omega)
// Jacobian of a frame (position/velocity) transformation
//
// Calling Sequence
// jacob = CL_rot_pvJacobian(M, omega)
//
// Description
// <itemizedlist><listitem>
// <p>Computes the jacobian of the transformation (pos1,vel1) to (pos2,vel2): </p> 
// <p>pos2 = M * pos1 </p> 
// <p>vel2 = M * (vel1 - omega ^ pos1) </p>
// <p>jacob = d(pv2)/d(pv1), more precisely jacob(i,j,:) = d(pv2(i,:)/d(pv1(j,:))</p>
// <p></p></listitem>
// </itemizedlist>
//
// Parameters
// M: Transformation matrix from frame1 to frame2 (pos2 = M * pos1) (3x3xN)
// omega: Angular velocity vector of frame2 wrt frame1 with coordinates in frame1 (3xN)
// jacob: Jacobian of the transformation: (pos1,vel1) to (pos2,vel2) (6x6xN)
//
// Authors
// CNES - DCT/SB
//
// Examples
// M = CL_rot_angles2matrix(3, %pi/2)
// omega = [0; 0; 2*%pi/86400];
// [jacob] = CL_rot_pvJacobian(M, omega) 

// Declarations:

// Code:

// Check number of input arguments (2 expected)
if (argn(2) <> 2)
  CL__error("Invalid number of input arguments"); 
end

// Check size of input arguments
if (size(M,1) <> 3 | size(M,2) <> 3 | size(omega,1) <> 3)
  CL__error("Invalid size of input arguments");
end

// Check size of input arguments
if (size(omega,2)*9 <> size(M,"*"))
  CL__error("Invalid size of input arguments");
end



// W: 3x3xn matrix such that: W * X = omega ^ X 
// pos2 = M * pos1 
// vel2 = M * (vel1 - W * pos1) 
// => jacob = [  M,     0; 
//              -M*W,  M  ] 

n = size(omega,2); 

W = zeros(3,3,n);
W(1,2,1:n) = -omega(3,:);
W(1,3,1:n) =  omega(2,:);
W(2,1,1:n) =  omega(3,:);
W(2,3,1:n) = -omega(1,:);
W(3,1,1:n) = -omega(2,:);
W(3,2,1:n) =  omega(1,:);

jacob = zeros(6,6,n);
jacob(1:3,1:3,1:n) = M;
jacob(4:6,4:6,1:n) = M;
jacob(4:6,1:3,1:n) = -M*W;

endfunction