C $Id: icss_velocity_trans.f,v 1.3 1998/07/24 21:54:42 asc Exp $
CCCC
C
C  ICSS_VELOCITY_TRANS - Used to convert velocities from one
C	             coordinate system to another.
C 
C  PURPOSE:  Vectors in GCI, GSE, GSM, GEO, HSEb(earth-oriented), or HS
C	     coordinates are accepted and transformed into the user's choice
C	     of GCI, GSE, GSM, GEO, HSEb or HS coordinates. However, not all
C            permutations are implemented.
C
C  UNIT TYPE:  SUBROUTINE
C
C  INVOCATION METHOD:  CALL ICSS_VELOCITY_TRANS   (src_sys, 
C                                              target_sys,
C	                                       orb_pos_vel, 
C                                              orb_pos_time, 
C                                              trans_orb_pos_vel,
C					       rotation_matrix,
C                                              velocity_req,
C                                              trans_orb_stat)
C
C  ARGUMENT LIST:
C
C  NAME	             TYPE     USE     DESCRIPTION
C  ----              ----     ---     -----------
C  SRC_SYS           I*4      I       COORDINATE SYS OF ENTERED ORBIT VECTOR
C  TARGET_SYS        I*4      I       REQ. COORD. SYS OF ORBIT VECTOR
C  ORB_POS_VEL(6)    R*8      I       ORBIT VECTOR TO BE TRANSFORMED
C  ORB_POS_TIME(2)   I*4      I       TIME OF ORB. VECTOR, YEAR-DAY,MILLI OF DAY
C  TRANS_ORB_POS_VEL
C              (6)   R*8      O       TRANSFORMED ORBIT VECTOR
C  ROTATION_MATRIX(9)R*8      O	      ROTATION MATRIX
C  VELOCITY_REQ      I*4      I       FLAG SHOWING THAT VELOCITIES ARE TO
C                                      BE TRANSFORMED
C  TRANS_ORB_STAT    I*4      O       FLAG SHOWING STATUS OF TRANS. REQUEST
C
C  FILE/RECORD REFERENCES:  NONE
C
C  EXTERNAL VARIABLES:  NONE
C
C  EXTERNAL REFERENCES:
C    IC_VAL_TRANS_ORB_REQ   Validates the source and target coordinate systems
C    IC_GCI_TO_GEO          Returns the GCI to GEO transformation matrix
C    IC_GCI_TO_GSE	    Returns the GCI to GSE transformation matrix
C    IC_GCI_TO_GSM	    Returns the GCI to GSM transformation matrix
C    MXTRP8		    Routine that transposes matrices, but also
C                            inverts rotation matrices (the matrix that
C                            transforms a vector from one system to another)
C    IC_GSE_TO_GEO	    Returns the GSE to GEO transformation matrix
C    IC_GSE_TO_GSM	    Returns the GSE to GSM transformation matrix
C    IC_GSM_TO_GEO	    Returns the GSM to GEO transformation matrix
C    IC_GSE_TO_HSEB         Returns the GSE to HSEb transformation matrix
C                             (need to shift origin also)
C    IC_HSEB_TO_HS          Returns the HSEB to HS transformation matrix
C    MULMAT		    Routine that multiplies two matrices
C    STR$UPCASE		    Converts a string to all upper case
C
C  ABNORMAL TERMINATION CONDITIONS, ERROR MESSAGES:
C
C  THE RETURN STATUS IS SET TO A NON-ZERO STATUS ROM A SUBORDINATE
C  UNIT THAT RETURNS AN ERROR
C
C ASSUMPTIONS, CONSTRAINTS, RESTRICTIONS:  Only the following transformations
C are implemented, currently:
C GCI to GEO
C GCI to GSE
C GCI t0 GSM
C GSE to GCI
C GSE to GEO
C GSE to GSM
C GSE to HSEb
C GSM to GEO
C GSM to GSM
C GSM to GSE
C HSEb to HS
C
C  DEVELOPMENT HISTORY
C
C  AUTHOR	CHANGE ID	RELEASE	  DATE	    DESCRIPTION OF CHANGE
C  ------	---------	-------   ----	    ---------------------
C  C. RAYMOND   Orig. unit      Rel 3.1   7/25/93   New unit based on ICSS_
C                                                    TRANSF_ORB;  created
C                                                    to eliminate the need
C                                                    for calling sequence 
C                                                    changes in KPGS.
C  B. SAMUELSON SPOF PORT                 05/02/94  Change: required to port
C     (CSC)     (see notes)                          icss routines to SPOF
C
C  A. DAVIS     ACE SCIENCE CENTER        08/15/97  Added HS, HSE
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     4.  This version uses numbers to indicate source an target coordinate
C         systems instead of characters to simplify conversion to different
C         platforms. The values are as follows:
C             GCI = 1
C             GSE = 2
C             GSM = 3
C             GEO = 4
C             HSEb = 5
C             HSEa = 6
C             HS  = 7
C
CCCC
C
C  PDL:
C
C  Set TRANS_ORB_STAT to successful
C  Validate SRC_SYS and TARGET_SYS
C  if an error in validation then
C     if error is that the source system equals the target system then
C        TRANS_ORB_POS = ORB_POS_VEL
C     endif
C     ABORT TO 990
C  endif
C
C  docase KEY ON ORBIT SOURCE SYSTEM
C  case 1 (SRC_SYS is equal to 'GCI')
C
C     docase SOURCE SYS IS GCI, GET CORRESPONDING ORBIT TARGET SYSTEM
C     case 1 (TARGET_SYS is equal to 4 (GEO))
C        call IC_GCI_TO_GEO to get the gci to geo TRANSFORM_MATRIX
C     case 2 (TARGET_SYS is equal to 2 (GSE))
C        call IC_GCI_TO_GSE to get the gci to gse TRANSFORM_MATRIX
C     case 3 (TARGET_SYS is equal to 3 (GSM))
C        call IC_GCI_TO_GSM to get the gci to gsm TRANSFORM_MATRIX
C     enddo
C
C  case 2 (SRC_SYS is equal to 2 (GSE))
C
C     docase SOURCE SYS IS GSE, GET CORRESPONDING ORBIT TARGET SYSTEM
C     case 1 (TARGET_SYS is equal to 1 (GCI))
C        call IC_GCI_TO_GSE to get the gci to gse TRANSFORM_MATRIX
C        call MXTRP8 routine to invert the TRANSFORM_MATRIX resulting
C         in the gse to gci TRANSFORM_MATRIX
C     case 2 (TARGET_SYS is equal to 4 (GEO))
C        call IC_GSE_TO_GEO to get the gse to geo TRANSFORM_MATRIX
C     case 3 (TARGET_SYS is equal to 3 (GSM))
C        call IC_GSE_TO_GSM to get the gse to gsm TRANSFORM_MATRIX
C     case 4 (TARGET_SYS is equal to HSEb)
C        call IC_GSE_TO_HSEb to get the GSE to HSEb TRANSFORM_MATRIX
C     enddo
C
C  case 3 (SRC_SYS is equal to 3 (GSM))
C
C     docase SOURCE SYS IS GSM, GET CORRESPONDING ORBIT TARGET SYSTEM
C     case 1 (TARGET_SYS is equal to 1 (GCI))
C        call IC_GCI_TO_GSM to get the gci to gsm TRANSFORM_MATRIX
C        call MXTRP8 routine to invert the TRANSFORM_MATRIX resulting
C         in the gsm to gci TRANSFORM_MATRIX
C     case 2 (TARGET_SYS is equal to 4 (GEO))
C        call IC_GSM_TO_GEO to get the gsm to geo TRANSFORM_MATRIX
C     case 3 (TARGET_SYS is equal to 2 (GSE))
C        call IC_GSE_TO_GSM to get the gse to gsm TRANSFORM_MATRIX
C        call MXTRP8 routine to invert the TRANSFORM_MATRIX resulting
C         in the gsm to gse TRANSFORM_MATRIX
C     enddo
C
C  case 4 (SRC_SYS is equal to GEO)
C     Not implemented
C
C  case 5 (SRC_SYS is equal to HSEb)
C     docase SOURCE SYS IS HSEb, GET CORRESPONDING ORBIT TARGET SYSTEM
C     case 1 (TARGET_SYS is equal to HS)
C        call IC_HSEb_TO_HS to get the HSEb to HS TRANSFORM_MATRIX
C     propose HSEb->GSE->GCI->Whatever (not implemented)
C     enddo
C
C  case 6 (SRC_SYS is equal to HS)
C        propose HS->HSEb->GSE->GCI->Whatever (not implemented)
C  enddo
C
C  if getting the transformation matrix was not successful abort to 990
C
C  call MULMAT routine to multiply the TRANSFORM_MATRIX and 
C   the given ORB_POS vector.  The result will be the TRANS_ORB_POS.
C
C  Return the transformation matrix in 9 element single dimension format
C
C990 CONTINUE
C
C  RETURN
C
CCCC
	subroutine ICSS_VELOCITY_TRANS(src_sys, 
     -				       target_sys,
     -				       orb_pos_vel, 
     -				       orb_pos_time, 
     -				       trans_orb_pos_vel, 
     -				       rotation_matrix,
     -                                 velocity_req,
     -				       trans_orb_stat)
