From 4ca876b6986ebead521e0a94d3958c3c7891ba08 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=94=D0=BE=D0=B1=D1=80=D1=8B=D0=B8=CC=86=20=D0=AD=D1=8D?= =?UTF-8?q?=D1=85?= Date: Wed, 23 Nov 2016 15:29:04 +0300 Subject: [PATCH] index graph wrapper for start and finish --- routing/astar_router.cpp | 34 ++--- routing/index_graph.cpp | 101 +++++-------- routing/index_graph.hpp | 37 ++--- routing/index_graph_starter.cpp | 156 +++++++++++++++++++++ routing/index_graph_starter.hpp | 96 +++++++++++++ routing/joint_index.cpp | 24 ---- routing/joint_index.hpp | 6 +- routing/road_index.hpp | 23 ++- routing/road_point.hpp | 5 + routing/routing.pro | 2 + routing/routing_tests/index_graph_test.cpp | 21 ++- 11 files changed, 360 insertions(+), 145 deletions(-) create mode 100644 routing/index_graph_starter.cpp create mode 100644 routing/index_graph_starter.hpp diff --git a/routing/astar_router.cpp b/routing/astar_router.cpp index 14c711c889..17c1abd2cf 100644 --- a/routing/astar_router.cpp +++ b/routing/astar_router.cpp @@ -7,6 +7,7 @@ #include "routing/car_model.hpp" #include "routing/features_road_graph.hpp" #include "routing/index_graph.hpp" +#include "routing/index_graph_starter.hpp" #include "routing/pedestrian_model.hpp" #include "routing/route.hpp" #include "routing/turns_generator.hpp" @@ -28,15 +29,16 @@ size_t constexpr kMaxRoadCandidates = 6; float constexpr kProgressInterval = 2; uint32_t constexpr kDrawPointsPeriod = 10; -vector ConvertToJunctions(IndexGraph const & graph, vector const & joints) +vector ConvertToJunctions(IndexGraphStarter const & starter, + vector const & joints) { vector roadPoints; - graph.RedressRoute(joints, roadPoints); + starter.RedressRoute(joints, roadPoints); vector junctions; junctions.reserve(roadPoints.size()); - Geometry const & geometry = graph.GetGeometry(); + Geometry const & geometry = starter.GetGraph().GetGeometry(); // TODO: Use real altitudes for pedestrian and bicycle routing. for (RoadPoint const & point : roadPoints) junctions.emplace_back(geometry.GetPoint(point), feature::kDefaultAltitudeMeters); @@ -109,14 +111,15 @@ IRouter::ResultCode AStarRouter::DoCalculateRoute(MwmSet::MwmId const & mwmId, if (!LoadIndex(mwmId, country, graph)) return IRouter::RouteFileNotExist; + IndexGraphStarter starter(graph, start, finish); + AStarProgress progress(0, 100); progress.Initialize(graph.GetGeometry().GetPoint(start), graph.GetGeometry().GetPoint(finish)); uint32_t drawPointsStep = 0; - auto onVisitJunction = [&delegate, &progress, &graph, &drawPointsStep](Joint::Id const & from, - Joint::Id const & target) { - m2::PointD const & point = graph.GetPoint(from); - m2::PointD const & targetPoint = graph.GetPoint(target); + auto onVisitJunction = [&](Joint::Id const & from, Joint::Id const & target) { + m2::PointD const & point = starter.GetPoint(from); + m2::PointD const & targetPoint = starter.GetPoint(target); auto const lastValue = progress.GetLastValue(); auto const newValue = progress.GetProgressForBidirectedAlgo(point, targetPoint); if (newValue - lastValue > kProgressInterval) @@ -126,20 +129,19 @@ IRouter::ResultCode AStarRouter::DoCalculateRoute(MwmSet::MwmId const & mwmId, ++drawPointsStep; }; - AStarAlgorithm algorithm; + AStarAlgorithm algorithm; RoutingResult routingResult; - Joint::Id const startJoint = graph.InsertJoint(start); - Joint::Id const finishJoint = graph.InsertJoint(finish); - auto const resultCode = algorithm.FindPathBidirectional(graph, startJoint, finishJoint, - routingResult, delegate, onVisitJunction); + auto const resultCode = + algorithm.FindPathBidirectional(starter, starter.GetStartJoint(), starter.GetFinishJoint(), + routingResult, delegate, onVisitJunction); switch (resultCode) { - case AStarAlgorithm::Result::NoPath: return IRouter::RouteNotFound; - case AStarAlgorithm::Result::Cancelled: return IRouter::Cancelled; - case AStarAlgorithm::Result::OK: - vector path = ConvertToJunctions(graph, routingResult.path); + case AStarAlgorithm::Result::NoPath: return IRouter::RouteNotFound; + case AStarAlgorithm::Result::Cancelled: return IRouter::Cancelled; + case AStarAlgorithm::Result::OK: + vector path = ConvertToJunctions(starter, routingResult.path); ReconstructRoute(m_directionsEngine.get(), *m_roadGraph, delegate, path, route); return IRouter::NoError; } diff --git a/routing/index_graph.cpp b/routing/index_graph.cpp index 8ae77dfc2f..6e3e0136c3 100644 --- a/routing/index_graph.cpp +++ b/routing/index_graph.cpp @@ -11,19 +11,11 @@ IndexGraph::IndexGraph(unique_ptr loader, shared_ptr & edges) const +void IndexGraph::GetEdgesList(Joint::Id jointId, bool isOutgoing, vector & edges) const { - GetEdgesList(jointId, true, edges); -} - -void IndexGraph::GetIngoingEdgesList(Joint::Id jointId, vector & edges) const -{ - GetEdgesList(jointId, false, edges); -} - -double IndexGraph::HeuristicCostEstimate(Joint::Id jointFrom, Joint::Id jointTo) const -{ - return m_estimator->CalcHeuristic(GetPoint(jointFrom), GetPoint(jointTo)); + m_jointIndex.ForEachPoint(jointId, [this, &edges, isOutgoing](RoadPoint const & rp) { + AddNeighboringEdges(rp, isOutgoing, edges); + }); } m2::PointD const & IndexGraph::GetPoint(Joint::Id jointId) const @@ -48,54 +40,34 @@ Joint::Id IndexGraph::InsertJoint(RoadPoint const & rp) return jointId; } -void IndexGraph::RedressRoute(vector const & route, vector & roadPoints) const +bool IndexGraph::JointLaysOnRoad(Joint::Id jointId, uint32_t featureId) const { - if (route.size() < 2) - { - if (route.size() == 1) - roadPoints.emplace_back(m_jointIndex.GetPoint(route[0])); + bool result = false; + m_jointIndex.ForEachPoint(jointId, [&result, featureId](RoadPoint const & rp) { + if (rp.GetFeatureId() == featureId) + result = true; + }); + + return result; +} + +inline void IndexGraph::AddNeighboringEdges(RoadPoint rp, bool isOutgoing, + vector & edges) const +{ + RoadGeometry const & road = m_geometry.GetRoad(rp.GetFeatureId()); + if (!road.IsRoad()) return; - } - roadPoints.reserve(route.size() * 2); + bool const bidirectional = !road.IsOneWay(); + if (!isOutgoing || bidirectional) + AddNeighboringEdge(road, rp, false /* forward */, edges); - for (size_t i = 0; i < route.size() - 1; ++i) - { - Joint::Id const prevJoint = route[i]; - Joint::Id const nextJoint = route[i + 1]; - - RoadPoint rp0; - RoadPoint rp1; - m_jointIndex.FindPointsWithCommonFeature(prevJoint, nextJoint, rp0, rp1); - if (i == 0) - roadPoints.emplace_back(rp0); - - uint32_t const featureId = rp0.GetFeatureId(); - uint32_t const pointFrom = rp0.GetPointId(); - uint32_t const pointTo = rp1.GetPointId(); - - if (pointFrom < pointTo) - { - for (uint32_t pointId = pointFrom + 1; pointId < pointTo; ++pointId) - roadPoints.emplace_back(featureId, pointId); - } - else if (pointFrom > pointTo) - { - for (uint32_t pointId = pointFrom - 1; pointId > pointTo; --pointId) - roadPoints.emplace_back(featureId, pointId); - } - else - { - MYTHROW(RootException, - ("Wrong equality pointFrom = pointTo =", pointFrom, ", featureId = ", featureId)); - } - - roadPoints.emplace_back(rp1); - } + if (isOutgoing || bidirectional) + AddNeighboringEdge(road, rp, true /* forward */, edges); } inline void IndexGraph::AddNeighboringEdge(RoadGeometry const & road, RoadPoint rp, bool forward, - vector & edges) const + vector & edges) const { pair const & neighbor = m_roadIndex.FindNeighbor(rp, forward); if (neighbor.first != Joint::kInvalidId) @@ -105,22 +77,17 @@ inline void IndexGraph::AddNeighboringEdge(RoadGeometry const & road, RoadPoint } } -inline void IndexGraph::GetEdgesList(Joint::Id jointId, bool isOutgoing, - vector & edges) const +void IndexGraph::AddDirectEdge(uint32_t featureId, uint32_t pointFrom, uint32_t pointTo, + Joint::Id target, bool forward, vector & edges) const { - edges.clear(); + RoadGeometry const & road = m_geometry.GetRoad(featureId); + if (!road.IsRoad()) + return; - m_jointIndex.ForEachPoint(jointId, [this, &edges, isOutgoing](RoadPoint const & rp) { - RoadGeometry const & road = m_geometry.GetRoad(rp.GetFeatureId()); - if (!road.IsRoad()) - return; + if (road.IsOneWay() && forward != (pointFrom < pointTo)) + return; - bool const bidirectional = !road.IsOneWay(); - if (!isOutgoing || bidirectional) - AddNeighboringEdge(road, rp, false /* forward */, edges); - - if (isOutgoing || bidirectional) - AddNeighboringEdge(road, rp, true /* forward */, edges); - }); + double const distance = m_estimator->CalcEdgesWeight(road, pointFrom, pointTo); + edges.emplace_back(target, distance); } } // namespace routing diff --git a/routing/index_graph.hpp b/routing/index_graph.hpp index 38aae8410e..8c535fb94f 100644 --- a/routing/index_graph.hpp +++ b/routing/index_graph.hpp @@ -36,30 +36,32 @@ private: class IndexGraph final { public: - // AStarAlgorithm types aliases: - using TVertexType = Joint::Id; - using TEdgeType = JointEdge; - IndexGraph() = default; explicit IndexGraph(unique_ptr loader, shared_ptr estimator); - // AStarAlgorithm overloads: - void GetOutgoingEdgesList(Joint::Id vertex, vector & edges) const; - void GetIngoingEdgesList(Joint::Id vertex, vector & edges) const; - double HeuristicCostEstimate(Joint::Id from, Joint::Id to) const; - + void AddDirectEdge(uint32_t featureId, uint32_t pointFrom, uint32_t pointTo, Joint::Id target, + bool forward, vector & edges) const; + void AddNeighboringEdges(RoadPoint rp, bool isOutgoing, vector & edges) const; + double CalcHeuristic(m2::PointD const & from, m2::PointD const & to) const + { + return m_estimator->CalcHeuristic(from, to); + } + void GetEdgesList(Joint::Id jointId, bool forward, vector & edges) const; + Joint::Id GetJointId(RoadPoint rp) const { return m_roadIndex.GetJointId(rp); } Geometry const & GetGeometry() const { return m_geometry; } m2::PointD const & GetPoint(Joint::Id jointId) const; - size_t GetNumRoads() const { return m_roadIndex.GetSize(); } - size_t GetNumJoints() const { return m_jointIndex.GetNumJoints(); } - size_t GetNumPoints() const { return m_jointIndex.GetNumPoints(); } + uint32_t GetNumRoads() const { return m_roadIndex.GetSize(); } + uint32_t GetNumJoints() const { return m_jointIndex.GetNumJoints(); } + uint32_t GetNumPoints() const { return m_jointIndex.GetNumPoints(); } void Import(vector const & joints); Joint::Id InsertJoint(RoadPoint const & rp); + bool JointLaysOnRoad(Joint::Id jointId, uint32_t featureId) const; - // Add intermediate points to route (those don't correspond to any joint). - // - // Also convert joint ids to RoadPoints. - void RedressRoute(vector const & route, vector & roadPoints) const; + template + void ForEachPoint(Joint::Id jointId, F && f) const + { + m_jointIndex.ForEachPoint(jointId, f); + } template void Serialize(Sink & sink) const @@ -78,8 +80,7 @@ public: private: void AddNeighboringEdge(RoadGeometry const & road, RoadPoint rp, bool forward, - vector & edges) const; - void GetEdgesList(Joint::Id jointId, bool forward, vector & edges) const; + vector & edges) const; Geometry m_geometry; shared_ptr m_estimator; diff --git a/routing/index_graph_starter.cpp b/routing/index_graph_starter.cpp new file mode 100644 index 0000000000..7b1f5658cd --- /dev/null +++ b/routing/index_graph_starter.cpp @@ -0,0 +1,156 @@ +#include "routing/index_graph_starter.hpp" + +namespace routing +{ +IndexGraphStarter::IndexGraphStarter(IndexGraph const & graph, RoadPoint startPoint, + RoadPoint finishPoint) + : m_graph(graph) + , m_startPoint(startPoint) + , m_finishPoint(finishPoint) + , m_startImplant(graph.GetNumJoints()) + , m_finishImplant(graph.GetNumJoints() + 1) + , m_startJoint(CalcStartJoint()) + , m_finishJoint(CalcFinishJoint()) +{ +} + +Joint::Id IndexGraphStarter::CalcStartJoint() const +{ + Joint::Id const jointId = m_graph.GetJointId(m_startPoint); + if (jointId == Joint::kInvalidId) + return m_startImplant; + + return jointId; +} + +Joint::Id IndexGraphStarter::CalcFinishJoint() const +{ + if (m_startPoint == m_finishPoint) + return CalcStartJoint(); + + Joint::Id const jointId = m_graph.GetJointId(m_finishPoint); + if (jointId == Joint::kInvalidId) + return m_finishImplant; + + return jointId; +} + +void IndexGraphStarter::GetEdgesList(Joint::Id jointId, bool isOutgoing, + vector & edges) const +{ + edges.clear(); + + if (jointId == m_startImplant) + { + m_graph.AddNeighboringEdges(m_startPoint, isOutgoing, edges); + + if (FinishIsImplant() && m_startPoint.GetFeatureId() == m_finishPoint.GetFeatureId()) + m_graph.AddDirectEdge(m_startPoint.GetFeatureId(), m_startPoint.GetPointId(), + m_finishPoint.GetPointId(), m_finishJoint, isOutgoing, edges); + + return; + } + + if (jointId == m_finishImplant) + { + m_graph.AddNeighboringEdges(m_finishPoint, isOutgoing, edges); + + if (StartIsImplant() && m_startPoint.GetFeatureId() == m_finishPoint.GetFeatureId()) + m_graph.AddDirectEdge(m_finishPoint.GetFeatureId(), m_finishPoint.GetPointId(), + m_startPoint.GetPointId(), m_startJoint, isOutgoing, edges); + + return; + } + + m_graph.GetEdgesList(jointId, isOutgoing, edges); + + if (StartIsImplant() && m_graph.JointLaysOnRoad(jointId, m_startPoint.GetFeatureId())) + { + vector startEdges; + m_graph.AddNeighboringEdges(m_startPoint, !isOutgoing, startEdges); + for (JointEdge const & edge : startEdges) + { + if (edge.GetTarget() == jointId) + edges.emplace_back(m_startJoint, edge.GetWeight()); + } + } + + if (FinishIsImplant() && m_graph.JointLaysOnRoad(jointId, m_finishPoint.GetFeatureId())) + { + vector finishEdges; + m_graph.AddNeighboringEdges(m_finishPoint, !isOutgoing, finishEdges); + for (JointEdge const & edge : finishEdges) + { + if (edge.GetTarget() == jointId) + edges.emplace_back(m_finishJoint, edge.GetWeight()); + } + } +} + +void IndexGraphStarter::RedressRoute(vector const & route, + vector & roadPoints) const +{ + if (route.size() < 2) + { + if (route.size() == 1) + roadPoints.emplace_back(m_startPoint); + return; + } + + roadPoints.reserve(route.size() * 2); + + for (size_t i = 0; i < route.size() - 1; ++i) + { + Joint::Id const prevJoint = route[i]; + Joint::Id const nextJoint = route[i + 1]; + + RoadPoint rp0; + RoadPoint rp1; + FindPointsWithCommonFeature(prevJoint, nextJoint, rp0, rp1); + if (i == 0) + roadPoints.emplace_back(rp0); + + uint32_t const featureId = rp0.GetFeatureId(); + uint32_t const pointFrom = rp0.GetPointId(); + uint32_t const pointTo = rp1.GetPointId(); + + if (pointFrom < pointTo) + { + for (uint32_t pointId = pointFrom + 1; pointId < pointTo; ++pointId) + roadPoints.emplace_back(featureId, pointId); + } + else if (pointFrom > pointTo) + { + for (uint32_t pointId = pointFrom - 1; pointId > pointTo; --pointId) + roadPoints.emplace_back(featureId, pointId); + } + else + { + MYTHROW(RootException, + ("Wrong equality pointFrom = pointTo =", pointFrom, ", featureId = ", featureId)); + } + + roadPoints.emplace_back(rp1); + } +} + +void IndexGraphStarter::FindPointsWithCommonFeature(Joint::Id jointId0, Joint::Id jointId1, + RoadPoint & result0, RoadPoint & result1) const +{ + bool found = false; + + ForEachPoint(jointId0, [&](RoadPoint const & rp0) { + ForEachPoint(jointId1, [&](RoadPoint const & rp1) { + if (rp0.GetFeatureId() == rp1.GetFeatureId() && !found) + { + result0 = rp0; + result1 = rp1; + found = true; + } + }); + }); + + if (!found) + MYTHROW(RootException, ("Can't find common feature for joints", jointId0, jointId1)); +} +} // namespace routing diff --git a/routing/index_graph_starter.hpp b/routing/index_graph_starter.hpp new file mode 100644 index 0000000000..104b4bd30a --- /dev/null +++ b/routing/index_graph_starter.hpp @@ -0,0 +1,96 @@ +#pragma once + +#include "routing/index_graph.hpp" +#include "routing/joint.hpp" + +namespace routing +{ +// The problem: +// IndexGraph contains only road points connected in joints. +// So it is possible IndexGraph doesn't contain start and finish. +// +// IndexGraphStarter implants start and finish for AStarAlgorithm. +// +class IndexGraphStarter final +{ +public: + // AStarAlgorithm types aliases: + using TVertexType = Joint::Id; + using TEdgeType = JointEdge; + + IndexGraphStarter(IndexGraph const & graph, RoadPoint startPoint, RoadPoint finishPoint); + + IndexGraph const & GetGraph() const { return m_graph; } + Joint::Id GetStartJoint() const { return m_startJoint; } + Joint::Id GetFinishJoint() const { return m_finishJoint; } + + m2::PointD const & GetPoint(Joint::Id jointId) const + { + if (jointId == m_startImplant) + return m_graph.GetGeometry().GetPoint(m_startPoint); + + if (jointId == m_finishImplant) + return m_graph.GetGeometry().GetPoint(m_finishPoint); + + return m_graph.GetPoint(jointId); + } + + void GetOutgoingEdgesList(Joint::Id jointId, vector & edges) const + { + GetEdgesList(jointId, true, edges); + } + + void GetIngoingEdgesList(Joint::Id jointId, vector & edges) const + { + GetEdgesList(jointId, false, edges); + } + + void GetEdgesList(Joint::Id jointId, bool isOutgoing, vector & edges) const; + + double HeuristicCostEstimate(Joint::Id from, Joint::Id to) const + { + return m_graph.CalcHeuristic(GetPoint(from), GetPoint(to)); + } + + // Add intermediate points to route (those don't correspond to any joint). + // + // Also convert joint ids to RoadPoints. + void RedressRoute(vector const & route, vector & roadPoints) const; + +private: + Joint::Id CalcStartJoint() const; + Joint::Id CalcFinishJoint() const; + + bool StartIsImplant() const { return m_startJoint == m_startImplant; } + bool FinishIsImplant() const { return m_finishJoint == m_finishImplant; } + + template + void ForEachPoint(Joint::Id jointId, F && f) const + { + if (jointId == m_startImplant) + { + f(m_startPoint); + return; + } + + if (jointId == m_finishImplant) + { + f(m_finishPoint); + return; + } + + m_graph.ForEachPoint(jointId, f); + } + + void FindPointsWithCommonFeature(Joint::Id jointId0, Joint::Id jointId1, RoadPoint & result0, + RoadPoint & result1) const; + + IndexGraph const & m_graph; + RoadPoint const m_startPoint; + RoadPoint const m_finishPoint; + Joint::Id const m_startImplant; + Joint::Id const m_finishImplant; + Joint::Id const m_startJoint; + Joint::Id const m_finishJoint; +}; +} // namespace routing diff --git a/routing/joint_index.cpp b/routing/joint_index.cpp index f8bbb2a680..2efd1c5bb8 100644 --- a/routing/joint_index.cpp +++ b/routing/joint_index.cpp @@ -15,34 +15,12 @@ void JointIndex::AppendToJoint(Joint::Id jointId, RoadPoint rp) m_dynamicJoints[jointId].AddPoint(rp); } -void JointIndex::FindPointsWithCommonFeature(Joint::Id jointId0, Joint::Id jointId1, - RoadPoint & result0, RoadPoint & result1) const -{ - bool found = false; - - ForEachPoint(jointId0, [&](RoadPoint const & rp0) { - ForEachPoint(jointId1, [&](RoadPoint const & rp1) { - if (rp0.GetFeatureId() == rp1.GetFeatureId() && !found) - { - result0 = rp0; - result1 = rp1; - found = true; - } - }); - }); - - if (!found) - MYTHROW(RootException, ("Can't find common feature for joints", jointId0, jointId1)); -} - void JointIndex::Build(RoadIndex const & roadIndex, uint32_t numJoints) { - // +2 is reserved space for start and finish. // + 1 is protection for 'End' method from out of bounds. // Call End(numJoints-1) requires more size, so add one more item. // Therefore m_offsets.size() == numJoints + 1, // And m_offsets.back() == m_points.size() - m_offsets.reserve(numJoints + 1 + 2); m_offsets.assign(numJoints + 1, 0); // Calculate shifted sizes. @@ -68,8 +46,6 @@ void JointIndex::Build(RoadIndex const & roadIndex, uint32_t numJoints) prevSum = sum; } - // +2 is reserved space for start and finish - m_points.reserve(sum + 2); m_points.resize(sum); // Now fill points, m_offsets[nextId] is current incrementing begin. diff --git a/routing/joint_index.hpp b/routing/joint_index.hpp index d54f4004c0..0bc95a6b3b 100644 --- a/routing/joint_index.hpp +++ b/routing/joint_index.hpp @@ -16,8 +16,8 @@ class JointIndex final { public: // Read comments in Build method about -1. - size_t GetNumJoints() const { return m_offsets.size() - 1; } - size_t GetNumPoints() const { return m_points.size(); } + uint32_t GetNumJoints() const { return m_offsets.size() - 1; } + uint32_t GetNumPoints() const { return m_points.size(); } RoadPoint GetPoint(Joint::Id jointId) const { return m_points[Begin(jointId)]; } template @@ -36,8 +36,6 @@ public: } void Build(RoadIndex const & roadIndex, uint32_t numJoints); - void FindPointsWithCommonFeature(Joint::Id jointId0, Joint::Id jointId1, RoadPoint & result0, - RoadPoint & result1) const; Joint::Id InsertJoint(RoadPoint const & rp); void AppendToJoint(Joint::Id jointId, RoadPoint rp); diff --git a/routing/road_index.hpp b/routing/road_index.hpp index e64f9bf474..1ceb9214aa 100644 --- a/routing/road_index.hpp +++ b/routing/road_index.hpp @@ -5,6 +5,7 @@ #include "coding/reader.hpp" #include "coding/write_to_sink.hpp" +#include "std/algorithm.hpp" #include "std/cstdint.hpp" #include "std/unordered_map.hpp" #include "std/utility.hpp" @@ -45,13 +46,25 @@ public: pair FindNeighbor(uint32_t pointId, bool forward) const { - int32_t const step = forward ? 1 : -1; + uint32_t const size = static_cast(m_jointIds.size()); - for (uint32_t i = pointId + step; i < m_jointIds.size(); i += step) + if (forward) { - Joint::Id const jointId = m_jointIds[i]; - if (jointId != Joint::kInvalidId) - return make_pair(jointId, i); + for (uint32_t i = pointId + 1; i < size; ++i) + { + Joint::Id const jointId = m_jointIds[i]; + if (jointId != Joint::kInvalidId) + return make_pair(jointId, i); + } + } + else + { + for (uint32_t i = min(pointId, size) - 1; i < size; --i) + { + Joint::Id const jointId = m_jointIds[i]; + if (jointId != Joint::kInvalidId) + return make_pair(jointId, i); + } } return make_pair(Joint::kInvalidId, 0); diff --git a/routing/road_point.hpp b/routing/road_point.hpp index ca580f0f1a..c613bca31f 100644 --- a/routing/road_point.hpp +++ b/routing/road_point.hpp @@ -19,6 +19,11 @@ public: uint32_t GetPointId() const { return m_pointId; } + bool operator==(RoadPoint const & rp) const + { + return m_featureId == rp.m_featureId && m_pointId == rp.m_pointId; + } + private: uint32_t m_featureId; uint32_t m_pointId; diff --git a/routing/routing.pro b/routing/routing.pro index 94f52611d7..b3c500cfc7 100644 --- a/routing/routing.pro +++ b/routing/routing.pro @@ -28,6 +28,7 @@ SOURCES += \ features_road_graph.cpp \ geometry.cpp \ index_graph.cpp \ + index_graph_starter.cpp \ joint.cpp \ joint_index.cpp \ nearest_edge_finder.cpp \ @@ -76,6 +77,7 @@ HEADERS += \ features_road_graph.hpp \ geometry.hpp \ index_graph.hpp \ + index_graph_starter.hpp \ joint.hpp \ joint_index.hpp \ loaded_path_segment.hpp \ diff --git a/routing/routing_tests/index_graph_test.cpp b/routing/routing_tests/index_graph_test.cpp index c1991977f2..22d1b7ad77 100644 --- a/routing/routing_tests/index_graph_test.cpp +++ b/routing/routing_tests/index_graph_test.cpp @@ -4,6 +4,7 @@ #include "routing/car_model.hpp" #include "routing/edge_estimator.hpp" #include "routing/index_graph.hpp" +#include "routing/index_graph_starter.hpp" #include "geometry/point2d.hpp" @@ -66,21 +67,22 @@ shared_ptr CreateEstimator() return CreateCarEdgeEstimator(make_shared()->GetVehicleModel()); } -void TestRoute(IndexGraph & graph, RoadPoint const & start, RoadPoint const & finish, +void TestRoute(IndexGraph const & graph, RoadPoint const & start, RoadPoint const & finish, size_t expectedLength) { LOG(LINFO, ("Test route", start.GetFeatureId(), ",", start.GetPointId(), "=>", finish.GetFeatureId(), ",", finish.GetPointId())); - AStarAlgorithm algorithm; + AStarAlgorithm algorithm; RoutingResult routingResult; - AStarAlgorithm::Result const resultCode = algorithm.FindPath( - graph, graph.InsertJoint(start), graph.InsertJoint(finish), routingResult, {}, {}); - vector roadPoints; - graph.RedressRoute(routingResult.path, roadPoints); + IndexGraphStarter starter(graph, start, finish); + auto const resultCode = algorithm.FindPath(starter, starter.GetStartJoint(), + starter.GetFinishJoint(), routingResult, {}, {}); + TEST_EQUAL(resultCode, AStarAlgorithm::Result::OK, ()); - TEST_EQUAL(resultCode, AStarAlgorithm::Result::OK, ()); + vector roadPoints; + starter.RedressRoute(routingResult.path, roadPoints); TEST_EQUAL(roadPoints.size(), expectedLength, ()); } @@ -88,10 +90,7 @@ void TestEdges(IndexGraph const & graph, Joint::Id jointId, vector const & expectedTargets, bool forward) { vector edges; - if (forward) - graph.GetOutgoingEdgesList(jointId, edges); - else - graph.GetIngoingEdgesList(jointId, edges); + graph.GetEdgesList(jointId, forward, edges); vector targets; for (JointEdge const & edge : edges)