diff --git a/routing/osrm2feature_map.hpp b/routing/osrm2feature_map.hpp index 4fb7d873c2..217540d26b 100644 --- a/routing/osrm2feature_map.hpp +++ b/routing/osrm2feature_map.hpp @@ -88,11 +88,11 @@ namespace OsrmMappingTypes }; #pragma pack (pop) -/// Checks if a smallSeg is inside a bigSeg and at least one point of a smallSeg is differ from +/// Checks if a smallSeg is inside a bigSeg and at least one point of a smallSeg differs from /// point of a bigSeg. Note that the smallSeg must be an ordered segment with 1 point length. bool IsInside(FtSeg const & bigSeg, FtSeg const & smallSeg); -/// Splits segment by splitter segment and take part of it. +/// Splits segment by splitter segment and takes part of it. /// Warning this function includes a whole splitter segment to a result segment described by the /// resultFromLeft variable. FtSeg SplitSegment(FtSeg const & segment, FtSeg const & splitter, bool const resultFromLeft); diff --git a/routing/osrm_helpers.cpp b/routing/osrm_helpers.cpp index f66020e342..1b35639d1a 100644 --- a/routing/osrm_helpers.cpp +++ b/routing/osrm_helpers.cpp @@ -71,7 +71,7 @@ void Point2PhantomNode::CalculateWeight(OsrmMappingTypes::FtSeg const & seg, bool calcFromRight, int & weight, int & offset) const { // nodeId can be INVALID_NODE_ID when reverse node is absent. This node has no weight. - if (nodeId == INVALID_NODE_ID || m_routingMapping.m_dataFacade.GetOutDegree(nodeId) == 0) + if (nodeId == INVALID_NODE_ID) { offset = 0; weight = 0; @@ -108,25 +108,30 @@ void Point2PhantomNode::CalculateWeight(OsrmMappingTypes::FtSeg const & seg, loader.GetFeatureByIndex(segment.m_fid, ft); ft.ParseGeometry(FeatureType::BEST_GEOMETRY); + // Find whole edge weight by node outgoing point. + if (segmentIndex == range.second - 1) + minWeight = GetMinNodeWeight(nodeId, ft.GetPoint(segment.m_pointEnd)); + + // Calculate distances. + double distance = CalculateDistance(ft, segment.m_pointStart, segment.m_pointEnd); + fullDistanceM += distance; + if (foundSeg) + continue; + if (segment.m_fid == seg.m_fid && OsrmMappingTypes::IsInside(segment, seg)) { auto const splittedSegment = OsrmMappingTypes::SplitSegment(segment, seg, !calcFromRight); distanceM += CalculateDistance(ft, splittedSegment.m_pointStart, splittedSegment.m_pointEnd); // node.m_seg always forward ordered (m_pointStart < m_pointEnd) distanceM -= MercatorBounds::DistanceOnEarth( - ft.GetPoint(calcFromRight ? seg.m_pointEnd : seg.m_pointStart), segPt); + ft.GetPoint(calcFromRight ? seg.m_pointStart : seg.m_pointEnd), segPt); foundSeg = true; } - - double distance = CalculateDistance(ft, segment.m_pointStart, segment.m_pointEnd); - if (!foundSeg) + else + { distanceM += distance; - fullDistanceM += distance; - - //Find whole edge weight by node outgoing point. - if (segmentIndex == range.second - 1) - minWeight = GetMinNodeWeight(nodeId, ft.GetPoint(segment.m_pointEnd)); + } } ASSERT_GREATER(fullDistanceM, 0, ("No valid segments on the edge.")); @@ -142,17 +147,17 @@ void Point2PhantomNode::CalculateWeight(OsrmMappingTypes::FtSeg const & seg, EdgeWeight Point2PhantomNode::GetMinNodeWeight(NodeID node, m2::PointD const & point) const { - static double const kInfinity = numeric_limits::infinity(); - static double const kReadCrossRadiusM = 1.0E-4; + static double const kInfinity = numeric_limits::max(); + static double const kReadCrossRadiusMercator = 1.0E-4; EdgeWeight minWeight = kInfinity; // Geting nodes by geometry. vector geomNodes; Point2Node p2n(m_routingMapping, geomNodes); m_index.ForEachInRectForMWM(p2n, - m2::RectD(point.x - kReadCrossRadiusM, point.y - kReadCrossRadiusM, - point.x + kReadCrossRadiusM, point.y + kReadCrossRadiusM), - scales::GetUpperScale(), m_routingMapping.GetMwmId()); + m2::RectD(point.x - kReadCrossRadiusMercator, point.y - kReadCrossRadiusMercator, + point.x + kReadCrossRadiusMercator, point.y + kReadCrossRadiusMercator), + scales::GetUpperScale(), m_routingMapping.GetMwmId()); sort(geomNodes.begin(), geomNodes.end()); geomNodes.erase(unique(geomNodes.begin(), geomNodes.end()), geomNodes.end()); @@ -162,9 +167,7 @@ EdgeWeight Point2PhantomNode::GetMinNodeWeight(NodeID node, m2::PointD const & p { QueryEdge::EdgeData const data = m_routingMapping.m_dataFacade.GetEdgeData(e, node); if (data.forward && !data.shortcut) - { minWeight = min(minWeight, data.distance); - } } for (NodeID const & adjacentNode : geomNodes) @@ -272,9 +275,9 @@ void Point2PhantomNode::CalculateWeights(FeatureGraphNode & node) const // Need to initialize weights for correct work of PhantomNode::GetForwardWeightPlusOffset // and PhantomNode::GetReverseWeightPlusOffset. CalculateWeight(node.segment, node.segmentPoint, node.node.forward_node_id, - true /* calcFromRight */, node.node.forward_weight, node.node.forward_offset); + false /* calcFromRight */, node.node.forward_weight, node.node.forward_offset); CalculateWeight(node.segment, node.segmentPoint, node.node.reverse_node_id, - false /* calcFromRight */, node.node.reverse_weight, node.node.reverse_offset); + true /* calcFromRight */, node.node.reverse_weight, node.node.reverse_offset); } void Point2Node::operator()(FeatureType const & ft) diff --git a/routing/osrm_helpers.hpp b/routing/osrm_helpers.hpp index 773b27556a..01871997dd 100644 --- a/routing/osrm_helpers.hpp +++ b/routing/osrm_helpers.hpp @@ -36,10 +36,10 @@ public: {} }; - // Finds nearest segment of a feature geometry. + // Finds nearest segment to a feature geometry. static void FindNearestSegment(FeatureType const & ft, m2::PointD const & point, Candidate & res); - // Sets point from where calculate weights. + // Sets point from where weights are calculated. void SetPoint(m2::PointD const & pt) { m_point = pt; } // Returns true if there are candidate features for routing tasks. diff --git a/routing/osrm_router.cpp b/routing/osrm_router.cpp index 73865cb3ad..80d9607637 100644 --- a/routing/osrm_router.cpp +++ b/routing/osrm_router.cpp @@ -50,6 +50,8 @@ 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.; } // namespace // TODO (ldragunov) Switch all RawRouteData and incapsulate to own omim types. using RawRouteData = InternalRouteResult; @@ -436,8 +438,7 @@ OsrmRouter::ResultCode OsrmRouter::MakeTurnAnnotation( turns::GetTurnDirection(*m_pIndex, turnInfo, turnItem); // ETA information. - // Osrm multiples seconds to 10, so we need to divide it back. - double const nodeTimeSeconds = pathData.segmentWeight / 10.0; + double const nodeTimeSeconds = pathData.segmentWeight * kOSRMWeightToSecondsMultiplier; #ifdef DEBUG double distMeters = 0.0; @@ -477,15 +478,43 @@ OsrmRouter::ResultCode OsrmRouter::MakeTurnAnnotation( //m_mapping.DumpSegmentByNode(path_data.node); - //Do not put out node geometry (we do not have it)! + bool const isStartSegment = (segmentIndex == 0); + bool const isEndSegment = (segmentIndex == numSegments - 1); + // Calculate estimated time for a start and a end node cases. + if (isStartSegment || isEndSegment) + { + double multiplier = 1.; + double weight = 0.; + if (isStartSegment) + { + // -1 because a whole node weight is already in esimated time, and we need to substruct time + // form a node start to a user point. + multiplier = -1.; + auto const & node = routingResult.sourceEdge.node; + if (pathSegments[segmentIndex].node == node.forward_node_id) + weight = node.forward_weight; + else + weight = node.reverse_weight; + } + if (isEndSegment) + { + auto const & node = routingResult.targetEdge.node; + if (pathSegments[segmentIndex].node == node.forward_node_id) + weight = node.forward_weight; + else + weight = node.reverse_weight; + } + estimatedTime += multiplier * kOSRMWeightToSecondsMultiplier * weight; + } + size_t startK = 0, endK = buffer.size(); - if (segmentIndex == 0) + if (isStartSegment) { if (!segBegin.IsValid()) continue; startK = FindIntersectingSeg(segBegin); } - if (segmentIndex + 1 == numSegments) + if (isEndSegment) { if (!segEnd.IsValid()) continue; @@ -503,40 +532,21 @@ OsrmRouter::ResultCode OsrmRouter::MakeTurnAnnotation( auto startIdx = seg.m_pointStart; auto endIdx = seg.m_pointEnd; - if (segmentIndex == 0) - { - if (pathSegments[segmentIndex].node == routingResult.sourceEdge.node.forward_node_id) - estimatedTime += -routingResult.sourceEdge.node.forward_weight/10; - else - estimatedTime += -routingResult.sourceEdge.node.reverse_weight/10; - } - if (segmentIndex == numSegments - 1) - { - if (pathSegments[segmentIndex].node == routingResult.sourceEdge.node.forward_node_id) - estimatedTime += routingResult.sourceEdge.node.forward_weight/10; - else - estimatedTime += routingResult.sourceEdge.node.reverse_weight/10; - } - - if (segmentIndex == 0 && k == startK && segBegin.IsValid()) + if (isStartSegment && k == startK && segBegin.IsValid()) startIdx = (seg.m_pointEnd > seg.m_pointStart) ? segBegin.m_pointStart : segBegin.m_pointEnd; - if (segmentIndex == numSegments - 1 && k == endK - 1 && segEnd.IsValid()) + if (isEndSegment && k == endK - 1 && segEnd.IsValid()) endIdx = (seg.m_pointEnd > seg.m_pointStart) ? segEnd.m_pointEnd : segEnd.m_pointStart; - if (seg.m_pointEnd > seg.m_pointStart) + if (startIdx < endIdx) { for (auto idx = startIdx; idx <= endIdx; ++idx) - { points.push_back(ft.GetPoint(idx)); - } } else { - for (auto idx = startIdx; idx > endIdx; --idx) - { + // I use big signed type because endIdx can be 0. + for (int64_t idx = startIdx; idx >= endIdx; --idx) points.push_back(ft.GetPoint(idx)); - } - points.push_back(ft.GetPoint(endIdx)); } } }