diff --git a/README.md b/README.md index 26a3476..0e4ebab 100644 --- a/README.md +++ b/README.md @@ -131,7 +131,7 @@ The current set of routing profiles can be found in the [`include/osr/routing/pr - `resolve_start_node(ways::routing, way_idx_t, node_idx_t, level_t, direction, Fn&& f)`: resolves all nodes that belong to this particular (`way_idx_t`, `node_idx_t`, `level_t`, `direction`) combination. `Fn f` will be called with each `node`. It's the task of the profile to give the routing algorithm and entry point to its overlay graph. - `resolve_all(ways::routing, node_ix_t, level_t, Fn&& f)`: Same as `resolve_start_node`, just without the condition that `way_idx_t` has to match. - - `adjacent(ways::routing, node, bitvec blocked, Fn&& f)`: Calls `Fn f` with each adjacent neighbor of the given `node`. This is used in the shortest path algorithm to expand a node and visit all its neighbors. This takes a runtime provided bit vector `blocked` into account where bit `i` indicates if `i` can be visited or not. This allows us to dynamically block nodes depending on the routing query. + - `adjacent(ways::routing, node, bitvec* blocked, sharing_data*, elevation_storage*, Fn&& f)`: Calls `Fn f` with each adjacent neighbor of the given `node`. This is used in the shortest path algorithm to expand a node and visit all its neighbors. This takes a runtime provided bit vector `blocked` into account where bit `i` indicates if `i` can be visited or not. This allows us to dynamically block nodes depending on the routing query. As we can see, each profile can define its own overlay graph on top of the data model. This gives us the flexibility to define a routing for anything we want from pedestrians or wheelchair users over cars, trucks, trains to ships without any additional memory overhead. Even combined profiles (e.g. walking, taking a bike, walking) can be implemented. Commonly, routing engines have to have a graph for each profile which makes it quite expensive (in terms of memory) to add a new profile on a global routing server. With our approach, a new profile doesn't come with extra costs. diff --git a/exe/backend/include/osr/backend/http_server.h b/exe/backend/include/osr/backend/http_server.h index d685b19..6011b20 100644 --- a/exe/backend/include/osr/backend/http_server.h +++ b/exe/backend/include/osr/backend/http_server.h @@ -5,6 +5,7 @@ #include "boost/asio/io_context.hpp" +#include "osr/elevation_storage.h" #include "osr/lookup.h" #include "osr/platforms.h" #include "osr/ways.h" @@ -17,6 +18,7 @@ struct http_server { ways const&, lookup const&, platforms const*, + elevation_storage const*, std::string const& static_file_path); ~http_server(); http_server(http_server const&) = delete; diff --git a/exe/backend/src/http_server.cc b/exe/backend/src/http_server.cc index 4eb87cd..f978231 100644 --- a/exe/backend/src/http_server.cc +++ b/exe/backend/src/http_server.cc @@ -76,12 +76,14 @@ struct http_server::impl { ways const& g, lookup const& l, platforms const* pl, + elevation_storage const* elevations, std::string const& static_file_path) : ioc_{ios}, thread_pool_{thread_pool}, w_{g}, l_{l}, pl_{pl}, + elevations_{elevations}, server_{ioc_} { try { if (!static_file_path.empty() && fs::is_directory(static_file_path)) { @@ -115,7 +117,8 @@ struct http_server::impl { auto const max_it = q.find("max"); auto const max = static_cast( max_it == q.end() ? 3600 : max_it->value().as_int64()); - auto const p = route(w_, l_, profile, from, to, max, dir, 100); + auto const p = route(w_, l_, profile, from, to, max, dir, 100, nullptr, + nullptr, elevations_); if (!p.has_value()) { cb(json_response(req, "could not find a valid path", http::status::not_found)); @@ -189,7 +192,15 @@ struct http_server::impl { case search_profile::kWheelchair: send_graph_response>(req, cb, gj); break; - case search_profile::kBike: send_graph_response(req, cb, gj); break; + case search_profile::kBike: + send_graph_response>(req, cb, gj); + break; + case search_profile::kBikeElevationLow: + send_graph_response>(req, cb, gj); + break; + case search_profile::kBikeElevationHigh: + send_graph_response>(req, cb, gj); + break; case search_profile::kCar: send_graph_response(req, cb, gj); break; case search_profile::kCarParking: send_graph_response>(req, cb, gj); @@ -352,6 +363,7 @@ struct http_server::impl { ways const& w_; lookup const& l_; platforms const* pl_; + elevation_storage const* elevations_; web_server server_; bool serve_static_files_{false}; std::string static_file_path_; @@ -362,8 +374,10 @@ http_server::http_server(boost::asio::io_context& ioc, ways const& w, lookup const& l, platforms const* pl, + elevation_storage const* elevation, std::string const& static_file_path) - : impl_(new impl(ioc, thread_pool, w, l, pl, static_file_path)) {} + : impl_(new impl(ioc, thread_pool, w, l, pl, elevation, static_file_path)) { +} http_server::~http_server() = default; diff --git a/exe/backend/src/main.cc b/exe/backend/src/main.cc index ce58671..98cf364 100644 --- a/exe/backend/src/main.cc +++ b/exe/backend/src/main.cc @@ -13,6 +13,7 @@ #include "net/stop_handler.h" #include "osr/backend/http_server.h" +#include "osr/elevation_storage.h" #include "osr/lookup.h" #include "osr/platforms.h" #include "osr/ways.h" @@ -89,12 +90,14 @@ int main(int argc, char const* argv[]) { if (pl != nullptr) { pl->build_rtree(w); } + auto const elevations = elevation_storage::try_open(opt.data_dir_); auto const l = lookup{w, opt.data_dir_, cista::mmap::protection::READ}; auto ioc = boost::asio::io_context{}; auto pool = boost::asio::io_context{}; - auto server = http_server{ioc, pool, w, l, pl.get(), opt.static_file_path_}; + auto server = http_server{ + ioc, pool, w, l, pl.get(), elevations.get(), opt.static_file_path_}; auto work_guard = boost::asio::make_work_guard(pool); auto threads = std::vector(std::max(1U, opt.threads_)); diff --git a/exe/benchmark.cc b/exe/benchmark.cc index 2b510e8..e8d735a 100644 --- a/exe/benchmark.cc +++ b/exe/benchmark.cc @@ -16,6 +16,7 @@ #include "utl/timer.h" +#include "osr/elevation_storage.h" #include "osr/lookup.h" #include "osr/routing/dijkstra.h" #include "osr/routing/profile.h" @@ -124,6 +125,7 @@ int main(int argc, char const* argv[]) { } auto const w = ways{opt.data_dir_, cista::mmap::protection::READ}; + auto const elevations = elevation_storage::try_open(opt.data_dir_); auto threads = std::vector(std::max(1U, opt.threads_)); auto results = std::vector{}; @@ -144,8 +146,8 @@ int main(int argc, char const* argv[]) { node_idx_t{cista::hash_combine(h, ++n, i.load()) % w.n_nodes()}; d.reset(opt.max_dist_); set_start(d, w, start); - d.template run(w, *w.r_, opt.max_dist_, - nullptr, nullptr); + d.template run( + w, *w.r_, opt.max_dist_, nullptr, nullptr, elevations.get()); auto const end_time = std::chrono::steady_clock::now(); { auto const guard = std::lock_guard{m}; @@ -168,5 +170,8 @@ int main(int argc, char const* argv[]) { }; run_benchmark.template operator()("car"); - run_benchmark.template operator()("bike"); + run_benchmark.template operator()>( + "bike (no elevation costs)"); + run_benchmark.template operator()>( + "bike (high elevation costs)"); } diff --git a/exe/extract.cc b/exe/extract.cc index fefa515..8608526 100644 --- a/exe/extract.cc +++ b/exe/extract.cc @@ -18,10 +18,11 @@ struct config : public conf::configuration { : configuration{"Options"}, in_{std::move(in)}, out_{std::move(out)} { param(in_, "in,i", "OpenStreetMap .osm.pbf input path"); param(out_, "out,o", "output directory"); + param(elevation_data_, "elevation_data,e", "directory with elevation data"); param(with_platforms_, "with_platforms,p", "extract platform info"); } - std::filesystem::path in_, out_; + std::filesystem::path in_, out_, elevation_data_; bool with_platforms_{false}; }; @@ -44,5 +45,5 @@ int main(int ac, char const** av) { utl::activate_progress_tracker("osr"); auto const silencer = utl::global_progress_bars{false}; - extract(c.with_platforms_, c.in_, c.out_); + extract(c.with_platforms_, c.in_, c.out_, c.elevation_data_); } \ No newline at end of file diff --git a/include/osr/elevation_storage.h b/include/osr/elevation_storage.h new file mode 100644 index 0000000..4a507f7 --- /dev/null +++ b/include/osr/elevation_storage.h @@ -0,0 +1,49 @@ +#pragma once + +#include +#include +#include + +#include "cista/mmap.h" + +#include "osr/types.h" +#include "osr/ways.h" + +namespace osr { + +namespace preprocessing::elevation { +struct provider; +} + +struct elevation_storage { + struct elevation { + elevation& operator+=(elevation const&); + elevation swap() const; + elevation_monotonic_t up_{0U}; + elevation_monotonic_t down_{0U}; + }; + struct encoding { + using coding = std::uint8_t; + explicit encoding(elevation const&); + encoding() = default; + elevation decode() const; + explicit operator bool() const; + coding up_ : 4 {}; + coding down_ : 4 {}; + }; + elevation_storage(std::filesystem::path const&, + cista::mmap::protection const mode); + static std::unique_ptr try_open( + std::filesystem::path const&); + void set_elevations(ways const&, preprocessing::elevation::provider const&); + elevation get_elevations(way_idx_t const way, + std::uint16_t const segment) const; + + mm_vecvec elevations_; +}; + +elevation_storage::elevation get_elevations(elevation_storage const*, + way_idx_t const way, + std::uint16_t const segment); + +} // namespace osr \ No newline at end of file diff --git a/include/osr/extract/extract.h b/include/osr/extract/extract.h index 66e74b2..402f8c9 100644 --- a/include/osr/extract/extract.h +++ b/include/osr/extract/extract.h @@ -4,6 +4,7 @@ namespace osr { void extract(bool with_platforms, std::filesystem::path const& in, - std::filesystem::path const& out); + std::filesystem::path const& out, + std::filesystem::path const& elevation_dir); } // namespace osr \ No newline at end of file diff --git a/include/osr/point.h b/include/osr/point.h index 5e47830..1c09d8d 100644 --- a/include/osr/point.h +++ b/include/osr/point.h @@ -23,6 +23,10 @@ struct point { return {l.x(), l.y()}; } + static point from_latlng(double lat, double lng) { + return from_location(osmium::Location(lng, lat)); + } + osmium::Location as_location() const { return osmium::Location{lat_, lng_}; } geo::latlng as_latlng() const { diff --git a/include/osr/preprocessing/elevation/dem_driver.h b/include/osr/preprocessing/elevation/dem_driver.h new file mode 100644 index 0000000..e732292 --- /dev/null +++ b/include/osr/preprocessing/elevation/dem_driver.h @@ -0,0 +1,28 @@ +#pragma once + +#include + +#include "cista/containers/rtree.h" + +#include "osr/point.h" +#include "osr/preprocessing/elevation/dem_tile.h" +#include "osr/preprocessing/elevation/resolution.h" +#include "osr/preprocessing/elevation/shared.h" + +namespace fs = std::filesystem; + +namespace osr::preprocessing::elevation { + +struct dem_driver { + dem_driver() = default; + bool add_tile(fs::path const&); + elevation_meters_t get(point const&) const; + tile_idx_t tile_idx(point const&) const; + resolution max_resolution() const; + std::size_t n_tiles() const; + + cista::raw::rtree rtree_{}; + std::vector tiles_{}; +}; + +} // namespace osr::preprocessing::elevation diff --git a/include/osr/preprocessing/elevation/dem_tile.h b/include/osr/preprocessing/elevation/dem_tile.h new file mode 100644 index 0000000..7622660 --- /dev/null +++ b/include/osr/preprocessing/elevation/dem_tile.h @@ -0,0 +1,43 @@ +#pragma once + +#include +#include +#include + +#include "osr/point.h" +#include "osr/preprocessing/elevation/resolution.h" +#include "osr/preprocessing/elevation/shared.h" + +namespace fs = std::filesystem; + +namespace osr::preprocessing::elevation { + +enum class pixel_type : std::uint8_t { int16, float32 }; + +union pixel_value { + std::int16_t int16_; + float float32_; +}; + +struct dem_tile { + explicit dem_tile(fs::path const& filename); + ~dem_tile(); + dem_tile(dem_tile&& grid) noexcept; + dem_tile(dem_tile const&) = delete; + dem_tile& operator=(dem_tile const&) = delete; + dem_tile& operator=(dem_tile&&) = delete; + + elevation_meters_t get(point const&) const; + tile_idx_t tile_idx(point const&) const; + coord_box get_coord_box() const; + + pixel_value get_raw(point const&) const; + pixel_type get_pixel_type() const; + resolution max_resolution() const; + +private: + struct impl; + std::unique_ptr impl_; +}; + +} // namespace osr::preprocessing::elevation diff --git a/include/osr/preprocessing/elevation/hgt_driver.h b/include/osr/preprocessing/elevation/hgt_driver.h new file mode 100644 index 0000000..dbc6fc8 --- /dev/null +++ b/include/osr/preprocessing/elevation/hgt_driver.h @@ -0,0 +1,36 @@ +#pragma once + +#include +#include +#include + +#include "cista/containers/rtree.h" + +#include "osr/point.h" +#include "osr/preprocessing/elevation/hgt_tile.h" +#include "osr/preprocessing/elevation/resolution.h" +#include "osr/preprocessing/elevation/shared.h" + +namespace fs = std::filesystem; + +namespace osr::preprocessing::elevation { + +static_assert(IsTile>); +static_assert(IsTile>); + +struct hgt_driver { + using hgt_tile_t = std::variant, hgt_tile<1201>>; + + hgt_driver() = default; + bool add_tile(fs::path const&); + elevation_meters_t get(point const&) const; + tile_idx_t tile_idx(point const&) const; + resolution max_resolution() const; + std::size_t n_tiles() const; + static std::optional open(fs::path const&); + + cista::raw::rtree rtree_; + std::vector tiles_; +}; + +} // namespace osr::preprocessing::elevation diff --git a/include/osr/preprocessing/elevation/hgt_tile.h b/include/osr/preprocessing/elevation/hgt_tile.h new file mode 100644 index 0000000..20a4db2 --- /dev/null +++ b/include/osr/preprocessing/elevation/hgt_tile.h @@ -0,0 +1,45 @@ +#pragma once + +#include +#include +#include + +#include "osr/point.h" +#include "osr/preprocessing/elevation/resolution.h" +#include "osr/preprocessing/elevation/shared.h" + +namespace osr::preprocessing::elevation { + +template +struct hgt_tile { + constexpr static auto const kBytesPerPixel = std::size_t{2U}; + + explicit hgt_tile(std::string const& filename, + std::int8_t const lat, + std::int16_t const lng); + ~hgt_tile(); + hgt_tile(hgt_tile&& grid) noexcept; + hgt_tile(hgt_tile const&) = delete; + hgt_tile& operator=(hgt_tile const&) = delete; + hgt_tile& operator=(hgt_tile&&) = delete; + + elevation_meters_t get(point const&) const; + tile_idx_t tile_idx(point const&) const; + + resolution max_resolution() const; + + coord_box get_coord_box() const; + + static constexpr std::size_t file_size() { + return RasterSize * RasterSize * kBytesPerPixel; + } + +private: + struct impl; + std::unique_ptr impl_; +}; + +extern template struct hgt_tile<3601U>; +extern template struct hgt_tile<1201U>; + +} // namespace osr::preprocessing::elevation diff --git a/include/osr/preprocessing/elevation/hgt_tile_def.h b/include/osr/preprocessing/elevation/hgt_tile_def.h new file mode 100644 index 0000000..71d2a66 --- /dev/null +++ b/include/osr/preprocessing/elevation/hgt_tile_def.h @@ -0,0 +1,158 @@ +#pragma once + +#include "osr/preprocessing/elevation/hgt_tile.h" + +#include +#include +#include +#include + +#include "cista/mmap.h" + +// SRTM HGT File Format +// +// https://lpdaac.usgs.gov/documents/179/SRTM_User_Guide_V3.pdf +// +// Usage (see User Guide 2.1.4 SRTM Topography Data Format): +// +// The names of individual data tiles refer to the latitude and longitude of the +// south west (lower left) corner of the tile. For example, N37W105 has its +// lower left corner at 37°N latitude and 105° West (W) longitude and covers +// (slightly more than) the area 37-38°N and 104 -105°W. To be more exact, the +// file name coordinates refer to the geometric center of the lower- left pixel, +// and all edge pixels of the tile are centered on whole-degree lines of +// latitude and / or longitude. The unit of elevation is meters as referenced to +// the WGS84/EGM96 geoid (NGA, 1997;Lemoine, 1998) +// +// Height files have the extension. HGT, and the DEM is provided as two-byte +// (16-bit) binary signed integer raster data. Two-byte signed integers can +// range from - 32,767 to 32,767m and can encompass the range of the Earth’s +// elevations. Header or trailer bytes are not embedded in the file. The data +// are stored in row major order, meaning all the data for the northernmost row, +// row 1, are followed by all the data for row 2, and so on . +// + +namespace osr::preprocessing::elevation { + +// Void value is used in version 1.0 and 2.1 only +constexpr auto const kVoidValue = std::int16_t{-32768}; + +template +struct hgt_tile::hgt_tile::impl { + + constexpr static auto kStepWidth = double{1. / (RasterSize - 1U)}; + constexpr static auto kCenterOffset = kStepWidth / 2.; + + impl(std::string const& filename, + std::int8_t const lat, + std::int16_t const lng) + : file_{cista::mmap{filename.data(), cista::mmap::protection::READ}}, + sw_lat_(lat), + sw_lng_(lng) {} + + template + std::size_t get_offset(point const& p) const { + auto const lat = p.lat(); + auto const lng = p.lng(); + auto const box = get_coord_box(); + if (box.min_lng_ <= lng && lng < box.max_lng_ && // + box.min_lat_ <= lat && lat < box.max_lat_) { + // Column: Left to right + auto const column = + std::clamp(static_cast( + std::floor(((lng - box.min_lng_) * (RasterSize - 1U)) / + RasterSize * UpperBound)), + std::size_t{0U}, UpperBound - 1U); + // Row: Top to bottom + auto const row = + std::clamp(static_cast( + std::floor(((box.max_lat_ - lat) * (RasterSize - 1U)) / + RasterSize * UpperBound)), + std::size_t{0U}, (UpperBound - 1U)); + // Data in row major order + return UpperBound * row + column; + } + return std::numeric_limits::max(); + } + + elevation_meters_t get(point const& p) const { + auto const offset = get_offset(p); + if (offset == std::numeric_limits::max()) { + return elevation_meters_t::invalid(); + } + return get(kBytesPerPixel * offset); + } + + elevation_meters_t get(std::size_t const offset) const { + assert(offset < kBytesPerPixel * RasterSize * RasterSize); + auto const byte_ptr = file_.data() + offset; + auto const raw_value = *reinterpret_cast(byte_ptr); + // Byte is stored in big-endian + auto const meters = std::endian::native == std::endian::big + ? raw_value + : std::byteswap(raw_value); + return (meters == kVoidValue) ? elevation_meters_t::invalid() + : elevation_meters_t{meters}; + } + + tile_idx_t tile_idx(point const& p) const { + constexpr auto const kSegments = 1 << (tile_idx_t::kSubTileIdxSize / 2); + auto const offset = get_offset(p); + return (offset == std::numeric_limits::max()) + ? tile_idx_t::invalid() + : tile_idx_t::from_sub_tile( + static_cast(offset)); + } + + coord_box get_coord_box() const { + return { + .min_lat_ = static_cast(sw_lat_ - kCenterOffset), + .min_lng_ = static_cast(sw_lng_ - kCenterOffset), + .max_lat_ = static_cast(sw_lat_ + 1.F + kCenterOffset), + .max_lng_ = static_cast(sw_lng_ + 1.F + kCenterOffset), + }; + } + + constexpr resolution max_resolution() const { + return {.x_ = kStepWidth, .y_ = kStepWidth}; + } + + cista::mmap file_{}; + // south west coordinate + std::int8_t sw_lat_; + std::int16_t sw_lng_; +}; + +template +hgt_tile::hgt_tile(std::string const& filename, + std::int8_t const lat, + std::int16_t const lng) + : impl_{std::make_unique(filename, lat, lng)} {} + +template +hgt_tile::~hgt_tile() = default; + +template +hgt_tile::hgt_tile(hgt_tile&& grid) noexcept = default; + +template +elevation_meters_t hgt_tile::get(point const& p) const { + return impl_->get(p); +} + +template +tile_idx_t hgt_tile::tile_idx(point const& p) const { + return impl_->tile_idx(p); +} + +template +resolution hgt_tile::max_resolution() const { + return impl_->max_resolution(); +} + +template +coord_box hgt_tile::get_coord_box() const { + return impl_->get_coord_box(); +} + +} // namespace osr::preprocessing::elevation diff --git a/include/osr/preprocessing/elevation/provider.h b/include/osr/preprocessing/elevation/provider.h new file mode 100644 index 0000000..03ea003 --- /dev/null +++ b/include/osr/preprocessing/elevation/provider.h @@ -0,0 +1,29 @@ +#pragma once + +#include + +#include "osr/point.h" +#include "osr/preprocessing/elevation/resolution.h" +#include "osr/preprocessing/elevation/shared.h" + +namespace osr::preprocessing::elevation { + +struct provider { + provider(std::filesystem::path const&); + ~provider(); + provider(provider const&) = delete; + provider& operator=(provider const&) = delete; + provider(provider&&) = delete; + provider& operator=(provider&&) = delete; + + elevation_meters_t get(point const&) const; + tile_idx_t tile_idx(point const&) const; + std::size_t driver_count() const; + resolution max_resolution() const; + +private: + struct impl; + std::unique_ptr impl_; +}; + +} // namespace osr::preprocessing::elevation diff --git a/include/osr/preprocessing/elevation/resolution.h b/include/osr/preprocessing/elevation/resolution.h new file mode 100644 index 0000000..f992c1e --- /dev/null +++ b/include/osr/preprocessing/elevation/resolution.h @@ -0,0 +1,22 @@ +#pragma once + +#include +#include + +namespace osr::preprocessing::elevation { + +struct resolution { + resolution& update(resolution const& o) { + if (std::isnan(x_) || x_ < o.x_) { + x_ = o.x_; + } + if (std::isnan(y_) || y_ < o.y_) { + y_ = o.y_; + } + return *this; + } + double x_{std::numeric_limits::quiet_NaN()}; + double y_{std::numeric_limits::quiet_NaN()}; +}; + +} // namespace osr::preprocessing::elevation \ No newline at end of file diff --git a/include/osr/preprocessing/elevation/shared.h b/include/osr/preprocessing/elevation/shared.h new file mode 100644 index 0000000..1e911ee --- /dev/null +++ b/include/osr/preprocessing/elevation/shared.h @@ -0,0 +1,74 @@ +#pragma once + +#include +#include +#include +#include + +#include "cista/strong.h" + +#include "osr/point.h" +#include "osr/preprocessing/elevation/resolution.h" + +namespace osr::preprocessing::elevation { + +using elevation_meters_t = + cista::strong; + +struct coord_box { + float min_lat_; + float min_lng_; + float max_lat_; + float max_lng_; +}; + +struct tile_idx_t { + using data_t = std::uint32_t; + constexpr static auto const kDriverIdxSize = 4; + constexpr static auto const kTileIdxSize = 16; + constexpr static auto const kSubTileIdxSize = + sizeof(data_t) * CHAR_BIT - kDriverIdxSize - kTileIdxSize; + + constexpr static tile_idx_t invalid() { + return { + .driver_idx_ = (1 << kDriverIdxSize) - 1, + .tile_idx_ = (1 << kTileIdxSize) - 1, + .sub_tile_idx_ = (1 << kSubTileIdxSize) - 1, + }; + } + static tile_idx_t from_sub_tile(data_t const& idx) { + auto t = invalid(); + t.sub_tile_idx_ = idx; + return t; + } + bool operator==(tile_idx_t const&) const = default; + std::strong_ordering operator<=>(tile_idx_t const& o) const { + return std::tie(driver_idx_, tile_idx_, sub_tile_idx_) <=> + std::tie(o.driver_idx_, o.tile_idx_, o.sub_tile_idx_); + } + + data_t driver_idx_ : kDriverIdxSize; + data_t tile_idx_ : kTileIdxSize; + data_t sub_tile_idx_ : kSubTileIdxSize; +}; +static_assert(sizeof(tile_idx_t) == sizeof(tile_idx_t::data_t)); + +template +concept IsProvider = + requires(ElevationProvider const& provider, point const& p) { + { provider.get(p) } -> std::same_as; + { provider.tile_idx(p) } -> std::same_as; + { provider.max_resolution() } -> std::same_as; + }; + +template +concept IsTile = IsProvider && requires(Tile const& tile) { + { tile.get_coord_box() } -> std::same_as; +}; + +template +concept IsDriver = IsProvider && requires(Driver const& driver) { + { driver.n_tiles() } -> std::same_as; +}; + +} // namespace osr::preprocessing::elevation diff --git a/include/osr/routing/dijkstra.h b/include/osr/routing/dijkstra.h index a8283d2..fe339f2 100644 --- a/include/osr/routing/dijkstra.h +++ b/include/osr/routing/dijkstra.h @@ -1,5 +1,6 @@ #pragma once +#include "osr/elevation_storage.h" #include "osr/routing/additional_edge.h" #include "osr/routing/dial.h" #include "osr/types.h" @@ -52,7 +53,8 @@ struct dijkstra { ways::routing const& r, cost_t const max, bitvec const* blocked, - sharing_data const* sharing) { + sharing_data const* sharing, + elevation_storage const* elevations) { while (!pq_.empty()) { auto l = pq_.pop(); if (get_cost(l.get_node()) < l.cost()) { @@ -67,9 +69,10 @@ struct dijkstra { auto const curr = l.get_node(); Profile::template adjacent( - r, curr, blocked, sharing, + r, curr, blocked, sharing, elevations, [&](node const neighbor, std::uint32_t const cost, distance_t, - way_idx_t const way, std::uint16_t, std::uint16_t) { + way_idx_t const way, std::uint16_t, std::uint16_t, + elevation_storage::elevation const) { if constexpr (kDebug) { std::cout << " NEIGHBOR "; neighbor.print(std::cout, w); @@ -100,15 +103,18 @@ struct dijkstra { cost_t const max, bitvec const* blocked, sharing_data const* sharing, + elevation_storage const* elevations, direction const dir) { if (blocked == nullptr) { - dir == direction::kForward - ? run(w, r, max, blocked, sharing) - : run(w, r, max, blocked, sharing); + dir == direction::kForward ? run( + w, r, max, blocked, sharing, elevations) + : run( + w, r, max, blocked, sharing, elevations); } else { - dir == direction::kForward - ? run(w, r, max, blocked, sharing) - : run(w, r, max, blocked, sharing); + dir == direction::kForward ? run( + w, r, max, blocked, sharing, elevations) + : run( + w, r, max, blocked, sharing, elevations); } } diff --git a/include/osr/routing/profile.h b/include/osr/routing/profile.h index 98329ed..d60a188 100644 --- a/include/osr/routing/profile.h +++ b/include/osr/routing/profile.h @@ -9,6 +9,8 @@ enum class search_profile : std::uint8_t { kFoot, kWheelchair, kBike, + kBikeElevationLow, + kBikeElevationHigh, kCar, kCarParking, kCarParkingWheelchair, diff --git a/include/osr/routing/profiles/bike.h b/include/osr/routing/profiles/bike.h index 2dbc583..a0655b7 100644 --- a/include/osr/routing/profiles/bike.h +++ b/include/osr/routing/profiles/bike.h @@ -1,13 +1,28 @@ #pragma once +#include "osr/elevation_storage.h" #include "osr/routing/mode.h" #include "osr/routing/route.h" +#include "osr/types.h" #include "osr/ways.h" namespace osr { struct sharing_data; +constexpr auto const kElevationNoCost = 0U; +// TODO Adjust value +constexpr auto const kElevationLowCost = 570U; +constexpr auto const kElevationHighCost = 3700U; + +// Some configurations (cost, exp): +// (250, 1000) +// (800, 1000) +// (570, 2100) +// (3700, 2100) + +template struct bike { static constexpr auto const kMaxMatchDistance = 100U; static constexpr auto const kOffroadPenalty = 1U; @@ -107,6 +122,7 @@ struct bike { node const n, bitvec const* blocked, sharing_data const*, + elevation_storage const* elevations, Fn&& fn) { for (auto const [way, i] : utl::zip_unchecked(w.node_ways_[n.n_], w.node_in_way_idx_[n.n_])) { @@ -130,10 +146,27 @@ struct bike { } auto const dist = w.way_node_dist_[way][std::min(from, to)]; + auto const elevation = [&]() { + auto const e = (from < to) ? get_elevations(elevations, way, from) + : get_elevations(elevations, way, to); + auto const in_direction = + (SearchDir == direction::kForward) == (from < to); + return in_direction ? e : e.swap(); + }(); + auto const elevation_cost = static_cast( + ElevationUpCost > 0U && dist > 0U + ? (ElevationExponentThousandth > 1000U + ? ElevationUpCost * + std::pow( + static_cast(to_idx(elevation.up_)) / + dist, + ElevationExponentThousandth / 1000.0) + : ElevationUpCost * to_idx(elevation.up_) / dist) + : 0); auto const cost = way_cost(target_way_prop, way_dir, dist) + - node_cost(target_node_prop); + node_cost(target_node_prop) + elevation_cost; fn(node{target_node}, static_cast(cost), dist, way, from, - to); + to, std::move(elevation)); }; if (i != 0U) { diff --git a/include/osr/routing/profiles/bike_sharing.h b/include/osr/routing/profiles/bike_sharing.h index b54e71c..2c8abd3 100644 --- a/include/osr/routing/profiles/bike_sharing.h +++ b/include/osr/routing/profiles/bike_sharing.h @@ -9,6 +9,7 @@ #include "utl/helpers/algorithm.h" +#include "osr/elevation_storage.h" #include "osr/routing/additional_edge.h" #include "osr/routing/mode.h" #include "osr/routing/profiles/bike.h" @@ -218,13 +219,15 @@ struct bike_sharing { return {.n_ = n.n_, .lvl_ = n.lvl_}; } - static bike::node to_bike(node const n) { return {.n_ = n.n_}; } + static bike::node to_bike(node const n) { + return {.n_ = n.n_}; + } static node to_node(footp::node const n, node_type const type) { return {.n_ = n.n_, .type_ = type, .lvl_ = n.lvl_}; } - static node to_node(bike::node const n, level_t const lvl) { + static node to_node(bike::node const n, level_t const lvl) { return {.n_ = n.n_, .type_ = node_type::kBike, .lvl_ = lvl}; } @@ -260,6 +263,7 @@ struct bike_sharing { node const n, bitvec const* blocked, sharing_data const* sharing, + elevation_storage const* elevations, Fn&& fn) { assert(sharing != nullptr); @@ -268,19 +272,21 @@ struct bike_sharing { fn(node{.n_ = ae.node_, .type_ = nt, .lvl_ = nt == node_type::kBike ? kNoLevel : n.lvl_}, - cost, ae.distance_, way_idx_t::invalid(), 0, 1); + cost, ae.distance_, way_idx_t::invalid(), 0, 1, + elevation_storage::elevation{}); }; auto const& continue_on_foot = [&](node_type const nt, bool const include_additional_edges, cost_t const switch_penalty = 0) { footp::template adjacent( - w, to_foot(n), blocked, nullptr, + w, to_foot(n), blocked, nullptr, elevations, [&](footp::node const neighbor, std::uint32_t const cost, distance_t const dist, way_idx_t const way, - std::uint16_t const from, std::uint16_t const to) { + std::uint16_t const from, std::uint16_t const to, + elevation_storage::elevation const elevation) { fn(to_node(neighbor, nt), cost + switch_penalty, dist, way, from, - to); + to, std::move(elevation)); }); if (include_additional_edges) { // walk to station or free-floating bike @@ -299,24 +305,26 @@ struct bike_sharing { auto const& continue_on_bike = [&](bool const include_additional_edges, cost_t const switch_penalty = 0) { - bike::adjacent( - w, to_bike(n), blocked, nullptr, - [&](bike::node const neighbor, std::uint32_t const cost, - distance_t const dist, way_idx_t const way, - std::uint16_t const from, std::uint16_t const to) { + bike::adjacent( + w, to_bike(n), blocked, nullptr, elevations, + [&](bike::node const neighbor, + std::uint32_t const cost, distance_t const dist, + way_idx_t const way, std::uint16_t const from, + std::uint16_t const to, + elevation_storage::elevation const elevation) { fn(to_node(neighbor, kNoLevel), cost + switch_penalty, dist, way, - from, to); + from, to, std::move(elevation)); }); if (include_additional_edges) { // drive to station if (auto const it = sharing->additional_edges_.find(n.n_); it != end(sharing->additional_edges_)) { for (auto const& ae : it->second) { - handle_additional_edge( - ae, node_type::kBike, - bike::way_cost(kAdditionalWayProperties, direction::kForward, - ae.distance_) + - switch_penalty); + handle_additional_edge(ae, node_type::kBike, + bike::way_cost( + kAdditionalWayProperties, + direction::kForward, ae.distance_) + + switch_penalty); } } } @@ -332,11 +340,11 @@ struct bike_sharing { for (auto const& ae : it->second) { if (n.is_initial_foot_node() && sharing->start_allowed_.test(n.n_)) { - handle_additional_edge( - ae, node_type::kBike, - bike::way_cost(kAdditionalWayProperties, direction::kForward, - ae.distance_) + - kStartSwitchPenalty); + handle_additional_edge(ae, node_type::kBike, + bike::way_cost( + kAdditionalWayProperties, + direction::kForward, ae.distance_) + + kStartSwitchPenalty); } else if (n.is_bike_node() && sharing->end_allowed_.test(n.n_)) { handle_additional_edge( ae, node_type::kTrailingFoot, @@ -368,11 +376,11 @@ struct bike_sharing { it != end(sharing->additional_edges_)) { for (auto const& ae : it->second) { if (n.is_trailing_foot_node() && sharing->end_allowed_.test(n.n_)) { - handle_additional_edge( - ae, node_type::kBike, - bike::way_cost(kAdditionalWayProperties, direction::kForward, - ae.distance_) + - kEndSwitchPenalty); + handle_additional_edge(ae, node_type::kBike, + bike::way_cost( + kAdditionalWayProperties, + direction::kForward, ae.distance_) + + kEndSwitchPenalty); } else if (n.is_bike_node() && sharing->start_allowed_.test(n.n_)) { handle_additional_edge( ae, node_type::kInitialFoot, diff --git a/include/osr/routing/profiles/car.h b/include/osr/routing/profiles/car.h index b6d1140..e5a4421 100644 --- a/include/osr/routing/profiles/car.h +++ b/include/osr/routing/profiles/car.h @@ -6,6 +6,7 @@ #include "utl/helpers/algorithm.h" +#include "osr/elevation_storage.h" #include "osr/routing/mode.h" #include "osr/routing/route.h" #include "osr/ways.h" @@ -164,6 +165,7 @@ struct car { node const n, bitvec const* blocked, sharing_data const*, + elevation_storage const*, Fn&& fn) { auto way_pos = way_pos_t{0U}; for (auto const [way, i] : @@ -199,7 +201,7 @@ struct car { auto const cost = way_cost(target_way_prop, way_dir, dist) + node_cost(target_node_prop) + (is_u_turn ? kUturnPenalty : 0U); - fn(target, cost, dist, way, from, to); + fn(target, cost, dist, way, from, to, elevation_storage::elevation{}); }; if (i != 0U) { diff --git a/include/osr/routing/profiles/car_parking.h b/include/osr/routing/profiles/car_parking.h index 35af440..6c2c6ef 100644 --- a/include/osr/routing/profiles/car_parking.h +++ b/include/osr/routing/profiles/car_parking.h @@ -6,6 +6,7 @@ #include "utl/helpers/algorithm.h" +#include "osr/elevation_storage.h" #include "osr/routing/mode.h" #include "osr/routing/profiles/car.h" #include "osr/routing/profiles/foot.h" @@ -240,6 +241,7 @@ struct car_parking { node const n, bitvec const* blocked, sharing_data const*, + elevation_storage const* elevations, Fn&& fn) { static constexpr auto const kFwd = SearchDir == direction::kForward; static constexpr auto const kBwd = SearchDir == direction::kBackward; @@ -248,26 +250,28 @@ struct car_parking { if (n.is_foot_node() || (kFwd && n.is_car_node() && is_parking)) { footp::template adjacent( - w, to_foot(n), blocked, nullptr, + w, to_foot(n), blocked, nullptr, elevations, [&](footp::node const neighbor, std::uint32_t const cost, distance_t const dist, way_idx_t const way, - std::uint16_t const from, std::uint16_t const to) { + std::uint16_t const from, std::uint16_t const to, + elevation_storage::elevation const elevation) { fn(to_node(neighbor), cost + (n.is_foot_node() ? 0 : kSwitchPenalty), dist, way, from, - to); + to, elevation); }); } if (n.is_car_node() || (kBwd && n.is_foot_node() && is_parking)) { car::template adjacent( - w, to_car(n), blocked, nullptr, + w, to_car(n), blocked, nullptr, elevations, [&](car::node const neighbor, std::uint32_t const cost, distance_t const dist, way_idx_t const way, - std::uint16_t const from, std::uint16_t const to) { + std::uint16_t const from, std::uint16_t const to, + elevation_storage::elevation const elevation) { auto const way_prop = w.way_properties_[way]; fn(to_node(neighbor, way_prop.from_level()), cost + (n.is_car_node() ? 0 : kSwitchPenalty), dist, way, from, - to); + to, elevation); }); } } diff --git a/include/osr/routing/profiles/foot.h b/include/osr/routing/profiles/foot.h index 76b1643..7e21776 100644 --- a/include/osr/routing/profiles/foot.h +++ b/include/osr/routing/profiles/foot.h @@ -2,6 +2,7 @@ #include "utl/for_each_bit_set.h" +#include "osr/elevation_storage.h" #include "osr/routing/mode.h" #include "osr/routing/tracking.h" #include "osr/ways.h" @@ -149,6 +150,7 @@ struct foot { node const n, bitvec const* blocked, sharing_data const*, + elevation_storage const*, Fn&& fn) { for (auto const [way, i] : utl::zip_unchecked(w.node_ways_[n.n_], w.node_in_way_idx_[n.n_])) { @@ -179,7 +181,8 @@ struct foot { auto const cost = way_cost(target_way_prop, way_dir, dist) + node_cost(target_node_prop); fn(node{target_node, target_lvl}, - static_cast(cost), dist, way, from, to); + static_cast(cost), dist, way, from, to, + elevation_storage::elevation{}); }); } else { auto const target_lvl = get_target_level(w, n.n_, n.lvl_, way); @@ -191,7 +194,7 @@ struct foot { auto const cost = way_cost(target_way_prop, way_dir, dist) + node_cost(target_node_prop); fn(node{target_node, *target_lvl}, static_cast(cost), - dist, way, from, to); + dist, way, from, to, elevation_storage::elevation{}); } }; diff --git a/include/osr/routing/route.h b/include/osr/routing/route.h index c475ae7..9e50536 100644 --- a/include/osr/routing/route.h +++ b/include/osr/routing/route.h @@ -5,6 +5,7 @@ #include "geo/polyline.h" +#include "osr/elevation_storage.h" #include "osr/location.h" #include "osr/lookup.h" #include "osr/routing/mode.h" @@ -30,11 +31,13 @@ struct path { way_idx_t way_{way_idx_t::invalid()}; cost_t cost_{kInfeasible}; distance_t dist_{0}; + elevation_storage::elevation elevation_{}; mode mode_{mode::kFoot}; }; cost_t cost_{kInfeasible}; double dist_{0.0}; + elevation_storage::elevation elevation_{}; std::vector segments_{}; bool uses_elevator_{false}; }; @@ -53,6 +56,7 @@ std::vector> route( double max_match_distance, bitvec const* blocked = nullptr, sharing_data const* sharing = nullptr, + elevation_storage const* = nullptr, std::function const& do_reconstruct = [](path const&) { return false; }); @@ -66,7 +70,8 @@ std::optional route(ways const&, direction, double max_match_distance, bitvec const* blocked = nullptr, - sharing_data const* sharing = nullptr); + sharing_data const* sharing = nullptr, + elevation_storage const* = nullptr); std::optional route(ways const&, search_profile, @@ -77,7 +82,8 @@ std::optional route(ways const&, cost_t const max, direction, bitvec const* blocked = nullptr, - sharing_data const* sharing = nullptr); + sharing_data const* sharing = nullptr, + elevation_storage const* = nullptr); std::vector> route( ways const&, @@ -90,6 +96,7 @@ std::vector> route( direction const, bitvec const* blocked = nullptr, sharing_data const* sharing = nullptr, + elevation_storage const* = nullptr, std::function const& do_reconstruct = [](path const&) { return false; }); diff --git a/include/osr/types.h b/include/osr/types.h index 0085c4c..c2d0feb 100644 --- a/include/osr/types.h +++ b/include/osr/types.h @@ -90,6 +90,8 @@ using multi_level_elevator_idx_t = cista::strong; using distance_t = std::uint16_t; +using elevation_monotonic_t = + cista::strong; using way_pos_t = std::uint8_t; diff --git a/src/elevation_storage.cc b/src/elevation_storage.cc new file mode 100644 index 0000000..d3da1cb --- /dev/null +++ b/src/elevation_storage.cc @@ -0,0 +1,367 @@ +#include "osr/elevation_storage.h" + +#include +#include +#include +#include +#include +#include + +#include "utl/enumerate.h" +#include "utl/pairwise.h" +#include "utl/parallel_for.h" +#include "utl/progress_tracker.h" +#include "utl/raii.h" + +#include "cista/strong.h" + +#include "osr/point.h" +#include "osr/preprocessing/elevation/provider.h" +#include "osr/preprocessing/elevation/shared.h" +#include "osr/preprocessing/elevation/resolution.h" + +namespace fs = std::filesystem; + +namespace osr { + +using preprocessing::elevation::elevation_meters_t; +using preprocessing::elevation::provider; +using preprocessing::elevation::resolution; +using preprocessing::elevation::tile_idx_t; + +using path_vec = std::vector; + +constexpr auto const kWriteMode = cista::mmap::protection::WRITE; + +cista::mmap mm(fs::path const& path, cista::mmap::protection const mode) { + return cista::mmap{path.string().data(), mode}; +} + +namespace elevation_files { +constexpr auto const kDataName = "elevation_data.bin"; +constexpr auto const kIndexName = "elevation_idx.bin"; +}; // namespace elevation_files + +elevation_storage::elevation_storage(fs::path const& p, + cista::mmap::protection const mode) + : elevations_{mm_vec{mm(p / elevation_files::kDataName, mode)}, + mm_vec>{ + mm(p / elevation_files::kIndexName, mode)}} {} + +std::unique_ptr elevation_storage::try_open( + fs::path const& path) { + for (auto const& filename : + {elevation_files::kDataName, elevation_files::kIndexName}) { + auto const full_path = path / filename; + if (!fs::exists(full_path)) { + std::cout << full_path << " does not exist\n"; + return nullptr; + } + } + return std::make_unique(path, + cista::mmap::protection::READ); +} + +elevation_storage::elevation get_way_elevation(provider const& provider, + point const& from, + point const& to, + resolution const& max_res) { + auto elevation = elevation_storage::elevation{}; + auto a = provider.get(from); + auto const b = provider.get(to); + if (a != elevation_meters_t::invalid() && + b != elevation_meters_t::invalid()) { + // TODO Approximation only for short ways + // Use slightly larger value to not skip intermediate values + constexpr auto const kSafetyFactor = 1.000001; + auto const min_lng_diff = + std::abs(180 - std::abs((to.lng() - from.lng()) - 180)); + auto const lat_diff = std::abs(to.lat() - from.lat()); + auto const steps = static_cast( + std::max(std::ceil(kSafetyFactor * lat_diff / max_res.y_), + std::ceil(kSafetyFactor * min_lng_diff / max_res.x_))); + if (steps > 1) { + auto const from_lat = from.lat(); + auto const from_lng = from.lng(); + auto const way_res = resolution{.x_ = (to.lat() - from_lat) / steps, + .y_ = (to.lng() - from_lng) / steps}; + for (auto s = 1; s < steps; ++s) { + auto const m = provider.get(point::from_latlng( + from_lat + s * way_res.x_, from_lng + s * way_res.y_)); + if (m != elevation_meters_t::invalid()) { + if (a < m) { + elevation.up_ += static_cast>( + to_idx(m - a)); + } else { + elevation.down_ += + static_cast>( + to_idx(a - m)); + } + a = m; + } + } + } + if (a < b) { + elevation.up_ += + static_cast>(to_idx(b - a)); + } else { + elevation.down_ += + static_cast>(to_idx(a - b)); + } + } + return elevation; +} + +struct way_ordering_t { + way_idx_t way_idx_; + tile_idx_t order_; +}; + +using way_ordering_vec = mm_vec; +way_ordering_vec calculate_way_order(path_vec& paths, + ways const& w, + provider const& provider, + utl::progress_tracker_ptr& pt) { + auto const& path = paths.emplace_back(fs::temp_directory_path() / + "temp_osr_extract_way_ordering"); + + auto ordering = mm_vec{mm(path, kWriteMode)}; + ordering.reserve(w.n_ways()); + + pt->in_high(w.n_ways()); + for (auto const [way_idx, way] : utl::enumerate(w.r_->way_nodes_)) { + if (!way.empty()) { + auto const node_point = w.get_node_pos(way.front()); + auto const tile_idx = provider.tile_idx(node_point); + if (tile_idx != tile_idx_t::invalid()) { + ordering.emplace_back(way_idx_t{way_idx}, std::move(tile_idx)); + } + } + pt->increment(); + } + + std::sort( +#if __cpp_lib_execution + std::execution::par_unseq, +#endif + begin(ordering), end(ordering), + [](way_ordering_t const& a, way_ordering_t const& b) { + return a.order_ < b.order_; + }); + + return ordering; +} + +mm_vec_map calculate_points(path_vec& paths, + ways const& w, + utl::progress_tracker_ptr& pt) { + auto const& path = + paths.emplace_back(fs::temp_directory_path() / "temp_osr_extract_points"); + auto points = + mm_vec_map{mm(path, cista::mmap::protection::WRITE)}; + + auto const size = w.n_nodes(); + points.resize(size); + pt->in_high(size); + + utl::parallel_for_run( + w.n_nodes(), + [&](std::size_t const& idx) { + auto const node_idx = node_idx_t{idx}; + if (!w.r_->node_ways_[node_idx].empty()) { + points[node_idx] = w.get_node_pos(node_idx); + } + }, + pt->update_fn()); + + return points; +} + +using sort_idx_t = cista::strong; + +struct mapping_t { + way_idx_t way_idx_; + sort_idx_t sort_idx_; +}; + +struct encoding_result_t { + osr::mm_vec mappings_; + mm_vecvec encodings_; +}; + +encoding_result_t calculate_way_encodings( + path_vec& paths, + ways const& w, + provider const& provider, + mm_vec const& ordering, + mm_vec_map const& points, + utl::progress_tracker_ptr& pt) { + paths.reserve(paths.size() + 3U); + auto const& mapping_path = paths.emplace_back(fs::temp_directory_path() / + "temp_osr_extract_mapping"); + auto const& encoding_data_path = paths.emplace_back( + fs::temp_directory_path() / "temp_osr_extract_unordered_encoding_data"); + auto const& encoding_idx_path = paths.emplace_back( + fs::temp_directory_path() / "temp_osr_extract_unordered_encoding_idx"); + + auto result = encoding_result_t{ + .mappings_ = osr::mm_vec{mm(mapping_path, kWriteMode)}, + .encodings_ = + osr::mm_vecvec{ + mm_vec{ + mm(encoding_data_path, kWriteMode)}, + mm_vec>{ + mm(encoding_idx_path, kWriteMode)}}, + }; + result.mappings_.reserve(ordering.size()); + auto const max_res = provider.max_resolution(); + auto m = std::mutex{}; + + pt->in_high(ordering.size()); + utl::parallel_for( + ordering, + [&](way_ordering_t const way_ordering) { + thread_local auto elevations = + std::vector{}; + + auto const& way_idx = way_ordering.way_idx_; + + // Calculate elevations + auto elevations_idx = std::size_t{0U}; + elevations.clear(); + for (auto const [from, to] : utl::pairwise(w.r_->way_nodes_[way_idx])) { + auto const elevation = elevation_storage::encoding{get_way_elevation( + provider, points[from], points[to], max_res)}; + if (elevation) { + elevations.resize(elevations_idx); + elevations.push_back(std::move(elevation)); + } + ++elevations_idx; + } + + // Store elevations unordered + if (!elevations.empty()) { + auto const lock = std::lock_guard{m}; + result.mappings_.emplace_back(way_idx, + sort_idx_t{result.encodings_.size()}); + result.encodings_.emplace_back(elevations); + } + }, + pt->update_fn()); + + return result; +} + +void write_ordered_encodings(elevation_storage& storage, + encoding_result_t&& result, + utl::progress_tracker_ptr& pt) { + std::sort( +#if __cpp_lib_execution + std::execution::par_unseq, +#endif + begin(result.mappings_), end(result.mappings_), + [](mapping_t const& a, mapping_t const& b) { + return a.way_idx_ < b.way_idx_; + }); + + pt->in_high(result.mappings_.size()); + for (auto const& mapping : result.mappings_) { + storage.elevations_.resize(to_idx(mapping.way_idx_) + 1U); + auto bucket = storage.elevations_.back(); + for (auto const e : result.encodings_[mapping.sort_idx_]) { + bucket.push_back(e); + } + pt->increment(); + } +} + +void elevation_storage::set_elevations(ways const& w, + provider const& provider) { + auto pt = utl::get_active_progress_tracker_or_activate("osr"); + auto cleanup_paths = utl::make_raii(path_vec{}, [](path_vec const& paths) { + auto e = std::error_code{}; + for (auto const& path : paths) { + fs::remove(path, e); + } + }); + + pt->status("Calculating way order").out_bounds(75, 79); + auto const processing_order = + calculate_way_order(cleanup_paths.get(), w, provider, pt); + pt->status("Precalculating way points").out_bounds(79, 81); + auto const points = calculate_points(cleanup_paths.get(), w, pt); + pt->status("Calculating way elevations").out_bounds(81, 89); + auto unordered_encodings = calculate_way_encodings( + cleanup_paths.get(), w, provider, processing_order, points, pt); + pt->status("Storing ordered elevations").out_bounds(89, 90); + write_ordered_encodings(*this, std::move(unordered_encodings), pt); +} + +elevation_storage::elevation elevation_storage::get_elevations( + way_idx_t const way, std::uint16_t const segment) const { + return (way < elevations_.size() && segment < elevations_[way].size()) + ? elevations_[way][segment].decode() + : elevation{}; +} + +elevation_storage::elevation get_elevations(elevation_storage const* elevations, + way_idx_t const way, + std::uint16_t const segment) { + return elevations == nullptr ? elevation_storage::elevation{} + : elevations->get_elevations(way, segment); +} + +elevation_storage::elevation& elevation_storage::elevation::operator+=( + elevation const& other) { + up_ += to_idx(other.up_); + down_ += to_idx(other.down_); + return *this; +} + +elevation_storage::elevation elevation_storage::elevation::swap() const { + return { + .up_ = down_, + .down_ = up_, + }; +} + +constexpr auto const kCompressedValues = std::array{ + elevation_monotonic_t{0U}, elevation_monotonic_t{1U}, + elevation_monotonic_t{2U}, elevation_monotonic_t{4U}, + elevation_monotonic_t{6U}, elevation_monotonic_t{8U}, + elevation_monotonic_t{11U}, elevation_monotonic_t{14U}, + elevation_monotonic_t{17U}, elevation_monotonic_t{21U}, + elevation_monotonic_t{25U}, elevation_monotonic_t{29U}, + elevation_monotonic_t{34U}, elevation_monotonic_t{38U}, + elevation_monotonic_t{43U}, elevation_monotonic_t{48U}}; + +elevation_storage::encoding::coding encode(elevation_monotonic_t const e) { + auto const c = std::ranges::lower_bound(kCompressedValues, e); + return (c == end(kCompressedValues)) + ? static_cast( + kCompressedValues.size() - 1) + : static_cast( + c - begin(kCompressedValues)); +} + +elevation_monotonic_t decode_helper( + elevation_storage::encoding::coding const c) { + assert(c < kCompressedValues.size()); + return kCompressedValues.at(c); +} + +elevation_storage::encoding::encoding(elevation const& e) + : up_{encode(e.up_)}, down_{encode(e.down_)} {} + +elevation_storage::elevation elevation_storage::encoding::decode() const { + return { + .up_ = decode_helper(up_), + .down_ = decode_helper(down_), + }; +} + +elevation_storage::encoding::operator bool() const { + return up_ != 0U || down_ != 0U; +} + +} // namespace osr diff --git a/src/extract.cc b/src/extract.cc index 4b16e95..7d208dd 100644 --- a/src/extract.cc +++ b/src/extract.cc @@ -28,9 +28,11 @@ #include "tiles/osm/hybrid_node_idx.h" #include "tiles/osm/tmp_file.h" +#include "osr/elevation_storage.h" #include "osr/extract/tags.h" #include "osr/lookup.h" #include "osr/platforms.h" +#include "osr/preprocessing/elevation/provider.h" #include "osr/ways.h" namespace osm = osmium; @@ -453,7 +455,8 @@ struct rel_ways_handler : public osm::handler::Handler { void extract(bool const with_platforms, fs::path const& in, - fs::path const& out) { + fs::path const& out, + fs::path const& elevation_dir) { auto ec = std::error_code{}; fs::remove_all(out, ec); if (!fs::is_directory(out)) { @@ -489,7 +492,7 @@ void extract(bool const with_platforms, w.node_way_counter_.reserve(12000000000); { // Collect node coordinates. - pt->status("Load OSM / Coordinates").in_high(file_size).out_bounds(0, 20); + pt->status("Load OSM / Coordinates").in_high(file_size).out_bounds(0, 15); auto node_idx_builder = tiles::hybrid_node_idx_builder{node_idx}; @@ -507,7 +510,7 @@ void extract(bool const with_platforms, auto elevator_nodes = hash_map{}; { // Extract streets, places, and areas. - pt->status("Load OSM / Ways").in_high(file_size).out_bounds(20, 50); + pt->status("Load OSM / Ways").in_high(file_size).out_bounds(15, 40); auto h = way_handler{w, pl.get(), rel_ways, elevator_nodes}; auto reader = @@ -539,6 +542,15 @@ void extract(bool const with_platforms, w.connect_ways(); + if (!elevation_dir.empty()) { + auto const provider = + osr::preprocessing::elevation::provider{elevation_dir}; + if (provider.driver_count() > 0) { + auto elevations = elevation_storage{out, cista::mmap::protection::WRITE}; + elevations.set_elevations(w, provider); + } + } + auto r = std::vector{}; { pt->status("Load OSM / Node Properties") diff --git a/src/lookup.cc b/src/lookup.cc index e49c097..f148285 100644 --- a/src/lookup.cc +++ b/src/lookup.cc @@ -48,8 +48,14 @@ match_t lookup::match(location const& query, return match(query, reverse, search_dir, max_match_distance, blocked); case search_profile::kBike: - return match(query, reverse, search_dir, max_match_distance, - blocked); + return match>(query, reverse, search_dir, + max_match_distance, blocked); + case search_profile::kBikeElevationLow: + return match>(query, reverse, search_dir, + max_match_distance, blocked); + case search_profile::kBikeElevationHigh: + return match>(query, reverse, search_dir, + max_match_distance, blocked); case search_profile::kCarParking: return match>(query, reverse, search_dir, max_match_distance, blocked); diff --git a/src/preprocessing/elevation/dem_driver.cc b/src/preprocessing/elevation/dem_driver.cc new file mode 100644 index 0000000..1e5de49 --- /dev/null +++ b/src/preprocessing/elevation/dem_driver.cc @@ -0,0 +1,64 @@ +#include "osr/preprocessing/elevation/dem_driver.h" + +#include + +namespace fs = std::filesystem; + +namespace osr::preprocessing::elevation { + +bool dem_driver::add_tile(fs::path const& path) { + auto const ext = path.extension().string(); + if (ext != ".hdr") { + return false; + } + + auto const idx = static_cast(tiles_.size()); + auto const& tile = tiles_.emplace_back(dem_tile{path}); + auto const box = tile.get_coord_box(); + auto const min = decltype(rtree_)::coord_t{box.min_lat_, box.min_lng_}; + auto const max = decltype(rtree_)::coord_t{box.max_lat_, box.max_lng_}; + rtree_.insert(min, max, idx); + return true; +} + +elevation_meters_t dem_driver::get(point const& point) const { + auto const p = decltype(rtree_)::coord_t{static_cast(point.lat()), + static_cast(point.lng())}; + auto elevation = elevation_meters_t::invalid(); + rtree_.search(p, p, + [&](auto const&, auto const&, std::size_t const& tile_idx) { + elevation = tiles_[tile_idx].get(point); + return elevation != elevation_meters_t::invalid(); + }); + return elevation; +} + +tile_idx_t dem_driver::tile_idx(point const& point) const { + auto const p = decltype(rtree_)::coord_t{static_cast(point.lat()), + static_cast(point.lng())}; + auto idx = tile_idx_t::invalid(); + rtree_.search(p, p, + [&](auto const&, auto const&, std::size_t const& tile_idx) { + idx = tiles_[tile_idx].tile_idx(point); + if (idx == tile_idx_t::invalid()) { + return false; + } + idx.tile_idx_ = static_cast(tile_idx); + return true; + }); + return idx; +} + +resolution dem_driver::max_resolution() const { + auto res = resolution{.x_ = std::numeric_limits::quiet_NaN(), + .y_ = std::numeric_limits::quiet_NaN()}; + for (auto const& tile : tiles_) { + auto const r = tile.max_resolution(); + res.update(r); + } + return res; +} + +std::size_t dem_driver::n_tiles() const { return tiles_.size(); } + +} // namespace osr::preprocessing::elevation diff --git a/src/preprocessing/elevation/dem_tile.cc b/src/preprocessing/elevation/dem_tile.cc new file mode 100644 index 0000000..7f18b5c --- /dev/null +++ b/src/preprocessing/elevation/dem_tile.cc @@ -0,0 +1,268 @@ +#include "osr/preprocessing/elevation/dem_tile.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "boost/algorithm/string/case_conv.hpp" + +#include "cista/mmap.h" +#include "cista/strong.h" + +#include "osr/preprocessing/elevation/shared.h" + +// EHdr / BIL File Format: +// http://www.gdal.org/frmt_various.html#EHdr +// http://downloads.esri.com/support/whitepapers/other_/eximgav.pdf +// http://desktop.arcgis.com/en/arcmap/10.3/manage-data/raster-and-images/bil-bip-and-bsq-raster-files.htm + +namespace fs = std::filesystem; + +namespace osr::preprocessing::elevation { + +constexpr auto const kNoData = std::int16_t{-32768}; + +using str_map = std::unordered_map; +using dem_exception = std::runtime_error; + +str_map read_hdr_file(fs::path const& filename) { + str_map map; + + auto f = std::ifstream{filename}; + while (f.good()) { + std::string key, value; + f >> key >> value; + if (f) { + boost::to_upper(key); + boost::to_upper(value); + map[key] = value; + } + } + + return map; +} + +std::string get_string(str_map const& map, + std::string const& key, + std::string const& def = "") { + auto const r = map.find(key); + if (r != end(map)) { + auto const& val = r->second; + return val.empty() ? def : val; + } else { + return def; + } +} + +int get_int(str_map const& map, std::string const& key, int def) { + auto const str = get_string(map, key); + return str.empty() ? def : std::stoi(str); +} + +unsigned get_uint(str_map const& map, std::string const& key, unsigned def) { + auto const str = get_string(map, key); + return str.empty() ? def : static_cast(std::stoul(str)); +} + +double get_double(str_map const& map, std::string const& key, double def) { + auto const str = get_string(map, key); + return str.empty() ? def : std::stod(str); +} + +struct bil_header { + explicit bil_header(fs::path const& bil_path) { + auto const hdr_path = fs::path{bil_path.parent_path() / + fs::path(bil_path.stem().string() + ".hdr")}; + if (!fs::exists(hdr_path)) { + throw dem_exception("Missing hdr file: " + hdr_path.string()); + } + auto const hdr = read_hdr_file(hdr_path); + init_hdr(hdr); + } + + void init_hdr(str_map const& hdr) { + rows_ = get_uint(hdr, "NROWS", 0); + cols_ = get_uint(hdr, "NCOLS", 0); + if (rows_ == 0 || cols_ == 0) { + throw dem_exception("Missing nrows/ncols"); + } + + if (get_uint(hdr, "NBANDS", 1) != 1) { + throw dem_exception("Unsupported nbands value"); + } + if (get_string(hdr, "BYTEORDER", "I") != "I") { + throw dem_exception("Unsupported byte order"); + } + if (get_uint(hdr, "SKIPBYTES", 0) != 0) { + throw dem_exception("Unsupported skipbytes"); + } + + auto const nbits = get_uint(hdr, "NBITS", 8); + auto const pixeltype = get_string(hdr, "PIXELTYPE", "UNSIGNEDINT"); + if (nbits == 16 && pixeltype[0] == 'S') { + pixel_type_ = pixel_type::int16; + } else if (nbits == 32 && pixeltype[0] == 'F') { + pixel_type_ = pixel_type::float32; + } else { + throw dem_exception("Unsupported pixeltype"); + } + pixel_size_ = nbits / 8; + row_size_ = pixel_size_ * cols_; + + ulx_ = get_double(hdr, "ULXMAP", std::numeric_limits::min()); + uly_ = get_double(hdr, "ULYMAP", std::numeric_limits::min()); + if (std::equal_to<>()(ulx_, std::numeric_limits::min()) || + std::equal_to<>()(uly_, std::numeric_limits::min())) { + throw dem_exception("Missing ulxmap/ulymap"); + } + + xdim_ = get_double(hdr, "XDIM", 0); + ydim_ = get_double(hdr, "YDIM", 0); + if (std::equal_to<>()(xdim_, 0.0) || std::equal_to<>()(ydim_, 0.0)) { + throw dem_exception("Missing xdim/ydim"); + } + brx_ = ulx_ + cols_ * xdim_; + bry_ = uly_ - rows_ * ydim_; + + switch (pixel_type_) { + case pixel_type::int16: + nodata_.int16_ = static_cast(get_int(hdr, "NODATA", 0)); + break; + case pixel_type::float32: + nodata_.float32_ = static_cast(get_double(hdr, "NODATA", 0)); + break; + } + + auto const bandrowbytes = get_uint(hdr, "BANDROWBYTES", row_size_); + auto const totalrowbytes = get_uint(hdr, "TOTALROWBYTES", row_size_); + if (bandrowbytes != row_size_ || totalrowbytes != row_size_) { + throw dem_exception("Unsupported bandrowbytes/totalrowbytes"); + } + } + + unsigned rows_{0}; + unsigned cols_{0}; + double ulx_{0}; // upper left lon + double uly_{0}; // upper left lat + double brx_{0}; // bottom right lon + double bry_{0}; // bottom right lat + double xdim_{0}; // x pixel dimension, degrees + double ydim_{0}; // y pixel dimension, degrees + unsigned pixel_size_{0}; // bytes per pixel + unsigned row_size_{0}; // bytes per row + pixel_type pixel_type_{pixel_type::int16}; + pixel_value nodata_{}; +}; + +fs::path get_bil_path(fs::path const& path) { + auto const bil_path = + fs::path{path.parent_path() / fs::path(path.stem().string() + ".bil")}; + if (!fs::exists(bil_path)) { + throw dem_exception("Missing bil file: " + bil_path.string()); + } + return bil_path; +} + +struct dem_tile::impl { + explicit impl(fs::path const& path) + : data_file_{get_bil_path(path)}, + hdr_{data_file_}, + mapped_file_{cista::mmap{data_file_.string().data(), + cista::mmap::protection::READ}} {} + + pixel_value get(point const& p) { + auto const lng = p.lng(); + auto const lat = p.lat(); + + if (lng < hdr_.ulx_ || lat > hdr_.uly_ || lng >= hdr_.brx_ || + lat <= hdr_.bry_) { + return hdr_.nodata_; + } + + auto const pix_x = static_cast((lng - hdr_.ulx_) / hdr_.xdim_); + auto const pix_y = static_cast((hdr_.uly_ - lat) / hdr_.ydim_); + assert(pix_x < hdr_.cols_); + assert(pix_y < hdr_.rows_); + auto const byte_pos = hdr_.row_size_ * pix_y + hdr_.pixel_size_ * pix_x; + + assert(byte_pos < mapped_file_.size()); + auto const* byte_ptr = mapped_file_.data() + byte_pos; + + pixel_value val{}; + + switch (hdr_.pixel_type_) { + case pixel_type::int16: + val.int16_ = *reinterpret_cast(byte_ptr); + break; + case pixel_type::float32: + val.float32_ = *reinterpret_cast(byte_ptr); + break; + } + + return val; + } + + fs::path data_file_; + bil_header hdr_; + cista::mmap mapped_file_; +}; + +dem_tile::dem_tile(fs::path const& filename) + : impl_(std::make_unique(filename)) {} + +dem_tile::dem_tile(dem_tile&& grid) noexcept : impl_(std::move(grid.impl_)) {} + +dem_tile::~dem_tile() = default; + +elevation_meters_t dem_tile::get(point const& p) const { + auto const val = get_raw(p); + switch (impl_->hdr_.pixel_type_) { + case pixel_type::int16: + if (val.int16_ == impl_->hdr_.nodata_.int16_) { + return elevation_meters_t::invalid(); + } else { + auto const value = val.int16_; + return value != kNoData ? elevation_meters_t{value} + : elevation_meters_t::invalid(); + } + case pixel_type::float32: + if (std::equal_to<>()(val.float32_, impl_->hdr_.nodata_.float32_)) { + return elevation_meters_t::invalid(); + } else { + auto const value = static_cast>( + std::round(val.float32_)); + return value != kNoData ? elevation_meters_t{value} + : elevation_meters_t::invalid(); + } + } + throw std::runtime_error{"dem_grid: invalid pixel type"}; +} + +tile_idx_t dem_tile::tile_idx(point const&) const { + // TODO Return non trivial sub tile index + return tile_idx_t::from_sub_tile(0U); +} + +coord_box dem_tile::get_coord_box() const { + return { + .min_lat_ = static_cast(impl_->hdr_.bry_), + .min_lng_ = static_cast(impl_->hdr_.ulx_), + .max_lat_ = static_cast(impl_->hdr_.uly_), + .max_lng_ = static_cast(impl_->hdr_.brx_), + }; +} + +pixel_value dem_tile::get_raw(point const& p) const { return impl_->get(p); } + +pixel_type dem_tile::get_pixel_type() const { return impl_->hdr_.pixel_type_; } + +resolution dem_tile::max_resolution() const { + return {.x_ = impl_->hdr_.xdim_, .y_ = impl_->hdr_.ydim_}; +} + +} // namespace osr::preprocessing::elevation diff --git a/src/preprocessing/elevation/hgt_driver.cc b/src/preprocessing/elevation/hgt_driver.cc new file mode 100644 index 0000000..74294be --- /dev/null +++ b/src/preprocessing/elevation/hgt_driver.cc @@ -0,0 +1,120 @@ +#include "osr/preprocessing/elevation/hgt_driver.h" + +#include +#include + +#include "utl/verify.h" + +namespace fs = std::filesystem; + +namespace osr::preprocessing::elevation { + +struct grid_point { + explicit grid_point(std::string const& filename) { + auto lat_dir = char{}; + auto lng_dir = char{}; + auto lat = int{}; + auto lng = int{}; + + auto s = std::stringstream{filename}; + s >> lat_dir >> lat >> lng_dir >> lng; + + utl::verify(lat_dir == 'S' || lat_dir == 'N', "Invalid direction '{}'", + lat_dir); + utl::verify(lng_dir == 'W' || lng_dir == 'E', "Invalid direction '{}'", + lng_dir); + utl::verify(-180 <= lng && lng <= 180, "Invalid longitude '{}'", lng); + utl::verify(-90 <= lat && lat <= 90, "Invalid latitude '{}'", lat); + + lat_ = static_cast((lat_dir == 'N') ? lat : -lat); + lng_ = static_cast((lng_dir == 'E') ? lng : -lng); + + utl::verify(-180 <= lng_ && lng_ < 180, "Invalid longitude '{}'", lng); + utl::verify(-90 <= lat_ && lat_ < 90, "Invalid latitude '{}'", lat); + } + + std::int8_t lat_; + std::int16_t lng_; +}; + +bool hgt_driver::add_tile(fs::path const& path) { + auto const ext = path.extension().string(); + if (ext != ".hgt") { + return false; + } + auto tile = open(path); + if (tile.has_value()) { + auto const box = std::visit( + [](IsTile auto const& t) { return t.get_coord_box(); }, *tile); + auto const min = decltype(rtree_)::coord_t{box.min_lat_, box.min_lng_}; + auto const max = decltype(rtree_)::coord_t{box.max_lat_, box.max_lng_}; + rtree_.insert(min, max, static_cast(tiles_.size())); + tiles_.emplace_back(std::move(tile.value())); + return true; + } else { + return false; + } +} + +elevation_meters_t hgt_driver::get(point const& point) const { + auto const p = decltype(rtree_)::coord_t{static_cast(point.lat()), + static_cast(point.lng())}; + auto elevation = elevation_meters_t::invalid(); + rtree_.search(p, p, + [&](auto const&, auto const&, std::size_t const& tile_idx) { + elevation = std::visit( + [&](IsTile auto const& t) -> elevation_meters_t { + return t.get(point); + }, + tiles_[tile_idx]); + return elevation != elevation_meters_t::invalid(); + }); + return elevation; +} + +tile_idx_t hgt_driver::tile_idx(point const& point) const { + auto const p = decltype(rtree_)::coord_t{static_cast(point.lat()), + static_cast(point.lng())}; + auto idx = tile_idx_t::invalid(); + rtree_.search(p, p, + [&](auto const&, auto const&, std::size_t const& tile_idx) { + idx = std::visit( + [&](IsTile auto const& t) -> tile_idx_t { + return t.tile_idx(point); + }, + tiles_[tile_idx]); + if (idx == tile_idx_t::invalid()) { + return false; + } + idx.tile_idx_ = static_cast(tile_idx); + return true; + }); + return idx; +} + +resolution hgt_driver::max_resolution() const { + auto res = resolution{}; + for (auto const& tile : tiles_) { + auto const r = std::visit( + [](IsTile auto const& t) { return t.max_resolution(); }, tile); + res.update(r); + } + return res; +} + +std::size_t hgt_driver::n_tiles() const { return tiles_.size(); } + +std::optional hgt_driver::open(fs::path const& path) { + auto const filename = path.filename(); + auto sw = grid_point{path.filename().string()}; + auto const file_size = fs::file_size(path); + switch (file_size) { + case hgt_tile<3601>::file_size(): + return hgt_tile<3601>{path.string(), sw.lat_, sw.lng_}; + case hgt_tile<1201>::file_size(): + return hgt_tile<1201>{path.string(), sw.lat_, sw.lng_}; + default: return {}; + } +} + +} // namespace osr::preprocessing::elevation diff --git a/src/preprocessing/elevation/hgt_tile.cc b/src/preprocessing/elevation/hgt_tile.cc new file mode 100644 index 0000000..1d53b7c --- /dev/null +++ b/src/preprocessing/elevation/hgt_tile.cc @@ -0,0 +1,9 @@ +#include "osr/preprocessing/elevation/hgt_tile.h" +#include "osr/preprocessing/elevation/hgt_tile_def.h" + +namespace osr::preprocessing::elevation { + +template struct hgt_tile<3601U>; +template struct hgt_tile<1201U>; + +} // namespace osr::preprocessing::elevation diff --git a/src/preprocessing/elevation/provider.cc b/src/preprocessing/elevation/provider.cc new file mode 100644 index 0000000..0094acc --- /dev/null +++ b/src/preprocessing/elevation/provider.cc @@ -0,0 +1,102 @@ +#include "osr/preprocessing/elevation/provider.h" + +#include +#include +#include +#include + +#include "osr/elevation_storage.h" +#include "osr/preprocessing/elevation/dem_driver.h" +#include "osr/preprocessing/elevation/hgt_driver.h" + +namespace osr::preprocessing::elevation { + +static_assert(IsDriver); +static_assert(IsDriver); + +using raster_driver = std::variant; + +struct provider::impl { + impl() = default; + + void add_driver(IsDriver auto&& driver) { + drivers_.emplace_back(std::move(driver)); + } + + elevation_meters_t get(point const& p) { + for (auto const& grid : drivers_) { + auto const data = std::visit( + [&](IsDriver auto const& driver) { return driver.get(p); }, grid); + if (data != elevation_meters_t::invalid()) { + return data; + } + } + return elevation_meters_t::invalid(); + } + + tile_idx_t tile_idx(point const& p) const { + for (auto driver_idx = tile_idx_t::data_t{0U}; driver_idx < drivers_.size(); + ++driver_idx) { + auto idx = + std::visit([&](IsDriver auto const& d) { return d.tile_idx(p); }, + drivers_[driver_idx]); + if (idx != tile_idx_t::invalid()) { + idx.driver_idx_ = driver_idx; + return idx; + } + } + return tile_idx_t::invalid(); + } + + std::vector drivers_; +}; + +provider::provider(std::filesystem::path const& p) + : impl_(std::make_unique()) { + if (std::filesystem::is_directory(p)) { + auto dem = dem_driver{}; + auto hgt = hgt_driver{}; + for (auto const& file : std::filesystem::recursive_directory_iterator(p)) { + [&]() { + if (!file.is_regular_file()) { + return; + } + auto const& path = file.path(); + if (dem.add_tile(path)) { + return; + } + if (hgt.add_tile(path)) { + return; + } + }(); + } + if (dem.n_tiles() > 0U) { + impl_->add_driver(std::move(dem)); + } + if (hgt.n_tiles() > 0U) { + impl_->add_driver(std::move(hgt)); + } + } +} + +provider::~provider() = default; + +elevation_meters_t provider::get(point const& p) const { return impl_->get(p); } + +tile_idx_t provider::tile_idx(point const& p) const { + return impl_->tile_idx(p); +} + +std::size_t provider::driver_count() const { return impl_->drivers_.size(); } + +resolution provider::max_resolution() const { + auto res = resolution{}; + for (auto const& driver : impl_->drivers_) { + auto const r = std::visit( + [](IsDriver auto const& d) { return d.max_resolution(); }, driver); + res.update(r); + } + return res; +} + +} // namespace osr::preprocessing::elevation diff --git a/src/route.cc b/src/route.cc index ce4759b..55516b0 100644 --- a/src/route.cc +++ b/src/route.cc @@ -1,11 +1,15 @@ #include "osr/routing/route.h" +#include +#include + #include "boost/thread/tss.hpp" #include "utl/concat.h" #include "utl/to_vec.h" #include "utl/verify.h" +#include "osr/elevation_storage.h" #include "osr/routing/dijkstra.h" #include "osr/routing/profiles/bike.h" #include "osr/routing/profiles/bike_sharing.h" @@ -25,6 +29,7 @@ struct connecting_way { std::uint16_t from_{}, to_{}; bool is_loop_{}; std::uint16_t distance_{}; + elevation_storage::elevation elevation_; }; template @@ -32,20 +37,22 @@ connecting_way find_connecting_way(ways const& w, ways::routing const& r, bitvec const* blocked, sharing_data const* sharing, + elevation_storage const* elevations, typename Profile::node const from, typename Profile::node const to, cost_t const expected_cost) { auto conn = std::optional{}; Profile::template adjacent( - r, from, blocked, sharing, + r, from, blocked, sharing, elevations, [&](typename Profile::node const target, std::uint32_t const cost, distance_t const dist, way_idx_t const way, std::uint16_t const a_idx, - std::uint16_t const b_idx) { + std::uint16_t const b_idx, + elevation_storage::elevation const elevation) { if (target == to && cost == expected_cost) { auto const is_loop = way != way_idx_t::invalid() && r.is_loop(way) && static_cast(std::abs(a_idx - b_idx)) == r.way_nodes_[way].size() - 2U; - conn = {way, a_idx, b_idx, is_loop, dist}; + conn = {way, a_idx, b_idx, is_loop, dist, std::move(elevation)}; } }); utl::verify( @@ -63,6 +70,7 @@ template connecting_way find_connecting_way(ways const& w, bitvec const* blocked, sharing_data const* sharing, + elevation_storage const* elevations, typename Profile::node const from, typename Profile::node const to, cost_t const expected_cost, @@ -70,10 +78,10 @@ connecting_way find_connecting_way(ways const& w, auto const call = [&]() { if (dir == direction::kForward) { return find_connecting_way( - w, *w.r_, blocked, sharing, from, to, expected_cost); + w, *w.r_, blocked, sharing, elevations, from, to, expected_cost); } else { return find_connecting_way( - w, *w.r_, blocked, sharing, from, to, expected_cost); + w, *w.r_, blocked, sharing, elevations, from, to, expected_cost); } }; @@ -89,20 +97,22 @@ double add_path(ways const& w, ways::routing const& r, bitvec const* blocked, sharing_data const* sharing, + elevation_storage const* elevations, typename Profile::node const from, typename Profile::node const to, cost_t const expected_cost, std::vector& path, direction const dir) { - auto const& [way, from_idx, to_idx, is_loop, distance] = - find_connecting_way(w, blocked, sharing, from, to, expected_cost, - dir); + auto const& [way, from_idx, to_idx, is_loop, distance, elevation] = + find_connecting_way(w, blocked, sharing, elevations, from, to, + expected_cost, dir); auto j = 0U; auto active = false; auto& segment = path.emplace_back(); segment.way_ = way; segment.dist_ = distance; segment.cost_ = expected_cost; + segment.elevation_ = elevation; segment.mode_ = to.get_mode(); if (way != way_idx_t::invalid()) { @@ -148,6 +158,7 @@ template path reconstruct(ways const& w, bitvec const* blocked, sharing_data const* sharing, + elevation_storage const* elevations, dijkstra const& d, way_candidate const& start, node_candidate const& dest, @@ -172,8 +183,8 @@ path reconstruct(ways const& w, if (pred.has_value()) { auto const expected_cost = static_cast(e.cost(n) - d.get_cost(*pred)); - dist += add_path(w, *w.r_, blocked, sharing, *pred, n, - expected_cost, segments, dir); + dist += add_path(w, *w.r_, blocked, sharing, elevations, *pred, + n, expected_cost, segments, dir); } else { break; } @@ -194,8 +205,13 @@ path reconstruct(ways const& w, .dist_ = static_cast(start_node.dist_to_node_), .mode_ = n.get_mode()}); std::reverse(begin(segments), end(segments)); + auto path_elevation = elevation_storage::elevation{}; + for (auto const& segment : segments) { + path_elevation += segment.elevation_; + } auto p = path{.cost_ = cost, .dist_ = start_node.dist_to_node_ + dist + dest.dist_to_node_, + .elevation_ = path_elevation, .segments_ = segments}; d.cost_.at(dest_node.get_key()).write(dest_node, p); return p; @@ -289,7 +305,8 @@ std::optional route(ways const& w, cost_t const max, direction const dir, bitvec const* blocked, - sharing_data const* sharing) { + sharing_data const* sharing, + elevation_storage const* elevations) { if (auto const direct = try_direct(from, to); direct.has_value()) { return *direct; } @@ -309,13 +326,13 @@ std::optional route(ways const& w, continue; } - d.run(w, *w.r_, max, blocked, sharing, dir); + d.run(w, *w.r_, max, blocked, sharing, elevations, dir); auto const c = best_candidate(w, d, to.lvl_, to_match, max, dir); if (c.has_value()) { auto const [nc, wc, node, p] = *c; - return reconstruct(w, blocked, sharing, d, start, *nc, node, - p.cost_, dir); + return reconstruct(w, blocked, sharing, elevations, d, start, + *nc, node, p.cost_, dir); } } @@ -334,6 +351,7 @@ std::vector> route( direction const dir, bitvec const* blocked, sharing_data const* sharing, + elevation_storage const* elevations, std::function const& do_reconstruct) { auto result = std::vector>{}; result.resize(to_match.size()); @@ -355,7 +373,7 @@ std::vector> route( } } - d.run(w, *w.r_, max, blocked, sharing, dir); + d.run(w, *w.r_, max, blocked, sharing, elevations, dir); auto found = 0U; for (auto const [m, t, r] : utl::zip(to_match, to, result)) { @@ -369,8 +387,8 @@ std::vector> route( auto [nc, wc, n, p] = *c; d.cost_.at(n.get_key()).write(n, p); if (do_reconstruct(p)) { - p = reconstruct(w, blocked, sharing, d, start, *nc, n, - p.cost_, dir); + p = reconstruct(w, blocked, sharing, elevations, d, start, + *nc, n, p.cost_, dir); p.uses_elevator_ = true; } r = std::make_optional(p); @@ -398,6 +416,7 @@ std::vector> route( double const max_match_distance, bitvec const* blocked, sharing_data const* sharing, + elevation_storage const* elevations, std::function const& do_reconstruct) { auto const r = [&]( dijkstra& d) -> std::vector> { @@ -410,7 +429,7 @@ std::vector> route( return l.match(x, true, dir, max_match_distance, blocked); }); return route(w, d, from, to, from_match, to_match, max, dir, blocked, - sharing, do_reconstruct); + sharing, elevations, do_reconstruct); }; switch (profile) { @@ -418,7 +437,12 @@ std::vector> route( return r(get_dijkstra>()); case search_profile::kWheelchair: return r(get_dijkstra>()); - case search_profile::kBike: return r(get_dijkstra()); + case search_profile::kBike: + return r(get_dijkstra>()); + case search_profile::kBikeElevationLow: + return r(get_dijkstra>()); + case search_profile::kBikeElevationHigh: + return r(get_dijkstra>()); case search_profile::kCar: return r(get_dijkstra()); case search_profile::kCarParking: return r(get_dijkstra>()); @@ -439,7 +463,8 @@ std::optional route(ways const& w, direction const dir, double const max_match_distance, bitvec const* blocked, - sharing_data const* sharing) { + sharing_data const* sharing, + elevation_storage const* elevations) { auto const r = [&](dijkstra& d) -> std::optional { auto const from_match = @@ -452,7 +477,7 @@ std::optional route(ways const& w, } return route(w, d, from, to, from_match, to_match, max, dir, blocked, - sharing); + sharing, elevations); }; switch (profile) { @@ -460,7 +485,12 @@ std::optional route(ways const& w, return r(get_dijkstra>()); case search_profile::kWheelchair: return r(get_dijkstra>()); - case search_profile::kBike: return r(get_dijkstra()); + case search_profile::kBike: + return r(get_dijkstra>()); + case search_profile::kBikeElevationLow: + return r(get_dijkstra>()); + case search_profile::kBikeElevationHigh: + return r(get_dijkstra>()); case search_profile::kCar: return r(get_dijkstra()); case search_profile::kCarParking: return r(get_dijkstra>()); @@ -483,6 +513,7 @@ std::vector> route( direction const dir, bitvec const* blocked, sharing_data const* sharing, + elevation_storage const* elevations, std::function const& do_reconstruct) { if (from_match.empty()) { return std::vector>(to.size()); @@ -491,7 +522,7 @@ std::vector> route( auto const r = [&]( dijkstra& d) -> std::vector> { return route(w, d, from, to, from_match, to_match, max, dir, blocked, - sharing, do_reconstruct); + sharing, elevations, do_reconstruct); }; switch (profile) { @@ -499,7 +530,12 @@ std::vector> route( return r(get_dijkstra>()); case search_profile::kWheelchair: return r(get_dijkstra>()); - case search_profile::kBike: return r(get_dijkstra()); + case search_profile::kBike: + return r(get_dijkstra>()); + case search_profile::kBikeElevationLow: + return r(get_dijkstra>()); + case search_profile::kBikeElevationHigh: + return r(get_dijkstra>()); case search_profile::kCar: return r(get_dijkstra()); case search_profile::kCarParking: return r(get_dijkstra>()); @@ -520,7 +556,8 @@ std::optional route(ways const& w, cost_t const max, direction const dir, bitvec const* blocked, - sharing_data const* sharing) { + sharing_data const* sharing, + elevation_storage const* elevations) { if (from_match.empty() || to_match.empty()) { return std::nullopt; } @@ -528,7 +565,7 @@ std::optional route(ways const& w, auto const r = [&](dijkstra& d) -> std::optional { return route(w, d, from, to, from_match, to_match, max, dir, blocked, - sharing); + sharing, elevations); }; switch (profile) { @@ -536,7 +573,12 @@ std::optional route(ways const& w, return r(get_dijkstra>()); case search_profile::kWheelchair: return r(get_dijkstra>()); - case search_profile::kBike: return r(get_dijkstra()); + case search_profile::kBike: + return r(get_dijkstra>()); + case search_profile::kBikeElevationLow: + return r(get_dijkstra>()); + case search_profile::kBikeElevationHigh: + return r(get_dijkstra>()); case search_profile::kCar: return r(get_dijkstra()); case search_profile::kCarParking: return r(get_dijkstra>()); diff --git a/src/routing/profile.cc b/src/routing/profile.cc index 7e8d192..eb41394 100644 --- a/src/routing/profile.cc +++ b/src/routing/profile.cc @@ -11,6 +11,10 @@ search_profile to_profile(std::string_view s) { case cista::hash("foot"): return search_profile::kFoot; case cista::hash("wheelchair"): return search_profile::kWheelchair; case cista::hash("bike"): return search_profile::kBike; + case cista::hash("bike_elevation_low"): + return search_profile::kBikeElevationLow; + case cista::hash("bike_elevation_high"): + return search_profile::kBikeElevationHigh; case cista::hash("car"): return search_profile::kCar; case cista::hash("car_parking"): return search_profile::kCarParking; case cista::hash("car_parking_wheelchair"): @@ -26,6 +30,8 @@ std::string_view to_str(search_profile const p) { case search_profile::kWheelchair: return "wheelchair"; case search_profile::kCar: return "car"; case search_profile::kBike: return "bike"; + case search_profile::kBikeElevationLow: return "bike_elevation_low"; + case search_profile::kBikeElevationHigh: return "bike_elevation_high"; case search_profile::kCarParking: return "car_parking"; case search_profile::kCarParkingWheelchair: return "car_parking_wheelchair"; case search_profile::kBikeSharing: return "bike_sharing"; diff --git a/src/ways.cc b/src/ways.cc index ec0c41f..cb724a1 100644 --- a/src/ways.cc +++ b/src/ways.cc @@ -58,7 +58,7 @@ void ways::connect_ways() { { // Assign graph node ids to every node with >1 way. pt->status("Create graph nodes") .in_high(node_way_counter_.size()) - .out_bounds(50, 60); + .out_bounds(40, 50); auto node_idx = node_idx_t{0U}; node_way_counter_.multi_.for_each_set_bit([&](std::uint64_t const b_idx) { @@ -74,7 +74,7 @@ void ways::connect_ways() { { pt->status("Connect ways") .in_high(way_osm_nodes_.size()) - .out_bounds(60, 90); + .out_bounds(50, 75); auto node_ways = mm_paged_vecvec{ cista::paged>{ mm_vec32{mm("tmp_node_ways_data.bin")}}, diff --git a/test/lvl_wildcard_test.cc b/test/lvl_wildcard_test.cc index aee7067..07c2a3d 100644 --- a/test/lvl_wildcard_test.cc +++ b/test/lvl_wildcard_test.cc @@ -30,7 +30,7 @@ TEST(routing, no_lvl_wildcard) { fs::remove_all(kTestFolder, ec); fs::create_directories(kTestFolder, ec); - extract(false, "test/stuttgart.osm.pbf", kTestFolder); + extract(false, "test/stuttgart.osm.pbf", kTestFolder, {}); auto w = osr::ways{kTestFolder, cista::mmap::protection::READ}; auto l = osr::lookup{w, kTestFolder, cista::mmap::protection::READ}; diff --git a/test/restriction_test.cc b/test/restriction_test.cc index 3ef2fe5..94221be 100644 --- a/test/restriction_test.cc +++ b/test/restriction_test.cc @@ -6,11 +6,17 @@ #include +#include "cista/mmap.h" + #include "fmt/core.h" #include "fmt/ranges.h" #include "osr/extract/extract.h" +#include "osr/location.h" #include "osr/lookup.h" +#include "osr/routing/profile.h" +#include "osr/routing/route.h" +#include "osr/types.h" #include "osr/ways.h" namespace fs = std::filesystem; @@ -22,17 +28,50 @@ TEST(extract, wa) { fs::remove_all(p, ec); fs::create_directories(p, ec); - osr::extract(false, "test/map.osm", "/tmp/osr_test"); + constexpr auto const kTestDir = "/tmp/osr_test"; + osr::extract(false, "test/map.osm", kTestDir, + "test/restriction_test_elevation/"); auto w = osr::ways{"/tmp/osr_test", cista::mmap::protection::READ}; auto l = osr::lookup{w, "/tmp/osr_test", cista::mmap::protection::READ}; + auto const elevations = elevation_storage::try_open(kTestDir); + ASSERT_TRUE(elevations); auto const n = w.find_node_idx(osm_node_idx_t{528944}); auto const rhoenring = w.find_way(osm_way_idx_t{120682496}); auto const arheilger = w.find_way(osm_way_idx_t{1068971150}); + // Crossing Eckhardt / Barkhaus + auto const n_dst = w.find_node_idx(osm_node_idx_t{586157}); + + ASSERT_TRUE(n.has_value()); + ASSERT_TRUE(n_dst.has_value()); + auto const from = location{w.get_node_pos(*n), kNoLevel}; + auto const to = location{w.get_node_pos(*n_dst), kNoLevel}; + constexpr auto const kMaxCost = cost_t{3600}; + constexpr auto const kMaxMatchDistance = 100; + auto const route_no_costs = route( + w, l, search_profile::kBike, from, to, kMaxCost, direction::kForward, + kMaxMatchDistance, nullptr, nullptr, elevations.get()); + auto const route_high_costs = + route(w, l, search_profile::kBikeElevationHigh, from, to, kMaxCost, + direction::kForward, kMaxMatchDistance, nullptr, nullptr, + elevations.get()); auto const is_restricted = w.r_->is_restricted( n.value(), w.r_->get_way_pos(n.value(), rhoenring.value()), w.r_->get_way_pos(n.value(), arheilger.value())); EXPECT_TRUE(is_restricted); + + constexpr auto const kShortestDistance = 163.0; + ASSERT_TRUE(route_no_costs.has_value()); + EXPECT_TRUE(std::abs(route_no_costs->dist_ - kShortestDistance) < 2.0); + // Upper bounds for elevations on each segment + EXPECT_EQ(elevation_monotonic_t{4U + 1U}, route_no_costs->elevation_.up_); + EXPECT_EQ(elevation_monotonic_t{0U + 6U}, route_no_costs->elevation_.down_); + + ASSERT_TRUE(route_high_costs.has_value()); + EXPECT_TRUE(route_high_costs->dist_ - kShortestDistance > 2.0); + // Upper bounds for elevations on each segment + EXPECT_EQ(elevation_monotonic_t{1U + 0U}, route_high_costs->elevation_.up_); + EXPECT_EQ(elevation_monotonic_t{4U + 0U}, route_high_costs->elevation_.down_); } diff --git a/test/restriction_test_elevation/create_data.py b/test/restriction_test_elevation/create_data.py new file mode 100755 index 0000000..1004d5a --- /dev/null +++ b/test/restriction_test_elevation/create_data.py @@ -0,0 +1,126 @@ +#!/usr/bin/env python3 +#-*- coding: utf8 -*- + +import sys + +BYTE_ORDER = 'little' + +def main(): + BASE = int.from_bytes(b'A_', byteorder=BYTE_ORDER, signed=True) + ( + Tile(tl=Point(8.65617, 49.8838761, ), br=Point(8.6575625, 49.8827837, ), size=Pos(7, 5), default=BASE) + # Start point + .set(Pos(6, 1), BASE + 2) + # Shorter path with higher elevation + .set(Pos(6, 2), BASE + 2) + .set(Pos(6, 3), BASE + 3) + .set(Pos(6, 4), BASE + 5) + .set(Pos(5, 4), BASE + 2) + .set(Pos(4, 4), BASE + 0) + .set(Pos(3, 4), BASE + 1) + # Longer path with lower elevation + .set(Pos(5, 1), BASE + 2) # Value might be skipped if steps is too low + .set(Pos(4, 1), BASE + 3) + .print() + .save('elevations_1') + ) + +class Point(object): + + x: float + y: float + + def __init__(self, x: float, y: float): + self.x = x + self.y = y + +class Pos(object): + + x: int + y: int + + def __init__(self, x: int, y: int): + self.x = x + self.y = y + + def __str__(self): + return f'({self.x}, {self.y})' + +class Grid(object): + + rows: list[list[int]] + + def __init__(self, width: int, height: int, default: int): + if not (0 < width and 0 < height): + print(f'Error: Invalid grid size {pos}', file=sys.stderr) + sys.exit(1) + self.rows = [[default for w in range(width + 1)] for h in range(height + 1)] + + def set(self, pos: Pos, value: int): + if not (0 <= pos.x <= self.width() and 0 <= pos.y <= self.height()): + print(f'Error: Position {pos} is out of bound', file=sys.stderr) + sys.exit(1) + self.rows[pos.y][pos.x] = value + + def width(self): + return len(self.rows[0]) - 1 + + def height(self): + return len(self.rows) - 1 + + def save(self, path: str): + with open(path, 'wb') as f: + for row in self.rows: + for value in row: + f.write(value.to_bytes(length=2, byteorder=BYTE_ORDER, signed=True)) + + def print(self): + for row in self.rows: + for entry in row: + print(f'{entry:>5} ', end='') + print() + +class Tile(object): + + tl: Point + br: Point + grid: Grid + + def __init__(self, tl: Point, br: Point, size: Pos, default: int = 0): + self.tl = tl + self.br = br + self.grid = Grid(width=size.x, height=size.y, default=default) + + def set(self, pos: Pos, value: int): + self.grid.set(pos, value) + return self + + def save(self, path: str): + width = self.grid.width() + height = self.grid.height() + with open(path + '.hdr', 'w') as f: + f.write(f''' +BYTEORDER I +LAYOUT BIL +NROWS {height + 1} +NCOLS {width + 1} +NBANDS 1 +NBITS 16 +BANDROWBYTES {2 * (width + 1)} +TOTALROWBYTES {2 * (width + 1)} +PIXELTYPE SIGNEDINT +ULXMAP {self.tl.x} +ULYMAP {self.tl.y} +XDIM {(self.br.x - self.tl.x) / width} +YDIM {(self.tl.y - self.br.y) / height} +NODATA -32767 +''') + self.grid.save(path + '.bil') + return self + + def print(self): + self.grid.print() + return self + +if __name__ == '__main__': + main() diff --git a/test/restriction_test_elevation/elevations_1.bil b/test/restriction_test_elevation/elevations_1.bil new file mode 100644 index 0000000..06fd07c --- /dev/null +++ b/test/restriction_test_elevation/elevations_1.bil @@ -0,0 +1 @@ +A_A_A_A_A_A_A_A_A_A_A_A_D_C_C_A_A_A_A_A_A_A_C_A_A_A_A_A_A_A_D_A_A_A_A_B_A_C_F_A_A_A_A_A_A_A_A_A_ \ No newline at end of file diff --git a/test/restriction_test_elevation/elevations_1.hdr b/test/restriction_test_elevation/elevations_1.hdr new file mode 100644 index 0000000..8b21448 --- /dev/null +++ b/test/restriction_test_elevation/elevations_1.hdr @@ -0,0 +1,15 @@ + +BYTEORDER I +LAYOUT BIL +NROWS 6 +NCOLS 8 +NBANDS 1 +NBITS 16 +BANDROWBYTES 16 +TOTALROWBYTES 16 +PIXELTYPE SIGNEDINT +ULXMAP 8.65617 +ULYMAP 49.8838761 +XDIM 0.00019892857142852653 +YDIM 0.00021848000000090906 +NODATA -32767 diff --git a/web/index.html b/web/index.html index 422c83b..7c88f02 100644 --- a/web/index.html +++ b/web/index.html @@ -121,6 +121,8 @@ + +