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