FUNCTION CvRotation, from_euler=from_euler, from_quaternion=from_quaternion, from_dcm=from_dcm, $
	from_geometric=from_geometric, to_euler=to_euler, to_quaternion=to_quaternion, to_dcm=to_dcm,	$
	to_geometric=to_geometric, degrees=degrees

@compile_opt.pro		; On error, return to caller

;+
; NAME:
;	CvRotation
; PURPOSE:
;	Convert between several ways of specifying rotations
; CATEGORY:
;	gen/idl/toolbox/math
; CALLING SEQUENCE:
;	R = CvRotation( from_euler=from_euler, /to_quaternion, /degrees)
; INPUTS:
; OPTIONAL INPUT PARAMETERS:
;	Only one of the from_* keywords is specified:
;
;	from_euler=from_euler
;				array[3,n]; type: real
;					triplets of Euler angles
;	from_quaternion=from_quaternion
;				array[4,n]; type: real
;					unit quaternion quatruplets with the scalar in array[3,n]
;					(MUST be unit quanternion)
;	from_dcm	array[3,3,n]; type: float
;					direction cosine matrices
;	from_geometric
;				array[3,n] or array[4,n]
;					unit vector, plus angle of rotation around unit vector.
;					array[3,n]: phase angle, latitude and angle of rotation
;					array[4,n]: rectangular coordinates and angle of rotation
;
;	Only one of the /to_* keywords is specified:
;
;	/to_euler	if set then the output is an array[3,n] of Euler angles
;	/to_quaternion
;				if set then the output is an array[4,n] on unit quaternions
;	/to_dcm 	if set then direction cosine matrices array[3,3,n] are returned
;	/to_geometric
;				if set then a unit vector in rectangular coordinates and an
;					angle of rotation around the unit vector, array[4,n]
;					are returned.

