//  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_interpQuat(t_ref,q_ref,t,method)
// Interpolation of quaternions
//
// Calling Sequence
// q = CL_rot_interpQuat(t_ref,q_ref,t [,method])
//
// Description
// <itemizedlist><listitem>
// <p>Given reference quaternions <b>q_ref</b> at reference times <b>t_ref</b>, 
// the function computes interpolated quaternions at times <b>t</b>.</p> 
// <p>Only one <b>method</b> of interpolation is avalaible at the moment: Slerp method (shortest path).</p> 
// <p></p>
// <p>Notes:</p>
// <p> - <b>t_ref</b> must be sorted in strictly increasing order.</p>
// <p> - Output quaternions corresponding to values of <b>t</b> outside of [<b>t_ref</b>(1),<b>t_ref</b>($)] are set to %nan.</p>
// <p></p><p></p></listitem>
// <listitem>
// <p>See <link linkend="Data types">Data types</link> for more details on quaternions. </p>
// </listitem>
// </itemizedlist>
//
// Parameters
// t_ref: Reference times (1xN with N>=2)
// q_ref: Reference quaternions at times t_ref (size N with N>=2)
// t: Requested times (1xP).
// method: (string, optional) Name of the method. Only "slerp" is available. Default is "slerp". (1x1)
// q : Interpolated quaternions at times t. (size P)
//
// Authors
// CNES - DCT/SB
//
// See also
// CL_rot_defQuat
// CL_rot_quatSlerp
// CL_rot_matrix2quat
//
// Examples
// t_ref = [1:10];
// q_ref = CL_rot_angles2quat([3,1,3],[t_ref;t_ref.^2;cos(t_ref)]);
// t = [1.5:8.5];
// q = CL_rot_interpQuat(t_ref,q_ref,t)

// Declarations:


// Code:
if (argn(2) < 3 | argn(2) > 4)
  CL__error("Invalid number of input arguments");
end

if (~exists("method", "local")); method = "slerp"; end;

if (typeof(t_ref) <> "constant" | typeof(q_ref) <> "CLquat" | ...
    typeof(t) <> "constant" | typeof(method) <> "string")
  CL__error("Invalid input arguments");
end

if (method <> "slerp")
  CL__error("Invalid method of interpolation");
end

N1 = size(t_ref,2);
N2 = size(q_ref);
if (N1 <> N2 | N1 < 2)
  CL__error("Invalid size for t_ref or q_ref");
end
  
if (size(t_ref,1) <> 1 | size(t,1) <> 1)
  CL__error("Invalid size for t_ref or t (number of rows)");
end
  
// q initialized to %nan
P = size(t,2);
q = CL__defQuat(%nan * ones(1,P), %nan * ones(3,P));
   
// I = (1xP) = index such that t_ref(I) < t(I) <= t_ref(I+1)
// NB: 
// - if t = t_ref(1) the function returns I=1
// - if t is outside [t_ref(1),t_ref($] the function returns 0
//   the quaternions for these index will be %nan
// - dsearch checks that t_ref is in stricly increasing order which guarantees
//   there wont be any division by zero in the code further below
I = dsearch(t,t_ref);
J = find(I <> 0);
I = I(J);

// p = interpolation parameter
// p = 0 --> t = t_ref(I) --> q = q_ref(I)
// p = 1 --> t = t_ref(I+1) --> q = q_ref(I+1)
p = (t(J) - t_ref(I)) ./ (t_ref(I+1) - t_ref(I));
q(J) = CL_rot_quatSlerp(q_ref(I),q_ref(I+1),p);

endfunction