00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 #include "GeographicLib/Geoid.hpp"
00011 #include <sstream>
00012 #include <cstdlib>
00013 #include <algorithm>
00014
00015 #define GEOGRAPHICLIB_GEOID_CPP "$Id: Geoid.cpp 6967 2011-02-19 15:53:41Z karney $"
00016
00017 RCSID_DECL(GEOGRAPHICLIB_GEOID_CPP)
00018 RCSID_DECL(GEOGRAPHICLIB_GEOID_HPP)
00019
00020 #if !defined(GEOID_DEFAULT_PATH)
00021 #if defined(_MSC_VER)
00022 #define GEOID_DEFAULT_PATH "C:/cygwin/usr/local/share/GeographicLib/geoids"
00023 #else
00024 #define GEOID_DEFAULT_PATH "/usr/local/share/GeographicLib/geoids"
00025 #endif
00026 #endif
00027 #if !defined(GEOID_DEFAULT_NAME)
00028 #define GEOID_DEFAULT_NAME "egm96-5"
00029 #endif
00030
00031 #if defined(_MSC_VER)
00032
00033 #pragma warning (disable: 4996)
00034 #endif
00035
00036 namespace GeographicLib {
00037
00038 using namespace std;
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104 const Math::real Geoid::c0 = 240;
00105 const Math::real Geoid::c3[stencilsize * nterms] = {
00106 9, -18, -88, 0, 96, 90, 0, 0, -60, -20,
00107 -9, 18, 8, 0, -96, 30, 0, 0, 60, -20,
00108 9, -88, -18, 90, 96, 0, -20, -60, 0, 0,
00109 186, -42, -42, -150, -96, -150, 60, 60, 60, 60,
00110 54, 162, -78, 30, -24, -90, -60, 60, -60, 60,
00111 -9, -32, 18, 30, 24, 0, 20, -60, 0, 0,
00112 -9, 8, 18, 30, -96, 0, -20, 60, 0, 0,
00113 54, -78, 162, -90, -24, 30, 60, -60, 60, -60,
00114 -54, 78, 78, 90, 144, 90, -60, -60, -60, -60,
00115 9, -8, -18, -30, -24, 0, 20, 60, 0, 0,
00116 -9, 18, -32, 0, 24, 30, 0, 0, -60, 20,
00117 9, -18, -8, 0, -24, -30, 0, 0, 60, 20,
00118 };
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154 const Math::real Geoid::c0n = 372;
00155 const Math::real Geoid::c3n[stencilsize * nterms] = {
00156 0, 0, -131, 0, 138, 144, 0, 0, -102, -31,
00157 0, 0, 7, 0, -138, 42, 0, 0, 102, -31,
00158 62, 0, -31, 0, 0, -62, 0, 0, 0, 31,
00159 124, 0, -62, 0, 0, -124, 0, 0, 0, 62,
00160 124, 0, -62, 0, 0, -124, 0, 0, 0, 62,
00161 62, 0, -31, 0, 0, -62, 0, 0, 0, 31,
00162 0, 0, 45, 0, -183, -9, 0, 93, 18, 0,
00163 0, 0, 216, 0, 33, 87, 0, -93, 12, -93,
00164 0, 0, 156, 0, 153, 99, 0, -93, -12, -93,
00165 0, 0, -45, 0, -3, 9, 0, 93, -18, 0,
00166 0, 0, -55, 0, 48, 42, 0, 0, -84, 31,
00167 0, 0, -7, 0, -48, -42, 0, 0, 84, 31,
00168 };
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188 const Math::real Geoid::c0s = 372;
00189 const Math::real Geoid::c3s[stencilsize * nterms] = {
00190 18, -36, -122, 0, 120, 135, 0, 0, -84, -31,
00191 -18, 36, -2, 0, -120, 51, 0, 0, 84, -31,
00192 36, -165, -27, 93, 147, -9, 0, -93, 18, 0,
00193 210, 45, -111, -93, -57, -192, 0, 93, 12, 93,
00194 162, 141, -75, -93, -129, -180, 0, 93, -12, 93,
00195 -36, -21, 27, 93, 39, 9, 0, -93, -18, 0,
00196 0, 0, 62, 0, 0, 31, 0, 0, 0, -31,
00197 0, 0, 124, 0, 0, 62, 0, 0, 0, -62,
00198 0, 0, 124, 0, 0, 62, 0, 0, 0, -62,
00199 0, 0, 62, 0, 0, 31, 0, 0, 0, -31,
00200 -18, 36, -64, 0, 66, 51, 0, 0, -102, 31,
00201 18, -36, 2, 0, -66, -51, 0, 0, 102, 31,
00202 };
00203
00204 Geoid::Geoid(const std::string& name, const std::string& path, bool cubic,
00205 bool threadsafe)
00206 : _name(name)
00207 , _dir(path)
00208 , _cubic(cubic)
00209 , _a( Constants::WGS84_a<real>() )
00210 , _e2( (2 - 1/Constants::WGS84_r<real>())/Constants::WGS84_r<real>() )
00211 , _degree( Math::degree<real>() )
00212 , _eps( sqrt(numeric_limits<real>::epsilon()) )
00213 , _threadsafe(false)
00214 {
00215 if (_dir.empty())
00216 _dir = DefaultGeoidPath();
00217 _filename = _dir + "/" + _name + ".pgm";
00218 _file.open(_filename.c_str(), ios::binary);
00219 if (!(_file.good()))
00220 throw GeographicErr("File not readable " + _filename);
00221 string s;
00222 if (!(getline(_file, s) && s == "P5"))
00223 throw GeographicErr("File not in PGM format " + _filename);
00224 _offset = numeric_limits<real>::max();
00225 _scale = 0;
00226 _maxerror = _rmserror = -1;
00227 _description = "NONE";
00228 _datetime = "UNKNOWN";
00229 while (getline(_file, s)) {
00230 if (s.empty())
00231 continue;
00232 if (s[0] == '#') {
00233 istringstream is(s);
00234 string commentid, key;
00235 if (!(is >> commentid >> key) || commentid != "#")
00236 continue;
00237 if (key == "Description" || key =="DateTime") {
00238 string::size_type p =
00239 s.find_first_not_of(" \t", unsigned(is.tellg()));
00240 if (p != string::npos)
00241 (key == "Description" ? _description : _datetime) = s.substr(p);
00242 } else if (key == "Offset") {
00243 if (!(is >> _offset))
00244 throw GeographicErr("Error reading offset " + _filename);
00245 } else if (key == "Scale") {
00246 if (!(is >> _scale))
00247 throw GeographicErr("Error reading scale " + _filename);
00248 } else if (key == (_cubic ? "MaxCubicError" : "MaxBilinearError")) {
00249
00250 is >> _maxerror;
00251 } else if (key == (_cubic ? "RMSCubicError" : "RMSBilinearError")) {
00252
00253 is >> _rmserror;
00254 }
00255 } else {
00256 istringstream is(s);
00257 if (!(is >> _width >> _height))
00258 throw GeographicErr("Error reading raster size " + _filename);
00259 break;
00260 }
00261 }
00262 {
00263 unsigned maxval;
00264 if (!(_file >> maxval))
00265 throw GeographicErr("Error reading maxval " + _filename);
00266 if (maxval != 0xffffu)
00267 throw GeographicErr("Maxval not equal to 2^16-1 " + _filename);
00268
00269 _datastart = (unsigned long long)(_file.tellg()) + 1ULL;
00270 _swidth = (unsigned long long)(_width);
00271 }
00272 if (_offset == numeric_limits<real>::max())
00273 throw GeographicErr("Offset not set " + _filename);
00274 if (_scale == 0)
00275 throw GeographicErr("Scale not set " + _filename);
00276 if (_scale < 0)
00277 throw GeographicErr("Scale must be positive " + _filename);
00278 if (_height < 2 || _width < 2)
00279
00280 throw GeographicErr("Raster size too small " + _filename);
00281 if (_width & 1)
00282
00283 throw GeographicErr("Raster width is odd " + _filename);
00284 if (!(_height & 1))
00285
00286 throw GeographicErr("Raster height is even " + _filename);
00287 _file.seekg(0, ios::end);
00288 if (!_file.good() ||
00289 _datastart + 2ULL * _swidth * (unsigned long long)(_height) !=
00290 (unsigned long long)(_file.tellg()))
00291
00292
00293 throw GeographicErr("File has the wrong length " + _filename);
00294 _rlonres = _width / real(360);
00295 _rlatres = (_height - 1) / real(180);
00296 _cache = false;
00297 _ix = _width;
00298 _iy = _height;
00299
00300 _file.exceptions(ifstream::eofbit | ifstream::failbit | ifstream::badbit);
00301 if (threadsafe) {
00302 CacheAll();
00303 _file.close();
00304 _threadsafe = true;
00305 }
00306 }
00307
00308 Math::real Geoid::height(real lat, real lon, bool gradp,
00309 real& gradn, real& grade) const {
00310 if (Math::isnan(lat) || Math::isnan(lon)) {
00311 if (gradp) gradn = grade = Math::NaN();
00312 return Math::NaN();
00313 }
00314 real
00315 fx = lon * _rlonres,
00316 fy = -lat * _rlatres;
00317 int
00318 ix = int(floor(fx)),
00319 iy = min((_height - 1)/2 - 1, int(floor(fy)));
00320 fx -= ix;
00321 fy -= iy;
00322 iy += (_height - 1)/2;
00323 ix += ix < 0 ? _width : ix >= _width ? -_width : 0;
00324 real v00 = 0, v01 = 0, v10 = 0, v11 = 0;
00325 real t[nterms];
00326
00327 if (_threadsafe || !(ix == _ix && iy == _iy)) {
00328 if (!_cubic) {
00329 v00 = rawval(ix , iy );
00330 v01 = rawval(ix + 1, iy );
00331 v10 = rawval(ix , iy + 1);
00332 v11 = rawval(ix + 1, iy + 1);
00333 } else {
00334 real v[stencilsize];
00335 int k = 0;
00336 v[k++] = rawval(ix , iy - 1);
00337 v[k++] = rawval(ix + 1, iy - 1);
00338 v[k++] = rawval(ix - 1, iy );
00339 v[k++] = rawval(ix , iy );
00340 v[k++] = rawval(ix + 1, iy );
00341 v[k++] = rawval(ix + 2, iy );
00342 v[k++] = rawval(ix - 1, iy + 1);
00343 v[k++] = rawval(ix , iy + 1);
00344 v[k++] = rawval(ix + 1, iy + 1);
00345 v[k++] = rawval(ix + 2, iy + 1);
00346 v[k++] = rawval(ix , iy + 2);
00347 v[k++] = rawval(ix + 1, iy + 2);
00348
00349 const real* c3x = iy == 0 ? c3n : iy == _height - 2 ? c3s : c3;
00350 real c0x = iy == 0 ? c0n : iy == _height - 2 ? c0s : c0;
00351 for (unsigned i = 0; i < nterms; ++i) {
00352 t[i] = 0;
00353 for (unsigned j = 0; j < stencilsize; ++j)
00354 t[i] += v[j] * c3x[nterms * j + i];
00355 t[i] /= c0x;
00356 }
00357 }
00358 } else {
00359 if (!_cubic) {
00360 v00 = _v00;
00361 v01 = _v01;
00362 v10 = _v10;
00363 v11 = _v11;
00364 } else
00365 copy(_t, _t + nterms, t);
00366 }
00367 if (!_cubic) {
00368 real
00369 a = (1 - fx) * v00 + fx * v01,
00370 b = (1 - fx) * v10 + fx * v11,
00371 c = (1 - fy) * a + fy * b,
00372 h = _offset + _scale * c;
00373 if (gradp) {
00374 real
00375 phi = lat * _degree,
00376 cosphi = cos(phi),
00377 sinphi = sin(phi),
00378 n = 1/sqrt(1 - _e2 * sinphi * sinphi);
00379 gradn = ((1 - fx) * (v00 - v10) + fx * (v01 - v11)) *
00380 _rlatres / (_degree * _a * (1 - _e2) * n * n * n);
00381 grade = (cosphi > _eps ?
00382 ((1 - fy) * (v01 - v00) + fy * (v11 - v10)) / cosphi :
00383 (sinphi > 0 ? v11 - v10 : v01 - v00) *
00384 _rlatres / _degree ) *
00385 _rlonres / (_degree * _a * n);
00386 gradn *= _scale;
00387 grade *= _scale;
00388 }
00389 if (!_threadsafe) {
00390 _ix = ix;
00391 _iy = iy;
00392 _v00 = v00;
00393 _v01 = v01;
00394 _v10 = v10;
00395 _v11 = v11;
00396 }
00397 return h;
00398 } else {
00399 real h = t[0] + fx * (t[1] + fx * (t[3] + fx * t[6])) +
00400 fy * (t[2] + fx * (t[4] + fx * t[7]) +
00401 fy * (t[5] + fx * t[8] + fy * t[9]));
00402 h = _offset + _scale * h;
00403 if (gradp) {
00404
00405 lat = min(lat, 90 - 1/(100 * _rlatres));
00406 lat = max(lat, -90 + 1/(100 * _rlatres));
00407 fy = (90 - lat) * _rlatres;
00408 fy -= int(fy);
00409 real
00410 phi = lat * _degree,
00411 cosphi = cos(phi),
00412 sinphi = sin(phi),
00413 n = 1/sqrt(1 - _e2 * sinphi * sinphi);
00414 gradn = t[2] + fx * (t[4] + fx * t[7]) +
00415 fy * (2 * t[5] + fx * 2 * t[8] + 3 * fy * t[9]);
00416 grade = t[1] + fx * (2 * t[3] + fx * 3 * t[6]) +
00417 fy * (t[4] + fx * 2 * t[7] + fy * t[8]);
00418 gradn *= - _rlatres / (_degree * _a * (1 - _e2) * n * n * n) * _scale;
00419 grade *= _rlonres / (_degree * _a * n * cosphi) * _scale;
00420 }
00421 if (!_threadsafe) {
00422 _ix = ix;
00423 _iy = iy;
00424 copy(t, t + nterms, _t);
00425 }
00426 return h;
00427 }
00428 }
00429
00430 void Geoid::CacheClear() const throw() {
00431 if (!_threadsafe) {
00432 _cache = false;
00433 try {
00434 _data.clear();
00435
00436 vector< vector<unsigned short> >().swap(_data);
00437 }
00438 catch (const exception&) {
00439 }
00440 }
00441 }
00442
00443 void Geoid::CacheArea(real south, real west, real north, real east) const {
00444 if (_threadsafe)
00445 throw GeographicErr("Attempt to change cache of threadsafe Geoid");
00446 if (south > north) {
00447 CacheClear();
00448 return;
00449 }
00450 if (west >= 180)
00451 west -= 360;
00452 if (east >= 180)
00453 east -= 360;
00454 if (east <= west)
00455 east += 360;
00456 int
00457 iw = int(floor(west * _rlonres)),
00458 ie = int(floor(east * _rlonres)),
00459 in = int(floor(-north * _rlatres)) + (_height - 1)/2,
00460 is = int(floor(-south * _rlatres)) + (_height - 1)/2;
00461 in = max(0, min(_height - 2, in));
00462 is = max(0, min(_height - 2, is));
00463 is += 1;
00464 ie += 1;
00465 if (_cubic) {
00466 in -= 1;
00467 is += 1;
00468 iw -= 1;
00469 ie += 1;
00470 }
00471 if (ie - iw >= _width - 1) {
00472
00473 iw = 0;
00474 ie = _width - 1;
00475 } else {
00476 ie += iw < 0 ? _width : iw >= _width ? -_width : 0;
00477 iw += iw < 0 ? _width : iw >= _width ? -_width : 0;
00478 }
00479 int oysize = int(_data.size());
00480 _xsize = ie - iw + 1;
00481 _ysize = is - in + 1;
00482 _xoffset = iw;
00483 _yoffset = in;
00484
00485 try {
00486 _data.resize(_ysize, vector<unsigned short>(_xsize));
00487 for (int iy = min(oysize, _ysize); iy--;)
00488 _data[iy].resize(_xsize);
00489 }
00490 catch (const bad_alloc&) {
00491 CacheClear();
00492 throw GeographicErr("Insufficient memory for caching " + _filename);
00493 }
00494
00495 try {
00496 vector<char> buf(2 * _xsize);
00497 for (int iy = in; iy <= is; ++iy) {
00498 int iy1 = iy, iw1 = iw;
00499 if (iy < 0 || iy >= _height) {
00500 iy1 = iy1 < 0 ? -iy1 : 2 * (_height - 1) - iy1;
00501 iw1 += _width/2;
00502 if (iw1 >= _width)
00503 iw1 -= _width;
00504 }
00505 int xs1 = min(_width - iw1, _xsize);
00506 filepos(iw1, iy1);
00507 _file.read(&(buf[0]), 2 * xs1);
00508 if (xs1 < _xsize) {
00509
00510 filepos(0, iy1);
00511 _file.read(&(buf[2 * xs1]), 2 * (_xsize - xs1));
00512 }
00513 for (int ix = 0; ix < _xsize; ++ix)
00514 _data[iy - in][ix] =
00515 (unsigned short)((unsigned char)buf[2 * ix] * 256u +
00516 (unsigned char)buf[2 * ix + 1]);
00517 }
00518 _cache = true;
00519 }
00520 catch (const exception& e) {
00521 CacheClear();
00522 throw GeographicErr(string("Error filling cache ") + e.what());
00523 }
00524 }
00525
00526 std::string Geoid::DefaultPath() {
00527 return string(GEOID_DEFAULT_PATH);
00528 }
00529
00530 std::string Geoid::GeoidPath() {
00531 string path;
00532 char* geoidpath = getenv("GEOID_PATH");
00533 if (geoidpath)
00534 path = string(geoidpath);
00535 return path;
00536 }
00537
00538 std::string Geoid::DefaultGeoidPath() {
00539 string path;
00540 char* geoidpath = getenv("GEOID_PATH");
00541 if (geoidpath)
00542 path = string(geoidpath);
00543 return path.length() ? path : string(GEOID_DEFAULT_PATH);
00544 }
00545
00546 std::string Geoid::DefaultGeoidName() {
00547 string name;
00548 char* geoidname = getenv("GEOID_NAME");
00549 if (geoidname)
00550 name = string(geoidname);
00551 return name.length() ? name : string(GEOID_DEFAULT_NAME);
00552 }
00553 }