rmat2quat.pro (Kronos IDL library)

;+
; Contains the rmat2quat function
;
; :Author:
;    Baptiste Cecconi
;
; :History:
;    2005/01/13: Created [adapted from m2q.f, CalTech (1995)]
;
;    2005/01/13: Last Edit
;-
;
;+
; Find a unit quaternion corresponding to a specified rotation matrix.
;
; :Returns:
;    quat - a quaternion [4] or an array of quaternions [4,n]
;
; :Params:
;    rmat_in: in, required, type='fltarr(3,3)/fltarr(3,3,n)'
;       a rotation matrix [3,3], or an array of rotation 
;       matrices [3,3,n]
;-
FUNCTION rmat2quat,rmat_in
; Checking dimensions
; ++++++++++++++++++++
; rmat must be [3x3] or [3x3xn]

rmat_check=0b

rmat = rmat_in

sr = size(rmat)

if sr(0) eq 2 or sr(0) eq 3 then  if sr(1) eq 3 and sr(2) eq 3 then rmat_check=1b


; Aborting if not passed
; ++++++++++++++++++++++

if not rmat_check then begin
    print,'ERROR, the input rotation matrix [RMAT] is not formatted correctly, please check... '
    print,'ABORTING.'
    quat = -1
    return,quat
endif

; Adjusting dimensions
; ++++++++++++++++++++
;
;   RMAT will be [3x3x1] or [3x3xn]
;  

if sr(0) eq 2 then rmat = reform(rmat,3,3,1)

rmat = double(rmat)

; Initializing QUAT
; +++++++++++++++++
;
;   QUAT will be [4xn] (n=>1)
;  

nn = n_elements(rmat)/9l
quat = dblarr(4,nn)
c = dblarr(nn)
s = dblarr(3,nn)

; Computing Trace of RMAT
; +++++++++++++++++++++++

tr = total(reform((reform(rmat,9,nn))([0,4,8],*),3,nn),1)

mtr = 1.d0 - tr

CC4  = 1.d0 + tr
S004 = mtr + 2.d0*reform(rmat(0,0,*),nn)
S114 = mtr + 2.d0*reform(rmat(1,1,*),nn)
S224 = mtr + 2.d0*reform(rmat(2,2,*),nn)

ww = where(cc4 gt 1.d0)
if ww(0) ne -1 then begin
    nnn = n_elements(ww)
    C(ww)   = sqrt(CC4(ww)/4.d0)
    FACTOR  = 1.d0/(C(ww)*4.d0)

    S(0,ww) = reform(rmat(2,1,ww)-rmat(1,2,ww),nnn) * FACTOR
    S(1,ww) = reform(rmat(0,2,ww)-rmat(2,0,ww),nnn) * FACTOR
    S(2,ww) = reform(rmat(1,0,ww)-rmat(0,1,ww),nnn) * FACTOR
endif 

ww = where(S004 gt 1.d0)
if ww(0) ne -1 then begin
    nnn = n_elements(ww)
    S(0,ww) = sqrt(S004(ww)/4.d0)
    FACTOR  = 1.d0/(S(0,ww)*4.d0)

    C(ww)   = reform(rmat(2,1,ww)-rmat(1,2,ww),nnn) * FACTOR
    S(1,ww) = reform(rmat(0,1,ww)+rmat(1,0,ww),nnn) * FACTOR
    S(2,ww) = reform(rmat(0,2,ww)+rmat(2,0,ww),nnn) * FACTOR
endif 

ww = where(S114 gt 1.d0)
if ww(0) ne -1 then begin
    nnn = n_elements(ww)
    S(1,ww) = sqrt(S114(ww)/4.d0)
    FACTOR  = 1.d0/(S(1,ww)*4.d0)

    C(ww)   = reform(rmat(0,2,ww)-rmat(2,0,ww),nnn) * FACTOR
    S(0,ww) = reform(rmat(0,1,ww)+rmat(1,0,ww),nnn) * FACTOR
    S(2,ww) = reform(rmat(1,2,ww)+rmat(2,1,ww),nnn) * FACTOR
endif 


ww = where(S224 gt 1.d0)
if ww(0) ne -1 then begin
    nnn = n_elements(ww)
    S(2,ww) = sqrt(S224(ww)/4.d0)
    FACTOR  = 1.d0/(S(2,ww)*4.d0)

    C(ww)   = reform(rmat(1,0,ww)-rmat(0,1,ww),nnn) * FACTOR
    S(0,ww) = reform(rmat(0,2,ww)+rmat(2,0,ww),nnn) * FACTOR
    S(1,ww) = reform(rmat(1,2,ww)+rmat(2,1,ww),nnn) * FACTOR
endif

L2 = C*C + total(S*S,1)

ww = where(L2 ne 1.d0)
if ww(0) ne -1 then begin
    POLISH  = 1.d0/(sqrt(L2(ww)))
    C(ww)   = C(ww)*POLISH
    S(0,ww) = S(0,ww)*POLISH
    S(1,ww) = S(1,ww)*POLISH
    S(2,ww) = S(2,ww)*POLISH
endif

quat(0,*) = C
quat(1:3,*) = S

ww = where(C le 0.d0)
if ww(0) ne -1 then quat(*,ww) = -quat(*,ww)

return,quat
end