uvsim: /home/jgoppert/Projects/uvsim/uvsim/src/uvsim/navigation/GeoMag.cc Source File

00001 /*
00002  * GeoMag.cc
00003  * Copyright (C) James Goppert 2009 <jgoppert@users.sourceforge.net>
00004  *
00005  * GeoMag.cc is free software: you can redistribute it and/or modify it
00006  * under the terms of the GNU General Public License as published by the
00007  * Free Software Foundation, either version 3 of the License, or
00008  * (at your option) any later version.
00009  *
00010  * GeoMag.cc is distributed in the hope that it will be useful, but
00011  * WITHOUT ANY WARRANTY; without even the implied warranty of
00012  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00013  * See the GNU General Public License for more details.
00014  *
00015  * You should have received a copy of the GNU General Public License along
00016  * with this program.  If not, see <http://www.gnu.org/licenses/>.
00017  */
00018 
00019 #include "GeoMag.h"
00020 
00021 namespace uvsim
00022 {
00023 
00024 GeoMag::GeoMag(std::string file, int maxDeg)
00025 {
00026     wmmdat = fopen(file.c_str(),"r");
00027     p = snorm;
00028 
00029     /* INITIALIZE CONSTANTS */
00030     pi = 3.14159265359;
00031     maxord = maxDeg;
00032     sp[0] = 0.0;
00033     cp[0] = *p = pp[0] = 1.0;
00034     dp[0][0] = 0.0;
00035     a = 6378.137;
00036     b = 6356.7523142;
00037     re = 6371.2;
00038     a2 = a*a;
00039     b2 = b*b;
00040     c2 = a2-b2;
00041     a4 = a2*a2;
00042     b4 = b2*b2;
00043     c4 = a4 - b4;
00044 
00045     /* READ WORLD MAGNETIC MODEL SPHERICAL HARMONIC COEFFICIENTS */
00046     c[0][0] = 0.0;
00047     cd[0][0] = 0.0;
00048 
00049     fgets(c_str, 80, wmmdat);
00050     sscanf(c_str,"%f%s",&epoch,model);
00051 S3:
00052     fgets(c_str, 80, wmmdat);
00053     /* CHECK FOR LAST LINE IN FILE */
00054     for (i=0; i<4 && (c_str[i] != '\0'); i++)
00055     {
00056         c_new[i] = c_str[i];
00057         c_new[i+1] = '\0';
00058     }
00059     icomp = strcmp("9999", c_new);
00060     if (icomp == 0) goto S4;
00061     /* END OF FILE NOT ENCOUNTERED, GET VALUES */
00062     sscanf(c_str,"%d%d%f%f%f%f",&n,&m,&gnm,&hnm,&dgnm,&dhnm);
00063     if (m <= n)
00064     {
00065         c[m][n] = gnm;
00066         cd[m][n] = dgnm;
00067         if (m != 0)
00068         {
00069             c[n][m-1] = hnm;
00070             cd[n][m-1] = dhnm;
00071         }
00072     }
00073     goto S3;
00074 
00075     /* CONVERT SCHMIDT NORMALIZED GAUSS COEFFICIENTS TO UNNORMALIZED */
00076 S4:
00077     *snorm = 1.0;
00078     for (n=1; n<=maxord; n++)
00079     {
00080         *(snorm+n) = *(snorm+n-1)*(float)(2*n-1)/(float)n;
00081         j = 2;
00082         for (m=0,D1=1,D2=(n-m+D1)/D1; D2>0; D2--,m+=D1)
00083         {
00084             k[m][n] = (float)(((n-1)*(n-1))-(m*m))/(float)((2*n-1)*(2*n-3));
00085             if (m > 0)
00086             {
00087                 flnmj = (float)((n-m+1)*j)/(float)(n+m);
00088                 *(snorm+n+m*13) = *(snorm+n+(m-1)*13)*sqrt(flnmj);
00089                 j = 1;
00090                 c[n][m-1] = *(snorm+n+m*13)*c[n][m-1];
00091                 cd[n][m-1] = *(snorm+n+m*13)*cd[n][m-1];
00092             }
00093             c[m][n] = *(snorm+n+m*13)*c[m][n];
00094             cd[m][n] = *(snorm+n+m*13)*cd[m][n];
00095         }
00096         fn[n] = (float)(n+1);
00097         fm[n] = (float)n;
00098     }
00099     k[1][1] = 0.0;
00100 
00101     otime = oalt = olat = olon = -1000.0;
00102     fclose(wmmdat);
00103 }
00104 
00105 
00106 void GeoMag::update(float alt, float glat, float glon, float time)
00107 {
00108     alt = alt/1000; // conver altitude to km
00109     dt = time - epoch;
00110     if (otime < 0.0 && (dt < 0.0 || dt > 5.0))
00111     {
00112         printf("\n\n WARNING - TIME EXTENDS BEYOND MODEL 5-YEAR LIFE SPAN");
00113         printf("\n CONTACT NGDC FOR PRODUCT UPDATES:");
00114         printf("\n         National Geophysical Data Center");
00115         printf("\n         NOAA EGC/2");
00116         printf("\n         325 Broadway");
00117         printf("\n         Boulder, CO 80303 USA");
00118         printf("\n         Attn: Susan McLean or Stefan Maus");
00119         printf("\n         Phone:  (303) 497-6478 or -6522");
00120         printf("\n         Email:  Susan.McLean@noaa.gov");
00121         printf("\n         or");
00122         printf("\n         Stefan.Maus@noaa.gov");
00123         printf("\n         Web: http://www.ngdc.noaa.gov/seg/WMM/");
00124         printf("\n\n EPOCH  = %.3lf",epoch);
00125         printf("\n TIME   = %.3lf",time);
00126     }
00127 
00128     dtr = pi/180.0;
00129     rlon = glon*dtr;
00130     rlat = glat*dtr;
00131     srlon = sin(rlon);
00132     srlat = sin(rlat);
00133     crlon = cos(rlon);
00134     crlat = cos(rlat);
00135     srlat2 = srlat*srlat;
00136     crlat2 = crlat*crlat;
00137     sp[1] = srlon;
00138     cp[1] = crlon;
00139 
00140     /* CONVERT FROM GEODETIC COORDS. TO SPHERICAL COORDS. */
00141     if (alt != oalt || glat != olat)
00142     {
00143         q = sqrt(a2-c2*srlat2);
00144         q1 = alt*q;
00145         q2 = ((q1+a2)/(q1+b2))*((q1+a2)/(q1+b2));
00146         ct = srlat/sqrt(q2*crlat2+srlat2);
00147         st = sqrt(1.0-(ct*ct));
00148         r2 = (alt*alt)+2.0*q1+(a4-c4*srlat2)/(q*q);
00149         r = sqrt(r2);
00150         d = sqrt(a2*crlat2+b2*srlat2);
00151         ca = (alt+d)/r;
00152         sa = c2*crlat*srlat/(r*d);
00153     }
00154     if (glon != olon)
00155     {
00156         for (m=2; m<=maxord; m++)
00157         {
00158             sp[m] = sp[1]*cp[m-1]+cp[1]*sp[m-1];
00159             cp[m] = cp[1]*cp[m-1]-sp[1]*sp[m-1];
00160         }
00161     }
00162     aor = re/r;
00163     ar = aor*aor;
00164     br = bt = bp = bpp = 0.0;
00165     for (n=1; n<=maxord; n++)
00166     {
00167         ar = ar*aor;
00168         for (m=0,D3=1,D4=(n+m+D3)/D3; D4>0; D4--,m+=D3)
00169         {
00170             /*
00171                COMPUTE UNNORMALIZED ASSOCIATED LEGENDRE POLYNOMIALS
00172                AND DERIVATIVES VIA RECURSION RELATIONS
00173             */
00174             if (alt != oalt || glat != olat)
00175             {
00176                 if (n == m)
00177                 {
00178                     *(p+n+m*13) = st**(p+n-1+(m-1)*13);
00179                     dp[m][n] = st*dp[m-1][n-1]+ct**(p+n-1+(m-1)*13);
00180                     goto S50;
00181                 }
00182                 if (n == 1 && m == 0)
00183                 {
00184                     *(p+n+m*13) = ct**(p+n-1+m*13);
00185                     dp[m][n] = ct*dp[m][n-1]-st**(p+n-1+m*13);
00186                     goto S50;
00187                 }
00188                 if (n > 1 && n != m)
00189                 {
00190                     if (m > n-2) *(p+n-2+m*13) = 0.0;
00191                     if (m > n-2) dp[m][n-2] = 0.0;
00192                     *(p+n+m*13) = ct**(p+n-1+m*13)-k[m][n]**(p+n-2+m*13);
00193                     dp[m][n] = ct*dp[m][n-1] - st**(p+n-1+m*13)-k[m][n]*dp[m][n-2];
00194                 }
00195             }
00196 S50:
00197             /*
00198                 TIME ADJUST THE GAUSS COEFFICIENTS
00199             */
00200             if (time != otime)
00201             {
00202                 tc[m][n] = c[m][n]+dt*cd[m][n];
00203                 if (m != 0) tc[n][m-1] = c[n][m-1]+dt*cd[n][m-1];
00204             }
00205             /*
00206                 ACCUMULATE TERMS OF THE SPHERICAL HARMONIC EXPANSIONS
00207             */
00208             par = ar**(p+n+m*13);
00209             if (m == 0)
00210             {
00211                 temp1 = tc[m][n]*cp[m];
00212                 temp2 = tc[m][n]*sp[m];
00213             }
00214             else
00215             {
00216                 temp1 = tc[m][n]*cp[m]+tc[n][m-1]*sp[m];
00217                 temp2 = tc[m][n]*sp[m]-tc[n][m-1]*cp[m];
00218             }
00219             bt = bt-ar*temp1*dp[m][n];
00220             bp += (fm[m]*temp2*par);
00221             br += (fn[n]*temp1*par);
00222             /*
00223                 SPECIAL CASE:  NORTH/SOUTH GEOGRAPHIC POLES
00224             */
00225             if (st == 0.0 && m == 1)
00226             {
00227                 if (n == 1) pp[n] = pp[n-1];
00228                 else pp[n] = ct*pp[n-1]-k[m][n]*pp[n-2];
00229                 parp = ar*pp[n];
00230                 bpp += (fm[m]*temp2*parp);
00231             }
00232         }
00233     }
00234     if (st == 0.0) bp = bpp;
00235     else bp /= st;
00236     /*
00237         ROTATE MAGNETIC VECTOR COMPONENTS FROM SPHERICAL TO
00238         GEODETIC COORDINATES
00239     */
00240     bx = -bt*ca-br*sa;
00241     by = bp;
00242     bz = bt*sa-br*ca;
00243     /*
00244         COMPUTE DECLINATION (DEC), INCLINATION (DIP) AND
00245         TOTAL INTENSITY (TI)
00246     */
00247     bh = sqrt((bx*bx)+(by*by));
00248     ti = sqrt((bh*bh)+(bz*bz));
00249     dec = atan2(by,bx)/dtr;
00250     dip = atan2(bz,bh)/dtr;
00251     /*
00252         COMPUTE MAGNETIC GRID VARIATION IF THE CURRENT
00253         GEODETIC POSITION IS IN THE ARCTIC OR ANTARCTIC
00254         (I.E. GLAT > +55 DEGREES OR GLAT < -55 DEGREES)
00255 
00256     TO -999.0
00257     */
00258     gv = -999.0;
00259     if (fabs(glat) >= 55.)
00260     {
00261         if (glat > 0.0 && glon >= 0.0) gv = dec-glon;
00262         if (glat > 0.0 && glon < 0.0) gv = dec+fabs(glon);
00263         if (glat < 0.0 && glon >= 0.0) gv = dec+glon;
00264         if (glat < 0.0 && glon < 0.0) gv = dec-fabs(glon);
00265         if (gv > +180.0) gv -= 360.0;
00266         if (gv < -180.0) gv += 360.0;
00267     }
00268     otime = time;
00269     oalt = alt;
00270     olat = glat;
00271     olon = glon;
00272     return;
00273 }
00274 
00275 GeoMag::~GeoMag()
00276 {
00277 }
00278 
00279 }
00280 // vim:ts=4:sw=4