//  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] = CL_rot_quatSlerp(q1,q2,p)
// Quaternion interpolation on the unit sphere using Slerp method
//
// Calling Sequence
// q = CL_rot_quatSlerp(q1,q2,p)
//
// Description
// <itemizedlist><listitem>
// <p>Interpolation between two quaternions using Slerp method (shortest path).</p> 
// <p> The interpolation is such that: </p>
// <p> - If p = 0: q = q1 </p>
// <p> - If p = 1: q = q2 </p>
// <p></p></listitem>
// <listitem>
// <p><b>Notes:</b></p>
// <p> - If q1 or q2 is null, q is equal to %nan.  </p>
// <p> - An empty quaternion is returned if q1, q2 or p is empty.  </p>
// <p> - See <link linkend="Data types">Data types</link> for more details on quaternions. </p>
// </listitem>
// </itemizedlist>
//
// Parameters
// q1 : Quaternion (size N or 1)
// q2 : Quaternion (size N or 1)
// p : Interpolation parameter (1xN or 1x1). Can be < 0 or > 1. 
// q : Interpolated quaternion (size N)
//
// Authors
// CNES - DCT/SB
//
// See also
// CL_rot_defQuat
// CL_rot_matrix2quat
//
// Examples
// q1 = CL_rot_axAng2quat([0;0;1],0.5); 
// q2 = CL_rot_axAng2quat([1;1;1], 1);  
//
// q = CL_rot_quatSlerp(q1,q2,0)  // q = q1
// q = CL_rot_quatSlerp(q1,q2,1)  // q = q2
// q = CL_rot_quatSlerp(q1,q2,(0:5)/5)  
// q = CL_rot_quatSlerp([q1,q1,q1],q2,[0.1, 0.2, 0.3])  
//

// Declarations:


// Code:
if (typeof(q1) <> "CLquat" | typeof(q2) <> "CLquat")
  CL__error("Wrong type of input argument. Quaternion expected");
end

if (size(p, 1) > 1)
  CL__error("Wrong dimension for argument ''p''");
end

N1 = size(q1);
N2 = size(q2);
Np = size(p,2);
N = max(N1, N2, Np);

// check sizes (assumed not 0 at this point) 
// Note: Np can be 0
if ~((N1 <= 1 | N1 == N) & (N2 <= 1 | N2 == N) & (Np <= 1 | Np == N)) 
  CL__error("Invalid size for q1, q2 or p")
end

// Empty quaternion returned if q1 and q2 empty or p empty 
if (N1 == 0 | N2 == 0 | Np == 0)
  q = CL__defQuat([], []); 
  return; // <= RETURN
end


// Interpolation
// NB: rotation angle is in [0, pi]
[axis, ang] = CL_rot_quat2axAng(q1' * q2); 

q = q1 * CL_rot_axAng2quat(axis, ang .* p);

// NB: if q1 or q2 == 0 => q is equal to %nan 
// test added for security to avoid division by 0

nq = norm(q); 
I = find(nq <> 0);
if (I <> [])
  q(I) = q(I) ./ nq(I); 
end
  

endfunction