C $Id: icss_payload_to_gse.f,v 1.2 1998/07/24 21:54:40 asc Exp ad $
C+
C
C  UNIT NAME: ICSS_PAYLOAD_TO_GSE 
C 
C  PURPOSE:  PERFORM COORDINATE TRANSFORMATION FROM NON-SPINNING PAYLOAD 
C            COORDINATES TO GSE COORDINATES 
C
C  UNIT TYPE:  FORTRAN SUBROUTINE
C
C  INVOCATION METHOD:  CALL ICSS_PAYLOAD_TO_GSE (PAYLOAD_POS,
C                                                GSE_ATT_POS,
C                                                ROTATION_MATRIX,
C                                                GSE_POS,
C                                                VEL_FLAG,
C                                                SC_VEL)

C
C  ARGUMENT LIST:
C
C  NAME	                 TYPE     USE	  DESCRIPTION
C  ----                  ----     ---     -----------
C  PAYLOAD_POS(3)        R*8      I       PAYLOAD COORDINATES TO BE TRANSFORMED
C  GSE_ATT_POS(2)        R*4      I       GSE ATTITUDE ANGLES (RA, DECL IN RAD)
C                                         AT THE EPOCH TIME OF THE SPACECRAFT
C                                         COORDINATES
C  ROTATION_MATRIX(9)    R*8      O       ROTATION MATRIX
C  GSE_POS(3)            R*8      O       TRANSFORMED GSE COORDINATES
C  VEL_FLAG              I*4      I       FLAG TO SHOW WHETHER THE QUANTITY
C                                          BEING TRANSFORMED IS A VELOCITY.
C                                          VALUES:
C                                            = 0, not a velocity, do not
C                                                 add translation vector.
C                                            > 0, is a velocity, add in
C                                                 spacecraft velocity
C                                                 vector.
C  SC_VEL(3)             R*8      I      SPACECRAFT VELOCITY IN GSE
C                                         COORDINATES. USED WHEN VEL_FLAG
C                                         IS NONZERO.
C
C  FILE/RECORD REFERENCES: NONE
C
C  NAME 		    USE		 DESCRIPTION
C  ----	    	            ---          -----------
C  
C  EXTERNAL VARIABLES: NONE
C
C  EXTERNAL REFERENCES:
C
C  MULMAT    - ROUTINE THAT MULTIPLIES TWO MATRICES
C
C  ABNORMAL TERMINATION CONDITIONS, ERROR MESSAGES: NONE
C
C  ASSUMPTIONS, CONSTRAINTS, RESTRICTIONS: NONE
C
C  DEVELOPMENT HISTORY:
C
C  AUTHOR	CHANGE ID      RELEASE	 DATE	   DESCRIPTION OF CHANGE
C  ------	---------      -------   ----	   ---------------------
C  M. WONG      CCR 80         B5R3      9/14/92   ORIGINAL IMPLEMENTATION
C  B. SAMUELSON SPOF PORT      NONE      04/11/94  Change: required to port
C     (CSC)     (see notes)                          icss routines to SPOF
C  B. SAMUELSON UNIX PROT                09/18/96  added C Raymond's
C                                                  velocity transforation 
C                                                  change to unix version
C
C  NOTES:
C
C     1.  The changes recorded under ID SPOF-PORT are required to make the
C         ICSS coordinate conversion routines, originally developed under
C         VAX-VMS 5.4 run on the UNIX-based workstations of the SPOF.  (Sun
C         SPARCstations and DEC DECstations).  The changes are as follows:
C            a.  Delete references to ICSS_INC
C            b.  Define Message texts or files to correspond to the messages
C                ICSS_SUCCESSFUL, etc. which are embedded in the error handling.
C            c.  Remove references to the NAG routines F01CRF and F01CKF (matrix
C                transposition and matrix multiplication routines).
C     2.  In addition, to successfully run the software packages, copies of the
C         Solar/Lunar/Planetary (SLP) file and timing coefficients file (TCC)
C         must be ported onto the SPOF.
C
C-
C  
C  PDL:
C
C  BUILD the transformation matrix from GSE right ascension and GSE declination
C   angles
C  CALL MULMAT to multiply the transformation matrix by payload coordinates to
C   get GSE coordinates
C  IF ( velocity flag set )
C  THEN
C     ADD spacecraft GSE velocity to output
C  ENDIF
C  RETURN
C
C
C      OPTIONS/EXTEND_SOURCE
      SUBROUTINE ICSS_PAYLOAD_TO_GSE (PAYLOAD_POS, GSE_ATT_POS,
     .                           ROTATION_MATRIX, GSE_POS, VEL_FLAG,
     .                           SC_VEL)

