From a91e7fef03ddf666d416a80280a2b21d8495aa3e Mon Sep 17 00:00:00 2001 From: Denis Koronchik Date: Wed, 8 Oct 2014 16:50:41 +0300 Subject: [PATCH] [routing] Fix error with route search run when start and end osrm nodes equal --- indexer/mercator.cpp | 7 +++ indexer/mercator.hpp | 3 + routing/osrm2feature_map.cpp | 6 ++ routing/osrm2feature_map.hpp | 9 +++ routing/osrm_router.cpp | 112 ++++++++++++++++++++++++++++++++++- routing/route.cpp | 20 ++----- 6 files changed, 140 insertions(+), 17 deletions(-) diff --git a/indexer/mercator.cpp b/indexer/mercator.cpp index e13f593e48..3f5bd9af8a 100644 --- a/indexer/mercator.cpp +++ b/indexer/mercator.cpp @@ -1,5 +1,7 @@ #include "mercator.hpp" +#include "../geometry/distance_on_sphere.hpp" + double MercatorBounds::minX = -180; double MercatorBounds::maxX = 180; double MercatorBounds::minY = -180; @@ -39,3 +41,8 @@ m2::PointD MercatorBounds::GetSmPoint(m2::PointD const & pt, double lonMetresR, return FromLatLon(newLat, newLon); } + +double MercatorBounds::DistanceOnEarth(m2::PointD const & p1, m2::PointD const & p2) +{ + return ms::DistanceOnEarth(YToLat(p1.y), XToLon(p1.x), YToLat(p2.y), XToLon(p2.x)); +} diff --git a/indexer/mercator.hpp b/indexer/mercator.hpp index e51bb9c7d0..139efd24d2 100644 --- a/indexer/mercator.hpp +++ b/indexer/mercator.hpp @@ -106,4 +106,7 @@ struct MercatorBounds return m2::RectD(FromLatLon(latLonRect.minY(), latLonRect.minX()), FromLatLon(latLonRect.maxY(), latLonRect.maxX())); } + + /// Calculates distance on Earth by two points in mercator + static double DistanceOnEarth(m2::PointD const & p1, m2::PointD const & p2); }; diff --git a/routing/osrm2feature_map.cpp b/routing/osrm2feature_map.cpp index 792a073448..ee06f65e80 100644 --- a/routing/osrm2feature_map.cpp +++ b/routing/osrm2feature_map.cpp @@ -226,6 +226,12 @@ void OsrmFtSegMapping::GetOsrmNodes(FtSegSetT & segments, OsrmNodesT & res, vola } } +void OsrmFtSegMapping::GetSegmentByIndex(size_t idx, FtSeg & seg) const +{ + ASSERT_LESS(idx, m_segments.size(), ()); + FtSeg(m_segments[idx]).Swap(seg); +} + pair OsrmFtSegMapping::GetSegmentsRange(OsrmNodeIdT nodeId) const { SegOffsetsT::const_iterator it = lower_bound(m_offsets.begin(), m_offsets.end(), SegOffset(nodeId, 0), diff --git a/routing/osrm2feature_map.hpp b/routing/osrm2feature_map.hpp index 43fd11e91f..ce29a0f643 100644 --- a/routing/osrm2feature_map.hpp +++ b/routing/osrm2feature_map.hpp @@ -52,6 +52,13 @@ public: return m_fid != INVALID_FID; } + void Swap(FtSeg & other) + { + swap(m_fid, other.m_fid); + swap(m_pointStart, other.m_pointStart); + swap(m_pointEnd, other.m_pointEnd); + } + friend string DebugPrint(FtSeg const & seg); }; @@ -103,6 +110,8 @@ public: typedef unordered_map > OsrmNodesT; void GetOsrmNodes(FtSegSetT & segments, OsrmNodesT & res, volatile bool const & requestCancel) const; + void GetSegmentByIndex(size_t idx, FtSeg & seg) const; + /// @name For debug purpose only. //@{ void DumpSegmentsByFID(uint32_t fID) const; diff --git a/routing/osrm_router.cpp b/routing/osrm_router.cpp index 1618b3bc14..4fba151902 100644 --- a/routing/osrm_router.cpp +++ b/routing/osrm_router.cpp @@ -3,6 +3,7 @@ #include "vehicle_model.hpp" #include "../geometry/distance.hpp" +#include "../geometry/distance_on_sphere.hpp" #include "../indexer/mercator.hpp" #include "../indexer/index.hpp" @@ -47,10 +48,12 @@ class Point2PhantomNode : private noncopyable size_t m_ptIdx; buffer_vector m_candidates[2]; uint32_t m_mwmId; + Index const * m_pIndex; public: - Point2PhantomNode(OsrmFtSegMapping const & mapping) - : m_mapping(mapping), m_ptIdx(0), m_mwmId(numeric_limits::max()) + Point2PhantomNode(OsrmFtSegMapping const & mapping, Index const * pIndex) + : m_mapping(mapping), m_ptIdx(0), + m_mwmId(numeric_limits::max()), m_pIndex(pIndex) { } @@ -104,6 +107,104 @@ public: m_candidates[m_ptIdx].push_back(res); } + double CalculateDistance(OsrmFtSegMapping::FtSeg const & s) const + { + ASSERT_NOT_EQUAL(s.m_pointStart, s.m_pointEnd, ()); + + Index::FeaturesLoaderGuard loader(*m_pIndex, m_mwmId); + FeatureType ft; + loader.GetFeature(s.m_fid, ft); + ft.ParseGeometry(FeatureType::BEST_GEOMETRY); + + double dist = 0.0; + size_t n = max(s.m_pointEnd, s.m_pointStart); + size_t i = min(s.m_pointStart, s.m_pointEnd) + 1; + do + { + dist += MercatorBounds::DistanceOnEarth(ft.GetPoint(i - 1), ft.GetPoint(i)); + ++i; + } while (i <= n); + + return dist; + } + + void CalculateOffset(OsrmFtSegMapping::FtSeg const & seg, m2::PointD const & segPt, NodeID & nodeId, int & offset, bool forward) const + { + if (nodeId == INVALID_NODE_ID) + return; + + double distance = 0; + auto const range = m_mapping.GetSegmentsRange(nodeId); + OsrmFtSegMapping::FtSeg s, cSeg; + + int si = forward ? range.second - 1 : range.first; + int ei = forward ? range.first - 1 : range.second; + int di = forward ? -1 : 1; + + for (int i = si; i != ei; i += di) + { + m_mapping.GetSegmentByIndex(i, s); + auto s1 = min(s.m_pointStart, s.m_pointEnd); + auto e1 = max(s.m_pointEnd, s.m_pointStart); + + // seg.m_pointEnd - seg.m_pointStart == 1, so check + // just a case, when seg is inside s + if ((seg.m_pointStart != s1 || seg.m_pointEnd != e1) && + (s1 <= seg.m_pointStart && e1 >= seg.m_pointEnd)) + { + cSeg.m_fid = s.m_fid; + + if (s.m_pointStart < s.m_pointEnd) + { + if (forward) + { + cSeg.m_pointEnd = seg.m_pointEnd; + cSeg.m_pointStart = s.m_pointStart; + } + else + { + cSeg.m_pointStart = seg.m_pointStart; + cSeg.m_pointEnd = s.m_pointEnd; + } + } + else + { + if (forward) + { + cSeg.m_pointStart = seg.m_pointStart; + cSeg.m_pointEnd = s.m_pointStart; + } + else + { + cSeg.m_pointEnd = seg.m_pointEnd; + cSeg.m_pointStart = s.m_pointEnd; + } + } + + distance += CalculateDistance(cSeg); + break; + } + else + distance += CalculateDistance(s); + } + + Index::FeaturesLoaderGuard loader(*m_pIndex, m_mwmId); + FeatureType ft; + loader.GetFeature(seg.m_fid, ft); + ft.ParseGeometry(FeatureType::BEST_GEOMETRY); + + // node.m_seg always forward ordered (m_pointStart < m_pointEnd) + distance -= MercatorBounds::DistanceOnEarth(ft.GetPoint(forward ? seg.m_pointEnd : seg.m_pointStart), segPt); + + offset = max(static_cast(distance), 1); + } + + void CalculateOffsets(OsrmRouter::FeatureGraphNode & node) const + { + CalculateOffset(node.m_seg, node.m_segPt, node.m_node.forward_node_id, node.m_node.forward_offset, true); + CalculateOffset(node.m_seg, node.m_segPt, node.m_node.reverse_node_id, node.m_node.reverse_offset, false); + } + void MakeResult(OsrmRouter::FeatureGraphNodeVecT & res, size_t maxCount, uint32_t & mwmId, bool needFinal, volatile bool const & requestCancel) { @@ -161,6 +262,8 @@ public: node.m_node.reverse_node_id = it->second.second; node.m_seg = segments[idx]; node.m_segPt = m_candidates[i][j].m_point; + + CalculateOffsets(node); } } } @@ -501,6 +604,9 @@ OsrmRouter::ResultCode OsrmRouter::CalculateRouteImpl(m2::PointD const & startPt points.front() = sNode.m_segPt; points.back() = eNode.m_segPt; + if (points.size() < 2) + return RouteNotFound; + route.SetGeometry(points.begin(), points.end()); return NoError; @@ -509,7 +615,7 @@ OsrmRouter::ResultCode OsrmRouter::CalculateRouteImpl(m2::PointD const & startPt IRouter::ResultCode OsrmRouter::FindPhantomNodes(string const & fName, m2::PointD const & startPt, m2::PointD const & finalPt, FeatureGraphNodeVecT & res, size_t maxCount, uint32_t & mwmId) { - Point2PhantomNode getter(m_mapping); + Point2PhantomNode getter(m_mapping, m_pIndex); auto processPt = [&] (m2::PointD const & p, size_t idx) { diff --git a/routing/route.cpp b/routing/route.cpp index f860f55bca..a07d8dcd84 100644 --- a/routing/route.cpp +++ b/routing/route.cpp @@ -19,14 +19,6 @@ static double const ON_ROAD_TOLERANCE_M = 20.0; static double const ON_END_TOLERANCE_M = 10.0; -double GetDistanceOnEarth(m2::PointD const & p1, m2::PointD const & p2) -{ - return ms::DistanceOnEarth(MercatorBounds::YToLat(p1.y), - MercatorBounds::XToLon(p1.x), - MercatorBounds::YToLat(p2.y), - MercatorBounds::XToLon(p2.x)); -} - Route::Route(string const & router, vector const & points, string const & name) : m_router(router), m_poly(points), m_name(name) { @@ -55,7 +47,7 @@ double Route::GetCurrentDistanceFromBegin() const ASSERT(m_current.IsValid(), ()); return ((m_current.m_ind > 0 ? m_segDistance[m_current.m_ind - 1] : 0.0) + - GetDistanceOnEarth(m_poly.GetPoint(m_current.m_ind), m_current.m_pt)); + MercatorBounds::DistanceOnEarth(m_poly.GetPoint(m_current.m_ind), m_current.m_pt)); } double Route::GetCurrentDistanceToEnd() const @@ -63,7 +55,7 @@ double Route::GetCurrentDistanceToEnd() const ASSERT(m_current.IsValid(), ()); return (m_segDistance.back() - m_segDistance[m_current.m_ind] + - GetDistanceOnEarth(m_current.m_pt, m_poly.GetPoint(m_current.m_ind + 1))); + MercatorBounds::DistanceOnEarth(m_current.m_pt, m_poly.GetPoint(m_current.m_ind + 1))); } bool Route::MoveIterator(location::GpsInfo const & info) const @@ -122,7 +114,7 @@ Route::IterT Route::FindProjection(m2::RectD const & posRect, double predictDist m2::PointD const currPos = posRect.Center(); res = GetClosestProjection(posRect, [&] (IterT const & it) { - return GetDistanceOnEarth(it.m_pt, currPos); + return MercatorBounds::DistanceOnEarth(it.m_pt, currPos); }); } @@ -139,11 +131,11 @@ double Route::GetDistanceOnPolyline(IterT const & it1, IterT const & it2) const ASSERT_LESS(it2.m_ind, n, ()); if (it1.m_ind == it2.m_ind) - return GetDistanceOnEarth(it1.m_pt, it2.m_pt); + return MercatorBounds::DistanceOnEarth(it1.m_pt, it2.m_pt); - return (GetDistanceOnEarth(it1.m_pt, m_poly.GetPoint(it1.m_ind + 1)) + + return (MercatorBounds::DistanceOnEarth(it1.m_pt, m_poly.GetPoint(it1.m_ind + 1)) + m_segDistance[it2.m_ind - 1] - m_segDistance[it1.m_ind] + - GetDistanceOnEarth(m_poly.GetPoint(it2.m_ind), it2.m_pt)); + MercatorBounds::DistanceOnEarth(m_poly.GetPoint(it2.m_ind), it2.m_pt)); } void Route::Update()