;+
;Function: qslerp,q,x1,x2
;
;Purpose: uses spherical linear interpolation to interpolate
;quaternions between elements of q
;
;Inputs: q: an Nx4 element array, representing a list of quaternions
;with N > 1, all quaternions must be unit quaternions(ie length/norm = 1)
;        x1: The input abcissa values of the quaternions,an array of
;        length N, abcissa values must also be monotonic
;
;        x2: The output abcissa values for the quaternions, can have
;        as many elements as wanted but must fall on the interval
;        [x[0],x[N-1]], an M element array, abcissa values must also
;        be monotonic
;
;        geometric(optional): this keyword allows you to specify that
;        it use the geometric formula for the slerp. The default
;        formula is probably faster and more numerically stable, the 
;        geometric option is just available for testing
;        Testing of the geometric method indicates that the norm of
;        the interpolated quaternions strays easily from unit length,
;        when it renormalizes results may be destabilized
;
;       
;
;Returns: an Mx4 element array of interpolated quaternions or -1L on
;failure
;
;
;;Notes: 
;Represention has q[0] = scalar component
;                 q[1] = vector x
;                 q[2] = vector y
;                 q[3] = vector z
;
;The vector component of the quaternion can also be thought of as
;an eigenvalue of the rotation the quaterion performs
;
;The scalar component can be thought of as the amount of rotation that
;the quaternion performs
;
;While the code may seem a little esoteric, it is vectorized and
;provides the most accurate results it can get
;
;Written by: Patrick Cruce(pcruce@igpp.ucla.edu)
;
; $LastChangedBy: aaflores $
; $LastChangedDate: 2012-01-26 16:59:44 -0800 (Thu, 26 Jan 2012) $
; $LastChangedRevision: 9629 $
; $URL: svn+ssh://thmsvn@ambrosia.ssl.berkeley.edu/repos/ssl_general/tags/tdas_8_00/misc/quaternion/qslerp.pro $
;-

function qslerp,q,x1, x2,geometric=geometric

compile_opt idl2

EQ_TOLERANCE = 1e-12 ;how close two numbers have to be to be considered equal
                     ;error in calculations can be assumed
                     ;to be at least as high as this number

;this is to avoid mutating the input variables
qi = q
x2i = x2
x1i = x1

;validate the inputs

;check that quaternions are consistent with generic quaternion invariants
qi = qvalidate(qi,'qi','qslerp')

if(size(qi,/n_dim) eq 0 && qi[0] eq -1) then return,qi

;check that input quaternions are unit length
qn = qnorm(qi)

if(size(qn,/n_dim) eq 0 && qn[0] eq -1) then begin

  dprint, 'Unable to calculate norm of input quaternions'

  return, -1

endif

idx = where(abs(qn - 1.0D) gt EQ_TOLERANCE)

if(idx[0] ne -1) then begin

  dprint, 'At least one input quaternion is not unit length'

  return, -1

endif

;guarantee correct number of input quaternions
qdims = size(qi, /dimensions)

if(qdims[0] ne n_elements(x1i)) then begin

  dprint, 'Number of input abcissa values does not match the number of input quaternions'

  return, -1

endif

;check that input abcissa values are monotonic

if(n_elements(x1i) gt 1) then begin 
   idx = where((x1i[1:n_elements(x1i)-1]-x1i[0:n_elements(x1i)-2]) lt 0)

   if(idx[0] ne -1) then begin

      dprint, 'input abcissa values not monotonic'

      return, -1

   endif
endif

if(n_elements(x2i) gt 1) then begin
;check that output abcissa values are strictly monotonic
   idx = where((x2i[1:n_elements(x2i)-1]-x2i[0:n_elements(x2i)-2]) le 0)

   if(idx[0] ne -1) then begin

      dprint, 'output abcissa values not monotonic'

      return, -1

   endif
endif
;construct the output array

q_out = make_array(n_elements(x2i), 4, /double)

;if output abcissa values are outside of the range of input abcissa
;values constant extrapolation is used

idx = where(x2i lt x1i[0])

if(idx[0] ne -1) then q_out[idx,*] = rebin(qi[0,*],n_elements(idx),4)

idx = where(x2i gt x1i[n_elements(x1i)-1])

if(idx[0] ne -1) then q_out[idx,*] = rebin(qi[n_elements(x1i)-1,*],n_elements(idx),4)

out_idx = where(x2i ge x1i[0] and x2i le x1i[n_elements(x1i)-1])

