;+
;
; Project   : s3drs reconstruction software
;                   
; Name      : (various test routines)
;               
; Purpose   : part of the s3drs test kit
;               
; Explanation: (archived here for completeness, not intended for users use)
;               
; Use       : (self-documented, see notes in code)
;    
; Written     : Sandy Antunes, NRC, 2005-2009
;               
;-            
;;
;; Return rotation matrix such that
;; rot3dmatrix([alphax,alphay,alphaz]) ## [X,Y,Z] gives your [X,Y,Z]
;; rotated alphax radians about the x axis, then alphay radians about
;; the y axis, and finally alphaz radians about the z axis. Note that
;; the axes are kept fixed. This allows an inverse operation to to
;; return alphax, alphay, alphaz from a given rotation matrix.

FUNCTION rot3dmatrix_angles,m

  m = double(m)

;  From mathematica:
;
;  mm = [[Cy Cz, Cz Sx Sy - Cx Sz, Cx Cz Sy + Sx Sz   ], $
;        [Cy Sz, Cx Cz + Sx Sy Sz, -(Cz Sx) + Cx Sy Sz],$
;        [-Sy  , Sx Cy           ,     Cx Cy          ]]

  cosyzero = (m(1,2) EQ 0 AND m(2,2) EQ 0)

  IF NOT cosyzero THEN BEGIN

     cys = 1

     xfind = atan(cys*m(1,2),cys*m(2,2)) ;; May be wrong quadrants!
     zfind = atan(cys*m(0,1),cys*m(0,0))

     sinx = sin(xfind) & sxgood = (abs(sinx) GT 0.05)
     cosx = cos(xfind) & cxgood = (abs(cosx) GT 0.05)

     wt = double(sxgood+cxgood)
     cosy = ((sxgood ? m(1,2)/sinx : 0)+(cxgood ? m(2,2)/cosx : 0))/wt

     yfind = atan(-m(0,2),cosy)

     return,[xfind,yfind,zfind]
  END

  ;; Cos[y] == 0 => yfind = +/- Pi/2

  yfind = atan(-m(0,2),0)

  ;; From mathematica we find that

  ;; MatrixForm[TrigFactor[r[x,+/-Pi/2,z]]]
  ;;
  ;; = [[ 0,  +/-Sin[x-z], +/-Cos[x+z]  ],$
  ;;    [ 0,     Cos[x-z],    -Sin[x-z] ],$
  ;;    [ -/+1,         0,           0  ]]

  ;; We thus arbitrarily set z = 0 and get:

  zfind = 0
  xfind = atan(-m(2,1),m(1,1))

  return,[xfind,yfind,zfind]
END