Initial commit

.gitignore

+*.o
+*.swp
+src/elevation_create_database
+src/elevation_query_database
+src/route_create_databases
+src/route_query_lookup_database
+src/route_query_nodes_database
+src/route

src/Makefile

+CC=g++
+#CXXFLAGS=-g -Wall -std=c++11
+CXXFLAGS=-O2 -std=c++11
+#LDFLAGS=-g -Wall
+LDFLAGS=-O2 -fwhole-program -flto
+LDLIBS_LMDB=-llmdb
+LDLIBS_PQXX=-lpqxx
+
+.PHONY: clean
+
+all: elevation_create_database elevation_query_database route_create_databases route_query_nodes_database route_query_lookup_database route
+
+elevation_create_database: elevation.o elevation_create_database.o
+	$(CC) $(LDFLAGS) -o $@ $+ $(LDLIBS_LMDB)
+
+elevation_query_database: elevation.o elevation_query_database.o
+	$(CC) $(LDFLAGS) -o $@ $+ $(LDLIBS_LMDB)
+
+route_create_databases: elevation.o conv_functions.o route_create_databases.o nodes_db.o lookup_db.o
+	$(CC) $(LDFLAGS) -o $@ $+ $(LDLIBS_LMDB) $(LDLIBS_PQXX)
+
+route_query_nodes_database: route_query_nodes_database.o nodes_db.o conv_functions.o
+	$(CC) $(LDFLAGS) -o $@ $+ $(LDLIBS_LMDB)
+
+route_query_lookup_database: route_query_lookup_database.o lookup.o functions.o nodes_db.o lookup_db.o conv_functions.o
+	$(CC) $(LDFLAGS) -o $@ $+ $(LDLIBS_LMDB)
+
+route: route.o route_db.o lookup.o functions.o nodes_db.o lookup_db.o conv_functions.o get_route.o
+	$(CC) $(LDFLAGS) -o $@ $+ $(LDLIBS_LMDB)
+
+elevation.o: elevation.cc elevation.h conf.h
+	$(CC) $(CXXFLAGS) -c $< -o $@
+
+elevation_query_database.o: elevation_query_database.cc conf.h elevation.h
+	$(CC) $(CXXFLAGS) -c $< -o $@
+
+elevation_create_database.o: elevation_create_database.cc conf.h elevation.h
+	$(CC) $(CXXFLAGS) -c $< -o $@
+
+conv_functions.o: conv_functions.cc conf.h elevation.h types.h conv_functions.h
+	$(CC) $(CXXFLAGS) -c $< -o $@
+
+route_create_databases.o: route_create_databases.cc lookup_db.h nodes_db.h elevation.h types.h conf.h
+	$(CC) $(CXXFLAGS) -c $< -o $@
+
+nodes_db.o: nodes_db.cc elevation.h types.h conv_functions.h conf.h nodes_db.h
+	$(CC) $(CXXFLAGS) -c $< -o $@
+
+lookup_db.o: lookup_db.cc lookup_db.h conf.h types.h
+	$(CC) $(CXXFLAGS) -c $< -o $@
+
+route_query_nodes_database.o: route_query_nodes_database.cc nodes_db.h conf.h
+	$(CC) $(CXXFLAGS) -c $< -o $@
+
+functions.o: functions.cc functions.h
+	$(CC) $(CXXFLAGS) -c $< -o $@
+
+lookup.o: lookup.cc lookup.h lookup_db.h types.h conf.h nodes_db.h
+	$(CC) $(CXXFLAGS) -c $< -o $@
+
+route_query_lookup_database.o: route_query_lookup_database.cc lookup.h conf.h
+	$(CC) $(CXXFLAGS) -c $< -o $@
+
+route_db.o: route_db.cc route_db.h conf.h lookup.h lookup_db.h nodes_db.h types.h
+	$(CC) $(CXXFLAGS) -c $< -o $@
+
+get_route.o: get_route.cc get_route.h
+	$(CC) $(CXXFLAGS) -c $< -o $@
+
+route.o: route.cc
+	$(CC) $(CXXFLAGS) -c $< -o $@
+
+clean:
+	rm -f *.o

src/conf.h

