//  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 [pos2,vel2,jacob] = CL_rot_pvConvert(pos1,vel1,M, omega,cvel,cjac)
// Converts position and velocity and computes the jacobian
//
// Calling Sequence
// [pos2,vel2,jacob] = CL_rot_pvConvert(pos1,vel1,M [,omega,cvel,cjac])
//
// Description
// <itemizedlist><listitem>
// <p>Computes position and velocity relative to frame2 given position and velocity relative to frame1: </p> 
// <p>pos2 = M * pos1 </p> 
// <p>vel2 = M * (vel1 - omega ^ pos1) </p> 
// <p>The velocity is computed if cvel == %t and omega is not empty.</p>
// <p>The jacobian is computed if cjac == %t and omega is not empty.</p>
// <p>jacob = d(pos2,vel2)/d(pos1,vel1), more precisely jacob(i,j,:) = d(pv2(i,:)/d(pv1(j,:))</p>
// <p></p></listitem>
// </itemizedlist>
//
// Parameters
// pos1: Position vector relative to frame1 (with coordinates given in frame1) (3xN)
// vel1: Velocity vector relative to frame1 (with coordinates given in frame1) (3xN or empty)
// M: Transformation matrix from frame1 to frame2 (pos2 = M * pos1) (3x3xN)
// omega: (optional) Angular velocity vector of frame2 wrt frame1 with coordinates in frame1 (3xN). Empty by default. 
// cvel: (boolean, optional) %t if velocity should be computed (1x1). Default is %t. 
// cjac: (boolean, optional) %t if jacobian should be computed (1x1). Default is %t. 
// pos2: Position vector relative to frame2 (with coordinates given in frame2) (3xN)
// vel2: Velocity vector relative to frame2 (with coordinates given in frame2) (3xN)
// jacob: (optional) 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];
// pos1 = [7000.e3;0;0]; 
// vel1 = [0; 7000; 0]; 
// [pos2,vel2,jacob] = CL_rot_pvConvert(pos1,vel1,M,omega) 

// Declarations:

// Code:

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

if (~exists("omega", "local")); omega = []; end
if (~exists("cvel", "local")); cvel = %t; end
if (~exists("cjac", "local")); cjac = %t; end

// vel1 and omega must be [] or of same size as pos1
N = size(pos1,2);
if ( (size(vel1,2) <> N & size(vel1,2) <> 0) | (size(omega,2) <> N & size(omega,2) <> 0))
  CL__error("Invalid size of input arguments"); 
end

// omega == [] => can't compute velocity or jacobian
if (omega == [])
  cvel = %f; 
  cjac = %f;  
end

// vel1 == [] => output velocity not computed
if (vel1 == [])
  cvel = %f; 
end

// Avoid computing not requested output arguments 
if (argn(1) < 2)
  cvel = %f;
end

// Avoid computing not requested output arguments 
if (argn(1) < 3)
  cjac = %f;
end

pos2 = M * pos1;
vel2 = [];
jacob = [];

if (cvel)
  vel2 = M * (vel1 - CL_cross(omega, pos1));
end

if (cjac)
  jacob = CL_rot_pvJacobian(M, omega);
end

endfunction