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 }