if(out_idx[0] eq -1) then return,reform(q_out)

x2i = x2i[out_idx]

;construct arguments to the slerp function, this includes the source
;quaternion list, the target quaternions list, and the proportion of
;interpolation list for each quaternion pair.  They should all have
;the same number of elements as the output abcissa value list

t_temp = interpol(dindgen(qdims[0]), x1i, x2i)

t_list = t_temp mod 1.0D

q_idx = long(floor(t_temp))

;if the last abcissa values are identical,the indexing scheme to
;generate the q_list could generate an overflow, the two conditionals
;below prevent this

idx = where(abs(t_list) le EQ_TOLERANCE) ;where t_list =~ 0.0

;put everything that requires no interpolation directly into the output
if(idx[0] ne -1) then begin

  q_out[out_idx[idx],*] = qi[q_idx[idx],*] 

endif

slerp_idx = where(abs(t_list) gt EQ_TOLERANCE) ;where t_list !=~ 0.0

;if there is nothing left, then we're done
if(slerp_idx[0] eq -1) then return, reform(q_out)

q_idx = q_idx[slerp_idx]
out_idx = out_idx[slerp_idx]
t_list = t_list[slerp_idx]

q1_list = qi[q_idx,*]

q2_list = qi[q_idx+1,*]

;calculate the dot product which is needed to to flip the
;appropriate quaternions to guarantee interpolation is done along the
;shortest path 
dotp = qdotp(q1_list, q2_list)

if(size(dotp, /n_dim) eq 0 && dotp eq -1) then return, -1

;the following code flips quaternions in q2_list to ensure the
;shortest path is followed 
idx = where(dotp lt 0.0D)

if(idx[0] ne -1) then q2_list[idx,*] = -q2_list[idx,*]

;interpolation cannot be performed on colinear quaternions
;it is assumed that colinear quaternions will be returned unchanged
;since dotp(q1,q2) = cos(angle between q1,q2) if dotp = 1.0 the
;quaternions are colinear
idx = where(abs(dotp - 1.0D) le EQ_TOLERANCE) ;where dotp = 1.0

;store colinear quaternions into output array
if(idx[0] ne -1) then q_out[out_idx[idx],*] = q1_list[idx,*]

;copy non-colinear quaternions for processing
idx = where(abs(dotp - 1.0D) gt EQ_TOLERANCE)

if(idx[0] eq -1) then return, reform(q_out)  ;if no non-colinear quaternions are left, we are done

dotp = dotp[idx]
t_list = t_list[idx]
q1_list = q1_list[idx,*]
q2_list = q2_list[idx,*]
out_idx = out_idx[idx]

;now the actual processing begins

;testing both methods to verify results
if keyword_set(geometric) then begin 

theta = acos(dotp)

sin_theta = sin(theta)

theta_t = theta*t_list

co1 = sin(theta - theta_t)/sin_theta
co2 = sin(theta_t)/sin_theta

q_out[out_idx,0] = co1*q1_list[*,0]+co2*q2_list[*,0]
q_out[out_idx,1] = co1*q1_list[*,1]+co2*q2_list[*,1]
q_out[out_idx,2] = co1*q1_list[*,2]+co2*q2_list[*,2]
q_out[out_idx,3] = co1*q1_list[*,3]+co2*q2_list[*,3]

endif else begin

;slerp will be performed by calculating: 
;((q2*(q1^-1))^t)*q1
;since the quaternions are unit q1^-1 = conjugate(q1)
;exponentiation can be calculated by transforming to 
;polar form cos(theta*t)+v*sin(theta*t)
;theta = acos(q[0])
;NOTE: this potentially more numerically stable implementation needs
;to be verified by comparison to the geometric slerp

q1_conj = qconj(q1_list)

q2_q1_prod = qdecompose(qmult(q2_list, q1_conj))

if(size(q2_q1_prod, /n_dim) eq 0 && q2_q1_prod[0] eq -1) then return, -1

;sometimes a dimension disappears.
if ndimen(q2_q1_prod) eq 1 && n_elements(q2_q1_prod) eq 4 then begin
  q2_q1_prod = reform(q2_q1_prod,1,4)
endif

theta_scale = q2_q1_prod[*,0]*t_list

q_total = qmult(qcompose(q2_q1_prod[*,1:3],theta_scale), q1_list)

if(size(q_total, /n_dim) eq 0 && q_total[0] eq -1) then return, -1

q_out[out_idx,*] = q_total

endelse

return, qnormalize(q_out)

end