From 57868f38ef0cbbe342fabe5e5ab5e8d1a38f95a1 Mon Sep 17 00:00:00 2001
From: DennisOSRM <luxen@kit.edu>
Date: Tue, 13 Dec 2011 10:13:48 +0100
Subject: [PATCH] First working version of SRTM lookup

---
 Util/SRTMImport/NASAGridSquare.cpp | 218 +++++++++++++++++++++++++++++
 Util/SRTMImport/NASAGridSquare.h   |  92 ++++++++++++
 Util/SRTMImport/SRTMLookup.h       | 104 ++++++++++++++
 Util/SRTMImport/utils.cpp          | 137 ++++++++++++++++++
 Util/SRTMImport/utils.h            |  83 +++++++++++
 5 files changed, 634 insertions(+)
 create mode 100644 Util/SRTMImport/NASAGridSquare.cpp
 create mode 100644 Util/SRTMImport/NASAGridSquare.h
 create mode 100644 Util/SRTMImport/SRTMLookup.h
 create mode 100644 Util/SRTMImport/utils.cpp
 create mode 100644 Util/SRTMImport/utils.h

diff --git a/Util/SRTMImport/NASAGridSquare.cpp b/Util/SRTMImport/NASAGridSquare.cpp
new file mode 100644
index 000000000..0149e1eb0
--- /dev/null
+++ b/Util/SRTMImport/NASAGridSquare.cpp
@@ -0,0 +1,218 @@
+/*
+Copyright (c) 2006  Alex Tingle
+
+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 License, or (at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include "NASAGridSquare.h"
+
+NasaGridSquare::~NasaGridSquare() {
+    if(data)
+        unload();
+}
+
+std::string NasaGridSquare::make_filename(const char* ext) const
+{
+    using namespace std;
+    char EW =(longitude>=0?'E':'W');
+    char NS =(latitude>=0?'N':'S');
+    stringstream ss;
+#ifdef ROOT_PATH
+    static const char* root_path =ROOT_PATH;
+#else
+    static const char* root_path =FloodUtils::getenvz("ROOT_PATH",".");
+#endif
+    ss<<setfill('0')
+                    << root_path
+                    << "/"
+                    <<NS
+                    <<setw(2)<<abs(latitude)<<setw(0)
+                    <<EW
+                    <<setw(3)<<abs(longitude)<<setw(0)
+                    <<'.'<<ext;
+    return ss.str();
+}
+
+void NasaGridSquare::parse_filename(const char* filename, int& lng, int& lat)
+{
+    char fn[12]; // N51E001.xyz
+    FloodUtils::strlcpy(fn,filename,sizeof(fn));
+    lng=( fn[3]=='E'? 1: -1 );
+    lat=( fn[0]=='N'? 1: -1 );
+    fn[3]=fn[7]='\0';
+    lng *= ::atoi(fn+4);
+    lat *= ::atoi(fn+1);
+}
+
+void NasaGridSquare::load(const char* filename) {
+    const std::string gzipfile =std::string(filename)+".zip";
+    bool triedZipped =false;
+    std::cout << "testing for zipped file " << gzipfile << std::endl;
+    if(FloodUtils::exists(gzipfile.c_str())) {
+        std::cout << gzipfile << " exists" << std::endl;
+    }
+    int errorcode = 0;
+    int errorcode2 = 0;
+    zip * test = zip_open(gzipfile.c_str(), 0, &errorcode);
+    if(errorcode) {
+        char buffer[1024];
+        zip_error_to_str(buffer, 1024, errorcode, errno);
+        ERR("Error occured opening zipfile " << filename << ", error: " << buffer);
+    } else {
+        unsigned entries = zip_get_num_files(test);
+        INFO("File has " << entries << " entries");
+        //for(unsigned i = 0; i < entries; ++i) {
+        struct zip_stat stat;
+        //ZIP_EXTERN int zip_stat_index(struct zip *, int, int, struct zip_stat *);
+        zip_stat_index(test, 0, 0, &stat);
+        INFO("included: " << stat.name);
+        INFO("uncompressed: " << stat.size);
+        INFO("compressed: " << stat.comp_size);
+        //        }
+        DELETE(data2);
+        data2 = new char[stat.size];
+
+        zip_file * file = zip_fopen_index(test, 0, 0 );
+        INFO("opened");
+        zip_file_error_get(file, &errorcode, &errorcode2);
+        INFO("opened: " << errorcode << ", " << errorcode2);
+        zip_fread(file, data2, stat.size);
+
+        INFO("read");
+        zip_fclose(file);
+        INFO("closed file");
+        unsigned short datum=((unsigned short*)data2)[493093];
+        short result=(datum>>8 | datum<<8);
+        INFO("493093: " << result);
+    }
+
+
+
+    zip_close(test);
+
+    while(true) {
+        fd=::open(filename,O_RDONLY);
+        if(fd>=0)
+            break;
+        DEBUG_ONLY(int saved_errno =errno;)
+        if(ENOENT!=errno || triedZipped || !FloodUtils::exists(gzipfile.c_str())) {
+            DB("Failed to open file "<<filename<<": "<<::strerror(saved_errno))
+                      return;
+        }
+        // Unzip and try again
+        DB("Unzipping "<<gzipfile)
+        std::cout << "unzipping" << std::endl;
+        std::stringstream ss;
+        ss<<"/bin/gzip -dc "<<gzipfile<<" > "<<filename;
+        int status =::system(ss.str().c_str());
+        if(-1 == status)
+        {
+            ERR("Failed to execute shell command ("<<ss.str()<<"): "<<::strerror(errno));
+            exit(-1);
+        }
+        else if(0 != WEXITSTATUS(status))
+        {
+            ERR("Failed to unzip file "<<gzipfile<<": "<<WEXITSTATUS(status));
+            exit(-1);
+        }
+        triedZipped=true;
+    }
+
+    // Get file length
+    struct stat file_stat;
+    if( ::fstat(fd,&file_stat)<0 )
+    {
+        ::perror("fstat error");
+        ::exit(-1);
+    }
+    num_bytes=file_stat.st_size;
+    if(!num_bytes)
+    {
+        ::close(fd);
+        return;
+    }
+
+    data =::mmap(0,num_bytes,PROT_READ,MAP_SHARED,fd,0);
+    if(MAP_FAILED==data)
+    {
+        ::perror("mmap error");
+        ::exit(-1);
+    }
+}
+
+void NasaGridSquare::unload()
+{
+    if(::munmap(data,num_bytes)<0)
+    {
+        ::perror("munmap error");
+        ::exit(-1);
+    }
+    ::close(fd);
+
+}
+
+bool NasaGridSquare::lngLat_to_colRow(
+        float lng_fraction, float lat_fraction,
+        unsigned int& col,  unsigned int&  row
+) const
+{
+    col =             int( 0.5 + lng_fraction * float(num_cols-1) );
+    row =num_rows-1 - int( 0.5 + lat_fraction * float(num_rows-1) );
+    return( col<num_cols && row<num_rows );
+}
+
+NasaGridSquare::NasaGridSquare(const char* filename):
+                            data(NULL), data2(NULL), longitude(0), latitude(0),
+                            fd(-1), num_bytes(0), num_rows(1), num_cols(1)
+{
+    parse_filename(filename,longitude,latitude);
+    load(filename);
+    if(has_data())
+        num_rows=num_cols=std::sqrt(num_bytes/2);
+}
+
+NasaGridSquare::NasaGridSquare(int lng, int lat) :
+                    data(NULL), data2(NULL), longitude(lng), latitude(lat),
+                    fd(-1), num_bytes(0), num_rows(1), num_cols(1)
+{
+    std::string filename =make_filename("hgt");
+    std::cout << "Loading " << filename << std::endl;
+    load(filename.c_str());
+    if(has_data())
+        num_rows=num_cols=std::sqrt(num_bytes/2);
+}
+
+short NasaGridSquare::height(float lng_fraction, float lat_fraction) const {
+    if(!data)
+        return NO_DATA;
+    unsigned int col,row;
+    if(!lngLat_to_colRow(lng_fraction,lat_fraction, col,row))
+        return NO_DATA;
+    return getHeight(col,row);
+}
+
+short NasaGridSquare::getHeight(unsigned int col, unsigned int row) const {
+    if(col>=num_cols || row>=num_rows)
+        return NO_DATA;
+    unsigned int i=( col + row * num_cols );
+//    unsigned short datum=((unsigned short*)data)[i];
+//    short result=(datum>>8 | datum<<8);
+    unsigned short datum2=((unsigned short*)data2)[i];
+    short result2=(datum2>>8 | datum2<<8);
+
+//    INFO("[" << i << "] result: " << result << ", result2: " << (int)result2);
+    return result2;
+}
+
diff --git a/Util/SRTMImport/NASAGridSquare.h b/Util/SRTMImport/NASAGridSquare.h
new file mode 100644
index 000000000..3fff42b52
--- /dev/null
+++ b/Util/SRTMImport/NASAGridSquare.h
@@ -0,0 +1,92 @@
+/*
+Copyright (c) 2006  Alex Tingle
+
+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 License, or (at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#ifndef NASA_GRID_SQUARE_H
+#define NASA_GRID_SQUARE_H
+
+#include <string>
+#include <zip.h>
+
+#include "utils.h"
+
+#include <cmath>
+#include <iostream>
+#include <string>
+#include "utils.h"
+
+#include <errno.h>
+#include <fcntl.h>
+#include <iomanip>
+#include <sstream>
+#include <cstdio>
+#include <cstdlib>
+#include <cstring>
+#include <sys/mman.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+#include "../../typedefs.h"
+
+class NasaGridSquare {
+public:
+    NasaGridSquare(const char* filename);
+    NasaGridSquare(int lng, int lat);
+    ~NasaGridSquare();
+    short height(float lng_fraction, float lat_fraction) const;
+    const static short NO_DATA =-32768; ///< NASA's magic number for no data.
+
+    /** Formula for grid squares' unique keys. */
+    static int key(int lng, int lat) {
+        return 1000*lat + lng;
+    }
+
+    /** Calculate unique key for this object. */
+    int key() const {return key(longitude,latitude);}
+    unsigned int cols() const {return num_cols;}
+    unsigned int rows() const {return num_rows;}
+    bool has_data() const {return bool(data);}
+
+private:
+    void* data; ///< mmap'd data file.
+    char * data2;
+    int longitude;
+    int latitude;
+    int fd; ///< File ID for mmap'd data file.
+    unsigned int num_bytes; ///< Length of data.
+    unsigned int num_rows;
+    unsigned int num_cols;
+
+    std::string make_filename(const char* ext) const;
+    void parse_filename(const char* filename, int& lng, int& lat);
+
+    /** Loads the file into memory, pointed to by 'data'. Sets num_bytes. */
+    void load(const char* filename);
+    void unload();
+    /** Converts lng,lat floats to col,row ints. Returns TRUE if result valid. */
+    bool lngLat_to_colRow(
+            float lng_fraction, float lat_fraction,
+            unsigned int& col,  unsigned int& row
+    ) const;
+    short getHeight(unsigned int col, unsigned int row) const;
+
+    friend class ReducedGridSquare;
+
+}; // end class NasaGridSquare
+
+#endif // NASA_GRID_SQUARE_H
diff --git a/Util/SRTMImport/SRTMLookup.h b/Util/SRTMImport/SRTMLookup.h
new file mode 100644
index 000000000..0258fc8e3
--- /dev/null
+++ b/Util/SRTMImport/SRTMLookup.h
@@ -0,0 +1,104 @@
+/*
+Copyright (c) 2006  Alex Tingle
+
+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 License, or (at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+*/
+
+#ifndef GRID_H
+#define GRID_H
+
+#include "NASAGridSquare.h"
+
+#include <boost/unordered_map.hpp>
+#include <assert.h>
+
+/** Interface to the GridSquares. Template parameter determines which kind of
+ *  raw data is used (NASA, or reduced). GridSquare data is globally cached, so
+ *  that many queries can be answered without having to constantly mmap & unmmap
+ *  the data files.
+ *
+ *  Typical usage:
+ *
+ *  Grid<NasaGridSquare> g();
+ *  cout << g.height(lng,lat) << endl;
+ *
+ */
+template<class T>
+class Grid {
+  static          boost::unordered_map<int,T*>            cache;
+  static typename boost::unordered_map<int,T*>::size_type cache_size;
+
+public:
+  /** The cache_limit defaults to 360. Adjust it to set the number of 
+   *  grid squares to store in memory. */
+  static typename boost::unordered_map<int,T*>::size_type cache_limit;
+
+  Grid(short sea_level_rise_ = 0 ) : sea_level_rise(sea_level_rise_) {
+    // Double check that this compiler truncates towards zero.
+    assert(-1 == int(float(-1.9)));
+  }
+
+  /** Returns the height above sea level for the given lat/lng. */
+  short height(float longitude, float latitude) const  {
+    int lng,lat;
+    float lng_fraction,lat_fraction;
+    split(longitude,lng,lng_fraction);
+    split(latitude ,lat,lat_fraction);
+    
+    int k =T::key(lng,lat);
+    typename boost::unordered_map<int,T*>::iterator pos =cache.find(k);
+    if(pos==cache.end())
+    {
+      if(cache_size>=cache_limit)
+          clear_cache();
+      pos = cache.insert(std::make_pair(k,new T(lng,lat))).first;
+      if(pos->second->has_data())
+          ++cache_size;
+    }
+    return pos->second->height(lng_fraction,lat_fraction);
+  }
+
+  /** Returns TRUE if the given lng/lat is underwater. */
+  bool underwater(float longitude, float latitude) const
+  {
+    short h =height(longitude,latitude);
+    return( h!=NasaGridSquare::NO_DATA && h<=sea_level_rise );
+  }
+  
+private:
+
+  /** Clears the cache of non-null GridSquares. */
+  void clear_cache() const {
+    cache.clear();
+  }
+
+  /** Split a floating point number (num) into integer (i) & fraction (f)
+   *  components. */
+  void split(float num, int& i, float& f) const {
+    if(num>=0.0)
+      i=int(num);
+    else
+      i=int(num)-1;
+    f=num-float(i);
+  }
+
+  short sea_level_rise;
+};
+
+template<class T> boost::unordered_map<int,T*>                     Grid<T>::cache;
+template<class T> typename boost::unordered_map<int,T*>::size_type Grid<T>::cache_size =0;
+template<class T> typename boost::unordered_map<int,T*>::size_type Grid<T>::cache_limit=360;
+
+#endif // GRID_H
diff --git a/Util/SRTMImport/utils.cpp b/Util/SRTMImport/utils.cpp
new file mode 100644
index 000000000..b3803da0d
--- /dev/null
+++ b/Util/SRTMImport/utils.cpp
@@ -0,0 +1,137 @@
+/*
+Copyright (c) 2006  Alex Tingle
+
+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 License, or (at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+*/
+
+#include "utils.h"
+
+#include <fstream>
+#include <iostream>
+#include <cmath>
+#include <stdlib.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+namespace FloodUtils {
+
+unsigned short sqrt(unsigned long a) {
+  return std::sqrt(a);
+}
+
+
+std::list<std::string> split(const std::string& s, char token) {
+  std::list<std::string> result;
+  std::string::size_type pos=0;
+  while(true)
+  {
+    pos=s.find_first_not_of(token,pos);
+    if(pos==std::string::npos) break;
+    std::string::size_type token_pos=s.find_first_of(token,pos);
+    result.push_back( s.substr(pos,token_pos-pos) );
+    pos=token_pos;
+  }
+  return result;
+}
+
+
+void googleXY_to_latlng(
+  double x, double y, int zoom,
+  double& lng, double& lat
+)
+{
+  googleX_to_lng(zoom,x,lng);
+  googleY_to_lat(zoom,y,lat);
+}
+void googleX_to_lng(int zoom, double x, double& lng)
+{
+  long tilesAtThisZoom =googleMaxTiles(zoom);
+  lng = -180.0 + (x * 360.0 / double(tilesAtThisZoom));
+}
+void googleY_to_lat(int zoom, double y, double& lat)
+{
+  // This function is the inverse Mercator projection.
+  // Look up 'Mercator projection' on Wikipedia for details.
+  long   pixels             = googleMaxTiles(zoom) * 256L;
+  double pixelsPerLonRadian = pixels / (2.0 * M_PI);
+  double bitmapOrigo        = pixels / 2;
+
+  double Y = (bitmapOrigo - y*256) / pixelsPerLonRadian;
+
+  double latRad = 2.0 * ::atan( ::exp(Y) ) - M_PI_2;
+  lat = latRad * 180.0 / M_PI;
+}
+
+
+/*
+ * Implementation copyright (C) 2004, Alex Tingle.
+ */
+
+size_t strlcpy(char* dst, const char* src, size_t bufsize)
+{
+  size_t srclen =strlen(src);
+  size_t result =srclen; /* Result is always the length of the src string */
+  if(bufsize>0)
+  {
+    if(srclen>=bufsize)
+       srclen=bufsize-1;
+    if(srclen>0)
+       memcpy(dst,src,srclen);
+    dst[srclen]='\0';
+  }
+  return result;
+}
+
+size_t strlcat(char* dst, const char* src, size_t bufsize)
+{
+  size_t dstlen =strlen(dst);
+  size_t srclen =strlen(src);
+  size_t result =dstlen+srclen;
+  /* truncate srclen to the buffer */
+  if(result>=bufsize)
+     srclen=bufsize-dstlen-1;
+  if(srclen>0) /* if there is anything left to copy. */
+  {
+    dst+=dstlen;
+    memcpy(dst,src,srclen);
+    dst[srclen]='\0';
+  }
+  return result;
+}
+
+
+int getpid()
+{
+  return int(::getpid());
+}
+
+
+const char* getenvz(const char* name, const char* dflt)
+{
+  const char* result =::getenv(name);
+  if(!result)
+      result=dflt;
+  return result;
+}
+
+
+bool exists(const char* path) {
+      std::ifstream ifile(path);
+      return ifile;
+}
+
+
+} // end namespace FloodUtils
diff --git a/Util/SRTMImport/utils.h b/Util/SRTMImport/utils.h
new file mode 100644
index 000000000..8119a4540
--- /dev/null
+++ b/Util/SRTMImport/utils.h
@@ -0,0 +1,83 @@
+/*
+Copyright (c) 2006  Alex Tingle
+
+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 License, or (at your option) any later version.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program; if not, write to the Free Software
+Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+*/
+
+#ifndef FLOOD_UTILS_H
+#define FLOOD_UTILS_H
+
+#include <string.h>
+#include <sstream>
+#include <stdio.h>
+#include <string>
+#include <list>
+
+/*
+#define ERR(x) { \
+  std::stringstream s; \
+  s<<FloodUtils::getpid()<<": "<<x<<"\n"; \
+  fprintf(stderr,s.str().c_str()); \
+}
+*/
+
+#define ROOT_PATH "/opt/storage/srtm/Eurasia"
+
+#if DEBUG_OUTPUT
+#  define DEBUG_ONLY(x) x
+#  define DB(x) ERR("[DEBUG] "<<x)
+#else
+#  define DEBUG_ONLY(x)
+#  define DB(x)
+#endif
+
+namespace FloodUtils {
+
+  /** Integer square root.
+   *  Algorithm by Jack W. Crenshaw: http://www.embedded.com/98/9802fe2.htm
+   */
+  unsigned short sqrt(unsigned long a);
+
+  /** Tokenise a string. */
+  std::list<std::string> split(const std::string& s, char token=',');
+
+  void googleXY_to_latlng(
+    double x, double y, int zoom,
+    double& lng, double& lat
+  );
+  void googleX_to_lng(int zoom, double x, double& lng);
+  void googleY_to_lat(int zoom, double y, double& lat);
+  
+  /** Calculate the number of tiles in x or y dimensions at a zoom level. */
+  inline long googleMaxTiles(int zoom) { return( 1L << (17 - zoom) ); }
+
+  /* Specs. & semantics from:
+   * http://www.courtesan.com/todd/papers/strlcpy.html
+   */
+  size_t strlcpy(char* dst, const char* src, size_t bufsize);
+  size_t strlcat(char* dst, const char* src, size_t bufsize);
+  
+  /** Convenience function for debug output */
+  int getpid();
+  
+  /** Convenience function for getting env vars as strings. */
+  const char* getenvz(const char* name, const char* dflt ="");
+  
+  /** Check for the existence of the names file. */
+  bool exists(const char* path);
+}
+
+
+#endif // FLOOD_UTILS_H