;+
; 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