diff --git a/CHANGELOG.md b/CHANGELOG.md index 6f08aed31..8efd0c592 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,8 +1,14 @@ # UNRELEASED - Changes from 5.18.0: + - Optimizations: + - CHANGED: Map matching is now almost twice as fast. [#5060](https://github.com/Project-OSRM/osrm-backend/pull/5060) + - CHANGED: Use Grisu2 for serializing floating point numbers. [#5188](https://github.com/Project-OSRM/osrm-backend/pull/5188) - Bugfixes: - FIXED: collapsing of ExitRoundabout instructions [#5114](https://github.com/Project-OSRM/osrm-backend/issues/5114) - FIXED: fix osrm-routed gdb not work issue [#5156](https://github.com/Project-OSRM/osrm-backend/issues/5156) + - FIXED: negative distances in table plugin annotation [#5106](https://github.com/Project-OSRM/osrm-backend/issues/5106) + - Misc: + - CHANGED: Support up to 512 named shared memory regions [#5185](https://github.com/Project-OSRM/osrm-backend/pull/5185) # 5.18.0 - Changes from 5.17.0: diff --git a/features/testbot/matching.feature b/features/testbot/matching.feature index 599afcb1d..3f3398443 100644 --- a/features/testbot/matching.feature +++ b/features/testbot/matching.feature @@ -792,4 +792,4 @@ Feature: Basic Map Matching When I match I should get | trace | geometry | a:distance | a:duration | a:weight | duration | | 2345 | 1.00018,1,1.000315,1 | 15.013264 | 1.5 | 1.5 | 1.5 | - | 4321 | 1.00027,1,1.000135,1 | 15.013264 | 1.5 | 1.5 | 1.5 | \ No newline at end of file + | 4321 | 1.00027,1,1.000135,1 | 15.013264 | 1.5 | 1.5 | 1.5 | diff --git a/include/engine/geospatial_query.hpp b/include/engine/geospatial_query.hpp index 4ecfc8118..ded53c7cb 100644 --- a/include/engine/geospatial_query.hpp +++ b/include/engine/geospatial_query.hpp @@ -449,6 +449,7 @@ template class GeospatialQuery const auto reverse_durations = datafacade.GetUncompressedReverseDurations(geometry_id); const auto forward_geometry = datafacade.GetUncompressedForwardGeometry(geometry_id); + const auto reverse_geometry = datafacade.GetUncompressedReverseGeometry(geometry_id); const auto forward_weight_offset = std::accumulate(forward_weights.begin(), @@ -479,19 +480,19 @@ template class GeospatialQuery datafacade.GetCoordinateOfNode(forward_geometry(data.fwd_segment_position)), point_on_segment); - const auto reverse_weight_offset = - std::accumulate(reverse_weights.begin(), - reverse_weights.end() - data.fwd_segment_position - 1, - EdgeWeight{0}); + const auto rev_segment_position = reverse_weights.size() - data.fwd_segment_position - 1; + + const auto reverse_weight_offset = std::accumulate( + reverse_weights.begin(), reverse_weights.begin() + rev_segment_position, EdgeWeight{0}); const auto reverse_duration_offset = std::accumulate(reverse_durations.begin(), - reverse_durations.end() - data.fwd_segment_position - 1, + reverse_durations.begin() + rev_segment_position, EdgeDuration{0}); EdgeDistance reverse_distance_offset = 0; - for (auto current = forward_geometry.begin(); - current < forward_geometry.end() - data.fwd_segment_position - 2; + for (auto current = reverse_geometry.begin(); + current < reverse_geometry.begin() + rev_segment_position; ++current) { reverse_distance_offset += util::coordinate_calculation::fccApproximateDistance( @@ -499,13 +500,11 @@ template class GeospatialQuery datafacade.GetCoordinateOfNode(*std::next(current))); } - EdgeWeight reverse_weight = - reverse_weights[reverse_weights.size() - data.fwd_segment_position - 1]; - EdgeDuration reverse_duration = - reverse_durations[reverse_durations.size() - data.fwd_segment_position - 1]; + EdgeWeight reverse_weight = reverse_weights[rev_segment_position]; + EdgeDuration reverse_duration = reverse_durations[rev_segment_position]; EdgeDistance reverse_distance = util::coordinate_calculation::fccApproximateDistance( point_on_segment, - datafacade.GetCoordinateOfNode(forward_geometry(data.fwd_segment_position + 1))); + datafacade.GetCoordinateOfNode(reverse_geometry(rev_segment_position))); ratio = std::min(1.0, std::max(0.0, ratio)); if (data.forward_segment_id.id != SPECIAL_SEGMENTID) @@ -693,7 +692,7 @@ template class GeospatialQuery const CoordinateList &coordinates; DataFacadeT &datafacade; }; -} -} +} // namespace engine +} // namespace osrm #endif diff --git a/include/engine/routing_algorithms/routing_base.hpp b/include/engine/routing_algorithms/routing_base.hpp index 0c0808a3f..4d00bf918 100644 --- a/include/engine/routing_algorithms/routing_base.hpp +++ b/include/engine/routing_algorithms/routing_base.hpp @@ -44,50 +44,19 @@ bool needsLoopBackwards(const PhantomNode &source_phantom, const PhantomNode &ta bool needsLoopForward(const PhantomNodes &phantoms); bool needsLoopBackwards(const PhantomNodes &phantoms); -template -void insertNodesInHeaps(Heap &forward_heap, Heap &reverse_heap, const PhantomNodes &nodes) +namespace detail { - const auto &source = nodes.source_phantom; - if (source.IsValidForwardSource()) - { - forward_heap.Insert(source.forward_segment_id.id, - -source.GetForwardWeightPlusOffset(), - source.forward_segment_id.id); - } - - if (source.IsValidReverseSource()) - { - forward_heap.Insert(source.reverse_segment_id.id, - -source.GetReverseWeightPlusOffset(), - source.reverse_segment_id.id); - } - - const auto &target = nodes.target_phantom; - if (target.IsValidForwardTarget()) - { - reverse_heap.Insert(target.forward_segment_id.id, - target.GetForwardWeightPlusOffset(), - target.forward_segment_id.id); - } - - if (target.IsValidReverseTarget()) - { - reverse_heap.Insert(target.reverse_segment_id.id, - target.GetReverseWeightPlusOffset(), - target.reverse_segment_id.id); - } -} - -template -void insertSourceInHeap(ManyToManyQueryHeap &heap, const PhantomNode &phantom_node) +template +void insertSourceInHeap(typename SearchEngineData::ManyToManyQueryHeap &heap, + const PhantomNode &phantom_node) { - if (phantom_node.IsValidForwardSource()) + if (phantom_node.IsValidForwardTarget()) { heap.Insert(phantom_node.forward_segment_id.id, -phantom_node.GetForwardWeightPlusOffset(), {phantom_node.forward_segment_id.id, -phantom_node.GetForwardDuration()}); } - if (phantom_node.IsValidReverseSource()) + if (phantom_node.IsValidReverseTarget()) { heap.Insert(phantom_node.reverse_segment_id.id, -phantom_node.GetReverseWeightPlusOffset(), @@ -95,8 +64,9 @@ void insertSourceInHeap(ManyToManyQueryHeap &heap, const PhantomNode &phantom_no } } -template -void insertTargetInHeap(ManyToManyQueryHeap &heap, const PhantomNode &phantom_node) +template +void insertTargetInHeap(typename SearchEngineData::ManyToManyQueryHeap &heap, + const PhantomNode &phantom_node) { if (phantom_node.IsValidForwardTarget()) { @@ -112,6 +82,109 @@ void insertTargetInHeap(ManyToManyQueryHeap &heap, const PhantomNode &phantom_no } } +template +void insertSourceInHeap(typename SearchEngineData::QueryHeap &heap, + const PhantomNode &phantom_node) +{ + if (phantom_node.IsValidForwardSource()) + { + heap.Insert(phantom_node.forward_segment_id.id, + -phantom_node.GetForwardWeightPlusOffset(), + phantom_node.forward_segment_id.id); + } + if (phantom_node.IsValidReverseSource()) + { + heap.Insert(phantom_node.reverse_segment_id.id, + -phantom_node.GetReverseWeightPlusOffset(), + phantom_node.reverse_segment_id.id); + } +} + +template +void insertTargetInHeap(typename SearchEngineData::QueryHeap &heap, + const PhantomNode &phantom_node) +{ + if (phantom_node.IsValidForwardTarget()) + { + heap.Insert(phantom_node.forward_segment_id.id, + phantom_node.GetForwardWeightPlusOffset(), + phantom_node.forward_segment_id.id); + } + if (phantom_node.IsValidReverseTarget()) + { + heap.Insert(phantom_node.reverse_segment_id.id, + phantom_node.GetReverseWeightPlusOffset(), + phantom_node.reverse_segment_id.id); + } +} +} // namespace detail + +inline void insertTargetInHeap(typename SearchEngineData::ManyToManyQueryHeap &heap, + const PhantomNode &phantom_node) +{ + detail::insertTargetInHeap(heap, phantom_node); +} +inline void insertTargetInHeap(typename SearchEngineData::ManyToManyQueryHeap &heap, + const PhantomNode &phantom_node) +{ + detail::insertTargetInHeap(heap, phantom_node); +} +inline void insertTargetInHeap(typename SearchEngineData::QueryHeap &heap, + const PhantomNode &phantom_node) +{ + detail::insertTargetInHeap(heap, phantom_node); +} +inline void insertTargetInHeap(typename SearchEngineData::QueryHeap &heap, + const PhantomNode &phantom_node) +{ + detail::insertTargetInHeap(heap, phantom_node); +} +inline void insertSourceInHeap(typename SearchEngineData::ManyToManyQueryHeap &heap, + const PhantomNode &phantom_node) +{ + detail::insertSourceInHeap(heap, phantom_node); +} +inline void insertSourceInHeap(typename SearchEngineData::ManyToManyQueryHeap &heap, + const PhantomNode &phantom_node) +{ + detail::insertSourceInHeap(heap, phantom_node); +} +inline void insertSourceInHeap(typename SearchEngineData::QueryHeap &heap, + const PhantomNode &phantom_node) +{ + detail::insertSourceInHeap(heap, phantom_node); +} +inline void insertSourceInHeap(typename SearchEngineData::QueryHeap &heap, + const PhantomNode &phantom_node) +{ + detail::insertSourceInHeap(heap, phantom_node); +} + +template +void insertNodesInHeaps(Heap &forward_heap, Heap &reverse_heap, const PhantomNodes &nodes) +{ + insertSourceInHeap(forward_heap, nodes.source_phantom); + insertTargetInHeap(reverse_heap, nodes.target_phantom); +} + +template +void insertSourceInHeap(typename SearchEngineData::ManyToManyQueryHeap &heap, + const PhantomNode &phantom_node) +{ + if (phantom_node.IsValidForwardSource()) + { + heap.Insert(phantom_node.forward_segment_id.id, + -phantom_node.GetForwardWeightPlusOffset(), + {phantom_node.forward_segment_id.id, -phantom_node.GetForwardDuration()}); + } + if (phantom_node.IsValidReverseSource()) + { + heap.Insert(phantom_node.reverse_segment_id.id, + -phantom_node.GetReverseWeightPlusOffset(), + {phantom_node.reverse_segment_id.id, -phantom_node.GetReverseDuration()}); + } +} + template void annotatePath(const FacadeT &facade, const PhantomNodes &phantom_node_pair, @@ -321,58 +394,10 @@ void annotatePath(const FacadeT &facade, } } -template -double getPathDistance(const DataFacade &facade, - const std::vector unpacked_path, - const PhantomNode &source_phantom, - const PhantomNode &target_phantom) -{ - using util::coordinate_calculation::detail::DEGREE_TO_RAD; - using util::coordinate_calculation::detail::EARTH_RADIUS; - - double distance = 0; - double prev_lat = - static_cast(util::toFloating(source_phantom.location.lat)) * DEGREE_TO_RAD; - double prev_lon = - static_cast(util::toFloating(source_phantom.location.lon)) * DEGREE_TO_RAD; - double prev_cos = std::cos(prev_lat); - for (const auto &p : unpacked_path) - { - const auto current_coordinate = facade.GetCoordinateOfNode(p.turn_via_node); - - const double current_lat = - static_cast(util::toFloating(current_coordinate.lat)) * DEGREE_TO_RAD; - const double current_lon = - static_cast(util::toFloating(current_coordinate.lon)) * DEGREE_TO_RAD; - const double current_cos = std::cos(current_lat); - - const double sin_dlon = std::sin((prev_lon - current_lon) / 2.0); - const double sin_dlat = std::sin((prev_lat - current_lat) / 2.0); - - const double aharv = sin_dlat * sin_dlat + prev_cos * current_cos * sin_dlon * sin_dlon; - const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv)); - distance += EARTH_RADIUS * charv; - - prev_lat = current_lat; - prev_lon = current_lon; - prev_cos = current_cos; - } - - const double current_lat = - static_cast(util::toFloating(target_phantom.location.lat)) * DEGREE_TO_RAD; - const double current_lon = - static_cast(util::toFloating(target_phantom.location.lon)) * DEGREE_TO_RAD; - const double current_cos = std::cos(current_lat); - - const double sin_dlon = std::sin((prev_lon - current_lon) / 2.0); - const double sin_dlat = std::sin((prev_lat - current_lat) / 2.0); - - const double aharv = sin_dlat * sin_dlat + prev_cos * current_cos * sin_dlon * sin_dlon; - const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv)); - distance += EARTH_RADIUS * charv; - - return distance; -} +EdgeDistance adjustPathDistanceToPhantomNodes(const std::vector &path, + const PhantomNode &source_phantom, + const PhantomNode &target_phantom, + const EdgeDistance distance); template InternalRouteResult extractRoute(const DataFacade &facade, diff --git a/include/engine/routing_algorithms/routing_base_mld.hpp b/include/engine/routing_algorithms/routing_base_mld.hpp index ce9b23d70..a5141a6bc 100644 --- a/include/engine/routing_algorithms/routing_base_mld.hpp +++ b/include/engine/routing_algorithms/routing_base_mld.hpp @@ -97,7 +97,6 @@ inline LevelID getNodeQueryLevel(const MultiLevelPartition &partition, const std::vector &phantom_indices) { auto min_level = [&partition, node](const PhantomNode &phantom_node) { - const auto &forward_segment = phantom_node.forward_segment_id; const auto forward_level = forward_segment.enabled ? partition.GetHighestDifferentLevel(node, forward_segment.id) @@ -120,7 +119,7 @@ inline LevelID getNodeQueryLevel(const MultiLevelPartition &partition, } return result; } -} +} // namespace // Heaps only record for each node its predecessor ("parent") on the shortest path. // For re-constructing the actual path we need to trace back all parent "pointers". @@ -391,21 +390,27 @@ UnpackedPath search(SearchEngineData &engine_working_data, EdgeWeight weight_upper_bound, Args... args) { - if (forward_heap.Empty() || reverse_heap.Empty()) + if (forward_heap.Empty() && reverse_heap.Empty()) { return std::make_tuple(INVALID_EDGE_WEIGHT, std::vector(), std::vector()); } const auto &partition = facade.GetMultiLevelPartition(); - BOOST_ASSERT(!forward_heap.Empty() && forward_heap.MinKey() < INVALID_EDGE_WEIGHT); - BOOST_ASSERT(!reverse_heap.Empty() && reverse_heap.MinKey() < INVALID_EDGE_WEIGHT); + BOOST_ASSERT(forward_heap.Empty() || forward_heap.MinKey() < INVALID_EDGE_WEIGHT); + BOOST_ASSERT(reverse_heap.Empty() || reverse_heap.MinKey() < INVALID_EDGE_WEIGHT); // run two-Target Dijkstra routing step. NodeID middle = SPECIAL_NODEID; EdgeWeight weight = weight_upper_bound; - EdgeWeight forward_heap_min = forward_heap.MinKey(); - EdgeWeight reverse_heap_min = reverse_heap.MinKey(); + + EdgeWeight forward_heap_min = 0; + if (!forward_heap.Empty()) + forward_heap_min = forward_heap.MinKey(); + EdgeWeight reverse_heap_min = 0; + if (!reverse_heap.Empty()) + reverse_heap_min = reverse_heap.MinKey(); + while (forward_heap.Size() + reverse_heap.Size() > 0 && forward_heap_min + reverse_heap_min < weight) { @@ -657,11 +662,7 @@ double getNetworkDistance(SearchEngineData &engine_working_data, const PhantomNode &target_phantom, EdgeWeight weight_upper_bound = INVALID_EDGE_WEIGHT) { - forward_heap.Clear(); - reverse_heap.Clear(); - const PhantomNodes phantom_nodes{source_phantom, target_phantom}; - insertNodesInHeaps(forward_heap, reverse_heap, phantom_nodes); EdgeWeight weight = INVALID_EDGE_WEIGHT; std::vector unpacked_nodes; @@ -680,11 +681,22 @@ double getNetworkDistance(SearchEngineData &engine_working_data, return std::numeric_limits::max(); } - std::vector unpacked_path; + EdgeDistance distance = 0; - annotatePath(facade, phantom_nodes, unpacked_nodes, unpacked_edges, unpacked_path); + if (!unpacked_nodes.empty()) + { + distance = std::accumulate(unpacked_nodes.begin(), + std::prev(unpacked_nodes.end()), + EdgeDistance{0}, + [&](const EdgeDistance distance, const auto node_id) { + return distance + computeEdgeDistance(facade, node_id); + }); + } - return getPathDistance(facade, unpacked_path, source_phantom, target_phantom); + distance = adjustPathDistanceToPhantomNodes( + unpacked_nodes, phantom_nodes.source_phantom, phantom_nodes.target_phantom, distance); + + return distance; } } // namespace mld diff --git a/include/storage/shared_datatype.hpp b/include/storage/shared_datatype.hpp index 720a37d46..926ca1ded 100644 --- a/include/storage/shared_datatype.hpp +++ b/include/storage/shared_datatype.hpp @@ -26,7 +26,7 @@ namespace serialization inline void read(io::BufferReader &reader, DataLayout &layout); inline void write(io::BufferWriter &writer, const DataLayout &layout); -} +} // namespace serialization namespace detail { @@ -52,7 +52,7 @@ inline std::string trimName(const std::string &name_prefix, const std::string &n return name; } } -} +} // namespace detail class DataLayout { @@ -165,7 +165,7 @@ struct SharedRegion static constexpr const int MAX_NAME_LENGTH = 254; SharedRegion() : name{0}, timestamp{0} {} - SharedRegion(const std::string &name_, std::uint64_t timestamp, std::uint8_t shm_key) + SharedRegion(const std::string &name_, std::uint64_t timestamp, std::uint16_t shm_key) : name{0}, timestamp{timestamp}, shm_key{shm_key} { std::copy_n(name_.begin(), std::min(MAX_NAME_LENGTH, name_.size()), name); @@ -175,14 +175,14 @@ struct SharedRegion char name[MAX_NAME_LENGTH + 1]; std::uint64_t timestamp; - std::uint8_t shm_key; + std::uint16_t shm_key; }; // Keeps a list of all shared regions in a fixed-sized struct // for fast access and deserialization. struct SharedRegionRegister { - using RegionID = std::uint8_t; + using RegionID = std::uint16_t; static constexpr const RegionID INVALID_REGION_ID = std::numeric_limits::max(); using ShmKey = decltype(SharedRegion::shm_key); @@ -250,12 +250,11 @@ struct SharedRegionRegister void ReleaseKey(ShmKey key) { shm_key_in_use[key] = false; } - static constexpr const std::uint8_t MAX_SHARED_REGIONS = - std::numeric_limits::max() - 1; + static constexpr const std::size_t MAX_SHARED_REGIONS = 512; static_assert(MAX_SHARED_REGIONS < std::numeric_limits::max(), "Number of shared memory regions needs to be less than the region id size."); - static constexpr const std::uint8_t MAX_SHM_KEYS = std::numeric_limits::max() - 1; + static constexpr const std::size_t MAX_SHM_KEYS = MAX_SHARED_REGIONS * 2; static constexpr const char *name = "osrm-region"; @@ -263,7 +262,7 @@ struct SharedRegionRegister std::array regions; std::array shm_key_in_use; }; -} -} +} // namespace storage +} // namespace osrm #endif /* SHARED_DATA_TYPE_HPP */ diff --git a/include/storage/shared_memory.hpp b/include/storage/shared_memory.hpp index 6fdedfcef..3b558238d 100644 --- a/include/storage/shared_memory.hpp +++ b/include/storage/shared_memory.hpp @@ -34,10 +34,10 @@ namespace storage struct OSRMLockFile { - boost::filesystem::path operator()() + template boost::filesystem::path operator()(const IdentifierT &id) { boost::filesystem::path temp_dir = boost::filesystem::temp_directory_path(); - boost::filesystem::path lock_file = temp_dir / "osrm.lock"; + boost::filesystem::path lock_file = temp_dir / ("osrm-" + std::to_string(id) + ".lock"); return lock_file; } }; @@ -93,7 +93,7 @@ class SharedMemory try { OSRMLockFile lock_file; - boost::interprocess::xsi_key key(lock_file().string().c_str(), id); + boost::interprocess::xsi_key key(lock_file(id).string().c_str(), id); result = RegionExists(key); } catch (...) @@ -106,7 +106,7 @@ class SharedMemory template static bool Remove(const IdentifierT id) { OSRMLockFile lock_file; - boost::interprocess::xsi_key key(lock_file().string().c_str(), id); + boost::interprocess::xsi_key key(lock_file(id).string().c_str(), id); return Remove(key); } @@ -287,10 +287,11 @@ class SharedMemory template std::unique_ptr makeSharedMemory(const IdentifierT &id, const uint64_t size = 0) { + static_assert(sizeof(id) == sizeof(std::uint16_t), "Key type is not 16 bits"); try { LockFileT lock_file; - if (!boost::filesystem::exists(lock_file())) + if (!boost::filesystem::exists(lock_file(id))) { if (0 == size) { @@ -298,10 +299,10 @@ std::unique_ptr makeSharedMemory(const IdentifierT &id, const uint } else { - boost::filesystem::ofstream ofs(lock_file()); + boost::filesystem::ofstream ofs(lock_file(id)); } } - return std::make_unique(lock_file(), id, size); + return std::make_unique(lock_file(id), id, size); } catch (const boost::interprocess::interprocess_exception &e) { @@ -310,7 +311,7 @@ std::unique_ptr makeSharedMemory(const IdentifierT &id, const uint throw util::exception(e.what() + SOURCE_REF); } } -} -} +} // namespace storage +} // namespace osrm #endif // SHARED_MEMORY_HPP diff --git a/include/storage/shared_monitor.hpp b/include/storage/shared_monitor.hpp index 46048e487..2084112d4 100644 --- a/include/storage/shared_monitor.hpp +++ b/include/storage/shared_monitor.hpp @@ -33,7 +33,7 @@ template class InvertedLock InvertedLock(Lock &lock) : lock(lock) { lock.unlock(); } ~InvertedLock() { lock.lock(); } }; -} +} // namespace // The shared monitor implementation based on a semaphore and mutex template struct SharedMonitor @@ -146,7 +146,9 @@ template struct SharedMonitor // like two-turnstile reusable barrier or boost/interprocess/sync/spin/condition.hpp // fail if a waiter is killed. - static constexpr int buffer_size = 256; + // Buffer size needs to be large enough to hold all the semaphores for every + // listener you want to support. + static constexpr int buffer_size = 4096 * 4; struct InternalData { @@ -232,8 +234,8 @@ template struct SharedMonitor bi::shared_memory_object shmem; bi::mapped_region region; }; -} -} +} // namespace storage +} // namespace osrm #undef USE_BOOST_INTERPROCESS_CONDITION diff --git a/include/util/coordinate_calculation.hpp b/include/util/coordinate_calculation.hpp index c946842a2..884169e4c 100644 --- a/include/util/coordinate_calculation.hpp +++ b/include/util/coordinate_calculation.hpp @@ -23,9 +23,6 @@ namespace detail { const constexpr double DEGREE_TO_RAD = 0.017453292519943295769236907684886; const constexpr double RAD_TO_DEGREE = 1. / DEGREE_TO_RAD; -// earth radius varies between 6,356.750-6,378.135 km (3,949.901-3,963.189mi) -// The IUGG value for the equatorial radius is 6378.137 km (3963.19 miles) -const constexpr long double EARTH_RADIUS = 6372797.560856; inline double degToRad(const double degree) { diff --git a/include/util/ieee754.hpp b/include/util/ieee754.hpp new file mode 100644 index 000000000..6a05c26f4 --- /dev/null +++ b/include/util/ieee754.hpp @@ -0,0 +1,517 @@ +#ifndef IEEE754_HPP +#define IEEE754_HPP +/** +Copyright (C) 2014 Milo Yip + +Imported from: +https://github.com/miloyip/dtoa-benchmark/blob/c4020c62754950d38a1aaaed2975b05b441d1e7d/src/milo/dtoa_milo.h + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +**/ + +#include +#include + +#if defined(_MSC_VER) +#include "msinttypes/stdint.h" +#include +#else +#include +#endif + +#define UINT64_C2(h, l) ((static_cast(h) << 32) | static_cast(l)) + +namespace osrm +{ +namespace util +{ +namespace ieee754 +{ + +struct DiyFp +{ + DiyFp() {} + + DiyFp(uint64_t f, int e) : f(f), e(e) {} + + DiyFp(double d) + { + union { + double d; + uint64_t u64; + } u = {d}; + + int biased_e = (u.u64 & kDpExponentMask) >> kDpSignificandSize; + uint64_t significand = (u.u64 & kDpSignificandMask); + if (biased_e != 0) + { + f = significand + kDpHiddenBit; + e = biased_e - kDpExponentBias; + } + else + { + f = significand; + e = kDpMinExponent + 1; + } + } + + DiyFp operator-(const DiyFp &rhs) const + { + assert(e == rhs.e); + assert(f >= rhs.f); + return DiyFp(f - rhs.f, e); + } + + DiyFp operator*(const DiyFp &rhs) const + { +#if defined(_MSC_VER) && defined(_M_AMD64) + uint64_t h; + uint64_t l = _umul128(f, rhs.f, &h); + if (l & (uint64_t(1) << 63)) // rounding + h++; + return DiyFp(h, e + rhs.e + 64); +#elif (__GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)) && defined(__x86_64__) + unsigned __int128 p = + static_cast(f) * static_cast(rhs.f); + uint64_t h = p >> 64; + uint64_t l = static_cast(p); + if (l & (uint64_t(1) << 63)) // rounding + h++; + return DiyFp(h, e + rhs.e + 64); +#else + const uint64_t M32 = 0xFFFFFFFF; + const uint64_t a = f >> 32; + const uint64_t b = f & M32; + const uint64_t c = rhs.f >> 32; + const uint64_t d = rhs.f & M32; + const uint64_t ac = a * c; + const uint64_t bc = b * c; + const uint64_t ad = a * d; + const uint64_t bd = b * d; + uint64_t tmp = (bd >> 32) + (ad & M32) + (bc & M32); + tmp += 1U << 31; /// mult_round + return DiyFp(ac + (ad >> 32) + (bc >> 32) + (tmp >> 32), e + rhs.e + 64); +#endif + } + + DiyFp Normalize() const + { +#if defined(_MSC_VER) && defined(_M_AMD64) + unsigned long index; + _BitScanReverse64(&index, f); + return DiyFp(f << (63 - index), e - (63 - index)); +#elif defined(__GNUC__) + int s = __builtin_clzll(f); + return DiyFp(f << s, e - s); +#else + DiyFp res = *this; + while (!(res.f & kDpHiddenBit)) + { + res.f <<= 1; + res.e--; + } + res.f <<= (kDiySignificandSize - kDpSignificandSize - 1); + res.e = res.e - (kDiySignificandSize - kDpSignificandSize - 1); + return res; +#endif + } + + DiyFp NormalizeBoundary() const + { +#if defined(_MSC_VER) && defined(_M_AMD64) + unsigned long index; + _BitScanReverse64(&index, f); + return DiyFp(f << (63 - index), e - (63 - index)); +#else + DiyFp res = *this; + while (!(res.f & (kDpHiddenBit << 1))) + { + res.f <<= 1; + res.e--; + } + res.f <<= (kDiySignificandSize - kDpSignificandSize - 2); + res.e = res.e - (kDiySignificandSize - kDpSignificandSize - 2); + return res; +#endif + } + + void NormalizedBoundaries(DiyFp *minus, DiyFp *plus) const + { + DiyFp pl = DiyFp((f << 1) + 1, e - 1).NormalizeBoundary(); + DiyFp mi = (f == kDpHiddenBit) ? DiyFp((f << 2) - 1, e - 2) : DiyFp((f << 1) - 1, e - 1); + mi.f <<= mi.e - pl.e; + mi.e = pl.e; + *plus = pl; + *minus = mi; + } + + static const int kDiySignificandSize = 64; + static const int kDpSignificandSize = 52; + static const int kDpExponentBias = 0x3FF + kDpSignificandSize; + static const int kDpMinExponent = -kDpExponentBias; + static const uint64_t kDpExponentMask = UINT64_C2(0x7FF00000, 0x00000000); + static const uint64_t kDpSignificandMask = UINT64_C2(0x000FFFFF, 0xFFFFFFFF); + static const uint64_t kDpHiddenBit = UINT64_C2(0x00100000, 0x00000000); + + uint64_t f; + int e; +}; + +inline DiyFp GetCachedPower(int e, int *K) +{ + // 10^-348, 10^-340, ..., 10^340 + static const uint64_t kCachedPowers_F[] = { + UINT64_C2(0xfa8fd5a0, 0x081c0288), UINT64_C2(0xbaaee17f, 0xa23ebf76), + UINT64_C2(0x8b16fb20, 0x3055ac76), UINT64_C2(0xcf42894a, 0x5dce35ea), + UINT64_C2(0x9a6bb0aa, 0x55653b2d), UINT64_C2(0xe61acf03, 0x3d1a45df), + UINT64_C2(0xab70fe17, 0xc79ac6ca), UINT64_C2(0xff77b1fc, 0xbebcdc4f), + UINT64_C2(0xbe5691ef, 0x416bd60c), UINT64_C2(0x8dd01fad, 0x907ffc3c), + UINT64_C2(0xd3515c28, 0x31559a83), UINT64_C2(0x9d71ac8f, 0xada6c9b5), + UINT64_C2(0xea9c2277, 0x23ee8bcb), UINT64_C2(0xaecc4991, 0x4078536d), + UINT64_C2(0x823c1279, 0x5db6ce57), UINT64_C2(0xc2109436, 0x4dfb5637), + UINT64_C2(0x9096ea6f, 0x3848984f), UINT64_C2(0xd77485cb, 0x25823ac7), + UINT64_C2(0xa086cfcd, 0x97bf97f4), UINT64_C2(0xef340a98, 0x172aace5), + UINT64_C2(0xb23867fb, 0x2a35b28e), UINT64_C2(0x84c8d4df, 0xd2c63f3b), + UINT64_C2(0xc5dd4427, 0x1ad3cdba), UINT64_C2(0x936b9fce, 0xbb25c996), + UINT64_C2(0xdbac6c24, 0x7d62a584), UINT64_C2(0xa3ab6658, 0x0d5fdaf6), + UINT64_C2(0xf3e2f893, 0xdec3f126), UINT64_C2(0xb5b5ada8, 0xaaff80b8), + UINT64_C2(0x87625f05, 0x6c7c4a8b), UINT64_C2(0xc9bcff60, 0x34c13053), + UINT64_C2(0x964e858c, 0x91ba2655), UINT64_C2(0xdff97724, 0x70297ebd), + UINT64_C2(0xa6dfbd9f, 0xb8e5b88f), UINT64_C2(0xf8a95fcf, 0x88747d94), + UINT64_C2(0xb9447093, 0x8fa89bcf), UINT64_C2(0x8a08f0f8, 0xbf0f156b), + UINT64_C2(0xcdb02555, 0x653131b6), UINT64_C2(0x993fe2c6, 0xd07b7fac), + UINT64_C2(0xe45c10c4, 0x2a2b3b06), UINT64_C2(0xaa242499, 0x697392d3), + UINT64_C2(0xfd87b5f2, 0x8300ca0e), UINT64_C2(0xbce50864, 0x92111aeb), + UINT64_C2(0x8cbccc09, 0x6f5088cc), UINT64_C2(0xd1b71758, 0xe219652c), + UINT64_C2(0x9c400000, 0x00000000), UINT64_C2(0xe8d4a510, 0x00000000), + UINT64_C2(0xad78ebc5, 0xac620000), UINT64_C2(0x813f3978, 0xf8940984), + UINT64_C2(0xc097ce7b, 0xc90715b3), UINT64_C2(0x8f7e32ce, 0x7bea5c70), + UINT64_C2(0xd5d238a4, 0xabe98068), UINT64_C2(0x9f4f2726, 0x179a2245), + UINT64_C2(0xed63a231, 0xd4c4fb27), UINT64_C2(0xb0de6538, 0x8cc8ada8), + UINT64_C2(0x83c7088e, 0x1aab65db), UINT64_C2(0xc45d1df9, 0x42711d9a), + UINT64_C2(0x924d692c, 0xa61be758), UINT64_C2(0xda01ee64, 0x1a708dea), + UINT64_C2(0xa26da399, 0x9aef774a), UINT64_C2(0xf209787b, 0xb47d6b85), + UINT64_C2(0xb454e4a1, 0x79dd1877), UINT64_C2(0x865b8692, 0x5b9bc5c2), + UINT64_C2(0xc83553c5, 0xc8965d3d), UINT64_C2(0x952ab45c, 0xfa97a0b3), + UINT64_C2(0xde469fbd, 0x99a05fe3), UINT64_C2(0xa59bc234, 0xdb398c25), + UINT64_C2(0xf6c69a72, 0xa3989f5c), UINT64_C2(0xb7dcbf53, 0x54e9bece), + UINT64_C2(0x88fcf317, 0xf22241e2), UINT64_C2(0xcc20ce9b, 0xd35c78a5), + UINT64_C2(0x98165af3, 0x7b2153df), UINT64_C2(0xe2a0b5dc, 0x971f303a), + UINT64_C2(0xa8d9d153, 0x5ce3b396), UINT64_C2(0xfb9b7cd9, 0xa4a7443c), + UINT64_C2(0xbb764c4c, 0xa7a44410), UINT64_C2(0x8bab8eef, 0xb6409c1a), + UINT64_C2(0xd01fef10, 0xa657842c), UINT64_C2(0x9b10a4e5, 0xe9913129), + UINT64_C2(0xe7109bfb, 0xa19c0c9d), UINT64_C2(0xac2820d9, 0x623bf429), + UINT64_C2(0x80444b5e, 0x7aa7cf85), UINT64_C2(0xbf21e440, 0x03acdd2d), + UINT64_C2(0x8e679c2f, 0x5e44ff8f), UINT64_C2(0xd433179d, 0x9c8cb841), + UINT64_C2(0x9e19db92, 0xb4e31ba9), UINT64_C2(0xeb96bf6e, 0xbadf77d9), + UINT64_C2(0xaf87023b, 0x9bf0ee6b)}; + static const int16_t kCachedPowers_E[] = { + -1220, -1193, -1166, -1140, -1113, -1087, -1060, -1034, -1007, -980, -954, -927, -901, + -874, -847, -821, -794, -768, -741, -715, -688, -661, -635, -608, -582, -555, + -529, -502, -475, -449, -422, -396, -369, -343, -316, -289, -263, -236, -210, + -183, -157, -130, -103, -77, -50, -24, 3, 30, 56, 83, 109, 136, + 162, 189, 216, 242, 269, 295, 322, 348, 375, 402, 428, 455, 481, + 508, 534, 561, 588, 614, 641, 667, 694, 720, 747, 774, 800, 827, + 853, 880, 907, 933, 960, 986, 1013, 1039, 1066}; + + // int k = static_cast(ceil((-61 - e) * 0.30102999566398114)) + 374; + double dk = + (-61 - e) * 0.30102999566398114 + 347; // dk must be positive, so can do ceiling in positive + int k = static_cast(dk); + if (k != dk) + k++; + + unsigned index = static_cast((k >> 3) + 1); + *K = -(-348 + static_cast(index << 3)); // decimal exponent no need lookup table + + assert(index < sizeof(kCachedPowers_F) / sizeof(kCachedPowers_F[0])); + return DiyFp(kCachedPowers_F[index], kCachedPowers_E[index]); +} + +inline void +GrisuRound(char *buffer, int len, uint64_t delta, uint64_t rest, uint64_t ten_kappa, uint64_t wp_w) +{ + while (rest < wp_w && delta - rest >= ten_kappa && (rest + ten_kappa < wp_w || /// closer + wp_w - rest > rest + ten_kappa - wp_w)) + { + buffer[len - 1]--; + rest += ten_kappa; + } +} + +inline unsigned CountDecimalDigit32(uint32_t n) +{ + // Simple pure C++ implementation was faster than __builtin_clz version in this situation. + if (n < 10) + return 1; + if (n < 100) + return 2; + if (n < 1000) + return 3; + if (n < 10000) + return 4; + if (n < 100000) + return 5; + if (n < 1000000) + return 6; + if (n < 10000000) + return 7; + if (n < 100000000) + return 8; + if (n < 1000000000) + return 9; + return 10; +} + +inline void +DigitGen(const DiyFp &W, const DiyFp &Mp, uint64_t delta, char *buffer, int *len, int *K) +{ + static const uint32_t kPow10[] = { + 1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000}; + const DiyFp one(uint64_t(1) << -Mp.e, Mp.e); + const DiyFp wp_w = Mp - W; + uint32_t p1 = static_cast(Mp.f >> -one.e); + uint64_t p2 = Mp.f & (one.f - 1); + int kappa = static_cast(CountDecimalDigit32(p1)); + *len = 0; + + while (kappa > 0) + { + uint32_t d; + switch (kappa) + { + case 10: + d = p1 / 1000000000; + p1 %= 1000000000; + break; + case 9: + d = p1 / 100000000; + p1 %= 100000000; + break; + case 8: + d = p1 / 10000000; + p1 %= 10000000; + break; + case 7: + d = p1 / 1000000; + p1 %= 1000000; + break; + case 6: + d = p1 / 100000; + p1 %= 100000; + break; + case 5: + d = p1 / 10000; + p1 %= 10000; + break; + case 4: + d = p1 / 1000; + p1 %= 1000; + break; + case 3: + d = p1 / 100; + p1 %= 100; + break; + case 2: + d = p1 / 10; + p1 %= 10; + break; + case 1: + d = p1; + p1 = 0; + break; + default: +#if defined(_MSC_VER) + __assume(0); +#elif __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 5) + __builtin_unreachable(); +#else + d = 0; +#endif + } + if (d || *len) + buffer[(*len)++] = '0' + static_cast(d); + kappa--; + uint64_t tmp = (static_cast(p1) << -one.e) + p2; + if (tmp <= delta) + { + *K += kappa; + GrisuRound( + buffer, *len, delta, tmp, static_cast(kPow10[kappa]) << -one.e, wp_w.f); + return; + } + } + + // kappa = 0 + for (;;) + { + p2 *= 10; + delta *= 10; + char d = static_cast(p2 >> -one.e); + if (d || *len) + buffer[(*len)++] = '0' + d; + p2 &= one.f - 1; + kappa--; + if (p2 < delta) + { + *K += kappa; + GrisuRound(buffer, *len, delta, p2, one.f, wp_w.f * kPow10[-kappa]); + return; + } + } +} + +inline void Grisu2(double value, char *buffer, int *length, int *K) +{ + const DiyFp v(value); + DiyFp w_m, w_p; + v.NormalizedBoundaries(&w_m, &w_p); + + const DiyFp c_mk = GetCachedPower(w_p.e, K); + const DiyFp W = v.Normalize() * c_mk; + DiyFp Wp = w_p * c_mk; + DiyFp Wm = w_m * c_mk; + Wm.f++; + Wp.f--; + DigitGen(W, Wp, Wp.f - Wm.f, buffer, length, K); +} + +inline const char *GetDigitsLut() +{ + static const char cDigitsLut[200] = { + '0', '0', '0', '1', '0', '2', '0', '3', '0', '4', '0', '5', '0', '6', '0', '7', '0', + '8', '0', '9', '1', '0', '1', '1', '1', '2', '1', '3', '1', '4', '1', '5', '1', '6', + '1', '7', '1', '8', '1', '9', '2', '0', '2', '1', '2', '2', '2', '3', '2', '4', '2', + '5', '2', '6', '2', '7', '2', '8', '2', '9', '3', '0', '3', '1', '3', '2', '3', '3', + '3', '4', '3', '5', '3', '6', '3', '7', '3', '8', '3', '9', '4', '0', '4', '1', '4', + '2', '4', '3', '4', '4', '4', '5', '4', '6', '4', '7', '4', '8', '4', '9', '5', '0', + '5', '1', '5', '2', '5', '3', '5', '4', '5', '5', '5', '6', '5', '7', '5', '8', '5', + '9', '6', '0', '6', '1', '6', '2', '6', '3', '6', '4', '6', '5', '6', '6', '6', '7', + '6', '8', '6', '9', '7', '0', '7', '1', '7', '2', '7', '3', '7', '4', '7', '5', '7', + '6', '7', '7', '7', '8', '7', '9', '8', '0', '8', '1', '8', '2', '8', '3', '8', '4', + '8', '5', '8', '6', '8', '7', '8', '8', '8', '9', '9', '0', '9', '1', '9', '2', '9', + '3', '9', '4', '9', '5', '9', '6', '9', '7', '9', '8', '9', '9'}; + return cDigitsLut; +} + +inline void WriteExponent(int K, char *buffer) +{ + if (K < 0) + { + *buffer++ = '-'; + K = -K; + } + + if (K >= 100) + { + *buffer++ = '0' + static_cast(K / 100); + K %= 100; + const char *d = GetDigitsLut() + K * 2; + *buffer++ = d[0]; + *buffer++ = d[1]; + } + else if (K >= 10) + { + const char *d = GetDigitsLut() + K * 2; + *buffer++ = d[0]; + *buffer++ = d[1]; + } + else + *buffer++ = '0' + static_cast(K); + + *buffer = '\0'; +} + +inline void Prettify(char *buffer, int length, int k) +{ + const int kk = length + k; // 10^(kk-1) <= v < 10^kk + + if (length <= kk && kk <= 21) + { + // 1234e7 -> 12340000000 + for (int i = length; i < kk; i++) + buffer[i] = '0'; + buffer[kk] = '.'; + buffer[kk + 1] = '0'; + buffer[kk + 2] = '\0'; + } + else if (0 < kk && kk <= 21) + { + // 1234e-2 -> 12.34 + memmove(&buffer[kk + 1], &buffer[kk], length - kk); + buffer[kk] = '.'; + buffer[length + 1] = '\0'; + } + else if (-6 < kk && kk <= 0) + { + // 1234e-6 -> 0.001234 + const int offset = 2 - kk; + memmove(&buffer[offset], &buffer[0], length); + buffer[0] = '0'; + buffer[1] = '.'; + for (int i = 2; i < offset; i++) + buffer[i] = '0'; + buffer[length + offset] = '\0'; + } + else if (length == 1) + { + // 1e30 + buffer[1] = 'e'; + WriteExponent(kk - 1, &buffer[2]); + } + else + { + // 1234e30 -> 1.234e33 + memmove(&buffer[2], &buffer[1], length - 1); + buffer[1] = '.'; + buffer[length + 1] = 'e'; + WriteExponent(kk - 1, &buffer[0 + length + 2]); + } +} + +inline void dtoa_milo(double value, char *buffer) +{ + // Not handling NaN and inf + assert(!isnan(value)); + assert(!isinf(value)); + + if (value == 0) + { + buffer[0] = '0'; + buffer[1] = '.'; + buffer[2] = '0'; + buffer[3] = '\0'; + } + else + { + if (value < 0) + { + *buffer++ = '-'; + value = -value; + } + int length, K; + Grisu2(value, buffer, &length, &K); + Prettify(buffer, length, K); + } +} +} // namespace ieee754 +} // namespace util +} // namespace osrm + +#endif // IEEE754_HPP \ No newline at end of file diff --git a/include/util/json_renderer.hpp b/include/util/json_renderer.hpp index f83524b28..5d6a24228 100644 --- a/include/util/json_renderer.hpp +++ b/include/util/json_renderer.hpp @@ -5,6 +5,7 @@ #define JSON_RENDERER_HPP #include "util/cast.hpp" +#include "util/ieee754.hpp" #include "util/string_util.hpp" #include "osrm/json_container.hpp" @@ -21,6 +22,11 @@ namespace util namespace json { +namespace +{ +constexpr int MAX_FLOAT_STRING_LENGTH = 256; +} + struct Renderer { explicit Renderer(std::ostream &_out) : out(_out) {} @@ -34,8 +40,31 @@ struct Renderer void operator()(const Number &number) const { - out.precision(10); - out << number.value; + char buffer[MAX_FLOAT_STRING_LENGTH] = {'\0'}; + ieee754::dtoa_milo(number.value, buffer); + + // Trucate to 10 decimal places + int pos = 0; + int decimalpos = 0; + while (decimalpos == 0 && pos < MAX_FLOAT_STRING_LENGTH && buffer[pos] != 0) + { + if (buffer[pos] == '.') + { + decimalpos = pos; + break; + } + ++pos; + } + while (pos < MAX_FLOAT_STRING_LENGTH && buffer[pos] != 0) + { + if (pos - decimalpos == 10) + { + buffer[pos] = '\0'; + break; + } + ++pos; + } + out << buffer; } void operator()(const Object &object) const diff --git a/src/engine/routing_algorithms/many_to_many_ch.cpp b/src/engine/routing_algorithms/many_to_many_ch.cpp index 8ee02b5ec..496bada8f 100644 --- a/src/engine/routing_algorithms/many_to_many_ch.cpp +++ b/src/engine/routing_algorithms/many_to_many_ch.cpp @@ -240,74 +240,12 @@ void calculateDistances(typename SearchEngineData::ManyToManyQuer } if (!packed_leg.empty()) { - auto annotation = + EdgeDistance annotation = ch::calculateEBGNodeAnnotations(facade, packed_leg.begin(), packed_leg.end()); + annotation = adjustPathDistanceToPhantomNodes( + packed_leg, source_phantom, target_phantom, annotation); distances_table[row_index * number_of_targets + column_index] = annotation; - - // check the direction of travel to figure out how to calculate the offset to/from - // the source/target - if (source_phantom.forward_segment_id.id == packed_leg.front()) - { - // ............ <-- calculateEGBAnnotation returns distance from 0 to 3 - // -->s <-- subtract offset to start at source - // ......... <-- want this distance as result - // entry 0---1---2---3--- <-- 3 is exit node - EdgeDistance offset = source_phantom.GetForwardDistance(); - distances_table[row_index * number_of_targets + column_index] -= offset; - } - else if (source_phantom.reverse_segment_id.id == packed_leg.front()) - { - // ............ <-- calculateEGBAnnotation returns distance from 0 to 3 - // s<------- <-- subtract offset to start at source - // ... <-- want this distance - // entry 0---1---2---3 <-- 3 is exit node - EdgeDistance offset = source_phantom.GetReverseDistance(); - distances_table[row_index * number_of_targets + column_index] -= offset; - } - if (target_phantom.forward_segment_id.id == packed_leg.back()) - { - // ............ <-- calculateEGBAnnotation returns distance from 0 to 3 - // ++>t <-- add offset to get to target - // ................ <-- want this distance as result - // entry 0---1---2---3--- <-- 3 is exit node - EdgeDistance offset = target_phantom.GetForwardDistance(); - distances_table[row_index * number_of_targets + column_index] += offset; - } - else if (target_phantom.reverse_segment_id.id == packed_leg.back()) - { - // ............ <-- calculateEGBAnnotation returns distance from 0 to 3 - // <++t <-- add offset to get from target - // ................ <-- want this distance as result - // entry 0---1---2---3--- <-- 3 is exit node - EdgeDistance offset = target_phantom.GetReverseDistance(); - distances_table[row_index * number_of_targets + column_index] += offset; - } - } - else - { - // there is no shortcut to unpack. source and target are on the same EBG Node. - // if the offset of the target is greater than the offset of the source, subtract it - if (target_phantom.GetForwardDistance() > source_phantom.GetForwardDistance()) - { - // --------->t <-- offsets - // ->s <-- subtract source offset from target offset - // ......... <-- want this distance as result - // entry 0---1---2---3--- <-- 3 is exit node - EdgeDistance offset = - target_phantom.GetForwardDistance() - source_phantom.GetForwardDistance(); - distances_table[row_index * number_of_targets + column_index] = offset; - } - else - { - // s<--- <-- offsets - // t<--------- <-- subtract source offset from target offset - // ...... <-- want this distance as result - // entry 0---1---2---3--- <-- 3 is exit node - EdgeDistance offset = - target_phantom.GetReverseDistance() - source_phantom.GetReverseDistance(); - distances_table[row_index * number_of_targets + column_index] = offset; - } } packed_leg.clear(); } diff --git a/src/engine/routing_algorithms/map_matching.cpp b/src/engine/routing_algorithms/map_matching.cpp index 7d1b75901..695ec5778 100644 --- a/src/engine/routing_algorithms/map_matching.cpp +++ b/src/engine/routing_algorithms/map_matching.cpp @@ -227,6 +227,9 @@ SubMatchingList mapMatching(SearchEngineData &engine_working_data, { continue; } + forward_heap.Clear(); + const auto &source_phantom = prev_unbroken_timestamps_list[s].phantom_node; + insertSourceInHeap(forward_heap, source_phantom); for (const auto s_prime : util::irange(0UL, current_viterbi.size())) { @@ -237,14 +240,19 @@ SubMatchingList mapMatching(SearchEngineData &engine_working_data, continue; } - double network_distance = - getNetworkDistance(engine_working_data, - facade, - forward_heap, - reverse_heap, - prev_unbroken_timestamps_list[s].phantom_node, - current_timestamps_list[s_prime].phantom_node, - weight_upper_bound); + reverse_heap.Clear(); + const auto &target_phantom = current_timestamps_list[s_prime].phantom_node; + insertTargetInHeap(reverse_heap, target_phantom); + + double network_distance = getNetworkDistance(engine_working_data, + facade, + forward_heap, + reverse_heap, + source_phantom, + target_phantom, + weight_upper_bound); + + network_distance = std::round(network_distance * 10) / 10; // get distance diff between loc1/2 and locs/s_prime const auto d_t = std::abs(network_distance - haversine_distance); diff --git a/src/engine/routing_algorithms/routing_base.cpp b/src/engine/routing_algorithms/routing_base.cpp index fb01472bb..472a8cc9e 100644 --- a/src/engine/routing_algorithms/routing_base.cpp +++ b/src/engine/routing_algorithms/routing_base.cpp @@ -33,6 +33,79 @@ bool needsLoopBackwards(const PhantomNodes &phantoms) return needsLoopBackwards(phantoms.source_phantom, phantoms.target_phantom); } +EdgeDistance adjustPathDistanceToPhantomNodes(const std::vector &path, + const PhantomNode &source_phantom, + const PhantomNode &target_phantom, + const EdgeDistance uncorrected_distance) +{ + EdgeDistance distance = uncorrected_distance; + if (!path.empty()) + { + + // check the direction of travel to figure out how to calculate the offset to/from + // the source/target + if (source_phantom.forward_segment_id.id == path.front()) + { + // ............ <-- calculateEGBAnnotation returns distance from 0 to 3 + // -->s <-- subtract offset to start at source + // ......... <-- want this distance as result + // entry 0---1---2---3--- <-- 3 is exit node + distance -= source_phantom.GetForwardDistance(); + } + else if (source_phantom.reverse_segment_id.id == path.front()) + { + // ............ <-- calculateEGBAnnotation returns distance from 0 to 3 + // s<------- <-- subtract offset to start at source + // ... <-- want this distance + // entry 0---1---2---3 <-- 3 is exit node + distance -= source_phantom.GetReverseDistance(); + } + if (target_phantom.forward_segment_id.id == path.back()) + { + // ............ <-- calculateEGBAnnotation returns distance from 0 to 3 + // ++>t <-- add offset to get to target + // ................ <-- want this distance as result + // entry 0---1---2---3--- <-- 3 is exit node + distance += target_phantom.GetForwardDistance(); + } + else if (target_phantom.reverse_segment_id.id == path.back()) + { + // ............ <-- calculateEGBAnnotation returns distance from 0 to 3 + // <++t <-- add offset to get from target + // ................ <-- want this distance as result + // entry 0---1---2---3--- <-- 3 is exit node + distance += target_phantom.GetReverseDistance(); + } + } + else + { + // there is no shortcut to unpack. source and target are on the same EBG Node. + // if the offset of the target is greater than the offset of the source, subtract it + if (target_phantom.GetForwardDistance() > source_phantom.GetForwardDistance()) + { + // --------->t <-- offsets + // ->s <-- subtract source offset from target offset + // ......... <-- want this distance as result + // entry 0---1---2---3--- <-- 3 is exit node + distance = target_phantom.GetForwardDistance() - source_phantom.GetForwardDistance(); + } + else + { + // s<--- <-- offsets + // t<--------- <-- subtract source offset from target offset + // ...... <-- want this distance as result + // entry 0---1---2---3--- <-- 3 is exit node + distance = target_phantom.GetReverseDistance() - source_phantom.GetReverseDistance(); + } + } + + BOOST_ASSERT_MSG(distance >= 0 || distance > -1.0f, + "Distance correction generated negative number"); + // guard against underflow errors caused by rounding + distance = std::max(EdgeDistance{0}, distance); + return distance; +} + } // namespace routing_algorithms } // namespace engine } // namespace osrm diff --git a/src/engine/routing_algorithms/routing_base_ch.cpp b/src/engine/routing_algorithms/routing_base_ch.cpp index 695ebe923..d118f86f4 100644 --- a/src/engine/routing_algorithms/routing_base_ch.cpp +++ b/src/engine/routing_algorithms/routing_base_ch.cpp @@ -100,7 +100,7 @@ void search(SearchEngineData & /*engine_working_data*/, const PhantomNodes & /*phantom_nodes*/, const EdgeWeight weight_upper_bound) { - if (forward_heap.Empty() || reverse_heap.Empty()) + if (forward_heap.Empty() && reverse_heap.Empty()) { weight = INVALID_EDGE_WEIGHT; return; @@ -110,10 +110,14 @@ void search(SearchEngineData & /*engine_working_data*/, weight = weight_upper_bound; // get offset to account for offsets on phantom nodes on compressed edges - const auto min_edge_offset = std::min(0, forward_heap.MinKey()); - BOOST_ASSERT(min_edge_offset <= 0); + EdgeWeight min_edge_offset = 0; + if (forward_heap.Size() > 0) + { + min_edge_offset = std::min(min_edge_offset, forward_heap.MinKey()); + BOOST_ASSERT(min_edge_offset <= 0); + } // we only every insert negative offsets for nodes in the forward heap - BOOST_ASSERT(reverse_heap.MinKey() >= 0); + BOOST_ASSERT(reverse_heap.Empty() || reverse_heap.MinKey() >= 0); // run two-Target Dijkstra routing step. while (0 < (forward_heap.Size() + reverse_heap.Size())) @@ -176,11 +180,6 @@ double getNetworkDistance(SearchEngineData &engine_working_data, const PhantomNode &target_phantom, EdgeWeight weight_upper_bound) { - forward_heap.Clear(); - reverse_heap.Clear(); - - insertNodesInHeaps(forward_heap, reverse_heap, {source_phantom, target_phantom}); - EdgeWeight weight = INVALID_EDGE_WEIGHT; std::vector packed_path; search(engine_working_data, @@ -199,14 +198,31 @@ double getNetworkDistance(SearchEngineData &engine_working_data, return std::numeric_limits::max(); } - std::vector unpacked_path; - unpackPath(facade, - packed_path.begin(), - packed_path.end(), - {source_phantom, target_phantom}, - unpacked_path); + EdgeDistance distance = 0; - return getPathDistance(facade, unpacked_path, source_phantom, target_phantom); + std::vector unpacked_nodes; + unpacked_nodes.reserve(packed_path.size()); + if (!packed_path.empty()) + { + unpacked_nodes.push_back(packed_path.front()); + unpackPath( + facade, packed_path.begin(), packed_path.end(), [&](const auto &edge, const auto &) { + BOOST_ASSERT(edge.first == unpacked_nodes.back()); + unpacked_nodes.push_back(edge.second); + }); + + distance = std::accumulate(unpacked_nodes.begin(), + std::prev(unpacked_nodes.end()), + EdgeDistance{0}, + [&](const EdgeDistance distance, const auto node_id) { + return distance + computeEdgeDistance(facade, node_id); + }); + } + + distance = + adjustPathDistanceToPhantomNodes(unpacked_nodes, source_phantom, target_phantom, distance); + + return distance; } } // namespace ch diff --git a/src/storage/storage.cpp b/src/storage/storage.cpp index bcdf03252..213217425 100644 --- a/src/storage/storage.cpp +++ b/src/storage/storage.cpp @@ -66,7 +66,7 @@ struct RegionHandle { std::unique_ptr memory; char *data_ptr; - std::uint8_t shm_key; + std::uint16_t shm_key; }; auto setupRegion(SharedRegionRegister &shared_register, const DataLayout &layout) diff --git a/src/tools/store.cpp b/src/tools/store.cpp index cf9d96526..bd21c9b4b 100644 --- a/src/tools/store.cpp +++ b/src/tools/store.cpp @@ -82,7 +82,8 @@ void springClean() } else { - for (auto key : util::irange(0, storage::SharedRegionRegister::MAX_SHM_KEYS)) + for (auto key : util::irange( + 0, storage::SharedRegionRegister::MAX_SHM_KEYS)) { deleteRegion(key); } diff --git a/src/util/coordinate_calculation.cpp b/src/util/coordinate_calculation.cpp index cb9adb10a..eb852aa30 100644 --- a/src/util/coordinate_calculation.cpp +++ b/src/util/coordinate_calculation.cpp @@ -22,6 +22,11 @@ namespace coordinate_calculation namespace { + +// earth radius varies between 6,356.750-6,378.135 km (3,949.901-3,963.189mi) +// The IUGG value for the equatorial radius is 6378.137 km (3963.19 miles) +const constexpr double EARTH_RADIUS = 6372797.560856; + class CheapRulerContainer { public: @@ -112,7 +117,7 @@ double haversineDistance(const Coordinate coordinate_1, const Coordinate coordin const double aharv = std::pow(std::sin(dlat / 2.0), 2.0) + std::cos(dlat1) * std::cos(dlat2) * std::pow(std::sin(dlong / 2.), 2); const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv)); - return detail::EARTH_RADIUS * charv; + return EARTH_RADIUS * charv; } double greatCircleDistance(const Coordinate coordinate_1, const Coordinate coordinate_2) @@ -133,7 +138,7 @@ double greatCircleDistance(const Coordinate coordinate_1, const Coordinate coord const double x_value = (float_lon2 - float_lon1) * std::cos((float_lat1 + float_lat2) / 2.0); const double y_value = float_lat2 - float_lat1; - return std::hypot(x_value, y_value) * detail::EARTH_RADIUS; + return std::hypot(x_value, y_value) * EARTH_RADIUS; } double perpendicularDistance(const Coordinate segment_source,