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 static int is_float_equal(float f1, float f2)
  57 {
  58         float precision = 0.00001f;
  59         return ((f1 - precision) < f2) && ((f1 + precision) > f2);
  60 }
  61
  62 int getDeclination(float *decl)
  63 {
  64         if (decl == NULL)
  65                 return -1;
  66
  67         *decl = g_declination;
  68
  69         return 0;
  70 }
  71
  72 int getInclination(float *incl)
  73 {
  74         if (incl == NULL)
  75                 return -1;
  76
  77         *incl = g_inclination;
  78
  79         return 0;
  80 }
  81
  82 int setCoordinate(float latitude, float longitude, float altitude, float *declination, float *inclination, int option)
  83 {
  84         float dec, dip, ti, gv;
  85         float h;
  86         float rTd = 0.017453292;
  87
  88         E0000(0, 12, 0.0, 0.0, 0.0, 0.0, NULL, NULL, NULL, NULL);
  89         E0000(1, 0, altitude, latitude, longitude, 2, &dec, &dip, &ti, &gv);
  90
  91         h = ti*(cos((dip*rTd)));
  92
  93         /* deal with geographic and magnetic poles */
  94
  95         if (h < 100.0) {    /* at magnetic poles */
  96                 dec = 0;
  97         }
  98
  99         if (option == 1) {
 100                 if (declination != NULL)
 101                         *declination = dec;
 102                 if (inclination != NULL)
 103                         *inclination = dip;
 104         } else if (option == 0) {
 105                 g_declination = dec;
 106                 g_inclination = dip;
 107         }
 108
 109         return 0;
 110 }
 111 /*************************************************************************/
 112
 113 static void E0000(int IENTRY, int maxdeg, float alt, float glat, float glon, float time, float *dec, float *dip, float *ti, float *gv)
 114 {
 115         static int maxord, n, m, j, D1, D2, D3, D4;
 116         static float tc[13][13], dp[13][13], snorm[169],
 117                         sp[13], cp[13], fn[13], fm[13], pp[13], k[13][13], pi, dtr, a, b, re,
 118                         a2, b2, c2, a4, b4, c4, flnmj, otime, oalt,
 119                         olat, olon, dt, rlon, rlat, srlon, srlat, crlon, crlat, srlat2,
 120                         crlat2, q, q1, q2, ct, st, r2, r, d, ca, sa, aor, ar, br, bt, bp, bpp,
 121                         par, temp1, temp2, parp, bx, by, bz, bh;
 122         static float *p = snorm;
 123
 124         switch (IENTRY) {case 0: goto GEOMAG; case 1: goto GEOMG1; }
 125
 126 GEOMAG:
 127         maxord = 12;
 128         sp[0] = 0.0;
 129         cp[0] = *p = pp[0] = 1.0;
 130         dp[0][0] = 0.0;
 131         a = 6378.137;
 132         b = 6356.7523142;
 133         re = 6371.2;
 134         a2 = a*a;
 135         b2 = b*b;
 136         c2 = a2-b2;
 137         a4 = a2*a2;
 138         b4 = b2*b2;
 139         c4 = a4 - b4;
 140
 141         *snorm = 1.0;
 142         fm[0] = 0.0;
 143         for (n = 1; n <= maxord; n++) {
 144                 *(snorm+n) = *(snorm+n-1)*(float)(2*n-1)/(float)n;
 145                 j = 2;
 146                 for (m = 0, D1 = 1, D2 = (n-m+D1)/D1; D2 > 0; D2--, m += D1) {
 147                         k[m][n] = (float)(((n-1)*(n-1))-(m*m))/(float)((2*n-1)*(2*n-3));
 148                         if (m > 0) {
 149                                 flnmj = (float)((n-m+1)*j)/(float)(n+m);
 150                                 *(snorm+n+m*13) = *(snorm+n+(m-1)*13)*sqrt(flnmj);
 151                                 j = 1;
 152                         }
 153                 }
 154                 fn[n] = (float)(n+1);
 155                 fm[n] = (float)n;
 156         }
 157         k[1][1] = 0.0;
 158
 159         otime = oalt = olat = olon = -1000.0;
 160
 161         return;
 162
 163         /*************************************************************************/
 164
 165 GEOMG1:
 166
 167         dt = time;
 168         pi = 3.14159265359;
 169         dtr = pi/180.0;
 170         rlon = glon*dtr;
 171         rlat = glat*dtr;
 172         srlon = sin(rlon);
 173         srlat = sin(rlat);
 174         crlon = cos(rlon);
 175         crlat = cos(rlat);
 176         srlat2 = srlat*srlat;
 177         crlat2 = crlat*crlat;
 178         sp[1] = srlon;
 179         cp[1] = crlon;
 180
 181         if (!is_float_equal(alt, oalt) || !is_float_equal(glat, olat)) {
 182                 q = sqrt(a2-c2*srlat2);
 183                 q1 = alt*q;
 184                 q2 = ((q1+a2)/(q1+b2))*((q1+a2)/(q1+b2));
 185                 ct = srlat/sqrt(q2*crlat2+srlat2);
 186                 st = sqrt(1.0-(ct*ct));
 187                 r2 = (alt*alt)+2.0*q1+(a4-c4*srlat2)/(q*q);
 188                 r = sqrt(r2);
 189                 d = sqrt(a2*crlat2+b2*srlat2);
 190                 ca = (alt+d)/r;
 191                 sa = c2*crlat*srlat/(r*d);
 192         }
 193         if (!is_float_equal(glon, olon)) {
 194                 for (m = 2; m <= maxord; m++) {
 195                         sp[m] = sp[1]*cp[m-1]+cp[1]*sp[m-1];
 196                         cp[m] = cp[1]*cp[m-1]-sp[1]*sp[m-1];
 197                 }
 198         }
 199         aor = re/r;
 200         ar = aor*aor;
 201         br = bt = bp = bpp = 0.0;
 202         for (n = 1; n <= maxord; n++) {
 203                 ar = ar*aor;
 204                 for (m = 0, D3 = 1, D4 = (n+m+D3)/D3; D4 > 0; D4--, m += D3) {
 205                         if (!is_float_equal(alt, oalt) || !is_float_equal(glat, olat)) {
 206                                 if (n == m && m != 0) {
 207                                         *(p+n+m*13) = st**(p+n-1+(m-1)*13);
 208                                         dp[m][n] = st*dp[m-1][n-1]+ct**(p+n-1+(m-1)*13);
 209                                         goto S50;
 210                                 }
 211                                 if (n == 1 && m == 0) {
 212                                         *(p+n+m*13) = ct**(p+n-1+m*13);
 213                                         dp[m][n] = ct*dp[m][n-1]-st**(p+n-1+m*13);
 214                                         goto S50;
 215                                 }
 216                                 if (n > 1 && n != m) {
 217                                         if (m > n-2) *(p+n-2+m*13) = 0.0;
 218                                         if (m > n-2) dp[m][n-2] = 0.0;
 219                                         *(p+n+m*13) = ct**(p+n-1+m*13)-k[m][n]**(p+n-2+m*13);
 220                                         dp[m][n] = ct*dp[m][n-1] - st**(p+n-1+m*13)-k[m][n]*dp[m][n-2];
 221                                 }
 222                         }
 223 S50:
 224                         if (!is_float_equal(time, otime)) {
 225                                 tc[m][n] = c[m][n]+dt*cd[m][n];
 226                                 if (m != 0) tc[n][m-1] = c[n][m-1]+dt*cd[n][m-1];
 227                         }
 228
 229                         par = ar**(p+n+m*13);
 230                         if (m == 0) {
 231                                 temp1 = tc[m][n]*cp[m];
 232                                 temp2 = tc[m][n]*sp[m];
 233                         } else {
 234                                 temp1 = tc[m][n]*cp[m]+tc[n][m-1]*sp[m];
 235                                 temp2 = tc[m][n]*sp[m]-tc[n][m-1]*cp[m];
 236                         }
 237                         bt = bt-ar*temp1*dp[m][n];
 238                         bp += (fm[m]*temp2*par);
 239                         br += (fn[n]*temp1*par);
 240
 241                         if (is_float_equal(st, 0.0) && m == 1) {
 242                                 if (n == 1) pp[n] = pp[n-1];
 243                                 else pp[n] = ct*pp[n-1]-k[m][n]*pp[n-2];
 244                                 parp = ar*pp[n];
 245                                 bpp += (fm[m]*temp2*parp);
 246                         }
 247                 }
 248         }
 249         if (is_float_equal(st, 0.0)) bp = bpp;
 250         else bp /= st;
 251
 252         bx = -bt*ca-br*sa;
 253         by = bp;
 254         bz = bt*sa-br*ca;
 255         bh = sqrt((bx*bx)+(by*by));
 256         *ti = sqrt((bh*bh)+(bz*bz));
 257         *dec = atan2(by, bx)/dtr;
 258         *dip = atan2(bz, bh)/dtr;
 259         *gv = -999.0;
 260         if (fabs(glat) >= 55.) {
 261                 if (glat > 0.0 && glon >= 0.0) *gv = *dec-glon;
 262                 if (glat > 0.0 && glon < 0.0) *gv = *dec+fabs(glon);
 263                 if (glat < 0.0 && glon >= 0.0) *gv = *dec+glon;
 264                 if (glat < 0.0 && glon < 0.0) *gv = *dec-fabs(glon);
 265                 if (*gv > +180.0) *gv -= 360.0;
 266                 if (*gv < -180.0) *gv += 360.0;
 267         }
 268         otime = time;
 269         oalt = alt;
 270         olat = glat;
 271         olon = glon;
 272         return;
 273 }
 274