+#ifndef H_CONF
+#define H_CONF 1
+
+#define RV_MAXIMUM_LMDB_SIZE 21474836480L // 20 GiB
+#define RV_ZCHAR_LENGTH 2048 // I love big numbers
+#define RV_TEMP_TABLES_PREFIX "rv_temp_"
+#define RV_NODES_DB_RELATIVE_PATH "nodes"
+#define RV_LOOKUP_DB_RELATIVE_PATH "lookup"
+#define RV_LOOKUP_PREC 100
+
+#define RV_ERROR_NO_NODE_FOUND 10
+
+#define RV_BLANKLINE fprintf(stderr,"\r%79s\r","")
+
+#define RV_PI 3.141592653589793
+#define RV_EARTH_RADIUS 6371000.0 // m
+#define RV_G 9.81 // m.s^-2
+#define RV_RHO_AIR 1.204 // kg.m^-3
+
+
+#define RV_ELEVATION_SQ_LEN 300 // modulo(1200,RV_ELEVATION_SQ_LEN) must be 0
+#define RV_POSTGRES_PIPELINE_RETAIN 0
+
+#define RV_MAX_PIPELINE 128
+
+#define RV_WIDTH_CYCLEWAY 1.5
+#define RV_WIDTH_OTHERS 2.5
+
+#define RV_MAXIMUM_USED_ANGLE 3 // rad
+#define RV_MAXIMUM_RADIUS_OF_CURVATURE 500 // m
+
+
+#endif

src/conv_functions.cc

+#include "conv_functions.h"
+
+void build_MDB_val_from_node_info(
+        MDB_val* mdb_value,
+        double & lon, 
+        double & lat, 
+        elevation_value & elevation, 
+        std::list<neighbor_t> & neighbors
+        )
+{
+    // Return a pointer (don't forget to free it with free()) to a
+    // build_node_info with allocation of the two internal structure in the same
+    // allocation. Design to be used to build a MDB_val
+    char* pc;
+    node_info_t ni;
+    size_t len;
+    len =   sizeof(node_info_fixed_t)
+            +neighbors.size()*sizeof(neightbor_t);
+    pc = (char*)malloc(len);
+    if(!pc)
+    {
+        fprintf(stderr,"Memory allocation failed\n");
+        abort();
+    }
+    
+    ni.fixed = (node_info_fixed_t*)pc;
+    
+    if(neighbors.size()>0)
+        ni.neighbors = (neighbor_t*)(pc+sizeof(node_info_fixed_t));
+    else
+        ni.neighbors = (neighbor_t*)NULL;
+
+    ni.fixed->lon = lon;
+    ni.fixed->lat = lat;
+    ni.fixed->elevation = elevation;
+    ni.fixed->neighbors_number = neighbors.size();
+
+    unsigned int i=0;
+    for(
+            std::list<neighbor_t>::iterator neighbors_it = neighbors.begin();
+            neighbors_it != neighbors.end();
+            neighbors_it++
+       )
+    {
+        ni.neighbors[i].from = (*neighbors_it).from;
+        ni.neighbors[i].to = (*neighbors_it).to;
+        ni.neighbors[i].way_kind = (*neighbors_it).way_kind;
+        i++;
+    }
+
+    mdb_value->mv_size = len;
+    mdb_value->mv_data = (void*)pc;
+}
+
+void free_MDB_val_from_node_info(
+        MDB_val* mdb_value
+        )
+{
+    free(mdb_value->mv_data);
+    mdb_value->mv_data = (void*)NULL;
+    mdb_value->mv_size = 0;
+}
+
+void build_node_info_from_MDB_val(
+        node_info_t* pni,
+        MDB_val* mdb_value
+        )
+{
+    if(mdb_value->mv_size<sizeof(node_info_fixed_t))
+    {
+        fprintf(stderr,"Error, database node info to short\n");
+        abort();
+    }
+    pni->fixed = (node_info_fixed_t*)(mdb_value->mv_data);
+    if(mdb_value->mv_size!=sizeof(node_info_fixed_t)+(pni->fixed->neighbors_number)*sizeof(neighbor_t))
+    {
+        fprintf(stderr,"Error, database node info size mismatch\n");
+        abort();
+    }
+    if(pni->fixed->neighbors_number>0)
+        pni->neighbors = (neighbor_t*)(((char*)mdb_value->mv_data)+sizeof(node_info_fixed_t));
+    else
+        pni->neighbors = (neighbor_t*)NULL;
+}
+
+
+
+
+
+
+
+    
+