C
	implicit NONE
C
C*  Calling Parameters
C
        integer*4	SRC_SYS	!coord sys of entered orbit vector
	integer*4       TARGET_SYS  !req coord sys of orbit vector
	real*8		ORB_POS_VEL(6)  !orbit vector to be transformed
	real*8		TRANS_ORB_POS_VEL(6)!transformed orbit vector
	integer*4	ORB_POS_TIME(2) !time of orb vector, year-day, milli
	real*8		ROTATION_MATRIX(9)  !Rotation matrix
	integer*4	TRANS_ORB_STAT  !status of transfer request
        integer*4       velocity_req    !flag to show velocities needed
        integer*4       STATUS
	real*8		trans_pos_vec(3)
C
C*  Other Variables
C
	real*8		TRANSFORM_MATRIX(3,3) !Transformation Matrix, Name is
C					!generic to allow a single matrix
C      					!multiplication statement
        real*8          TEMP_MATRIX(3,3)!temporary result matrix
        real*8          TEMP_MATRIX2(3,3)!temporary result matrix 2
        real*8          CORR_MAT(3,3)   !correction matrix
	real*8          OUT_VEL(3)      !Output velocity working vector
	real*8          CORR_VEL(3)     !velocity corrections
	real*8          DERIV_MATRIX(3,3) ! Holds time derivative matrix 
                                        !returned from lower level routines
	integer*4       I,J             !Do-loop indices
