Index: trunk/psLib/src/astro/psEarthOrientation.c =================================================================== --- trunk/psLib/src/astro/psEarthOrientation.c (revision 5483) +++ trunk/psLib/src/astro/psEarthOrientation.c (revision 5493) @@ -2,5 +2,4 @@ * * @brief Function implementations for earth orientation calculations -* transformation * * @ingroup EarthOrientation @@ -9,6 +8,6 @@ * @author Robert Daniel DeSonia, MHPCC * -* @version $Revision: 1.10 $ $Name: not supported by cvs2svn $ -* @date $Date: 2005-11-07 20:52:43 $ +* @version $Revision: 1.11 $ $Name: not supported by cvs2svn $ +* @date $Date: 2005-11-10 00:13:50 $ * * Copyright 2005 Maui High Performance Computing Center, University of Hawaii @@ -63,4 +62,7 @@ psEarthPole* pole = psAlloc(sizeof(psEarthPole)); psMemSetDeallocator(pole, (psFreeFunc) earthPoleFree); + pole->x = 0.0; + pole->y = 0.0; + pole->s = 0.0; return pole; } @@ -210,7 +212,8 @@ if (directionVector == NULL) printf("actualVector is null\n"); - mu = acos(directionVector->x*actualVector->x + - directionVector->y*actualVector->y + - directionVector->z*actualVector->z); + // mu = acos(directionVector->x*actualVector->x + + mu = (directionVector->x*actualVector->x + + directionVector->y*actualVector->y + + directionVector->z*actualVector->z); //rp = apparent - mu * direction; @@ -288,7 +291,8 @@ // use dot product to calculate the angle of separation // N.B., assuming the psSphereToCube function returns a unit vector. - double theta = acos(sunVector->x*actualVector->x + - sunVector->y*actualVector->y + - sunVector->z*actualVector->z); + // double theta = acos(sunVector->x*actualVector->x + + double theta = (sunVector->x*actualVector->x + + sunVector->y*actualVector->y + + sunVector->z*actualVector->z); double r0 = PS_AU * tan(theta); @@ -322,6 +326,6 @@ deflection = SEC_TO_RAD(deflection); theta = atan(r0/PS_AU) * tan(deflection); - // phi = sqrt( deflection*deflection - theta*theta ); - phi = deflection * cos(asin(theta/deflection)); + phi = sqrt( deflection*deflection - theta*theta ); + // phi = deflection * cos(asin(theta/deflection)); apparent->r = theta; apparent->d = phi; @@ -582,23 +586,101 @@ } - psEarthPole* psEOC_GetPolarMotion(const psTime *time, psTimeBulletin bulletin) { + return NULL; } +static double DMOD(double x, double y) +{ + double value = x - y * trunc(x/y); + return value; +} psEarthPole* psEOC_PolarTideCorr(const psTime *time) { + // Check for null parameter + PS_ASSERT_PTR_NON_NULL(time, NULL); + psEarthPole *out = psEarthPoleAlloc(); + + // Convert psTime to MJD + double MJD = psTimeToMJD(time); + + // Calculate number of Julian centuries since 2000 + //XXX: NOT SURE IF THIS IS CORRECT FOR THIS SITUATION + double RJD = ( MJD - MJD_2000 ) / JULIAN_CENTURY; + + //Formula comes from fortran reference + //DMOD in fortran ref. = double remainder -> x - y * trunc(x/y) + double T, L, LPRIME, CAPF, CAPD, OMEGA, THETA, CORX, CORY, CORZ; + double ARG1, ARG2, ARG3, ARG4, ARG5, ARG6, ARG7, ARG8; + double T2, T3, T4; + T = (RJD - 51544.5) / 36525.0; + T2 = T*T; + T3 = T*T*T; + T4 = T*T*T*T; + L = -0.0002447 * T4 + 0.051635 * T3 + 31.8792 * T2 + 1717915923.2178 * T + 485868.249036; + L = DMOD(L, 1296000.0); + LPRIME = -0.00001149 * T4 - 0.000136 * T3 - 0.5532 * T2 + 129596581.0481 * T + 1287104.79305; + LPRIME = DMOD(LPRIME, 1296000.0); + CAPF = 0.00000417 * T4 - 0.001037 * T3 - 12.7512 * T2 + 1739527262.8478 * T + 335779.526232; + CAPF = DMOD(CAPF, 1296000.0); + CAPD = -0.00003169 * T4 + 0.006593 * T3 - 6.3706 * T2 + 1602961601.209 * T + 1072260.70369; + CAPD = DMOD(CAPD, 1296000.0); + OMEGA = -0.00005939 * T4 + 0.007702 * T3 + 7.4722 * T2 - 6962890.2665 * T + 450160.398036; + OMEGA = DMOD(OMEGA, 1296000.0); + THETA = (67310.54841 + (876600.0 * 3600.0 + 8640184.812866) * T + 0.093104 * T2 - + 6.2e-6 * T3) * 15.0 + 648000.0; + ARG7 = DMOD((-L - 2.0 * CAPF - 2.0 * OMEGA + THETA) * M_PI / 648000.0, 2.0 * M_PI) + - M_PI / 2.0; + ARG1 = DMOD((-2.0 * CAPF - 2.0 * OMEGA + THETA) * M_PI / 648000.0, 2.0 * M_PI) - M_PI / 2.0; + ARG2 = DMOD((-2.0 * CAPF + 2.0 * CAPD - 2.0 * OMEGA + THETA) * M_PI / 648000.0, 2.0 * M_PI) + - M_PI / 2.0; + ARG3 = DMOD(THETA * M_PI / 648000.0, 2.0 * M_PI) - M_PI / 2.0; + ARG4 = DMOD((-L - 2.0 * CAPF - 2.0 * OMEGA + 2.0 * THETA) * M_PI / 648000.0, 2.0 * M_PI); + ARG5 = DMOD((-2.0 * CAPF - 2.0 * OMEGA + 2.0 * THETA) * M_PI / 648000.0, 2.0 * M_PI); + ARG6 = DMOD((-2.0 * CAPF + 2.0 * CAPD - 2.0 * OMEGA + 2.0 * THETA) * M_PI / 648000.0, + 2.0 * M_PI); + ARG8 = DMOD((2.0 * THETA) * M_PI / 648000.0, 2.0 * M_PI); + CORX = -0.026 * sin(ARG7) + 0.006 * cos(ARG7) + -0.133 * sin(ARG1) + 0.049 * cos(ARG1) + -0.050 * sin(ARG2) + 0.025 * cos(ARG2) + -0.152 * sin(ARG3) + 0.078 * cos(ARG3) + -0.057 * sin(ARG4) - 0.013 * cos(ARG4) + -0.330 * sin(ARG5) - 0.028 * cos(ARG5) + -0.145 * sin(ARG6) + 0.064 * cos(ARG6) + -0.036 * sin(ARG8) + 0.017 * cos(ARG8); + CORY = -0.006 * sin(ARG7) - 0.026 * cos(ARG7) + -0.049 * sin(ARG1) - 0.133 * cos(ARG1) + -0.025 * sin(ARG2) - 0.050 * cos(ARG2) + -0.078 * sin(ARG3) - 0.152 * cos(ARG3) + +0.011 * sin(ARG4) + 0.033 * cos(ARG4) + +0.037 * sin(ARG5) + 0.196 * cos(ARG5) + +0.059 * sin(ARG6) + 0.087 * cos(ARG6) + +0.018 * sin(ARG8) + 0.022 * cos(ARG8); + CORZ = 0.0245 * sin(ARG7) + 0.0503 * cos(ARG7) + +0.1210 * sin(ARG1) + 0.1605 * cos(ARG1) + +0.0286 * sin(ARG2) + 0.0516 * cos(ARG2) + +0.0864 * sin(ARG3) + 0.1771 * cos(ARG3) + -0.0380 * sin(ARG4) - 0.0154 * cos(ARG4) + -0.1617 * sin(ARG5) - 0.0720 * cos(ARG5) + -0.0759 * sin(ARG6) - 0.0004 * cos(ARG6) + -0.0196 * sin(ARG8) - 0.0038 * cos(ARG8); + CORX = CORX * 1.0e-3; + CORY = CORY * 1.0e-3; + CORZ = CORZ * 0.1e-3; + + out->x = CORX; + out->y = CORY; + out->s = CORZ; + + return out; +} + +psEarthPole* psEOC_NutationCorr(psTime *time) +{ return NULL; } - - -psEarthPole* psEOC_NutationCorr(psTime *time) -{ - return NULL; -} - psSphereRot* psSphereRot_ITRStoTEO(const psEarthPole* motion) @@ -621,44 +703,45 @@ //Convert rotation matrix to quaternions - double diag_sum[3]; - int maxi; - double recip; - diag_sum[0] = 1.0 + A[0][0] - A[1][1] - A[2][2]; - diag_sum[1] = 1.0 - A[0][0] + A[1][1] - A[2][2]; - diag_sum[2] = 1.0 - A[0][0] - A[1][1] + A[2][2]; - diag_sum[3] = 1.0 + A[0][0] + A[1][1] + A[2][2]; - - maxi = 0; - for (int i = 1; i < 4; ++i) { - if (diag_sum[i] > diag_sum[maxi]) { - maxi = i; + out = rotMatrix_To_Quat(A); + /* double diag_sum[3]; + int maxi; + double recip; + diag_sum[0] = 1.0 + A[0][0] - A[1][1] - A[2][2]; + diag_sum[1] = 1.0 - A[0][0] + A[1][1] - A[2][2]; + diag_sum[2] = 1.0 - A[0][0] - A[1][1] + A[2][2]; + diag_sum[3] = 1.0 + A[0][0] + A[1][1] + A[2][2]; + + maxi = 0; + for (int i = 1; i < 4; ++i) { + if (diag_sum[i] > diag_sum[maxi]) { + maxi = i; + } } - } - - double p = 0.5 * sqrt(diag_sum[maxi]); - recip = 1.0 / (4.0 * p); - - if (maxi == 0) { - out->q0 = p; - out->q1 = recip * (A[0][1] + A[1][0]); - out->q2 = recip * (A[2][0] + A[0][2]); - out->q3 = recip * (A[1][2] - A[2][1]); - } else if (maxi == 1) { - out->q0 = recip * (A[0][1] + A[1][0]); - out->q1 = p; - out->q2 = recip * (A[1][2] + A[2][1]); - out->q3 = recip * (A[2][0] - A[0][2]); - } else if (maxi == 2) { - out->q0 = recip * (A[2][0] + A[0][2]); - out->q1 = recip * (A[1][2] + A[2][1]); - out->q2 = p; - out->q3 = recip * (A[0][1] - A[1][0]); - } else if (maxi == 3) { - out->q0 = recip * (A[1][2] - A[2][1]); - out->q1 = recip * (A[2][0] - A[0][2]); - out->q2 = recip * (A[0][1] - A[1][0]); - out->q3 = p; - } - + + double p = 0.5 * sqrt(diag_sum[maxi]); + recip = 1.0 / (4.0 * p); + + if (maxi == 0) { + out->q0 = p; + out->q1 = recip * (A[0][1] + A[1][0]); + out->q2 = recip * (A[2][0] + A[0][2]); + out->q3 = recip * (A[1][2] - A[2][1]); + } else if (maxi == 1) { + out->q0 = recip * (A[0][1] + A[1][0]); + out->q1 = p; + out->q2 = recip * (A[1][2] + A[2][1]); + out->q3 = recip * (A[2][0] - A[0][2]); + } else if (maxi == 2) { + out->q0 = recip * (A[2][0] + A[0][2]); + out->q1 = recip * (A[1][2] + A[2][1]); + out->q2 = p; + out->q3 = recip * (A[0][1] - A[1][0]); + } else if (maxi == 3) { + out->q0 = recip * (A[1][2] - A[2][1]); + out->q1 = recip * (A[2][0] - A[0][2]); + out->q2 = recip * (A[0][1] - A[1][0]); + out->q3 = p; + } + */ return out; }