;+
; $Id: sc2cart.pro,v 1.3 2008/01/16 14:39:11 bewsher Exp $
;
; Project: STEREO-SECCHI
;
; Name: sc2cart
;
; Purpose: To convert spacecraft pointing to Cartesian points in a
;          known reference frame.
;          Note, this is a low level code, and would usually not be
;          called directly. 
;
; Category: STEREO, SECCHI, HI, Calibration
;
; Explanation: For the transformation we use 4x4 transformation
; matrices discussed in e.g. '3D computer graphics' by Alan Watt 
;
; Syntax: out = hi2sc(vec,roll_hi_deg,pitch_hi_deg,yaw_hi_deg)
;
; Example:
;
; Inputs: vec - (3xN) array of vector postions to transform
;         roll_deg - spacecraft roll angle (in degrees)
;         pitch_deg - spacecraft pitch angle (in degrees)
;         yaw_deg - spacecraft yaw angle (in degrees)
;
; Opt. Inputs: None
;
; Outputs: xy    - a 3xn array of transformed vector positions
;
; Opt. Outputs: None
;
; Keywords: None
;
; Calls: None
;
; Common: None
;
; Restrictions: None 
;
; Side effects: None
;
; Prev. Hist.: None
;
; History: 27-Jun-2007 by Daniel Brown
;
; Contact: Daniel Brown (dob@aber.ac.uk) 
;
; $Log: sc2cart.pro,v $
; Revision 1.3  2008/01/16 14:39:11  bewsher
; Modified to be more robust with single point inputs
;
; Revision 1.2  2008/01/04 21:36:22  colaninn
; changed fltarr to dblarr
;
; Revision 1.1  2007/11/28 12:05:54  bewsher
; First releases of hi_calib_point and associated code
;
;-
function sc2cart,vec,roll_deg,pitch_deg,yaw_deg


;sz=size(vec)
;npts=sz(2)
npts = n_elements(vec(0,*))

theta=float(90-pitch_deg)*!dpi/180.
phi=float(yaw_deg)*!dpi/180.
roll=float(roll_deg)*!dpi/180.

; normal direction (los direction)
normx=sin(theta)*cos(phi)
normy=sin(theta)*sin(phi)
normz=cos(theta)

; an 'up' direction ('screen y')
; basic up
vdx=0.
vdy=0.
vdz=1.

; calculate vd.norm
vd_norm=vdx*normx + vdy*normy + vdz*normz

; calculate perpendicular up
vxtmp=vdx - vd_norm*normx
vytmp=vdy - vd_norm*normy
vztmp=vdz - vd_norm*normz

; normalise
ndiv=sqrt(vxtmp^2 + vytmp^2 + vztmp^2)
vx=vxtmp/ndiv
vy=vytmp/ndiv
vz=vztmp/ndiv

; a sideways direction ('screen x')
ux=normy*vz - normz*vy
uy=normz*vx - normx*vz
uz=normx*vy - normy*vx

; location of eye
cx=0.
cy=0.
cz=0.

; transformation matrices for screen space
tmat=[[1., 0., 0., -cx], [0., 1., 0., -cy], [0., 0., 1., -cz], [0., 0., 0., 1.]]
rmat=[[ux, uy, uz, 0.], [vx, vy, vz, 0.], [normx, normy, normz, 0.], [0., 0., 0., 1.]]

; transformation for roll
rollmat=[[cos(roll), -sin(roll), 0., 0.], [sin(roll), cos(roll), 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]

; combine transformation matrices
tview=rollmat##(rmat##tmat)

; now invert the transformation matrix to go from sc to cart
itview=invert(tview,stat)
if (stat ne 0) then print,'matrix inversion failed'

; apply transformation to data
vout=dblarr(4,npts)*0.
for i=0,npts-1 do begin
    vout(*,i)=transpose(itview##transpose(vec(*,i)))
endfor

return,vout
end