C
C*  Begin executable code
C
	TRANS_ORB_STAT = 0
	STATUS = 0
C
C*  Validate source system and target system
C
        if ((SRC_SYS .lt. 1) .or. (SRC_SYS .gt. 8)) then
           TRANS_ORB_STAT = 1
           goto 990
        else if ((TARGET_SYS .lt. 1) .or. (TARGET_SYS .gt. 8)) then
           TRANS_ORB_STAT = 1
           goto 990
        endif 
	if (SRC_SYS .eq. TARGET_SYS) then
	   TRANS_ORB_POS_VEL(1) = ORB_POS_VEL(1)
	   TRANS_ORB_POS_VEL(2) = ORB_POS_VEL(2)
	   TRANS_ORB_POS_VEL(3) = ORB_POS_VEL(3)
	   TRANS_ORB_POS_VEL(4) = ORB_POS_VEL(4)
	   TRANS_ORB_POS_VEL(5) = ORB_POS_VEL(5)
	   TRANS_ORB_POS_VEL(6) = ORB_POS_VEL(6)
           goto 990
	endif
C
C*  Get the appropriate transformation matrix
C
	if (SRC_SYS .eq. 1) then
	   if (TARGET_SYS .eq. 4) then
	      call IC_GCI_TO_GEO (ORB_POS_TIME, TRANSFORM_MATRIX,
     1                            DERIV_MATRIX, VELOCITY_REQ)
	   else if (TARGET_SYS .eq. 2) then
	      call IC_GCI_TO_GSE (ORB_POS_TIME, 
     1                            TRANSFORM_MATRIX, DERIV_MATRIX,
     2                            VELOCITY_REQ, STATUS)
	   else if (TARGET_SYS .eq. 3) then
	      call IC_GCI_TO_GSM (ORB_POS_TIME, 
     1                            TRANSFORM_MATRIX, DERIV_MATRIX,
     2                            VELOCITY_REQ, STATUS)
	   else if (TARGET_SYS .eq. 5) then
	      call IC_GCI_TO_GSE (ORB_POS_TIME, 
     1                 TEMP_MATRIX, DERIV_MATRIX,VELOCITY_REQ, STATUS)
              call MULMAT(TEMP_MATRIX,ORB_POS_VEL,3,3,1,
     1                                             TRANS_ORB_POS_VEL)
	      call IC_GSE_TO_HSEB (ORB_POS_TIME, 
     1             TEMP_MATRIX2, DERIV_MATRIX,VELOCITY_REQ, STATUS,
     2                                    trans_pos_vec)
