diff options
author | Eric S. Raymond <esr@thyrsus.com> | 2010-04-14 05:34:17 -0400 |
---|---|---|
committer | Eric S. Raymond <esr@thyrsus.com> | 2010-04-14 05:34:17 -0400 |
commit | e37de01df5aaf6161cd88424cb522f590355f5d8 (patch) | |
tree | 58100ce4ee258635e5cf3bd5a31f97c835ed7716 /geoid.c | |
parent | 29f16317f536d8f525bff3a3b8f9541076fab550 (diff) | |
download | gpsd-e37de01df5aaf6161cd88424cb522f590355f5d8.tar.gz |
Reindented some small utility modules. All regression tests pass.
Diffstat (limited to 'geoid.c')
-rw-r--r-- | geoid.c | 113 |
1 files changed, 63 insertions, 50 deletions
@@ -14,21 +14,28 @@ static double fix_minuz(double d); -static double bilinear(double x1, double y1, double x2, double y2, double x, double y, double z11, double z12, double z21, double z22) +static double bilinear(double x1, double y1, double x2, double y2, double x, + double y, double z11, double z12, double z21, + double z22) { - double delta; + double delta; - if (y1 == y2 && x1 == x2 ) return (z11); - if (y1 == y2 && x1 != x2 ) return (z22*(x-x1)+z11*(x2-x))/(x2-x1); - if (x1 == x2 && y1 != y2 ) return (z22*(y-y1)+z11*(y2-y))/(y2-y1); + if (y1 == y2 && x1 == x2) + return (z11); + if (y1 == y2 && x1 != x2) + return (z22 * (x - x1) + z11 * (x2 - x)) / (x2 - x1); + if (x1 == x2 && y1 != y2) + return (z22 * (y - y1) + z11 * (y2 - y)) / (y2 - y1); - delta=(y2-y1)*(x2-x1); + delta = (y2 - y1) * (x2 - x1); - return (z22*(y-y1)*(x-x1)+z12*(y2-y)*(x-x1)+z21*(y-y1)*(x2-x)+z11*(y2-y)*(x2-x))/delta; + return (z22 * (y - y1) * (x - x1) + z12 * (y2 - y) * (x - x1) + + z21 * (y - y1) * (x2 - x) + z11 * (y2 - y) * (x2 - x)) / delta; } /* return geoid separtion (MSL - WGS84) in meters, given a lat/lot in degrees */ +/* *INDENT-OFF* */ /*@ +charint @*/ double wgs84_separation(double lat, double lon) { @@ -55,65 +62,71 @@ double wgs84_separation(double lat, double lon) /* 80N */ 3, 1, -2, -3, -3, -3, -1, 3, 1, 5, 9, 11, 19, 27, 31, 34, 33, 34, 33, 34, 28, 23, 17, 13, 9, 4, 4, 1, -2, -2, 0, 2, 3, 2, 1, 1, 3, /* 90N */ 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13, 13}; /*@ -charint @*/ - int ilat, ilon; - int ilat1, ilat2, ilon1, ilon2; +/* *INDENT-ON* */ +int ilat, ilon; +int ilat1, ilat2, ilon1, ilon2; + +ilat = (int)floor((90. + lat) / 10); +ilon = (int)floor((180. + lon) / 10); - ilat=(int)floor(( 90.+lat)/10); - ilon=(int)floor((180.+lon)/10); - /* sanity checks to prevent segfault on bad data */ - if ( ( ilat > 90 ) || ( ilat < -90 ) ) { - return 0.0; - } - if ( ( ilon > 180 ) || ( ilon < -180 ) ) { - return 0.0; - } - - ilat1=ilat; - ilon1=ilon; - ilat2=(ilat < GEOID_ROW-1)? ilat+1:ilat; - ilon2=(ilon < GEOID_COL-1)? ilon+1:ilon; - - return bilinear( - ilon1*10.-180.,ilat1*10.-90., - ilon2*10.-180.,ilat2*10.-90., - lon, lat, - (double)geoid_delta[ilon1+ilat1*GEOID_COL], - (double)geoid_delta[ilon2+ilat1*GEOID_COL], - (double)geoid_delta[ilon1+ilat2*GEOID_COL], - (double)geoid_delta[ilon2+ilat2*GEOID_COL] - ); +if ((ilat > 90) || (ilat < -90)) { + return 0.0; +} +if ((ilon > 180) || (ilon < -180)) { + return 0.0; +} + +ilat1 = ilat; +ilon1 = ilon; +ilat2 = (ilat < GEOID_ROW - 1) ? ilat + 1 : ilat; +ilon2 = (ilon < GEOID_COL - 1) ? ilon + 1 : ilon; + +return bilinear(ilon1 * 10. - 180., ilat1 * 10. - 90., + ilon2 * 10. - 180., ilat2 * 10. - 90., + lon, lat, + (double)geoid_delta[ilon1 + ilat1 * GEOID_COL], + (double)geoid_delta[ilon2 + ilat1 * GEOID_COL], + (double)geoid_delta[ilon1 + ilat2 * GEOID_COL], + (double)geoid_delta[ilon2 + ilat2 * GEOID_COL] + ); } void ecef_to_wgs84fix(struct gps_fix_t *fix, double *separation, - double x, double y, double z, - double vx, double vy, double vz) + double x, double y, double z, + double vx, double vy, double vz) /* fill in WGS84 position/velocity fields from ECEF coordinates */ { - double lambda,phi,p,theta,n,h,vnorth,veast,heading; - const double a = WGS84A; /* equatorial radius */ - const double b = WGS84B; /* polar radius */ - const double e2 = (a*a - b*b) / (a*a); - const double e_2 = (a*a - b*b) / (b*b); + double lambda, phi, p, theta, n, h, vnorth, veast, heading; + const double a = WGS84A; /* equatorial radius */ + const double b = WGS84B; /* polar radius */ + const double e2 = (a * a - b * b) / (a * a); + const double e_2 = (a * a - b * b) / (b * b); /* geodetic location */ - lambda = atan2(y,x); - /*@ -evalorder @*/ - p = sqrt(pow(x,2) + pow(y,2)); - theta = atan2(z*a,p*b); - phi = atan2(z + e_2*b*pow(sin(theta),3),p - e2*a*pow(cos(theta),3)); - n = a / sqrt(1.0 - e2*pow(sin(phi),2)); + lambda = atan2(y, x); + /*@ -evalorder @*/ + p = sqrt(pow(x, 2) + pow(y, 2)); + theta = atan2(z * a, p * b); + phi = + atan2(z + e_2 * b * pow(sin(theta), 3), + p - e2 * a * pow(cos(theta), 3)); + n = a / sqrt(1.0 - e2 * pow(sin(phi), 2)); h = p / cos(phi) - n; fix->latitude = phi * RAD_2_DEG; fix->longitude = lambda * RAD_2_DEG; *separation = wgs84_separation(fix->latitude, fix->longitude); fix->altitude = h - *separation; /* velocity computation */ - vnorth = -vx*sin(phi)*cos(lambda)-vy*sin(phi)*sin(lambda)+vz*cos(phi); - veast = -vx*sin(lambda)+vy*cos(lambda); - fix->climb = vx*cos(phi)*cos(lambda)+vy*cos(phi)*sin(lambda)+vz*sin(phi); - fix->speed = sqrt(pow(vnorth,2) + pow(veast,2)); + vnorth = + -vx * sin(phi) * cos(lambda) - vy * sin(phi) * sin(lambda) + + vz * cos(phi); + veast = -vx * sin(lambda) + vy * cos(lambda); + fix->climb = + vx * cos(phi) * cos(lambda) + vy * cos(phi) * sin(lambda) + + vz * sin(phi); + fix->speed = sqrt(pow(vnorth, 2) + pow(veast, 2)); heading = atan2(fix_minuz(veast), fix_minuz(vnorth)); /*@ +evalorder @*/ if (heading < 0) |