C
      IMPLICIT NONE
C
C-- ARGUMENT DECLARATIONS
C
      REAL*8  PAYLOAD_POS(3)
      REAL*4  GSE_ATT_POS(2)
      REAL*8  ROTATION_MATRIX(9)
      REAL*8  GSE_POS(3)
      INTEGER*4 VEL_FLAG
      REAL*8  SC_VEL(3)
C
C-- LOCAL VARIABLE DECLARATIONS
C
      REAL*8  RAC                                    !GSE RIGHT ASCENSION
      REAL*8  DEC                                    !GSE DECLINATION
      REAL*8  EPX, EPY, EPZ                          !TEMPORARY VARIABLES
      REAL*8  COSA, SINA, COSD, SIND                 !ELEMENTS FOR BUILDING
                                                     !TRANSFORMATION MATRIX
      REAL*8  TRANSFORM_MATRIX(3,3)                  !TRANSFORMATION MATRIX

      INTEGER*4  I                                   !DO-LOOP COUNTER

C
C-----------------------------------BEGIN EXECUTABLE CODE ICSS_PAYLOAD_TO_GSE---
C
C-- CONVERT TO R*8 VALUES
C
      RAC = DBLE(GSE_ATT_POS(1))
      DEC = DBLE(GSE_ATT_POS(2))
C
      EPX = DCOS(DEC) * DCOS(RAC)
      EPY = DCOS(DEC) * DSIN(RAC)
      EPZ = DSIN(DEC)
C
C-- COMPUTE SINE VALUES AND COSINE VALUES OF ANGLES A AND D
C
      COSA = DSQRT(EPY*EPY + EPZ*EPZ)
      SINA = EPX
      COSD = EPZ/COSA
      SIND = EPY/COSA
C
C-- CONSTRUCT THE TRANSFORMATION MATRIX
C
      TRANSFORM_MATRIX(1,1) = COSA
      TRANSFORM_MATRIX(1,2) = 0.
      TRANSFORM_MATRIX(1,3) = SINA
      TRANSFORM_MATRIX(2,1) = -SINA * SIND
      TRANSFORM_MATRIX(2,2) = COSD
      TRANSFORM_MATRIX(2,3) = COSA * SIND
      TRANSFORM_MATRIX(3,1) = -SINA * COSD
      TRANSFORM_MATRIX(3,2) = -SIND
      TRANSFORM_MATRIX(3,3) =  COSA * COSD
C
C-- RETURN THE TRANSFORMATION MSTRIX
C
      ROTATION_MATRIX(1) = TRANSFORM_MATRIX(1,1)
      ROTATION_MATRIX(2) = TRANSFORM_MATRIX(2,1)
      ROTATION_MATRIX(3) = TRANSFORM_MATRIX(3,1)
      ROTATION_MATRIX(4) = TRANSFORM_MATRIX(1,2)
      ROTATION_MATRIX(5) = TRANSFORM_MATRIX(2,2)
      ROTATION_MATRIX(6) = TRANSFORM_MATRIX(3,2)
      ROTATION_MATRIX(7) = TRANSFORM_MATRIX(1,3)
      ROTATION_MATRIX(8) = TRANSFORM_MATRIX(2,3)
      ROTATION_MATRIX(9) = TRANSFORM_MATRIX(3,3)
C
C-- COMPUTE TRANSFORMED GSE COORDINATES
C
      CALL MULMAT (TRANSFORM_MATRIX, PAYLOAD_POS, 3, 3, 1, GSE_POS)
      IF ( VEL_FLAG .NE. 0 ) THEN
         DO I = 1, 3
            GSE_POS(I) = GSE_POS(I) + SC_VEL(I)
         END DO
      ENDIF
C
C-- EXIT
C
      RETURN
      END