src/conv_functions.h

+#ifndef H_CONV_FUNCTIONS
+#define H_CONV_FUNCTIONS
+
+#include "types.h"
+#include <list>
+
+void build_MDB_val_from_node_info(
+        MDB_val* mdb_value,
+        double & lon, 
+        double & lat, 
+        elevation_value & elevation, 
+        std::list<neighbor_t> & neighbors
+        );
+
+void free_MDB_val_from_node_info(
+        MDB_val* mdb_value
+        );
+
+void build_node_info_from_MDB_val(
+        node_info_t* pni,
+        MDB_val* mdb_value
+        );
+
+#endif
+
+
+    
+

src/elevation.cc

+#include "elevation.h"
+
+elevation::elevation(const char* db_location, bool rwi)
+{
+    int rc;
+    rw = rwi;
+
+    rc = mdb_env_create(&env);
+    if(rc)
+    {
+        fprintf(stderr, "mdb_env_create: (%d) %s\n", rc, mdb_strerror(rc));
+        abort();
+    }
+
+    rc = mdb_env_set_mapsize(env, RV_MAXIMUM_LMDB_SIZE);
+    if(rc)
+    {
+        fprintf(stderr, "mdb_env_set_mapsize: (%d) %s\n", rc, mdb_strerror(rc));
+        abort();
+    }
+
+    rc = mdb_env_open(env, db_location, rw?MDB_WRITEMAP:0, 0644);
+    if(rc)
+    {
+        fprintf(stderr, "mdb_env_open: (%d) %s\n", rc, mdb_strerror(rc));
+        abort();
+    }
+
+    if(rw)
+        rc = mdb_txn_begin(env, NULL, 0, &txn);
+    else
+        rc = mdb_txn_begin(env, NULL, MDB_RDONLY, &txn);
+    if(rc)
+    {
+        fprintf(stderr, "mdb_txn_begin: (%d) %s\n", rc, mdb_strerror(rc));
+        abort();
+    }
+
+    rc = mdb_dbi_open(txn, NULL, 0, &dbi);
+    if(rc)
+    {
+        fprintf(stderr, "mdb_dbi_open: (%d) %s\n", rc, mdb_strerror(rc));
+        abort();
+    }
+}
+
+elevation::~elevation()
+{
+    mdb_txn_abort(txn);
+    mdb_close(env, dbi);
+    mdb_env_close(env);
+}
+
+void elevation::commit()
+{
+    if(rw)
+    {
+        int rc;
+
+        rc = mdb_txn_commit(txn);
+        if(rc)
+        {
+            fprintf(stderr, "mdb_txn_commit: (%d) %s\n", rc, mdb_strerror(rc));
+            abort();
+        }
+
+        rc = mdb_txn_begin(env, NULL, 0, &txn);
+        if(rc)
+        {
+            fprintf(stderr, "mdb_txn_begin: (%d) %s\n", rc, mdb_strerror(rc));
+            abort();
+        }
+    }
+}
+
+elevation_value elevation::get_elevation(double & lon, double & lat)
+{
+    double lon_s = lon * 3600;
+    double lat_s = lat * 3600;
+
+    int lon_s_1 = ((int)((lon_s + 1296000) / 3))*3 - 1296000;
+    int lat_s_1 = ((int)((lat_s + 1296000) / 3))*3 - 1296000;
+
+    int lon_s_2 = lon_s_1 + 3;
+    int lat_s_2 = lat_s_1 + 3;
+
+    double xlon = (lon_s - lon_s_1) / 3;
+    double xlat = (lat_s - lat_s_1) / 3;
+
+    elevation_value *pele_lon1_lat1, *pele_lon1_lat2, *pele_lon2_lat1, *pele_lon2_lat2;
+    pele_lon1_lat1=grid_point(lon_s_1,lat_s_1);
+    if(!pele_lon1_lat1)
+    {
+        fprintf(stderr,"Grid point not found, lon_s = %i, lat_s = %i\n",lon_s_1,lat_s_1);
+        abort();
+    }
+    pele_lon1_lat2=grid_point(lon_s_1,lat_s_2);
+    if(!pele_lon1_lat2)
+    {
+        fprintf(stderr,"Grid point not found, lon_s = %i, lat_s = %i\n",lon_s_1,lat_s_2);
+        abort();
+    }
+    pele_lon2_lat1=grid_point(lon_s_2,lat_s_1);
+    if(!pele_lon2_lat1)
+    {
+        fprintf(stderr,"Grid point not found, lon_s = %i, lat_s = %i\n",lon_s_2,lat_s_1);
+        abort();
+    }
+    pele_lon2_lat2=grid_point(lon_s_2,lat_s_2);
+    if(!pele_lon2_lat2)
+    {
+        fprintf(stderr,"Grid point not found, lon_s = %i, lat_s = %i\n",lon_s_2,lat_s_2);
+        abort();
+    }
+
+    elevation_value ele_lon1_latX, ele_lon2_latX;
+    ele_lon1_latX = (1-xlat) * (*pele_lon1_lat1) + xlat * (*pele_lon1_lat2);
+    ele_lon2_latX = (1-xlat) * (*pele_lon2_lat1) + xlat * (*pele_lon2_lat2);
+
+    elevation_value ele_lonX_latX;
+    ele_lonX_latX = (1-xlon)*ele_lon1_latX + xlon*ele_lon2_latX;
+
+    return(ele_lonX_latX);
+}
+
+elevation_value* elevation::grid_point(int32_t & lon_s, int32_t & lat_s, bool ptr_even_missing)
+{
+    // with lon_s,lat_s we determine the isblock
+    int32_t lon_s_begin = ((lon_s + 3600*360)/(3*RV_ELEVATION_SQ_LEN))*RV_ELEVATION_SQ_LEN*3 - 3600*360;
+    int32_t lat_s_begin = ((lat_s + 3600*360)/(3*RV_ELEVATION_SQ_LEN))*RV_ELEVATION_SQ_LEN*3 - 3600*360;
+
+    unsigned int pos = (lon_s-lon_s_begin)/3*RV_ELEVATION_SQ_LEN+(lat_s-lat_s_begin)/3;
+
+    MDB_val mdb_key, mdb_data;
+    elevation_key key;
+    
+    key.lon_s = lon_s_begin;
+    key.lat_s = lat_s_begin;
+    mdb_key.mv_size = sizeof(elevation_key);
+    mdb_key.mv_data = &key;
+
+    int rc;
+    rc = mdb_get(txn,dbi,&mdb_key,&mdb_data);
+    if(rc)
+    {
+        if(rc == MDB_NOTFOUND)
+        {
+            if(rw)
+            {
+                elevation_value* ptr = (elevation_value*)malloc(RV_ELEVATION_SQ_LEN*RV_ELEVATION_SQ_LEN*sizeof(elevation_value));
+                for(unsigned int i=0;i<RV_ELEVATION_SQ_LEN*RV_ELEVATION_SQ_LEN;i++)
+                {
+                    ptr[i]=-32768;
+                }
+
+                mdb_data.mv_size=RV_ELEVATION_SQ_LEN*RV_ELEVATION_SQ_LEN*sizeof(elevation_value);
+                mdb_data.mv_data=ptr;
+                rc = mdb_put(txn, dbi, &mdb_key, &mdb_data, 0);
+                if(rc)
+                {
+                    fprintf(stderr, "mdb_put: (%d) %s\n", rc, mdb_strerror(rc));
+                    abort();
+                }
+
+                free(ptr);
+
+                return(grid_point(lon_s,lat_s,ptr_even_missing));
+            }
+            else
+                return NULL;
+        }
+        fprintf(stderr, "mdb_get: (%d) %s\n", rc, mdb_strerror(rc));
+        abort();
+    }
+
+    elevation_value* res =  ((elevation_value*)(mdb_data.mv_data)) + pos;
+    if(ptr_even_missing)
+        return(res);
+    if(*res==-32768)
+        return(NULL);
+    return(res);
+    
+}
+

