diff --git a/map/framework.cpp b/map/framework.cpp index 7cdc6a0936..f54b98bdd6 100644 --- a/map/framework.cpp +++ b/map/framework.cpp @@ -2100,6 +2100,11 @@ void Framework::BuildRoute(m2::PointD const & destination) vector absentFiles; if (code == IRouter::NoError) { + if(route.GetPoly().GetSize() < 2) + { + LOG(LWARNING, ("Invalide track for drawing. Have only ",route.GetPoly().GetSize(), "points")); + return; + } InsertRoute(route); GetLocationState()->RouteBuilded(); ShowRectExVisibleScale(route.GetPoly().GetLimitRect()); diff --git a/routing/astar_router.cpp b/routing/astar_router.cpp index 592bc8a191..3f0e01d44f 100644 --- a/routing/astar_router.cpp +++ b/routing/astar_router.cpp @@ -21,12 +21,6 @@ void SortUnique(vector & v) v.resize(unique(v.begin(), v.end()) - v.begin()); } -template -bool Contains(vector const & v, E const & e) -{ - return binary_search(v.begin(), v.end(), e); -} - void ReconstructRoute(RoadPos const & v, map const & parent, vector & route) { @@ -135,9 +129,11 @@ IRouter::ResultCode AStarRouter::CalculateRouteM2M(vector const & start if (v.dist > bestDistance[v.pos]) continue; - if (Contains(sortedStartPos, v.pos)) + /// We need the original start position because it contains the projection point to the road feature. + auto pos = lower_bound(sortedStartPos.begin(), sortedStartPos.end(), v.pos); + if (pos != sortedStartPos.end() && *pos==v.pos) { - ReconstructRoute(v.pos, parent, route); + ReconstructRoute(*pos, parent, route); return IRouter::NoError; } diff --git a/routing/astar_router.hpp b/routing/astar_router.hpp index 241e603ab6..4aeb62a019 100644 --- a/routing/astar_router.hpp +++ b/routing/astar_router.hpp @@ -12,7 +12,7 @@ class AStarRouter : public RoadGraphRouter { typedef RoadGraphRouter BaseT; public: - AStarRouter(Index const * pIndex = 0) : BaseT(pIndex) {} + AStarRouter(Index const * pIndex = 0) : BaseT(pIndex, unique_ptr(new PedestrianModel())) {} // IRouter overrides: string GetName() const override { return "astar-pedestrian"; } diff --git a/routing/dijkstra_router.hpp b/routing/dijkstra_router.hpp index 2a08257c0c..f8a5fdd8b0 100644 --- a/routing/dijkstra_router.hpp +++ b/routing/dijkstra_router.hpp @@ -11,7 +11,7 @@ class DijkstraRouter : public RoadGraphRouter typedef RoadGraphRouter BaseT; public: - DijkstraRouter(Index const * pIndex = 0) : BaseT(pIndex) {} + DijkstraRouter(Index const * pIndex = 0) : BaseT(pIndex,unique_ptr(new PedestrianModel())) {} // IRouter overrides: string GetName() const override { return "pedestrian-dijkstra"; } diff --git a/routing/features_road_graph.cpp b/routing/features_road_graph.cpp index 6d5296b3d8..56e5ce0048 100644 --- a/routing/features_road_graph.cpp +++ b/routing/features_road_graph.cpp @@ -17,6 +17,7 @@ namespace uint32_t const FEATURE_CACHE_SIZE = 10; uint32_t const READ_ROAD_SCALE = 13; double const READ_CROSS_EPSILON = 1.0E-4; +double const NORMALIZE_TO_SECONDS = 3.6; /* 60 * 60 / 1000 m/s to km/h*/ uint32_t indexFound = 0; uint32_t indexCheck = 0; @@ -153,6 +154,9 @@ void FeaturesRoadGraph::ReconstructPath(RoadPosVectorT const & positions, Route // Initialize starting point. LoadFeature(positions.back().GetFeatureId(), ft1); + m2::PointD prevPoint = positions.back().GetSegEndpoint(); + poly.push_back(prevPoint); + for (size_t i = count-1; i > 0; --i) { RoadPos const & pos1 = positions[i]; @@ -175,7 +179,6 @@ void FeaturesRoadGraph::ReconstructPath(RoadPosVectorT const & positions, Route int const inc = pos1.IsForward() ? -1 : 1; int ptID = pos1.GetSegStartPointId(); m2::PointD curPoint; - m2::PointD prevPoint = ft1.GetPoint(ptID); double segmentLength = 0.0; do { @@ -192,13 +195,18 @@ void FeaturesRoadGraph::ReconstructPath(RoadPosVectorT const & positions, Route segmentLength += CalcDistanceMeters(lastPt, prevPoint); // Calculation total feature time. Seconds. - trackTime += 3.6 /* normalization to seconds*/ * segmentLength / m_vehicleModel->GetSpeed(ft1); + trackTime += NORMALIZE_TO_SECONDS * segmentLength / m_vehicleModel->GetSpeed(ft1); // Assign current processing feature. if (diffIDs) ft1.SwapGeometry(ft2); } + poly.push_back(positions.front().GetSegEndpoint()); + trackTime += NORMALIZE_TO_SECONDS * CalcDistanceMeters(poly.back(), prevPoint) / m_vehicleModel->GetSpeed(ft1); + + ASSERT_GREATER(poly.size(), 1, ("Track with no points")); + if (poly.size() > 1) { Route::TurnsT turnsDir; diff --git a/routing/nearest_finder.cpp b/routing/nearest_finder.cpp new file mode 100644 index 0000000000..523ca57ba3 --- /dev/null +++ b/routing/nearest_finder.cpp @@ -0,0 +1,68 @@ +#include "nearest_finder.hpp" + +namespace routing +{ + +void NearestFinder::operator()(const FeatureType &ft) +{ + if (ft.GetFeatureType() != feature::GEOM_LINE || m_vehicleModel->GetSpeed(ft) == 0.0) + return; + + Candidate res; + + ft.ParseGeometry(FeatureType::BEST_GEOMETRY); + + size_t const count = ft.GetPointsCount(); + ASSERT_GREATER(count, 1, ()); + for (size_t i = 1; i < count; ++i) + { + /// @todo Probably, we need to get exact projection distance in meters. + m2::ProjectionToSection segProj; + segProj.SetBounds(ft.GetPoint(i - 1), ft.GetPoint(i)); + + m2::PointD const pt = segProj(m_point); + double const d = m_point.SquareLength(pt); + if (d < res.m_dist) + { + res.m_dist = d; + res.m_fid = ft.GetID().m_offset; + res.m_segIdx = i - 1; + res.m_point = pt; + res.m_isOneway = m_vehicleModel->IsOneWay(ft); + + if (m_mwmId == numeric_limits::max()) + m_mwmId = ft.GetID().m_mwm; + ASSERT_EQUAL(m_mwmId, ft.GetID().m_mwm, ()); + } + } + + if (res.m_fid != INVALID_FID) + m_candidates.push_back(res); +} + +void NearestFinder::MakeResult(vector &res, const size_t maxCount) +{ + if (m_mwmId == numeric_limits::max()) + return; + sort(m_candidates.begin(), m_candidates.end(), [] (Candidate const & r1, Candidate const & r2) + { + return (r1.m_dist < r2.m_dist); + }); + + res.clear(); + res.reserve(maxCount); + + for (Candidate const & candidate: m_candidates) + { + if (res.size() == maxCount) + break; + res.push_back(RoadPos(candidate.m_fid, true, candidate.m_segIdx, candidate.m_point)); + if (res.size() == maxCount) + break; + if (candidate.m_isOneway) + continue; + res.push_back(RoadPos(candidate.m_fid, false, candidate.m_segIdx, candidate.m_point)); + } +} + +} //namespace routing diff --git a/routing/nearest_finder.hpp b/routing/nearest_finder.hpp new file mode 100644 index 0000000000..a4a3323374 --- /dev/null +++ b/routing/nearest_finder.hpp @@ -0,0 +1,54 @@ +#pragma once + +#include "road_graph.hpp" +#include "vehicle_model.hpp" + +#include "../geometry/distance.hpp" +#include "../geometry/point2d.hpp" + +#include "../indexer/index.hpp" + +#include "../std/unique_ptr.hpp" + +namespace routing +{ + +class NearestFinder +{ + static constexpr uint32_t INVALID_FID = numeric_limits::max(); + + struct Candidate + { + double m_dist; + uint32_t m_segIdx; + m2::PointD m_point; + uint32_t m_fid; + bool m_isOneway; + Candidate() : m_dist(numeric_limits::max()), m_fid(INVALID_FID) {} + }; + + m2::PointD m_point; + m2::PointD m_direction; + unique_ptr const & m_vehicleModel; + buffer_vector m_candidates; + uint32_t m_mwmId; + +public: + NearestFinder(m2::PointD const & point, + m2::PointD const & direction, + unique_ptr const & vehicleModel) + : m_point(point), m_direction(direction), m_vehicleModel(vehicleModel), + m_mwmId(numeric_limits::max()) + {} + + bool HasCandidates() const { return !m_candidates.empty(); } + + void operator() (FeatureType const & ft); + + void MakeResult(vector & res, size_t const maxCount); + + uint32_t GetMwmID() {return m_mwmId;} + +}; + +} diff --git a/routing/road_graph_router.cpp b/routing/road_graph_router.cpp index cd5ecd12b6..644f459363 100644 --- a/routing/road_graph_router.cpp +++ b/routing/road_graph_router.cpp @@ -2,6 +2,7 @@ #include "features_road_graph.hpp" #include "route.hpp" #include "vehicle_model.hpp" +#include "nearest_finder.hpp" #include "../indexer/feature.hpp" #include "../indexer/ftypes_matcher.hpp" @@ -13,93 +14,31 @@ #include "../base/logging.hpp" +namespace +{ +size_t const MAX_ROAD_CANDIDATES = 2; +double const FEATURE_BY_POINT_RADIUS_M = 100.0; +} namespace routing { -class Point2RoadPos -{ - m2::PointD m_point; - double m_minDist; - m2::PointD m_posBeg; - m2::PointD m_posEnd; - size_t m_segIdx; - bool m_isOneway; - FeatureID m_fID; - IVehicleModel const * m_vehicleModel; - -public: - Point2RoadPos(m2::PointD const & pt, IVehicleModel const * vehicleModel) - : m_point(pt), m_minDist(numeric_limits::max()), m_vehicleModel(vehicleModel) - { - } - - void operator() (FeatureType const & ft) - { - if (ft.GetFeatureType() != feature::GEOM_LINE) - return; - - double const speed = m_vehicleModel->GetSpeed(ft); - if (speed <= 0.0) - return; - - ft.ParseGeometry(FeatureType::BEST_GEOMETRY); - - size_t const count = ft.GetPointsCount() - 1; - for (size_t i = 0; i < count; ++i) - { - m2::DistanceToLineSquare segDist; - m2::PointD const & p1 = ft.GetPoint(i); - m2::PointD const & p2 = ft.GetPoint(i + 1); - segDist.SetBounds(p1, p2); - double const d = segDist(m_point); - if (d < m_minDist) - { - m_minDist = d; - m_segIdx = i; - m_fID = ft.GetID(); - m_isOneway = m_vehicleModel->IsOneWay(ft); - m_posBeg = p1; - m_posEnd = p2; - } - } - } - - size_t GetMwmID() const - { - return m_fID.m_mwm; - } - - void GetResults(vector & res) - { - if (m_fID.IsValid()) - { - res.push_back(RoadPos(m_fID.m_offset, true, m_segIdx, m_posEnd)); - if (!m_isOneway) - res.push_back(RoadPos(m_fID.m_offset, false, m_segIdx, m_posBeg)); - } - } -}; - RoadGraphRouter::~RoadGraphRouter() { } -RoadGraphRouter::RoadGraphRouter(Index const * pIndex) : - m_vehicleModel(new CarModel()), m_pIndex(pIndex) -{ - -} +RoadGraphRouter::RoadGraphRouter(Index const * pIndex, unique_ptr && vehicleModel) + : m_vehicleModel(move(vehicleModel)), m_pIndex(pIndex) {} size_t RoadGraphRouter::GetRoadPos(m2::PointD const & pt, vector & pos) { - Point2RoadPos getter(pt, m_vehicleModel.get()); - m_pIndex->ForEachInRect(getter, - MercatorBounds::RectByCenterXYAndSizeInMeters(pt, 100.0), + NearestFinder finder(pt, m2::PointD::Zero() /* undirected */, m_vehicleModel); + m_pIndex->ForEachInRect(finder, + MercatorBounds::RectByCenterXYAndSizeInMeters(pt, FEATURE_BY_POINT_RADIUS_M), FeaturesRoadGraph::GetStreetReadScale()); - getter.GetResults(pos); - return getter.GetMwmID(); + finder.MakeResult(pos, MAX_ROAD_CANDIDATES); + return finder.GetMwmID(); } bool RoadGraphRouter::IsMyMWM(size_t mwmID) const @@ -114,7 +53,6 @@ IRouter::ResultCode RoadGraphRouter::CalculateRoute(m2::PointD const & startPoin { vector finalPos; size_t mwmID = GetRoadPos(finalPoint, finalPos); - if (!finalPos.empty() && !IsMyMWM(mwmID)) m_roadGraph.reset(new FeaturesRoadGraph(m_pIndex, mwmID)); diff --git a/routing/road_graph_router.hpp b/routing/road_graph_router.hpp index 6f6bf78799..9315961455 100644 --- a/routing/road_graph_router.hpp +++ b/routing/road_graph_router.hpp @@ -16,7 +16,7 @@ namespace routing class RoadGraphRouter : public IRouter { public: - RoadGraphRouter(Index const * pIndex); + RoadGraphRouter(Index const * pIndex, unique_ptr && vehicleModel); ~RoadGraphRouter(); ResultCode CalculateRoute(m2::PointD const & startPoint, m2::PointD const & startDirection, @@ -32,7 +32,7 @@ protected: bool IsMyMWM(size_t mwmID) const; unique_ptr m_roadGraph; - unique_ptr m_vehicleModel; + unique_ptr const m_vehicleModel; Index const * m_pIndex; // non-owning ptr }; diff --git a/routing/routing.pro b/routing/routing.pro index 5df1107422..654787f88f 100644 --- a/routing/routing.pro +++ b/routing/routing.pro @@ -17,6 +17,7 @@ SOURCES += \ cross_routing_context.cpp \ dijkstra_router.cpp \ features_road_graph.cpp \ + nearest_finder.cpp \ osrm2feature_map.cpp \ osrm_online_router.cpp \ osrm_router.cpp \ @@ -27,13 +28,13 @@ SOURCES += \ turns.cpp \ vehicle_model.cpp \ - HEADERS += \ astar_router.hpp \ async_router.hpp \ cross_routing_context.hpp \ dijkstra_router.hpp \ features_road_graph.hpp \ + nearest_finder.hpp \ osrm2feature_map.hpp \ osrm_data_facade.hpp \ osrm_online_router.hpp \