C
              call MULMAT(TEMP_MATRIX2,TRANS_ORB_POS_VEL,3,3,1,
     1                                            ORB_POS_VEL)
              call MATADD(ORB_POS_VEL,trans_pos_vec,3,1,
     1                                            TRANS_ORB_POS_VEL)
C
              call MULMAT(TEMP_MATRIX2,TEMP_MATRIX,3,3,3,
     1                                             TRANSFORM_MATRIX)
C                  NO VELOCITY YET
              TRANS_ORB_POS_VEL(4) = 0.D0
              TRANS_ORB_POS_VEL(5) = 0.D0
              TRANS_ORB_POS_VEL(6) = 0.D0
           else ! Unimplemented target
              STATUS = 1
	   endif
	else if (SRC_SYS .eq. 2) then
	   if (TARGET_SYS .eq. 1) then
	      call IC_GCI_TO_GSE (ORB_POS_TIME, 
     1                            TRANSFORM_MATRIX, DERIV_MATRIX,
     2                            VELOCITY_REQ, STATUS)
	      call MXTRP8(TRANSFORM_MATRIX, TEMP_MATRIX, 3, 3)
              TRANSFORM_MATRIX(1,1) = TEMP_MATRIX(1,1)
              TRANSFORM_MATRIX(2,1) = TEMP_MATRIX(2,1)
              TRANSFORM_MATRIX(3,1) = TEMP_MATRIX(3,1)
              TRANSFORM_MATRIX(1,2) = TEMP_MATRIX(1,2)
              TRANSFORM_MATRIX(2,2) = TEMP_MATRIX(2,2)
              TRANSFORM_MATRIX(3,2) = TEMP_MATRIX(3,2)
              TRANSFORM_MATRIX(1,3) = TEMP_MATRIX(1,3)
              TRANSFORM_MATRIX(2,3) = TEMP_MATRIX(2,3)
              TRANSFORM_MATRIX(3,3) = TEMP_MATRIX(3,3)
	      call MULMAT(DERIV_MATRIX,TRANSFORM_MATRIX,3,3,3,CORR_MAT)
              call MULMAT(TRANSFORM_MATRIX,CORR_MAT,3,3,3,DERIV_MATRIX)
              do i=1,3
                 do j=1,3
                    DERIV_MATRIX(i,j) = - DERIV_MATRIX(i,j)
                 enddo
              enddo
	   else if (TARGET_SYS .eq. 4) then
	      call IC_GSE_TO_GEO (ORB_POS_TIME, 
     1                            TRANSFORM_MATRIX, DERIV_MATRIX,
     2                            VELOCITY_REQ,STATUS)
	   else if (TARGET_SYS .eq. 3) then
	      call IC_GSE_TO_GSM (ORB_POS_TIME, 
     1                            TRANSFORM_MATRIX, DERIV_MATRIX,
     2                            VELOCITY_REQ, STATUS)
	   else if (TARGET_SYS .eq. 5) then

	      call IC_GSE_TO_HSEb (ORB_POS_TIME, 
     1             TRANSFORM_MATRIX, DERIV_MATRIX,VELOCITY_REQ, STATUS,
     2                                    trans_pos_vec)
