From ffc40a5710596a5a94b41b3fe2cef541157895d1 Mon Sep 17 00:00:00 2001 From: Lev Dragunov Date: Mon, 18 Apr 2016 13:56:33 +0300 Subject: [PATCH 1/3] Turn generation massive refactoring. --- routing/osrm_router.cpp | 284 ++++++++++++++++---------------- routing/osrm_router.hpp | 17 -- routing/turns_generator.cpp | 316 +++++++++++++++--------------------- routing/turns_generator.hpp | 66 +++++++- 4 files changed, 336 insertions(+), 347 deletions(-) diff --git a/routing/osrm_router.cpp b/routing/osrm_router.cpp index 34d5471a10..f9782d6230 100644 --- a/routing/osrm_router.cpp +++ b/routing/osrm_router.cpp @@ -50,12 +50,141 @@ double constexpr kMwmLoadedProgress = 10.0f; double constexpr kPointsFoundProgress = 15.0f; double constexpr kCrossPathFoundProgress = 50.0f; double constexpr kPathFoundProgress = 70.0f; -// Osrm multiples seconds to 10, so we need to divide it back. -double constexpr kOSRMWeightToSecondsMultiplier = 1./10.; + +double PiMinusTwoVectorsAngle(m2::PointD const & p, m2::PointD const & p1, m2::PointD const & p2) +{ + return math::pi - ang::TwoVectorsAngle(p, p1, p2); +} } // namespace -// TODO (ldragunov) Switch all RawRouteData and incapsulate to own omim types. + using RawRouteData = InternalRouteResult; +class OSRMRoutingResultGraph : public turns::IRoutingResultGraph +{ +public: + virtual vector const & GetSegments() const override + { + return m_loadedSegments; + } + virtual void GetPossibleTurns(NodeID node, m2::PointD const & ingoingPoint, + m2::PointD const & junctionPoint, + turns::TTurnCandidates & candidates) const override + { + double const kReadCrossEpsilon = 1.0E-4; + double const kFeaturesNearTurnMeters = 3.0; + + // Geting nodes by geometry. + vector geomNodes; + helpers::Point2Node p2n(m_routingMapping, geomNodes); + + m_index.ForEachInRectForMWM( + p2n, m2::RectD(junctionPoint.x - kReadCrossEpsilon, junctionPoint.y - kReadCrossEpsilon, + junctionPoint.x + kReadCrossEpsilon, junctionPoint.y + kReadCrossEpsilon), + scales::GetUpperScale(), m_routingMapping.GetMwmId()); + + sort(geomNodes.begin(), geomNodes.end()); + geomNodes.erase(unique(geomNodes.begin(), geomNodes.end()), geomNodes.end()); + + // Filtering virtual edges. + vector adjacentNodes; + for (EdgeID const e : m_routingMapping.m_dataFacade.GetAdjacentEdgeRange(node)) + { + QueryEdge::EdgeData const data = m_routingMapping.m_dataFacade.GetEdgeData(e, node); + if (data.forward && !data.shortcut) + { + adjacentNodes.push_back(m_routingMapping.m_dataFacade.GetTarget(e)); + ASSERT_NOT_EQUAL(m_routingMapping.m_dataFacade.GetTarget(e), SPECIAL_NODEID, ()); + } + } + + for (NodeID const adjacentNode : geomNodes) + { + if (adjacentNode == node) + continue; + for (EdgeID const e : m_routingMapping.m_dataFacade.GetAdjacentEdgeRange(adjacentNode)) + { + if (m_routingMapping.m_dataFacade.GetTarget(e) != node) + continue; + QueryEdge::EdgeData const data = m_routingMapping.m_dataFacade.GetEdgeData(e, adjacentNode); + if (!data.shortcut && data.backward) + adjacentNodes.push_back(adjacentNode); + } + } + + // Preparing candidates. + for (NodeID const targetNode : adjacentNodes) + { + auto const range = m_routingMapping.m_segMapping.GetSegmentsRange(targetNode); + OsrmMappingTypes::FtSeg seg; + m_routingMapping.m_segMapping.GetSegmentByIndex(range.first, seg); + if (!seg.IsValid()) + continue; + + FeatureType ft; + Index::FeaturesLoaderGuard loader(m_index, m_routingMapping.GetMwmId()); + loader.GetFeatureByIndex(seg.m_fid, ft); + ft.ParseGeometry(FeatureType::BEST_GEOMETRY); + + m2::PointD const outgoingPoint = ft.GetPoint( + seg.m_pointStart < seg.m_pointEnd ? seg.m_pointStart + 1 : seg.m_pointStart - 1); + ASSERT_LESS(MercatorBounds::DistanceOnEarth(junctionPoint, ft.GetPoint(seg.m_pointStart)), + kFeaturesNearTurnMeters, ()); + + double const a = + my::RadToDeg(PiMinusTwoVectorsAngle(junctionPoint, ingoingPoint, outgoingPoint)); + candidates.emplace_back(a, targetNode, ftypes::GetHighwayClass(ft)); + } + + sort(candidates.begin(), candidates.end(), + [](turns::TurnCandidate const & t1, turns::TurnCandidate const & t2) + { + return t1.angle < t2.angle; + }); + } + + virtual double GetShortestPathLength() const override { return m_rawResult.shortestPathLength; } + virtual m2::PointD const & GetStartPoint() const override + { + return m_rawResult.sourceEdge.segmentPoint; + } + virtual m2::PointD const & GetEndPoint() const override + { + return m_rawResult.targetEdge.segmentPoint; + } + + OSRMRoutingResultGraph(Index const & index, RoutingMapping & mapping, RawRoutingResult & result) + : m_rawResult(result), m_index(index), m_routingMapping(mapping) + { + for (auto const & pathSegments : m_rawResult.unpackedPathSegments) + { + auto numSegments = pathSegments.size(); + m_loadedSegments.reserve(numSegments); + for (size_t segmentIndex = 0; segmentIndex < numSegments; ++segmentIndex) + { + bool isStartNode = (segmentIndex == 0); + bool isEndNode = (segmentIndex == numSegments - 1); + if (isStartNode || isEndNode) + { + m_loadedSegments.emplace_back(m_routingMapping, m_index, pathSegments[segmentIndex], + m_rawResult.sourceEdge, m_rawResult.targetEdge, isStartNode, + isEndNode); + } + else + { + m_loadedSegments.emplace_back(m_routingMapping, m_index, pathSegments[segmentIndex]); + } + } + } + } + + ~OSRMRoutingResultGraph() {} +private: + vector m_loadedSegments; + RawRoutingResult m_rawResult; + Index const & m_index; + RoutingMapping & m_routingMapping; +}; + // static bool OsrmRouter::CheckRoutingAbility(m2::PointD const & startPoint, m2::PointD const & finalPoint, TCountryFileFn const & countryFileFn, Index * index) @@ -191,7 +320,8 @@ OsrmRouter::ResultCode OsrmRouter::MakeRouteFromCrossesPath(TCheckedPath const & Route::TTimes mwmTimes; Route::TStreets mwmStreets; vector mwmPoints; - if (MakeTurnAnnotation(routingResult, mwmMapping, delegate, mwmPoints, mwmTurnsDir, mwmTimes, mwmStreets) != NoError) + OSRMRoutingResultGraph resultGraph(*m_pIndex, *mwmMapping, routingResult); + if (MakeTurnAnnotation(resultGraph, delegate, mwmPoints, mwmTurnsDir, mwmTimes, mwmStreets) != NoError) { LOG(LWARNING, ("Can't load road path data from disk for", mwmMapping->GetCountryName())); return RouteNotFound; @@ -360,7 +490,8 @@ OsrmRouter::ResultCode OsrmRouter::CalculateRoute(m2::PointD const & startPoint, Route::TStreets streets; vector points; - if (MakeTurnAnnotation(routingResult, startMapping, delegate, points, turnsDir, times, streets) != NoError) + OSRMRoutingResultGraph resultGraph(*m_pIndex, *startMapping, routingResult); + if (MakeTurnAnnotation(resultGraph, delegate, points, turnsDir, times, streets) != NoError) { LOG(LWARNING, ("Can't load road path data from disk!")); return RouteNotFound; @@ -419,149 +550,6 @@ IRouter::ResultCode OsrmRouter::FindPhantomNodes(m2::PointD const & point, return NoError; } -// @todo(vbykoianko) This method shall to be refactored. It shall be split into several -// methods. All the functionality shall be moved to the turns_generator unit. -// @todo(vbykoianko) For the time being MakeTurnAnnotation generates the turn annotation -// and the route polyline at the same time. It is better to generate it separately -// to be able to use the route without turn annotation. -OsrmRouter::ResultCode OsrmRouter::MakeTurnAnnotation( - RawRoutingResult const & routingResult, TRoutingMappingPtr const & mapping, - RouterDelegate const & delegate, vector & points, Route::TTurns & turnsDir, - Route::TTimes & times, Route::TStreets & streets) -{ - ASSERT(mapping, ()); - double estimatedTime = 0; - - LOG(LDEBUG, ("Shortest path length:", routingResult.shortestPathLength)); - -#ifdef DEBUG - size_t lastIdx = 0; -#endif - - for (auto const & pathSegments : routingResult.unpackedPathSegments) - { - INTERRUPT_WHEN_CANCELLED(delegate); - - // Get all computed route coordinates. - size_t const numSegments = pathSegments.size(); - - // Construct loaded segments. - vector loadedSegments; - loadedSegments.reserve(numSegments); - for (size_t segmentIndex = 0; segmentIndex < numSegments; ++segmentIndex) - { - bool isStartNode = (segmentIndex == 0); - bool isEndNode = (segmentIndex == numSegments - 1); - if (isStartNode || isEndNode) - { - loadedSegments.emplace_back(*mapping, *m_pIndex, pathSegments[segmentIndex], - routingResult.sourceEdge, routingResult.targetEdge, isStartNode, - isEndNode); - } - else - { - loadedSegments.emplace_back(*mapping, *m_pIndex, pathSegments[segmentIndex]); - } - } - - // Annotate turns. - size_t skipTurnSegments = 0; - for (size_t segmentIndex = 0; segmentIndex < numSegments; ++segmentIndex) - { - auto const & loadedSegment = loadedSegments[segmentIndex]; - - // ETA information. - double const nodeTimeSeconds = loadedSegment.m_weight * kOSRMWeightToSecondsMultiplier; - - // Street names. I put empty names too, to avoid freezing old street name while riding on - // unnamed street. - streets.emplace_back(max(points.size(), static_cast(1)) - 1, loadedSegment.m_name); - - // Turns information. - if (segmentIndex > 0 && !points.empty() && skipTurnSegments == 0) - { - turns::TurnItem turnItem; - turnItem.m_index = static_cast(points.size() - 1); - - skipTurnSegments = CheckUTurnOnRoute(loadedSegments, segmentIndex, turnItem); - - turns::TurnInfo turnInfo(loadedSegments[segmentIndex - 1], loadedSegments[segmentIndex]); - - if (turnItem.m_turn == turns::TurnDirection::NoTurn) - turns::GetTurnDirection(*m_pIndex, *mapping, turnInfo, turnItem); - -#ifdef DEBUG - double distMeters = 0.0; - for (size_t k = lastIdx + 1; k < points.size(); ++k) - distMeters += MercatorBounds::DistanceOnEarth(points[k - 1], points[k]); - LOG(LDEBUG, ("Speed:", 3.6 * distMeters / nodeTimeSeconds, "kmph; Dist:", distMeters, "Time:", - nodeTimeSeconds, "s", lastIdx, "e", points.size(), "source:", turnItem.m_sourceName, - "target:", turnItem.m_targetName)); - lastIdx = points.size(); -#endif - times.push_back(Route::TTimeItem(points.size(), estimatedTime)); - - // Lane information. - if (turnItem.m_turn != turns::TurnDirection::NoTurn) - { - turnItem.m_lanes = turnInfo.m_ingoing.m_lanes; - turnsDir.push_back(move(turnItem)); - } - } - - estimatedTime += nodeTimeSeconds; - if (skipTurnSegments > 0) - --skipTurnSegments; - - // Path geometry. - points.insert(points.end(), loadedSegment.m_path.begin(), loadedSegment.m_path.end()); - } - } - - // Path found. Points will be replaced by start and end edges points. - if (points.size() == 1) - points.push_back(points.front()); - - if (points.size() < 2) - return RouteNotFound; - - if (routingResult.sourceEdge.segment.IsValid()) - points.front() = routingResult.sourceEdge.segmentPoint; - if (routingResult.targetEdge.segment.IsValid()) - points.back() = routingResult.targetEdge.segmentPoint; - - times.push_back(Route::TTimeItem(points.size() - 1, estimatedTime)); - if (routingResult.targetEdge.segment.IsValid()) - { - turnsDir.emplace_back( - turns::TurnItem(static_cast(points.size()) - 1, turns::TurnDirection::ReachedYourDestination)); - } - turns::FixupTurns(points, turnsDir); - -#ifdef DEBUG - for (auto t : turnsDir) - { - LOG(LDEBUG, (turns::GetTurnString(t.m_turn), ":", t.m_index, t.m_sourceName, "-", t.m_targetName, "exit:", t.m_exitNum)); - } - - size_t last = 0; - double lastTime = 0; - for (Route::TTimeItem & t : times) - { - double dist = 0; - for (size_t i = last + 1; i <= t.first; ++i) - dist += MercatorBounds::DistanceOnEarth(points[i - 1], points[i]); - - double time = t.second - lastTime; - - LOG(LDEBUG, ("distance:", dist, "start:", last, "end:", t.first, "Time:", time, "Speed:", 3.6 * dist / time)); - last = t.first; - lastTime = t.second; - } -#endif - LOG(LDEBUG, ("Estimated time:", estimatedTime, "s")); - return OsrmRouter::NoError; -} } // namespace routing diff --git a/routing/osrm_router.hpp b/routing/osrm_router.hpp index b9cf3d0128..6c28e3b361 100644 --- a/routing/osrm_router.hpp +++ b/routing/osrm_router.hpp @@ -6,7 +6,6 @@ #include "routing/router.hpp" #include "routing/routing_mapping.hpp" - namespace feature { class TypesHolder; } class Index; @@ -73,22 +72,6 @@ protected: m2::PointD const & direction, TFeatureGraphNodeVec & res, size_t maxCount, TRoutingMappingPtr const & mapping); - /*! - * \brief Compute turn and time estimation structs for OSRM raw route. - * \param routingResult OSRM routing result structure to annotate. - * \param mapping Feature mappings. - * \param delegate Routing callbacks delegate. - * \param points Storage for unpacked points of the path. - * \param turnsDir output turns annotation storage. - * \param times output times annotation storage. - * \param streets output street names along the path. - * \return routing operation result code. - */ - ResultCode MakeTurnAnnotation(RawRoutingResult const & routingResult, - TRoutingMappingPtr const & mapping, RouterDelegate const & delegate, - vector & points, Route::TTurns & turnsDir, - Route::TTimes & times, Route::TStreets & streets); - private: /*! * \brief Makes route (points turns and other annotations) from the map cross structs and submits diff --git a/routing/turns_generator.cpp b/routing/turns_generator.cpp index 3f95843d4b..794f91dc03 100644 --- a/routing/turns_generator.cpp +++ b/routing/turns_generator.cpp @@ -30,6 +30,9 @@ double constexpr kMinDistMeters = 200.; size_t constexpr kNotSoCloseMaxPointsCount = 3; double constexpr kNotSoCloseMinDistMeters = 30.; +// Osrm multiples seconds to 10, so we need to divide it back. +double constexpr kOSRMWeightToSecondsMultiplier = 1. / 10.; + typedef vector TGeomTurnCandidate; double PiMinusTwoVectorsAngle(m2::PointD const & p, m2::PointD const & p1, m2::PointD const & p2) @@ -37,27 +40,6 @@ double PiMinusTwoVectorsAngle(m2::PointD const & p, m2::PointD const & p1, m2::P return math::pi - ang::TwoVectorsAngle(p, p1, p2); } -/*! - * \brief The TurnCandidate struct contains information about possible ways from a junction. - */ -struct TurnCandidate -{ - /*! - * angle is an angle of the turn in degrees. It means angle is 180 minus - * an angle between the current edge and the edge of the candidate. A counterclockwise rotation. - * The current edge is an edge which belongs the route and located before the junction. - * angle belongs to the range [-180; 180]; - */ - double angle; - /*! - * node is a possible node (a possible way) from the juction. - */ - NodeID node; - - TurnCandidate(double a, NodeID n) : angle(a), node(n) {} -}; -using TTurnCandidates = vector; - /*! * \brief The Point2Geometry class is responsable for looking for all adjacent to junctionPoint * road network edges. Including the current edge. @@ -103,31 +85,6 @@ public: DISALLOW_COPY_AND_MOVE(Point2Geometry); }; - -OsrmMappingTypes::FtSeg GetSegment(NodeID node, RoutingMapping const & routingMapping, - TGetIndexFunction GetIndex) -{ - auto const segmentsRange = routingMapping.m_segMapping.GetSegmentsRange(node); - OsrmMappingTypes::FtSeg seg; - routingMapping.m_segMapping.GetSegmentByIndex(GetIndex(segmentsRange), seg); - return seg; -} - -ftypes::HighwayClass GetOutgoingHighwayClass(NodeID outgoingNode, - RoutingMapping const & routingMapping, - Index const & index) -{ - OsrmMappingTypes::FtSeg const seg = - GetSegment(outgoingNode, routingMapping, GetFirstSegmentPointIndex); - if (!seg.IsValid()) - return ftypes::HighwayClass::Error; - - Index::FeaturesLoaderGuard loader(index, routingMapping.GetMwmId()); - FeatureType ft; - loader.GetFeatureByIndex(seg.m_fid, ft); - return ftypes::GetHighwayClass(ft); -} - /*! * \brief Returns false when * - the route leads from one big road to another one; @@ -135,8 +92,7 @@ ftypes::HighwayClass GetOutgoingHighwayClass(NodeID outgoingNode, * - and the turn is GoStraight or TurnSlight*. */ bool KeepTurnByHighwayClass(TurnDirection turn, TTurnCandidates const & possibleTurns, - TurnInfo const & turnInfo, Index const & index, - RoutingMapping & mapping) + TurnInfo const & turnInfo) { if (!IsGoStraightOrSlightTurn(turn)) return true; // The road significantly changes its direction here. So this turn shall be kept. @@ -150,7 +106,7 @@ bool KeepTurnByHighwayClass(TurnDirection turn, TTurnCandidates const & possible { if (t.node == turnInfo.m_outgoing.m_nodeId) continue; - ftypes::HighwayClass const highwayClass = GetOutgoingHighwayClass(t.node, mapping, index); + ftypes::HighwayClass const highwayClass = t.highwayClass; if (static_cast(highwayClass) > static_cast(maxClassForPossibleTurns)) maxClassForPossibleTurns = highwayClass; } @@ -183,15 +139,13 @@ bool KeepTurnByHighwayClass(TurnDirection turn, TTurnCandidates const & possible * \brief Returns false when other possible turns leads to service roads; */ bool KeepRoundaboutTurnByHighwayClass(TurnDirection turn, TTurnCandidates const & possibleTurns, - TurnInfo const & turnInfo, Index const & index, - RoutingMapping & mapping) + TurnInfo const & turnInfo) { for (auto const & t : possibleTurns) { if (t.node == turnInfo.m_outgoing.m_nodeId) continue; - ftypes::HighwayClass const highwayClass = GetOutgoingHighwayClass(t.node, mapping, index); - if (static_cast(highwayClass) != static_cast(ftypes::HighwayClass::Service)) + if (static_cast(t.highwayClass) != static_cast(ftypes::HighwayClass::Service)) return true; } return false; @@ -206,51 +160,10 @@ bool DiscardTurnByIngoingAndOutgoingEdges(TurnDirection intermediateDirection, turn.m_sourceName == turn.m_targetName; } -/*! - * \brief GetTurnGeometry looks for all the road network edges near ingoingPoint. - * GetTurnGeometry fills candidates with angles of all the incoming and outgoint segments. - * \warning GetTurnGeometry should be used carefully because it's a time-consuming function. - * \warning In multilevel crossroads there is an insignificant possibility that candidates - * is filled with redundant segments of roads of different levels. - */ -void GetTurnGeometry(m2::PointD const & junctionPoint, m2::PointD const & ingoingPoint, - TGeomTurnCandidate & candidates, RoutingMapping const & mapping, - Index const & index) -{ - Point2Geometry getter(junctionPoint, ingoingPoint, candidates); - index.ForEachInRectForMWM( - getter, MercatorBounds::RectByCenterXYAndSizeInMeters(junctionPoint, kFeaturesNearTurnMeters), - scales::GetUpperScale(), mapping.GetMwmId()); -} - -/*! - * \param junctionPoint is a point of the junction. - * \param ingoingPointOneSegment is a point one segment before the junction along the route. - * \param mapping is a route mapping. - * \return number of all the segments which joins junctionPoint. That means - * the number of ingoing segments plus the number of outgoing segments. - * \warning NumberOfIngoingAndOutgoingSegments should be used carefully because - * it's a time-consuming function. - * \warning In multilevel crossroads there is an insignificant possibility that the returned value - * contains redundant segments of roads of different levels. - */ -size_t NumberOfIngoingAndOutgoingSegments(m2::PointD const & junctionPoint, - m2::PointD const & ingoingPointOneSegment, - RoutingMapping const & mapping, Index const & index) -{ - TGeomTurnCandidate geoNodes; - // TODO(vbykoianko) It is repeating of a time consumption operation. The first time - // the geometry is extracted in GetPossibleTurns and the second time here. - // It shall be fixed. For the time being this repeating time consumption method - // is called relevantly seldom. - GetTurnGeometry(junctionPoint, ingoingPointOneSegment, geoNodes, mapping, index); - return geoNodes.size(); -} - bool KeepTurnByIngoingEdges(m2::PointD const & junctionPoint, m2::PointD const & ingoingPointOneSegment, m2::PointD const & outgoingPoint, bool hasMultiTurns, - RoutingMapping const & routingMapping, Index const & index) + TTurnCandidates const & nodes) { double const turnAngle = my::RadToDeg(PiMinusTwoVectorsAngle(junctionPoint, ingoingPointOneSegment, outgoingPoint)); @@ -259,9 +172,7 @@ bool KeepTurnByIngoingEdges(m2::PointD const & junctionPoint, // The code below is resposible for cases when there is only one way to leave the junction. // Such junction has to be kept as a turn when it's not a slight turn and it has ingoing edges // (one or more); - return hasMultiTurns || (!isGoStraightOrSlightTurn && - NumberOfIngoingAndOutgoingSegments(junctionPoint, ingoingPointOneSegment, - routingMapping, index) > 2); + return hasMultiTurns || (!isGoStraightOrSlightTurn && nodes.size() > 2); } bool FixupLaneSet(TurnDirection turn, vector & lanes, @@ -375,81 +286,6 @@ m2::PointD GetPointForTurn(vector const & path, m2::PointD const & j return nextPoint; } -// OSRM graph contains preprocessed edges without proper information about adjecency. -// So, to determine we must read the nearest geometry and check its adjacency by OSRM road graph. -void GetPossibleTurns(Index const & index, NodeID node, m2::PointD const & ingoingPoint, - m2::PointD const & junctionPoint, RoutingMapping & routingMapping, - TTurnCandidates & candidates) -{ - double const kReadCrossEpsilon = 1.0E-4; - - // Geting nodes by geometry. - vector geomNodes; - helpers::Point2Node p2n(routingMapping, geomNodes); - - index.ForEachInRectForMWM( - p2n, m2::RectD(junctionPoint.x - kReadCrossEpsilon, junctionPoint.y - kReadCrossEpsilon, - junctionPoint.x + kReadCrossEpsilon, junctionPoint.y + kReadCrossEpsilon), - scales::GetUpperScale(), routingMapping.GetMwmId()); - - sort(geomNodes.begin(), geomNodes.end()); - geomNodes.erase(unique(geomNodes.begin(), geomNodes.end()), geomNodes.end()); - - // Filtering virtual edges. - vector adjacentNodes; - for (EdgeID const e : routingMapping.m_dataFacade.GetAdjacentEdgeRange(node)) - { - QueryEdge::EdgeData const data = routingMapping.m_dataFacade.GetEdgeData(e, node); - if (data.forward && !data.shortcut) - { - adjacentNodes.push_back(routingMapping.m_dataFacade.GetTarget(e)); - ASSERT_NOT_EQUAL(routingMapping.m_dataFacade.GetTarget(e), SPECIAL_NODEID, ()); - } - } - - for (NodeID const adjacentNode : geomNodes) - { - if (adjacentNode == node) - continue; - for (EdgeID const e : routingMapping.m_dataFacade.GetAdjacentEdgeRange(adjacentNode)) - { - if (routingMapping.m_dataFacade.GetTarget(e) != node) - continue; - QueryEdge::EdgeData const data = routingMapping.m_dataFacade.GetEdgeData(e, adjacentNode); - if (!data.shortcut && data.backward) - adjacentNodes.push_back(adjacentNode); - } - } - - // Preparing candidates. - for (NodeID const targetNode : adjacentNodes) - { - auto const range = routingMapping.m_segMapping.GetSegmentsRange(targetNode); - OsrmMappingTypes::FtSeg seg; - routingMapping.m_segMapping.GetSegmentByIndex(range.first, seg); - if (!seg.IsValid()) - continue; - - FeatureType ft; - Index::FeaturesLoaderGuard loader(index, routingMapping.GetMwmId()); - loader.GetFeatureByIndex(seg.m_fid, ft); - ft.ParseGeometry(FeatureType::BEST_GEOMETRY); - - m2::PointD const outgoingPoint = ft.GetPoint( - seg.m_pointStart < seg.m_pointEnd ? seg.m_pointStart + 1 : seg.m_pointStart - 1); - ASSERT_LESS(MercatorBounds::DistanceOnEarth(junctionPoint, ft.GetPoint(seg.m_pointStart)), - kFeaturesNearTurnMeters, ()); - - double const a = my::RadToDeg(PiMinusTwoVectorsAngle(junctionPoint, ingoingPoint, outgoingPoint)); - candidates.emplace_back(a, targetNode); - } - - sort(candidates.begin(), candidates.end(), [](TurnCandidate const & t1, TurnCandidate const & t2) - { - return t1.angle < t2.angle; - }); -} - size_t GetIngoingPointIndex(const size_t start, const size_t end, const size_t i) { return end > start ? end - i : end + i; @@ -472,7 +308,7 @@ LoadedPathSegment::LoadedPathSegment(RoutingMapping & mapping, Index const & ind : m_highwayClass(ftypes::HighwayClass::Undefined) , m_onRoundabout(false) , m_isLink(false) - , m_weight(osrmPathSegment.segmentWeight) + , m_weight(osrmPathSegment.segmentWeight * kOSRMWeightToSecondsMultiplier) , m_nodeId(osrmPathSegment.node) { buffer_vector buffer; @@ -625,6 +461,7 @@ LoadedPathSegment::LoadedPathSegment(RoutingMapping & mapping, Index const & ind size_t endIndex = isEndNode ? findIntersectingSeg(endGraphNode.segment) + 1 : buffer.size(); LoadPathGeometry(buffer, startIndex, endIndex, index, mapping, startGraphNode, endGraphNode, isStartNode, isEndNode); + m_weight *= kOSRMWeightToSecondsMultiplier; } bool TurnInfo::IsSegmentsValid() const @@ -637,6 +474,123 @@ bool TurnInfo::IsSegmentsValid() const return true; } +// @todo(vbykoianko) This method shall to be refactored. It shall be split into several +// methods. All the functionality shall be moved to the turns_generator unit. + +// @todo(vbykoianko) For the time being MakeTurnAnnotation generates the turn annotation +// and the route polyline at the same time. It is better to generate it separately +// to be able to use the route without turn annotation. +IRouter::ResultCode MakeTurnAnnotation(turns::IRoutingResultGraph const & result, + RouterDelegate const & delegate, vector & points, + Route::TTurns & turnsDir, Route::TTimes & times, + Route::TStreets & streets) +{ + double estimatedTime = 0; + + LOG(LDEBUG, ("Shortest th length:", result.GetShortestPathLength())); + +#ifdef DEBUG + size_t lastIdx = 0; +#endif + + if (delegate.IsCancelled()) + return IRouter::Cancelled; + // Annotate turns. + size_t skipTurnSegments = 0; + auto const & loadedSegments = result.GetSegments(); + for (auto loadedSegmentIt = loadedSegments.cbegin(); loadedSegmentIt != loadedSegments.cend(); + ++loadedSegmentIt) + { + // ETA information. + double const nodeTimeSeconds = loadedSegmentIt->m_weight; + + // Street names. I put empty names too, to avoid freezing old street name while riding on + // unnamed street. + streets.emplace_back(max(points.size(), static_cast(1)) - 1, loadedSegmentIt->m_name); + + // Turns information. + if (!points.empty() && skipTurnSegments == 0) + { + turns::TurnItem turnItem; + turnItem.m_index = static_cast(points.size() - 1); + + size_t segmentIndex = distance(loadedSegments.begin(), loadedSegmentIt); + skipTurnSegments = CheckUTurnOnRoute(loadedSegments, segmentIndex, turnItem); + + turns::TurnInfo turnInfo(loadedSegments[segmentIndex - 1], *loadedSegmentIt); + + if (turnItem.m_turn == turns::TurnDirection::NoTurn) + turns::GetTurnDirection(result, turnInfo, turnItem); + +#ifdef DEBUG + double distMeters = 0.0; + for (size_t k = lastIdx + 1; k < points.size(); ++k) + distMeters += MercatorBounds::DistanceOnEarth(points[k - 1], points[k]); + LOG(LDEBUG, ("Speed:", 3.6 * distMeters / nodeTimeSeconds, "kmph; Dist:", distMeters, "Time:", + nodeTimeSeconds, "s", lastIdx, "e", points.size(), "source:", + turnItem.m_sourceName, "target:", turnItem.m_targetName)); + lastIdx = points.size(); +#endif + times.push_back(Route::TTimeItem(points.size(), estimatedTime)); + + // Lane information. + if (turnItem.m_turn != turns::TurnDirection::NoTurn) + { + turnItem.m_lanes = turnInfo.m_ingoing.m_lanes; + turnsDir.push_back(move(turnItem)); + } + } + + estimatedTime += nodeTimeSeconds; + if (skipTurnSegments > 0) + --skipTurnSegments; + + // Path geometry. + points.insert(points.end(), loadedSegmentIt->m_path.begin(), loadedSegmentIt->m_path.end()); + } + + // Path found. Points will be replaced by start and end edges points. + if (points.size() == 1) + points.push_back(points.front()); + + if (points.size() < 2) + return IRouter::ResultCode::RouteNotFound; + + points.front() = result.GetStartPoint(); + points.back() = result.GetEndPoint(); + + times.push_back(Route::TTimeItem(points.size() - 1, estimatedTime)); + turnsDir.emplace_back(turns::TurnItem(static_cast(points.size()) - 1, + turns::TurnDirection::ReachedYourDestination)); + turns::FixupTurns(points, turnsDir); + +#ifdef DEBUG + for (auto t : turnsDir) + { + LOG(LDEBUG, (turns::GetTurnString(t.m_turn), ":", t.m_index, t.m_sourceName, "-", + t.m_targetName, "exit:", t.m_exitNum)); + } + + size_t last = 0; + double lastTime = 0; + for (Route::TTimeItem & t : times) + { + double dist = 0; + for (size_t i = last + 1; i <= t.first; ++i) + dist += MercatorBounds::DistanceOnEarth(points[i - 1], points[i]); + + double time = t.second - lastTime; + + LOG(LDEBUG, ("distance:", dist, "start:", last, "end:", t.first, "Time:", time, "Speed:", + 3.6 * dist / time)); + last = t.first; + lastTime = t.second; + } +#endif + LOG(LDEBUG, ("Estimated time:", estimatedTime, "s")); + return IRouter::ResultCode::NoError; +} + double CalculateMercatorDistanceAlongPath(uint32_t startPointIndex, uint32_t endPointIndex, vector const & points) { @@ -835,8 +789,7 @@ TurnDirection IntermediateDirection(const double angle) return FindDirectionByAngle(kLowerBounds, angle); } -void GetTurnDirection(Index const & index, RoutingMapping & mapping, TurnInfo & turnInfo, - TurnItem & turn) +void GetTurnDirection(IRoutingResultGraph const & result, TurnInfo & turnInfo, TurnItem & turn) { if (!turnInfo.IsSegmentsValid()) return; @@ -867,8 +820,8 @@ void GetTurnDirection(Index const & index, RoutingMapping & mapping, TurnInfo & ASSERT_GREATER(turnInfo.m_ingoing.m_path.size(), 1, ()); m2::PointD const ingoingPointOneSegment = turnInfo.m_ingoing.m_path[turnInfo.m_ingoing.m_path.size() - 2]; TTurnCandidates nodes; - GetPossibleTurns(index, turnInfo.m_ingoing.m_nodeId, ingoingPointOneSegment, junctionPoint, - mapping, nodes); + result.GetPossibleTurns(turnInfo.m_ingoing.m_nodeId, ingoingPointOneSegment, junctionPoint, + nodes); size_t const numNodes = nodes.size(); bool const hasMultiTurns = numNodes > 1; @@ -892,14 +845,15 @@ void GetTurnDirection(Index const & index, RoutingMapping & mapping, TurnInfo & if (turnInfo.m_ingoing.m_onRoundabout || turnInfo.m_outgoing.m_onRoundabout) { - bool const keepTurnByHighwayClass = KeepRoundaboutTurnByHighwayClass(turn.m_turn, nodes, turnInfo, index, mapping); + bool const keepTurnByHighwayClass = + KeepRoundaboutTurnByHighwayClass(turn.m_turn, nodes, turnInfo); turn.m_turn = GetRoundaboutDirection(turnInfo.m_ingoing.m_onRoundabout, turnInfo.m_outgoing.m_onRoundabout, hasMultiTurns, keepTurnByHighwayClass); return; } - bool const keepTurnByHighwayClass = KeepTurnByHighwayClass(turn.m_turn, nodes, turnInfo, index, mapping); + bool const keepTurnByHighwayClass = KeepTurnByHighwayClass(turn.m_turn, nodes, turnInfo); if (!turn.m_keepAnyway && !keepTurnByHighwayClass) { turn.m_turn = TurnDirection::NoTurn; @@ -911,7 +865,7 @@ void GetTurnDirection(Index const & index, RoutingMapping & mapping, TurnInfo & kNotSoCloseMinDistMeters, GetIngoingPointIndex); if (!KeepTurnByIngoingEdges(junctionPoint, notSoCloseToTheTurnPoint, outgoingPoint, hasMultiTurns, - mapping, index)) + nodes)) { turn.m_turn = TurnDirection::NoTurn; return; diff --git a/routing/turns_generator.hpp b/routing/turns_generator.hpp index a7725f04ca..ff2acbfffe 100644 --- a/routing/turns_generator.hpp +++ b/routing/turns_generator.hpp @@ -3,6 +3,7 @@ #include "routing/osrm2feature_map.hpp" #include "routing/osrm_engine.hpp" #include "routing/route.hpp" +#include "routing/router.hpp" #include "routing/turns.hpp" #include "std/function.hpp" @@ -61,6 +62,69 @@ private: FeatureGraphNode const & endGraphNode, bool isStartNode, bool isEndNode); }; +/*! + * \brief The TurnCandidate struct contains information about possible ways from a junction. + */ +struct TurnCandidate +{ + /*! + * angle is an angle of the turn in degrees. It means angle is 180 minus + * an angle between the current edge and the edge of the candidate. A counterclockwise rotation. + * The current edge is an edge which belongs the route and located before the junction. + * angle belongs to the range [-180; 180]; + */ + double angle; + /*! + * node is a possible node (a possible way) from the juction. + * May be NodeId for OSRM router or FeatureId::index for graph router. + */ + uint32_t node; + /*! + * \brief highwayClass field for the road class caching. Because feature reading is a long + * function. + */ + ftypes::HighwayClass highwayClass; + + TurnCandidate(double a, uint32_t n, ftypes::HighwayClass c) : angle(a), node(n), highwayClass(c) + { + } +}; + +using TTurnCandidates = vector; + +/*! + * \brief The IRoutingResultGraph interface for the routing result. Uncouple router from the + * annotation code that describes turns. See routers for detail implementations. + */ +class IRoutingResultGraph +{ +public: + virtual vector const & GetSegments() const = 0; + virtual void GetPossibleTurns(NodeID node, m2::PointD const & ingoingPoint, + m2::PointD const & junctionPoint, + TTurnCandidates & candidates) const = 0; + virtual double GetShortestPathLength() const = 0; + virtual m2::PointD const & GetStartPoint() const = 0; + virtual m2::PointD const & GetEndPoint() const = 0; + + virtual ~IRoutingResultGraph() {} +}; + +/*! + * \brief Compute turn and time estimation structs for the abstract route result. + * \param routingResult abstract routing result to annotate. + * \param delegate Routing callbacks delegate. + * \param points Storage for unpacked points of the path. + * \param turnsDir output turns annotation storage. + * \param times output times annotation storage. + * \param streets output street names along the path. + * \return routing operation result code. + */ +IRouter::ResultCode MakeTurnAnnotation(turns::IRoutingResultGraph const & result, + RouterDelegate const & delegate, vector & points, + Route::TTurns & turnsDir, Route::TTimes & times, + Route::TStreets & streets); + /*! * \brief The TurnInfo struct is a representation of a junction. * It has ingoing and outgoing edges and method to check if these edges are valid. @@ -141,7 +205,7 @@ TurnDirection GetRoundaboutDirection(bool isIngoingEdgeRoundabout, bool isOutgoi * \param turnInfo is used for cashing some information while turn calculation. * \param turn is used for keeping the result of turn calculation. */ -void GetTurnDirection(Index const & index, RoutingMapping & mapping, turns::TurnInfo & turnInfo, +void GetTurnDirection(IRoutingResultGraph const & result, turns::TurnInfo & turnInfo, TurnItem & turn); /*! From 8ab2a6dac3dd67106dec0f359b1e70c2aadf596b Mon Sep 17 00:00:00 2001 From: Lev Dragunov Date: Mon, 18 Apr 2016 18:33:48 +0300 Subject: [PATCH 2/3] Review fixes. --- routing/osrm_router.cpp | 22 ++++++++++--- routing/turns_generator.cpp | 66 +++++-------------------------------- routing/turns_generator.hpp | 3 +- 3 files changed, 28 insertions(+), 63 deletions(-) diff --git a/routing/osrm_router.cpp b/routing/osrm_router.cpp index f9782d6230..982461fd9a 100644 --- a/routing/osrm_router.cpp +++ b/routing/osrm_router.cpp @@ -68,7 +68,8 @@ public: } virtual void GetPossibleTurns(NodeID node, m2::PointD const & ingoingPoint, m2::PointD const & junctionPoint, - turns::TTurnCandidates & candidates) const override + size_t & ingoingCount, + turns::TTurnCandidates & outgoingTurns) const override { double const kReadCrossEpsilon = 1.0E-4; double const kFeaturesNearTurnMeters = 3.0; @@ -87,14 +88,21 @@ public: // Filtering virtual edges. vector adjacentNodes; + ingoingCount = 0; for (EdgeID const e : m_routingMapping.m_dataFacade.GetAdjacentEdgeRange(node)) { QueryEdge::EdgeData const data = m_routingMapping.m_dataFacade.GetEdgeData(e, node); - if (data.forward && !data.shortcut) + if (data.shortcut) + continue; + if (data.forward) { adjacentNodes.push_back(m_routingMapping.m_dataFacade.GetTarget(e)); ASSERT_NOT_EQUAL(m_routingMapping.m_dataFacade.GetTarget(e), SPECIAL_NODEID, ()); } + else + { + ++ingoingCount; + } } for (NodeID const adjacentNode : geomNodes) @@ -106,8 +114,12 @@ public: if (m_routingMapping.m_dataFacade.GetTarget(e) != node) continue; QueryEdge::EdgeData const data = m_routingMapping.m_dataFacade.GetEdgeData(e, adjacentNode); - if (!data.shortcut && data.backward) + if (data.shortcut) + continue; + if (data.backward) adjacentNodes.push_back(adjacentNode); + else + ++ingoingCount; } } @@ -132,10 +144,10 @@ public: double const a = my::RadToDeg(PiMinusTwoVectorsAngle(junctionPoint, ingoingPoint, outgoingPoint)); - candidates.emplace_back(a, targetNode, ftypes::GetHighwayClass(ft)); + outgoingTurns.emplace_back(a, targetNode, ftypes::GetHighwayClass(ft)); } - sort(candidates.begin(), candidates.end(), + sort(outgoingTurns.begin(), outgoingTurns.end(), [](turns::TurnCandidate const & t1, turns::TurnCandidate const & t2) { return t1.angle < t2.angle; diff --git a/routing/turns_generator.cpp b/routing/turns_generator.cpp index 794f91dc03..9336d6624e 100644 --- a/routing/turns_generator.cpp +++ b/routing/turns_generator.cpp @@ -40,51 +40,6 @@ double PiMinusTwoVectorsAngle(m2::PointD const & p, m2::PointD const & p1, m2::P return math::pi - ang::TwoVectorsAngle(p, p1, p2); } -/*! - * \brief The Point2Geometry class is responsable for looking for all adjacent to junctionPoint - * road network edges. Including the current edge. - */ -class Point2Geometry -{ - m2::PointD m_junctionPoint, m_ingoingPoint; - TGeomTurnCandidate & m_candidates; - -public: - Point2Geometry(m2::PointD const & junctionPoint, m2::PointD const & ingoingPoint, - TGeomTurnCandidate & candidates) - : m_junctionPoint(junctionPoint), m_ingoingPoint(ingoingPoint), m_candidates(candidates) - { - } - - void operator()(FeatureType const & ft) - { - if (!CarModel::Instance().IsRoad(ft)) - return; - ft.ParseGeometry(FeatureType::BEST_GEOMETRY); - size_t const count = ft.GetPointsCount(); - ASSERT_GREATER(count, 1, ()); - - // @TODO(vbykoianko) instead of checking all the feature points probably - // it's enough just to check the start and the finish of the feature. - for (size_t i = 0; i < count; ++i) - { - if (MercatorBounds::DistanceOnEarth(m_junctionPoint, ft.GetPoint(i)) < - kFeaturesNearTurnMeters) - { - if (i > 0) - m_candidates.push_back(my::RadToDeg( - PiMinusTwoVectorsAngle(m_junctionPoint, m_ingoingPoint, ft.GetPoint(i - 1)))); - if (i < count - 1) - m_candidates.push_back(my::RadToDeg( - PiMinusTwoVectorsAngle(m_junctionPoint, m_ingoingPoint, ft.GetPoint(i + 1)))); - return; - } - } - } - - DISALLOW_COPY_AND_MOVE(Point2Geometry); -}; - /*! * \brief Returns false when * - the route leads from one big road to another one; @@ -160,10 +115,11 @@ bool DiscardTurnByIngoingAndOutgoingEdges(TurnDirection intermediateDirection, turn.m_sourceName == turn.m_targetName; } +// turnEdgesCount calculates both ingoing ond outgoing edges without user's edge. bool KeepTurnByIngoingEdges(m2::PointD const & junctionPoint, m2::PointD const & ingoingPointOneSegment, m2::PointD const & outgoingPoint, bool hasMultiTurns, - TTurnCandidates const & nodes) + size_t const turnEdgesCount) { double const turnAngle = my::RadToDeg(PiMinusTwoVectorsAngle(junctionPoint, ingoingPointOneSegment, outgoingPoint)); @@ -172,7 +128,7 @@ bool KeepTurnByIngoingEdges(m2::PointD const & junctionPoint, // The code below is resposible for cases when there is only one way to leave the junction. // Such junction has to be kept as a turn when it's not a slight turn and it has ingoing edges // (one or more); - return hasMultiTurns || (!isGoStraightOrSlightTurn && nodes.size() > 2); + return hasMultiTurns || (!isGoStraightOrSlightTurn && turnEdgesCount > 1); } bool FixupLaneSet(TurnDirection turn, vector & lanes, @@ -474,12 +430,6 @@ bool TurnInfo::IsSegmentsValid() const return true; } -// @todo(vbykoianko) This method shall to be refactored. It shall be split into several -// methods. All the functionality shall be moved to the turns_generator unit. - -// @todo(vbykoianko) For the time being MakeTurnAnnotation generates the turn annotation -// and the route polyline at the same time. It is better to generate it separately -// to be able to use the route without turn annotation. IRouter::ResultCode MakeTurnAnnotation(turns::IRoutingResultGraph const & result, RouterDelegate const & delegate, vector & points, Route::TTurns & turnsDir, Route::TTimes & times, @@ -560,8 +510,7 @@ IRouter::ResultCode MakeTurnAnnotation(turns::IRoutingResultGraph const & result points.back() = result.GetEndPoint(); times.push_back(Route::TTimeItem(points.size() - 1, estimatedTime)); - turnsDir.emplace_back(turns::TurnItem(static_cast(points.size()) - 1, - turns::TurnDirection::ReachedYourDestination)); + turnsDir.emplace_back(turns::TurnItem(static_cast(points.size()) - 1, turns::TurnDirection::ReachedYourDestination)); turns::FixupTurns(points, turnsDir); #ifdef DEBUG @@ -797,6 +746,8 @@ void GetTurnDirection(IRoutingResultGraph const & result, TurnInfo & turnInfo, T ASSERT_LESS(MercatorBounds::DistanceOnEarth(turnInfo.m_ingoing.m_path.back(), turnInfo.m_outgoing.m_path.front()), kFeaturesNearTurnMeters, ()); + ASSERT(!turnInfo.m_ingoing.m_path.empty(), ()); + ASSERT(!turnInfo.m_outgoing.m_path.empty(), ()); m2::PointD const junctionPoint = turnInfo.m_ingoing.m_path.back(); m2::PointD const ingoingPoint = GetPointForTurn(turnInfo.m_ingoing.m_path, junctionPoint, @@ -820,8 +771,9 @@ void GetTurnDirection(IRoutingResultGraph const & result, TurnInfo & turnInfo, T ASSERT_GREATER(turnInfo.m_ingoing.m_path.size(), 1, ()); m2::PointD const ingoingPointOneSegment = turnInfo.m_ingoing.m_path[turnInfo.m_ingoing.m_path.size() - 2]; TTurnCandidates nodes; + size_t ingoingCount; result.GetPossibleTurns(turnInfo.m_ingoing.m_nodeId, ingoingPointOneSegment, junctionPoint, - nodes); + ingoingCount, nodes); size_t const numNodes = nodes.size(); bool const hasMultiTurns = numNodes > 1; @@ -865,7 +817,7 @@ void GetTurnDirection(IRoutingResultGraph const & result, TurnInfo & turnInfo, T kNotSoCloseMinDistMeters, GetIngoingPointIndex); if (!KeepTurnByIngoingEdges(junctionPoint, notSoCloseToTheTurnPoint, outgoingPoint, hasMultiTurns, - nodes)) + nodes.size() + ingoingCount)) { turn.m_turn = TurnDirection::NoTurn; return; diff --git a/routing/turns_generator.hpp b/routing/turns_generator.hpp index ff2acbfffe..ea96be9e33 100644 --- a/routing/turns_generator.hpp +++ b/routing/turns_generator.hpp @@ -102,7 +102,8 @@ public: virtual vector const & GetSegments() const = 0; virtual void GetPossibleTurns(NodeID node, m2::PointD const & ingoingPoint, m2::PointD const & junctionPoint, - TTurnCandidates & candidates) const = 0; + size_t & ingoingCount, + TTurnCandidates & outgoingTurns) const = 0; virtual double GetShortestPathLength() const = 0; virtual m2::PointD const & GetStartPoint() const = 0; virtual m2::PointD const & GetEndPoint() const = 0; From 82f3d1894c7bf3c622ac34e10f95b3b34cbe43e1 Mon Sep 17 00:00:00 2001 From: Lev Dragunov Date: Tue, 19 Apr 2016 13:51:25 +0300 Subject: [PATCH 3/3] Ongoing fix. --- routing/turns_generator.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/routing/turns_generator.cpp b/routing/turns_generator.cpp index 9336d6624e..af4606190c 100644 --- a/routing/turns_generator.cpp +++ b/routing/turns_generator.cpp @@ -743,11 +743,11 @@ void GetTurnDirection(IRoutingResultGraph const & result, TurnInfo & turnInfo, T if (!turnInfo.IsSegmentsValid()) return; + ASSERT(!turnInfo.m_ingoing.m_path.empty(), ()); + ASSERT(!turnInfo.m_outgoing.m_path.empty(), ()); ASSERT_LESS(MercatorBounds::DistanceOnEarth(turnInfo.m_ingoing.m_path.back(), turnInfo.m_outgoing.m_path.front()), kFeaturesNearTurnMeters, ()); - ASSERT(!turnInfo.m_ingoing.m_path.empty(), ()); - ASSERT(!turnInfo.m_outgoing.m_path.empty(), ()); m2::PointD const junctionPoint = turnInfo.m_ingoing.m_path.back(); m2::PointD const ingoingPoint = GetPointForTurn(turnInfo.m_ingoing.m_path, junctionPoint,