diff --git a/indexer/feature.hpp b/indexer/feature.hpp index 9e1a81884f..b74721f221 100644 --- a/indexer/feature.hpp +++ b/indexer/feature.hpp @@ -305,6 +305,12 @@ public: void SwapGeometry(FeatureType & r); + inline void SwapPoints(buffer_vector & points) const + { + ASSERT(m_bPointsParsed, ()); + return m_points.swap(points); + } + private: void ParseAll(int scale) const; diff --git a/map/framework.cpp b/map/framework.cpp index c40f800960..4f233ddd07 100644 --- a/map/framework.cpp +++ b/map/framework.cpp @@ -16,6 +16,7 @@ #include "routing/route.hpp" #include "routing/osrm_router.hpp" +#include "routing/astar_router.hpp" #include "search/search_engine.hpp" #include "search/result.hpp" @@ -275,10 +276,11 @@ Framework::Framework() m_storage.Init(bind(&Framework::UpdateAfterDownload, this, _1, _2)); LOG(LDEBUG, ("Storage initialized")); - m_routingSession.SetRouter(new OsrmRouter(&m_model.GetIndex(), [this] (m2::PointD const & pt) + m_routingSession.SetRouter(new AStarRouter(&m_model.GetIndex())); + /*m_routingSession.SetRouter(new OsrmRouter(&m_model.GetIndex(), [this] (m2::PointD const & pt) { return GetSearchEngine()->GetCountryFile(pt); - })); + }));*/ LOG(LDEBUG, ("Routing engine initialized")); LOG(LINFO, ("System languages:", languages::GetPreferred())); diff --git a/routing/astar_router.cpp b/routing/astar_router.cpp new file mode 100644 index 0000000000..c61ba067bd --- /dev/null +++ b/routing/astar_router.cpp @@ -0,0 +1,119 @@ +#include "astar_router.hpp" + +#include "../base/logging.hpp" +#include "../geometry/distance_on_sphere.hpp" +#include "../indexer/mercator.hpp" + + +namespace routing +{ + +static double const MAX_SPEED = 36.11111111111; // m/s + +void AStarRouter::SetFinalRoadPos(vector const & finalPos) +{ + ASSERT_GREATER(finalPos.size(), 0, ()); + for (size_t i = 0; i < finalPos.size(); ++i) + { + pair t = m_entries.insert(ShortestPath(finalPos[i])); + ASSERT(t.second, ()); + m_queue.push(PossiblePath(&*t.first)); + } + +} + +void AStarRouter::CalculateRoute(vector const & startPos, vector & route) +{ + route.clear(); + set startSet(startPos.begin(), startPos.end()); + set startFeatureSet; + for (vector::const_iterator it = startPos.begin(); it != startPos.end(); ++it) + startFeatureSet.insert(it->GetFeatureId()); + + while (!m_queue.empty()) + { + PossiblePath const & currPath = m_queue.top(); + double const fScore = currPath.m_fScore; + ShortestPath const * current = currPath.m_path; + current->RemovedFromOpenSet(); + m_queue.pop(); + + current->SetVisited(); + + bool const bStartFeature = startFeatureSet.find(current->GetPos().GetFeatureId()) != startFeatureSet.end(); + if (bStartFeature && startSet.find(current->GetPos()) != startSet.end()) + { + ReconstructRoute(current, route); + return; + } + + IRoadGraph::TurnsVectorT turns; + m_pRoadGraph->GetPossibleTurns(current->GetPos(), turns, bStartFeature); + for (IRoadGraph::TurnsVectorT::const_iterator it = turns.begin(); it != turns.end(); ++it) + { + PossibleTurn const & turn = *it; + pair t = m_entries.insert(ShortestPath(turn.m_pos, current)); + if (t.second || !t.first->IsVisited()) + { + ShortestPath const * nextPath = &*t.first; + ASSERT_GREATER(turn.m_speed, 0.0, ()); + double nextScoreG = fScore + turn.m_secondsCovered;//DistanceBetween(current, nextPath) / turn.m_speed; + + if (!nextPath->IsInOpenSet() || nextScoreG < nextPath->GetScoreG()) + { + nextPath->SetParent(current); + nextPath->SetScoreG(nextScoreG); + if (!nextPath->IsInOpenSet()) + { + m_queue.push(PossiblePath(nextPath, nextScoreG + HeuristicCostEstimate(nextPath, startSet))); + nextPath->AppendedIntoOpenSet(); + } + } + } + } + } + + LOG(LDEBUG, ("No route found!")); + // Route not found. +} + +double AStarRouter::HeuristicCostEstimate(AStarRouter::ShortestPath const * s1, set const & goals) +{ + /// @todo support of more than one goals + ASSERT_GREATER(goals.size(), 0, ()); + + m2::PointD const & b = (*goals.begin()).GetEndCoordinates(); + m2::PointD const & e = s1->GetPos().GetEndCoordinates(); + + return ms::DistanceOnEarth(MercatorBounds::YToLat(b.y), + MercatorBounds::XToLon(b.x), + MercatorBounds::YToLat(e.y), + MercatorBounds::XToLon(e.x)) / MAX_SPEED; +} + +double AStarRouter::DistanceBetween(ShortestPath const * p1, ShortestPath const * p2) +{ + + m2::PointD const & b = p1->GetPos().GetEndCoordinates(); + m2::PointD const & e = p2->GetPos().GetEndCoordinates(); + return ms::DistanceOnEarth(MercatorBounds::YToLat(b.y), + MercatorBounds::XToLon(b.x), + MercatorBounds::YToLat(e.y), + MercatorBounds::XToLon(e.x)); +} + +void AStarRouter::ReconstructRoute(ShortestPath const * path, vector & route) const +{ + ShortestPath const * p = path; + + while (p) + { + route.push_back(p->GetPos()); + if (p->GetParentEntry()) + p = p->GetParentEntry(); + else + p = 0; + } +} + +} diff --git a/routing/astar_router.hpp b/routing/astar_router.hpp new file mode 100644 index 0000000000..fa2724ae6c --- /dev/null +++ b/routing/astar_router.hpp @@ -0,0 +1,89 @@ +#pragma once + +#include "road_graph_router.hpp" +#include "../std/queue.hpp" + +namespace routing +{ + +class AStarRouter : public RoadGraphRouter +{ + typedef RoadGraphRouter BaseT; +public: + AStarRouter(Index const * pIndex = 0) : BaseT(pIndex) {} + + virtual string GetName() const { return "routeme"; } + virtual void SetFinalRoadPos(vector const & finalPos); + virtual void CalculateRoute(vector const & startPos, vector & route); + +protected: + + class ShortestPath + { + public: + explicit ShortestPath(RoadPos pos, ShortestPath const * pParentEntry = NULL, double gScore = 0.0) + : m_pos(pos), m_pParentEntry(pParentEntry), m_gScore(gScore), m_isInOpenSet(false), m_isVisited(false) {} + + bool operator < (ShortestPath const & e) const + { + return m_pos < e.m_pos; + } + + RoadPos GetPos() const { return m_pos; } + ShortestPath const * GetParentEntry() const { return m_pParentEntry; } + bool IsVisited() const { return m_isVisited; } + void SetParent(ShortestPath const * pParentEntry) const + { + ASSERT_NOT_EQUAL(this, pParentEntry, ()); + m_pParentEntry = pParentEntry; + } + + void SetVisited() const { m_isVisited = true; } + + void AppendedIntoOpenSet() const { m_isInOpenSet = true; } + void RemovedFromOpenSet() const { m_isInOpenSet = false; } + bool IsInOpenSet() const { return m_isInOpenSet; } + + inline void SetScoreG(double g) const { m_gScore = g; } + inline double GetScoreG() const { return m_gScore; } + + private: + RoadPos m_pos; + mutable ShortestPath const * m_pParentEntry; + mutable double m_gScore; + mutable bool m_isInOpenSet; + mutable bool m_isVisited; + }; + + struct PossiblePath + { + ShortestPath const * m_path; + double m_fScore; + + explicit PossiblePath(ShortestPath const * path, double fScore = 0.0) : m_path(path), m_fScore(fScore) {} + + bool operator < (PossiblePath const & p) const + { + if (m_fScore != p.m_fScore) + return m_fScore > p.m_fScore; + + if (m_path->GetScoreG() != p.m_path->GetScoreG()) + return m_path->GetScoreG() > p.m_path->GetScoreG(); + + return !(m_path < p.m_path); + } + }; + + double HeuristicCostEstimate(ShortestPath const * s1, set const & goals); + double DistanceBetween(ShortestPath const * p1, ShortestPath const * p2); + void ReconstructRoute(ShortestPath const * path, vector & route) const; + + typedef priority_queue PossiblePathQueueT; + PossiblePathQueueT m_queue; + + typedef set ShortPathSetT; + ShortPathSetT m_entries; +}; + + +} diff --git a/routing/dijkstra_router.cpp b/routing/dijkstra_router.cpp new file mode 100644 index 0000000000..7655bee8ae --- /dev/null +++ b/routing/dijkstra_router.cpp @@ -0,0 +1,93 @@ +#include "dijkstra_router.hpp" +#include "../base/assert.hpp" +#include "../base/logging.hpp" +#include "../std/set.hpp" + +namespace routing +{ + +DijkstraRouter::ShortestPath const * const DijkstraRouter::ShortestPath::FINAL_POS + = reinterpret_cast(1); + +void DijkstraRouter::SetFinalRoadPos(vector const & finalPos) +{ + m_entries = PathSet(); + m_queue = PossiblePathQueue(); + + for (size_t i = 0; i < finalPos.size(); ++i) + { + pair t = m_entries.insert(ShortestPath(finalPos[i])); + ASSERT(t.second, ()); + m_queue.push(PossiblePath(0.0, &*t.first, ShortestPath::FINAL_POS)); + } +} + +void DijkstraRouter::CalculateRoute(vector const & startPos, vector & route) +{ + route.clear(); + set startSet(startPos.begin(), startPos.end()); + set startFeatureSet; + for (vector::const_iterator it = startPos.begin(); it != startPos.end(); ++it) + startFeatureSet.insert(it->GetFeatureId()); + while (!m_queue.empty()) + { + double const cost = m_queue.top().m_cost; + ShortestPath const * const pEntry = m_queue.top().m_pEntry; + ShortestPath const * const pParentEntry = m_queue.top().m_pParentEntry; + m_queue.pop(); + + LOG(LDEBUG, ("Visiting", pEntry->GetPos(), "with cost", cost)); + + if (pEntry->IsVisited()) + { + LOG(LDEBUG, ("Already visited before, skipping.")); + continue; + } + pEntry->SetParentAndMarkVisited(pParentEntry); + +#ifdef DEBUG + if (pParentEntry == ShortestPath::FINAL_POS) + { + LOG(LDEBUG, ("Setting parent to", "FINAL_POS")); + } + else + { + LOG(LDEBUG, ("Setting parent to", pParentEntry->GetPos())); + } +#endif + + bool const bStartFeature = startFeatureSet.find(pEntry->GetPos().GetFeatureId()) != startFeatureSet.end(); + + if (bStartFeature && startSet.find(pEntry->GetPos()) != startSet.end()) + { + LOG(LDEBUG, ("Found result!")); + // Reached one of the starting points! + for (ShortestPath const * pE = pEntry; pE != ShortestPath::FINAL_POS; pE = pE->GetParentEntry()) + route.push_back(pE->GetPos()); + LOG(LDEBUG, (route)); + return; + } + + IRoadGraph::TurnsVectorT turns; + m_pRoadGraph->GetPossibleTurns(pEntry->GetPos(), turns, bStartFeature); + LOG(LDEBUG, ("Getting all turns", turns)); + for (IRoadGraph::TurnsVectorT::const_iterator iTurn = turns.begin(); iTurn != turns.end(); ++iTurn) + { + PossibleTurn const & turn = *iTurn; + pair t = m_entries.insert(ShortestPath(turn.m_pos)); + if (t.second || !t.first->IsVisited()) + { + // We've either never pushed or never poped this road before. + double nextCost = cost + turn.m_secondsCovered; + m_queue.push(PossiblePath(nextCost, &*t.first, pEntry)); + LOG(LDEBUG, ("Pushing", t.first->GetPos(), "with cost", nextCost)); + } + } + } + + LOG(LDEBUG, ("No route found!")); + // Route not found. +} + + +} // namespace routing diff --git a/routing/dijkstra_router.hpp b/routing/dijkstra_router.hpp new file mode 100644 index 0000000000..6312873e58 --- /dev/null +++ b/routing/dijkstra_router.hpp @@ -0,0 +1,92 @@ +#pragma once + +#include "road_graph_router.hpp" + +#include "../geometry/point2d.hpp" + +#include "../std/queue.hpp" +#include "../std/set.hpp" +#include "../std/vector.hpp" + + +namespace routing +{ + +class DijkstraRouter : public RoadGraphRouter +{ + typedef RoadGraphRouter BaseT; + +public: + DijkstraRouter(Index const * pIndex = 0) : BaseT(pIndex) {} + + virtual string GetName() const { return "routeme"; } + virtual void SetFinalRoadPos(vector const & finalPos); + virtual void CalculateRoute(vector const & startPos, vector & route); + +private: + class ShortestPath + { + public: + explicit ShortestPath(RoadPos pos, ShortestPath const * pParentEntry = NULL) + : m_pos(pos), m_pParentEntry(pParentEntry) {} + + bool operator < (ShortestPath const & e) const + { + return m_pos < e.m_pos; + } + + RoadPos GetPos() const { return m_pos; } + ShortestPath const * GetParentEntry() const { return m_pParentEntry; } + bool IsVisited() const { return m_pParentEntry != NULL; } + void SetParentAndMarkVisited(ShortestPath const * pParentEntry) const { m_pParentEntry = pParentEntry; } + + static ShortestPath const * const FINAL_POS; + + private: + RoadPos m_pos; + mutable ShortestPath const * m_pParentEntry; + }; + + struct PossiblePath + { + double m_cost; + ShortestPath const * m_pEntry; + ShortestPath const * m_pParentEntry; + + PossiblePath(double cost, ShortestPath const * pEntry, ShortestPath const * pParentEntry) + : m_cost(cost), m_pEntry(pEntry), m_pParentEntry(pParentEntry) {} + + bool operator < (PossiblePath const & p) const + { + if (m_cost != p.m_cost) + return m_cost > p.m_cost; + + // Comparisons below are used to make the algorithm stable. + if (m_pEntry->GetPos() < p.m_pEntry->GetPos()) + return true; + if (p.m_pEntry->GetPos() < m_pEntry->GetPos()) + return false; + if (!m_pParentEntry && p.m_pParentEntry) + return true; + if (!p.m_pParentEntry && m_pParentEntry) + return false; + if (!m_pParentEntry && !p.m_pParentEntry) + { + if (m_pParentEntry->GetPos() < p.m_pParentEntry->GetPos()) + return true; + if (p.m_pParentEntry->GetPos() < m_pParentEntry->GetPos()) + return false; + } + + return false; + } + }; + + typedef set PathSet; + PathSet m_entries; + + typedef priority_queue PossiblePathQueue; + PossiblePathQueue m_queue; +}; + +} // namespace routing diff --git a/routing/features_road_graph.cpp b/routing/features_road_graph.cpp new file mode 100644 index 0000000000..67c5ee32d4 --- /dev/null +++ b/routing/features_road_graph.cpp @@ -0,0 +1,331 @@ +#include "features_road_graph.hpp" +#include "route.hpp" +#include "vehicle_model.hpp" + +#include "../indexer/index.hpp" +#include "../indexer/classificator.hpp" +#include "../indexer/ftypes_matcher.hpp" + +#include "../geometry/distance_on_sphere.hpp" + +#include "../base/logging.hpp" + +namespace routing +{ + +uint32_t const FEATURE_CACHE_SIZE = 10; +uint32_t const READ_ROAD_SCALE = 13; +double const READ_CROSS_EPSILON = 1.0E-4; +double const DEFAULT_SPEED_MS = 15.0; + +uint32_t indexFound = 0; +uint32_t indexCheck = 0; +uint32_t crossFound = 0; +uint32_t crossCheck = 0; + + +FeaturesRoadGraph::FeaturesRoadGraph(Index const * pIndex, size_t mwmID) + : m_pIndex(pIndex), m_mwmID(mwmID), m_vehicleModel(new CarModel()), m_cache(FEATURE_CACHE_SIZE), + m_cacheMiss(0), m_cacheAccess(0) +{ +} + +uint32_t FeaturesRoadGraph::GetStreetReadScale() +{ + return READ_ROAD_SCALE; +} + +class CrossFeaturesLoader +{ + uint32_t m_featureID; + FeaturesRoadGraph & m_graph; + m2::PointD m_point; + IRoadGraph::TurnsVectorT & m_turns; + size_t m_count; + +public: + CrossFeaturesLoader(uint32_t fID, FeaturesRoadGraph & graph, + m2::PointD const & pt, IRoadGraph::TurnsVectorT & turns) + : m_featureID(fID), m_graph(graph), m_point(pt), m_turns(turns), m_count(0) + { + } + + size_t GetCount() const + { + return m_count; + } + + void operator()(FeatureType & ft) + { + FeatureID const fID = ft.GetID(); + if (m_featureID == fID.m_offset || fID.m_mwm != m_graph.GetMwmID()) + return; + + /// @todo remove overhead with type and speed checks (now speed loads in cache creation) + // check type to skip not line objects + feature::TypesHolder types(ft); + if (types.GetGeoType() != feature::GEOM_LINE) + return; + + // skip roads with null speed + double const speed = m_graph.GetSpeed(ft); + if (speed <= 0.0) + return; + + // load feature from cache + FeaturesRoadGraph::CachedFeature const & fc = m_graph.GetCachedFeature(fID.m_offset, ft, false); + ASSERT_EQUAL(speed, fc.m_speed, ()); + + size_t const count = fc.m_points.size(); + + PossibleTurn t; + t.m_speed = fc.m_speed; + t.m_startPoint = fc.m_points[0]; + t.m_endPoint = fc.m_points[count - 1]; + + for (size_t i = 0; i < count; ++i) + { + m2::PointD const & p = fc.m_points[i]; + + crossCheck++; + + /// @todo Is this a correct way to compare? + if (!m2::AlmostEqual(m_point, p)) + continue; + + crossFound++; + + if (i > 0) + { + ++m_count; + t.m_pos = RoadPos(fID.m_offset, true, i - 1, p); + m_turns.push_back(t); + } + + if (!fc.m_isOneway && (i < count - 1)) + { + ++m_count; + t.m_pos = RoadPos(fID.m_offset, false, i, p); + m_turns.push_back(t); + } + } + } +}; + +double CalcDistanceMeters(m2::PointD const & p1, m2::PointD const & p2) +{ + return ms::DistanceOnEarth(MercatorBounds::YToLat(p1.y), MercatorBounds::XToLon(p1.x), + MercatorBounds::YToLat(p2.y), MercatorBounds::XToLon(p2.x)); +} + +void FeaturesRoadGraph::LoadFeature(uint32_t id, FeatureType & ft) +{ + Index::FeaturesLoaderGuard loader(*m_pIndex, m_mwmID); + loader.GetFeature(id, ft); + ft.ParseGeometry(FeatureType::BEST_GEOMETRY); + + ASSERT_EQUAL(ft.GetFeatureType(), feature::GEOM_LINE, (id)); + ASSERT_GREATER(ft.GetPointsCount(), 1, (id)); +} + +void FeaturesRoadGraph::GetPossibleTurns(RoadPos const & pos, vector & turns, bool noOptimize /*= true*/) +{ + uint32_t const ftID = pos.GetFeatureId(); + FeatureType ft; + CachedFeature const fc = GetCachedFeature(ftID, 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; + + int startID = pos.GetPointId(); + ASSERT_GREATER(count, 1, ()); + ASSERT_LESS(startID, count, ()); + if (!isForward) + ++startID; + + PossibleTurn thisTurn; + thisTurn.m_speed = fc.m_speed; + thisTurn.m_startPoint = fc.m_points[0]; + thisTurn.m_endPoint = fc.m_points[count - 1]; + + double distance = 0.0; + double time = 0.0; + for (int i = startID; i >= 0 && i < count; i += inc) + { + 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; + } + } + + // Check cycle + if (m2::AlmostEqual(fc.m_points[0], fc.m_points[count - 1])) + { + /// @todo calculate distance and speed + if (isForward) + { + double distance = 0; + for (int i = pos.GetPointId(); i > 0; --i) + distance += CalcDistanceMeters(fc.m_points[i], fc.m_points[i - 1]); + + 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.GetPointId(); 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); + } + } +} + +void FeaturesRoadGraph::ReconstructPath(RoadPosVectorT const & positions, Route & route) +{ + LOG(LINFO, ("Cache miss: ", GetCacheMiss() * 100, " Access count: ", m_cacheAccess)); + LOG(LINFO, ("Index check: ", indexCheck, " Index found: ", indexFound, " (", 100.0 * (double)indexFound / (double)indexCheck, "%)")); + LOG(LINFO, ("Cross check: ", crossCheck, " Cross found: ", crossFound, " (", 100.0 * (double)crossFound / (double)crossCheck, "%)")); + + size_t count = positions.size(); + if (count < 2) + return; + + FeatureType ft1; + vector poly; + + // Initialize starting point. + LoadFeature(positions.back().GetFeatureId(), ft1); + + for (size_t i = count-1; i > 0; --i) + { + RoadPos const & pos1 = positions[i]; + RoadPos const & pos2 = positions[i-1]; + + FeatureType ft2; + + // Find next connection point + m2::PointD lastPt; + bool const diffIDs = pos1.GetFeatureId() != pos2.GetFeatureId(); + if (diffIDs) + { + LoadFeature(pos2.GetFeatureId(), ft2); + lastPt = ft2.GetPoint(pos2.GetPointId() + (pos2.IsForward() ? 1 : 0)); + } + else + lastPt = ft1.GetPoint(pos2.GetPointId() + (pos1.IsForward() ? 1 : 0)); + + // Accumulate points from start point id to pt. + int const inc = pos1.IsForward() ? -1 : 1; + int ptID = pos1.GetPointId() + (pos1.IsForward() ? 0 : 1); + m2::PointD pt; + do + { + pt = ft1.GetPoint(ptID); + poly.push_back(pt); + + LOG(LDEBUG, (pt, pos1.GetFeatureId(), ptID)); + + ptID += inc; + + } while (!m2::AlmostEqual(pt, lastPt)); + + // Assign current processing feature. + if (diffIDs) + ft1.SwapGeometry(ft2); + } + + route.SetGeometry(poly.rbegin(), poly.rend()); +} + +bool FeaturesRoadGraph::IsOneWay(FeatureType const & ft) const +{ + return m_vehicleModel->IsOneWay(ft); +} + +double FeaturesRoadGraph::GetSpeed(FeatureType const & ft) const +{ + return m_vehicleModel->GetSpeed(ft); +} + +FeaturesRoadGraph::CachedFeature const & FeaturesRoadGraph::GetCachedFeature( + uint32_t const ftId, FeatureType & ft, bool fullLoad) +{ + bool found = false; + CachedFeature & f = m_cache.Find(ftId, found); + + if (!found) + { + if (fullLoad) + LoadFeature(ftId, ft); + else + ft.ParseGeometry(FeatureType::BEST_GEOMETRY); + + f.m_isOneway = IsOneWay(ft); + f.m_speed = GetSpeed(ft); + ft.SwapPoints(f.m_points); + m_cacheMiss++; + } + m_cacheAccess++; + + return f; +} + +} // namespace routing diff --git a/routing/features_road_graph.hpp b/routing/features_road_graph.hpp new file mode 100644 index 0000000000..122f141c2c --- /dev/null +++ b/routing/features_road_graph.hpp @@ -0,0 +1,63 @@ +#pragma once + +#include "road_graph.hpp" + +#include "../indexer/feature_data.hpp" +#include "../geometry/point2d.hpp" +#include "../std/scoped_ptr.hpp" +#include "../std/vector.hpp" +#include "../base/cache.hpp" + + +class Index; +class FeatureType; + + +namespace routing +{ + +class IVehicleModel; + +class FeaturesRoadGraph : public IRoadGraph +{ + struct CachedFeature + { + CachedFeature() : m_speed(0), m_isOneway(false) {} + + buffer_vector m_points; + double m_speed; + bool m_isOneway; + }; + + CachedFeature const & GetCachedFeature(uint32_t const ftId, FeatureType & ft, bool fullLoad); + +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); + + static uint32_t GetStreetReadScale(); + + inline size_t GetMwmID() const { return m_mwmID; } + + double GetCacheMiss() const { return (double)m_cacheMiss / (double)m_cacheAccess; } + +private: + friend class CrossFeaturesLoader; + + bool IsOneWay(FeatureType const & ft) const; + double GetSpeed(FeatureType const & ft) const; + void LoadFeature(uint32_t id, FeatureType & ft); + +private: + Index const * m_pIndex; + size_t m_mwmID; + scoped_ptr m_vehicleModel; + my::Cache m_cache; + + uint32_t m_cacheMiss; + uint32_t m_cacheAccess; +}; + +} // namespace routing diff --git a/routing/road_graph.cpp b/routing/road_graph.cpp new file mode 100644 index 0000000000..f7a5a408b6 --- /dev/null +++ b/routing/road_graph.cpp @@ -0,0 +1,25 @@ +#include "road_graph.hpp" + +#include "../base/assert.hpp" + +#include "../std/sstream.hpp" + + +namespace routing +{ + +RoadPos::RoadPos(uint32_t featureId, bool bForward, size_t pointId, m2::PointD const & p) + : m_featureId((featureId << 1) + (bForward ? 1 : 0)), m_pointId(pointId), m_endCoordinates(p) +{ + ASSERT_LESS(featureId, 1U << 31, ()); + ASSERT_LESS(pointId, 0xFFFFFFFF, ()); +} + +string DebugPrint(RoadPos const & r) +{ + ostringstream ss; + ss << "{" << r.GetFeatureId() << ", " << r.IsForward() << ", " << r.m_pointId << "}"; + return ss.str(); +} + +} // namespace routing diff --git a/routing/road_graph.hpp b/routing/road_graph.hpp new file mode 100644 index 0000000000..a0826f275b --- /dev/null +++ b/routing/road_graph.hpp @@ -0,0 +1,85 @@ +#pragma once + +#include "../geometry/point2d.hpp" + +#include "../base/string_utils.hpp" + +#include "../std/vector.hpp" + + +namespace routing +{ + +class Route; + +/// Defines position on a feature with direction. +class RoadPos +{ +public: + RoadPos() : m_featureId(0), m_pointId(0), m_endCoordinates(0, 0) {} + RoadPos(uint32_t featureId, bool bForward, size_t pointId, m2::PointD const & p = m2::PointD::Zero()); + + uint32_t GetFeatureId() const { return m_featureId >> 1; } + 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); + } + + bool operator < (RoadPos const & r) const + { + if (m_featureId != r.m_featureId) + return m_featureId < r.m_featureId; + if (m_pointId != r.m_pointId) + return m_pointId < r.m_pointId; + return false; + } + + m2::PointD const & GetEndCoordinates() const { return m_endCoordinates; } + +private: + friend string DebugPrint(RoadPos const & r); + + uint32_t m_featureId; + uint32_t m_pointId; + m2::PointD m_endCoordinates; +}; + +/// The turn from the old to the new road. +struct PossibleTurn +{ + /// New road information. + RoadPos m_pos; + m2::PointD m_startPoint, m_endPoint; + double m_speed; + + /// 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: + typedef vector TurnsVectorT; + typedef vector RoadPosVectorT; + typedef vector PointsVectorT; + + 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; + /// Construct route by road positions (doesn't include first and last section). + virtual void ReconstructPath(RoadPosVectorT const & positions, Route & route) = 0; +}; + +} // namespace routing diff --git a/routing/road_graph_router.cpp b/routing/road_graph_router.cpp new file mode 100644 index 0000000000..1a5c9c390f --- /dev/null +++ b/routing/road_graph_router.cpp @@ -0,0 +1,149 @@ +#include "road_graph_router.hpp" +#include "features_road_graph.hpp" +#include "route.hpp" +#include "vehicle_model.hpp" + +#include "../indexer/feature.hpp" +#include "../indexer/ftypes_matcher.hpp" +#include "../indexer/index.hpp" + +#include "../geometry/distance.hpp" + +#include "../base/timer.hpp" + +#include "../base/logging.hpp" + + +namespace routing +{ + +class Point2RoadPos +{ + m2::PointD m_point; + double m_minDist; + m2::PointD m_posBeg; + m2::PointD m_posEnd; + size_t m_segIdx; + bool m_isOneway; + FeatureID m_fID; + IVehicleModel const * m_vehicleModel; + +public: + Point2RoadPos(m2::PointD const & pt, IVehicleModel const * vehicleModel) + : m_point(pt), m_minDist(numeric_limits::max()), m_vehicleModel(vehicleModel) + { + } + + void operator() (FeatureType const & ft) + { + if (ft.GetFeatureType() != feature::GEOM_LINE) + return; + + double const speed = m_vehicleModel->GetSpeed(ft); + if (speed <= 0.0) + return; + + ft.ParseGeometry(FeatureType::BEST_GEOMETRY); + + size_t const count = ft.GetPointsCount() - 1; + for (size_t i = 0; i < count; ++i) + { + m2::DistanceToLineSquare segDist; + m2::PointD const & p1 = ft.GetPoint(i); + m2::PointD const & p2 = ft.GetPoint(i + 1); + segDist.SetBounds(p1, p2); + double const d = segDist(m_point); + if (d < m_minDist) + { + m_minDist = d; + m_segIdx = i; + m_fID = ft.GetID(); + m_isOneway = m_vehicleModel->IsOneWay(ft); + m_posBeg = p1; + m_posEnd = p2; + } + } + } + + size_t GetMwmID() const + { + return m_fID.m_mwm; + } + + void GetResults(vector & res) + { + if (m_fID.IsValid()) + { + res.push_back(RoadPos(m_fID.m_offset, true, m_segIdx, m_posEnd)); + if (!m_isOneway) + res.push_back(RoadPos(m_fID.m_offset, false, m_segIdx, m_posBeg)); + } + } +}; + +RoadGraphRouter::~RoadGraphRouter() +{ +} + +RoadGraphRouter::RoadGraphRouter(Index const * pIndex) : + m_vehicleModel(new CarModel()), m_pIndex(pIndex) +{ + +} + +size_t RoadGraphRouter::GetRoadPos(m2::PointD const & pt, vector & pos) +{ + Point2RoadPos getter(pt, m_vehicleModel.get()); + m_pIndex->ForEachInRect(getter, + MercatorBounds::RectByCenterXYAndSizeInMeters(pt, 100.0), + FeaturesRoadGraph::GetStreetReadScale()); + + getter.GetResults(pos); + return getter.GetMwmID(); +} + +bool RoadGraphRouter::IsMyMWM(size_t mwmID) const +{ + return (m_pRoadGraph && dynamic_cast(m_pRoadGraph.get())->GetMwmID() == mwmID); +} + +void RoadGraphRouter::SetFinalPoint(m2::PointD const & finalPt) +{ + vector finalPos; + size_t const mwmID = GetRoadPos(finalPt, finalPos); + + if (!finalPos.empty()) + { + if (!IsMyMWM(mwmID)) + m_pRoadGraph.reset(new FeaturesRoadGraph(m_pIndex, mwmID)); + + SetFinalRoadPos(finalPos); + } +} + +//void RoadGraphRouter::CalculateRoute(m2::PointD const & startPt, ReadyCallback const & callback) +void RoadGraphRouter::CalculateRoute(m2::PointD const & startingPt, ReadyCallback const & callback, m2::PointD const & direction) +{ + if (!m_pRoadGraph) + return; + + vector startPos; + size_t const mwmID = GetRoadPos(startingPt, startPos); + + if (startPos.empty() || !IsMyMWM(mwmID)) + return; + + my::Timer timer; + timer.Reset(); + + vector routePos; + CalculateRoute(startPos, routePos); + + LOG(LINFO, ("Route calculation time: ", timer.ElapsedSeconds())); + + Route route(GetName()); + m_pRoadGraph->ReconstructPath(routePos, route); + callback(route, InternalError); +} + +} // namespace routing diff --git a/routing/road_graph_router.hpp b/routing/road_graph_router.hpp new file mode 100644 index 0000000000..c5089518e0 --- /dev/null +++ b/routing/road_graph_router.hpp @@ -0,0 +1,43 @@ +#pragma once + +#include "router.hpp" +#include "road_graph.hpp" + +#include "../geometry/point2d.hpp" + +#include "../std/vector.hpp" +#include "../std/scoped_ptr.hpp" + + +class Index; + +namespace routing +{ + +class IVehicleModel; + +class RoadGraphRouter : public IRouter +{ +public: + RoadGraphRouter(Index const * pIndex); + ~RoadGraphRouter(); + + virtual void SetFinalPoint(m2::PointD const & finalPt); + //virtual void CalculateRoute(m2::PointD const & startPt, ReadyCallback const & callback); + virtual void CalculateRoute(m2::PointD const & startingPt, ReadyCallback const & callback, m2::PointD const & direction = m2::PointD::Zero()); + + + virtual void SetFinalRoadPos(vector const & finalPos) = 0; + virtual void CalculateRoute(vector const & startPos, vector & route) = 0; + virtual void SetRoadGraph(IRoadGraph * pRoadGraph) { m_pRoadGraph.reset(pRoadGraph); } + +protected: + size_t GetRoadPos(m2::PointD const & pt, vector & pos); + bool IsMyMWM(size_t mwmID) const; + + scoped_ptr m_pRoadGraph; + scoped_ptr m_vehicleModel; + Index const * m_pIndex; +}; + +} // namespace routing diff --git a/routing/routing.pro b/routing/routing.pro index a675ca1cac..fe430a8f35 100644 --- a/routing/routing.pro +++ b/routing/routing.pro @@ -20,6 +20,11 @@ SOURCES += \ routing_mapping.cpp \ turns.cpp \ vehicle_model.cpp \ + astar_router.cpp \ + dijkstra_router.cpp \ + features_road_graph.cpp \ + road_graph_router.cpp \ + road_graph.cpp HEADERS += \ cross_routing_context.hpp \ @@ -32,3 +37,8 @@ HEADERS += \ routing_mapping.h \ turns.hpp \ vehicle_model.hpp \ + astar_router.hpp \ + dijkstra_router.hpp \ + features_road_graph.hpp \ + road_graph_router.hpp \ + road_graph.hpp diff --git a/routing/routing_tests/astar_router_test.cpp b/routing/routing_tests/astar_router_test.cpp new file mode 100644 index 0000000000..9c84a1b2df --- /dev/null +++ b/routing/routing_tests/astar_router_test.cpp @@ -0,0 +1,115 @@ +#include "../../testing/testing.hpp" + +#include "road_graph_builder.hpp" +#include "features_road_graph_test.hpp" + +#include "../astar_router.hpp" +#include "../features_road_graph.hpp" +#include "../route.hpp" + +#include "../../base/logging.hpp" +#include "../../base/macros.hpp" + + +using namespace routing; +using namespace routing_test; + + +// Use mock graph source. +template +void TestAStarRouterMock(RoadPos (&finalPos)[finalPosSize], + RoadPos (&startPos)[startPosSize], + RoadPos (&expected)[expectedSize]) +{ + RoadGraphMockSource * graph = new RoadGraphMockSource(); + InitRoadGraphMockSourceWithTest2(*graph); + + AStarRouter router; + router.SetRoadGraph(graph); + router.SetFinalRoadPos(vector(&finalPos[0], &finalPos[0] + ARRAY_SIZE(finalPos))); + vector result; + router.CalculateRoute(vector(&startPos[0], &startPos[0] + ARRAY_SIZE(startPos)), result); + TEST_EQUAL(vector(&expected[0], &expected[0] + ARRAY_SIZE(expected)), result, ()); +} + +// Use mwm features graph source. +template +void TestAStarRouterMWM(RoadPos (&finalPos)[finalPosSize], + RoadPos (&startPos)[startPosSize], + RoadPos (&expected)[expectedSize], + size_t pointsCount) +{ + FeatureRoadGraphTester tester("route_test2.mwm"); + + AStarRouter router; + router.SetRoadGraph(tester.GetGraph()); + + vector finalV(&finalPos[0], &finalPos[0] + ARRAY_SIZE(finalPos)); + tester.Name2FeatureID(finalV); + router.SetFinalRoadPos(finalV); + + vector startV(&startPos[0], &startPos[0] + ARRAY_SIZE(startPos)); + tester.Name2FeatureID(startV); + + vector result; + router.CalculateRoute(startV, result); + LOG(LDEBUG, (result)); + + Route route(router.GetName()); + tester.GetGraph()->ReconstructPath(result, route); + LOG(LDEBUG, (route)); + TEST_EQUAL(route.GetPoly().GetSize(), pointsCount, ()); + + tester.FeatureID2Name(result); + TEST_EQUAL(vector(&expected[0], &expected[0] + ARRAY_SIZE(expected)), result, ()); +} + + +UNIT_TEST(AStar_Router_City_Simple) +{ + // Uncomment to see debug log. + //my::g_LogLevel = LDEBUG; + + RoadPos finalPos[] = { RoadPos(7, true, 0) }; + RoadPos startPos[] = { RoadPos(1, true, 0) }; + + RoadPos expected1[] = { RoadPos(1, true, 0), + RoadPos(1, true, 1), + RoadPos(8, true, 0), + RoadPos(8, true, 1), + RoadPos(7, true, 0) }; + TestAStarRouterMock(finalPos, startPos, expected1); + + RoadPos expected2[] = { RoadPos(1, true, 0), + RoadPos(1, true, 1), + RoadPos(8, true, 1), + RoadPos(7, true, 0) }; + TestAStarRouterMWM(finalPos, startPos, expected2, 4); +} + + +UNIT_TEST(AStar_Router_City_ReallyFunnyLoop) +{ + // Uncomment to see debug log. + //my::g_LogLevel = LDEBUG; + + RoadPos finalPos[] = { RoadPos(1, true, 0) }; + RoadPos startPos[] = { RoadPos(1, true, 1) }; + RoadPos expected1[] = { RoadPos(1, true, 1), + RoadPos(8, true, 0), + RoadPos(8, true, 1), + RoadPos(8, true, 3), // algorithm skips 8,2 segment + RoadPos(4, false, 0), + RoadPos(0, false, 1), + RoadPos(0, false, 0), + RoadPos(1, true, 0) }; + TestAStarRouterMock(finalPos, startPos, expected1); + + RoadPos expected2[] = { RoadPos(1, true, 1), + RoadPos(8, true, 4), + RoadPos(2, true, 1), + RoadPos(0, false, 0), + RoadPos(1, true, 0) }; + TestAStarRouterMWM(finalPos, startPos, expected2, 9); + +} diff --git a/routing/routing_tests/dijkstra_router_test.cpp b/routing/routing_tests/dijkstra_router_test.cpp new file mode 100644 index 0000000000..b6ce916284 --- /dev/null +++ b/routing/routing_tests/dijkstra_router_test.cpp @@ -0,0 +1,115 @@ +#include "../../testing/testing.hpp" + +#include "road_graph_builder.hpp" +#include "features_road_graph_test.hpp" + +#include "../dijkstra_router.hpp" +#include "../features_road_graph.hpp" +#include "../route.hpp" + +#include "../../base/logging.hpp" +#include "../../base/macros.hpp" + + +using namespace routing; +using namespace routing_test; + + +// Use mock graph source. +template +void TestDijkstraRouterMock(RoadPos (&finalPos)[finalPosSize], + RoadPos (&startPos)[startPosSize], + RoadPos (&expected)[expectedSize]) +{ + RoadGraphMockSource * graph = new RoadGraphMockSource(); + InitRoadGraphMockSourceWithTest2(*graph); + + DijkstraRouter router; + router.SetRoadGraph(graph); + router.SetFinalRoadPos(vector(&finalPos[0], &finalPos[0] + ARRAY_SIZE(finalPos))); + vector result; + router.CalculateRoute(vector(&startPos[0], &startPos[0] + ARRAY_SIZE(startPos)), result); + TEST_EQUAL(vector(&expected[0], &expected[0] + ARRAY_SIZE(expected)), result, ()); +} + +// Use mwm features graph source. +template +void TestDijkstraRouterMWM(RoadPos (&finalPos)[finalPosSize], + RoadPos (&startPos)[startPosSize], + RoadPos (&expected)[expectedSize], + size_t pointsCount) +{ + FeatureRoadGraphTester tester("route_test2.mwm"); + + DijkstraRouter router; + router.SetRoadGraph(tester.GetGraph()); + + vector finalV(&finalPos[0], &finalPos[0] + ARRAY_SIZE(finalPos)); + tester.Name2FeatureID(finalV); + router.SetFinalRoadPos(finalV); + + vector startV(&startPos[0], &startPos[0] + ARRAY_SIZE(startPos)); + tester.Name2FeatureID(startV); + + vector result; + router.CalculateRoute(startV, result); + LOG(LDEBUG, (result)); + + Route route(router.GetName()); + tester.GetGraph()->ReconstructPath(result, route); + LOG(LDEBUG, (route)); + TEST_EQUAL(route.GetPoly().GetSize(), pointsCount, ()); + + tester.FeatureID2Name(result); + TEST_EQUAL(vector(&expected[0], &expected[0] + ARRAY_SIZE(expected)), result, ()); +} + + +UNIT_TEST(Dijkstra_Router_City_Simple) +{ + // Uncomment to see debug log. + //my::g_LogLevel = LDEBUG; + + RoadPos finalPos[] = { RoadPos(7, true, 0) }; + RoadPos startPos[] = { RoadPos(1, true, 0) }; + + RoadPos expected1[] = { RoadPos(1, true, 0), + RoadPos(1, true, 1), + RoadPos(8, true, 0), + RoadPos(8, true, 1), + RoadPos(7, true, 0) }; + TestDijkstraRouterMock(finalPos, startPos, expected1); + + RoadPos expected2[] = { RoadPos(1, true, 0), + RoadPos(1, true, 1), + RoadPos(8, true, 1), + RoadPos(7, true, 0) }; + TestDijkstraRouterMWM(finalPos, startPos, expected2, 4); +} + + +UNIT_TEST(Dijkstra_Router_City_ReallyFunnyLoop) +{ + // Uncomment to see debug log. + //my::g_LogLevel = LDEBUG; + + RoadPos finalPos[] = { RoadPos(1, true, 0) }; + RoadPos startPos[] = { RoadPos(1, true, 1) }; + RoadPos expected1[] = { RoadPos(1, true, 1), + RoadPos(8, true, 0), + RoadPos(8, true, 1), + RoadPos(8, true, 3), // algorithm skips 8,2 segment + RoadPos(4, false, 0), + RoadPos(0, false, 1), + RoadPos(0, false, 0), + RoadPos(1, true, 0) }; + TestDijkstraRouterMock(finalPos, startPos, expected1); + + RoadPos expected2[] = { RoadPos(1, true, 1), + RoadPos(8, true, 4), + RoadPos(2, true, 1), + RoadPos(0, false, 0), + RoadPos(1, true, 0) }; + TestDijkstraRouterMWM(finalPos, startPos, expected2, 9); + +} diff --git a/routing/routing_tests/features_road_graph_test.cpp b/routing/routing_tests/features_road_graph_test.cpp new file mode 100644 index 0000000000..8ee3c9def2 --- /dev/null +++ b/routing/routing_tests/features_road_graph_test.cpp @@ -0,0 +1,264 @@ +#include "../../testing/testing.hpp" + +#include "features_road_graph_test.hpp" + +#include "../route.hpp" + +#include "../../indexer/classificator_loader.hpp" +#include "../../indexer/feature.hpp" +#include "../../indexer/ftypes_matcher.hpp" + +#include "../../base/logging.hpp" + + +using namespace routing; + +namespace routing_test +{ + +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; + } +}; + +bool TestResult(IRoadGraph::TurnsVectorT const & vec, RoadPos const & pos, double d) +{ + return find_if(vec.begin(), vec.end(), EqualPos(pos, d)) != vec.end(); +} + + + +void Name2IdMapping::operator()(FeatureType const & ft) +{ + if (!ftypes::IsStreetChecker::Instance()(ft)) + return; + + string name; + VERIFY(ft.GetName(0, name), ()); + + m_name2Id[name] = ft.GetID().m_offset; + m_id2Name[ft.GetID().m_offset] = name; +} + +uint32_t Name2IdMapping::GetId(string const & name) +{ + ASSERT(m_name2Id.find(name) != m_name2Id.end(), ()); + return m_name2Id[name]; +} + +string const & Name2IdMapping::GetName(uint32_t id) +{ + ASSERT(m_id2Name.find(id) != m_id2Name.end(), ()); + return m_id2Name[id]; +} + + +FeatureRoadGraphTester::FeatureRoadGraphTester(string const & name) +{ + classificator::Load(); + + m2::RectD rect; + if (!m_index.Add(name, rect)) + { + LOG(LERROR, ("MWM file not found")); + return; + } + + m_graph = new FeaturesRoadGraph(&m_index, 0); + + m_index.ForEachInRect(m_mapping, MercatorBounds::FullRect(), m_graph->GetStreetReadScale()); +} + +void FeatureRoadGraphTester::FeatureID2Name(routing::RoadPos & pos) +{ + string const & name = m_mapping.GetName(pos.GetFeatureId()); + int id = 0; + VERIFY(strings::to_int(name, id), (name)); + + pos = RoadPos(static_cast(id), pos.IsForward(), pos.GetPointId()); +} + +void FeatureRoadGraphTester::FeatureID2Name(IRoadGraph::TurnsVectorT & vec) +{ + for (size_t i = 0; i < vec.size(); ++i) + FeatureID2Name(vec[i].m_pos); +} + +void FeatureRoadGraphTester::FeatureID2Name(vector & vec) +{ + for (size_t i = 0; i < vec.size(); ++i) + FeatureID2Name(vec[i]); +} + +void FeatureRoadGraphTester::Name2FeatureID(vector & vec) +{ + for (size_t i = 0; i < vec.size(); ++i) + vec[i] = RoadPos(m_mapping.GetId(strings::to_string(vec[i].GetFeatureId())), + vec[i].IsForward(), + vec[i].GetPointId()); +} + +void FeatureRoadGraphTester::GetPossibleTurns(RoadPos const & pos, IRoadGraph::TurnsVectorT & vec, bool noOptimize/* = true*/) +{ + m_graph->GetPossibleTurns(RoadPos(m_mapping.GetId(strings::to_string(pos.GetFeatureId())), + pos.IsForward(), pos.GetPointId()), vec, noOptimize); + FeatureID2Name(vec); +} + +template +void FeatureRoadGraphTester::ReconstructPath(routing::RoadPos (&arr)[N], vector & vec) +{ + vector positions(arr, arr + N); + Name2FeatureID(positions); + + Route route("dummy"); + m_graph->ReconstructPath(positions, route); + vec.assign(route.GetPoly().Begin(), route.GetPoly().End()); +} + +} + + +using namespace routing_test; + + +UNIT_TEST(FRG_TurnsTest_MWM1) +{ + FeatureRoadGraphTester tester("route_test1.mwm"); + + { + IRoadGraph::TurnsVectorT vec; + tester.GetPossibleTurns(RoadPos(0, true, 1), vec); + TEST_EQUAL(vec.size(), 1, ()); + TEST(TestResult(vec, RoadPos(0, true, 0), -1), ()); + } + + { + IRoadGraph::TurnsVectorT vec; + tester.GetPossibleTurns(RoadPos(0, false, 1), vec); + TEST_EQUAL(vec.size(), 7, ()); + TEST(TestResult(vec, RoadPos(0, false, 2), -1), ()); + TEST(TestResult(vec, RoadPos(0, false, 3), -1), ()); + TEST(TestResult(vec, RoadPos(1, true, 1), -1), ()); + TEST(TestResult(vec, RoadPos(1, false, 2), -1), ()); + TEST(TestResult(vec, RoadPos(2, true, 0), -1), ()); + TEST(TestResult(vec, RoadPos(3, false, 0), -1), ()); + TEST(TestResult(vec, RoadPos(3, true, 2), -1), ()); + } + + { + IRoadGraph::TurnsVectorT vec; + tester.GetPossibleTurns(RoadPos(1, true, 0), vec); + TEST_EQUAL(vec.size(), 0, ()); + } + + { + IRoadGraph::TurnsVectorT vec; + tester.GetPossibleTurns(RoadPos(1, false, 0), vec, false); + TEST_EQUAL(vec.size(), 3, ()); + TEST(TestResult(vec, RoadPos(1, false, 2), 10), ()); + TEST(TestResult(vec, RoadPos(0, true, 1), 10), ()); + TEST(TestResult(vec, RoadPos(0, false, 2), 10), ()); + } +} + +UNIT_TEST(FRG_TurnsTest_MWM2) +{ + FeatureRoadGraphTester tester("route_test2.mwm"); + + { + IRoadGraph::TurnsVectorT vec; + tester.GetPossibleTurns(RoadPos(0, false, 0), vec, false); + TEST_EQUAL(vec.size(), 8, ()); + TEST(TestResult(vec, RoadPos(0, false, 1), -1), ()); + TEST(TestResult(vec, RoadPos(0, false, 2), -1), ()); + TEST(TestResult(vec, RoadPos(0, false, 3), -1), ()); + TEST(TestResult(vec, RoadPos(0, false, 4), -1), ()); + TEST(TestResult(vec, RoadPos(2, true, 1), -1), ()); + TEST(TestResult(vec, RoadPos(5, false, 0), -1), ()); + TEST(TestResult(vec, RoadPos(6, false, 0), -1), ()); + TEST(TestResult(vec, RoadPos(4, false, 0), -1), ()); + } + + { + IRoadGraph::TurnsVectorT vec; + tester.GetPossibleTurns(RoadPos(8, true, 0), vec, false); + TEST_EQUAL(vec.size(), 2, ()); + TEST(TestResult(vec, RoadPos(1, true, 1), -1), ()); + TEST(TestResult(vec, RoadPos(8, true, 5), -1), ()); + } + + { + IRoadGraph::TurnsVectorT vec; + tester.GetPossibleTurns(RoadPos(2, true, 1), vec, false); + TEST_EQUAL(vec.size(), 4, ()); + TEST(TestResult(vec, RoadPos(3, true, 0), -1), ()); + TEST(TestResult(vec, RoadPos(3, false, 1), -1), ()); + TEST(TestResult(vec, RoadPos(2, true, 0), -1), ()); + TEST(TestResult(vec, RoadPos(8, true, 4), -1), ()); + } + + { + IRoadGraph::TurnsVectorT vec; + tester.GetPossibleTurns(RoadPos(3, false, 0), vec, false); + TEST_EQUAL(vec.size(), 5, ()); + TEST(TestResult(vec, RoadPos(3, false, 1), -1), ()); + TEST(TestResult(vec, RoadPos(3, false, 2), -1), ()); + TEST(TestResult(vec, RoadPos(2, true, 0), -1), ()); + TEST(TestResult(vec, RoadPos(6, true, 0), -1), ()); + TEST(TestResult(vec, RoadPos(6, false, 1), -1), ()); + } + + { + IRoadGraph::TurnsVectorT vec; + tester.GetPossibleTurns(RoadPos(7, false, 0), vec); + TEST_EQUAL(vec.size(), 0, ()); + } + + { + IRoadGraph::TurnsVectorT vec; + tester.GetPossibleTurns(RoadPos(7, true, 0), vec); + TEST_EQUAL(vec.size(), 1, ()); + TEST(TestResult(vec, RoadPos(8, true, 1), -1), ()); + } + + { + IRoadGraph::TurnsVectorT vec; + tester.GetPossibleTurns(RoadPos(8, true, 3), vec, false); + TEST_EQUAL(vec.size(), 7, ()); + TEST(TestResult(vec, RoadPos(8, true, 2), -1), ()); + TEST(TestResult(vec, RoadPos(5, true, 0), -1), ()); + TEST(TestResult(vec, RoadPos(5, false, 1), -1), ()); + TEST(TestResult(vec, RoadPos(7, false, 0), -1), ()); + TEST(TestResult(vec, RoadPos(8, true, 1), -1), ()); + TEST(TestResult(vec, RoadPos(1, true, 1), -1), ()); + TEST(TestResult(vec, RoadPos(8, true, 5), -1), ()); + } +} + +UNIT_TEST(FRG_ReconstructTest_MWM2) +{ + // Uncomment to see debug log. + //my::g_LogLevel = LDEBUG; + + FeatureRoadGraphTester tester("route_test2.mwm"); + + { + RoadPos arr[] = { + RoadPos(8, true, 4), + RoadPos(2, true, 0), + RoadPos(3, true, 2) + }; + + vector vec; + tester.ReconstructPath(arr, vec); + TEST_EQUAL(vec.size(), 3, ()); + } +} diff --git a/routing/routing_tests/features_road_graph_test.hpp b/routing/routing_tests/features_road_graph_test.hpp new file mode 100644 index 0000000000..d391596df7 --- /dev/null +++ b/routing/routing_tests/features_road_graph_test.hpp @@ -0,0 +1,48 @@ +#pragma once +#include "../features_road_graph.hpp" + +#include "../../indexer/index.hpp" + +#include "../../std/scoped_ptr.hpp" + + +namespace routing_test +{ + +class Name2IdMapping +{ + map m_name2Id; + map m_id2Name; + +public: + void operator()(FeatureType const & ft); + + uint32_t GetId(string const & name); + + string const & GetName(uint32_t id); +}; + +class FeatureRoadGraphTester +{ + routing::FeaturesRoadGraph * m_graph; + Name2IdMapping m_mapping; + Index m_index; + + void FeatureID2Name(routing::RoadPos & pos); + +public: + FeatureRoadGraphTester(string const & name); + + routing::FeaturesRoadGraph * GetGraph() { return m_graph; } + + void FeatureID2Name(routing::IRoadGraph::TurnsVectorT & vec); + void FeatureID2Name(vector & vec); + void Name2FeatureID(vector & vec); + + void GetPossibleTurns(routing::RoadPos const & pos, routing::IRoadGraph::TurnsVectorT & vec, bool noOptimize = true); + + template + void ReconstructPath(routing::RoadPos (&arr)[N], vector & vec); +}; + +}