; docformat = 'rst'
;+
;This function takes the spherical linear interpolation between two
;quaternions at time t which is in GPS microseconds.
;
; :Examples:
;   result = slerp(quat0_, quat1_, gps_time)
;-
;+
; :Params:
;   q0_ : in, required, type=vector of double
;     First quaternion for the spherical interpolation.
;   q1_ : in, required, type=vector of double  
;     Second quaternion for the spherical interpolation.
;   t : in, required, type=double
;     Time in GPS microseconds for the spherical interpolation.
;     
; :Returns: 
;   Returns the spherical interpolation of the two quaternions at time t.
;-
function slerp,q0_,q1_,t
    q0=reform(q0_)
    q1=reform(q1_)
    q0=q0/norm(q0)
    q1=q1/norm(q1)
    
    ;dot product as if quaternions were 4D vectors. This is not 
    ;necessarily related to the rotation between the two quaternions.
    c_theta0=total(q0*q1) 
  
    ;Make sure we're going the short way around. Rotationally, q==-q.
    if c_theta0 lt 0 then begin 
      q1=-q1                
      c_theta0=total(q0*q1)
    endif
    
    theta0=acos(c_theta0)
    theta=theta0*t
    q2=q1-c_theta0*q0
    q2=q2/norm(q2)
    
    if n_elements(theta) gt 1 then begin
      result=dblarr(n_elements(theta),4)
      for i=0,n_elements(theta)-1 do begin
        result[i,*]=q0*cos(theta[i])+q2*sin(theta[i])
      endfor
      return,result
    endif else begin
      return,q0*cos(theta)+q2*sin(theta)
    endelse

end