src/geomagnetic_field.c

   1 /*
   2  * This file is part of WMM source code.
   3  * The original code is the WMM Source from National Oceanic And Atmospheric.
   4  *
   5  * See the license below for more details.
   6  *
   7  * The WMM source code is in the public domain and not licensed or under
   8  * copyright. The information and software may be used freely by the public.
   9  * As required by 17 U.S.C. 403, third parties producing copyrighted works
  10  * consisting predominantly of the material produced by U.S.
  11  * government agencies must provide notice with such work  identifying the U.S.
  12  * Government material incorporated and stating that such material is not
  13  * subject to copyright protection.
  14  */
  15
  16 #include <math.h>
  17 #include <stdlib.h>
  18
  19 const float c[13][13] = {
  20         {0.0, -29496.6, -3594.9, 3350.2, 3992.6, -1818.3, 1051.0, 2158.4, 1226.7, 512.8, -360.9, 1033.3, -1452.4, },
  21         {4944.4, -1586.3, 5241.4, -7122.5, 4476.4, 3631.5, 1296.8, -2663.8, 543.0, 1197.6, -1532.7, -699.6, -179.4, },
  22         {-4689.9, -498.9, 1445.0, 2385.6, 652.3, 1539.3, 1135.8, -136.1, -813.2, 369.4, 189.6, -859.0, 238.5, },
  23         {-490.5, 487.8, -424.2, 501.2, -746.9, -664.0, -1408.7, 927.7, -231.9, -431.5, -181.8, 557.5, 649.2, },
  24         {1584.9, -826.5, 343.7, -228.6, 66.1, -361.6, -124.4, 171.7, -516.0, 174.8, -23.4, -119.8, -292.1, },
  25         {453.4, 1451.7, -556.3, 0.0, 70.8, -5.5, 30.7, 64.2, 170.6, -417.8, 184.8, 79.2, 300.6, },
  26         {-393.2, 659.0, 612.7, -361.8, 7.2, 36.9, -52.3, 4.1, 74.8, -12.2, -12.4, -75.3, -20.8, },
  27         {-2053.7, -611.1, 133.1, 307.5, 43.2, -67.1, -2.1, 3.2, -35.3, 63.3, 44.1, 19.8, 58.5, },
  28         {737.3, -1121.6, 492.9, -465.2, 247.7, 48.1, -27.1, 1.1, -2.3, -22.0, 25.4, 41.0, -23.4, },
  29         {-2611.8, 1249.5, 1062.2, -405.9, -249.3, 139.2, 15.8, -15.8, 4.3, -6.2, -2.7, 0.9, -10.2, },
  30         {681.2, -21.1, 776.8, 514.2, -532.2, -41.3, -78.2, -16.4, -5.3, -4.9, -1.7, 1.9, 1.9, },
  31         {93.3, 695.4, -196.8, -431.1, 142.6, -37.6, -124.0, -29.6, -18.5, -5.2, -1.0, 2.2, -2.2, },
  32         {-807.3, 238.5, 1363.4, -1217.3, 167.0, 125.0, 0.0, 5.9, 7.7, -8.5, -0.6, 0.5, 0.0, }
  33 };
  34
  35 const float cd[13][13] = {
  36         {0.0, 11.6, -18.1, 1.0, -7.9, -7.9, -2.9, 2.7, -5.0, 0.0, 0.0, 0.0, 0.0, },
  37         {-25.9, 16.5, -7.6, -12.6, 12.7, 6.1, -3.8, -3.5, 6.7, -12.7, 0.0, 0.0, 0.0, },
  38         {-39.0, -10.2, 1.6, -5.6, -34.0, -13.8, -1.5, -17.4, -33.6, 0.0, -21.1, 0.0, 79.5, },
  39         {22.4, -7.6, -2.1, -6.1, 9.6, -4.7, 19.9, 26.6, 8.3, 24.9, 33.1, 32.8, 64.9, },
  40         {6.1, 10.6, 8.2, -0.6, -1.6, 2.0, -9.3, 4.9, -5.3, -22.6, 0.0, 0.0, -48.7, },
  41         {4.1, 13.8, 5.6, 8.9, -0.4, 0.7, -0.7, 1.9, 4.4, -10.1, -7.4, 0.0, 0.0, },
  42         {-3.8, -31.4, -4.0, -3.3, 1.2, 0.6, 1.1, -1.7, 2.1, 1.7, -8.3, 0.0, 0.0, },
  43         {24.8, 8.7, -2.0, -1.2, -4.9, -0.7, 0.2, 0.4, -1.5, -0.8, 0.0, 0.0, 0.0, },
  44         {-6.7, 11.2, 16.6, 10.7, 1.5, -0.7, 1.0, 0.2, 0.1, -1.0, -0.8, 0.0, 0.0, },
  45         {0.0, -21.7, 0.0, -5.6, 3.4, 0.0, -1.5, 0.8, 0.1, -0.1, -0.5, 0.0, 0.0, },
  46         {24.3, -21.1, 0.0, -11.7, -7.4, 0.0, -2.0, -1.6, 0.0, -0.1, -0.1, -0.3, 0.0, },
  47         {0.0, 40.9, 0.0, 24.0, 0.0, 9.4, 0.0, -2.3, -0.9, 0.0, -0.1, 0.0, -0.3, },
  48         {0.0, 0.0, 0.0, 0.0, 0.0, 20.8, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, }
  49 };
  50
  51 float g_declination = 0;
  52 float g_inclination = 0;
  53
  54 static void E0000(int IENTRY, int maxdeg, float alt, float glat, float glon, float time, float *dec, float *dip, float *ti, float *gv);
  55
  56 int getDeclination(float *decl)
  57 {
  58         if (decl == NULL)
  59                 return -1;
  60
  61         *decl = g_declination;
  62
  63         return 0;
  64 }
  65
  66 int getInclination(float *incl)
  67 {
  68         if (incl == NULL)
  69                 return -1;
  70
  71         *incl = g_inclination;
  72
  73         return 0;
  74 }
  75
  76 int setCoordinate(float latitude, float longitude, float altitude, float *declination, float *inclination, int option)
  77 {
  78         float dec, dip, ti, gv;
  79         float h;
  80         float rTd = 0.017453292;
  81
  82         E0000(0, 12, 0.0, 0.0, 0.0, 0.0, NULL, NULL, NULL, NULL);
  83         E0000(1, 0, altitude, latitude, longitude, 2, &dec, &dip, &ti, &gv);
  84
  85         h = ti*(cos((dip*rTd)));
  86
  87         /* deal with geographic and magnetic poles */
  88
  89         if (h < 100.0) {    /* at magnetic poles */
  90                 dec = 0;
  91         }
  92
  93         if (option == 1) {
  94                 if (declination != NULL)
  95                         *declination = dec;
  96                 if (inclination != NULL)
  97                         *inclination = dip;
  98         } else if (option == 0) {
  99                 g_declination = dec;
 100                 g_inclination = dip;
 101         }
 102
 103         return 0;
 104 }
 105 /*************************************************************************/
 106
 107 static void E0000(int IENTRY, int maxdeg, float alt, float glat, float glon, float time, float *dec, float *dip, float *ti, float *gv)
 108 {
 109         static int maxord, n, m, j, D1, D2, D3, D4;
 110         static float tc[13][13], dp[13][13], snorm[169],
 111                         sp[13], cp[13], fn[13], fm[13], pp[13], k[13][13], pi, dtr, a, b, re,
 112                         a2, b2, c2, a4, b4, c4, flnmj, otime, oalt,
 113                         olat, olon, dt, rlon, rlat, srlon, srlat, crlon, crlat, srlat2,
 114                         crlat2, q, q1, q2, ct, st, r2, r, d, ca, sa, aor, ar, br, bt, bp, bpp,
 115                         par, temp1, temp2, parp, bx, by, bz, bh;
 116         static float *p = snorm;
 117
 118         switch (IENTRY) {case 0: goto GEOMAG; case 1: goto GEOMG1; }
 119
 120 GEOMAG:
 121         maxord = 12;
 122         sp[0] = 0.0;
 123         cp[0] = *p = pp[0] = 1.0;
 124         dp[0][0] = 0.0;
 125         a = 6378.137;
 126         b = 6356.7523142;
 127         re = 6371.2;
 128         a2 = a*a;
 129         b2 = b*b;
 130         c2 = a2-b2;
 131         a4 = a2*a2;
 132         b4 = b2*b2;
 133         c4 = a4 - b4;
 134
 135         *snorm = 1.0;
 136         fm[0] = 0.0;
 137         for (n = 1; n <= maxord; n++) {
 138                 *(snorm+n) = *(snorm+n-1)*(float)(2*n-1)/(float)n;
 139                 j = 2;
 140                 for (m = 0, D1 = 1, D2 = (n-m+D1)/D1; D2 > 0; D2--, m += D1) {
 141                         k[m][n] = (float)(((n-1)*(n-1))-(m*m))/(float)((2*n-1)*(2*n-3));
 142                         if (m > 0) {
 143                                 flnmj = (float)((n-m+1)*j)/(float)(n+m);
 144                                 *(snorm+n+m*13) = *(snorm+n+(m-1)*13)*sqrt(flnmj);
 145                                 j = 1;
 146                         }
 147                 }
 148                 fn[n] = (float)(n+1);
 149                 fm[n] = (float)n;
 150         }
 151         k[1][1] = 0.0;
 152
 153         otime = oalt = olat = olon = -1000.0;
 154
 155         return;
 156
 157         /*************************************************************************/
 158
 159 GEOMG1:
 160
 161         dt = time;
 162         pi = 3.14159265359;
 163         dtr = pi/180.0;
 164         rlon = glon*dtr;
 165         rlat = glat*dtr;
 166         srlon = sin(rlon);
 167         srlat = sin(rlat);
 168         crlon = cos(rlon);
 169         crlat = cos(rlat);
 170         srlat2 = srlat*srlat;
 171         crlat2 = crlat*crlat;
 172         sp[1] = srlon;
 173         cp[1] = crlon;
 174
 175         if (alt != oalt || glat != olat) {
 176                 q = sqrt(a2-c2*srlat2);
 177                 q1 = alt*q;
 178                 q2 = ((q1+a2)/(q1+b2))*((q1+a2)/(q1+b2));
 179                 ct = srlat/sqrt(q2*crlat2+srlat2);
 180                 st = sqrt(1.0-(ct*ct));
 181                 r2 = (alt*alt)+2.0*q1+(a4-c4*srlat2)/(q*q);
 182                 r = sqrt(r2);
 183                 d = sqrt(a2*crlat2+b2*srlat2);
 184                 ca = (alt+d)/r;
 185                 sa = c2*crlat*srlat/(r*d);
 186         }
 187         if (glon != olon) {
 188                 for (m = 2; m <= maxord; m++) {
 189                         sp[m] = sp[1]*cp[m-1]+cp[1]*sp[m-1];
 190                         cp[m] = cp[1]*cp[m-1]-sp[1]*sp[m-1];
 191                 }
 192         }
 193         aor = re/r;
 194         ar = aor*aor;
 195         br = bt = bp = bpp = 0.0;
 196         for (n = 1; n <= maxord; n++) {
 197                 ar = ar*aor;
 198                 for (m = 0, D3 = 1, D4 = (n+m+D3)/D3; D4 > 0; D4--, m += D3) {
 199                         if (alt != oalt || glat != olat) {
 200                                 if (n == m && m != 0) {
 201                                         *(p+n+m*13) = st**(p+n-1+(m-1)*13);
 202                                         dp[m][n] = st*dp[m-1][n-1]+ct**(p+n-1+(m-1)*13);
 203                                         goto S50;
 204                                 }
 205                                 if (n == 1 && m == 0) {
 206                                         *(p+n+m*13) = ct**(p+n-1+m*13);
 207                                         dp[m][n] = ct*dp[m][n-1]-st**(p+n-1+m*13);
 208                                         goto S50;
 209                                 }
 210                                 if (n > 1 && n != m) {
 211                                         if (m > n-2) *(p+n-2+m*13) = 0.0;
 212                                         if (m > n-2) dp[m][n-2] = 0.0;
 213                                         *(p+n+m*13) = ct**(p+n-1+m*13)-k[m][n]**(p+n-2+m*13);
 214                                         dp[m][n] = ct*dp[m][n-1] - st**(p+n-1+m*13)-k[m][n]*dp[m][n-2];
 215                                 }
 216                         }
 217 S50:
 218                         if (time != otime) {
 219                                 tc[m][n] = c[m][n]+dt*cd[m][n];
 220                                 if (m != 0) tc[n][m-1] = c[n][m-1]+dt*cd[n][m-1];
 221                         }
 222
 223                         par = ar**(p+n+m*13);
 224                         if (m == 0) {
 225                                 temp1 = tc[m][n]*cp[m];
 226                                 temp2 = tc[m][n]*sp[m];
 227                         } else {
 228                                 temp1 = tc[m][n]*cp[m]+tc[n][m-1]*sp[m];
 229                                 temp2 = tc[m][n]*sp[m]-tc[n][m-1]*cp[m];
 230                         }
 231                         bt = bt-ar*temp1*dp[m][n];
 232                         bp += (fm[m]*temp2*par);
 233                         br += (fn[n]*temp1*par);
 234
 235                         if (st == 0.0 && m == 1) {
 236                                 if (n == 1) pp[n] = pp[n-1];
 237                                 else pp[n] = ct*pp[n-1]-k[m][n]*pp[n-2];
 238                                 parp = ar*pp[n];
 239                                 bpp += (fm[m]*temp2*parp);
 240                         }
 241                 }
 242         }
 243         if (st == 0.0) bp = bpp;
 244         else bp /= st;
 245
 246         bx = -bt*ca-br*sa;
 247         by = bp;
 248         bz = bt*sa-br*ca;
 249         bh = sqrt((bx*bx)+(by*by));
 250         *ti = sqrt((bh*bh)+(bz*bz));
 251         *dec = atan2(by, bx)/dtr;
 252         *dip = atan2(bz, bh)/dtr;
 253         *gv = -999.0;
 254         if (fabs(glat) >= 55.) {
 255                 if (glat > 0.0 && glon >= 0.0) *gv = *dec-glon;
 256                 if (glat > 0.0 && glon < 0.0) *gv = *dec+fabs(glon);
 257                 if (glat < 0.0 && glon >= 0.0) *gv = *dec+glon;
 258                 if (glat < 0.0 && glon < 0.0) *gv = *dec-fabs(glon);
 259                 if (*gv > +180.0) *gv -= 360.0;
 260                 if (*gv < -180.0) *gv += 360.0;
 261         }
 262         otime = time;
 263         oalt = alt;
 264         olat = glat;
 265         olon = glon;
 266         return;
 267 }
 268