;+
; NAME:
;   QTERP
;
; AUTHOR:
;   Craig B. Markwardt, NASA/GSFC Code 662, Greenbelt, MD 20770
;   craigm@lheamail.gsfc.nasa.gov
;   UPDATED VERSIONs can be found on my WEB PAGE: 
;      http://cow.physics.wisc.edu/~craigm/idl/idl.html
;
; PURPOSE:
;   Smoothly interpolate from a grid of quaternions (spline or slerp)
;
; MAJOR TOPICS:
;   Geometry
;
; CALLING SEQUENCE:
;   QNEW = QTERP(TGRID, QGRID, TNEW, [/SLERP], QDIFF=, [/RESET])
;
; DESCRIPTION:
;
;  The function QTERP is used to interplate from a set of known unit
;  quaternions specified on a grid of independent values, to a new set
;  of independent values.  For example, given a set of quaternions at
;  specified key times, QTERP can interpolate at any points between
;  those times.  This has applications for computer animation and
;  spacecraft attitude control.
;
;  The "grid" of quaternions can be regularly or irregularly sampled.
;  The new values can also be regularly or irregularly sampled.
;
;  The simplest case comes when one wants to interpolate between two
;  quaternions Q1 and Q2.  In that case the user should specify the
;  gridded quaterion as QGRID = [[Q1], [Q2]], with grid points at
;  TGRID = [0d, 1d].  Then the user can sample any intermediate
;  orientation by specifying TNEW anywhere between 0 and 1.
;
;  The user has the option of performing pure spline interpolation of
;  the quaternion components (the default technique).  The resulting
;  interpolants are normalized to be unit quaternions.  This option is
;  useful for fast interpolation of quaternions, but suffers if the
;  grid is not well sampled enough.  Spline interpolation will not
;  strictly find the shortest path between two orientations.
;
;  The second option is to use Spherical Linear IntERPolation, or
;  SLERPing, to interpolate between quaternions (by specifying the
;  SLERP keyword).  This technique is guaranteed to find the shortest
;  path between two orientations, but is somewhat slower than spline
;  interpolation.  This approach involves computing a finite
;  difference of the data.  To avoid repeated computation of the
;  difference on every call, users can pass a named variable in the
;  QDIFF keyword.  This value can be reset with the RESET keyword.
;
;  Conventions for storing quaternions vary in the literature and from
;  library to library.  This library uses the convention that the
;  first three components of each quaternion are the 3-vector axis of
;  rotation, and the 4th component is the rotation angle.  Expressed
;  in formulae, a single quaternion is given by:
;
;     Q(0:2) = [VX, VY, VZ]*SIN(PHI/2)
;     Q(3)   =              COS(PHI/2)
;
;  where PHI is the rotation angle, and VAXIS = [VX, VY, VZ] is the
;  rotation eigen axis expressed as a unit vector.  This library
;  accepts quaternions of both signs, but by preference returns
;  quaternions with a positive 4th component.
;
;  Users must have the VALUE_LOCATE() function, available either in
;  IDL 5.3 or later, or from the Markwardt web page.
;
; INPUTS:
;
;   TGRID - a vector of N independent variable values.  In the
;           simplest case, this can be [0, 1, ...] up to the number of
;           quaternions in the grid.  The grid sampling does not have
;           to be uniform.
;
;   QGRID - an 4xN array of unit quaternions specified on the grid.
;
;   TNEW - a vector of M desired independent variable values which
;          sample the grid specified by TGRID.  The desired values do
;          not have to be uniformly sampled.
;
; RETURNS:
;
;   A 4xM array of unit quaternions, where is M is the number of
;   desired samples.
;
;
; KEYWORD PARAMETERS:
;
;   SLERP - if set, then spherical linear interpolation is performed.
;           The default is to perform spline interpolation on the
;           quaternion coefficients.
;
;   QDIFF - upon return, QDIFF is filled with finite difference values
;           which can be used to speed computations in subsequent
;           calls.  Users should be aware that QDIFF may be
;           inadvertently reused from one call to the next.  When the
;           difference data should no longer be reused, the named
;           variable passed to the QDIFF keyword should be set to a
;           scalar, or the /RESET keyword should be used.
;
;   RESET - if set, then the QDIFF finite difference will be forced to
;           be recalculated, even if there is already data present and
;           passed to the QDIFF keyword.
;
;
; EXAMPLE:
;
;   This example starts with two quaternions representing rotations of
;   0 degrees and 45 degrees, and forms 1001 quaternions which are
;   smooth interpolations between 0 and 45 degrees.
;
;   ;; Create a grid of two quaternions at times 0 and 1
;   Q0 = qtcompose([1,0,0], 0D)       & T0 = 0D
;   Q1 = qtcompose([1,0,0], !dpi/4)   & T1 = 1D
;
;   ;; Put the grid elements into an array
;   TGRID = [T0, T1]
;   QGRID = [[Q0], [Q1]]
;
;   ;; Make an array of 11 values smoothly varying from 0 to 1
;   TNEW = dindgen(11)/10d
;
;   ;; Perform spherical linear interpolation
;   QNEW = QTERP(TGRID, QGRID, TNEW, /SLERP)
;
;   --->  (interpolated results in QNEW)
;  
;       0.0000000       0.0000000       0.0000000       1.0000000
;     0.039259816       0.0000000       0.0000000      0.99922904
;     0.078459096       0.0000000       0.0000000      0.99691733
;      0.11753740       0.0000000       0.0000000      0.99306846
;      0.15643447       0.0000000       0.0000000      0.98768834
;      0.19509032       0.0000000       0.0000000      0.98078528
;      0.23344536       0.0000000       0.0000000      0.97236992
;      0.27144045       0.0000000       0.0000000      0.96245524
;      0.30901699       0.0000000       0.0000000      0.95105652
;      0.34611706       0.0000000       0.0000000      0.93819134
;      0.38268343       0.0000000       0.0000000      0.92387953
;   
;
; SEE ALSO
;   QTANG, QTAXIS, QTCOMPOSE, QTERP, QTEXP, QTFIND, QTINV, QTLOG,
;   QTMAT, QTMULT, QTPOW, QTVROT
;
; MODIFICATION HISTORY:
;   Written, July 2001, CM
;   Documented, Dec 2001, CM
;   Usage message; check for 0- and 1-length quaternions; handle case
;      when quaternions are GE 180 degrees apart; handle case of
;      interpolating beyond end of known grid, 15 Mar 2002, CM
;   Use simplified QTMULT with /INV, 21 Sep 2007, CM
;   Added sample output, 29 Sep 2008, CM
;   Handle pathalogical case when some input quaternions were NAN,
;     2012-10-10, CM
;
;  $Id: qterp.pro,v 1.9 2012/10/10 23:27:05 cmarkwar Exp $
;
;-
; Copyright (C) 2001, 2002, 2007, 2008, 2012, Craig Markwardt
; This software is provided as is without any warranty whatsoever.
; Permission to use, copy, modify, and distribute modified or
; unmodified copies is granted, provided this copyright and disclaimer
; are included unchanged.
;-