c
              call MULMAT(TRANSFORM_MATRIX,orb_pos_vel,3,3,1,
     1                                           TRANS_ORB_POS_VEL)
              call MATADD(TRANS_ORB_POS_VEL,trans_pos_vec,3,1,
     1                                          ORB_POS_VEL)
c
              TRANS_ORB_POS_VEL(1) = ORB_POS_VEL(1)
              TRANS_ORB_POS_VEL(2) = ORB_POS_VEL(2)
              TRANS_ORB_POS_VEL(3) = ORB_POS_VEL(3)
C                  NO VELOCITY YET
              TRANS_ORB_POS_VEL(4) = 0.
              TRANS_ORB_POS_VEL(5) = 0.
              TRANS_ORB_POS_VEL(6) = 0.

           else !Unimplemented target
	      STATUS = 1
	   endif
	else if (SRC_SYS .eq. 3) then
	   if (TARGET_SYS .eq. 1) then
	      call IC_GCI_TO_GSM (ORB_POS_TIME, 
     1                            TRANSFORM_MATRIX, DERIV_MATRIX,
     2                            VELOCITY_REQ, STATUS)
	      call MXTRP8(TRANSFORM_MATRIX, TEMP_MATRIX, 3, 3)
              TRANSFORM_MATRIX(1,1) = TEMP_MATRIX(1,1)
              TRANSFORM_MATRIX(2,1) = TEMP_MATRIX(2,1)
              TRANSFORM_MATRIX(3,1) = TEMP_MATRIX(3,1)
              TRANSFORM_MATRIX(1,2) = TEMP_MATRIX(1,2)
              TRANSFORM_MATRIX(2,2) = TEMP_MATRIX(2,2)
              TRANSFORM_MATRIX(3,2) = TEMP_MATRIX(3,2)
              TRANSFORM_MATRIX(1,3) = TEMP_MATRIX(1,3)
              TRANSFORM_MATRIX(2,3) = TEMP_MATRIX(2,3)
              TRANSFORM_MATRIX(3,3) = TEMP_MATRIX(3,3)
	      call MULMAT(DERIV_MATRIX,TRANSFORM_MATRIX,3,3,3,CORR_MAT)
              call MULMAT(TRANSFORM_MATRIX,CORR_MAT,3,3,3,DERIV_MATRIX)
              do i=1,3
                 do j=1,3
                    DERIV_MATRIX(i,j) = - DERIV_MATRIX(i,j)
                 enddo
              enddo
	   else if (TARGET_SYS .eq. 4) then
	      call IC_GSM_TO_GEO (ORB_POS_TIME, 
     1                            TRANSFORM_MATRIX, DERIV_MATRIX,
     2                            VELOCITY_REQ, STATUS)
           else if (TARGET_SYS .eq. 2) then
	      call IC_GSE_TO_GSM (ORB_POS_TIME, 
     1                            TRANSFORM_MATRIX, DERIV_MATRIX,
     2                            VELOCITY_REQ, STATUS)
	      call MXTRP8(TRANSFORM_MATRIX, TEMP_MATRIX, 3, 3)
              TRANSFORM_MATRIX(1,1) = TEMP_MATRIX(1,1)
              TRANSFORM_MATRIX(2,1) = TEMP_MATRIX(2,1)
              TRANSFORM_MATRIX(3,1) = TEMP_MATRIX(3,1)
              TRANSFORM_MATRIX(1,2) = TEMP_MATRIX(1,2)
              TRANSFORM_MATRIX(2,2) = TEMP_MATRIX(2,2)
              TRANSFORM_MATRIX(3,2) = TEMP_MATRIX(3,2)
              TRANSFORM_MATRIX(1,3) = TEMP_MATRIX(1,3)
              TRANSFORM_MATRIX(2,3) = TEMP_MATRIX(2,3)
              TRANSFORM_MATRIX(3,3) = TEMP_MATRIX(3,3)
	      call MULMAT(DERIV_MATRIX,TRANSFORM_MATRIX,3,3,3,CORR_MAT)
              call MULMAT(TRANSFORM_MATRIX,CORR_MAT,3,3,3,DERIV_MATRIX)
              do i=1,3
                 do j=1,3
                    DERIV_MATRIX(i,j) = - DERIV_MATRIX(i,j)
                 enddo
              enddo
	   else ! Unimplemented target
	      STATUS = 1
	   endif
	else if (SRC_SYS .eq. 4) then
