// Released under GPL 3/26/00 EAW
// Adaptions and modifications for the SimGear project 3/27/2000 CLO
//
+// Removed all pow() calls and made static roots[][] arrays to
+// save many sqrt() calls on subsequent invocations
+// left old code as SGMagVarOrig() for testing purposes
+// 3/28/2000 Norman Vine -- nhv@yahoo.com
+//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
// published by the Free Software Foundation; either version 2 of the
static double sm[13];
static double cm[13];
+static double root[13];
+static double roots[13][13][2];
/* Convert date to Julian day 1950-2049 */
unsigned long int yymmdd_to_julian_days( int yy, int mm, int dd )
}
-/* return variation (in degrees) given geodetic latitude (radians), longitude
-(radians) ,height (km) and (Julian) date
-N and E lat and long are positive, S and W negative
+/*
+ * return variation (in radians) given geodetic latitude (radians),
+ * longitude(radians), height (km) and (Julian) date
+ * N and E lat and long are positive, S and W negative
*/
double SGMagVar( double lat, double lon, double h, long dat, double* field )
/* reference dates */
long date0_wmm2000 = yymmdd_to_julian_days(0,1,1);
+ double yearfrac,sr,r,theta,c,s,psi,fn,fn_0,B_r,B_theta,B_phi,X,Y,Z;
+ double sinpsi, cospsi, inv_s;
+
+ static int been_here = 0;
+
+ double sinlat = sin(lat);
+ double coslat = cos(lat);
+
+ /* convert to geocentric coords: */
+ // sr = sqrt(pow(a*coslat,2.0)+pow(b*sinlat,2.0));
+ sr = sqrt(a*a*coslat*coslat + b*b*sinlat*sinlat);
+ /* sr is effective radius */
+ theta = atan2(coslat * (h*sr + a*a),
+ sinlat * (h*sr + b*b));
+ /* theta is geocentric co-latitude */
+
+ r = h*h + 2.0*h * sr +
+ (a*a*a*a - ( a*a*a*a - b*b*b*b ) * sinlat*sinlat ) /
+ (a*a - (a*a - b*b) * sinlat*sinlat );
+
+ r = sqrt(r);
+
+ /* r is geocentric radial distance */
+ c = cos(theta);
+ s = sin(theta);
+ inv_s = 1.0 / s;
+
+ /*zero out arrays */
+ for ( n = 0; n <= nmax; n++ ) {
+ for ( m = 0; m <= n; m++ ) {
+ P[n][m] = 0;
+ DP[n][m] = 0;
+ }
+ }
+
+ /* diagonal elements */
+ P[0][0] = 1;
+ P[1][1] = s;
+ DP[0][0] = 0;
+ DP[1][1] = c;
+ P[1][0] = c ;
+ DP[1][0] = -s;
+
+ // these values will not change for subsequent function calls
+ if( !been_here ) {
+ for ( n = 2; n <= nmax; n++ ) {
+ root[n] = sqrt((2.0*n-1) / (2.0*n));
+ }
+
+ for ( m = 0; m <= nmax; m++ ) {
+ double mm = m*m;
+ for ( n = max(m + 1, 2); n <= nmax; n++ ) {
+ roots[m][n][0] = sqrt((n-1)*(n-1) - mm);
+ roots[m][n][1] = 1.0 / sqrt( n*n - mm);
+ }
+ }
+ been_here = 1;
+ }
+
+ for ( n=2; n <= nmax; n++ ) {
+ // double root = sqrt((2.0*n-1) / (2.0*n));
+ P[n][n] = P[n-1][n-1] * s * root[n];
+ DP[n][n] = (DP[n-1][n-1] * s + P[n-1][n-1] * c) *
+ root[n];
+ }
+
+ /* lower triangle */
+ for ( m = 0; m <= nmax; m++ ) {
+ // double mm = m*m;
+ for ( n = max(m + 1, 2); n <= nmax; n++ ) {
+ // double root1 = sqrt((n-1)*(n-1) - mm);
+ // double root2 = 1.0 / sqrt( n*n - mm);
+ P[n][m] = (P[n-1][m] * c * (2.0*n-1) -
+ P[n-2][m] * roots[m][n][0]) *
+ roots[m][n][1];
+
+ DP[n][m] = ((DP[n-1][m] * c - P[n-1][m] * s) *
+ (2.0*n-1) - DP[n-2][m] * roots[m][n][0]) *
+ roots[m][n][1];
+ }
+ }
+
+ /* compute gnm, hnm at dat */
+ /* WMM2000 */
+ yearfrac = (dat - date0_wmm2000) / 365.25;
+ for ( n = 1; n <= nmax; n++ ) {
+ for ( m = 0; m <= nmax; m++ ) {
+ gnm[n][m] = gnm_wmm2000[n][m] + yearfrac * gtnm_wmm2000[n][m];
+ hnm[n][m] = hnm_wmm2000[n][m] + yearfrac * htnm_wmm2000[n][m];
+ }
+ }
+
+ /* compute sm (sin(m lon) and cm (cos(m lon)) */
+ for ( m = 0; m <= nmax; m++ ) {
+ sm[m] = sin(m * lon);
+ cm[m] = cos(m * lon);
+ }
+
+ /* compute B fields */
+ B_r = 0.0;
+ B_theta = 0.0;
+ B_phi = 0.0;
+ fn_0 = r_0/r;
+ fn = fn_0 * fn_0;
+
+ for ( n = 1; n <= nmax; n++ ) {
+ double c1_n=0;
+ double c2_n=0;
+ double c3_n=0;
+ for ( m = 0; m <= n; m++ ) {
+ double tmp = (gnm[n][m] * cm[m] + hnm[n][m] * sm[m]);
+ c1_n=c1_n + tmp * P[n][m];
+ c2_n=c2_n + tmp * DP[n][m];
+ c3_n=c3_n + m * (gnm[n][m] * sm[m] - hnm[n][m] * cm[m]) * P[n][m];
+ }
+ // fn=pow(r_0/r,n+2.0);
+ fn *= fn_0;
+ B_r = B_r + (n + 1) * c1_n * fn;
+ B_theta = B_theta - c2_n * fn;
+ B_phi = B_phi + c3_n * fn * inv_s;
+ }
+
+ /* Find geodetic field components: */
+ psi = theta - ((pi / 2.0) - lat);
+ sinpsi = sin(psi);
+ cospsi = cos(psi);
+ X = -B_theta * cospsi - B_r * sinpsi;
+ Y = B_phi;
+ Z = B_theta * sinpsi - B_r * cospsi;
+
+ field[0]=B_r;
+ field[1]=B_theta;
+ field[2]=B_phi;
+ field[3]=X;
+ field[4]=Y;
+ field[5]=Z; /* output fields */
+
+ /* find variation, leave in radians! */
+ return atan2(Y, X); /* E is positive */
+}
+
+
+#ifdef TEST_NHV_HACKS
+double SGMagVarOrig( double lat, double lon, double h, long dat, double* field )
+{
+ /* output field B_r,B_th,B_phi,B_x,B_y,B_z */
+ int n,m;
+ /* reference dates */
+ long date0_wmm2000 = yymmdd_to_julian_days(0,1,1);
+
double yearfrac,sr,r,theta,c,s,psi,fn,B_r,B_theta,B_phi,X,Y,Z;
/* convert to geocentric coords: */
r = sqrt(r);
- /* r is geocentric radial distance */
+ /* r is geocentric radial distance */
c = cos(theta);
s = sin(theta);
- /*zero out arrays */
+ /*zero out arrays */
for ( n = 0; n <= nmax; n++ ) {
for ( m = 0; m <= n; m++ ) {
P[n][m] = 0;
field[4]=Y;
field[5]=Z; /* output fields */
- /* find variation, leave in radians! */
+ /* find variation, leave in radians! */
return atan2(Y, X); /* E is positive */
}
-
+#endif // TEST_NHV_HACKS
projects / simgear.git / commitdiff