function qterp, t0, q0, t1, qdiff=qdiff, reset=reset, slerp=slerp

  if n_params() EQ 0 then begin
      info = 1
      USAGE:
      message, 'USAGE:', /info
      message, 'QNEW = QTERP(TGRID, QGRIDJ, TNEW, [/SLERP, QDIFF=, /RESET])',$
        info=info
      return, 0
  endif

  nq = n_elements(q0)/4
  if nq EQ 0 then goto, USAGE

  ;; If there is only one quaternion, replicate it, since there is
  ;; nothing to interpolate
  if nq EQ 1 then $
    return, rebin(reform(q0,4,1),4,n_elements(t1))

  if keyword_set(slerp) then begin
      if n_elements(qdiff)/4 NE nq-1 OR keyword_set(reset) then begin
          qdiff = qtmult(q0(*,0:nq-2), /inv1, q0(*,1:*))

          ;; Normalize the quaternions to get the smallest path
          wh = where(qdiff(3,*) LT 0, ct)
          if ct GT 0 then qdiff(*,wh) = -qdiff(*,wh)
      endif
      ii = value_locate(t0, t1) < (nq-2) > 0
      hh = (t1-t0(ii))/(t0(ii+1)-t0(ii))

      return, qtmult(q0(*,ii), qtpow(qdiff(*,ii),hh))
  endif
  
  q1 = (q0(*,0) # t1) & q1(*) = 0
  for i = 0, 3 do $
    q1(i,*) = spl_interp(t0, q0(i,*), spl_init(t0, q0(i,*)), t1)
  tot = sqrt(total(q1^2,1))
  for i = 0, 3 do $
    q1(i,*) = q1(i,*) / tot

  return, q1
end