;
; XFORM3D - Fitting function for transforming 3D data (rotate & translate)
; 
;   XYZ - 3-d data, concatenated X Y and Z   = [X, Y, Z]
;
;   P - parameters of fit
;       p(0) - offset to subtract before the rotation (X)
;       p(1) - offset to subtract before the rotation (Y)
;       p(2) - offset to subtract before the rotation (Z)
;       p(3) - euler axis rotation (Z axis) = longitude  (deg)
;       p(4) - euler axis rotation (Y axis) = co-latitude (deg)
;       p(5) - euler axis rotation (X axis) = roll (deg)
;
;   INVERT - if set, then apply inverse transformation rather than
;            forward transformation
;

; Utility routine ANGUNITVEC computes unit vector from longitude and
; latitude euler angles.
function angunitvec, a0, d0, declination=declination

  dtor = !dpi/180D
  n = min([n_elements(a0), n_elements(d0)])
  unit = dblarr(3,n)
  unit = reform(unit, 3, n, /overwrite)
  if keyword_set(declination) then $
    d = double(90D - d0)*dtor else d = double(d0)*dtor
  a = a0*dtor

  unit(0,*) = cos(a)*sin(d)
  unit(1,*) = sin(a)*sin(d)
  unit(2,*) = cos(d)
  if n_elements(unit) EQ 3 then unit = unit(*) ;; Allow dimensions to relax

  return, unit
end


; Main transformation routine
function xform3d, xyz, p, invert=invert
  n = n_elements(xyz)/3

  vecs = reform(xyz,n,3)
  if NOT keyword_set(invert) then $
    for i = 0, 2 do vecs(*,i) = vecs(*,i) - p(i)

  q = qtcompose(angunitvec(p(3),p(4)), p(5)*!dpi/180d)
  vnew = transpose(qtvrot(transpose(vecs), q, invert=invert))

  if keyword_set(invert) then $
    for i = 0, 2 do vnew(*,i) = vnew(*,i) + p(i)

  return, vnew(*)
end