diff options
author | Добрый Ээх <bukharaev@gmail.com> | 2016-11-16 21:08:50 +0300 |
---|---|---|
committer | Добрый Ээх <bukharaev@gmail.com> | 2016-11-24 17:53:04 +0300 |
commit | be1213029222d42a7a230f6c165f668a1d0ffd8b (patch) | |
tree | 0c1f7122955d546be7bd772520171a7c8ef26f87 /routing | |
parent | dd7599998201423b95a260ee6ff972bb310812c6 (diff) |
indexed astar router
Diffstat (limited to 'routing')
-rw-r--r-- | routing/astar_router.cpp | 193 | ||||
-rw-r--r-- | routing/astar_router.hpp | 47 | ||||
-rw-r--r-- | routing/car_router.cpp | 18 | ||||
-rw-r--r-- | routing/car_router.hpp | 5 | ||||
-rw-r--r-- | routing/directions_engine.cpp | 33 | ||||
-rw-r--r-- | routing/directions_engine.hpp | 6 | ||||
-rw-r--r-- | routing/features_road_graph.cpp | 8 | ||||
-rw-r--r-- | routing/features_road_graph.hpp | 6 | ||||
-rw-r--r-- | routing/fseg.hpp | 46 | ||||
-rw-r--r-- | routing/fseg_index.cpp | 39 | ||||
-rw-r--r-- | routing/fseg_index.hpp | 129 | ||||
-rw-r--r-- | routing/geometry.cpp | 122 | ||||
-rw-r--r-- | routing/geometry.hpp | 30 | ||||
-rw-r--r-- | routing/index_graph.cpp | 197 | ||||
-rw-r--r-- | routing/index_graph.hpp | 84 | ||||
-rw-r--r-- | routing/joint.hpp | 76 | ||||
-rw-r--r-- | routing/road_graph_router.cpp | 50 | ||||
-rw-r--r-- | routing/road_graph_router.hpp | 5 | ||||
-rw-r--r-- | routing/routing.pro | 10 | ||||
-rw-r--r-- | routing/routing_integration_tests/routing_test_tools.cpp | 1 | ||||
-rw-r--r-- | routing/routing_tests/index_graph_test.cpp | 219 | ||||
-rw-r--r-- | routing/routing_tests/routing_tests.pro | 1 |
22 files changed, 1249 insertions, 76 deletions
diff --git a/routing/astar_router.cpp b/routing/astar_router.cpp new file mode 100644 index 0000000000..51c04daf64 --- /dev/null +++ b/routing/astar_router.cpp @@ -0,0 +1,193 @@ +#include "astar_router.hpp" + +#include "routing/base/astar_algorithm.hpp" +#include "routing/base/astar_progress.hpp" +#include "routing/bicycle_directions.hpp" +#include "routing/bicycle_model.hpp" +#include "routing/car_model.hpp" +#include "routing/features_road_graph.hpp" +#include "routing/index_graph.hpp" +#include "routing/pedestrian_model.hpp" +#include "routing/route.hpp" +#include "routing/turns_generator.hpp" + +#include "indexer/feature_altitude.hpp" +#include "indexer/routing_section.hpp" + +#include "geometry/distance.hpp" +#include "geometry/point2d.hpp" + +namespace +{ +size_t constexpr kMaxRoadCandidates = 6; +float constexpr kProgressInterval = 2; + +using namespace routing; + +vector<Junction> ConvertToJunctions(IndexGraph const & graph, vector<JointId> const & joints) +{ + vector<FSegId> const & fsegs = graph.RedressRoute(joints); + + vector<Junction> junctions; + junctions.reserve(fsegs.size()); + + Geometry const & geometry = graph.GetGeometry(); + // TODO: Use real altitudes for pedestrian and bicycle routing. + for (FSegId const & fseg : fsegs) + junctions.emplace_back(geometry.GetPoint(fseg), feature::kDefaultAltitudeMeters); + + return junctions; +} +} // namespace + +namespace routing +{ +AStarRouter::AStarRouter(const char * name, Index const & index, + TCountryFileFn const & countryFileFn, + shared_ptr<VehicleModelFactory> vehicleModelFactory, + unique_ptr<IDirectionsEngine> directionsEngine) + : m_name(name) + , m_index(index) + , m_countryFileFn(countryFileFn) + , m_roadGraph( + make_unique<FeaturesRoadGraph>(index, IRoadGraph::Mode::ObeyOnewayTag, vehicleModelFactory)) + , m_vehicleModelFactory(vehicleModelFactory) + , m_directionsEngine(move(directionsEngine)) +{ + ASSERT(name, ()); + ASSERT(!m_name.empty(), ()); + ASSERT(m_vehicleModelFactory, ()); + ASSERT(m_directionsEngine, ()); +} + +IRouter::ResultCode AStarRouter::CalculateRoute(MwmSet::MwmId const & mwmId, + m2::PointD const & startPoint, + m2::PointD const & /* startDirection */, + m2::PointD const & finalPoint, + RouterDelegate const & delegate, Route & route) +{ + string const country = m_countryFileFn(startPoint); + + Edge startEdge = Edge::MakeFake({} /* startJunction */, {} /* endJunction */); + if (!FindClosestEdge(mwmId, startPoint, startEdge)) + return IRouter::StartPointNotFound; + + Edge finishEdge = Edge::MakeFake({} /* startJunction */, {} /* endJunction */); + if (!FindClosestEdge(mwmId, finalPoint, finishEdge)) + return IRouter::EndPointNotFound; + + FSegId const start(startEdge.GetFeatureId().m_index, startEdge.GetSegId()); + FSegId const finish(finishEdge.GetFeatureId().m_index, finishEdge.GetSegId()); + + IndexGraph graph( + CreateGeometry(m_index, mwmId, m_vehicleModelFactory->GetVehicleModelForCountry(country))); + + if (!LoadIndex(mwmId, graph)) + return IRouter::RouteFileNotExist; + + AStarProgress progress(0, 100); + + auto onVisitJunctionFn = [&delegate, &progress, &graph](JointId const & from, + JointId const & /*finish*/) { + m2::PointD const & point = graph.GetPoint(from); + auto const lastValue = progress.GetLastValue(); + auto const newValue = progress.GetProgressForDirectedAlgo(point); + if (newValue - lastValue > kProgressInterval) + delegate.OnProgress(newValue); + delegate.OnPointCheck(point); + }; + + AStarAlgorithm<IndexGraph> algorithm; + + RoutingResult<JointId> routingResult; + AStarAlgorithm<IndexGraph>::Result const resultCode = + algorithm.FindPathBidirectional(graph, graph.InsertJoint(start), graph.InsertJoint(finish), + routingResult, delegate, onVisitJunctionFn); + + switch (resultCode) + { + case AStarAlgorithm<IndexGraph>::Result::NoPath: return IRouter::RouteNotFound; + case AStarAlgorithm<IndexGraph>::Result::Cancelled: return IRouter::Cancelled; + case AStarAlgorithm<IndexGraph>::Result::OK: + vector<Junction> path = ConvertToJunctions(graph, routingResult.path); + ReconstructRoute(m_directionsEngine.get(), *m_roadGraph, delegate, path, route); + return IRouter::NoError; + } +} + +bool AStarRouter::FindClosestEdge(MwmSet::MwmId const & mwmId, m2::PointD const & point, + Edge & closestEdge) const +{ + vector<pair<Edge, Junction>> candidates; + m_roadGraph->FindClosestEdges(point, kMaxRoadCandidates, candidates); + + double minDistance = numeric_limits<double>::max(); + size_t minIndex = candidates.size(); + + for (size_t i = 0; i < candidates.size(); ++i) + { + Edge const & edge = candidates[i].first; + if (edge.GetFeatureId().m_mwmId != mwmId) + continue; + + m2::DistanceToLineSquare<m2::PointD> squareDistance; + squareDistance.SetBounds(edge.GetStartJunction().GetPoint(), edge.GetEndJunction().GetPoint()); + double const distance = squareDistance(point); + if (distance < minDistance) + { + minDistance = distance; + minIndex = i; + } + } + + if (minIndex < candidates.size()) + { + closestEdge = candidates[minIndex].first; + return true; + } + + return false; +} + +bool AStarRouter::LoadIndex(MwmSet::MwmId const & mwmId, IndexGraph & graph) +{ + MwmSet::MwmHandle mwmHandle = m_index.GetMwmHandleById(mwmId); + MwmValue const * mwmValue = mwmHandle.GetValue<MwmValue>(); + if (!mwmValue) + { + LOG(LERROR, ("mwmValue == null")); + return false; + } + + try + { + my::Timer timer; + FilesContainerR::TReader reader(mwmValue->m_cont.GetReader(ROUTING_FILE_TAG)); + ReaderSource<FilesContainerR::TReader> src(reader); + feature::RoutingSectionHeader header; + header.Deserialize(src); + graph.Deserialize(src); + LOG(LINFO, (ROUTING_FILE_TAG, "section loaded in ", timer.ElapsedSeconds(), "seconds")); + return true; + } + catch (Reader::OpenException const & e) + { + LOG(LERROR, ("File", mwmValue->GetCountryFileName(), "Error while reading", ROUTING_FILE_TAG, + "section.", e.Msg())); + return false; + } +} + +unique_ptr<AStarRouter> CreateCarAStarBidirectionalRouter(Index & index, + TCountryFileFn const & countryFileFn) +{ + shared_ptr<VehicleModelFactory> vehicleModelFactory = make_shared<CarModelFactory>(); + // @TODO Bicycle turn generation engine is used now. It's ok for the time being. + // But later a special car turn generation engine should be implemented. + unique_ptr<IDirectionsEngine> directionsEngine = make_unique<BicycleDirectionsEngine>(index); + unique_ptr<AStarRouter> router = + make_unique<AStarRouter>("astar-bidirectional-car", index, countryFileFn, + move(vehicleModelFactory), move(directionsEngine)); + return router; +} +} // namespace routing diff --git a/routing/astar_router.hpp b/routing/astar_router.hpp new file mode 100644 index 0000000000..58cbf4bfa3 --- /dev/null +++ b/routing/astar_router.hpp @@ -0,0 +1,47 @@ +#pragma once + +#include "routing/directions_engine.hpp" +#include "routing/road_graph.hpp" +#include "routing/router.hpp" +#include "routing/vehicle_model.hpp" + +#include "indexer/index.hpp" + +#include "std/shared_ptr.hpp" +#include "std/unique_ptr.hpp" +#include "std/vector.hpp" + +namespace routing +{ +class IndexGraph; + +class AStarRouter +{ +public: + AStarRouter(const char * name, Index const & index, TCountryFileFn const & countryFileFn, + shared_ptr<VehicleModelFactory> vehicleModelFactory, + unique_ptr<IDirectionsEngine> directionsEngine); + + string const & GetName() const { return m_name; } + + IRouter::ResultCode CalculateRoute(MwmSet::MwmId const & mwmId, m2::PointD const & startPoint, + m2::PointD const & startDirection, + m2::PointD const & finalPoint, RouterDelegate const & delegate, + Route & route); + +private: + bool FindClosestEdge(MwmSet::MwmId const & mwmId, m2::PointD const & point, + Edge & closestEdge) const; + bool LoadIndex(MwmSet::MwmId const & mwmId, IndexGraph & graph); + + string m_name; + Index const & m_index; + TCountryFileFn m_countryFileFn; + unique_ptr<IRoadGraph> m_roadGraph; + shared_ptr<VehicleModelFactory> m_vehicleModelFactory; + unique_ptr<IDirectionsEngine> m_directionsEngine; +}; + +unique_ptr<AStarRouter> CreateCarAStarBidirectionalRouter(Index & index, + TCountryFileFn const & countryFileFn); +} // namespace routing diff --git a/routing/car_router.cpp b/routing/car_router.cpp index 4ddd1260ba..08c13421a1 100644 --- a/routing/car_router.cpp +++ b/routing/car_router.cpp @@ -244,8 +244,8 @@ bool CarRouter::CheckRoutingAbility(m2::PointD const & startPoint, m2::PointD co } CarRouter::CarRouter(Index & index, TCountryFileFn const & countryFileFn, - unique_ptr<IRouter> router) - : m_index(index), m_indexManager(countryFileFn, index), m_router(move(router)) + unique_ptr<AStarRouter> localRouter) + : m_index(index), m_indexManager(countryFileFn, index), m_router(move(localRouter)) { } @@ -256,11 +256,6 @@ void CarRouter::ClearState() m_cachedTargets.clear(); m_cachedTargetPoint = m2::PointD::Zero(); m_indexManager.Clear(); - - if (m_router) - m_router->ClearState(); - else - LOG(LERROR, ("m_router is not initialized.")); } bool CarRouter::FindRouteMSMT(TFeatureGraphNodeVec const & sources, @@ -570,10 +565,11 @@ IRouter::ResultCode CarRouter::FindSingleRouteDispatcher(FeatureGraphNode const LOG(LERROR, ("m_router is not initialized.")); return IRouter::InternalError; } - LOG(LINFO, (m_router->GetName(), "route from", MercatorBounds::ToLatLon(source.segmentPoint), "to", - MercatorBounds::ToLatLon(target.segmentPoint))); - result = m_router->CalculateRoute(source.segmentPoint, m2::PointD(0, 0) /* direction */, - target.segmentPoint, delegate, mwmRoute); + LOG(LINFO, (m_router->GetName(), "route from", MercatorBounds::ToLatLon(source.segmentPoint), + "to", MercatorBounds::ToLatLon(target.segmentPoint))); + result = m_router->CalculateRoute(source.mwmId, source.segmentPoint, + m2::PointD(0, 0) /* direction */, target.segmentPoint, + delegate, mwmRoute); } else { diff --git a/routing/car_router.hpp b/routing/car_router.hpp index 60e4f23946..4f974869d5 100644 --- a/routing/car_router.hpp +++ b/routing/car_router.hpp @@ -1,5 +1,6 @@ #pragma once +#include "routing/astar_router.hpp" #include "routing/osrm_data_facade.hpp" #include "routing/osrm_engine.hpp" #include "routing/route.hpp" @@ -34,7 +35,7 @@ public: typedef vector<double> GeomTurnCandidateT; CarRouter(Index & index, TCountryFileFn const & countryFileFn, - unique_ptr<IRouter> roadGraphRouter); + unique_ptr<AStarRouter> localRouter); virtual string GetName() const override; @@ -114,6 +115,6 @@ private: RoutingIndexManager m_indexManager; - unique_ptr<IRouter> m_router; + unique_ptr<AStarRouter> m_router; }; } // namespace routing diff --git a/routing/directions_engine.cpp b/routing/directions_engine.cpp index 53d5ed602a..4d52e92ba6 100644 --- a/routing/directions_engine.cpp +++ b/routing/directions_engine.cpp @@ -87,4 +87,37 @@ bool IDirectionsEngine::ReconstructPath(IRoadGraph const & graph, vector<Junctio return true; } + +void ReconstructRoute(IDirectionsEngine * engine, IRoadGraph const & graph, + my::Cancellable const & cancellable, vector<Junction> & path, Route & route) +{ + CHECK(!path.empty(), ("Can't reconstruct route from an empty list of positions.")); + + // By some reason there're two adjacent positions on a road with + // the same end-points. This could happen, for example, when + // direction on a road was changed. But it doesn't matter since + // this code reconstructs only geometry of a route. + path.erase(unique(path.begin(), path.end()), path.end()); + if (path.size() == 1) + path.emplace_back(path.back()); + + Route::TTimes times; + Route::TTurns turnsDir; + vector<Junction> junctions; + // @TODO(bykoianko) streetNames is not filled in Generate(). It should be done. + Route::TStreets streetNames; + if (engine) + engine->Generate(graph, path, times, turnsDir, junctions, cancellable); + + vector<m2::PointD> routeGeometry; + JunctionsToPoints(junctions, routeGeometry); + feature::TAltitudes altitudes; + JunctionsToAltitudes(junctions, altitudes); + + route.SetGeometry(routeGeometry.begin(), routeGeometry.end()); + route.SetSectionTimes(move(times)); + route.SetTurnInstructions(move(turnsDir)); + route.SetStreetNames(move(streetNames)); + route.SetAltitudes(move(altitudes)); +} } // namespace routing diff --git a/routing/directions_engine.hpp b/routing/directions_engine.hpp index 54ec9cdd4e..c82b1c5d59 100644 --- a/routing/directions_engine.hpp +++ b/routing/directions_engine.hpp @@ -9,7 +9,6 @@ namespace routing { - class IDirectionsEngine { public: @@ -17,8 +16,7 @@ public: virtual void Generate(IRoadGraph const & graph, vector<Junction> const & path, Route::TTimes & times, Route::TTurns & turns, - vector<Junction> & routeGeometry, - my::Cancellable const & cancellable) = 0; + vector<Junction> & routeGeometry, my::Cancellable const & cancellable) = 0; protected: /// \brief constructs route based on |graph| and |path|. Fills |routeEdges| with the route. @@ -31,4 +29,6 @@ protected: Route::TTimes & times) const; }; +void ReconstructRoute(IDirectionsEngine * engine, IRoadGraph const & graph, + my::Cancellable const & cancellable, vector<Junction> & path, Route & route); } // namespace routing diff --git a/routing/features_road_graph.cpp b/routing/features_road_graph.cpp index 79c61a0a58..78f1bfc33c 100644 --- a/routing/features_road_graph.cpp +++ b/routing/features_road_graph.cpp @@ -47,8 +47,8 @@ FeaturesRoadGraph::Value::Value(MwmSet::MwmHandle handle) : m_mwmHandle(move(han } FeaturesRoadGraph::CrossCountryVehicleModel::CrossCountryVehicleModel( - unique_ptr<VehicleModelFactory> vehicleModelFactory) - : m_vehicleModelFactory(move(vehicleModelFactory)) + shared_ptr<VehicleModelFactory> vehicleModelFactory) + : m_vehicleModelFactory(vehicleModelFactory) , m_maxSpeedKMPH(m_vehicleModelFactory->GetVehicleModel()->GetMaxSpeed()) { } @@ -108,8 +108,8 @@ void FeaturesRoadGraph::RoadInfoCache::Clear() m_cache.clear(); } FeaturesRoadGraph::FeaturesRoadGraph(Index const & index, IRoadGraph::Mode mode, - unique_ptr<VehicleModelFactory> vehicleModelFactory) - : m_index(index), m_mode(mode), m_vehicleModel(move(vehicleModelFactory)) + shared_ptr<VehicleModelFactory> vehicleModelFactory) + : m_index(index), m_mode(mode), m_vehicleModel(vehicleModelFactory) { } diff --git a/routing/features_road_graph.hpp b/routing/features_road_graph.hpp index 9e21db0d87..7657e7dc6f 100644 --- a/routing/features_road_graph.hpp +++ b/routing/features_road_graph.hpp @@ -26,7 +26,7 @@ private: class CrossCountryVehicleModel : public IVehicleModel { public: - CrossCountryVehicleModel(unique_ptr<VehicleModelFactory> vehicleModelFactory); + CrossCountryVehicleModel(shared_ptr<VehicleModelFactory> vehicleModelFactory); // IVehicleModel overrides: double GetSpeed(FeatureType const & f) const override; @@ -39,7 +39,7 @@ private: private: IVehicleModel * GetVehicleModel(FeatureID const & featureId) const; - unique_ptr<VehicleModelFactory> const m_vehicleModelFactory; + shared_ptr<VehicleModelFactory> const m_vehicleModelFactory; double const m_maxSpeedKMPH; mutable map<MwmSet::MwmId, shared_ptr<IVehicleModel>> m_cache; @@ -59,7 +59,7 @@ private: public: FeaturesRoadGraph(Index const & index, IRoadGraph::Mode mode, - unique_ptr<VehicleModelFactory> vehicleModelFactory); + shared_ptr<VehicleModelFactory> vehicleModelFactory); static uint32_t GetStreetReadScale(); diff --git a/routing/fseg.hpp b/routing/fseg.hpp new file mode 100644 index 0000000000..c868d13248 --- /dev/null +++ b/routing/fseg.hpp @@ -0,0 +1,46 @@ +#pragma once + +#include "coding/reader.hpp" +#include "coding/write_to_sink.hpp" + +#include "std/cstdint.hpp" +#include "std/limits.hpp" + +namespace routing +{ +class FSegId final +{ + static uint32_t constexpr kInvalidFeatureId = numeric_limits<uint32_t>::max(); + +public: + static FSegId MakeInvalid() { return FSegId(kInvalidFeatureId, 0); } + + FSegId() : m_featureId(kInvalidFeatureId), m_segId(0) {} + + FSegId(uint32_t featureId, uint32_t segId) : m_featureId(featureId), m_segId(segId) {} + + uint32_t GetFeatureId() const { return m_featureId; } + + uint32_t GetSegId() const { return m_segId; } + + bool IsValid() const { return m_featureId != kInvalidFeatureId; } + + template <class TSink> + void Serialize(TSink & sink) const + { + WriteToSink(sink, m_featureId); + WriteToSink(sink, m_segId); + } + + template <class TSource> + void Deserialize(TSource & src) + { + m_featureId = ReadPrimitiveFromSource<decltype(m_featureId)>(src); + m_segId = ReadPrimitiveFromSource<decltype(m_segId)>(src); + } + +private: + uint32_t m_featureId; + uint32_t m_segId; +}; +} // namespace routing diff --git a/routing/fseg_index.cpp b/routing/fseg_index.cpp new file mode 100644 index 0000000000..e42ff4d68a --- /dev/null +++ b/routing/fseg_index.cpp @@ -0,0 +1,39 @@ +#include "fseg_index.hpp" + +#include "std/utility.hpp" + +namespace routing +{ +void FSegIndex::Export(vector<Joint> const & joints) +{ + for (JointId jointId = 0; jointId < joints.size(); ++jointId) + { + Joint const & joint = joints[jointId]; + for (uint32_t i = 0; i < joint.GetSize(); ++i) + { + FSegId const & entry = joint.GetEntry(i); + RoadJointIds & roadJoints = m_roads[entry.GetFeatureId()]; + roadJoints.AddJoint(entry.GetSegId(), jointId); + } + } +} + +pair<JointId, uint32_t> FSegIndex::FindNeigbor(FSegId fseg, bool forward) const +{ + auto const it = m_roads.find(fseg.GetFeatureId()); + if (it == m_roads.cend()) + return make_pair(kInvalidJointId, 0); + + RoadJointIds const & joints = it->second; + int32_t const step = forward ? 1 : -1; + + for (uint32_t segId = fseg.GetSegId() + step; segId < joints.GetSize(); segId += step) + { + JointId const jointId = joints.GetJointId(segId); + if (jointId != kInvalidJointId) + return make_pair(jointId, segId); + } + + return make_pair(kInvalidJointId, 0); +} +} // namespace routing diff --git a/routing/fseg_index.hpp b/routing/fseg_index.hpp new file mode 100644 index 0000000000..4655dc282d --- /dev/null +++ b/routing/fseg_index.hpp @@ -0,0 +1,129 @@ +#pragma once + +#include "routing/joint.hpp" + +#include "coding/reader.hpp" +#include "coding/write_to_sink.hpp" + +#include "std/cstdint.hpp" +#include "std/unordered_map.hpp" +#include "std/vector.hpp" + +namespace routing +{ +class RoadJointIds final +{ +public: + JointId GetJointId(uint32_t segId) const + { + if (segId < m_jointIds.size()) + return m_jointIds[segId]; + + return kInvalidJointId; + } + + void AddJoint(uint32_t segId, JointId jointId) + { + if (segId >= m_jointIds.size()) + m_jointIds.insert(m_jointIds.end(), segId + 1 - m_jointIds.size(), kInvalidJointId); + + ASSERT_EQUAL(m_jointIds[segId], kInvalidJointId, ()); + m_jointIds[segId] = jointId; + } + + template <typename F> + void ForEachJoint(F && f) const + { + for (uint32_t segId = 0; segId < m_jointIds.size(); ++segId) + { + JointId const jointId = m_jointIds[segId]; + if (jointId != kInvalidJointId) + f(segId, jointId); + } + } + + size_t GetSize() const { return m_jointIds.size(); } + + template <class TSink> + void Serialize(TSink & sink) const + { + WriteToSink(sink, static_cast<JointId>(m_jointIds.size())); + for (JointId jointId : m_jointIds) + WriteToSink(sink, jointId); + } + + template <class TSource> + void Deserialize(TSource & src) + { + JointId const jointsSize = ReadPrimitiveFromSource<JointId>(src); + m_jointIds.reserve(jointsSize); + for (JointId i = 0; i < jointsSize; ++i) + { + JointId const jointId = ReadPrimitiveFromSource<JointId>(src); + m_jointIds.emplace_back(jointId); + } + } + +private: + vector<JointId> m_jointIds; +}; + +class FSegIndex final +{ +public: + void Export(vector<Joint> const & joints); + + void AddJoint(FSegId fseg, JointId jointId) + { + RoadJointIds & road = m_roads[fseg.GetFeatureId()]; + road.AddJoint(fseg.GetSegId(), jointId); + } + + pair<JointId, uint32_t> FindNeigbor(FSegId fseg, bool forward) const; + + template <class TSink> + void Serialize(TSink & sink) const + { + WriteToSink(sink, static_cast<uint32_t>(m_roads.size())); + for (auto it = m_roads.begin(); it != m_roads.end(); ++it) + { + uint32_t const featureId = it->first; + WriteToSink(sink, featureId); + it->second.Serialize(sink); + } + } + + template <class TSource> + void Deserialize(TSource & src) + { + size_t const roadsSize = static_cast<size_t>(ReadPrimitiveFromSource<uint32_t>(src)); + for (size_t i = 0; i < roadsSize; ++i) + { + uint32_t featureId = ReadPrimitiveFromSource<decltype(featureId)>(src); + m_roads[featureId].Deserialize(src); + } + } + + uint32_t GetSize() const { return m_roads.size(); } + + JointId GetJointId(FSegId fseg) const + { + auto const it = m_roads.find(fseg.GetFeatureId()); + if (it == m_roads.end()) + return kInvalidJointId; + + return it->second.GetJointId(fseg.GetSegId()); + } + + template <typename F> + void ForEachRoad(F && f) const + { + for (auto it = m_roads.begin(); it != m_roads.end(); ++it) + f(it->first, it->second); + } + +private: + // map from feature id to RoadJointIds. + unordered_map<uint32_t, RoadJointIds> m_roads; +}; +} // namespace routing diff --git a/routing/geometry.cpp b/routing/geometry.cpp new file mode 100644 index 0000000000..c7431f44bd --- /dev/null +++ b/routing/geometry.cpp @@ -0,0 +1,122 @@ +#include "geometry.hpp" + +#include "geometry/mercator.hpp" + +#include "base/assert.hpp" + +namespace +{ +using namespace routing; + +double constexpr kMPH2MPS = 1000.0 / (60 * 60); + +inline double TimeBetweenSec(m2::PointD const & from, m2::PointD const & to, double speedMPS) +{ + ASSERT_GREATER(speedMPS, 0.0, ()); + + double const distanceM = MercatorBounds::DistanceOnEarth(from, to); + return distanceM / speedMPS; +} + +class GeometryImpl final : public Geometry +{ +public: + GeometryImpl(Index const & index, MwmSet::MwmId const & mwmId, + shared_ptr<IVehicleModel> vehicleModel); + + // Geometry overrides: + bool IsRoad(uint32_t featureId) const override; + bool IsOneWay(uint32_t featureId) const override; + m2::PointD const & GetPoint(FSegId fseg) const override; + double CalcEdgesWeight(uint32_t featureId, uint32_t pointStart, + uint32_t pointFinish) const override; + double CalcHeuristic(FSegId from, FSegId to) const override; + +private: + FeatureType const & GetFeature(uint32_t featureId) const; + FeatureType const & LoadFeature(uint32_t featureId) const; + + Index const & m_index; + MwmSet::MwmId const m_mwmId; + shared_ptr<IVehicleModel> m_vehicleModel; + double const m_maxSpeedMPS; + mutable map<uint32_t, FeatureType> m_features; +}; + +GeometryImpl::GeometryImpl(Index const & index, MwmSet::MwmId const & mwmId, + shared_ptr<IVehicleModel> vehicleModel) + : m_index(index) + , m_mwmId(mwmId) + , m_vehicleModel(vehicleModel) + , m_maxSpeedMPS(vehicleModel->GetMaxSpeed() * kMPH2MPS) +{ +} + +bool GeometryImpl::IsRoad(uint32_t featureId) const +{ + return m_vehicleModel->IsRoad(GetFeature(featureId)); +} + +bool GeometryImpl::IsOneWay(uint32_t featureId) const +{ + return m_vehicleModel->IsOneWay(GetFeature(featureId)); +} + +m2::PointD const & GeometryImpl::GetPoint(FSegId fseg) const +{ + return GetFeature(fseg.GetFeatureId()).GetPoint(fseg.GetSegId()); +} + +double GeometryImpl::CalcEdgesWeight(uint32_t featureId, uint32_t pointFrom, uint32_t pointTo) const +{ + uint32_t const start = min(pointFrom, pointTo); + uint32_t const finish = max(pointFrom, pointTo); + FeatureType const & feature = GetFeature(featureId); + + ASSERT_LESS(finish, feature.GetPointsCount(), ()); + + double result = 0.0; + double const speedMPS = m_vehicleModel->GetSpeed(feature) * kMPH2MPS; + for (uint32_t i = start; i < finish; ++i) + { + result += TimeBetweenSec(feature.GetPoint(i), feature.GetPoint(i + 1), speedMPS); + } + + return result; +} + +double GeometryImpl::CalcHeuristic(FSegId from, FSegId to) const +{ + return TimeBetweenSec(GetPoint(from), GetPoint(to), m_maxSpeedMPS); +} + +FeatureType const & GeometryImpl::GetFeature(uint32_t featureId) const +{ + auto it = m_features.find(featureId); + if (it != m_features.end()) + return it->second; + + return LoadFeature(featureId); +} + +FeatureType const & GeometryImpl::LoadFeature(uint32_t featureId) const +{ + Index::FeaturesLoaderGuard guard(m_index, m_mwmId); + FeatureType & feature = m_features[featureId]; + bool const isFound = guard.GetFeatureByIndex(featureId, feature); + ASSERT(isFound, ("Feature", featureId, "not found")); + if (isFound) + feature.ParseGeometry(FeatureType::BEST_GEOMETRY); + + return feature; +} +} // namespace + +namespace routing +{ +unique_ptr<Geometry> CreateGeometry(Index const & index, MwmSet::MwmId const & mwmId, + shared_ptr<IVehicleModel> vehicleModel) +{ + return make_unique<GeometryImpl>(index, mwmId, vehicleModel); +} +} // namespace routing diff --git a/routing/geometry.hpp b/routing/geometry.hpp new file mode 100644 index 0000000000..b4426c3fb8 --- /dev/null +++ b/routing/geometry.hpp @@ -0,0 +1,30 @@ +#pragma once + +#include "routing/fseg.hpp" +#include "routing/vehicle_model.hpp" + +#include "indexer/index.hpp" + +#include "geometry/point2d.hpp" + +#include "std/cstdint.hpp" +#include "std/shared_ptr.hpp" +#include "std/unique_ptr.hpp" + +namespace routing +{ +class Geometry +{ +public: + virtual ~Geometry() = default; + virtual bool IsRoad(uint32_t featureId) const = 0; + virtual bool IsOneWay(uint32_t featureId) const = 0; + virtual m2::PointD const & GetPoint(FSegId fseg) const = 0; + virtual double CalcEdgesWeight(uint32_t featureId, uint32_t pointFrom, + uint32_t pointTo) const = 0; + virtual double CalcHeuristic(FSegId from, FSegId to) const = 0; +}; + +unique_ptr<Geometry> CreateGeometry(Index const & index, MwmSet::MwmId const & mwmId, + shared_ptr<IVehicleModel> vehicleModel); +} // namespace routing diff --git a/routing/index_graph.cpp b/routing/index_graph.cpp new file mode 100644 index 0000000000..36acd31717 --- /dev/null +++ b/routing/index_graph.cpp @@ -0,0 +1,197 @@ +#include "index_graph.hpp" + +#include "base/assert.hpp" + +namespace routing +{ +IndexGraph::IndexGraph(unique_ptr<Geometry> geometry) : m_geometry(move(geometry)) {} + +void IndexGraph::GetOutgoingEdgesList(JointId jointId, vector<TEdgeType> & edges) const +{ + GetEdgesList(jointId, edges, true); +} + +void IndexGraph::GetIngoingEdgesList(JointId jointId, vector<TEdgeType> & edges) const +{ + GetEdgesList(jointId, edges, false); +} + +double IndexGraph::HeuristicCostEstimate(JointId jointFrom, JointId jointTo) const +{ + return m_geometry->CalcHeuristic(GetFSeg(jointFrom), GetFSeg(jointTo)); +} + +m2::PointD const & IndexGraph::GetPoint(JointId jointId) const +{ + return m_geometry->GetPoint(GetFSeg(jointId)); +} + +void IndexGraph::Export(vector<Joint> const & joints) +{ + m_fsegIndex.Export(joints); + BuildJoints(joints.size()); +} + +JointId IndexGraph::InsertJoint(FSegId fseg) +{ + JointId const existId = m_fsegIndex.GetJointId(fseg); + if (existId != kInvalidJointId) + return existId; + + JointId const jointId = m_jointOffsets.size(); + m_jointOffsets.emplace_back(JointOffset(m_fsegs.size(), m_fsegs.size() + 1)); + m_fsegs.emplace_back(fseg); + m_fsegIndex.AddJoint(fseg, jointId); + return jointId; +} + +vector<FSegId> IndexGraph::RedressRoute(vector<JointId> const & route) const +{ + vector<FSegId> fsegs; + if (route.size() < 2) + { + if (route.size() == 1) + fsegs.emplace_back(GetFSeg(route[0])); + return fsegs; + } + + fsegs.reserve(route.size() * 2); + + for (size_t i = 0; i < route.size() - 1; ++i) + { + JointId const prevJoint = route[i]; + JointId const nextJoint = route[i + 1]; + + auto const & pair = FindSharedFeature(prevJoint, nextJoint); + if (!pair.first.IsValid()) + { + fsegs.clear(); + return fsegs; + } + + if (i == 0) + fsegs.push_back(pair.first); + + uint32_t const featureId = pair.first.GetFeatureId(); + uint32_t const segFrom = pair.first.GetSegId(); + uint32_t const segTo = pair.second.GetSegId(); + + ASSERT_NOT_EQUAL(segFrom, segTo, ()); + + if (segFrom < segTo) + { + for (uint32_t seg = segFrom + 1; seg < segTo; ++seg) + fsegs.push_back({featureId, seg}); + } + else + { + for (uint32_t seg = segFrom - 1; seg > segTo; --seg) + fsegs.push_back({featureId, seg}); + } + + fsegs.push_back(pair.second); + } + + return fsegs; +} + +FSegId IndexGraph::GetFSeg(JointId jointId) const +{ + return m_fsegs[GetJointOffset(jointId).Begin()]; +} + +inline JointOffset const & IndexGraph::GetJointOffset(JointId jointId) const +{ + ASSERT_LESS(jointId, m_jointOffsets.size(), ("JointId out of bounds")); + return m_jointOffsets[jointId]; +} + +pair<FSegId, FSegId> IndexGraph::FindSharedFeature(JointId jointId0, JointId jointId1) const +{ + JointOffset const & offset0 = GetJointOffset(jointId0); + JointOffset const & offset1 = GetJointOffset(jointId1); + + for (size_t i = offset0.Begin(); i < offset0.End(); ++i) + { + FSegId const & fseg0 = m_fsegs[i]; + for (size_t j = offset1.Begin(); j < offset1.End(); ++j) + { + FSegId const & fseg1 = m_fsegs[j]; + if (fseg0.GetFeatureId() == fseg1.GetFeatureId()) + return make_pair(fseg0, fseg1); + } + } + + LOG(LERROR, ("Can't find shared feature for joints", jointId0, jointId1)); + return make_pair(FSegId::MakeInvalid(), FSegId::MakeInvalid()); +} + +void IndexGraph::BuildJoints(uint32_t jointsAmount) +{ + // +2 is reserved space for start and finish + m_jointOffsets.reserve(jointsAmount + 2); + m_jointOffsets.resize(jointsAmount, {0, 0}); + + m_fsegIndex.ForEachRoad([this](uint32_t featureId, RoadJointIds const & road) { + road.ForEachJoint([this](uint32_t segId, uint32_t jointId) { + ASSERT_LESS(jointId, m_jointOffsets.size(), ()); + m_jointOffsets[jointId].IncSize(); + }); + }); + + uint32_t offset = 0; + for (size_t i = 0; i < m_jointOffsets.size(); ++i) + { + JointOffset & jointOffset = m_jointOffsets[i]; + uint32_t const size = jointOffset.Size(); + ASSERT_GREATER(size, 0, ()); + + jointOffset.Assign(offset); + offset += size; + } + + // +2 is reserved space for start and finish + m_fsegs.reserve(offset + 2); + m_fsegs.resize(offset); + + m_fsegIndex.ForEachRoad([this](uint32_t featureId, RoadJointIds const & road) { + road.ForEachJoint([this, featureId](uint32_t segId, uint32_t jointId) { + ASSERT_LESS(jointId, m_jointOffsets.size(), ()); + JointOffset & jointOffset = m_jointOffsets[jointId]; + m_fsegs[jointOffset.End()] = {featureId, segId}; + jointOffset.IncSize(); + }); + }); +} + +void IndexGraph::AddNeigborEdge(vector<TEdgeType> & edges, FSegId fseg, bool forward) const +{ + pair<JointId, uint32_t> const & pair = m_fsegIndex.FindNeigbor(fseg, forward); + if (pair.first != kInvalidJointId) + { + double const distance = + m_geometry->CalcEdgesWeight(fseg.GetFeatureId(), fseg.GetSegId(), pair.second); + edges.push_back({pair.first, distance}); + } +} + +void IndexGraph::GetEdgesList(JointId jointId, vector<TEdgeType> & edges, bool forward) const +{ + edges.clear(); + + JointOffset const & offset = GetJointOffset(jointId); + for (size_t i = offset.Begin(); i < offset.End(); ++i) + { + FSegId const & fseg = m_fsegs[i]; + if (!m_geometry->IsRoad(fseg.GetFeatureId())) + continue; + + bool const twoWay = !m_geometry->IsOneWay(fseg.GetFeatureId()); + if (!forward || twoWay) + AddNeigborEdge(edges, fseg, false); + + if (forward || twoWay) + AddNeigborEdge(edges, fseg, true); + } +} +} // namespace routing diff --git a/routing/index_graph.hpp b/routing/index_graph.hpp new file mode 100644 index 0000000000..4e94af5ab5 --- /dev/null +++ b/routing/index_graph.hpp @@ -0,0 +1,84 @@ +#pragma once + +#include "routing/fseg.hpp" +#include "routing/fseg_index.hpp" +#include "routing/geometry.hpp" +#include "routing/joint.hpp" + +#include "coding/reader.hpp" +#include "coding/write_to_sink.hpp" + +#include "geometry/point2d.hpp" + +#include "std/cstdint.hpp" + +namespace routing +{ +class JointEdge final +{ +public: + JointEdge(JointId target, double weight) : target(target), weight(weight) {} + JointId GetTarget() const { return target; } + double GetWeight() const { return weight; } +private: + JointId target; + double weight; +}; + +class IndexGraph final +{ +public: + using TVertexType = JointId; + using TEdgeType = JointEdge; + + IndexGraph() = default; + explicit IndexGraph(unique_ptr<Geometry> geometry); + + // TGraph overloads: + void GetOutgoingEdgesList(TVertexType vertex, vector<TEdgeType> & edges) const; + void GetIngoingEdgesList(TVertexType vertex, vector<TEdgeType> & edges) const; + double HeuristicCostEstimate(TVertexType from, TVertexType to) const; + + // Access methods. + Geometry const & GetGeometry() const { return *m_geometry; } + m2::PointD const & GetPoint(JointId jointId) const; + size_t GetRoadsAmount() const { return m_fsegIndex.GetSize(); } + size_t GetJointsAmount() const { return m_jointOffsets.size(); } + size_t GetFSegsAmount() const { return m_fsegs.size(); } + void Export(vector<Joint> const & joints); + JointId InsertJoint(FSegId fseg); + vector<FSegId> RedressRoute(vector<JointId> const & route) const; + + template <class TSink> + void Serialize(TSink & sink) const + { + WriteToSink(sink, static_cast<JointId>(GetJointsAmount())); + m_fsegIndex.Serialize(sink); + } + + template <class TSource> + void Deserialize(TSource & src) + { + uint32_t const jointsSize = ReadPrimitiveFromSource<uint32_t>(src); + m_fsegIndex.Deserialize(src); + BuildJoints(jointsSize); + } + +private: + // Access methods. + FSegId GetFSeg(JointId jointId) const; + JointOffset const & GetJointOffset(JointId jointId) const; + pair<FSegId, FSegId> FindSharedFeature(JointId jointId0, JointId jointId1) const; + + void BuildJoints(uint32_t jointsAmount); + + // Edge methods. + void AddNeigborEdge(vector<TEdgeType> & edges, FSegId fseg, bool forward) const; + void GetEdgesList(JointId jointId, vector<TEdgeType> & edges, bool forward) const; + + unique_ptr<Geometry> m_geometry; + FSegIndex m_fsegIndex; + vector<JointOffset> m_jointOffsets; + vector<FSegId> m_fsegs; +}; +} // namespace routing diff --git a/routing/joint.hpp b/routing/joint.hpp new file mode 100644 index 0000000000..cc645ce34c --- /dev/null +++ b/routing/joint.hpp @@ -0,0 +1,76 @@ +#pragma once + +#include "routing/fseg.hpp" + +#include "coding/reader.hpp" +#include "coding/write_to_sink.hpp" + +#include "std/cstdint.hpp" +#include "std/limits.hpp" +#include "std/vector.hpp" + +namespace routing +{ +using JointId = uint32_t; +JointId constexpr kInvalidJointId = numeric_limits<JointId>::max(); + +class Joint final +{ +public: + void AddEntry(FSegId entry) { m_entries.emplace_back(entry); } + + size_t GetSize() const { return m_entries.size(); } + + FSegId const & GetEntry(size_t i) const { return m_entries[i]; } + + template <class TSink> + void Serialize(TSink & sink) const + { + WriteToSink(sink, static_cast<uint32_t>(m_entries.size())); + for (auto const & entry : m_entries) + { + entry.Serialize(sink); + } + } + + template <class TSource> + void Deserialize(TSource & src) + { + size_t const size = static_cast<size_t>(ReadPrimitiveFromSource<uint32_t>(src)); + m_entries.resize(size); + for (size_t i = 0; i < size; ++i) + { + m_entries[i].Deserialize(src); + } + } + +private: + vector<FSegId> m_entries; +}; + +class JointOffset final +{ +public: + JointOffset() : m_begin(0), m_end(0) {} + + JointOffset(uint32_t begin, uint32_t end) : m_begin(begin), m_end(end) {} + + uint32_t Begin() const { return m_begin; } + + uint32_t End() const { return m_end; } + + uint32_t Size() const { return m_end - m_begin; } + + void Assign(uint32_t offset) + { + m_begin = offset; + m_end = offset; + } + + void IncSize() { ++m_end; } + +private: + uint32_t m_begin; + uint32_t m_end; +}; +} // namespace routing diff --git a/routing/road_graph_router.cpp b/routing/road_graph_router.cpp index 06409c83ef..a4a8db19d7 100644 --- a/routing/road_graph_router.cpp +++ b/routing/road_graph_router.cpp @@ -1,5 +1,6 @@ #include "routing/road_graph_router.hpp" +#include "routing/astar_router.hpp" #include "routing/bicycle_directions.hpp" #include "routing/bicycle_model.hpp" #include "routing/car_model.hpp" @@ -225,7 +226,7 @@ IRouter::ResultCode RoadGraphRouter::CalculateRoute(m2::PointD const & startPoin ASSERT_EQUAL(result.path.front(), startPos, ()); ASSERT_EQUAL(result.path.back(), finalPos, ()); ASSERT_GREATER(result.distance, 0., ()); - ReconstructRoute(move(result.path), route, delegate); + ReconstructRoute(m_directionsEngine.get(), *m_roadGraph, delegate, result.path, route); } m_roadGraph->ResetFakes(); @@ -246,39 +247,6 @@ IRouter::ResultCode RoadGraphRouter::CalculateRoute(m2::PointD const & startPoin return IRouter::RouteNotFound; } -void RoadGraphRouter::ReconstructRoute(vector<Junction> && path, Route & route, - my::Cancellable const & cancellable) const -{ - CHECK(!path.empty(), ("Can't reconstruct route from an empty list of positions.")); - - // By some reason there're two adjacent positions on a road with - // the same end-points. This could happen, for example, when - // direction on a road was changed. But it doesn't matter since - // this code reconstructs only geometry of a route. - path.erase(unique(path.begin(), path.end()), path.end()); - if (path.size() == 1) - path.emplace_back(path.back()); - - Route::TTimes times; - Route::TTurns turnsDir; - vector<Junction> junctions; - // @TODO(bykoianko) streetNames is not filled in Generate(). It should be done. - Route::TStreets streetNames; - if (m_directionsEngine) - m_directionsEngine->Generate(*m_roadGraph, path, times, turnsDir, junctions, cancellable); - - vector<m2::PointD> routeGeometry; - JunctionsToPoints(junctions, routeGeometry); - feature::TAltitudes altitudes; - JunctionsToAltitudes(junctions, altitudes); - - route.SetGeometry(routeGeometry.begin(), routeGeometry.end()); - route.SetSectionTimes(move(times)); - route.SetTurnInstructions(move(turnsDir)); - route.SetStreetNames(move(streetNames)); - route.SetAltitudes(move(altitudes)); -} - unique_ptr<IRouter> CreatePedestrianAStarRouter(Index & index, TCountryFileFn const & countryFileFn) { unique_ptr<VehicleModelFactory> vehicleModelFactory(new PedestrianModelFactory()); @@ -311,18 +279,4 @@ unique_ptr<IRouter> CreateBicycleAStarBidirectionalRouter(Index & index, TCountr move(vehicleModelFactory), move(algorithm), move(directionsEngine))); return router; } - -unique_ptr<IRouter> CreateCarAStarBidirectionalRouter(Index & index, - TCountryFileFn const & countryFileFn) -{ - unique_ptr<VehicleModelFactory> vehicleModelFactory = make_unique<CarModelFactory>(); - unique_ptr<IRoutingAlgorithm> algorithm = make_unique<AStarBidirectionalRoutingAlgorithm>(); - // @TODO Bicycle turn generation engine is used now. It's ok for the time being. - // But later a special car turn generation engine should be implemented. - unique_ptr<IDirectionsEngine> directionsEngine = make_unique<BicycleDirectionsEngine>(index); - unique_ptr<IRouter> router = make_unique<RoadGraphRouter>( - "astar-bidirectional-car", index, countryFileFn, IRoadGraph::Mode::ObeyOnewayTag, - move(vehicleModelFactory), move(algorithm), move(directionsEngine)); - return router; -} } // namespace routing diff --git a/routing/road_graph_router.hpp b/routing/road_graph_router.hpp index 14405a38d6..b7a9f00cd9 100644 --- a/routing/road_graph_router.hpp +++ b/routing/road_graph_router.hpp @@ -37,9 +37,6 @@ public: Route & route) override; private: - void ReconstructRoute(vector<Junction> && junctions, Route & route, - my::Cancellable const & cancellable) const; - /// Checks existance and add absent maps to route. /// Returns true if map exists bool CheckMapExistence(m2::PointD const & point, Route & route) const; @@ -55,6 +52,4 @@ private: unique_ptr<IRouter> CreatePedestrianAStarRouter(Index & index, TCountryFileFn const & countryFileFn); unique_ptr<IRouter> CreatePedestrianAStarBidirectionalRouter(Index & index, TCountryFileFn const & countryFileFn); unique_ptr<IRouter> CreateBicycleAStarBidirectionalRouter(Index & index, TCountryFileFn const & countryFileFn); -unique_ptr<IRouter> CreateCarAStarBidirectionalRouter(Index & index, - TCountryFileFn const & countryFileFn); } // namespace routing diff --git a/routing/routing.pro b/routing/routing.pro index e4792d64ec..f354cefcb3 100644 --- a/routing/routing.pro +++ b/routing/routing.pro @@ -13,6 +13,7 @@ INCLUDEPATH += $$ROOT_DIR/3party/jansson/src \ $$ROOT_DIR/3party/osrm/osrm-backend/third_party SOURCES += \ + astar_router.cpp \ async_router.cpp \ base/followed_polyline.cpp \ bicycle_directions.cpp \ @@ -24,6 +25,9 @@ SOURCES += \ cross_routing_context.cpp \ directions_engine.cpp \ features_road_graph.cpp \ + fseg_index.cpp \ + geometry.cpp \ + index_graph.cpp \ nearest_edge_finder.cpp \ online_absent_fetcher.cpp \ online_cross_fetcher.cpp \ @@ -53,6 +57,7 @@ SOURCES += \ HEADERS += \ + astar_router.hpp \ async_router.hpp \ base/astar_algorithm.hpp \ base/followed_polyline.hpp \ @@ -65,6 +70,11 @@ HEADERS += \ cross_routing_context.hpp \ directions_engine.hpp \ features_road_graph.hpp \ + fseg.hpp \ + fseg_index.hpp \ + geometry.hpp \ + index_graph.hpp \ + joint.hpp \ loaded_path_segment.hpp \ nearest_edge_finder.hpp \ online_absent_fetcher.hpp \ diff --git a/routing/routing_integration_tests/routing_test_tools.cpp b/routing/routing_integration_tests/routing_test_tools.cpp index 378d8513ac..53bfb3a551 100644 --- a/routing/routing_integration_tests/routing_test_tools.cpp +++ b/routing/routing_integration_tests/routing_test_tools.cpp @@ -7,6 +7,7 @@ #include "geometry/distance_on_sphere.hpp" #include "geometry/latlon.hpp" +#include "routing/astar_router.hpp" #include "routing/online_absent_fetcher.hpp" #include "routing/online_cross_fetcher.hpp" #include "routing/road_graph_router.hpp" diff --git a/routing/routing_tests/index_graph_test.cpp b/routing/routing_tests/index_graph_test.cpp new file mode 100644 index 0000000000..d5793ca1e2 --- /dev/null +++ b/routing/routing_tests/index_graph_test.cpp @@ -0,0 +1,219 @@ +#include "testing/testing.hpp" + +#include "routing/base/astar_algorithm.hpp" +#include "routing/index_graph.hpp" + +#include "geometry/point2d.hpp" + +#include "base/assert.hpp" + +#include "std/algorithm.hpp" +#include "std/map.hpp" +#include "std/vector.hpp" + +namespace +{ +using namespace routing; + +class TestGeometry : public Geometry +{ +public: + // Geometry overrides: + bool IsRoad(uint32_t featureId) const override; + bool IsOneWay(uint32_t featureId) const override; + m2::PointD const & GetPoint(FSegId fseg) const override; + double CalcEdgesWeight(uint32_t featureId, uint32_t pointStart, + uint32_t pointFinish) const override; + double CalcHeuristic(FSegId from, FSegId to) const override; + + void AddRoad(uint32_t featureId, vector<m2::PointD> const & points); + +private: + map<uint32_t, vector<m2::PointD>> m_features; +}; + +bool TestGeometry::IsRoad(uint32_t featureId) const +{ + return m_features.find(featureId) != m_features.end(); +} + +bool TestGeometry::IsOneWay(uint32_t featureId) const { return false; } + +m2::PointD const & TestGeometry::GetPoint(FSegId fseg) const +{ + auto it = m_features.find(fseg.GetFeatureId()); + if (it == m_features.end()) + { + ASSERT(false, ("Can't find feature", fseg.GetFeatureId())); + static m2::PointD invalidResult(-1.0, -1.0); + return invalidResult; + } + + ASSERT_LESS(fseg.GetSegId(), it->second.size(), ("featureId =", fseg.GetFeatureId())); + return it->second[fseg.GetSegId()]; +} + +double TestGeometry::CalcEdgesWeight(uint32_t featureId, uint32_t pointFrom, uint32_t pointTo) const +{ + auto it = m_features.find(featureId); + if (it == m_features.end()) + { + ASSERT(false, ("Can't find feature", featureId)); + return 0.0; + } + vector<m2::PointD> const & points = it->second; + + uint32_t const start = min(pointFrom, pointTo); + uint32_t const finish = max(pointFrom, pointTo); + ASSERT_LESS(finish, points.size(), ()); + + double result = 0.0; + for (uint32_t i = start; i < finish; ++i) + result += points[i].Length(points[i + 1]); + + return result; +} + +double TestGeometry::CalcHeuristic(FSegId from, FSegId to) const +{ + return GetPoint(from).Length(GetPoint(to)); +} + +void TestGeometry::AddRoad(uint32_t featureId, vector<m2::PointD> const & points) +{ + auto it = m_features.find(featureId); + if (it != m_features.end()) + { + ASSERT(false, ("Already contains feature", featureId)); + return; + } + + m_features[featureId] = points; +} + +Joint MakeJoint(vector<FSegId> const & points) +{ + Joint joint; + for (auto const & point : points) + { + joint.AddEntry(point); + } + return joint; +} + +void CheckRoute(IndexGraph & graph, FSegId const & start, FSegId const & finish, + size_t expectedLength) +{ + LOG(LINFO, ("Check route", start.GetFeatureId(), ",", start.GetSegId(), "=>", + finish.GetFeatureId(), ",", finish.GetSegId())); + + AStarAlgorithm<IndexGraph> algorithm; + RoutingResult<JointId> routingResult; + + AStarAlgorithm<IndexGraph>::Result const resultCode = algorithm.FindPath( + graph, graph.InsertJoint(start), graph.InsertJoint(finish), routingResult, {}, {}); + vector<FSegId> const & fsegs = graph.RedressRoute(routingResult.path); + + TEST_EQUAL(resultCode, AStarAlgorithm<IndexGraph>::Result::OK, ()); + TEST_EQUAL(fsegs.size(), expectedLength, ()); +} + +uint32_t AbsDelta(uint32_t v0, uint32_t v1) { return v0 > v1 ? v0 - v1 : v1 - v0; } +} // namespace + +namespace routing_test +{ +// R1: +// +// -2 +// -1 +// R0: -2 -1 0 1 2 +// 1 +// 2 +// +UNIT_TEST(FindPathCross) +{ + unique_ptr<TestGeometry> geometry = make_unique<TestGeometry>(); + geometry->AddRoad(0, {{-2.0, 0.0}, {-1.0, 0.0}, {0.0, 0.0}, {1.0, 0.0}, {2.0, 0.0}}); + geometry->AddRoad(1, {{0.0, -2.0}, {-1.0, 0.0}, {0.0, 0.0}, {0.0, 1.0}, {0.0, 2.0}}); + + IndexGraph graph(move(geometry)); + + graph.Export({MakeJoint({{0, 2}, {1, 2}})}); + + vector<FSegId> points; + for (uint32_t i = 0; i < 5; ++i) + { + points.emplace_back(0, i); + points.emplace_back(1, i); + } + + for (auto const & start : points) + { + for (auto const & finish : points) + { + uint32_t expectedLength; + if (start.GetFeatureId() == finish.GetFeatureId()) + expectedLength = AbsDelta(start.GetSegId(), finish.GetSegId()) + 1; + else + expectedLength = AbsDelta(start.GetSegId(), 2) + AbsDelta(finish.GetSegId(), 2) + 1; + CheckRoute(graph, start, finish, expectedLength); + } + } +} + +// R4 R5 R6 R7 +// +// R0: 0 - * - * - * +// | | | | +// R1: * - 1 - * - * +// | | | | +// R2 * - * - 2 - * +// | | | | +// R3 * - * - * - 3 +// +UNIT_TEST(FindPathManhattan) +{ + uint32_t constexpr kCitySize = 4; + unique_ptr<TestGeometry> geometry = make_unique<TestGeometry>(); + for (uint32_t i = 0; i < kCitySize; ++i) + { + vector<m2::PointD> horizontalRoad; + vector<m2::PointD> verticalRoad; + for (uint32_t j = 0; j < kCitySize; ++j) + { + horizontalRoad.emplace_back((double)i, (double)j); + verticalRoad.emplace_back((double)j, (double)i); + } + geometry->AddRoad(i, horizontalRoad); + geometry->AddRoad(i + kCitySize, verticalRoad); + } + + IndexGraph graph(move(geometry)); + + vector<Joint> joints; + for (uint32_t i = 0; i < kCitySize; ++i) + { + for (uint32_t j = 0; j < kCitySize; ++j) + { + joints.emplace_back(MakeJoint({{i, j}, {j + kCitySize, i}})); + } + } + graph.Export(joints); + + for (uint32_t startY = 0; startY < kCitySize; ++startY) + { + for (uint32_t startX = 0; startX < kCitySize; ++startX) + { + for (uint32_t finishY = 0; finishY < kCitySize; ++finishY) + { + for (uint32_t finishX = 0; finishX < kCitySize; ++finishX) + { + CheckRoute(graph, {startX, startY}, {finishX, finishY}, + AbsDelta(startX, finishX) + AbsDelta(startY, finishY) + 1); + } + } + } + } +} +} // namespace routing_test diff --git a/routing/routing_tests/routing_tests.pro b/routing/routing_tests/routing_tests.pro index 0f55d2ccba..100399f067 100644 --- a/routing/routing_tests/routing_tests.pro +++ b/routing/routing_tests/routing_tests.pro @@ -27,6 +27,7 @@ SOURCES += \ async_router_test.cpp \ cross_routing_tests.cpp \ followed_polyline_test.cpp \ + index_graph_test.cpp \ nearest_edge_finder_tests.cpp \ online_cross_fetcher_test.cpp \ osrm_router_test.cpp \ |