From 8d52b0e54123f623c5556c0af5a4b50880b8ea84 Mon Sep 17 00:00:00 2001 From: Lev Dragunov Date: Tue, 6 Oct 2015 17:50:12 +0300 Subject: [PATCH 1/4] [routing] New node start weight calculation. --- generator/feature_builder.cpp | 3 +- generator/generator_tests/osm_type_test.cpp | 2 +- routing/car_model.cpp | 7 + routing/car_model.hpp | 4 +- routing/osrm2feature_map.cpp | 62 +++- routing/osrm2feature_map.hpp | 11 +- routing/osrm_helpers.cpp | 298 ++++++++++++++++ routing/osrm_helpers.hpp | 95 ++++++ routing/osrm_router.cpp | 323 +----------------- routing/routing.pro | 2 + .../osrm_route_test.cpp | 7 +- .../routing_tests/routing_mapping_test.cpp | 32 ++ routing/turns_generator.cpp | 33 +- 13 files changed, 519 insertions(+), 360 deletions(-) create mode 100644 routing/osrm_helpers.cpp create mode 100644 routing/osrm_helpers.hpp diff --git a/generator/feature_builder.cpp b/generator/feature_builder.cpp index 8aba7ab405..1ee871fdab 100644 --- a/generator/feature_builder.cpp +++ b/generator/feature_builder.cpp @@ -196,9 +196,8 @@ namespace bool FeatureBuilder1::IsRoad() const { - static routing::CarModel const carModel; static routing::PedestrianModel const pedModel; - return carModel.IsRoad(m_params.m_Types) || pedModel.IsRoad(m_params.m_Types); + return routing::CarModel::Instance().IsRoad(m_params.m_Types) || pedModel.IsRoad(m_params.m_Types); } bool FeatureBuilder1::PreSerialize() diff --git a/generator/generator_tests/osm_type_test.cpp b/generator/generator_tests/osm_type_test.cpp index 1054ad0ca0..63e36888e4 100644 --- a/generator/generator_tests/osm_type_test.cpp +++ b/generator/generator_tests/osm_type_test.cpp @@ -550,7 +550,7 @@ UNIT_TEST(OsmType_Hwtag) UNIT_TEST(OsmType_Ferry) { - routing::CarModel carModel; + routing::CarModel const & carModel = routing::CarModel::Instance(); char const * arr[][2] = { { "motorcar", "yes" }, diff --git a/routing/car_model.cpp b/routing/car_model.cpp index f8dd438adb..f63ae0cb21 100644 --- a/routing/car_model.cpp +++ b/routing/car_model.cpp @@ -52,4 +52,11 @@ CarModel::CarModel() SetAdditionalRoadTypes(classif(), arr, ARRAY_SIZE(arr)); } +// static +CarModel const & CarModel::Instance() +{ + static CarModel const instance; + return instance; +} + } // namespace routing diff --git a/routing/car_model.hpp b/routing/car_model.hpp index 60d7226058..8170df3a75 100644 --- a/routing/car_model.hpp +++ b/routing/car_model.hpp @@ -7,8 +7,10 @@ namespace routing class CarModel : public VehicleModel { -public: CarModel(); + +public: + static CarModel const & Instance(); }; } // namespace routing diff --git a/routing/osrm2feature_map.cpp b/routing/osrm2feature_map.cpp index beb889908c..7e3d109855 100644 --- a/routing/osrm2feature_map.cpp +++ b/routing/osrm2feature_map.cpp @@ -32,7 +32,9 @@ const routing::TNodesList kEmptyList; namespace routing { -OsrmMappingTypes::FtSeg::FtSeg(uint32_t fid, uint32_t ps, uint32_t pe) +namespace OsrmMappingTypes +{ +FtSeg::FtSeg(uint32_t fid, uint32_t ps, uint32_t pe) : m_fid(fid), m_pointStart(static_cast(ps)), m_pointEnd(static_cast(pe)) @@ -42,17 +44,17 @@ OsrmMappingTypes::FtSeg::FtSeg(uint32_t fid, uint32_t ps, uint32_t pe) CHECK_EQUAL(m_pointEnd, pe, ()); } -OsrmMappingTypes::FtSeg::FtSeg(uint64_t x) +FtSeg::FtSeg(uint64_t x) : m_fid(x & 0xFFFFFFFF), m_pointStart(x >> 48), m_pointEnd((x >> 32) & 0xFFFF) { } -uint64_t OsrmMappingTypes::FtSeg::Store() const +uint64_t FtSeg::Store() const { return (uint64_t(m_pointStart) << 48) + (uint64_t(m_pointEnd) << 32) + uint64_t(m_fid); } -bool OsrmMappingTypes::FtSeg::Merge(FtSeg const & other) +bool FtSeg::Merge(FtSeg const & other) { if (other.m_fid != m_fid) return false; @@ -79,7 +81,7 @@ bool OsrmMappingTypes::FtSeg::Merge(FtSeg const & other) return false; } -bool OsrmMappingTypes::FtSeg::IsIntersect(FtSeg const & other) const +bool FtSeg::IsIntersect(FtSeg const & other) const { if (other.m_fid != m_fid) return false; @@ -92,7 +94,7 @@ bool OsrmMappingTypes::FtSeg::IsIntersect(FtSeg const & other) const return my::IsIntersect(s1, e1, s2, e2); } -string OsrmMappingTypes::DebugPrint(OsrmMappingTypes::FtSeg const & seg) +string DebugPrint(FtSeg const & seg) { stringstream ss; ss << "{ fID = " << seg.m_fid << @@ -101,13 +103,59 @@ string OsrmMappingTypes::DebugPrint(OsrmMappingTypes::FtSeg const & seg) return ss.str(); } -string OsrmMappingTypes::DebugPrint(OsrmMappingTypes::SegOffset const & off) +string DebugPrint(SegOffset const & off) { stringstream ss; ss << "{ " << off.m_nodeId << ", " << off.m_offset << " }"; return ss.str(); } +bool IsInside(FtSeg const & bigSeg, FtSeg const & smallSeg) +{ + ASSERT_EQUAL(bigSeg.m_fid, smallSeg.m_fid, ()); + ASSERT_EQUAL(smallSeg.m_pointEnd - smallSeg.m_pointStart, 1, ()); + + auto segmentLeft = min(bigSeg.m_pointStart, bigSeg.m_pointEnd); + auto segmentRight = max(bigSeg.m_pointEnd, bigSeg.m_pointStart); + + return (smallSeg.m_pointStart != segmentLeft || smallSeg.m_pointEnd != segmentRight) && + (segmentLeft <= smallSeg.m_pointStart && segmentRight >= smallSeg.m_pointEnd); +} + +FtSeg SplitSegment(FtSeg const & segment, FtSeg const & splitter, bool const resultFromLeft) +{ + FtSeg resultSeg; + resultSeg.m_fid = segment.m_fid; + + if (segment.m_pointStart < segment.m_pointEnd) + { + if (resultFromLeft) + { + resultSeg.m_pointStart = segment.m_pointStart; + resultSeg.m_pointEnd = splitter.m_pointEnd; + } + else + { + resultSeg.m_pointStart = splitter.m_pointStart; + resultSeg.m_pointEnd = segment.m_pointEnd; + } + } + else + { + if (resultFromLeft) + { + resultSeg.m_pointStart = segment.m_pointStart; + resultSeg.m_pointEnd = splitter.m_pointStart; + } + else + { + resultSeg.m_pointStart = splitter.m_pointEnd; + resultSeg.m_pointEnd = segment.m_pointEnd; + } + } + return resultSeg; +} +} // namespace OsrmMappingTypes void OsrmFtSegMapping::Clear() { diff --git a/routing/osrm2feature_map.hpp b/routing/osrm2feature_map.hpp index e285c7f6ce..5cd35a36ac 100644 --- a/routing/osrm2feature_map.hpp +++ b/routing/osrm2feature_map.hpp @@ -95,7 +95,16 @@ namespace OsrmMappingTypes } }; #pragma pack (pop) -} + +/// Checks if a smallSeg is inside a bigSeg and at least one point of a smallSeg is differ 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. +/// 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); +} // namespace OsrmMappingTypes class OsrmFtSegMapping; diff --git a/routing/osrm_helpers.cpp b/routing/osrm_helpers.cpp new file mode 100644 index 0000000000..42d60f3481 --- /dev/null +++ b/routing/osrm_helpers.cpp @@ -0,0 +1,298 @@ +#include "car_model.hpp" +#include "osrm_helpers.hpp" + +#include "indexer/scales.hpp" + +#include "geometry/distance.hpp" + +namespace routing +{ +namespace helpers +{ +// static +void Point2PhantomNode::FindNearestSegment(FeatureType const & ft, m2::PointD const & point, + Candidate & res) +{ + ft.ParseGeometry(FeatureType::BEST_GEOMETRY); + + size_t const count = ft.GetPointsCount(); + uint32_t const featureId = ft.GetID().m_index; + ASSERT_GREATER(count, 1, ()); + for (size_t i = 1; i < count; ++i) + { + m2::ProjectionToSection segProj; + segProj.SetBounds(ft.GetPoint(i - 1), ft.GetPoint(i)); + + m2::PointD const pt = segProj(point); + double const d = point.SquareLength(pt); + if (d < res.m_dist) + { + res.m_dist = d; + res.m_fid = featureId; + res.m_segIdx = static_cast(i - 1); + res.m_point = pt; + } + } +} + +void Point2PhantomNode::operator()(FeatureType const & ft) +{ + //TODO(gardster) Extract GEOM_LINE check into CatModel. + if (ft.GetFeatureType() != feature::GEOM_LINE || !CarModel::Instance().IsRoad(ft)) + return; + + Candidate res; + + FindNearestSegment(ft, m_point, res); + + if (res.m_fid != kInvalidFid) + m_candidates.push_back(res); +} + +double Point2PhantomNode::CalculateDistance(OsrmMappingTypes::FtSeg const & s) const +{ + ASSERT_NOT_EQUAL(s.m_pointStart, s.m_pointEnd, ()); + + Index::FeaturesLoaderGuard loader(m_index, m_routingMapping.GetMwmId()); + FeatureType ft; + loader.GetFeatureByIndex(s.m_fid, ft); + ft.ParseGeometry(FeatureType::BEST_GEOMETRY); + + double distMeters = 0.0; + size_t const n = max(s.m_pointEnd, s.m_pointStart); + size_t i = min(s.m_pointStart, s.m_pointEnd) + 1; + do + { + distMeters += MercatorBounds::DistanceOnEarth(ft.GetPoint(i - 1), ft.GetPoint(i)); + ++i; + } while (i <= n); + + return distMeters; +} + +void Point2PhantomNode::CalculateWeight(OsrmMappingTypes::FtSeg const & seg, + m2::PointD const & segPt, NodeID const & nodeId, + bool calcFromRight, int & weight) 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) + { + weight = 0; + return; + } + + // Offset is measured in milliseconds. We don't know about speed restrictions on the road. + // So we find it by a whole edge weight. + // Distance from the node border to the projection point is in meters. + double distanceM = 0; + auto const range = m_routingMapping.m_segMapping.GetSegmentsRange(nodeId); + OsrmMappingTypes::FtSeg segment; + + size_t startIndex = calcFromRight ? range.second - 1 : range.first; + size_t endIndex = calcFromRight ? range.first - 1 : range.second; + int indexIncrement = calcFromRight ? -1 : 1; + + for (size_t segmentIndex = startIndex; segmentIndex != endIndex; segmentIndex += indexIncrement) + { + m_routingMapping.m_segMapping.GetSegmentByIndex(segmentIndex, segment); + if (!segment.IsValid()) + continue; + + if (segment.m_fid == seg.m_fid && OsrmMappingTypes::IsInside(segment, seg)) + { + distanceM += CalculateDistance(OsrmMappingTypes::SplitSegment(segment, seg, !calcFromRight)); + break; + } + + distanceM += CalculateDistance(segment); + } + + Index::FeaturesLoaderGuard loader(m_index, m_routingMapping.GetMwmId()); + FeatureType ft; + loader.GetFeatureByIndex(seg.m_fid, ft); + ft.ParseGeometry(FeatureType::BEST_GEOMETRY); + + // node.m_seg always forward ordered (m_pointStart < m_pointEnd) + distanceM -= MercatorBounds::DistanceOnEarth( + ft.GetPoint(calcFromRight ? seg.m_pointEnd : seg.m_pointStart), segPt); + + // Whole node distance in meters. + double fullDistanceM = 0.0; + for (size_t segmentIndex = startIndex; segmentIndex != endIndex; segmentIndex += indexIncrement) + { + m_routingMapping.m_segMapping.GetSegmentByIndex(segmentIndex, segment); + if (!segment.IsValid()) + continue; + fullDistanceM += CalculateDistance(segment); + } + + ASSERT_GREATER(fullDistanceM, 0, ("No valid segments on the edge.")); + double const ratio = (fullDistanceM == 0) ? 0 : distanceM / fullDistanceM; + ASSERT_LESS_OR_EQUAL(ratio, 1., ()); + + // Minimal OSRM edge weight in milliseconds. + EdgeWeight minWeight = 0; + + if (segment.IsValid()) + { + FeatureType ft; + loader.GetFeatureByIndex(segment.m_fid, ft); + ft.ParseGeometry(FeatureType::BEST_GEOMETRY); + minWeight = GetMinNodeWeight(nodeId, ft.GetPoint(segment.m_pointEnd)); + } + weight = max(static_cast(minWeight * ratio), 0); +} + +EdgeWeight Point2PhantomNode::GetMinNodeWeight(NodeID node, m2::PointD const & point) const +{ + static double const kInfinity = numeric_limits::infinity(); + static double const kReadCrossRadiusM = 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()); + + sort(geomNodes.begin(), geomNodes.end()); + geomNodes.erase(unique(geomNodes.begin(), geomNodes.end()), geomNodes.end()); + + // Filtering virtual edges. + 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) + { + minWeight = min(minWeight, data.distance); + } + } + + 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) + minWeight = min(minWeight, data.distance); + } + } + // If we can't determine edge weight. + if (minWeight == kInfinity) + return 0; + return minWeight; +} + +void Point2PhantomNode::MakeResult(vector & res, size_t maxCount, + string const & mwmName) +{ + vector segments; + + segments.resize(maxCount); + + OsrmFtSegMapping::FtSegSetT segmentSet; + sort(m_candidates.begin(), m_candidates.end(), [](Candidate const & r1, Candidate const & r2) + { + return (r1.m_dist < r2.m_dist); + }); + + size_t const n = min(m_candidates.size(), maxCount); + for (size_t j = 0; j < n; ++j) + { + OsrmMappingTypes::FtSeg & seg = segments[j]; + Candidate const & c = m_candidates[j]; + + seg.m_fid = c.m_fid; + seg.m_pointStart = c.m_segIdx; + seg.m_pointEnd = c.m_segIdx + 1; + + segmentSet.insert(&seg); + } + + OsrmFtSegMapping::OsrmNodesT nodes; + m_routingMapping.m_segMapping.GetOsrmNodes(segmentSet, nodes); + + res.clear(); + res.resize(maxCount); + + for (size_t j = 0; j < maxCount; ++j) + { + if (!segments[j].IsValid()) + continue; + + auto it = nodes.find(segments[j].Store()); + if (it == nodes.cend()) + continue; + + FeatureGraphNode & node = res[j]; + + if (!m_direction.IsAlmostZero()) + { + // Filter income nodes by direction mode + OsrmMappingTypes::FtSeg const & node_seg = segments[j]; + FeatureType feature; + Index::FeaturesLoaderGuard loader(m_index, m_routingMapping.GetMwmId()); + loader.GetFeatureByIndex(node_seg.m_fid, feature); + feature.ParseGeometry(FeatureType::BEST_GEOMETRY); + m2::PointD const featureDirection = feature.GetPoint(node_seg.m_pointEnd) - feature.GetPoint(node_seg.m_pointStart); + bool const sameDirection = (m2::DotProduct(featureDirection, m_direction) > 0); + if (sameDirection) + { + node.node.forward_node_id = it->second.first; + node.node.reverse_node_id = INVALID_NODE_ID; + } + else + { + node.node.forward_node_id = INVALID_NODE_ID; + node.node.reverse_node_id = it->second.second; + } + } + else + { + node.node.forward_node_id = it->second.first; + node.node.reverse_node_id = it->second.second; + } + + node.segment = segments[j]; + node.segmentPoint = m_candidates[j].m_point; + node.mwmName = mwmName; + + CalculateOffsets(node); + } + res.erase(remove_if(res.begin(), res.end(), + [](FeatureGraphNode const & f) + { + return f.mwmName.empty(); + }), + res.end()); +} + +void Point2PhantomNode::CalculateOffsets(FeatureGraphNode & node) const +{ + CalculateWeight(node.segment, node.segmentPoint, node.node.forward_node_id, + true /* calcFromRight */, node.node.forward_weight); + CalculateWeight(node.segment, node.segmentPoint, node.node.reverse_node_id, + false /* calcFromRight */, node.node.reverse_weight); + + // Need to initialize weights for correct work of PhantomNode::GetForwardWeightPlusOffset + // and PhantomNode::GetReverseWeightPlusOffset. + node.node.forward_offset = 0; + node.node.reverse_offset = 0; +} + +void Point2Node::operator()(FeatureType const & ft) +{ + if (ft.GetFeatureType() != feature::GEOM_LINE || !CarModel::Instance().IsRoad(ft)) + return; + uint32_t const featureId = ft.GetID().m_index; + for (auto const n : m_routingMapping.m_segMapping.GetNodeIdByFid(featureId)) + n_nodeIds.push_back(n); +} +} // namespace helpers +} // namespace routing diff --git a/routing/osrm_helpers.hpp b/routing/osrm_helpers.hpp new file mode 100644 index 0000000000..730f39498a --- /dev/null +++ b/routing/osrm_helpers.hpp @@ -0,0 +1,95 @@ +#pragma once + +#include "geometry/point2d.hpp" + +#include "osrm2feature_map.hpp" +#include "osrm_data_facade.hpp" +#include "osrm_engine.hpp" +#include "routing_mapping.hpp" + +#include "indexer/index.hpp" + +namespace routing +{ +namespace helpers +{ +/// Class-getter for making routing tasks by geometry point. +class Point2PhantomNode +{ +public: + Point2PhantomNode(RoutingMapping const & mapping, Index const & index, + m2::PointD const & direction) + : m_direction(direction), m_index(index), m_routingMapping(mapping) + { + } + + struct Candidate + { + // Square distance from point to geometry in meters. + double m_dist; + uint32_t m_segIdx; + uint32_t m_fid; + m2::PointD m_point; + + Candidate() : m_dist(numeric_limits::max()), + m_segIdx(numeric_limits::max()), m_fid(kInvalidFid) + {} + }; + + // Finds nearest segment of a feature geometry. + static void FindNearestSegment(FeatureType const & ft, m2::PointD const & point, Candidate & res); + + // Sets point from where calculate weights. + void SetPoint(m2::PointD const & pt) { m_point = pt; } + + // Returns true if there are candidate features for routing tasks. + bool HasCandidates() const { return !m_candidates.empty(); } + + // Getter method. + void operator()(FeatureType const & ft); + + /// Makes OSRM tasks result vector. + void MakeResult(vector & res, size_t maxCount, string const & mwmName); + +private: + // Calculates whole segment distance in meters. + double CalculateDistance(OsrmMappingTypes::FtSeg const & s) const; + + /// Calculates part of a node weight in the OSRM format. Projection point @segPt divides node on + /// two parts. So we find weight of a part, set by the @calcFromRight parameter. + void CalculateWeight(OsrmMappingTypes::FtSeg const & seg, m2::PointD const & segPt, + NodeID const & nodeId, bool calcFromRight, int & weight) const; + + /// Returns minimal weight of the node. + EdgeWeight GetMinNodeWeight(NodeID node, m2::PointD const & point) const; + + /// Calculates weights and offsets section of the routing tasks. + void CalculateOffsets(FeatureGraphNode & node) const; + + m2::PointD m_point; + m2::PointD const m_direction; + buffer_vector m_candidates; + Index const & m_index; + RoutingMapping const & m_routingMapping; + + DISALLOW_COPY(Point2PhantomNode); +}; + +/// Class-getter for finding OSRM nodes near geometry point. +class Point2Node +{ + RoutingMapping const & m_routingMapping; + vector & n_nodeIds; + +public: + Point2Node(RoutingMapping const & routingMapping, vector & nodeID) + : m_routingMapping(routingMapping), n_nodeIds(nodeID) + { + } + + void operator()(FeatureType const & ft); + + DISALLOW_COPY_AND_MOVE(Point2Node); +}; +} // namespace helpers +} // namespace routing diff --git a/routing/osrm_router.cpp b/routing/osrm_router.cpp index 24174242c6..038e35e60d 100644 --- a/routing/osrm_router.cpp +++ b/routing/osrm_router.cpp @@ -2,6 +2,7 @@ #include "cross_mwm_router.hpp" #include "online_cross_fetcher.hpp" #include "osrm2feature_map.hpp" +#include "osrm_helpers.hpp" #include "osrm_router.hpp" #include "turns_generator.hpp" @@ -54,318 +55,6 @@ double constexpr kPathFoundProgress = 70.0f; // TODO (ldragunov) Switch all RawRouteData and incapsulate to own omim types. using RawRouteData = InternalRouteResult; -namespace -{ - -class Point2PhantomNode -{ - -public: - Point2PhantomNode(OsrmFtSegMapping const & mapping, Index const * pIndex, - m2::PointD const & direction, TDataFacade const & facade) - : m_direction(direction), m_mapping(mapping), m_pIndex(pIndex), m_dataFacade(facade) - { - } - - struct Candidate - { - double m_dist; - uint32_t m_segIdx; - uint32_t m_fid; - m2::PointD m_point; - - Candidate() : m_dist(numeric_limits::max()), m_fid(kInvalidFid) {} - }; - - static void FindNearestSegment(FeatureType const & ft, m2::PointD const & point, Candidate & res) - { - ft.ParseGeometry(FeatureType::BEST_GEOMETRY); - - size_t const count = ft.GetPointsCount(); - uint32_t const featureId = ft.GetID().m_index; - ASSERT_GREATER(count, 1, ()); - for (size_t i = 1; i < count; ++i) - { - m2::ProjectionToSection segProj; - segProj.SetBounds(ft.GetPoint(i - 1), ft.GetPoint(i)); - - m2::PointD const pt = segProj(point); - double const d = point.SquareLength(pt); - if (d < res.m_dist) - { - res.m_dist = d; - res.m_fid = featureId; - res.m_segIdx = static_cast(i - 1); - res.m_point = pt; - } - } - } - - void SetPoint(m2::PointD const & pt) - { - m_point = pt; - } - - bool HasCandidates() const - { - return !m_candidates.empty(); - } - - void operator() (FeatureType const & ft) - { - static CarModel const carModel; - if (ft.GetFeatureType() != feature::GEOM_LINE || !carModel.IsRoad(ft)) - return; - - Candidate res; - - FindNearestSegment(ft, m_point, res); - - if (!m_mwmId.IsAlive()) - m_mwmId = ft.GetID().m_mwmId; - ASSERT_EQUAL(m_mwmId, ft.GetID().m_mwmId, ()); - - if (res.m_fid != kInvalidFid) - m_candidates.push_back(res); - } - - double CalculateDistance(OsrmMappingTypes::FtSeg const & s) const - { - ASSERT_NOT_EQUAL(s.m_pointStart, s.m_pointEnd, ()); - - Index::FeaturesLoaderGuard loader(*m_pIndex, m_mwmId); - FeatureType ft; - loader.GetFeatureByIndex(s.m_fid, ft); - ft.ParseGeometry(FeatureType::BEST_GEOMETRY); - - double distMeters = 0.0; - size_t const n = max(s.m_pointEnd, s.m_pointStart); - size_t i = min(s.m_pointStart, s.m_pointEnd) + 1; - do - { - distMeters += MercatorBounds::DistanceOnEarth(ft.GetPoint(i - 1), ft.GetPoint(i)); - ++i; - } while (i <= n); - - return distMeters; - } - - /// Calculates part of a node weight in the OSRM format. Projection point @segPt divides node on - /// two parts. So we find weight of a part, set by the @offsetToRight parameter. - void CalculateWeight(OsrmMappingTypes::FtSeg const & seg, m2::PointD const & segPt, NodeID & nodeId, int & weight, bool offsetToRight) const - { - // nodeId can be INVALID_NODE_ID when reverse node is absent. This node has no weight. - if (nodeId == INVALID_NODE_ID || m_dataFacade.GetOutDegree(nodeId) == 0) - { - weight = 0; - return; - } - - // Distance from the node border to the projection point in meters. - double distance = 0; - auto const range = m_mapping.GetSegmentsRange(nodeId); - OsrmMappingTypes::FtSeg segment; - OsrmMappingTypes::FtSeg currentSegment; - - size_t startIndex = offsetToRight ? range.second - 1 : range.first; - size_t endIndex = offsetToRight ? range.first - 1 : range.second; - int indexIncrement = offsetToRight ? -1 : 1; - - for (size_t segmentIndex = startIndex; segmentIndex != endIndex; segmentIndex += indexIncrement) - { - m_mapping.GetSegmentByIndex(segmentIndex, segment); - if (!segment.IsValid()) - continue; - - auto segmentLeft = min(segment.m_pointStart, segment.m_pointEnd); - auto segmentRight = max(segment.m_pointEnd, segment.m_pointStart); - - // seg.m_pointEnd - seg.m_pointStart == 1, so check - // just a case, when seg is inside s - if ((seg.m_pointStart != segmentLeft || seg.m_pointEnd != segmentRight) && - (segmentLeft <= seg.m_pointStart && segmentRight >= seg.m_pointEnd)) - { - currentSegment.m_fid = segment.m_fid; - - if (segment.m_pointStart < segment.m_pointEnd) - { - if (offsetToRight) - { - currentSegment.m_pointEnd = seg.m_pointEnd; - currentSegment.m_pointStart = segment.m_pointStart; - } - else - { - currentSegment.m_pointStart = seg.m_pointStart; - currentSegment.m_pointEnd = segment.m_pointEnd; - } - } - else - { - if (offsetToRight) - { - currentSegment.m_pointStart = segment.m_pointEnd; - currentSegment.m_pointEnd = seg.m_pointEnd; - } - else - { - currentSegment.m_pointEnd = seg.m_pointStart; - currentSegment.m_pointStart = segment.m_pointStart; - } - } - - distance += CalculateDistance(currentSegment); - break; - } - else - distance += CalculateDistance(segment); - } - - Index::FeaturesLoaderGuard loader(*m_pIndex, m_mwmId); - FeatureType ft; - loader.GetFeatureByIndex(seg.m_fid, ft); - ft.ParseGeometry(FeatureType::BEST_GEOMETRY); - - // node.m_seg always forward ordered (m_pointStart < m_pointEnd) - distance -= MercatorBounds::DistanceOnEarth(ft.GetPoint(offsetToRight ? seg.m_pointEnd : seg.m_pointStart), segPt); - - // Offset is measured in decades of seconds. We don't know about speed restrictions on the road. - // So we find it by a whole edge weight. - - // Whole node distance in meters. - double fullDistanceM = 0.0; - for (size_t segmentIndex = startIndex; segmentIndex != endIndex; segmentIndex += indexIncrement) - { - m_mapping.GetSegmentByIndex(segmentIndex, segment); - if (!segment.IsValid()) - continue; - fullDistanceM += CalculateDistance(segment); - } - - ASSERT_GREATER(fullDistanceM, 0, ("No valid segments on the edge.")); - double const ratio = (fullDistanceM == 0) ? 0 : distance / fullDistanceM; - - auto const beginEdge = m_dataFacade.BeginEdges(nodeId); - auto const endEdge = m_dataFacade.EndEdges(nodeId); - // Minimal OSRM edge weight in decades of seconds. - EdgeWeight minWeight = m_dataFacade.GetEdgeData(beginEdge, nodeId).distance; - for (EdgeID i = beginEdge + 1; i != endEdge; ++i) - minWeight = min(m_dataFacade.GetEdgeData(i, nodeId).distance, minWeight); - - weight = max(static_cast(minWeight * ratio), 0); - } - - void CalculateOffsets(FeatureGraphNode & node) const - { - CalculateWeight(node.segment, node.segmentPoint, node.node.forward_node_id, node.node.forward_weight, true /* offsetToRight */); - CalculateWeight(node.segment, node.segmentPoint, node.node.reverse_node_id, node.node.reverse_weight, false /* offsetToRight */); - - // need to initialize weights for correct work of PhantomNode::GetForwardWeightPlusOffset - // and PhantomNode::GetReverseWeightPlusOffset - node.node.forward_offset = 0; - node.node.reverse_offset = 0; - } - - void MakeResult(TFeatureGraphNodeVec & res, size_t maxCount, string const & mwmName) - { - if (!m_mwmId.IsAlive()) - return; - - vector segments; - - segments.resize(maxCount); - - OsrmFtSegMapping::FtSegSetT segmentSet; - sort(m_candidates.begin(), m_candidates.end(), [] (Candidate const & r1, Candidate const & r2) - { - return (r1.m_dist < r2.m_dist); - }); - - size_t const n = min(m_candidates.size(), maxCount); - for (size_t j = 0; j < n; ++j) - { - OsrmMappingTypes::FtSeg & seg = segments[j]; - Candidate const & c = m_candidates[j]; - - seg.m_fid = c.m_fid; - seg.m_pointStart = c.m_segIdx; - seg.m_pointEnd = c.m_segIdx + 1; - - segmentSet.insert(&seg); - } - - OsrmFtSegMapping::OsrmNodesT nodes; - m_mapping.GetOsrmNodes(segmentSet, nodes); - - res.clear(); - res.resize(maxCount); - - for (size_t j = 0; j < maxCount; ++j) - { - size_t const idx = j; - - if (!segments[idx].IsValid()) - continue; - - auto it = nodes.find(segments[idx].Store()); - if (it == nodes.end()) - continue; - - FeatureGraphNode & node = res[idx]; - - if (!m_direction.IsAlmostZero()) - { - // Filter income nodes by direction mode - OsrmMappingTypes::FtSeg const & node_seg = segments[idx]; - FeatureType feature; - Index::FeaturesLoaderGuard loader(*m_pIndex, m_mwmId); - loader.GetFeatureByIndex(node_seg.m_fid, feature); - feature.ParseGeometry(FeatureType::BEST_GEOMETRY); - m2::PointD const featureDirection = feature.GetPoint(node_seg.m_pointEnd) - feature.GetPoint(node_seg.m_pointStart); - bool const sameDirection = (m2::DotProduct(featureDirection, m_direction) / (featureDirection.Length() * m_direction.Length()) > 0); - if (sameDirection) - { - node.node.forward_node_id = it->second.first; - node.node.reverse_node_id = INVALID_NODE_ID; - } - else - { - node.node.forward_node_id = INVALID_NODE_ID; - node.node.reverse_node_id = it->second.second; - } - } - else - { - node.node.forward_node_id = it->second.first; - node.node.reverse_node_id = it->second.second; - } - - node.segment = segments[idx]; - node.segmentPoint = m_candidates[j].m_point; - node.mwmName = mwmName; - - CalculateOffsets(node); - } - res.erase(remove_if(res.begin(), res.end(), [](FeatureGraphNode const & f) - { - return f.mwmName.empty(); - }), - res.end()); - } - -private: - m2::PointD m_point; - m2::PointD const m_direction; - OsrmFtSegMapping const & m_mapping; - buffer_vector m_candidates; - MwmSet::MwmId m_mwmId; - Index const * m_pIndex; - TDataFacade const & m_dataFacade; - - DISALLOW_COPY(Point2PhantomNode); -}; -} // namespace - // static bool OsrmRouter::CheckRoutingAbility(m2::PointD const & startPoint, m2::PointD const & finalPoint, TCountryFileFn const & countryFileFn, Index * index) @@ -410,7 +99,7 @@ void FindGraphNodeOffsets(uint32_t const nodeId, m2::PointD const & point, { graphNode.segmentPoint = point; - Point2PhantomNode::Candidate best; + helpers::Point2PhantomNode::Candidate best; auto range = mapping->m_segMapping.GetSegmentsRange(nodeId); for (size_t i = range.first; i < range.second; ++i) @@ -424,8 +113,8 @@ void FindGraphNodeOffsets(uint32_t const nodeId, m2::PointD const & point, Index::FeaturesLoaderGuard loader(*pIndex, mapping->GetMwmId()); loader.GetFeatureByIndex(s.m_fid, ft); - Point2PhantomNode::Candidate mappedSeg; - Point2PhantomNode::FindNearestSegment(ft, point, mappedSeg); + helpers::Point2PhantomNode::Candidate mappedSeg; + helpers::Point2PhantomNode::FindNearestSegment(ft, point, mappedSeg); OsrmMappingTypes::FtSeg seg; seg.m_fid = mappedSeg.m_fid; @@ -690,7 +379,7 @@ IRouter::ResultCode OsrmRouter::FindPhantomNodes(m2::PointD const & point, TRoutingMappingPtr const & mapping) { ASSERT(mapping, ()); - Point2PhantomNode getter(mapping->m_segMapping, m_pIndex, direction, mapping->m_dataFacade); + helpers::Point2PhantomNode getter(*mapping, *m_pIndex, direction); getter.SetPoint(point); m_pIndex->ForEachInRectForMWM(getter, MercatorBounds::RectByCenterXYAndSizeInMeters( @@ -726,7 +415,7 @@ OsrmRouter::ResultCode OsrmRouter::MakeTurnAnnotation( LOG(LDEBUG, ("Shortest path length:", routingResult.shortestPathLength)); //! @todo: Improve last segment time calculation - CarModel carModel; + CarModel const & carModel = CarModel::Instance(); #ifdef DEBUG size_t lastIdx = 0; #endif diff --git a/routing/routing.pro b/routing/routing.pro index 9d904325b6..f5037099be 100644 --- a/routing/routing.pro +++ b/routing/routing.pro @@ -25,6 +25,7 @@ SOURCES += \ online_cross_fetcher.cpp \ osrm2feature_map.cpp \ osrm_engine.cpp \ + osrm_helpers.cpp \ osrm_router.cpp \ pedestrian_directions.cpp \ pedestrian_model.cpp \ @@ -60,6 +61,7 @@ HEADERS += \ osrm2feature_map.hpp \ osrm_data_facade.hpp \ osrm_engine.hpp \ + osrm_helpers.hpp \ osrm_router.hpp \ pedestrian_directions.hpp \ pedestrian_model.hpp \ diff --git a/routing/routing_integration_tests/osrm_route_test.cpp b/routing/routing_integration_tests/osrm_route_test.cpp index b52a428d1f..d1ee10b98e 100644 --- a/routing/routing_integration_tests/osrm_route_test.cpp +++ b/routing/routing_integration_tests/osrm_route_test.cpp @@ -213,7 +213,8 @@ namespace UNIT_TEST(RussiaSmolenskRussiaMoscowTimeTest) { TRouteResult const routeResult = integration::CalculateRoute( - integration::GetOsrmComponents(), {32.05489, 65.78463}, {0., 0.}, {37.60169, 67.45807}); + integration::GetOsrmComponents(), MercatorBounds::FromLatLon(54.7998, 32.05489), {0., 0.}, + MercatorBounds::FromLatLon(55.753, 37.60169)); Route const & route = *routeResult.first; IRouter::ResultCode const result = routeResult.second; @@ -225,8 +226,8 @@ namespace UNIT_TEST(RussiaMoscowLenigradskiy39GeroevPanfilovtsev22TimeTest) { TRouteResult const routeResult = integration::CalculateRoute( - integration::GetOsrmComponents(), {37.53804, 67.53647}, {0., 0.}, {37.40990, 67.64474}); - + integration::GetOsrmComponents(), MercatorBounds::FromLatLon(55.7971, 37.53804), {0., 0.}, + MercatorBounds::FromLatLon(55.8579, 37.40990)); Route const & route = *routeResult.first; IRouter::ResultCode const result = routeResult.second; TEST_EQUAL(result, IRouter::NoError, ()); diff --git a/routing/routing_tests/routing_mapping_test.cpp b/routing/routing_tests/routing_mapping_test.cpp index 5f0303925d..619af953c7 100644 --- a/routing/routing_tests/routing_mapping_test.cpp +++ b/routing/routing_tests/routing_mapping_test.cpp @@ -96,4 +96,36 @@ UNIT_TEST(IndexManagerLockManagementTest) manager.Clear(); TEST_EQUAL(generator.GetNumRefs(), 0, ()); } + +UNIT_TEST(FtSegIsInsideTest) +{ + OsrmMappingTypes::FtSeg seg(123, 1, 5); + OsrmMappingTypes::FtSeg inside(123, 1, 2); + TEST(OsrmMappingTypes::IsInside(seg, inside), ()); + OsrmMappingTypes::FtSeg inside2(123, 3, 4); + TEST(OsrmMappingTypes::IsInside(seg, inside2), ()); + OsrmMappingTypes::FtSeg inside3(123, 4, 5); + TEST(OsrmMappingTypes::IsInside(seg, inside3), ()); + OsrmMappingTypes::FtSeg bseg(123, 5, 1); + TEST(OsrmMappingTypes::IsInside(bseg, inside), ()); + TEST(OsrmMappingTypes::IsInside(bseg, inside2), ()); + TEST(OsrmMappingTypes::IsInside(bseg, inside3), ()); +} + +UNIT_TEST(FtSegSplitSegmentiTest) +{ + OsrmMappingTypes::FtSeg seg(123, 1, 5); + OsrmMappingTypes::FtSeg bseg(123, 5, 1); + OsrmMappingTypes::FtSeg splitter(123, 2, 3); + + OsrmMappingTypes::FtSeg res1(123, 2, 5); + TEST_EQUAL(res1, OsrmMappingTypes::SplitSegment(seg, splitter, false), ()); + OsrmMappingTypes::FtSeg res2(123, 1, 3); + TEST_EQUAL(res2, OsrmMappingTypes::SplitSegment(seg, splitter, true), ()); + + OsrmMappingTypes::FtSeg res3(123, 3, 1); + TEST_EQUAL(res3, OsrmMappingTypes::SplitSegment(bseg, splitter, false), ()); + OsrmMappingTypes::FtSeg res4(123, 5, 2); + TEST_EQUAL(res4, OsrmMappingTypes::SplitSegment(bseg, splitter, true), ()); +} } // namespace diff --git a/routing/turns_generator.cpp b/routing/turns_generator.cpp index e2766dc17a..fb305175f7 100644 --- a/routing/turns_generator.cpp +++ b/routing/turns_generator.cpp @@ -1,7 +1,8 @@ #include "turns_generator.hpp" -#include "routing/car_model.hpp" -#include "routing/routing_mapping.hpp" +#include "car_model.hpp" +#include "osrm_helpers.hpp" +#include "routing_mapping.hpp" #include "indexer/ftypes_matcher.hpp" #include "indexer/scales.hpp" @@ -73,8 +74,7 @@ public: void operator()(FeatureType const & ft) { - static CarModel const carModel; - if (ft.GetFeatureType() != feature::GEOM_LINE || !carModel.IsRoad(ft)) + if (ft.GetFeatureType() != feature::GEOM_LINE || !CarModel::Instance().IsRoad(ft)) return; ft.ParseGeometry(FeatureType::BEST_GEOMETRY); size_t const count = ft.GetPointsCount(); @@ -101,29 +101,6 @@ public: DISALLOW_COPY_AND_MOVE(Point2Geometry); }; -class Point2Node -{ - RoutingMapping const & m_routingMapping; - vector & n_nodeIds; - -public: - Point2Node(RoutingMapping const & routingMapping, vector & nodeID) - : m_routingMapping(routingMapping), n_nodeIds(nodeID) - { - } - - void operator()(FeatureType const & ft) - { - static CarModel const carModel; - if (ft.GetFeatureType() != feature::GEOM_LINE || !carModel.IsRoad(ft)) - return; - uint32_t const featureId = ft.GetID().m_index; - for (auto const n : m_routingMapping.m_segMapping.GetNodeIdByFid(featureId)) - n_nodeIds.push_back(n); - } - - DISALLOW_COPY_AND_MOVE(Point2Node); -}; OsrmMappingTypes::FtSeg GetSegment(NodeID node, RoutingMapping const & routingMapping, TGetIndexFunction GetIndex) @@ -380,7 +357,7 @@ void GetPossibleTurns(Index const & index, NodeID node, m2::PointD const & ingoi // Geting nodes by geometry. vector geomNodes; - Point2Node p2n(routingMapping, geomNodes); + helpers::Point2Node p2n(routingMapping, geomNodes); index.ForEachInRectForMWM( p2n, m2::RectD(junctionPoint.x - kReadCrossEpsilon, junctionPoint.y - kReadCrossEpsilon, From 4d463f324a15f9b0543d18053b9847e121af5d1e Mon Sep 17 00:00:00 2001 From: Lev Dragunov Date: Thu, 15 Oct 2015 16:15:39 +0300 Subject: [PATCH 2/4] xcode build fix. --- routing/osrm_helpers.cpp | 24 +++++++++---------- routing/osrm_helpers.hpp | 8 +++---- routing/osrm_router.cpp | 22 ++++++++++------- .../indexer/indexer.xcodeproj/project.pbxproj | 12 ---------- .../routing/routing.xcodeproj/project.pbxproj | 8 +++++++ 5 files changed, 37 insertions(+), 37 deletions(-) diff --git a/routing/osrm_helpers.cpp b/routing/osrm_helpers.cpp index 42d60f3481..c7b0bde0cb 100644 --- a/routing/osrm_helpers.cpp +++ b/routing/osrm_helpers.cpp @@ -37,7 +37,7 @@ void Point2PhantomNode::FindNearestSegment(FeatureType const & ft, m2::PointD co void Point2PhantomNode::operator()(FeatureType const & ft) { - //TODO(gardster) Extract GEOM_LINE check into CatModel. + //TODO(gardster) Extract GEOM_LINE check into CarModel. if (ft.GetFeatureType() != feature::GEOM_LINE || !CarModel::Instance().IsRoad(ft)) return; @@ -72,7 +72,7 @@ double Point2PhantomNode::CalculateDistance(OsrmMappingTypes::FtSeg const & s) c void Point2PhantomNode::CalculateWeight(OsrmMappingTypes::FtSeg const & seg, m2::PointD const & segPt, NodeID const & nodeId, - bool calcFromRight, int & weight) const + 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) @@ -140,7 +140,8 @@ void Point2PhantomNode::CalculateWeight(OsrmMappingTypes::FtSeg const & seg, ft.ParseGeometry(FeatureType::BEST_GEOMETRY); minWeight = GetMinNodeWeight(nodeId, ft.GetPoint(segment.m_pointEnd)); } - weight = max(static_cast(minWeight * ratio), 0); + offset = minWeight; + weight = max(static_cast(minWeight * ratio), 0) - minWeight; } EdgeWeight Point2PhantomNode::GetMinNodeWeight(NodeID node, m2::PointD const & point) const @@ -263,7 +264,7 @@ void Point2PhantomNode::MakeResult(vector & res, size_t maxCou node.segmentPoint = m_candidates[j].m_point; node.mwmName = mwmName; - CalculateOffsets(node); + CalculateWeights(node); } res.erase(remove_if(res.begin(), res.end(), [](FeatureGraphNode const & f) @@ -273,17 +274,14 @@ void Point2PhantomNode::MakeResult(vector & res, size_t maxCou res.end()); } -void Point2PhantomNode::CalculateOffsets(FeatureGraphNode & node) const +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); + true /* 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); - - // Need to initialize weights for correct work of PhantomNode::GetForwardWeightPlusOffset - // and PhantomNode::GetReverseWeightPlusOffset. - node.node.forward_offset = 0; - node.node.reverse_offset = 0; + false /* calcFromRight */, node.node.reverse_weight, node.node.reverse_offset); } void Point2Node::operator()(FeatureType const & ft) @@ -292,7 +290,7 @@ void Point2Node::operator()(FeatureType const & ft) return; uint32_t const featureId = ft.GetID().m_index; for (auto const n : m_routingMapping.m_segMapping.GetNodeIdByFid(featureId)) - n_nodeIds.push_back(n); + m_nodeIds.push_back(n); } } // namespace helpers } // namespace routing diff --git a/routing/osrm_helpers.hpp b/routing/osrm_helpers.hpp index 730f39498a..4f9a9748f1 100644 --- a/routing/osrm_helpers.hpp +++ b/routing/osrm_helpers.hpp @@ -58,13 +58,13 @@ private: /// Calculates part of a node weight in the OSRM format. Projection point @segPt divides node on /// two parts. So we find weight of a part, set by the @calcFromRight parameter. void CalculateWeight(OsrmMappingTypes::FtSeg const & seg, m2::PointD const & segPt, - NodeID const & nodeId, bool calcFromRight, int & weight) const; + NodeID const & nodeId, bool calcFromRight, int & weight, int & offset) const; /// Returns minimal weight of the node. EdgeWeight GetMinNodeWeight(NodeID node, m2::PointD const & point) const; /// Calculates weights and offsets section of the routing tasks. - void CalculateOffsets(FeatureGraphNode & node) const; + void CalculateWeights(FeatureGraphNode & node) const; m2::PointD m_point; m2::PointD const m_direction; @@ -79,11 +79,11 @@ private: class Point2Node { RoutingMapping const & m_routingMapping; - vector & n_nodeIds; + vector & m_nodeIds; public: Point2Node(RoutingMapping const & routingMapping, vector & nodeID) - : m_routingMapping(routingMapping), n_nodeIds(nodeID) + : m_routingMapping(routingMapping), m_nodeIds(nodeID) { } diff --git a/routing/osrm_router.cpp b/routing/osrm_router.cpp index 038e35e60d..73865cb3ad 100644 --- a/routing/osrm_router.cpp +++ b/routing/osrm_router.cpp @@ -1,4 +1,3 @@ -#include "car_model.hpp" #include "cross_mwm_router.hpp" #include "online_cross_fetcher.hpp" #include "osrm2feature_map.hpp" @@ -414,8 +413,6 @@ OsrmRouter::ResultCode OsrmRouter::MakeTurnAnnotation( LOG(LDEBUG, ("Shortest path length:", routingResult.shortestPathLength)); - //! @todo: Improve last segment time calculation - CarModel const & carModel = CarModel::Instance(); #ifdef DEBUG size_t lastIdx = 0; #endif @@ -506,7 +503,20 @@ OsrmRouter::ResultCode OsrmRouter::MakeTurnAnnotation( auto startIdx = seg.m_pointStart; auto endIdx = seg.m_pointEnd; - bool const needTime = (segmentIndex == 0) || (segmentIndex == numSegments - 1); + 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()) startIdx = (seg.m_pointEnd > seg.m_pointStart) ? segBegin.m_pointStart : segBegin.m_pointEnd; @@ -518,16 +528,12 @@ OsrmRouter::ResultCode OsrmRouter::MakeTurnAnnotation( for (auto idx = startIdx; idx <= endIdx; ++idx) { points.push_back(ft.GetPoint(idx)); - if (needTime && idx > startIdx) - estimatedTime += MercatorBounds::DistanceOnEarth(ft.GetPoint(idx - 1), ft.GetPoint(idx)) / carModel.GetSpeed(ft); } } else { for (auto idx = startIdx; idx > endIdx; --idx) { - if (needTime) - estimatedTime += MercatorBounds::DistanceOnEarth(ft.GetPoint(idx - 1), ft.GetPoint(idx)) / carModel.GetSpeed(ft); points.push_back(ft.GetPoint(idx)); } points.push_back(ft.GetPoint(endIdx)); diff --git a/xcode/indexer/indexer.xcodeproj/project.pbxproj b/xcode/indexer/indexer.xcodeproj/project.pbxproj index 098c52a48a..453303d521 100644 --- a/xcode/indexer/indexer.xcodeproj/project.pbxproj +++ b/xcode/indexer/indexer.xcodeproj/project.pbxproj @@ -48,8 +48,6 @@ 670D04AD1B0BA8580013A7AC /* interval_index_101.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 670D04AA1B0BA8580013A7AC /* interval_index_101.hpp */; }; 670EE56C1B60033A001E8064 /* features_vector.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 670EE56A1B60033A001E8064 /* features_vector.cpp */; }; 670EE56D1B60033A001E8064 /* unique_index.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 670EE56B1B60033A001E8064 /* unique_index.hpp */; }; - 670F291F1BA86D5400F2ABF4 /* drules_city_rank_table.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 670F291D1BA86D5400F2ABF4 /* drules_city_rank_table.cpp */; }; - 670F29201BA86D5400F2ABF4 /* drules_city_rank_table.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 670F291E1BA86D5400F2ABF4 /* drules_city_rank_table.hpp */; }; 6726C1D11A49DAAC005EEA39 /* feature_meta.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 6726C1CF1A49DAAC005EEA39 /* feature_meta.cpp */; }; 6726C1D21A49DAAC005EEA39 /* feature_meta.hpp in Headers */ = {isa = PBXBuildFile; fileRef = 6726C1D01A49DAAC005EEA39 /* feature_meta.hpp */; }; 674125131B4C02F100A3E828 /* map_style_reader.cpp in Sources */ = {isa = PBXBuildFile; fileRef = 674125101B4C02F100A3E828 /* map_style_reader.cpp */; }; @@ -195,8 +193,6 @@ 670D05AB1B0E07040013A7AC /* defaults.xcconfig */ = {isa = PBXFileReference; lastKnownFileType = text.xcconfig; name = defaults.xcconfig; path = ../defaults.xcconfig; sourceTree = ""; }; 670EE56A1B60033A001E8064 /* features_vector.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = features_vector.cpp; sourceTree = ""; }; 670EE56B1B60033A001E8064 /* unique_index.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = unique_index.hpp; sourceTree = ""; }; - 670F291D1BA86D5400F2ABF4 /* drules_city_rank_table.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = drules_city_rank_table.cpp; sourceTree = ""; }; - 670F291E1BA86D5400F2ABF4 /* drules_city_rank_table.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = drules_city_rank_table.hpp; sourceTree = ""; }; 6726C1CF1A49DAAC005EEA39 /* feature_meta.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = feature_meta.cpp; sourceTree = ""; }; 6726C1D01A49DAAC005EEA39 /* feature_meta.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = feature_meta.hpp; sourceTree = ""; }; 674125101B4C02F100A3E828 /* map_style_reader.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = map_style_reader.cpp; sourceTree = ""; }; @@ -321,10 +317,6 @@ isa = PBXGroup; children = ( 670C61571AB068C100C38A8C /* Libs */, - 678FB4181BB43D8900C49B9C /* drules_selector_parser.cpp */, - 678FB4191BB43D8900C49B9C /* drules_selector_parser.hpp */, - 678FB41A1BB43D8900C49B9C /* drules_selector.cpp */, - 678FB41B1BB43D8900C49B9C /* drules_selector.hpp */, 670C611F1AB065E100C38A8C /* testingmain.cpp */, 670C60F91AB065B100C38A8C /* categories_test.cpp */, 670C60FA1AB065B100C38A8C /* cell_coverer_test.cpp */, @@ -407,8 +399,6 @@ 6758AECE1BB4413000C26E27 /* drules_selector_parser.hpp */, 6758AECF1BB4413000C26E27 /* drules_selector.cpp */, 6758AED01BB4413000C26E27 /* drules_selector.hpp */, - 670F291D1BA86D5400F2ABF4 /* drules_city_rank_table.cpp */, - 670F291E1BA86D5400F2ABF4 /* drules_city_rank_table.hpp */, 674125101B4C02F100A3E828 /* map_style_reader.cpp */, 674125111B4C02F100A3E828 /* map_style_reader.hpp */, 674125121B4C02F100A3E828 /* map_style.hpp */, @@ -511,7 +501,6 @@ buildActionMask = 2147483647; files = ( 675341361A3F540F00A0A8C3 /* mercator.hpp in Headers */, - 670F29201BA86D5400F2ABF4 /* drules_city_rank_table.hpp in Headers */, 6753414E1A3F540F00A0A8C3 /* types_mapping.hpp in Headers */, 6753411F1A3F540F00A0A8C3 /* feature_loader.hpp in Headers */, 675341151A3F540F00A0A8C3 /* feature_covering.hpp in Headers */, @@ -676,7 +665,6 @@ 675341211A3F540F00A0A8C3 /* feature_utils.cpp in Sources */, 670C61151AB065B100C38A8C /* interval_index_test.cpp in Sources */, 670C61121AB065B100C38A8C /* geometry_serialization_test.cpp in Sources */, - 670F291F1BA86D5400F2ABF4 /* drules_city_rank_table.cpp in Sources */, 675341231A3F540F00A0A8C3 /* feature_visibility.cpp in Sources */, 670C610F1AB065B100C38A8C /* checker_test.cpp in Sources */, 675341161A3F540F00A0A8C3 /* feature_data.cpp in Sources */, diff --git a/xcode/routing/routing.xcodeproj/project.pbxproj b/xcode/routing/routing.xcodeproj/project.pbxproj index fb25ed31db..ad58900978 100644 --- a/xcode/routing/routing.xcodeproj/project.pbxproj +++ b/xcode/routing/routing.xcodeproj/project.pbxproj @@ -70,6 +70,8 @@ A1616E2B1B6B60AB003F078E /* router_delegate.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A1616E291B6B60AB003F078E /* router_delegate.cpp */; }; A1616E2C1B6B60AB003F078E /* router_delegate.hpp in Headers */ = {isa = PBXBuildFile; fileRef = A1616E2A1B6B60AB003F078E /* router_delegate.hpp */; }; A1616E2E1B6B60B3003F078E /* astar_progress.hpp in Headers */ = {isa = PBXBuildFile; fileRef = A1616E2D1B6B60B3003F078E /* astar_progress.hpp */; }; + A17B42981BCFBD0E00A1EAE4 /* osrm_helpers.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A17B42961BCFBD0E00A1EAE4 /* osrm_helpers.cpp */; settings = {ASSET_TAGS = (); }; }; + A17B42991BCFBD0E00A1EAE4 /* osrm_helpers.hpp in Headers */ = {isa = PBXBuildFile; fileRef = A17B42971BCFBD0E00A1EAE4 /* osrm_helpers.hpp */; settings = {ASSET_TAGS = (); }; }; A1876BC61BB19C4300C9C743 /* speed_camera.cpp in Sources */ = {isa = PBXBuildFile; fileRef = A1876BC41BB19C4300C9C743 /* speed_camera.cpp */; settings = {ASSET_TAGS = (); }; }; A1876BC71BB19C4300C9C743 /* speed_camera.hpp in Headers */ = {isa = PBXBuildFile; fileRef = A1876BC51BB19C4300C9C743 /* speed_camera.hpp */; settings = {ASSET_TAGS = (); }; }; /* End PBXBuildFile section */ @@ -140,6 +142,8 @@ A1616E291B6B60AB003F078E /* router_delegate.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = router_delegate.cpp; sourceTree = ""; }; A1616E2A1B6B60AB003F078E /* router_delegate.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = router_delegate.hpp; sourceTree = ""; }; A1616E2D1B6B60B3003F078E /* astar_progress.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = astar_progress.hpp; sourceTree = ""; }; + A17B42961BCFBD0E00A1EAE4 /* osrm_helpers.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; name = osrm_helpers.cpp; path = ../../routing/osrm_helpers.cpp; sourceTree = ""; }; + A17B42971BCFBD0E00A1EAE4 /* osrm_helpers.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; name = osrm_helpers.hpp; path = ../../routing/osrm_helpers.hpp; sourceTree = ""; }; A1876BC41BB19C4300C9C743 /* speed_camera.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = speed_camera.cpp; sourceTree = ""; }; A1876BC51BB19C4300C9C743 /* speed_camera.hpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.h; path = speed_camera.hpp; sourceTree = ""; }; /* End PBXFileReference section */ @@ -169,6 +173,8 @@ 675343EF1A3F640D00A0A8C3 = { isa = PBXGroup; children = ( + A17B42961BCFBD0E00A1EAE4 /* osrm_helpers.cpp */, + A17B42971BCFBD0E00A1EAE4 /* osrm_helpers.hpp */, 670D05A61B0E02560013A7AC /* defaults.xcconfig */, 675343FA1A3F640D00A0A8C3 /* routing */, 675343F91A3F640D00A0A8C3 /* Products */, @@ -277,6 +283,7 @@ 6753441C1A3F644F00A0A8C3 /* route.hpp in Headers */, A1616E2C1B6B60AB003F078E /* router_delegate.hpp in Headers */, 67C7D4301B4EB48F00FE41AA /* turns_sound.hpp in Headers */, + A17B42991BCFBD0E00A1EAE4 /* osrm_helpers.hpp in Headers */, 67C7D42E1B4EB48F00FE41AA /* turns_sound_settings.hpp in Headers */, 670EE55E1B6001E7001E8064 /* routing_session.hpp in Headers */, 670EE55F1B6001E7001E8064 /* routing_settings.hpp in Headers */, @@ -365,6 +372,7 @@ 670D049E1B0B4A970013A7AC /* nearest_edge_finder.cpp in Sources */, 674F9BD61B0A580E00704FFA /* turns_generator.cpp in Sources */, 675344171A3F644F00A0A8C3 /* osrm_router.cpp in Sources */, + A17B42981BCFBD0E00A1EAE4 /* osrm_helpers.cpp in Sources */, 674F9BD21B0A580E00704FFA /* road_graph_router.cpp in Sources */, A1876BC61BB19C4300C9C743 /* speed_camera.cpp in Sources */, 671F58BD1B874EC80032311E /* followed_polyline.cpp in Sources */, From 00cd1a074e0dbc07bc6e747ce846b8866eb59037 Mon Sep 17 00:00:00 2001 From: Lev Dragunov Date: Thu, 15 Oct 2015 16:42:09 +0300 Subject: [PATCH 3/4] PR fixes. --- routing/osrm2feature_map.cpp | 6 +- routing/osrm2feature_map.hpp | 12 +- routing/osrm_helpers.cpp | 107 ++++++++---------- routing/osrm_helpers.hpp | 10 +- .../osrm_route_test.cpp | 2 +- routing/turns_generator.cpp | 2 +- routing/vehicle_model.cpp | 2 +- 7 files changed, 64 insertions(+), 77 deletions(-) diff --git a/routing/osrm2feature_map.cpp b/routing/osrm2feature_map.cpp index 7e3d109855..f42038d8c7 100644 --- a/routing/osrm2feature_map.cpp +++ b/routing/osrm2feature_map.cpp @@ -116,7 +116,7 @@ bool IsInside(FtSeg const & bigSeg, FtSeg const & smallSeg) ASSERT_EQUAL(smallSeg.m_pointEnd - smallSeg.m_pointStart, 1, ()); auto segmentLeft = min(bigSeg.m_pointStart, bigSeg.m_pointEnd); - auto segmentRight = max(bigSeg.m_pointEnd, bigSeg.m_pointStart); + auto segmentRight = max(bigSeg.m_pointStart, bigSeg.m_pointEnd); return (smallSeg.m_pointStart != segmentLeft || smallSeg.m_pointEnd != segmentRight) && (segmentLeft <= smallSeg.m_pointStart && segmentRight >= smallSeg.m_pointEnd); @@ -220,7 +220,7 @@ void OsrmFtSegMapping::DumpSegmentByNode(TOsrmNodeId nodeId) const #endif } -void OsrmFtSegMapping::GetOsrmNodes(FtSegSetT & segments, OsrmNodesT & res) const +void OsrmFtSegMapping::GetOsrmNodes(TFtSegVec const & segments, OsrmNodesT & res) const { auto addResFn = [&] (uint64_t seg, TOsrmNodeId nodeId, bool forward) { @@ -246,7 +246,7 @@ void OsrmFtSegMapping::GetOsrmNodes(FtSegSetT & segments, OsrmNodesT & res) cons for (auto it = segments.begin(); it != segments.end(); ++it) { - OsrmMappingTypes::FtSeg const & seg = *(*it); + OsrmMappingTypes::FtSeg const & seg = *it; TNodesList const & nodeIds = m_backwardIndex.GetNodeIdByFid(seg.m_fid); diff --git a/routing/osrm2feature_map.hpp b/routing/osrm2feature_map.hpp index 5cd35a36ac..4fb7d873c2 100644 --- a/routing/osrm2feature_map.hpp +++ b/routing/osrm2feature_map.hpp @@ -86,14 +86,6 @@ namespace OsrmMappingTypes friend string DebugPrint(SegOffset const & off); }; - - struct FtSegLess - { - bool operator() (FtSeg const * a, FtSeg const * b) const - { - return a->Store() < b->Store(); - } - }; #pragma pack (pop) /// Checks if a smallSeg is inside a bigSeg and at least one point of a smallSeg is differ from @@ -134,7 +126,7 @@ public: class OsrmFtSegMapping { public: - typedef set FtSegSetT; + using TFtSegVec = vector; void Clear(); void Load(FilesMappingContainer & cont, platform::LocalCountryFile const & localFile); @@ -156,7 +148,7 @@ public: } typedef unordered_map > OsrmNodesT; - void GetOsrmNodes(FtSegSetT & segments, OsrmNodesT & res) const; + void GetOsrmNodes(TFtSegVec const & segments, OsrmNodesT & res) const; void GetSegmentByIndex(size_t idx, OsrmMappingTypes::FtSeg & seg) const; TNodesList const & GetNodeIdByFid(uint32_t fid) const diff --git a/routing/osrm_helpers.cpp b/routing/osrm_helpers.cpp index c7b0bde0cb..f66020e342 100644 --- a/routing/osrm_helpers.cpp +++ b/routing/osrm_helpers.cpp @@ -37,8 +37,7 @@ void Point2PhantomNode::FindNearestSegment(FeatureType const & ft, m2::PointD co void Point2PhantomNode::operator()(FeatureType const & ft) { - //TODO(gardster) Extract GEOM_LINE check into CarModel. - if (ft.GetFeatureType() != feature::GEOM_LINE || !CarModel::Instance().IsRoad(ft)) + if (!CarModel::Instance().IsRoad(ft)) return; Candidate res; @@ -49,18 +48,15 @@ void Point2PhantomNode::operator()(FeatureType const & ft) m_candidates.push_back(res); } -double Point2PhantomNode::CalculateDistance(OsrmMappingTypes::FtSeg const & s) const +double Point2PhantomNode::CalculateDistance(FeatureType const & ft, + size_t const pointStart, + size_t const pointEnd) const { - ASSERT_NOT_EQUAL(s.m_pointStart, s.m_pointEnd, ()); - - Index::FeaturesLoaderGuard loader(m_index, m_routingMapping.GetMwmId()); - FeatureType ft; - loader.GetFeatureByIndex(s.m_fid, ft); - ft.ParseGeometry(FeatureType::BEST_GEOMETRY); + ASSERT_NOT_EQUAL(pointStart, pointEnd, ()); double distMeters = 0.0; - size_t const n = max(s.m_pointEnd, s.m_pointStart); - size_t i = min(s.m_pointStart, s.m_pointEnd) + 1; + size_t const n = max(pointStart, pointEnd); + size_t i = min(pointStart, pointEnd) + 1; do { distMeters += MercatorBounds::DistanceOnEarth(ft.GetPoint(i - 1), ft.GetPoint(i)); @@ -77,69 +73,69 @@ void Point2PhantomNode::CalculateWeight(OsrmMappingTypes::FtSeg const & seg, // 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) { + offset = 0; weight = 0; return; } + Index::FeaturesLoaderGuard loader(m_index, m_routingMapping.GetMwmId()); + // Offset is measured in milliseconds. We don't know about speed restrictions on the road. // So we find it by a whole edge weight. // Distance from the node border to the projection point is in meters. - double distanceM = 0; + double distanceM = 0.; + // Whole node distance in meters. + double fullDistanceM = 0.; + // Minimal OSRM edge weight in milliseconds. + EdgeWeight minWeight = 0; + auto const range = m_routingMapping.m_segMapping.GetSegmentsRange(nodeId); OsrmMappingTypes::FtSeg segment; - size_t startIndex = calcFromRight ? range.second - 1 : range.first; - size_t endIndex = calcFromRight ? range.first - 1 : range.second; - int indexIncrement = calcFromRight ? -1 : 1; + size_t const startIndex = calcFromRight ? range.second - 1 : range.first; + size_t const endIndex = calcFromRight ? range.first - 1 : range.second; + int const indexIncrement = calcFromRight ? -1 : 1; + bool foundSeg = false; + m2::PointD lastPoint; for (size_t segmentIndex = startIndex; segmentIndex != endIndex; segmentIndex += indexIncrement) { m_routingMapping.m_segMapping.GetSegmentByIndex(segmentIndex, segment); if (!segment.IsValid()) continue; + FeatureType ft; + loader.GetFeatureByIndex(segment.m_fid, ft); + ft.ParseGeometry(FeatureType::BEST_GEOMETRY); + if (segment.m_fid == seg.m_fid && OsrmMappingTypes::IsInside(segment, seg)) { - distanceM += CalculateDistance(OsrmMappingTypes::SplitSegment(segment, seg, !calcFromRight)); - break; + 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); + + foundSeg = true; } - distanceM += CalculateDistance(segment); - } + double distance = CalculateDistance(ft, segment.m_pointStart, segment.m_pointEnd); + if (!foundSeg) + distanceM += distance; + fullDistanceM += distance; - Index::FeaturesLoaderGuard loader(m_index, m_routingMapping.GetMwmId()); - FeatureType ft; - loader.GetFeatureByIndex(seg.m_fid, ft); - ft.ParseGeometry(FeatureType::BEST_GEOMETRY); - - // node.m_seg always forward ordered (m_pointStart < m_pointEnd) - distanceM -= MercatorBounds::DistanceOnEarth( - ft.GetPoint(calcFromRight ? seg.m_pointEnd : seg.m_pointStart), segPt); - - // Whole node distance in meters. - double fullDistanceM = 0.0; - for (size_t segmentIndex = startIndex; segmentIndex != endIndex; segmentIndex += indexIncrement) - { - m_routingMapping.m_segMapping.GetSegmentByIndex(segmentIndex, segment); - if (!segment.IsValid()) - continue; - fullDistanceM += CalculateDistance(segment); + //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.")); double const ratio = (fullDistanceM == 0) ? 0 : distanceM / fullDistanceM; ASSERT_LESS_OR_EQUAL(ratio, 1., ()); - // Minimal OSRM edge weight in milliseconds. - EdgeWeight minWeight = 0; - - if (segment.IsValid()) - { - FeatureType ft; - loader.GetFeatureByIndex(segment.m_fid, ft); - ft.ParseGeometry(FeatureType::BEST_GEOMETRY); - minWeight = GetMinNodeWeight(nodeId, ft.GetPoint(segment.m_pointEnd)); - } + // OSRM calculates edge weight form start to user point how offset + weight. + // But it doesn't place info about start and end edge result weight into result structure. + // So we store whole edge weight into offset and calculates this weights at a postprocessing step. offset = minWeight; weight = max(static_cast(minWeight * ratio), 0) - minWeight; } @@ -162,7 +158,7 @@ EdgeWeight Point2PhantomNode::GetMinNodeWeight(NodeID node, m2::PointD const & p geomNodes.erase(unique(geomNodes.begin(), geomNodes.end()), geomNodes.end()); // Filtering virtual edges. - for (EdgeID const e : m_routingMapping.m_dataFacade.GetAdjacentEdgeRange(node)) + 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) @@ -171,11 +167,11 @@ EdgeWeight Point2PhantomNode::GetMinNodeWeight(NodeID node, m2::PointD const & p } } - for (NodeID const adjacentNode : geomNodes) + for (NodeID const & adjacentNode : geomNodes) { if (adjacentNode == node) continue; - for (EdgeID const e : m_routingMapping.m_dataFacade.GetAdjacentEdgeRange(adjacentNode)) + for (EdgeID const & e : m_routingMapping.m_dataFacade.GetAdjacentEdgeRange(adjacentNode)) { if (m_routingMapping.m_dataFacade.GetTarget(e) != node) continue; @@ -195,15 +191,14 @@ void Point2PhantomNode::MakeResult(vector & res, size_t maxCou { vector segments; - segments.resize(maxCount); - - OsrmFtSegMapping::FtSegSetT segmentSet; sort(m_candidates.begin(), m_candidates.end(), [](Candidate const & r1, Candidate const & r2) { return (r1.m_dist < r2.m_dist); }); size_t const n = min(m_candidates.size(), maxCount); + segments.resize(n); + for (size_t j = 0; j < n; ++j) { OsrmMappingTypes::FtSeg & seg = segments[j]; @@ -212,12 +207,10 @@ void Point2PhantomNode::MakeResult(vector & res, size_t maxCou seg.m_fid = c.m_fid; seg.m_pointStart = c.m_segIdx; seg.m_pointEnd = c.m_segIdx + 1; - - segmentSet.insert(&seg); } OsrmFtSegMapping::OsrmNodesT nodes; - m_routingMapping.m_segMapping.GetOsrmNodes(segmentSet, nodes); + m_routingMapping.m_segMapping.GetOsrmNodes(segments, nodes); res.clear(); res.resize(maxCount); @@ -286,10 +279,10 @@ void Point2PhantomNode::CalculateWeights(FeatureGraphNode & node) const void Point2Node::operator()(FeatureType const & ft) { - if (ft.GetFeatureType() != feature::GEOM_LINE || !CarModel::Instance().IsRoad(ft)) + if (!CarModel::Instance().IsRoad(ft)) return; uint32_t const featureId = ft.GetID().m_index; - for (auto const n : m_routingMapping.m_segMapping.GetNodeIdByFid(featureId)) + for (auto const & n : m_routingMapping.m_segMapping.GetNodeIdByFid(featureId)) m_nodeIds.push_back(n); } } // namespace helpers diff --git a/routing/osrm_helpers.hpp b/routing/osrm_helpers.hpp index 4f9a9748f1..773b27556a 100644 --- a/routing/osrm_helpers.hpp +++ b/routing/osrm_helpers.hpp @@ -13,7 +13,7 @@ namespace routing { namespace helpers { -/// Class-getter for making routing tasks by geometry point. +/// A helper class that prepares structures for OSRM by a geometry near a point. class Point2PhantomNode { public: @@ -45,15 +45,16 @@ public: // Returns true if there are candidate features for routing tasks. bool HasCandidates() const { return !m_candidates.empty(); } - // Getter method. + // Functor, for getting features from a index foreach method. void operator()(FeatureType const & ft); /// Makes OSRM tasks result vector. void MakeResult(vector & res, size_t maxCount, string const & mwmName); private: - // Calculates whole segment distance in meters. - double CalculateDistance(OsrmMappingTypes::FtSeg const & s) const; + // Calculates distance in meters on the feature from startPoint to endPoint. + double CalculateDistance(FeatureType const & feature, size_t const startPoint, + size_t const endPoint) const; /// Calculates part of a node weight in the OSRM format. Projection point @segPt divides node on /// two parts. So we find weight of a part, set by the @calcFromRight parameter. @@ -87,6 +88,7 @@ public: { } + // Functor, for getting features from a index foreach method. void operator()(FeatureType const & ft); DISALLOW_COPY_AND_MOVE(Point2Node); diff --git a/routing/routing_integration_tests/osrm_route_test.cpp b/routing/routing_integration_tests/osrm_route_test.cpp index d1ee10b98e..7b44b339c2 100644 --- a/routing/routing_integration_tests/osrm_route_test.cpp +++ b/routing/routing_integration_tests/osrm_route_test.cpp @@ -232,6 +232,6 @@ namespace IRouter::ResultCode const result = routeResult.second; TEST_EQUAL(result, IRouter::NoError, ()); - integration::TestRouteTime(route, 910.); + integration::TestRouteTime(route, 900.); } } // namespace diff --git a/routing/turns_generator.cpp b/routing/turns_generator.cpp index fb305175f7..6451cf514a 100644 --- a/routing/turns_generator.cpp +++ b/routing/turns_generator.cpp @@ -74,7 +74,7 @@ public: void operator()(FeatureType const & ft) { - if (ft.GetFeatureType() != feature::GEOM_LINE || !CarModel::Instance().IsRoad(ft)) + if (!CarModel::Instance().IsRoad(ft)) return; ft.ParseGeometry(FeatureType::BEST_GEOMETRY); size_t const count = ft.GetPointsCount(); diff --git a/routing/vehicle_model.cpp b/routing/vehicle_model.cpp index 466e6f87dc..9f2cfe4412 100644 --- a/routing/vehicle_model.cpp +++ b/routing/vehicle_model.cpp @@ -63,7 +63,7 @@ bool VehicleModel::IsOneWay(feature::TypesHolder const & types) const bool VehicleModel::IsRoad(FeatureType const & f) const { - return IsRoad(feature::TypesHolder(f)); + return (f.GetFeatureType() == feature::GEOM_LINE) && IsRoad(feature::TypesHolder(f)); } bool VehicleModel::IsRoad(uint32_t type) const From d6dae813cf1b332d054bcb95d1d45bcb17731620 Mon Sep 17 00:00:00 2001 From: Lev Dragunov Date: Tue, 20 Oct 2015 11:46:49 +0300 Subject: [PATCH 4/4] PR fixes --- routing/osrm2feature_map.hpp | 4 +-- routing/osrm_helpers.cpp | 41 ++++++++++++---------- routing/osrm_helpers.hpp | 4 +-- routing/osrm_router.cpp | 68 +++++++++++++++++++++--------------- 4 files changed, 65 insertions(+), 52 deletions(-) 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)); } } }