C          Unimplemented
           STATUS = 1
	else if (SRC_SYS .eq. 5) then
           if (TARGET_SYS .eq. 6) then
C
             call IC_HSEb_TO_HSEa (ORB_POS_TIME,
     1                            TRANSFORM_MATRIX, DERIV_MATRIX,
     2                            VELOCITY_REQ, STATUS)
             call MULMAT(TRANSFORM_MATRIX,ORB_POS_VEL,3,3,1,
     1                                           TRANS_ORB_POS_VEL)
C                  NO VELOCITY YET
              TRANS_ORB_POS_VEL(4) = 0.
              TRANS_ORB_POS_VEL(5) = 0.
              TRANS_ORB_POS_VEL(6) = 0.
           else if (TARGET_SYS .eq. 7) then
C
             call IC_HSEb_TO_HSEa (ORB_POS_TIME,
     1                            TEMP_MATRIX, DERIV_MATRIX,
     2                            VELOCITY_REQ, STATUS)
	     call IC_HSEa_TO_HS (ORB_POS_TIME, 
     1                 TEMP_MATRIX2, DERIV_MATRIX,VELOCITY_REQ, STATUS)
C
             call MULMAT(TEMP_MATRIX2,TEMP_MATRIX,3,3,3,
     1                                             TRANSFORM_MATRIX)
             call MULMAT(TRANSFORM_MATRIX,ORB_POS_VEL,3,3,1,
     1                                           TRANS_ORB_POS_VEL)
C                  NO VELOCITY YET
              TRANS_ORB_POS_VEL(4) = 0.
              TRANS_ORB_POS_VEL(5) = 0.
              TRANS_ORB_POS_VEL(6) = 0.
           else ! Unimplemented target
              STATUS = 1
           endif
	endif
C
C*  Check status of the transformation matrix
C
	if (STATUS .ne. 0) goto 990
C
C*  call MULMAT ROUTINE TO MULTIPLY THE TRANSFORMATION MATRIX BY
C*   THE GIVEN ORBIT VECTOR.  THE RESULT WILL BE THE TARGET ORBIT POSITION.
C
        if (TARGET_SYS .NE. 5 .AND. TARGET_SYS .NE. 6 
     1                              .AND. TARGET_SYS .NE. 7) then        
          call MULMAT (TRANSFORM_MATRIX,ORB_POS_VEL(1),3,3,1,
     1                                         TRANS_ORB_POS_VEL)
          call MULMAT (TRANSFORM_MATRIX,ORB_POS_VEL(4),3,3,1,
     1                                                   OUT_VEL)
          call MULMAT (DERIV_MATRIX,ORB_POS_VEL(1),3,3,1,CORR_VEL)
          TRANS_ORB_POS_VEL(4) = OUT_VEL(1) + CORR_VEL(1)
          TRANS_ORB_POS_VEL(5) = OUT_VEL(2) + CORR_VEL(2)
          TRANS_ORB_POS_VEL(6) = OUT_VEL(3) + CORR_VEL(3)
        endif

C
C*  Return the transformation matrix
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
990     if (STATUS .ne. 0) 
     .               TRANS_ORB_STAT = STATUS

	RETURN
C
	END









