From 17ee4f6992ec53e9a63f8f25f3527b11b0dd326f Mon Sep 17 00:00:00 2001 From: Yuri Gorshenin Date: Wed, 22 Apr 2015 16:15:19 +0300 Subject: [PATCH] [pedestrian] Fixes to a P2P routing. --- routing/astar_router.cpp | 2 -- routing/road_graph.cpp | 64 +++++++++++++++++++++++++++++++++-- routing/road_graph.hpp | 28 ++++++++++++--- routing/road_graph_router.cpp | 8 ++--- 4 files changed, 87 insertions(+), 15 deletions(-) diff --git a/routing/astar_router.cpp b/routing/astar_router.cpp index 10da8686ed..c4e3a2396c 100644 --- a/routing/astar_router.cpp +++ b/routing/astar_router.cpp @@ -22,9 +22,7 @@ IRouter::ResultCode AStarRouter::CalculateRoute(RoadPos const & startPos, RoadPo // returned by algorithm should be replaced by correct start and // final positions. ASSERT_EQUAL(route.front(), startPos, ()); - route.front() = startPos; ASSERT_EQUAL(route.back(), finalPos, ()); - route.back() = finalPos; return IRouter::NoError; case TAlgorithm::Result::NoPath: return IRouter::RouteNotFound; diff --git a/routing/road_graph.cpp b/routing/road_graph.cpp index c6d320583c..c364618ff1 100644 --- a/routing/road_graph.cpp +++ b/routing/road_graph.cpp @@ -37,6 +37,12 @@ RoadPos::RoadPos(uint32_t featureId, bool forward, size_t segId, m2::PointD cons ASSERT_LESS(segId, numeric_limits::max(), ()); } +// static +bool RoadPos::IsFakeFeatureId(uint32_t featureId) +{ + return featureId == kFakeStartFeatureId || featureId == kFakeFinalFeatureId; +} + string DebugPrint(RoadPos const & r) { ostringstream ss; @@ -110,7 +116,11 @@ void IRoadGraph::ReconstructPath(RoadPosVectorT const & positions, Route & route path.push_back(curPoint); double const lengthM = CalcDistanceMeters(prevPoint, curPoint); - trackTimeSec += lengthM / (GetSpeedKMPH(positions[i - 1].GetFeatureId()) * KMPH2MPS); + uint32_t const prevFeatureId = positions[i - 1].GetFeatureId(); + double const speedMPS = RoadPos::IsFakeFeatureId(prevFeatureId) + ? MAX_SPEED_MPS + : GetSpeedKMPH(prevFeatureId) * KMPH2MPS; + trackTimeSec += lengthM / speedMPS; prevPoint = curPoint; } @@ -139,6 +149,8 @@ void IRoadGraph::GetNearestTurns(RoadPos const & pos, TurnsVectorT & turns) { uint32_t const featureId = pos.GetFeatureId(); + // For fake start and final positions just add vicinity turns as + // nearest turns. if (featureId == RoadPos::kFakeStartFeatureId) { turns.insert(turns.end(), m_startVicinityTurns.begin(), m_startVicinityTurns.end()); @@ -162,24 +174,66 @@ void IRoadGraph::GetNearestTurns(RoadPos const & pos, TurnsVectorT & turns) CrossTurnsLoader loader(cross, turns); ForEachFeatureClosestToCross(cross, loader); + + AddFakeTurns(pos, roadInfo, m_startVicinityRoadPoss, turns); + AddFakeTurns(pos, roadInfo, m_finalVicinityRoadPoss, turns); + + // It's also possible to move from a start's or final's vicinity + // positions to start or final points. + for (PossibleTurn const & turn : m_fakeTurns[pos]) + turns.push_back(turn); } -void IRoadGraph::SetFakeTurns(RoadPos const & rp, vector const & vicinity) +void IRoadGraph::AddFakeTurns(RoadPos const & pos, RoadInfo const & roadInfo, + RoadPosVectorT const & vicinity, TurnsVectorT & turns) +{ + for (RoadPos const & vpos : vicinity) + { + if (!vpos.SameRoadSegmentAndDirection(pos)) + continue; + + // It's also possible to move from a road position to start's or + // final's vicinity positions if they're on the same road segment. + PossibleTurn turn; + turn.m_secondsCovered = TimeBetweenSec(pos.GetSegEndpoint(), vpos.GetSegEndpoint()); + turn.m_speedKMPH = roadInfo.m_speedKMPH; + turn.m_startPoint = roadInfo.m_points.front(); + turn.m_endPoint = roadInfo.m_points.back(); + turn.m_pos = vpos; + turns.push_back(turn); + } +} + +void IRoadGraph::ResetFakeTurns() +{ + m_startVicinityTurns.clear(); + m_startVicinityRoadPoss.clear(); + m_finalVicinityTurns.clear(); + m_finalVicinityRoadPoss.clear(); + m_fakeTurns.clear(); +} + +void IRoadGraph::AddFakeTurns(RoadPos const & rp, vector const & vicinity) { vector * turns = nullptr; + vector * roadPoss = nullptr; uint32_t const featureId = rp.GetFeatureId(); switch (featureId) { case RoadPos::kFakeStartFeatureId: turns = &m_startVicinityTurns; + roadPoss = &m_startVicinityRoadPoss; break; case RoadPos::kFakeFinalFeatureId: turns = &m_finalVicinityTurns; + roadPoss = &m_finalVicinityRoadPoss; break; default: CHECK(false, ("Can't add fake turns from a valid road (featureId ", featureId, ").")); } - turns->clear(); + + roadPoss->insert(roadPoss->end(), vicinity.begin(), vicinity.end()); + for (RoadPos const & vrp : vicinity) { PossibleTurn turn; @@ -187,6 +241,10 @@ void IRoadGraph::SetFakeTurns(RoadPos const & rp, vector const & vicini /// @todo Do we need other fields? Do we even need m_secondsCovered? turn.m_secondsCovered = TimeBetweenSec(rp.GetSegEndpoint(), vrp.GetSegEndpoint()); turns->push_back(turn); + + /// Add a fake turn from a vicincy road position to a fake point. + turn.m_pos = rp; + m_fakeTurns[vrp].push_back(turn); } } diff --git a/routing/road_graph.hpp b/routing/road_graph.hpp index e41ca7de63..0f7d995f16 100644 --- a/routing/road_graph.hpp +++ b/routing/road_graph.hpp @@ -7,6 +7,7 @@ #include "base/string_utils.hpp" #include "std/vector.hpp" +#include "std/map.hpp" namespace routing { @@ -29,6 +30,8 @@ public: RoadPos(uint32_t featureId, bool forward, size_t segId, m2::PointD const & p = m2::PointD::Zero()); + static bool IsFakeFeatureId(uint32_t featureId); + uint32_t GetFeatureId() const { return m_featureId >> 1; } bool IsForward() const { return (m_featureId & 1) != 0; } uint32_t GetSegId() const { return m_segId; } @@ -36,22 +39,26 @@ public: uint32_t GetSegEndPointId() const { return m_segId + (IsForward() ? 1 : 0); } m2::PointD const & GetSegEndpoint() const { return m_segEndpoint; } - bool operator==(RoadPos const & r) const + bool SameRoadSegmentAndDirection(RoadPos const & r) const { - return (m_featureId == r.m_featureId && m_segId == r.m_segId); + return m_featureId == r.m_featureId && m_segId == r.m_segId; } - bool operator!=(RoadPos const & r) const + bool operator==(RoadPos const & r) const { - return (m_featureId != r.m_featureId || m_segId != r.m_segId); + return m_featureId == r.m_featureId && m_segId == r.m_segId && m_segEndpoint == r.m_segEndpoint; } + bool operator!=(RoadPos const & r) const { return !(*this == r); } + bool operator<(RoadPos const & r) const { if (m_featureId != r.m_featureId) return m_featureId < r.m_featureId; if (m_segId != r.m_segId) return m_segId < r.m_segId; + if (m_segEndpoint != r.m_segEndpoint) + return m_segEndpoint < r.m_segEndpoint; return false; } @@ -143,9 +150,12 @@ public: /// "pos" section. void GetNearestTurns(RoadPos const & pos, TurnsVectorT & turns); + /// Removes all fake turns and vertices from the graph. + void ResetFakeTurns(); + /// Adds fake turns from fake position rp to real vicinity /// positions. - void SetFakeTurns(RoadPos const & rp, vector const & vicinity); + void AddFakeTurns(RoadPos const & rp, RoadPosVectorT const & vicinity); protected: // Returns RoadInfo for a road corresponding to featureId. @@ -158,13 +168,21 @@ protected: virtual void ForEachFeatureClosestToCross(m2::PointD const & cross, CrossTurnsLoader & turnsLoader) = 0; + void AddFakeTurns(RoadPos const & pos, RoadInfo const & roadInfo, RoadPosVectorT const & vicinity, + TurnsVectorT & turns); + // The way we find edges leading from start/final positions and from all other positions // differ: for start/final we find positions in some vicinity of the starting // point; for other positions we extract turns from the road graph. This non-uniformity // comes from the fact that a start/final position does not necessarily fall on a feature // (i.e. on a road). vector m_startVicinityTurns; + vector m_startVicinityRoadPoss; + vector m_finalVicinityTurns; + vector m_finalVicinityRoadPoss; + + map m_fakeTurns; }; // A class which represents an edge used by RoadGraph. diff --git a/routing/road_graph_router.cpp b/routing/road_graph_router.cpp index 87b672086f..fd587a9cf9 100644 --- a/routing/road_graph_router.cpp +++ b/routing/road_graph_router.cpp @@ -76,17 +76,15 @@ IRouter::ResultCode RoadGraphRouter::CalculateRoute(m2::PointD const & startPoin RoadPos startPos(RoadPos::kFakeStartFeatureId, true /* forward */, 0 /* segId */, startPoint); RoadPos finalPos(RoadPos::kFakeFinalFeatureId, true /* forward */, 0 /* segId */, finalPoint); - m_roadGraph->SetFakeTurns(startPos, startVicinity); - m_roadGraph->SetFakeTurns(finalPos, finalVicinity); + m_roadGraph->ResetFakeTurns(); + m_roadGraph->AddFakeTurns(startPos, startVicinity); + m_roadGraph->AddFakeTurns(finalPos, finalVicinity); vector routePos; IRouter::ResultCode resultCode = CalculateRoute(startPos, finalPos, routePos); - /// @todo They have fake feature ids. Can we still draw them? ASSERT(routePos.back() == finalPos, ()); - routePos.pop_back(); ASSERT(routePos.front() == startPos, ()); - routePos.erase(routePos.begin()); LOG(LINFO, ("Route calculation time:", timer.ElapsedSeconds(), "result code:", resultCode));