diff --git a/routing/index_graph_starter_joints.hpp b/routing/index_graph_starter_joints.hpp index 991483810f..9c02ecc280 100644 --- a/routing/index_graph_starter_joints.hpp +++ b/routing/index_graph_starter_joints.hpp @@ -63,9 +63,9 @@ public: bool AreWavesConnectible(std::map & forwardParents, Vertex const & commonVertex, std::map & backwardParents) override { - auto converter = [&](JointSegment const & vertex) { - if (vertex.IsRealSegment()) - return vertex.GetFeatureId(); + auto converter = [&](JointSegment const & vertex) + { + ASSERT(!vertex.IsRealSegment(), ()); auto const it = m_fakeJointSegments.find(vertex); ASSERT(it != m_fakeJointSegments.cend(), ()); diff --git a/routing/restriction_loader.cpp b/routing/restriction_loader.cpp index b5ef10efaf..6ef8f42242 100644 --- a/routing/restriction_loader.cpp +++ b/routing/restriction_loader.cpp @@ -80,31 +80,38 @@ RestrictionVec && RestrictionLoader::StealRestrictions() return std::move(m_restrictions); } +bool IsRestrictionFromRoads(IndexGraph const & graph, std::vector const & restriction) +{ + for (auto const & featureId : restriction) + { + if (!graph.IsRoad(featureId)) + return false; + } + + return true; +} + /// \brief We store |Only| restriction in mwm, like /// Only, featureId_1, ... , featureId_N +/// /// We convert such restriction to |No| following way: -/// For each pair of neighbouring features (featureId_1 and featureId_2 for ex.) -/// we forbid to go from featureId_1 to wherever except featureId_2. -/// Then do this for featureId_2 and featureId_3 and so on. +/// For each M, greater than 1, and M first features: +/// featureId_1, ... , featureId_(M - 1), featureId_M, ... +/// +/// We create restrictionNo from features: +/// featureId_1, ... , featureId_(M - 1), featureId_K +/// where featureId_K - has common joint with featureId_(M - 1) and featureId_M. void ConvertRestrictionsOnlyToNoAndSort(IndexGraph const & graph, RestrictionVec const & restrictionsOnly, RestrictionVec & restrictionsNo) { - for (auto const & restriction : restrictionsOnly) + for (std::vector const & restriction : restrictionsOnly) { - if (std::any_of(restriction.begin(), restriction.end(), - [&graph](auto const & item) - { - return !graph.IsRoad(item); - })) - { - continue; - } - CHECK_GREATER_OR_EQUAL(restriction.size(), 2, ()); - // vector of simple no_restriction - from|to. - std::vector> toAdd; - bool error = false; + + if (!IsRestrictionFromRoads(graph, restriction)) + continue; + for (size_t i = 1; i < restriction.size(); ++i) { auto const curFeatureId = restriction[i]; @@ -115,31 +122,22 @@ void ConvertRestrictionsOnlyToNoAndSort(IndexGraph const & graph, GetCommonEndJoint(graph.GetRoad(curFeatureId), graph.GetRoad(prevFeatureId)); if (common == Joint::kInvalidId) - { - error = true; break; - } - // Adding restriction of type No for all features of joint |common| except for - // |prevFeatureId| -> |curFeatureId|. + std::vector commonFeatures; + commonFeatures.resize(i + 1); + std::copy(restriction.begin(), restriction.begin() + i, commonFeatures.begin()); + graph.ForEachPoint(common, [&](RoadPoint const & rp) { if (rp.GetFeatureId() != curFeatureId) - toAdd.emplace_back(prevFeatureId, rp.GetFeatureId()); + { + commonFeatures.back() = rp.GetFeatureId(); + restrictionsNo.emplace_back(commonFeatures); + } }); } - - if (error) - continue; - - for (auto const & fromToPair : toAdd) - { - std::vector newRestriction = - {fromToPair.first /* from */, fromToPair.second /* to */}; - - restrictionsNo.emplace_back(std::move(newRestriction)); - } } base::SortUnique(restrictionsNo); diff --git a/routing/single_vehicle_world_graph.cpp b/routing/single_vehicle_world_graph.cpp index 575370948b..dd0ca951d9 100644 --- a/routing/single_vehicle_world_graph.cpp +++ b/routing/single_vehicle_world_graph.cpp @@ -250,6 +250,24 @@ void SingleVehicleWorldGraph::SetAStarParents(bool forward, map +NumMwmId GetCommonMwmInChain(vector const & chain) +{ + NumMwmId mwmId = kFakeNumMwmId; + for (size_t i = 0; i < chain.size(); ++i) + { + if (chain[i].GetMwmId() == kFakeNumMwmId) + continue; + + if (mwmId != kFakeNumMwmId && mwmId != chain[i].GetMwmId()) + return kFakeNumMwmId; + + mwmId = chain[i].GetMwmId(); + } + + return mwmId; +} + template bool SingleVehicleWorldGraph::AreWavesConnectibleImpl(map const & forwardParents, @@ -258,7 +276,6 @@ SingleVehicleWorldGraph::AreWavesConnectibleImpl(map con function && fakeFeatureConverter) { vector chain; - NumMwmId mwmId = kFakeNumMwmId; auto const fillUntilNextFeatureId = [&](VertexType const & cur, map const & parents) { auto startFeatureId = cur.GetFeatureId(); @@ -305,22 +322,14 @@ SingleVehicleWorldGraph::AreWavesConnectibleImpl(map con } } - map parents; - for (size_t i = 1; i < chain.size(); ++i) - { - parents[chain[i]] = chain[i - 1]; - if (chain[i].IsRealSegment()) - { - if (mwmId != kFakeNumMwmId && mwmId != chain[i].GetMwmId()) - return true; - - mwmId = chain[i].GetMwmId(); - } - } - + NumMwmId mwmId = GetCommonMwmInChain(chain); if (mwmId == kFakeNumMwmId) return true; + map parents; + for (size_t i = 1; i < chain.size(); ++i) + parents[chain[i]] = chain[i - 1]; + auto & currentIndexGraph = GetIndexGraph(mwmId); for (size_t i = 1; i < chain.size(); ++i) {