//  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 [q,Inok] = CL_rot_defRotVec(u1,v1,u2,v2)
// Rotation transforming 2 vectors into 2 vectors
//
// Calling Sequence
// [q,Inok] = CL_rot_defRotVec(u1,v1,u2,v2)
//
// Description
// <itemizedlist><listitem>
// <p>Computes the rotation that transforms 2 vectors into 2 vectors. </p>
// <p>Except for possible scale factors and vector adjustments, the rotation R is such that:  </p>
// <p>R(u1) = u2 and R(v1) = v2 </p>
// <p>The rotation R is returned as a quaternion (<b>q</b>). 
// In practice, the rotation is determined such that (assuming the vectors not aligned): </p>
// <p>R(u1/||u1||) = u2/||u2|| </p>
// <p>R(u1^v1/||u1^v1||) = u2^v2/||u2^v2|| </p>
// <p>so that the angular separations between u1 and v1, and u2 and v2 
// do not matter (provided they remain in ]0,pi[). </p> 
// <p></p></listitem>
// <listitem>
// <p><b>Notes:</b></p>
// <p>If (u1 and v1) or (u2 and v2) are colinear, then the rotation is not uniquely defined or may not exist, 
// and the corresponding indices are returned in <b>Inok</b>. In that case, the returned 
// rotation is the rotation that transforms u1 into u2 about an axis perpendicular to the plane (u1,u2) which is 
// the one with the smallest rotation angle. If additionnaly u1 and u2 are colinear, 
// then the rotation is either the identity or a 180 degree rotation about an axis perpendicular to u1.</p>
// <p></p></listitem>
// <listitem>
// <p>Use note: </p>
// <p>The function can be used for frame determination 
// when 2 vectors are known in 2 different frames: </p>
// <p>- u1,v1: coordinates relative to frame 1 </p>
// <p>- u2,v2: coordinates relative to frame 2 </p>
// <p>=> q = CL_rot_defRotVec(u1,v1,u2,v2) gives the rotation that 
// transforms the basis vectors of frame2 into the basis vectors of frame 1. </p>
// <p>The frame transformation matrix M from Frame 1 to Frame 2 is then: 
// M = CL_rot_quat2matrix(q'), and is such that: u2 = M*u1 and v2 = M*v1 </p>
// </listitem>
// </itemizedlist>
//
// Parameters
// u1: First vector (3xN or 3x1)
// v1: Second vector (3xN or 3x1)
// u2: Desired image of u1 by the rotation (3xN or 3x1)
// v2: Desired image of v1 by the rotation (3xN or 3x1)
// q: Quaternion that defines the rotation (dim N)
// Inok: Indices for which u1,v1,u2,v2 do not define a unique rotation. 
//
// Authors
// CNES - DCT/SB
//
// See also
// CL_rot_defQuat
// CL_rot_defFrameVec
//
// Examples
// // Example1: (u1,v1) and (u2,v2) have the same angular separation:
// u1 = [ 1 ; 2 ; 3 ];
// v1 = [ 3 ; 1 ; 2 ];
// q = CL_rot_axAng2quat([0;0;1], 0.1)
// u2 = CL_rot_rotVect(q,u1); 
// v2 = CL_rot_rotVect(q,v1);
// q2 = CL_rot_defRotVec(u1,v1,u2,v2) // q == q2
//
// // Example2: (u1,v1) and (u2,v2) have different angular separations:
// u1 = [ 1 ; 2 ; 3 ];
// v1 = [ 3 ; 1 ; 2 ];
// q = CL_rot_axAng2quat([0;0;1], 0.1)
// u2 = CL_rot_rotVect(q,u1); 
// v2 = CL_rot_rotVect(q,v1) + 10 * u2;
// q2 = CL_rot_defRotVec(u1,v1,u2,v2) //q == q2
//
// // Example3: One pair of vectors
// u1 = [ 1 ; 2 ; 3 ];
// u2 = [ 1 ; 4 ; 5 ];
// [q,ind] = CL_rot_defRotVec(u1,u1,u2,u2); // ind == 1
// 
// // Example4: frame determnation
// kep = [7000.e3; 0.01; %pi/3; 0; %pi/2; 0]; // orbital elements
// [p,v] = CL_oe_kep2car(kep); // inertial position and velocity
// M = CL_fr_qswMat(p,v); // inertial to "qsw" local frame
// u2 = [1;2;3];   // "2" => coord. relative to "qsw"
// v2 = [-1;1;0];   
// u1 = M' * u2;   // "1" => coord relative to inertial frame
// v1 = M' * v2;     
// q = CL_rot_defRotVec(u1,v1,u2,v2); 
// CL_rot_quat2matrix(q') - M  // => 0
// q = CL_rot_defRotVec(u2,v2,u1,v1); 
// CL_rot_quat2matrix(q) - M  // => 0
//
// M1 = CL_rot_defFrameVec(u1,v1,1,2); // inertial => vectors
// M2 = CL_rot_defFrameVec(u2,v2,1,2); // qsw => vectors
// M2'*M1 - M // => 0 



// ----------------------------------------------------------
// Declarations:
// ----------------------------------------------------------

tol = 1.e-16; // tolerance for vector alignment (angle squared) 

// Solution for: u1 -> u2
// => rotation based on u1 and u2 only 
// axis = u1^u2, 
// except in special cases:  
// => axis = u1 or is perpendicular to u1 (arbitrary direction)
// NB: 
// - all vectors must have the same size
// - axis not normalized

