diff --git a/routing/road_graph.cpp b/routing/road_graph.cpp index 5e3b43700f..252312a425 100644 --- a/routing/road_graph.cpp +++ b/routing/road_graph.cpp @@ -1 +1,16 @@ #include "road_graph.hpp" + +#include "../std/sstream.hpp" + + +namespace routing +{ + +string DebugPrint(RoadPos const & r) +{ + ostringstream ss; + ss << "{" << r.GetFeatureId() << ", " << r.m_pointId << ", " << r.IsForward() << "}"; + return ss.str(); +} + +} // namespace routing diff --git a/routing/road_graph.hpp b/routing/road_graph.hpp index ebfeaa9e8d..bd69b26f86 100644 --- a/routing/road_graph.hpp +++ b/routing/road_graph.hpp @@ -3,6 +3,7 @@ #include "../geometry/point2d.hpp" #include "../base/assert.hpp" +#include "../base/string_utils.hpp" #include "../std/vector.hpp" @@ -14,6 +15,7 @@ namespace routing class RoadPos { public: + RoadPos() : m_featureId(0), m_pointId(0) {} RoadPos(uint32_t featureId, bool bForward, size_t pointId) : m_featureId((featureId << 1) + (bForward ? 1 : 0)), m_pointId(pointId) { @@ -25,7 +27,14 @@ public: bool IsForward() const { return (m_featureId & 1) != 0; } uint32_t GetPointId() const { return m_pointId; } + bool operator==(RoadPos const & r) const + { + return (m_featureId == r.m_featureId && m_pointId == r.m_pointId); + } + private: + friend string DebugPrint(RoadPos const & r); + uint32_t m_featureId; uint32_t m_pointId; }; @@ -41,12 +50,28 @@ struct PossibleTurn /// Distance and time to get to this turn on old road. double m_metersCovered; double m_secondsCovered; + + PossibleTurn() : m_metersCovered(0.0), m_secondsCovered(0.0) {} }; +inline string DebugPrint(PossibleTurn const & r) +{ + return DebugPrint(r.m_pos); +} + class IRoadGraph { public: - virtual void GetPossibleTurns(RoadPos const & pos, vector & turns) = 0; + typedef vector TurnsVectorT; + typedef vector RoadPosVectorT; + typedef vector PointsVectorT; + + /// Find all line feature sections, that follow the to pos section. + virtual void GetPossibleTurns(RoadPos const & pos, TurnsVectorT & turns) = 0; + /// Calculate distance in meters between two RoadPos that placed on the same feature + virtual double GetFeatureDistance(RoadPos const & p1, RoadPos const & p2) = 0; + /// Construct full path by road positions + virtual void ReconstructPath(RoadPosVectorT const & positions, PointsVectorT & poly) = 0; }; -} +} // namespace routing diff --git a/routing/routing_tests/road_graph_builder.cpp b/routing/routing_tests/road_graph_builder.cpp new file mode 100644 index 0000000000..a4f82718d7 --- /dev/null +++ b/routing/routing_tests/road_graph_builder.cpp @@ -0,0 +1,107 @@ +#include "road_graph_builder.hpp" + +#include "../../std/algorithm.hpp" + +using namespace routing; + +namespace routing_test +{ + +void RoadInfo::Swap(RoadInfo & r) +{ + m_points.swap(r.m_points); + std::swap(m_speedMS, r.m_speedMS); + std::swap(m_bothSides, r.m_bothSides); +} + +void RoadGraphMockSource::AddRoad(RoadInfo & rd) +{ + /// @todo Do ASSERT for RoadInfo params. + + uint32_t const id = m_roads.size(); + + // Count of sections! (not points) + size_t count = rd.m_points.size(); + ASSERT_GREATER(count, 1, ()); + + for (size_t i = 0; i < count; ++i) + { + TurnsMapT::iterator j = m_turns.insert(make_pair(rd.m_points[i], TurnsVectorT())).first; + + PossibleTurn t; + t.m_startPoint = rd.m_points.front(); + t.m_endPoint = rd.m_points.back(); + t.m_speed = rd.m_speedMS; + + if (i > 0) + { + t.m_pos = RoadPos(id, true, i - 1); + j->second.push_back(t); + } + + if (rd.m_bothSides && (i < count - 1)) + { + t.m_pos = RoadPos(id, false, i); + j->second.push_back(t); + } + } + + m_roads.push_back(RoadInfo()); + m_roads.back().Swap(rd); +} + +void RoadGraphMockSource::GetPossibleTurns(RoadPos const & pos, TurnsVectorT & turns) +{ + uint32_t const fID = pos.GetFeatureId(); + ASSERT_LESS(fID, m_roads.size(), ()); + + vector const & points = m_roads[fID].m_points; + + int const inc = pos.IsForward() ? -1 : 1; + int startID = pos.GetPointId(); + int const count = static_cast(points.size()); + + if (!pos.IsForward()) + ++startID; + + double const speed = m_roads[fID].m_speedMS; + + double distance = 0.0; + double time = 0.0; + for (int i = startID; i >= 0 && i < count; i += inc) + { + double const len = points[i - inc].Length(points[i]); + distance += len; + time += len / speed; + + TurnsMapT::const_iterator j = m_turns.find(points[i]); + if (j != m_turns.end()) + { + vector const & vec = j->second; + for (size_t k = 0; k < vec.size(); ++k) + { + if (vec[k].m_pos.GetFeatureId() != pos.GetFeatureId() || + vec[k].m_pos.IsForward() == pos.IsForward()) + { + PossibleTurn t = vec[k]; + + t.m_metersCovered = distance; + t.m_secondsCovered = time; + turns.push_back(t); + } + } + } + } +} + +double RoadGraphMockSource::GetFeatureDistance(RoadPos const & p1, RoadPos const & p2) +{ + return 0.0; +} + +void RoadGraphMockSource::ReconstructPath(RoadPosVectorT const & positions, PointsVectorT & poly) +{ + +} + +} // namespace routing_test diff --git a/routing/routing_tests/road_graph_builder.hpp b/routing/routing_tests/road_graph_builder.hpp new file mode 100644 index 0000000000..fa9cbd7e60 --- /dev/null +++ b/routing/routing_tests/road_graph_builder.hpp @@ -0,0 +1,54 @@ +#pragma once + +#include "../road_graph.hpp" + +namespace routing_test +{ + +struct RoadInfo +{ + vector m_points; + double m_speedMS; + bool m_bothSides; + + RoadInfo() : m_speedMS(0.0), m_bothSides(false) {} + void Swap(RoadInfo & r); +}; + +inline void swap(RoadInfo & r1, RoadInfo & r2) +{ + r1.Swap(r2); +} + +class RoadGraphMockSource : public routing::IRoadGraph +{ + vector m_roads; + + struct LessPoint + { + bool operator() (m2::PointD const & p1, m2::PointD const & p2) const + { + double const eps = 1.0E-6; + + if (p1.x + eps < p2.x) + return true; + else if (p1.x > p2.x + eps) + return false; + + return (p1.y + eps < p2.y); + } + }; + + typedef vector TurnsVectorT; + typedef map TurnsMapT; + TurnsMapT m_turns; + +public: + void AddRoad(RoadInfo & rd); + + virtual void GetPossibleTurns(routing::RoadPos const & pos, TurnsVectorT & turns); + virtual double GetFeatureDistance(routing::RoadPos const & p1, routing::RoadPos const & p2); + virtual void ReconstructPath(RoadPosVectorT const & positions, PointsVectorT & poly); +}; + +} // namespace routing_test diff --git a/routing/routing_tests/road_graph_builder_test.cpp b/routing/routing_tests/road_graph_builder_test.cpp new file mode 100644 index 0000000000..bc8fc432fd --- /dev/null +++ b/routing/routing_tests/road_graph_builder_test.cpp @@ -0,0 +1,272 @@ +#include "../../testing/testing.hpp" + +#include "road_graph_builder.hpp" + +#include "../../base/logging.hpp" + + +using namespace routing; +using namespace routing_test; + + +namespace +{ + +class EqualPos +{ + RoadPos m_pos; + double m_distance; +public: + EqualPos(RoadPos const & pos, double d) : m_pos(pos), m_distance(d) {} + bool operator() (PossibleTurn const & r) const { return (r.m_pos == m_pos && r.m_metersCovered == m_distance); } +}; + +bool test_result(IRoadGraph::TurnsVectorT const & vec, RoadPos const & pos, double d) +{ + return find_if(vec.begin(), vec.end(), EqualPos(pos, d)) != vec.end(); +} + +} + + +UNIT_TEST(RG_Builder_Smoke) +{ + RoadGraphMockSource src; + + { + RoadInfo ri; + ri.m_bothSides = true; + ri.m_speedMS = 40; + ri.m_points.push_back(m2::PointD(0, 0)); + ri.m_points.push_back(m2::PointD(5, 0)); + ri.m_points.push_back(m2::PointD(10, 0)); + ri.m_points.push_back(m2::PointD(15, 0)); + ri.m_points.push_back(m2::PointD(20, 0)); + src.AddRoad(ri); + } + + { + RoadInfo ri; + ri.m_bothSides = true; + ri.m_speedMS = 40; + ri.m_points.push_back(m2::PointD(10, -10)); + ri.m_points.push_back(m2::PointD(10, -5)); + ri.m_points.push_back(m2::PointD(10, 0)); + ri.m_points.push_back(m2::PointD(10, 5)); + ri.m_points.push_back(m2::PointD(10, 10)); + src.AddRoad(ri); + } + + { + RoadInfo ri; + ri.m_bothSides = true; + ri.m_speedMS = 40; + ri.m_points.push_back(m2::PointD(15, -5)); + ri.m_points.push_back(m2::PointD(15, 0)); + src.AddRoad(ri); + } + + { + RoadInfo ri; + ri.m_bothSides = true; + ri.m_speedMS = 40; + ri.m_points.push_back(m2::PointD(20, 0)); + ri.m_points.push_back(m2::PointD(25, 5)); + ri.m_points.push_back(m2::PointD(15, 5)); + ri.m_points.push_back(m2::PointD(20, 0)); + src.AddRoad(ri); + } + + { + IRoadGraph::TurnsVectorT vec; + src.GetPossibleTurns(RoadPos(0, true, 1), vec); + TEST_EQUAL(vec.size(), 1, ()); + TEST_EQUAL(vec[0].m_pos, RoadPos(0, true, 0), ()); + } + + { + IRoadGraph::TurnsVectorT vec; + src.GetPossibleTurns(RoadPos(0, false, 1), vec); + TEST_EQUAL(vec.size(), 7, ()); + TEST(test_result(vec, RoadPos(0, false, 2), 5), ()); + TEST(test_result(vec, RoadPos(0, false, 3), 10), ()); + TEST(test_result(vec, RoadPos(1, true, 1), 5), ()); + TEST(test_result(vec, RoadPos(1, false, 2), 5), ()); + TEST(test_result(vec, RoadPos(2, true, 0), 10), ()); + TEST(test_result(vec, RoadPos(3, false, 0), 15), ()); + TEST(test_result(vec, RoadPos(3, true, 2), 15), ()); + } + + { + IRoadGraph::TurnsVectorT vec; + src.GetPossibleTurns(RoadPos(1, true, 0), vec); + TEST_EQUAL(vec.size(), 0, ()); + } + + { + IRoadGraph::TurnsVectorT vec; + src.GetPossibleTurns(RoadPos(1, false, 0), vec); + TEST_EQUAL(vec.size(), 5, ()); + TEST(test_result(vec, RoadPos(1, false, 1), 5), ()); + TEST(test_result(vec, RoadPos(1, false, 2), 10), ()); + TEST(test_result(vec, RoadPos(1, false, 3), 15), ()); + TEST(test_result(vec, RoadPos(0, true, 1), 10), ()); + TEST(test_result(vec, RoadPos(0, false, 2), 10), ()); + } + + RoadInfo ri0; + ri0.m_bothSides = true; + ri0.m_speedMS = 40; + ri0.m_points.push_back(m2::PointD(0, 0)); + ri0.m_points.push_back(m2::PointD(10, 0)); + ri0.m_points.push_back(m2::PointD(25, 0)); + ri0.m_points.push_back(m2::PointD(35, 0)); + ri0.m_points.push_back(m2::PointD(70, 0)); + ri0.m_points.push_back(m2::PointD(80, 0)); + + RoadInfo ri1; + ri1.m_bothSides = false; + ri1.m_speedMS = 20; + ri1.m_points.push_back(m2::PointD(0, 0)); + ri1.m_points.push_back(m2::PointD(5, 10)); + ri1.m_points.push_back(m2::PointD(5, 40)); + + RoadInfo ri2; + ri2.m_bothSides = false; + ri2.m_speedMS = 20; + ri2.m_points.push_back(m2::PointD(12, 25)); + ri2.m_points.push_back(m2::PointD(10, 10)); + ri2.m_points.push_back(m2::PointD(10, 0)); + + RoadInfo ri3; + ri3.m_bothSides = true; + ri3.m_speedMS = 30; + ri3.m_points.push_back(m2::PointD(5, 10)); + ri3.m_points.push_back(m2::PointD(10, 10)); + ri3.m_points.push_back(m2::PointD(70, 10)); + ri3.m_points.push_back(m2::PointD(80, 10)); + + RoadInfo ri4; + ri4.m_bothSides = true; + ri4.m_speedMS = 20; + ri4.m_points.push_back(m2::PointD(25, 0)); + ri4.m_points.push_back(m2::PointD(27, 25)); + + RoadInfo ri5; + ri5.m_bothSides = true; + ri5.m_speedMS = 30; + ri5.m_points.push_back(m2::PointD(35, 0)); + ri5.m_points.push_back(m2::PointD(37, 30)); + ri5.m_points.push_back(m2::PointD(70, 30)); + ri5.m_points.push_back(m2::PointD(80, 30)); + + RoadInfo ri6; + ri6.m_bothSides = true; + ri6.m_speedMS = 20; + ri6.m_points.push_back(m2::PointD(70, 0)); + ri6.m_points.push_back(m2::PointD(70, 10)); + ri6.m_points.push_back(m2::PointD(70, 30)); + + RoadInfo ri7; + ri7.m_bothSides = true; + ri7.m_speedMS = 20; + ri7.m_points.push_back(m2::PointD(39, 55)); + ri7.m_points.push_back(m2::PointD(80, 55)); + + RoadInfo ri8; + ri8.m_bothSides = false; + ri8.m_speedMS = 30; + ri8.m_points.push_back(m2::PointD(5, 40)); + ri8.m_points.push_back(m2::PointD(18, 55)); + ri8.m_points.push_back(m2::PointD(39, 55)); + ri8.m_points.push_back(m2::PointD(37, 30)); + ri8.m_points.push_back(m2::PointD(27, 25)); + ri8.m_points.push_back(m2::PointD(12, 25)); + ri8.m_points.push_back(m2::PointD(5, 40)); + + RoadGraphMockSource graph; + graph.AddRoad(ri0); + graph.AddRoad(ri1); + graph.AddRoad(ri2); + graph.AddRoad(ri3); + graph.AddRoad(ri4); + graph.AddRoad(ri5); + graph.AddRoad(ri6); + graph.AddRoad(ri7); + graph.AddRoad(ri8); + + { + IRoadGraph::TurnsVectorT vec; + graph.GetPossibleTurns(RoadPos(0, false, 0), vec); + TEST_EQUAL(vec.size(), 8, ()); + TEST(test_result(vec, RoadPos(0, false, 1), 10), ()); + TEST(test_result(vec, RoadPos(0, false, 2), 25), ()); + TEST(test_result(vec, RoadPos(0, false, 3), 35), ()); + TEST(test_result(vec, RoadPos(0, false, 4), 70), ()); + TEST(test_result(vec, RoadPos(2, true, 1), 10), ()); + TEST(test_result(vec, RoadPos(5, false, 0), 35), ()); + TEST(test_result(vec, RoadPos(6, false, 0), 70), ()); + TEST(test_result(vec, RoadPos(4, false, 0), 25), ()); + } + + { + IRoadGraph::TurnsVectorT vec; + graph.GetPossibleTurns(RoadPos(8, true, 0), vec); + double const d = m2::PointD(18, 55).Length(m2::PointD(5, 40)); + TEST_EQUAL(vec.size(), 2, ()); + TEST(test_result(vec, RoadPos(1, true, 1), d), ()); + TEST(test_result(vec, RoadPos(8, true, 5), d), ()); + } + + { + IRoadGraph::TurnsVectorT vec; + graph.GetPossibleTurns(RoadPos(2, true, 1), vec); + TEST_EQUAL(vec.size(), 4, ()); + TEST(test_result(vec, RoadPos(3, true, 0), 10), ()); + TEST(test_result(vec, RoadPos(3, false, 1), 10), ()); + TEST(test_result(vec, RoadPos(2, true, 0), 10), ()); + TEST(test_result(vec, RoadPos(8, true, 4), 10 + m2::PointD(10, 10).Length(m2::PointD(12, 25))), ()); + } + + { + IRoadGraph::TurnsVectorT vec; + graph.GetPossibleTurns(RoadPos(3, false, 0), vec); + TEST_EQUAL(vec.size(), 5, ()); + TEST(test_result(vec, RoadPos(3, false, 1), 5), ()); + TEST(test_result(vec, RoadPos(3, false, 2), 65), ()); + TEST(test_result(vec, RoadPos(2, true, 0), 5), ()); + TEST(test_result(vec, RoadPos(6, true, 0), 65), ()); + TEST(test_result(vec, RoadPos(6, false, 1), 65), ()); + } + + { + IRoadGraph::TurnsVectorT vec; + graph.GetPossibleTurns(RoadPos(7, false, 0), vec); + TEST_EQUAL(vec.size(), 0, ()); + } + + { + IRoadGraph::TurnsVectorT vec; + graph.GetPossibleTurns(RoadPos(7, true, 0), vec); + TEST_EQUAL(vec.size(), 1, ()); + TEST(test_result(vec, RoadPos(8, true, 1), 41), ()); + } + + { + IRoadGraph::TurnsVectorT vec; + graph.GetPossibleTurns(RoadPos(8, true, 3), vec); + double d = m2::PointD(27, 25).Length(m2::PointD(37, 30)); + TEST_EQUAL(vec.size(), 8, ()); + TEST(test_result(vec, RoadPos(8, true, 2), d), ()); + TEST(test_result(vec, RoadPos(5, true, 0), d), ()); + TEST(test_result(vec, RoadPos(5, false, 1), d), ()); + d += m2::PointD(37, 30).Length(m2::PointD(39, 55)); + TEST(test_result(vec, RoadPos(7, false, 0), d), ());; + TEST(test_result(vec, RoadPos(8, true, 1), d), ()); + d += m2::PointD(18, 55).Length(m2::PointD(39, 55)); + TEST(test_result(vec, RoadPos(8, true, 0), d), ()); + d += m2::PointD(5, 40).Length(m2::PointD(18, 55)); + TEST(test_result(vec, RoadPos(1, true, 1), d), ()); + TEST(test_result(vec, RoadPos(8, true, 5), d), ()); + } +} diff --git a/routing/routing_tests/routing_tests.pro b/routing/routing_tests/routing_tests.pro index b83bd4a617..c95892df70 100644 --- a/routing/routing_tests/routing_tests.pro +++ b/routing/routing_tests/routing_tests.pro @@ -13,8 +13,11 @@ include($$ROOT_DIR/common.pri) SOURCES += \ ../../testing/testingmain.cpp \ routing_smoke.cpp \ + road_graph_builder.cpp \ + road_graph_builder_test.cpp \ -HEADERS += +HEADERS += \ + road_graph_builder.hpp \