diff --git a/libs/geometry/parametrized_segment.hpp b/libs/geometry/parametrized_segment.hpp index 5bb32d0f3..8d5a79669 100644 --- a/libs/geometry/parametrized_segment.hpp +++ b/libs/geometry/parametrized_segment.hpp @@ -1,5 +1,6 @@ #pragma once +#include "geometry/mercator.hpp" #include "geometry/point2d.hpp" #include "base/math.hpp" @@ -59,9 +60,20 @@ public: /** * @brief Returns the point of the segment that is closest to `p`. + * + * @param p The checkpoint + * @param snapToEnds If true, the result is the endpoint of the segment which is closest to `p` */ - m2::PointD ClosestPointTo(Point const & p) const + m2::PointD ClosestPointTo(Point const & p, bool snapToEnds = false) const { + if (snapToEnds) + { + if (mercator::DistanceOnEarth(p, m_p0) < mercator::DistanceOnEarth(p, m_p1)) + return m_p0; + else + return m_p1; + } + m2::PointD const diff(p - m_p0); double const t = DotProduct(m_d, diff); diff --git a/libs/routing/fake_ending.cpp b/libs/routing/fake_ending.cpp index 683bad7e3..4a32f439f 100644 --- a/libs/routing/fake_ending.cpp +++ b/libs/routing/fake_ending.cpp @@ -18,12 +18,12 @@ using namespace routing; using namespace std; LatLonWithAltitude CalcProjectionToSegment(LatLonWithAltitude const & begin, LatLonWithAltitude const & end, - m2::PointD const & point) + m2::PointD const & point, bool snapToEnds) { m2::ParametrizedSegment segment(mercator::FromLatLon(begin.GetLatLon()), mercator::FromLatLon(end.GetLatLon())); - auto const projectedPoint = segment.ClosestPointTo(point); + auto const projectedPoint = segment.ClosestPointTo(point, snapToEnds); auto const distBeginToEnd = ms::DistanceOnEarth(begin.GetLatLon(), end.GetLatLon()); auto const projectedLatLon = mercator::ToLatLon(projectedPoint); @@ -45,7 +45,8 @@ bool Projection::operator==(Projection const & other) const tie(other.m_segment, other.m_isOneWay, other.m_segmentFront, other.m_segmentBack, other.m_junction); } -FakeEnding MakeFakeEnding(vector const & segments, m2::PointD const & point, WorldGraph & graph) +FakeEnding MakeFakeEnding(vector const & segments, m2::PointD const & point, + WorldGraph & graph, bool snapToEnds) { FakeEnding ending; double averageAltitude = 0.0; @@ -57,7 +58,7 @@ FakeEnding MakeFakeEnding(vector const & segments, m2::PointD const & p bool const oneWay = graph.IsOneWay(segment.GetMwmId(), segment.GetFeatureId()); auto const & frontJunction = graph.GetJunction(segment, true /* front */); auto const & backJunction = graph.GetJunction(segment, false /* front */); - auto const & projectedJunction = CalcProjectionToSegment(backJunction, frontJunction, point); + auto const & projectedJunction = CalcProjectionToSegment(backJunction, frontJunction, point, snapToEnds); ending.m_projections.emplace_back(segment, oneWay, frontJunction, backJunction, projectedJunction); @@ -69,13 +70,14 @@ FakeEnding MakeFakeEnding(vector const & segments, m2::PointD const & p return ending; } -FakeEnding MakeFakeEnding(Segment const & segment, m2::PointD const & point, IndexGraph & graph) +FakeEnding MakeFakeEnding(Segment const & segment, m2::PointD const & point, IndexGraph & graph, + bool snapToEnds) { auto const & road = graph.GetRoadGeometry(segment.GetFeatureId()); bool const oneWay = road.IsOneWay(); auto const & frontJunction = road.GetJunction(segment.GetPointId(true /* front */)); auto const & backJunction = road.GetJunction(segment.GetPointId(false /* front */)); - auto const & projectedJunction = CalcProjectionToSegment(backJunction, frontJunction, point); + auto const & projectedJunction = CalcProjectionToSegment(backJunction, frontJunction, point, snapToEnds); FakeEnding ending; ending.m_originJunction = LatLonWithAltitude(mercator::ToLatLon(point), projectedJunction.GetAltitude()); diff --git a/libs/routing/fake_ending.hpp b/libs/routing/fake_ending.hpp index f12215603..5cc59a7d0 100644 --- a/libs/routing/fake_ending.hpp +++ b/libs/routing/fake_ending.hpp @@ -40,9 +40,11 @@ struct FakeEnding final std::vector m_projections; }; -FakeEnding MakeFakeEnding(std::vector const & segments, m2::PointD const & point, WorldGraph & graph); -FakeEnding MakeFakeEnding(Segment const & segment, m2::PointD const & point, IndexGraph & graph); +FakeEnding MakeFakeEnding(std::vector const & segments, m2::PointD const & point, + WorldGraph & graph, bool snapToEnds = false); +FakeEnding MakeFakeEnding(Segment const & segment, m2::PointD const & point, IndexGraph & graph, + bool snapToEnds = false); LatLonWithAltitude CalcProjectionToSegment(LatLonWithAltitude const & begin, LatLonWithAltitude const & end, - m2::PointD const & point); + m2::PointD const & point, bool snapToEnds = false); } // namespace routing diff --git a/libs/routing/features_road_graph.cpp b/libs/routing/features_road_graph.cpp index 13c894983..f0bd2ec08 100644 --- a/libs/routing/features_road_graph.cpp +++ b/libs/routing/features_road_graph.cpp @@ -143,9 +143,10 @@ void FeaturesRoadGraphBase::ForEachFeatureClosestToCross(m2::PointD const & cros } void FeaturesRoadGraphBase::FindClosestEdges(m2::RectD const & rect, uint32_t count, - vector> & vicinities) const + vector> & vicinities, + bool snapToEnds) const { - NearestEdgeFinder finder(rect.Center(), nullptr /* IsEdgeProjGood */); + NearestEdgeFinder finder(rect.Center(), nullptr /* IsEdgeProjGood */, snapToEnds); m_dataSource.ForEachStreet([&](FeatureType & ft) { diff --git a/libs/routing/features_road_graph.hpp b/libs/routing/features_road_graph.hpp index 712236bad..fed223351 100644 --- a/libs/routing/features_road_graph.hpp +++ b/libs/routing/features_road_graph.hpp @@ -85,7 +85,8 @@ public: /// @{ void ForEachFeatureClosestToCross(m2::PointD const & cross, ICrossEdgesLoader & edgesLoader) const override; void FindClosestEdges(m2::RectD const & rect, uint32_t count, - std::vector> & vicinities) const override; + std::vector> & vicinities, + bool snapToEnds = false) const override; std::vector FindRoads(m2::RectD const & rect, IsGoodFeatureFn const & isGoodFeature) const override; void GetFeatureTypes(FeatureID const & featureId, feature::TypesHolder & types) const override; diff --git a/libs/routing/index_router.cpp b/libs/routing/index_router.cpp index c67d9dee0..9415fe9bf 100644 --- a/libs/routing/index_router.cpp +++ b/libs/routing/index_router.cpp @@ -314,7 +314,7 @@ bool IndexRouter::FindClosestProjectionToRoad(m2::PointD const & point, m2::Poin std::vector candidates; uint32_t const count = direction.IsAlmostZero() ? 1 : 4; - m_roadGraph.FindClosestEdges(rect, count, candidates); + m_roadGraph.FindClosestEdges(rect, count, candidates, (GetMode() == Mode::Decoding)); if (candidates.empty()) return false; @@ -1146,10 +1146,10 @@ int IndexRouter::PointsOnEdgesSnapping::Snap(m2::PointD const & start, m2::Point // One of startEnding or finishEnding will be empty here. if (startEnding.m_projections.empty()) - startEnding = MakeFakeEnding(m_startSegments, start, m_graph); + startEnding = MakeFakeEnding(m_startSegments, start, m_graph, (m_router.GetMode() == Mode::Decoding)); if (finishEnding.m_projections.empty()) - finishEnding = MakeFakeEnding(finishSegments, finish, m_graph); + finishEnding = MakeFakeEnding(finishSegments, finish, m_graph, (m_router.GetMode() == Mode::Decoding)); return 0; } @@ -1231,12 +1231,11 @@ bool IndexRouter::PointsOnEdgesSnapping::IsFencedOff(m2::PointD const & point, E return false; } -// static void IndexRouter::PointsOnEdgesSnapping::RoadsToNearestEdges(m2::PointD const & point, vector const & roads, IsEdgeProjGood const & isGood, vector & edgeProj) { - NearestEdgeFinder finder(point, isGood); + NearestEdgeFinder finder(point, isGood, (m_router.GetMode() == Mode::Decoding)); for (auto const & road : roads) finder.AddInformationSource(road); diff --git a/libs/routing/index_router.hpp b/libs/routing/index_router.hpp index 4fbcd078f..ae19f2564 100644 --- a/libs/routing/index_router.hpp +++ b/libs/routing/index_router.hpp @@ -247,8 +247,8 @@ private: static bool IsFencedOff(m2::PointD const & point, EdgeProjectionT const & edgeProjection, std::vector const & fences); - static void RoadsToNearestEdges(m2::PointD const & point, std::vector const & roads, - IsEdgeProjGood const & isGood, std::vector & edgeProj); + void RoadsToNearestEdges(m2::PointD const & point, std::vector const & roads, + IsEdgeProjGood const & isGood, std::vector & edgeProj); Segment GetSegmentByEdge(Edge const & edge) const; diff --git a/libs/routing/nearest_edge_finder.cpp b/libs/routing/nearest_edge_finder.cpp index f7c0a41fa..964127823 100644 --- a/libs/routing/nearest_edge_finder.cpp +++ b/libs/routing/nearest_edge_finder.cpp @@ -9,9 +9,11 @@ namespace routing { using namespace std; -NearestEdgeFinder::NearestEdgeFinder(m2::PointD const & point, IsEdgeProjGood const & isEdgeProjGood) +NearestEdgeFinder::NearestEdgeFinder(m2::PointD const & point, IsEdgeProjGood const & isEdgeProjGood, + bool snapToEnds) : m_point(point) , m_isEdgeProjGood(isEdgeProjGood) + , m_snapToEnds(snapToEnds) {} void NearestEdgeFinder::AddInformationSource(IRoadGraph::FullRoadInfo const & roadInfo) @@ -28,7 +30,7 @@ void NearestEdgeFinder::AddInformationSource(IRoadGraph::FullRoadInfo const & ro { m2::ParametrizedSegment segment(junctions[i - 1].GetPoint(), junctions[i].GetPoint()); - m2::PointD const closestPoint = segment.ClosestPointTo(m_point); + m2::PointD const closestPoint = segment.ClosestPointTo(m_point, m_snapToEnds); double const squaredDist = m_point.SquaredLength(closestPoint); if (squaredDist < res.m_squaredDist) @@ -48,7 +50,7 @@ void NearestEdgeFinder::AddInformationSource(IRoadGraph::FullRoadInfo const & ro geometry::Altitude const startAlt = segStart.GetAltitude(); geometry::Altitude const endAlt = segEnd.GetAltitude(); m2::ParametrizedSegment segment(junctions[idx - 1].GetPoint(), junctions[idx].GetPoint()); - m2::PointD const closestPoint = segment.ClosestPointTo(m_point); + m2::PointD const closestPoint = segment.ClosestPointTo(m_point, m_snapToEnds); double const segLenM = mercator::DistanceOnEarth(segStart.GetPoint(), segEnd.GetPoint()); geometry::Altitude projPointAlt = geometry::kDefaultAltitudeMeters; diff --git a/libs/routing/nearest_edge_finder.hpp b/libs/routing/nearest_edge_finder.hpp index bcdf428f2..c7546d713 100644 --- a/libs/routing/nearest_edge_finder.hpp +++ b/libs/routing/nearest_edge_finder.hpp @@ -27,7 +27,12 @@ using IsEdgeProjGood = std::function & res) const; m2::PointD const m_point; + bool m_snapToEnds; std::vector m_candidates; IsEdgeProjGood m_isEdgeProjGood; }; diff --git a/libs/routing/road_graph.hpp b/libs/routing/road_graph.hpp index 4258d8ba8..5bbdfa588 100644 --- a/libs/routing/road_graph.hpp +++ b/libs/routing/road_graph.hpp @@ -263,7 +263,7 @@ public: /// then returns empty array. using EdgeProjectionT = std::pair; virtual void FindClosestEdges(m2::RectD const & /*rect*/, uint32_t /*count*/, - std::vector & /*vicinities*/) const + std::vector & /*vicinities*/, bool snapToEnds) const {} /// \returns Vector of pairs FeatureID and corresponding RoadInfo for road features diff --git a/libs/traffxml/traff_decoder.cpp b/libs/traffxml/traff_decoder.cpp index 07dba8986..622d86be8 100644 --- a/libs/traffxml/traff_decoder.cpp +++ b/libs/traffxml/traff_decoder.cpp @@ -964,11 +964,15 @@ void RoutingTraffDecoder::AddDecodedSegment(traffxml::MultiMwmColoring & decoded void RoutingTraffDecoder::TruncateRoute(std::vector & rsegments, routing::Checkpoints const & checkpoints, bool backwards) { + double startWeight = 0; double const endWeight = rsegments.back().GetTimeFromBeginningSec(); // erase leading and trailing fake segments while(!rsegments.empty() && rsegments.front().GetSegment().GetMwmId() == routing::kFakeNumMwmId) + { + startWeight = rsegments.front().GetTimeFromBeginningSec(); rsegments.erase(rsegments.begin()); + } while(!rsegments.empty() && rsegments.back().GetSegment().GetMwmId() == routing::kFakeNumMwmId) rsegments.pop_back(); @@ -985,7 +989,7 @@ void RoutingTraffDecoder::TruncateRoute(std::vector & rse // Cost saved by omitting the last `end` segments. double endSaving = 0; - TruncateStart(rsegments, checkpoints, start, startSaving, + TruncateStart(rsegments, checkpoints, start, startSaving, startWeight, backwards ? m_endJunctions : m_startJunctions); TruncateEnd(rsegments, checkpoints, end, endSaving, endWeight, backwards ? m_startJunctions : m_endJunctions); @@ -1016,7 +1020,7 @@ void RoutingTraffDecoder::TruncateRoute(std::vector & rse rsegments.erase(rsegments.begin() + end + 1, rsegments.end()); start = 0; startSaving = 0; - TruncateStart(rsegments, checkpoints, start, startSaving, + TruncateStart(rsegments, checkpoints, start, startSaving, startWeight, backwards ? m_endJunctions : m_startJunctions); rsegments.erase(rsegments.begin(), rsegments.begin() + start); } @@ -1497,7 +1501,7 @@ std::vector ParseRef(std::string const & ref) void TruncateStart(std::vector & rsegments, routing::Checkpoints const & checkpoints, - size_t & start, double & startSaving, + size_t & start, double & startSaving, double const startWeight, std::map const & junctions) { if (rsegments.empty()) @@ -1535,6 +1539,16 @@ void TruncateStart(std::vector & rsegments, startSaving = newStartSaving; } } + /* + * The router may return a route that starts and ends with a partial segment. In decoder mode, + * we don’t allow starting or ending a segment at an intermediate point but use the nearest + * segment endpoint instead. However, this may leave us with a zero-length segment, which we + * need to remove so we don’t overshoot the reference point. + */ + // TODO not if we have an `at` point + if ((start == 0) && (rsegments.size() > 1) + && (rsegments[0].GetTimeFromBeginningSec() == startWeight)) + start = 1; } void TruncateEnd(std::vector & rsegments, @@ -1574,5 +1588,16 @@ void TruncateEnd(std::vector & rsegments, endSaving = newEndSaving; } } + /* + * The router may return a route that starts and ends with a partial segment. In decoder mode, + * we don’t allow starting or ending a segment at an intermediate point but use the nearest + * segment endpoint instead. However, this may leave us with a zero-length segment, which we + * need to remove so we don’t overshoot the reference point. + */ + // TODO not if we have an `at` point + if ((end == (rsegments.size() - 1)) && (rsegments.size() > 1) + && (rsegments[rsegments.size() - 1].GetTimeFromBeginningSec() + == rsegments[rsegments.size() - 2].GetTimeFromBeginningSec())) + end--; } } // namespace traffxml diff --git a/libs/traffxml/traff_decoder.hpp b/libs/traffxml/traff_decoder.hpp index 2dc4b12c9..4ccbe382d 100644 --- a/libs/traffxml/traff_decoder.hpp +++ b/libs/traffxml/traff_decoder.hpp @@ -596,11 +596,12 @@ std::vector ParseRef(std::string const & ref); * @param checkpoints The reference points (at least two) * @param start Index of the first segment to keep * @param startSaving Cost saved by truncating + * @param startWeight Weight of the fake segments up to the first real segment * @param junctions Junctions with the weight of their leap segment */ void TruncateStart(std::vector & rsegments, routing::Checkpoints const & checkpoints, - size_t & start, double & startSaving, + size_t & start, double & startSaving, double const startWeight, std::map const & junctions); /**