//  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 [angle] = CL_vectAngle(u,v)
// Angle between vectors
//
// Calling Sequence
// angle = CL_vectAngle(u,v)
//
// Description
// <itemizedlist><listitem>
// <p>Computes the angle between two vectors.</p>
// <p> The angle is between 0 and pi (included). If one of the 
// 2 vectors is a null vector, the angle is 0.  </p>
// </listitem>
// </itemizedlist>
//
// Parameters
// u: Vector or matrix (considered as a set of column vectors) (3x1 or 3xN)
// v: Vector or matrix or matrix (considered as a set of column vectors) (3x1 or 3xN)
// angle: Angle between u and v (1xN)
//
// Authors
// CNES - DCT/SB
//
// See also
// CL_cross
// CL_dot
// CL_norm
//
// Examples
// // u and v : set of column vectors
// u = [[1;0;0], [1;0;0], [0;0;0]];
// v = [[0;1;0], [1;0;0.001], [1;0;0]];
// CL_vectAngle(u,v) * 180/%pi
//
// // u: set of column vectors, v : one column vector
// u = [[1;0;0], [1;0;0], [0;1;0.001]];
// v = [0;1;0];
// CL_vectAngle(u,v) * 180/%pi
 
// Declarations:


// Code:
[u, v] = CL__checkInputs(u, 3, v, 3); 

if (u == [] | v == [])
  if (u == [] & v == [])
    // 2 empty vectors => empty output
    angle = []; 
    return; // <= RETURN
  else
    CL__error("Vectors have inconsistent sizes");  
  end
end

[u1,norm_u] = CL_unitVector(u);
[v1,norm_v] = CL_unitVector(v);

angle = %nan * ones(norm_u);
cosang = CL_dot(u1,v1); // NB: may contain %nan
threshold = 0.9999;

// NB: if norm_u or norm_v is 0, u1 and u2 contain %nan, 
// but 'find' will not return the corresponding indices.  
// => see the test at end of function

// vectors are NOT almost aligned => compute using acos
I = find(abs(cosang) <=  threshold);
angle(I) = acos(cosang(I));

// vectors are almost aligned => compute using asin
J = find(abs(cosang) >  threshold);
if ( J <> [] )
  sinang = CL_norm(CL_cross(u1(:,J),v1(:,J)));

  I = find(cosang(J) >  0);
  angle(J(I)) = asin(sinang(I));
  I = find(cosang(J) <  0);
  angle(J(I)) = %pi - asin(sinang(I)); 
end

// angle is 0 if at least one vector is 0 (and not %nan)
K = find( (norm_u == 0 & ~isnan(norm_v)) | (norm_v == 0 & ~isnan(norm_u))); 
angle(K) = 0;

endfunction