diff options
author | Yuri Gorshenin <y@maps.me> | 2016-09-13 23:00:40 +0300 |
---|---|---|
committer | Yuri Gorshenin <y@maps.me> | 2016-09-14 18:04:35 +0300 |
commit | a41dc789d13150bbf0b25d64d44df5d1809d0585 (patch) | |
tree | ba558bb5c6e7ae100ea90dd4488a77e090f3eac4 /routing | |
parent | 46193618b6d2b93c533149c40b775f0db96e2e08 (diff) |
[routing] Fixed fake edges construction.
Diffstat (limited to 'routing')
-rw-r--r-- | routing/directions_engine.cpp | 2 | ||||
-rw-r--r-- | routing/road_graph.cpp | 234 | ||||
-rw-r--r-- | routing/road_graph.hpp | 30 | ||||
-rw-r--r-- | routing/road_graph_router.cpp | 32 |
4 files changed, 99 insertions, 199 deletions
diff --git a/routing/directions_engine.cpp b/routing/directions_engine.cpp index 6b162e5d51..53d5ed602a 100644 --- a/routing/directions_engine.cpp +++ b/routing/directions_engine.cpp @@ -13,7 +13,7 @@ void IDirectionsEngine::CalculateTimes(IRoadGraph const & graph, vector<Junction Route::TTimes & times) const { times.clear(); - if (path.size() <= 1) + if (path.size() < 1) return; // graph.GetMaxSpeedKMPH() below is used on purpose. diff --git a/routing/road_graph.cpp b/routing/road_graph.cpp index c7ed2612c4..a2ef84c276 100644 --- a/routing/road_graph.cpp +++ b/routing/road_graph.cpp @@ -6,9 +6,11 @@ #include "geometry/mercator.hpp" #include "geometry/distance_on_sphere.hpp" +#include "geometry/segment2d.hpp" #include "base/assert.hpp" #include "base/math.hpp" +#include "base/stl_helpers.hpp" #include "std/limits.hpp" #include "std/sstream.hpp" @@ -17,50 +19,19 @@ namespace routing { namespace { -vector<Edge>::const_iterator FindEdgeContainingPoint(vector<Edge> const & edges, m2::PointD const & pt) +bool OnEdge(Junction const & p, Edge const & ab) { - // A P B - // o-----------x---------------o - - auto const liesOnEdgeFn = [&pt](Edge const & e) - { - // Point P lies on edge AB if: - // - P corresponds to edge's start point A or edge's end point B - // - angle between PA and PB is 180 degrees - - m2::PointD const v1 = e.GetStartJunction().GetPoint() - pt; - m2::PointD const v2 = e.GetEndJunction().GetPoint() - pt; - if (my::AlmostEqualAbs(v1, m2::PointD::Zero(), kPointsEqualEpsilon) - || my::AlmostEqualAbs(v2, m2::PointD::Zero(), kPointsEqualEpsilon)) - { - // Point p corresponds to the start or the end of the edge. - return true; - } - - // If an angle between two vectors is 180 degrees then dot product is negative and cross product is 0. - if (m2::DotProduct(v1, v2) < 0.0) - { - double constexpr kEpsilon = 1e-9; - if (my::AlmostEqualAbs(m2::CrossProduct(v1, v2), 0.0, kEpsilon)) - { - // Point p lies on edge. - return true; - } - } - - return false; - }; - - return find_if(edges.begin(), edges.end(), liesOnEdgeFn); + auto const & a = ab.GetStartJunction(); + auto const & b = ab.GetEndJunction(); + return m2::IsPointOnSegment(p.GetPoint(), a.GetPoint(), b.GetPoint()); } -/// \brief Reverses |edges| starting from index |beginIdx| and upto the end of |v|. -void ReverseEdges(size_t beginIdx, IRoadGraph::TEdgeVector & edges) +void SplitEdge(Edge const & ab, Junction const & p, vector<Edge> & edges) { - ASSERT_LESS_OR_EQUAL(beginIdx, edges.size(), ("Index too large.")); - - for (size_t i = beginIdx; i < edges.size(); ++i) - edges[i] = edges[i].GetReverseEdge(); + auto const & a = ab.GetStartJunction(); + auto const & b = ab.GetEndJunction(); + edges.push_back(Edge::MakeFake(a, p)); + edges.push_back(Edge::MakeFake(p, b)); } } // namespace @@ -74,7 +45,7 @@ Junction::Junction(m2::PointD const & point, feature::TAltitude altitude) string DebugPrint(Junction const & r) { ostringstream ss; - ss << "Junction{point:" << DebugPrint(r.m_point) << ", altitude:}" << r.GetAltitude(); + ss << "Junction{point:" << DebugPrint(r.m_point) << ", altitude:" << r.GetAltitude() << "}"; return ss.str(); } @@ -199,172 +170,56 @@ void IRoadGraph::GetRegularIngoingEdges(Junction const & junction, TEdgeVector & void IRoadGraph::GetFakeOutgoingEdges(Junction const & junction, TEdgeVector & edges) const { - auto const itr = m_outgoingEdges.find(junction); - if (itr == m_outgoingEdges.cend()) - return; - - edges.reserve(edges.size() + itr->second.size()); - edges.insert(edges.end(), itr->second.begin(), itr->second.end()); + auto const it = m_fakeOutgoingEdges.find(junction); + if (it != m_fakeOutgoingEdges.cend()) + edges.insert(edges.end(), it->second.begin(), it->second.end()); } void IRoadGraph::GetFakeIngoingEdges(Junction const & junction, TEdgeVector & edges) const { - size_t const oldSize = edges.size(); - GetFakeOutgoingEdges(junction, edges); - ReverseEdges(oldSize, edges); + auto const it = m_fakeIngoingEdges.find(junction); + if (it != m_fakeIngoingEdges.cend()) + edges.insert(edges.end(), it->second.begin(), it->second.end()); } void IRoadGraph::ResetFakes() { - m_outgoingEdges.clear(); + m_fakeOutgoingEdges.clear(); + m_fakeIngoingEdges.clear(); } + void IRoadGraph::AddFakeEdges(Junction const & junction, vector<pair<Edge, Junction>> const & vicinity) { for (auto const & v : vicinity) { - Edge const & closestEdge = v.first; + Edge const & ab = v.first; Junction const p = v.second; - if (p == closestEdge.GetStartJunction() || p == closestEdge.GetEndJunction()) - { - // The point is mapped on the start junction of the edge or on the end junction of the edge: - // o M o M - // ^ ^ - // | | - // | | - // (P) A o--------------->o B or A o--------------->o B (P) (the feature is A->B) - // Here AB is a feature, M is a junction, which is projected to A (where P is projection), - // P is the closest junction of the feature to the junction M. - - // Add outgoing edges for M. - TEdgeVector & edgesM = m_outgoingEdges[junction]; - edgesM.push_back(Edge::MakeFake(junction, p)); - - // Add outgoing edges for P. - TEdgeVector & edgesP = m_outgoingEdges[p]; - GetRegularOutgoingEdges(p, edgesP); - edgesP.push_back(Edge::MakeFake(p, junction)); - } - else - { - Edge edgeToSplit = closestEdge; - - vector<Edge> splittingToFakeEdges; - if (HasBeenSplitToFakes(closestEdge, splittingToFakeEdges)) - { - // Edge AB has already been split by some point Q and this point P - // should split AB one more time - // o M - // ^ - // | - // | - // A o<-------x--------------x------------->o B - // P Q - - auto const itr = FindEdgeContainingPoint(splittingToFakeEdges, p.GetPoint()); - CHECK(itr != splittingToFakeEdges.end(), ()); - - edgeToSplit = *itr; - } - else - { - // The point P is mapped in the middle of the feature AB - - TEdgeVector & edgesA = m_outgoingEdges[edgeToSplit.GetStartJunction()]; - if (edgesA.empty()) - GetRegularOutgoingEdges(edgeToSplit.GetStartJunction(), edgesA); - - TEdgeVector & edgesB = m_outgoingEdges[edgeToSplit.GetEndJunction()]; - if (edgesB.empty()) - GetRegularOutgoingEdges(edgeToSplit.GetEndJunction(), edgesB); - } - - // o M - // ^ - // | - // | - // A o<-------x------->o B - // P - // Here AB is the edge to split, M is a junction and P is the projection of M on AB, - - // Edge AB is split into two fake edges AP and PB (similarly BA edge has been split into two - // fake edges BP and PA). In the result graph edges AB and BA are redundant, therefore edges AB and BA are - // replaced by fake edges AP + PB and BP + PA. - - Edge const ab = edgeToSplit; - - Edge const pa(ab.GetFeatureId(), false /* forward */, ab.GetSegId(), p, ab.GetStartJunction()); - Edge const pb(ab.GetFeatureId(), true /* forward */, ab.GetSegId(), p, ab.GetEndJunction()); - Edge const pm = Edge::MakeFake(p, junction); - - // Add outgoing edges to point P. - TEdgeVector & edgesP = m_outgoingEdges[p]; - edgesP.push_back(pa); - edgesP.push_back(pb); - edgesP.push_back(pm); - - // Add outgoing edges for point M. - m_outgoingEdges[junction].push_back(pm.GetReverseEdge()); - - // Replace AB edge with AP edge. - TEdgeVector & edgesA = m_outgoingEdges[pa.GetEndJunction()]; - Edge const ap = pa.GetReverseEdge(); - edgesA.erase(remove_if(edgesA.begin(), edgesA.end(), [&](Edge const & e) { return e.SameRoadSegmentAndDirection(ap); }), edgesA.end()); - edgesA.push_back(ap); - - // Replace BA edge with BP edge. - TEdgeVector & edgesB = m_outgoingEdges[pb.GetEndJunction()]; - Edge const bp = pb.GetReverseEdge(); - edgesB.erase(remove_if(edgesB.begin(), edgesB.end(), [&](Edge const & e) { return e.SameRoadSegmentAndDirection(bp); }), edgesB.end()); - edgesB.push_back(bp); - } - } - - // m_outgoingEdges may contain duplicates. Remove them. - for (auto & m : m_outgoingEdges) - { - TEdgeVector & edges = m.second; - sort(edges.begin(), edges.end()); - edges.erase(unique(edges.begin(), edges.end()), edges.end()); - } -} + vector<Edge> edges; + SplitEdge(ab, p, edges); + edges.push_back(Edge::MakeFake(junction, p)); + edges.push_back(Edge::MakeFake(p, junction)); -bool IRoadGraph::HasBeenSplitToFakes(Edge const & edge, vector<Edge> & fakeEdges) const -{ - vector<Edge> tmp; - - if (m_outgoingEdges.find(edge.GetStartJunction()) == m_outgoingEdges.end() || - m_outgoingEdges.find(edge.GetEndJunction()) == m_outgoingEdges.end()) - return false; - - auto i = m_outgoingEdges.end(); - Junction junction = edge.GetStartJunction(); - while (m_outgoingEdges.end() != (i = m_outgoingEdges.find(junction))) - { - auto const & edges = i->second; + ForEachFakeEdge([&](Edge const & uv) + { + if (OnEdge(p, uv)) + SplitEdge(uv, p, edges); + }); - auto const j = find_if(edges.begin(), edges.end(), [&edge](Edge const & e) { return e.SameRoadSegmentAndDirection(edge); }); - if (j == edges.end()) + for (auto const & uv : edges) { - ASSERT(fakeEdges.empty(), ()); - return false; + auto const & u = uv.GetStartJunction(); + auto const & v = uv.GetEndJunction(); + m_fakeOutgoingEdges[u].push_back(uv); + m_fakeIngoingEdges[v].push_back(uv); } - - tmp.push_back(*j); - - junction = j->GetEndJunction(); - if (junction == edge.GetEndJunction()) - break; } - if (i == m_outgoingEdges.end()) - return false; - if (tmp.empty()) - return false; - - fakeEdges.swap(tmp); - return true; + for (auto & m : m_fakeIngoingEdges) + my::SortUnique(m.second); + for (auto & m : m_fakeOutgoingEdges) + my::SortUnique(m.second); } double IRoadGraph::GetSpeedKMPH(Edge const & edge) const @@ -382,6 +237,15 @@ void IRoadGraph::GetEdgeTypes(Edge const & edge, feature::TypesHolder & types) c GetFeatureTypes(edge.GetFeatureId(), types); } +string DebugPrint(IRoadGraph::Mode mode) +{ + switch (mode) + { + case IRoadGraph::Mode::ObeyOnewayTag: return "ObeyOnewayTag"; + case IRoadGraph::Mode::IgnoreOnewayTag: return "IgnoreOnewayTag"; + } +} + IRoadGraph::RoadInfo MakeRoadInfoForTesting(bool bidirectional, double speedKMPH, initializer_list<m2::PointD> const & points) { diff --git a/routing/road_graph.hpp b/routing/road_graph.hpp index 880377dc71..dee5275414 100644 --- a/routing/road_graph.hpp +++ b/routing/road_graph.hpp @@ -145,9 +145,11 @@ public: { if (!my::AlmostEqualAbs(m_cross.GetPoint(), roadInfo.m_junctions[i].GetPoint(), kPointsEqualEpsilon)) + { continue; + } - if (i < roadInfo.m_junctions.size() - 1) + if (i + 1 < roadInfo.m_junctions.size()) { // Head of the edge. // m_cross @@ -256,13 +258,31 @@ private: /// \brief Finds all ingoing fake edges for junction. void GetFakeIngoingEdges(Junction const & junction, TEdgeVector & edges) const; - /// Determines if the edge has been split by fake edges and if yes returns these fake edges. - bool HasBeenSplitToFakes(Edge const & edge, vector<Edge> & fakeEdges) const; + template <typename Fn> + void ForEachFakeEdge(Fn && fn) + { + for (auto const & m : m_fakeIngoingEdges) + { + for (auto const & e : m.second) + fn(e); + } + + for (auto const & m : m_fakeOutgoingEdges) + { + for (auto const & e : m.second) + fn(e); + } + } - // Map of outgoing edges for junction - map<Junction, TEdgeVector> m_outgoingEdges; + // Map of ingoing edges for junctions. + map<Junction, TEdgeVector> m_fakeIngoingEdges; + + // Map of outgoing edges for junctions. + map<Junction, TEdgeVector> m_fakeOutgoingEdges; }; +string DebugPrint(IRoadGraph::Mode mode); + IRoadGraph::RoadInfo MakeRoadInfoForTesting(bool bidirectional, double speedKMPH, initializer_list<m2::PointD> const & points); diff --git a/routing/road_graph_router.cpp b/routing/road_graph_router.cpp index 2ce3321978..9e1ae3ae0e 100644 --- a/routing/road_graph_router.cpp +++ b/routing/road_graph_router.cpp @@ -96,22 +96,38 @@ bool CheckGraphConnectivity(IRoadGraph const & graph, Junction const & junction, void FindClosestEdges(IRoadGraph const & graph, m2::PointD const & point, vector<pair<Edge, Junction>> & vicinity) { - // WARNING: Take only one vicinity - // It is an oversimplification that is not as easily - // solved as tuning up this constant because if you set it too high - // you risk to find a feature that you cannot in fact reach because of - // an obstacle. Using only the closest feature minimizes (but not - // eliminates) this risk. - + // WARNING: Take only one vicinity, with, maybe, it's inverse. It + // is an oversimplification that is not as easily solved as tuning + // up this constant because if you set it too high you risk to find + // a feature that you cannot in fact reach because of an obstacle. + // Using only the closest feature minimizes (but not eliminates) + // this risk. vector<pair<Edge, Junction>> candidates; graph.FindClosestEdges(point, kMaxRoadCandidates, candidates); vicinity.clear(); for (auto const & candidate : candidates) { - if (CheckGraphConnectivity(graph, candidate.first.GetStartJunction(), kTestConnectivityVisitJunctionsLimit)) + auto const & edge = candidate.first; + if (CheckGraphConnectivity(graph, edge.GetStartJunction(), kTestConnectivityVisitJunctionsLimit)) { vicinity.emplace_back(candidate); + + // Need to add a reverse edge, if exists, because fake edges + // must be added for reverse edge too. + IRoadGraph::TEdgeVector revEdges; + graph.GetOutgoingEdges(edge.GetEndJunction(), revEdges); + for (auto const & revEdge : revEdges) + { + if (revEdge.GetFeatureId() == edge.GetFeatureId() && + revEdge.GetEndJunction() == edge.GetStartJunction() && + revEdge.GetSegId() == edge.GetSegId()) + { + vicinity.emplace_back(revEdge, candidate.second); + break; + } + } + break; } } |