Changeset 5493 for trunk/psLib/src/astro/psEarthOrientation.c

Timestamp:

Nov 9, 2005, 2:13:51 PM (21 years ago)

Author:

drobbin

Message:

Implemented functions PolarTideCorr, SphereRot CEOtoGCRS & ITRStoTEO (& tests) - still need verification!

File:

• trunk/psLib/src/astro/psEarthOrientation.c (modified) (8 diffs)

trunk/psLib/src/astro/psEarthOrientation.c

@@ -2 +2 @@
 *
 *  @brief Function implementations for earth orientation calculations
-*  transformation
 *
 *  @ingroup EarthOrientation
@@ -9 +8 @@
 *  @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 +62 @@
     psEarthPole* pole = psAlloc(sizeof(psEarthPole));
     psMemSetDeallocator(pole, (psFreeFunc) earthPoleFree);
+    pole->x = 0.0;
+    pole->y = 0.0;
+    pole->s = 0.0;
     return pole;
 }
@@ -210 +212 @@
     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 +291 @@
     // 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 +326 @@
     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 +586 @@
 }
 
-
 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 +703 @@
 
     //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;
 }