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

Timestamp:

Nov 7, 2005, 10:52:43 AM (21 years ago)

Author:

drobbin

Message:

update doxygen, implemented sphereRot functions in EOC.

File:

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

trunk/psLib/src/astro/psEarthOrientation.c

@@ -9 +9 @@
 *  @author Robert Daniel DeSonia, MHPCC
 *
-*  @version $Revision: 1.9 $ $Name: not supported by cvs2svn $
-*  @date $Date: 2005-11-03 04:31:22 $
+*  @version $Revision: 1.10 $ $Name: not supported by cvs2svn $
+*  @date $Date: 2005-11-07 20:52:43 $
 *
 *  Copyright 2005 Maui High Performance Computing Center, University of Hawaii
@@ -52 +52 @@
 static bool eocInitialized = false;
 
+static psSphereRot *rotMatrix_To_Quat(double A[3][3]);
+
+static void earthPoleFree(psEarthPole *pole)
+{
+    // There are non dynamic allocated items
+}
+
+psEarthPole *psEarthPoleAlloc(void)
+{
+    psEarthPole* pole = psAlloc(sizeof(psEarthPole));
+    psMemSetDeallocator(pole, (psFreeFunc) earthPoleFree);
+    return pole;
+}
+
 static bool eocInit()
 {
@@ -139 +153 @@
     psFree(xTable);
     psFree(yTable);
-    psFree(sTable);
+    psFree(sTable);    // There are non dynamic allocated items
+
 
     xTable = NULL;
@@ -179 +194 @@
         apparent->dErr = 0.0;
     }
-    psSphere *rp = psSphereAlloc();
-    psSphere *r_p = psSphereAlloc();
+    //    psSphere *rp = psSphereAlloc();
+    psCube *rp = psCubeAlloc();
+    //    psSphere *r_p = psSphereAlloc();
+    psCube *r_p = psCubeAlloc();
     double mu = 0.0;
     double mu_p = 0.0;
@@ -188 +205 @@
     //    mu = apparent->r * direction->r + apparent->d * direction->d;
     psCube* directionVector = psSphereToCube(direction);
+    if (directionVector == NULL)
+        printf("directionVector is null\n");
     psCube* actualVector = psSphereToCube(actual);
