diff options
| -rw-r--r-- | generator/feature_builder.cpp | 3 | ||||
| -rw-r--r-- | generator/generator_tests/osm_type_test.cpp | 2 | ||||
| -rw-r--r-- | routing/car_model.cpp | 7 | ||||
| -rw-r--r-- | routing/car_model.hpp | 4 | ||||
| -rw-r--r-- | routing/osrm2feature_map.cpp | 62 | ||||
| -rw-r--r-- | routing/osrm2feature_map.hpp | 11 | ||||
| -rw-r--r-- | routing/osrm_helpers.cpp | 298 | ||||
| -rw-r--r-- | routing/osrm_helpers.hpp | 95 | ||||
| -rw-r--r-- | routing/osrm_router.cpp | 323 | ||||
| -rw-r--r-- | routing/routing.pro | 2 | ||||
| -rw-r--r-- | routing/routing_integration_tests/osrm_route_test.cpp | 7 | ||||
| -rw-r--r-- | routing/routing_tests/routing_mapping_test.cpp | 32 | ||||
| -rw-r--r-- | routing/turns_generator.cpp | 33 |
13 files changed, 519 insertions, 360 deletions
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<uint16_t>(ps)), m_pointEnd(static_cast<uint16_t>(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<m2::PointD> 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<uint32_t>(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<int>(minWeight * ratio), 0); +} + +EdgeWeight Point2PhantomNode::GetMinNodeWeight(NodeID node, m2::PointD const & point) const +{ + static double const kInfinity = numeric_limits<EdgeWeight>::infinity(); + static double const kReadCrossRadiusM = 1.0E-4; + EdgeWeight minWeight = kInfinity; + // Geting nodes by geometry. + vector<NodeID> 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<FeatureGraphNode> & res, size_t maxCount, + string const & mwmName) +{ + vector<OsrmMappingTypes::FtSeg> 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<double>::max()), + m_segIdx(numeric_limits<uint32_t>::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<FeatureGraphNode> & 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<Candidate, 128> 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<NodeID> & n_nodeIds; + +public: + Point2Node(RoutingMapping const & routingMapping, vector<NodeID> & 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<double>::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<m2::PointD> 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<uint32_t>(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<int>(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<OsrmMappingTypes::FtSeg> 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<Candidate, 128> 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<NodeID> & n_nodeIds; - -public: - Point2Node(RoutingMapping const & routingMapping, vector<NodeID> & 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<NodeID> geomNodes; - Point2Node p2n(routingMapping, geomNodes); + helpers::Point2Node p2n(routingMapping, geomNodes); index.ForEachInRectForMWM( p2n, m2::RectD(junctionPoint.x - kReadCrossEpsilon, junctionPoint.y - kReadCrossEpsilon, |