;	/degrees	if set, all angles are in degrees (default: radians)
; OUTPUTS:
;	out 		array[3,n], array[4,n]; type: same as input
;					Euler angles, unit quaternion, direction cosine matrix
;					or unit vector with angle of rotation.
; CALLS:
;	InitVar, ToRadians, IsType, boost, AngleRange
; SEE ALSO:
;	EulerRotate
; RESTRICTIONS:
;	The quaternion to Euler angle conversion may still have problems near qx = qy = 0
;	(rotations around the z-axis) and q0 = qz = 0 (rotations around axis in x-y plane.
;	The singularities themselves (strictly qx=qy=0 and strictly q0=qz=0) are dealt
;	with properly.
; PROCEDURE:
;	The Euler angle triplets define rotation around z-axis, y-axis and
;	z-axis respectively.
;
;	A specific rotation can be specified by more then one set of Euler
;	angles or quaternions. For Euler angles [pi+alfa, -beta, pi+gamma] is the same
;	rotation as [alfa, beta, gamma]. For quaternions [-qx,-qy,-qz,-q0] is the same
;	rotation as [qx,qy,qz,q0]. If q = cos(phi/2)+sin(phi/2)*u and
;	-q = cos(chi/2)+sin(chi/2)*v (for unit vectors u and v), then v=-u and
;	chi/2=180-phi/2, i.e. chi=360-phi. So -q is a rotation over -phi after changing
;	the sign of the rotation vector.
;
;	The conversion /to_euler always returns a positive beta in [0,180].
; MODIFICATION HISTORY:
;	FEB-2003, Paul Hick (UCSD/CASS)
;	MAR-2003, Paul Hick (UCSD/CASS; pphick@ucsd.edu)
;		Rewrite. Moved scalar quaternion component from first to last position
;		Improved calculation of quaternions from DCM or Euler angles
;		(avoiding singularities for q0 close to zero).
;-

InitVar, to_euler		, /key
InitVar, to_quaternion	, /key
InitVar, to_dcm			, /key
InitVar, to_geometric	, /key

rpm = ToRadians(degrees=degrees)

; First convert from input spec to unit quaternions

case 1 of

IsType(from_dcm, /defined): begin

	; Convert from direction cosine matrix to unit quaternion

	dims = size(from_dcm, /dim) 					; dims = [3,3,.,...]
	nrot = n_elements(from_dcm)/(dims[0]*dims[1])	; dims[0] = dims[1] = 3

	dcm = reform( from_dcm, [dims[0],dims[1],nrot] )

	dcm_trace = dcm[0,0,*]+dcm[1,1,*]+dcm[2,2,*]

	; The trace of the DCM matrix is 3*q0*q0-(qx*qx+qy*qy+qz*qz) = 4*q0*q0-1
	; If this is positive then abs(q0) > 1/2.

	trace_pos = where(dcm_trace gt 0, complement=trace_neg, ncomplement=ntrace_neg)

	; Quaternion template

	case IsType(from_dcm, /generic_int) of
	0: out = make_array(type=IsType(from_dcm), dim=[4,nrot], /nozero)
	1: out = make_array(type=IsType(0.0     ), dim=[4,nrot], /nozero)
	endcase

	if trace_pos[0] ne -1 then begin

		; Take the positive solution for q0

		tmp = sqrt(1+dcm_trace[trace_pos])
		out[3,trace_pos] = 0.5*tmp
		tmp = 0.5/tmp

		; Difference of terms on opposite sides of diagonal

		dcm_tmp = dcm[*,*,trace_pos]

		out[0,trace_pos] = (dcm_tmp[2,1,*]-dcm_tmp[1,2,*])*tmp
		out[1,trace_pos] = (dcm_tmp[0,2,*]-dcm_tmp[2,0,*])*tmp
		out[2,trace_pos] = (dcm_tmp[1,0,*]-dcm_tmp[0,1,*])*tmp

	endif

	if trace_neg[0] ne -1 then begin

		dcm_tmp = dcm[*,*,trace_neg]

		xx = dcm_tmp[0,0,*]
		yy = dcm_tmp[1,1,*]
		zz = dcm_tmp[2,2,*]

		;big = intarr(ntrace_neg, /nozero)
		big = intarr(ntrace_neg)-1

		xbig = where( xx gt yy and xx gt zz, complement=xnotbig )

		if xbig[0] ne -1 then big[xbig] = 0

		if xnotbig[0] ne -1 then begin

			ybig = where( yy[xnotbig] gt zz[xnotbig], complement=zbig )

			if ybig[0] ne -1 then big[xnotbig[ybig]] = 1 
			if zbig[0] ne -1 then big[xnotbig[zbig]] = 2

		endif

		i = big
		j = (i+1) mod 3
		k = (j+1) mod 3

		n = indgen(ntrace_neg)			; Pick one element from each of matrix

		tmp = sqrt( 1+(dcm_tmp[i,i,n] - (dcm_tmp[j,j,n]+dcm_tmp[k,k,n])) )
		out[i,trace_neg] = 0.5*tmp
		tmp = 0.5/tmp

		out[3,trace_neg] = (dcm_tmp[k,j,n]-dcm_tmp[j,k,n])*tmp
		out[j,trace_neg] = (dcm_tmp[j,i,n]+dcm_tmp[i,j,n])*tmp
		out[k,trace_neg] = (dcm_tmp[k,i,n]+dcm_tmp[i,k,n])*tmp

	endif

end

IsType(from_euler, /defined): begin

	; Convert from Euler angles to unit quaternions

	dims = size(from_euler, /dim)					; dims = [3,.,...]
	nrot = n_elements(from_euler)/dims[0]			; dims[0] = 3

	out = reform( from_euler, [dims[0],nrot] )

	alfa  = out[0,*]*rpm
	beta  = out[1,*]*rpm
	gamma = out[2,*]*rpm

	case IsType(from_euler, /generic_int) of
	0: out = make_array(type=IsType(from_euler),dim=[3,3,nrot], /nozero)
	1: out = make_array(type=IsType(0.0       ),dim=[3,3,nrot], /nozero)
	endcase

	sin_alfa  = sin(alfa )
	cos_alfa  = cos(alfa )
	sin_beta  = sin(beta )
	cos_beta  = cos(beta )
	sin_gamma = sin(gamma)
	cos_gamma = cos(gamma)

	out[0,0,*] =  cos_alfa*cos_beta*cos_gamma-sin_alfa*sin_gamma
	out[1,1,*] = -sin_alfa*cos_beta*sin_gamma+cos_alfa*cos_gamma
	out[2,2,*] =  cos_beta

	out[1,0,*] =  sin_alfa*cos_beta*cos_gamma+cos_alfa*sin_gamma
	out[0,1,*] = -cos_alfa*cos_beta*sin_gamma-sin_alfa*cos_gamma
	
	out[2,0,*] = -sin_beta*cos_gamma
	out[0,2,*] =  cos_alfa*sin_beta

	out[2,1,*] =  sin_beta*sin_gamma
	out[1,2,*] =  sin_alfa*sin_beta

	out = CvRotation(from_dcm=out, /to_quaternion)

	; This is technically correct but runs into numerical problems when q0=0

	;one_plus_cosb = 1+cos(beta)
	;sinb = sin(beta)
	;alfa_plus_gamma = alfa+gamma

	; Sum of terms along diagonal is 4*q0*q0-1
	; Take the positive solution for q0

	;q0 = 0.5*sqrt( one_plus_cosb*(1+cos(alfa_plus_gamma)) )

	; Difference of terms on opposite sides of diagonal

	;qx = -sinb*(sin(alfa)-sin(gamma))/(4*q0)
	;qy =  sinb*(cos(alfa)+cos(gamma))/(4*q0)
	;qz =  one_plus_cosb*sin(alfa_plus_gamma)/(4*q0)

	;out = reform([ qx, qy, qz, q0 ], 4, nrot)

end

IsType(from_geometric, /defined): begin

	dims = size(from_geometric, /dim)				; dims = [4,.,..]
	nrot = n_elements(from_geometric)/dims[0]

	out = reform( from_geometric, [dims[0],nrot] )
	if IsType(out, /generic_int) then out = float(out)

	if dims[0] eq 3 then begin
		boost, out, replicate(1, nrot)
		out = out[[0,1,3,2],*] 
		out[0:2,*] = cv_coord(from_sphere=out[0:2,*], /to_rect)
	endif else	$
		out[0:2,*] = out[0:2,*]/(replicate(1,3)#sqrt(total(out[0:2,*]*out[0:2,*],1)))

	phi  = out[3,*]/2.0*rpm
	sinp = sin(phi)

	;out = [out[0,*]*sp, out[1,*]*sp, out[2,*]*sp, cos(phi)]
	out = [out[0:2,*]*[sinp,sinp,sinp], cos(phi)]

end

IsType(from_quaternion, /defined): begin

	; Convert from quaternion to unit quaternion

	dims = size(from_quaternion, /dim)				; dims = [4,.,..]
	nrot = n_elements(from_quaternion)/dims[0]		; dims[0] = 4

	out = reform( from_quaternion, [dims[0],nrot] )
	if IsType(out, /generic_int) then out = float(out)

	; Make sure the quaternions are already unit quaternions

	;out = out/(replicate(1,dims[0])#sqrt(total(out*out,1)))

end

endcase



; Convert from unit quaternions to output spec

n_dim_one_rot = 1+IsType(from_dcm, /defined)		; # dimensions in single rotation in input

case 1 of

to_dcm: begin

	qx = out[0,*]
	qy = out[1,*]
	qz = out[2,*]
	q0 = out[3,*]

	q00 = q0*q0
	qxx = qx*qx
	qyy = qy*qy
	qzz = qz*qz

	q0x = q0*qx
	q0y = q0*qy
	q0z = q0*qz

	qxy = qx*qy
	qxz = qx*qz
	qyz = qy*qz
	
	out = make_array( type=IsType(out), dim=[3,3,nrot] )

	out[0,0,*] = q00+qxx-qyy-qzz
	out[1,1,*] = q00-qxx+qyy-qzz
	out[2,2,*] = q00-qxx-qyy+qzz

	out[1,0,*] = 2*(qxy+q0z)
	out[0,1,*] = 2*(qxy-q0z)

	out[2,0,*] = 2*(qxz-q0y)
	out[0,2,*] = 2*(qxz+q0y)

	out[2,1,*] = 2*(qyz+q0x)
	out[1,2,*] = 2*(qyz-q0x)

	dim_one_rot = [3,3] 							; Dimension of single rotation in output

end

to_euler: begin

	qx = out[0,*]
	qy = out[1,*]
	qz = out[2,*]
	q0 = out[3,*]

	q0x = q0*qx
	q0y = q0*qy
	qxz = qx*qz
	qyz = qy*qz

	out = [ atan( qyz-q0x , qxz+q0y ) , acos( (q0*q0-qx*qx-qy*qy+qz*qz) > (-1) < 1 ), atan( qyz+q0x , -qxz+q0y ) ]/rpm

	; The above solution runs into problems when the arguments of the atan are both close to zero,
	; e.g. if qx = qy = 0 (rotations around the z-axis) or q0 = qz = 0 (rotations around axis
	; in x-y plane. This takes cares of the worst:

	zero = where( qx eq 0 and qy eq 0, nzero)
	if nzero ne 0 then begin
		tmp = replicate(0,1,nzero)
		out[*,zero] = [ 2*atan(qz[0,zero],q0[0,zero])/rpm, tmp, tmp]
	endif

	zero = where( qz eq 0 and q0 eq 0, nzero)
	if nzero ne 0 then begin
		tmp = atan(qx[0,zero],qy[0,zero])
		out[*,zero] = [ -tmp , replicate(!pi,1,nzero), tmp]/rpm
	endif

	dim_one_rot = 3

end

to_geometric: begin

	phi = acos(out[3,*] > (-1) < 1)

	out = [out[0:2,*]/(replicate(1,3)#sin(phi)), AngleRange(2*phi,/pi)/rpm]

	dim_one_rot = 4
		
end

to_quaternion: begin

	dim_one_rot = 4

end

endcase

if n_elements(dims) gt n_dim_one_rot then dims = [dim_one_rot,dims[n_dim_one_rot:*]] else dims = dim_one_rot
out = reform(out, dims, /overwrite)

return, out  &  end