+    if (directionVector == NULL)
+        printf("actualVector is null\n");
     mu = acos(directionVector->x*actualVector->x +
               directionVector->y*actualVector->y +
@@ -194 +215 @@
 
     //rp = apparent - mu * direction;
-    rp->r = actual->r - mu * direction->r;
-    rp->d = actual->d - mu * direction->d;
+    //    rp->r = actual->r - mu * direction->r;
+    //    rp->d = actual->d - mu * direction->d;
+    rp->x = actualVector->x - mu * directionVector->x;
+    rp->y = actualVector->y - mu * directionVector->y;
+    rp->z = actualVector->z - mu * directionVector->z;
 
     mu_p = mu + speed * ((mu * mu - 1.0) / (1.0 - speed * mu));
 
     //Not sure if this is right.  ADD gets a scalar from division of rp (a vector)
-    psCube* rpVector = psSphereToCube(rp);
-    a = sqrt( (1.0 - mu_p * mu_p) /
-              (sqrt(rpVector->x*rpVector->x
-                    + rpVector->y*rpVector->y
-                    + rpVector->z*rpVector->z)) );
-
+    //Paul claims rp is the modulus or square of the vector components
+    /*    psCube* rpVector = psSphereToCube(rp);
+        a = sqrt( (1.0 - mu_p * mu_p) / ( rpVector->x*rpVector->x +
+                rpVector->y*rpVector->y + rpVector->z*rpVector->z)  );
+    */
+    a = (1.0 - mu_p * mu_p) / (rp->x * rp->x + rp->y * rp->y + rp->z * rp->z);
+    if (a < 0.0) {
+        printf("a is negative\n");
+        psFree(rp);
+        psFree(r_p);
+        psFree(directionVector);
+        psFree(actualVector);
+        psFree(apparent)
+        return NULL;
+    } else
+        a = sqrt(a);
     //r_p = mu_p * direction + a * rp;
-    r_p->r = mu_p * direction->r + a * rp->r;
-    r_p->d = mu_p * direction->d + a * rp->d;
-
-    *apparent = *r_p;
+    //    r_p->r = mu_p * direction->r + a * rp->r;
+    //    r_p->d = mu_p * direction->d + a * rp->d;
+    r_p->x = mu_p * directionVector->x + a * rp->x;
+    r_p->y = mu_p * directionVector->y + a * rp->y;
+    r_p->z = mu_p * directionVector->z + a * rp->z;
+
+    psSphere *r_pSphere = psCubeToSphere(r_p);
+    if (r_pSphere == NULL)
+        printf("r_pSphere is null\n");
+
+    *apparent = *r_pSphere;
     /*
         psSphereRot *rot = NULL;
@@ -223 +264 @@
     psFree(rp);
     psFree(r_p);
+    psFree(r_pSphere)
     psFree(directionVector);
     psFree(actualVector);
-    psFree(rpVector);
+    //    psFree(rpVector);
     return apparent;
 }
@@ -280 +322 @@
     deflection = SEC_TO_RAD(deflection);
     theta = atan(r0/PS_AU) * tan(deflection);
-    phi = sqrt( deflection*deflection - theta*theta );
+    //    phi = sqrt( deflection*deflection - theta*theta );
+    phi = deflection * cos(asin(theta/deflection));
     apparent->r = theta;
     apparent->d = phi;
@@ -447 +490 @@
 }
 
+static psSphereRot *rotMatrix_To_Quat(double A[3][3])
+{
+    int i;
+    psSphereRot *new = (psSphereRot*)psAlloc(sizeof(psSphereRot));
+    //    psMemSetDeallocator(new, (psFreeFunc)sphereRotFree);
+    //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 (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) {
+        new->q0 = p;
+        new->q1 = recip * (A[0][1] + A[1][0]);
+        new->q2 = recip * (A[2][0] + A[0][2]);
+        new->q3 = recip * (A[1][2] - A[2][1]);
+    } else if (maxi == 1) {
+        new->q0 = recip * (A[0][1] + A[1][0]);
+        new->q1 = p;
+        new->q2 = recip * (A[1][2] + A[2][1]);
+        new->q3 = recip * (A[2][0] - A[0][2]);
+    } else if (maxi == 2) {
+        new->q0 = recip * (A[2][0] + A[0][2]);
+        new->q1 = recip * (A[1][2] + A[2][1]);
+        new->q2 = p;
+        new->q3 = recip * (A[0][1] - A[1][0]);
+    } else if (maxi == 3) {
+        new->q0 = recip * (A[1][2] - A[2][1]);
+        new->q1 = recip * (A[2][0] - A[0][2]);
+        new->q2 = recip * (A[0][1] - A[1][0]);
+        new->q3 = p;
+    }
+    return new;
+}
 
 psSphereRot* psSphereRot_CEOtoGCRS(const psEarthPole *pole)
 {
-    return NULL;
+    PS_ASSERT_PTR_NON_NULL(pole,NULL);
+    double A[3][3];
+    psSphereRot *out = NULL;
+
+    //Setup the rotation matrix and scalar value, a, as outlined by the ADD
+    //XXX: Used formula for rotation matrix D from mathworld for z-axis rotation
+    double a =  1.0 / (1.0 + sqrt(1.0 - (pole->x*pole->x + pole->y*pole->y) ) );
+    A[0][0] = (1.0 - a*pole->x*pole->x)*cos(pole->s) - a*pole->x*pole->y*sin(pole->s);
+    A[0][1] = -a*pole->x*pole->y*cos(pole->s) + (1.0 - a*pole->y*pole->y)*sin(pole->s);
+    A[0][2] = pole->x*cos(pole->s) + pole->y*sin(pole->s);
+    A[1][0] = -(1.0 - a*pole->x*pole->x)*sin(pole->s) - a*pole->x*pole->y*cos(pole->s);
+    A[1][1] = a*pole->x*pole->y*sin(pole->s) + (1.0 - a*pole->y*pole->y)*cos(pole->s);
+    A[1][2] = -pole->x*sin(pole->s) + pole->y*cos(pole->s);
+    A[2][0] = -pole->x;
+    A[2][1] = -pole->y;
+    A[2][2] = 1.0 - a*(pole->x*pole->x + pole->y*pole->y);
+
+    out = rotMatrix_To_Quat(A);
+
+    return out;
 }
 
@@ -495 +604 @@
 psSphereRot* psSphereRot_ITRStoTEO(const psEarthPole* motion)
 {
-    return NULL;
-}
+    PS_ASSERT_PTR_NON_NULL(motion,NULL);
+    double A[3][3];
+    psSphereRot *out = NULL;
+
+    //Setup Rotation Matrix for transformation (x,y,z rotation)
+    //XXX: May need to be (z,y,x as in Mathworld?)
+    A[0][0] = cos(motion->x)*cos(-motion->s);
+    A[0][1] = sin(motion->y)*sin(motion->x)*cos(-motion->s) + cos(motion->y)*sin(-motion->s);
+    A[0][2] = -cos(motion->y)*sin(motion->x)*cos(-motion->s) + sin(motion->y)*sin(-motion->s);
+    A[1][0] = -cos(motion->x)*sin(-motion->s);
+    A[1][1] = -sin(motion->y)*sin(motion->x)*sin(-motion->s) + cos(motion->y)*cos(-motion->s);
+    A[1][2] = cos(motion->y)*sin(motion->x)*sin(-motion->s) + sin(motion->y)*cos(-motion->s);
+    A[2][0] = sin(motion->x);
+    A[2][1] = -sin(motion->y)*cos(motion->x);
+    A[2][2] = cos(motion->y)*cos(motion->x);
+
+    //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;
+        }
+    }
+
+    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;
+}