;+
; NAME:
;   QTFIND
;
; 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:
;   Find quaternion(s) from direction cosine matrix
;
; MAJOR TOPICS:
;   Geometry
;
; CALLING SEQUENCE:
;   Q = QTFIND(MATRIX)
;
; DESCRIPTION:
;
;   The function QTFIND determines one or more unit quaternions from
;   direction cosine matrices.
;
;   This routine is optimized to avoid singularities which occur when
;   any one of the quaternion components is nearly zero.  Up to four
;   different transformations are attempted to maximize the precision
;   of all four quaternion components.
;
;   QTFIND and QTMAT are functional inverses: use QTFIND to convert a
;   known direction cosine matrix to a new quaternion; use QTMAT to
;   convert a known quaternion to matrix representation.
;
;   NUMERICAL ACCURACY: In a test of 1 billion randomly chosen
;   normalized quaternions at double precision, the maximum numerical
;   error was less than 4.5E-16 in each quaternion component, which is
;   comparable to the numerical accuracy of the double precision
;   representation itself.
;
;  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.
;
;
; INPUTS:
;
;   MATRIX - array of one or more direction cosine matrices.  For a
;            single matrix, MATRIX should be a 3x3 array.  For N
;            matrices, MATRIX should be a 3x3xN array.  The arrays are
;            assumed to be valid rotation matrices.
;
;
; RETURNS:
;
;   The resulting unit quaternions.  For a single matrix, returns a
;   single quaternion as a 4-vector.  For N matrices, returns N
;   quaternions as a 4xN array.
;
;
; KEYWORD PARAMETERS:
;
;   NONE
;
; EXAMPLE:
;
;   ;; Form a rotation matrix about the Z axis by 32 degrees
;   th1 = 32d*!dpi/180         
;   mat1 = [[cos(th1),-sin(th1),0],[sin(th1),cos(th1),0],[0,0,1]]
;   
;   ;; Form a rotation matrix about the X axis by 116 degrees
;   th2 = 116d*!dpi/180
;   mat2 = [[1,0,0],[0,cos(th2),-sin(th2)],[0,sin(th2),cos(th2)]]
;
;   ;; Find the quaternion that represents MAT1, MAT2 and the
;   composition of the two, MAT2 ## MAT1.
;
;    print, qtfind(mat1), qtfind(mat2), qtfind(mat2 ## mat1)
;       0.0000000       0.0000000      0.27563736      0.96126170
;      0.84804810       0.0000000       0.0000000      0.52991926
;      0.81519615     -0.23375373      0.14606554      0.50939109
;
;
; SEE ALSO
;   QTANG, QTAXIS, QTCOMPOSE, QTERP, QTEXP, QTFIND, QTINV, QTLOG,
;   QTMAT, QTMULT, QTPOW, QTVROT
;
; MODIFICATION HISTORY:
;   Written, July 2001, CM
;   Documented, Dec 2001, CM
;   Re-added check to enforce q(3) GE 0, 15 Mar 2002, CM
;   Usage message, error checking, 15 Mar 2002, CM
;   Fixed bug which could produce all-zero quaternion; add
;     documentation about numerical accuracy, 2014-03-04, CM
;
;  $Id: qtfind.pro,v 1.9 2014/10/20 21:37:08 cmarkwar Exp $
;
;-
; Copyright (C) 2001, 2002, 2014, 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 qtfind, amat

; THIS ROUTINE CONVERTS ROTATION MATRIX AMAT INTO QUATERNION AQT
; IT ASSUMES AMAT IS A VALID ROTATION MATRIX
; THIS IS ADAPTED FROM CHAPTER 12 BY F.L.MARKLEY
;
; MODIFIED 11/22/95 TO AVOID SINGULARITIES (E.G., Q4=0.)
; THE SQUARE OF ONE OF THE QUATERNION ELEMENTS MUST BE >= 0.25
;	SINCE THE 4 SUM TO 1.
; MOD 11/24/95 TO MAKE SURE Q4 >= 0
; MOD 14-DEC-95 TO FIX BUG OF WRONG SIGN OF Q4 IF Q1,Q3,&Q4 < .5

  if n_params() EQ 0 then begin
      info = 1
      USAGE:
      message, 'USAGE:', /info
      message, 'Q = QTFIND(MATRIX)', /info
      message, '   MATRIX must be a 3x3xN array of direction cosines', $
        info=info
      return, 0
  endif

  sz = size(amat)
  if sz(0) LT 2 then begin
      DIM_ERROR:
      
      message, 'ERROR: MATRIX must be a 3x3xN array', /info
      return, 0
  endif
  if sz(1) NE 3 OR sz(2) NE 3 then goto, DIM_ERROR

  nq = n_elements(amat)/9
  ad0 = amat(0,0,*) & ad1 = amat(1,1,*) & ad2 = amat(2,2,*)
  a12 = amat(1,2,*) & a21 = amat(2,1,*)
  a20 = amat(2,0,*) & a02 = amat(0,2,*)
  a01 = amat(0,1,*) & a10 = amat(1,0,*)

  n1 = nq
  q0 = replicate(amat(0)*0+0., nq) & q1 = q0 & q2 = q0 & q3 = q0
  mask = bytarr(nq)

  qd = 1. + ad0 + ad1 + ad2
  wh = where(qd GE 0.99, ct)
  if ct GT 0 then begin
      qx = 0.5*sqrt(qd(wh))
      q3(wh) = qx
      qx = qx * 4
      q0(wh) = (a12-a21)(wh)/qx
      q1(wh) = (a20-a02)(wh)/qx
      q2(wh) = (a01-a10)(wh)/qx
      n1 = n1 - ct
      mask(wh) = 1
  endif 
  if n1 GT 0 then begin
      qd = 1. + ad0 - ad1 - ad2
      wh = where(mask EQ 0 AND qd GE 0.99, ct)
      if ct GT 0 then begin
          qx = 0.5*sqrt(qd(wh))
          q0(wh) = qx
          qx = qx * 4
          q3(wh) = (a12-a21)(wh)/qx
          q2(wh) = (a20+a02)(wh)/qx
          q1(wh) = (a01+a10)(wh)/qx
          n1 = n1 - ct
          mask(wh) = 1
      endif
  endif
  if n1 GT 0 then begin
      qd = 1. - ad0 - ad1 + ad2
      wh = where(mask EQ 0 AND qd GE 0.99, ct)
      if ct GT 0 then begin
          qx = 0.5*sqrt(qd(wh))
          q2(wh) = qx
          qx = qx * 4
          q1(wh) = (a12+a21)(wh)/qx
          q0(wh) = (a20+a02)(wh)/qx
          q3(wh) = (a01-a10)(wh)/qx
          n1 = n1 - ct
          mask(wh) = 1
      endif
  endif
  if n1 GT 0 then begin
      qd = 1. - ad0 + ad1 - ad2
      wh = where(mask EQ 0 AND qd GE 0.99, ct)
      if ct GT 0 then begin
          qx = 0.5*sqrt(qd(wh))
          q1(wh) = qx
          qx = qx * 4
          q2(wh) = (a12+a21)(wh)/qx
          q3(wh) = (a20-a02)(wh)/qx
          q0(wh) = (a01+a10)(wh)/qx
          n1 = n1 - ct
          ;; mask(wh) = 1
      endif
  endif

  wh = where(q3 LT 0, ct)
  if ct GT 0 then begin
      q0(wh) = -q0(wh)
      q1(wh) = -q1(wh)
      q2(wh) = -q2(wh)
      q3(wh) = -q3(wh)
  endif

  return, transpose([[q0],[q1],[q2],[q3]])
end