function [axis,ang] = solve1_rot(u1,u2)

  // likely solution
  axis = CL_cross(u1,u2);
  ang = CL_vectAngle(u1,u2);  

  // special cases
  I = find(CL_dot(axis) < tol);
  if (I <> []) // not valid axis

    J = I(find(CL_dot(u1(:,I),u2(:,I)) > 0)); // u2 == u1
    axis(:,J) = u1(:,J); 
    ang(J) = 0; 

    J = I(find(CL_dot(u1(:,I),u2(:,I)) < 0)); // u2 == -u1
    axis(:,J) = CL__axisPerp(u1(:,J)); // OK if J==[]
    ang(J) = %pi; 

  end

endfunction


// Solution for: u1 -> u2, v1 -> v2
// (v1 perp. to u1 and v2 perp. to u2 -- not checked
// and all norms == 1
// => axis = (u2-u1)^(v2-v1) 
// Special cases:  
// => axis = (u1^v1)^(u2^v2) or (u1^v1) or (u1^v1)^(u2-u1)
//
// NB: 
// - all vectors must have the same size
// - axis not normalized
// 
// Algorithm: 
// 1) likely solution: axis perpendicular to u2-u1 and v2-v1 
//    => axis is 0 if: u1==u2 or v1==v2 or u2-u1//v2-v1 
// 2) if u2-u1//v2-v1 
//    <=> a*(u2-u1)=b*(v2-v1)
//    <=> a*u2-b*v2 = a*u1-b*v1 => invariant
//    => axis in (u2,v2) plane and in (u1,v1) plane 
//    => axis = w1^w2;  w1=u1^v1;  w2=u2^v2
// 3) if axis in (u2,v2) plane 
//    axis can be 0 if w1==w2 or w1==-w2 (||w1||==||w2||==1)
//    w1 == w2  => axis=w1  because R(w1) == w2 
//    w1 == -w2 => axis=w1^(u2-u1) (axis perp to w1 and to u2-u1) 
//          (not 0 as u2 perp to w1 = -w2 and u1 too and u1<>u2)

function [axis,ang] = solve2_rot(u1,u2,v1,v2)

  // likely solution
  axis = CL_cross(u2-u1, v2-v1); 
  I = find(CL_dot(axis) < tol); 

  if (I <> []) // not valid axis
    cond1 = CL_dot(u2-u1) < tol; 
    cond2 = CL_dot(v2-v1) < tol; 

    J = I(find(cond1(I))); 
    axis(:,J) = u1(:,J); 

    J = I(find(cond2(I))); 
    axis(:,J) = v1(:,J); 

    J = I(find(~cond1(I) & ~cond2(I))); 
    if (J <> [])
      w1 = CL_cross(u1,v1); // norm == 1
      w2 = CL_cross(u2,v2); // norm == 1
      axis(:,J) = CL_cross(w1(:,J), w2(:,J)); 

      K = J(find(CL_dot(w1(:,J)-w2(:,J)) < tol));  
      axis(:,K) = w1(:,K); 

      K = J(find(CL_dot(w1(:,J)+w2(:,J)) < tol)); 
      axis(:,K) = CL_cross(w1(:,K), u2(:,K)-u1(:,K)); 

      if (K <> []) // not needed - added in case...
        L = K(find(CL_dot(axis(:,K)) < tol)); 
        axis(:,L) = u1(:,L); 
      end

    end 
  end

  // compute rotation angle

  // determines the best pair of vectors 
  // (most perpendicular to axis)
  I = find(abs(CL_dot(axis,v1)) < abs(CL_dot(axis,u1))); 
  u1(:,I) = v1(:,I); 
  u2(:,I) = v2(:,I); 

  u1 = CL_cross(axis, u1); 
  u2 = CL_cross(axis, u2); 

  w = CL_cross(u1,u2); 
  ang = atan(CL_norm(w), CL_dot(u1,u2));
  I = find(CL_dot(axis, w) < 0); 
  ang(I) = -ang(I); 

endfunction


// ----------------------------------------------------------
// main 
// ----------------------------------------------------------

// Check argument sizes, and resize if necessary: 
// unit vectors before resizing (for efficiency)
// check vectors norms are not 0)
// (NB: norms compared to 0 volontarily)

[u1, nu1] = CL_unitVector(u1); 
[v1, nv1] = CL_unitVector(v1); 
[u2, nu2] = CL_unitVector(u2); 
[v2, nv2] = CL_unitVector(v2); 

[u1,v1,u2,v2,N] = CL__checkInputs(u1,3,v1,3,u2,3,v2,3);

if (find (nu1 .* nu2 .* nv1 .* nv2 == 0) <> [])
  CL__error("Vectors should not be zero");
end



// initialization
axis = %nan * ones(3,N); 
ang = %nan * ones(1,N); 

[w1,nw1] = CL_unitVector(CL_cross(u1,v1)); 
[w2,nw2] = CL_unitVector(CL_cross(u2,v2));
 
// condition for not "nominal" situation
cond_NOK = nw1.*nw1 < tol | nw2.*nw2 < tol; 

// case: u1 // v1 or u2 // v2
I = find(cond_NOK);
if (I <> [])
  [axis(:,I), ang(I)] = solve1_rot(u1(:,I),u2(:,I)); 
end

// "normal" case
I = find(~cond_NOK);
if (I <> [])
  [axis(:,I), ang(I)] = solve2_rot(u1(:,I),u2(:,I),w1(:,I),w2(:,I)); 
end

// quaternion and indicator 
q = CL_rot_axAng2quat(axis, ang); 
Inok = find(cond_NOK); 

endfunction