From 54d2373b59bda6e3be82e67ab1711c1394e07fa4 Mon Sep 17 00:00:00 2001 From: Yuri Gorshenin Date: Fri, 10 Apr 2015 01:14:29 +0300 Subject: [PATCH] [pedestrian] Fixes to algorithm that computes adjacency lists. --- routing/astar_router.cpp | 9 +- routing/dijkstra_router.cpp | 5 +- routing/features_road_graph.cpp | 151 +++++++----------- routing/features_road_graph.hpp | 5 +- routing/road_graph.hpp | 7 +- .../features_road_graph_test.cpp | 18 +-- .../features_road_graph_test.hpp | 2 +- routing/routing_tests/road_graph_builder.cpp | 3 +- routing/routing_tests/road_graph_builder.hpp | 6 +- 9 files changed, 87 insertions(+), 119 deletions(-) diff --git a/routing/astar_router.cpp b/routing/astar_router.cpp index 71087cc9f3..6df8469a75 100644 --- a/routing/astar_router.cpp +++ b/routing/astar_router.cpp @@ -66,17 +66,16 @@ IRouter::ResultCode AStarRouter::CalculateRouteM2M(vector const & start if (v.GetReducedDist() > m_bestDistance[v.GetPos()]) continue; - /// @todo why do we even need this? - bool isStartFeature = binary_search(sortedStartFeatures.begin(), sortedStartFeatures.end(), v.GetPos().GetFeatureId()); - - if (isStartFeature && binary_search(sortedStartPos.begin(), sortedStartPos.end(), v.GetPos())) + if (binary_search(sortedStartFeatures.begin(), sortedStartFeatures.end(), + v.GetPos().GetFeatureId()) && + binary_search(sortedStartPos.begin(), sortedStartPos.end(), v.GetPos())) { ReconstructRoute(v.GetPos(), route); return IRouter::NoError; } IRoadGraph::TurnsVectorT turns; - m_roadGraph->GetPossibleTurns(v.GetPos(), turns, isStartFeature); + m_roadGraph->GetNearestTurns(v.GetPos(), turns); for (IRoadGraph::TurnsVectorT::const_iterator it = turns.begin(); it != turns.end(); ++it) { PossibleTurn const & turn = *it; diff --git a/routing/dijkstra_router.cpp b/routing/dijkstra_router.cpp index ac0ccf8e0a..dd41186b2b 100644 --- a/routing/dijkstra_router.cpp +++ b/routing/dijkstra_router.cpp @@ -83,14 +83,13 @@ IRouter::ResultCode DijkstraRouter::CalculateRouteM2M(vector const & st if (v.dist > dist[v.pos]) continue; - bool const isStartFeature = Contains(sortedStartFeatures, v.pos.GetFeatureId()); - if (isStartFeature && Contains(sortedStartPos, v.pos)) + if (Contains(sortedStartFeatures, v.pos.GetFeatureId()) && Contains(sortedStartPos, v.pos)) { ReconstructPath(v.pos, parent, route); return IRouter::NoError; } IRoadGraph::TurnsVectorT turns; - m_roadGraph->GetPossibleTurns(v.pos, turns, isStartFeature); + m_roadGraph->GetNearestTurns(v.pos, turns); for (PossibleTurn const & turn : turns) { RoadPos const & pos = turn.m_pos; diff --git a/routing/features_road_graph.cpp b/routing/features_road_graph.cpp index 6b8a9e2888..583b639a90 100644 --- a/routing/features_road_graph.cpp +++ b/routing/features_road_graph.cpp @@ -130,113 +130,82 @@ void FeaturesRoadGraph::LoadFeature(uint32_t id, FeatureType & ft) ASSERT_GREATER(ft.GetPointsCount(), 1, (id)); } -void FeaturesRoadGraph::GetPossibleTurns(RoadPos const & pos, vector & turns, bool noOptimize /*= true*/) +void FeaturesRoadGraph::GetNearestTurns(RoadPos const & pos, vector & turns) { - uint32_t const ftID = pos.GetFeatureId(); + uint32_t const featureId = pos.GetFeatureId(); FeatureType ft; - CachedFeature const fc = GetCachedFeature(ftID, ft, true); + CachedFeature const fc = GetCachedFeature(featureId, ft, true); if (fc.m_speed <= 0.0) return; - int const count = static_cast(fc.m_points.size()); - bool const isForward = pos.IsForward(); - int const inc = isForward ? -1 : 1; + size_t const numPoints = fc.m_points.size(); + if (numPoints < 2) + return; - int startID = pos.GetSegId(); - ASSERT_GREATER(count, 1, ()); - ASSERT_LESS(startID, count, ()); - if (!isForward) - ++startID; + bool const isForward = pos.IsForward(); + + uint32_t const segId = pos.GetSegId(); + uint32_t const startPointId = pos.GetSegStartPointId(); + uint32_t const endPointId = pos.GetSegEndPointId(); + ASSERT_LESS(startPointId, numPoints, ()); + ASSERT_LESS(endPointId, numPoints, ()); PossibleTurn thisTurn; thisTurn.m_speed = fc.m_speed; - thisTurn.m_startPoint = fc.m_points[0]; - thisTurn.m_endPoint = fc.m_points[count - 1]; + thisTurn.m_startPoint = fc.m_points.front(); + thisTurn.m_endPoint = fc.m_points.back(); - double distance = 0.0; - double time = 0.0; - for (int i = startID; i >= 0 && i < count; i += inc) + double const segmentDistance = + CalcDistanceMeters(fc.m_points[startPointId], fc.m_points[endPointId]); + double const segmentTime = segmentDistance / fc.m_speed; + + m2::PointD const & startPoint = fc.m_points[startPointId]; + + // Find possible turns to startPoint from other features. + CrossFeaturesLoader crossLoader(featureId, *this, startPoint, turns); + m_pIndex->ForEachInRect( + crossLoader, m2::RectD(startPoint.x - READ_CROSS_EPSILON, startPoint.y - READ_CROSS_EPSILON, + startPoint.x + READ_CROSS_EPSILON, startPoint.y + READ_CROSS_EPSILON), + READ_ROAD_SCALE); + + indexCheck++; + + if (crossLoader.GetCount() > 0) + indexFound++; + + // Push turn points for this feature. + if (isForward && segId > 0) { - ASSERT_GREATER(i - inc, -1, ()); - ASSERT_LESS(i - inc, count, ()); - - double const segmentDistance = CalcDistanceMeters(fc.m_points[i], fc.m_points[i - inc]); - distance += segmentDistance; - time += segmentDistance / fc.m_speed; - - m2::PointD const & pt = fc.m_points[i]; - - // Find possible turns to point[i] from other features. - size_t const last = turns.size(); - CrossFeaturesLoader crossLoader(ftID, *this, pt, turns); - m_pIndex->ForEachInRect(crossLoader, - m2::RectD(pt.x - READ_CROSS_EPSILON, pt.y - READ_CROSS_EPSILON, - pt.x + READ_CROSS_EPSILON, pt.y + READ_CROSS_EPSILON), - READ_ROAD_SCALE); - - indexCheck++; - - if (crossLoader.GetCount() > 0) - indexFound++; - - // Skip if there are no turns on point - if (/*crossLoader.GetCount() > 0 ||*/ noOptimize) - { - // Push turn points for this feature. - if (isForward) - { - if (i > 0) - { - thisTurn.m_pos = RoadPos(ftID, true, i - 1, pt); - turns.push_back(thisTurn); - } - } - else - { - if (!fc.m_isOneway && (i != count - 1)) - { - thisTurn.m_pos = RoadPos(ftID, false, i, pt); - turns.push_back(thisTurn); - } - } - } - - // Update distance and time information. - for (size_t j = last; j < turns.size(); ++j) - { - turns[j].m_metersCovered = distance; - turns[j].m_secondsCovered = time; - turns[j].m_speed = DEFAULT_SPEED_MS; - } + thisTurn.m_pos = RoadPos(featureId, isForward, segId - 1, startPoint); + turns.push_back(thisTurn); + } + else if (!isForward && !fc.m_isOneway && segId + 2 < numPoints) + { + thisTurn.m_pos = RoadPos(featureId, isForward, segId + 1, startPoint); + turns.push_back(thisTurn); } - // Check cycle - if (m2::AlmostEqual(fc.m_points[0], fc.m_points[count - 1])) + // Update distance and time information. + for (PossibleTurn & turn : turns) { - /// @todo calculate distance and speed - if (isForward) - { - double distance = 0; - for (int i = pos.GetSegId(); i > 0; --i) - distance += CalcDistanceMeters(fc.m_points[i], fc.m_points[i - 1]); + turn.m_metersCovered = segmentDistance; + turn.m_secondsCovered = segmentTime; + turn.m_speed = DEFAULT_SPEED_MS; + } - thisTurn.m_pos = RoadPos(ftID, true, count - 2, fc.m_points[count - 1]); - thisTurn.m_metersCovered = distance; - thisTurn.m_secondsCovered = distance / DEFAULT_SPEED_MS; - turns.push_back(thisTurn); - } - else if (!fc.m_isOneway) - { - double distance = 0; - for (size_t i = pos.GetSegId(); i < count - 1; ++i) - distance += CalcDistanceMeters(fc.m_points[i], fc.m_points[i + 1]); - - thisTurn.m_pos = RoadPos(ftID, false, 0, fc.m_points[0]); - thisTurn.m_metersCovered = distance; - thisTurn.m_secondsCovered = distance / DEFAULT_SPEED_MS; - turns.push_back(thisTurn); - } + // Check cycle. + if (!m2::AlmostEqual(fc.m_points.front(), fc.m_points.back())) + return; + if (isForward && segId == 0) + { + thisTurn.m_pos = RoadPos(featureId, isForward, numPoints - 2, fc.m_points.back()); + turns.push_back(thisTurn); + } + else if (!isForward && !fc.m_isOneway && segId + 2 == numPoints) + { + thisTurn.m_pos = RoadPos(featureId, isForward, 0, fc.m_points.front()); + turns.push_back(thisTurn); } } diff --git a/routing/features_road_graph.hpp b/routing/features_road_graph.hpp index 2abfd904aa..fe2bff254f 100644 --- a/routing/features_road_graph.hpp +++ b/routing/features_road_graph.hpp @@ -33,8 +33,9 @@ class FeaturesRoadGraph : public IRoadGraph public: FeaturesRoadGraph(Index const * pIndex, size_t mwmID); - virtual void GetPossibleTurns(RoadPos const & pos, vector & turns, bool noOptimize = true); - virtual void ReconstructPath(RoadPosVectorT const & positions, Route & route); + // IRoadGraph overrides: + void GetNearestTurns(RoadPos const & pos, vector & turns) override; + void ReconstructPath(RoadPosVectorT const & positions, Route & route) override; static uint32_t GetStreetReadScale(); diff --git a/routing/road_graph.hpp b/routing/road_graph.hpp index 67f0a01789..e564ea7a07 100644 --- a/routing/road_graph.hpp +++ b/routing/road_graph.hpp @@ -90,9 +90,10 @@ public: virtual ~IRoadGraph() {} - /// Find all feature sections (turns), that route to the "pos" section. - virtual void GetPossibleTurns(RoadPos const & pos, TurnsVectorT & turns, - bool noOptimize = true) = 0; + /// Finds all nearest feature sections (turns), that route to the + /// "pos" section. + virtual void GetNearestTurns(RoadPos const & pos, TurnsVectorT & turns) = 0; + /// Construct route by road positions (doesn't include first and last section). virtual void ReconstructPath(RoadPosVectorT const & positions, Route & route) = 0; }; diff --git a/routing/routing_tests/features_road_graph_test.cpp b/routing/routing_tests/features_road_graph_test.cpp index 823fd0b0db..7924973572 100644 --- a/routing/routing_tests/features_road_graph_test.cpp +++ b/routing/routing_tests/features_road_graph_test.cpp @@ -104,11 +104,11 @@ void FeatureRoadGraphTester::Name2FeatureID(vector & vec) vec[i].GetSegId()); } -void FeatureRoadGraphTester::GetPossibleTurns(RoadPos const & pos, IRoadGraph::TurnsVectorT & vec, bool noOptimize/* = true*/) +void FeatureRoadGraphTester::GetPossibleTurns(RoadPos const & pos, IRoadGraph::TurnsVectorT & vec) { - m_graph->GetPossibleTurns(RoadPos(m_mapping.GetId(strings::to_string(pos.GetFeatureId())), + m_graph->GetNearestTurns(RoadPos(m_mapping.GetId(strings::to_string(pos.GetFeatureId())), pos.IsForward(), pos.GetSegId()), - vec, noOptimize); + vec); FeatureID2Name(vec); } @@ -161,7 +161,7 @@ UNIT_TEST(FRG_TurnsTest_MWM1) { IRoadGraph::TurnsVectorT vec; - tester.GetPossibleTurns(RoadPos(1, false, 0), vec, false); + tester.GetPossibleTurns(RoadPos(1, false, 0), vec); TEST_EQUAL(vec.size(), 3, ()); TEST(TestResult(vec, RoadPos(1, false, 2), 10), ()); TEST(TestResult(vec, RoadPos(0, true, 1), 10), ()); @@ -175,7 +175,7 @@ UNIT_TEST(FRG_TurnsTest_MWM2) { IRoadGraph::TurnsVectorT vec; - tester.GetPossibleTurns(RoadPos(0, false, 0), vec, false); + tester.GetPossibleTurns(RoadPos(0, false, 0), vec); TEST_EQUAL(vec.size(), 8, ()); TEST(TestResult(vec, RoadPos(0, false, 1), -1), ()); TEST(TestResult(vec, RoadPos(0, false, 2), -1), ()); @@ -189,7 +189,7 @@ UNIT_TEST(FRG_TurnsTest_MWM2) { IRoadGraph::TurnsVectorT vec; - tester.GetPossibleTurns(RoadPos(8, true, 0), vec, false); + tester.GetPossibleTurns(RoadPos(8, true, 0), vec); TEST_EQUAL(vec.size(), 2, ()); TEST(TestResult(vec, RoadPos(1, true, 1), -1), ()); TEST(TestResult(vec, RoadPos(8, true, 5), -1), ()); @@ -197,7 +197,7 @@ UNIT_TEST(FRG_TurnsTest_MWM2) { IRoadGraph::TurnsVectorT vec; - tester.GetPossibleTurns(RoadPos(2, true, 1), vec, false); + tester.GetPossibleTurns(RoadPos(2, true, 1), vec); TEST_EQUAL(vec.size(), 4, ()); TEST(TestResult(vec, RoadPos(3, true, 0), -1), ()); TEST(TestResult(vec, RoadPos(3, false, 1), -1), ()); @@ -207,7 +207,7 @@ UNIT_TEST(FRG_TurnsTest_MWM2) { IRoadGraph::TurnsVectorT vec; - tester.GetPossibleTurns(RoadPos(3, false, 0), vec, false); + tester.GetPossibleTurns(RoadPos(3, false, 0), vec); TEST_EQUAL(vec.size(), 5, ()); TEST(TestResult(vec, RoadPos(3, false, 1), -1), ()); TEST(TestResult(vec, RoadPos(3, false, 2), -1), ()); @@ -231,7 +231,7 @@ UNIT_TEST(FRG_TurnsTest_MWM2) { IRoadGraph::TurnsVectorT vec; - tester.GetPossibleTurns(RoadPos(8, true, 3), vec, false); + tester.GetPossibleTurns(RoadPos(8, true, 3), vec); TEST_EQUAL(vec.size(), 7, ()); TEST(TestResult(vec, RoadPos(8, true, 2), -1), ()); TEST(TestResult(vec, RoadPos(5, true, 0), -1), ()); diff --git a/routing/routing_tests/features_road_graph_test.hpp b/routing/routing_tests/features_road_graph_test.hpp index 3e6ebc77ba..f71b6b347b 100644 --- a/routing/routing_tests/features_road_graph_test.hpp +++ b/routing/routing_tests/features_road_graph_test.hpp @@ -39,7 +39,7 @@ public: void FeatureID2Name(vector & vec); void Name2FeatureID(vector & vec); - void GetPossibleTurns(routing::RoadPos const & pos, routing::IRoadGraph::TurnsVectorT & vec, bool noOptimize = true); + void GetPossibleTurns(routing::RoadPos const & pos, routing::IRoadGraph::TurnsVectorT & vec); template void ReconstructPath(routing::RoadPos (&arr)[N], vector & vec); diff --git a/routing/routing_tests/road_graph_builder.cpp b/routing/routing_tests/road_graph_builder.cpp index 81f0f342b4..c35827d536 100644 --- a/routing/routing_tests/road_graph_builder.cpp +++ b/routing/routing_tests/road_graph_builder.cpp @@ -43,8 +43,7 @@ void RoadGraphMockSource::AddRoad(RoadInfo & rd) m_roads.back().Swap(rd); } -void RoadGraphMockSource::GetPossibleTurns(RoadPos const & pos, TurnsVectorT & turns, - bool /*noOptimize = true*/) +void RoadGraphMockSource::GetNearestTurns(RoadPos const & pos, TurnsVectorT & turns) { uint32_t const fID = pos.GetFeatureId(); ASSERT_LESS(fID, m_roads.size(), ()); diff --git a/routing/routing_tests/road_graph_builder.hpp b/routing/routing_tests/road_graph_builder.hpp index cf573574a6..65bf15e2e5 100644 --- a/routing/routing_tests/road_graph_builder.hpp +++ b/routing/routing_tests/road_graph_builder.hpp @@ -47,9 +47,9 @@ class RoadGraphMockSource : public routing::IRoadGraph public: void AddRoad(RoadInfo & rd); - virtual void GetPossibleTurns(routing::RoadPos const & pos, TurnsVectorT & turns, - bool noOptimize = true); - virtual void ReconstructPath(RoadPosVectorT const & positions, routing::Route & route); + // routing::IRoadGraph overrides: + void GetNearestTurns(routing::RoadPos const & pos, TurnsVectorT & turns) override; + void ReconstructPath(RoadPosVectorT const & positions, routing::Route & route) override; }; void InitRoadGraphMockSourceWithTest1(RoadGraphMockSource & graphMock);