src/elevation.h

+#ifndef H_ELEVATION
+#define H_ELEVATION 1
+
+#include <string>
+#include "conf.h"
+#include <lmdb.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <assert.h>
+
+struct elevation_key
+{
+    int32_t lon_s;
+    int32_t lat_s;
+};
+
+typedef struct elevation_key elevation_key;
+typedef float elevation_value;
+
+class elevation
+{
+    public:
+        bool rw;
+        MDB_env* env;
+        MDB_dbi dbi;
+        MDB_txn* txn;
+    elevation(const char* db_location, bool rwi = false);
+    ~elevation();
+    void commit();
+    elevation_value get_elevation(double & lon, double & lat);
+    elevation_value* grid_point(int32_t & lon_s, int32_t & lat_s, bool ptr_even_missing = false);
+};
+
+#endif
+

src/elevation_create_database.cc

+#include <list>
+#include <libgen.h>
+#include <string>
+#include <stdexcept>
+#include <fstream>
+
+#include "elevation.h"
+
+
+int main(int argc, char** argv)
+{
+    if(argc<3)
+    {
+        fprintf(stderr,"Usage: %s db_path srtm_files...\n",argv[0]);
+        abort();
+    }
+    
+    elevation db(argv[1],true);
+    
+    std::list< std::pair<int32_t,int32_t> > missing_data;
+
+    for(int i=2; i<argc; i++)
+    {
+        std::string filename = basename(argv[i]);
+        if(
+                filename.size() == 11
+                && filename.substr(7) == std::string(".hgt")
+                && (filename[0] == 'N' || filename[0] == 'S')
+                && (filename[3] == 'E' || filename[3] == 'W')
+          )
+        {
+            int lat,lon;
+            try
+            {
+                lat = std::stoi(filename.substr(1,2));
+                lon = std::stoi(filename.substr(4,3));
+            }
+            catch (const std::invalid_argument& ia)
+            {
+                fprintf(stderr,"invalid filename %s\n",argv[i]);
+                continue;
+            }
+
+            if(filename[0] == 'S')
+                lat *= -1;
+            if(filename[3] == 'W')
+                lon *= -1;
+
+            fprintf(stderr,"input file %s for (lon,lat) in [%i,%i[ x [%i,%i[... ",argv[i],lon,lon+1,lat,lat+1);
+            std::ifstream ifs(argv[i]);
+            if(!ifs.is_open())
+            {
+                fprintf(stderr,"ERROR openning file\n");
+                continue;
+            }
+            for(int lat_iter_3=3600; lat_iter_3>=0; lat_iter_3-=3)
+            {
+                for(int lon_iter_3=0; lon_iter_3<=3600; lon_iter_3+=3)
+                {
+                    if(ifs.eof())
+                    {
+                        fprintf(stderr,"Unexpected end of file\n");
+                        abort();
+                    }
+
+                    char c[2];
+                    ifs.read(c,2);
+
+                    short s;
+                    s=(unsigned char)(c[0]);
+                    s=s<<8;
+                    s+=(unsigned char)(c[1]);
+
+                    if(lon_iter_3!=3600 && lat_iter_3!=3600)
+                    {
+                        int32_t lon_s=3600*lon+lon_iter_3;
+                        int32_t lat_s=3600*lat+lat_iter_3;
+                        if(s!=-32768)
+                        {
+                            elevation_value* sf = db.grid_point(lon_s, lat_s,true);
+                            *sf = s;
+                        }
+                        else
+                        {
+                            missing_data.push_back(std::pair<int32_t,int32_t>(lon_s,lat_s));
+                            if(lon_s%3!=0 || lat_s%3!=0)
+                            {
+                                fprintf(stderr,"(init) What the fuck %u %u\n",lon_s,lat_s);
+                            }
+                        }
+                    }
+                }
+            }
+            fprintf(stderr,"done\n");
+        }
+        else
+        {
+            fprintf(stderr,"invalid filename %s\n",argv[i]);
+        }
+
+
+    }
+    fprintf(stderr,"Commit... ");
+    db.commit();
+    fprintf(stderr,"done\n");
+
+    fprintf(stderr,"Length of missing data: %lu\n",missing_data.size());
+    unsigned int iter = 0;
+
+    unsigned int old_missing_data_size = missing_data.size();
+    while(missing_data.size()!=0)
+    {
+        iter++;
+        fprintf(stderr, "Pass %i... ",iter);
+
+        std::list< std::pair< std::pair<int32_t,int32_t>, elevation_value> > new_data;
+
+        std::list< std::pair<int32_t,int32_t> >::iterator missing_data_it = missing_data.begin();
+        while(missing_data_it != missing_data.end())
+        {
+            int32_t lon_s = missing_data_it->first;
+            int32_t lat_s = missing_data_it->second;
+
+            if(lon_s%3!=0 || lat_s%3!=0)
+            {
+                fprintf(stderr,"What the fuck %u %u\n",lon_s,lat_s);
+            }
+
+            int32_t lon_s_w = lon_s - 3;
+            int32_t lon_s_e = lon_s + 3;
+            int32_t lat_s_s = lat_s - 3;
+            int32_t lat_s_n = lat_s + 3;
+
+            elevation_value *pneiW, *pneiE, *pneiS, *pneiN;
+
+            pneiE = db.grid_point(lon_s_e, lat_s);
+            pneiW = db.grid_point(lon_s_w, lat_s);
+            pneiS = db.grid_point(lon_s, lat_s_s);
+            pneiN = db.grid_point(lon_s, lat_s_n);
+
+            if( !(!pneiN) + !(!pneiS) + !(!pneiW) + !(!pneiE) >= 2)
+            {
+                elevation_value sum = 0;
+                int n=0;
+                if(pneiE)
+                {
+                    sum += *pneiE;
+                    n += 1;
+                }
+                if(pneiW)
+                {
+                    sum += *pneiW;
+                    n += 1;
+                }
+                if(pneiS)
+                {
+                    sum += *pneiS;
+                    n += 1;
+                }
+                if(pneiN)
+                {
+                    sum += *pneiN;
+                    n += 1;
+                }
+                elevation_value elev = sum/n;
+
+                new_data.push_back(std::pair< std::pair<int32_t,int32_t>, elevation_value>( *missing_data_it, elev));
+                std::list< std::pair<int32_t,int32_t> >::iterator missing_data_it_old = missing_data_it;
+                missing_data_it++;
+                missing_data.erase(missing_data_it_old);
+            }
+            else
+                missing_data_it++;
+        }
+
+        fprintf(stderr,"writing... ");
+        for(
+                std::list< std::pair< std::pair<int32_t,int32_t>, elevation_value> >::iterator new_data_iter = new_data.begin();
+                new_data_iter!=new_data.end();
+                new_data_iter++
+           )
+        {
+            elevation_value* pev = db.grid_point( 
+                                        (*new_data_iter).first.first,
+                                        (*new_data_iter).first.second,
+                                        true);
+            *pev = (*new_data_iter).second;
+        }
+
+        fprintf(stderr,"commit... ");
+        db.commit();
+        fprintf(stderr,"done\n");
+        fprintf(stderr,"Length of missing data: %lu\n",missing_data.size());
+        if(old_missing_data_size == missing_data.size())
+        {
+            fprintf(stderr,"Erreur\n");
+            for(std::list<std::pair<int32_t,int32_t> >::iterator m = missing_data.begin();m!=missing_data.end();m++)
+            {
+                fprintf(stderr,"%u\t%u\n",m->first,m->second);
+            }
+            abort();
+        }
+
+        old_missing_data_size = missing_data.size();
+    }
+
+    
+    return(0);
+}

src/elevation_query_database.cc

+#include "elevation.h"
+#include <iostream>
+
+int main(int argc, char** argv)
+{
+    if(argc<2)
+    {
+        fprintf(stderr,"Usage: %s db_path\nPut lon lat coords on stdin\n",argv[0]);
+        abort();
+    }
+
+    elevation db(argv[1]);
+
+    while(!std::cin.eof())
+    {
+        double lon,lat;
+        std::cin >> lon;
+        if(std::cin.eof())
+            break;
+        std::cin >> lat;
+
+        elevation_value elev = db.get_elevation(lon,lat);
+        printf("%f\n",elev);
+    }
+
+    return(0);
+}
+
+

src/functions.h

+#ifndef H_FUNCTIONS
+#define H_FUNCTIONS 1
+
+#include "types.h"
+#include <cmath>
+
+double rv_distance(
+        double & lon1,
+        double & lat1,
+        double & lon2,
+        double & lat2);
+