Repo created
This commit is contained in:
parent
4af19165ec
commit
68073add76
12458 changed files with 12350765 additions and 2 deletions
217
libs/routing/CMakeLists.txt
Normal file
217
libs/routing/CMakeLists.txt
Normal file
|
|
@ -0,0 +1,217 @@
|
|||
project(routing)
|
||||
|
||||
set(SRC
|
||||
absent_regions_finder.cpp
|
||||
absent_regions_finder.hpp
|
||||
async_router.cpp
|
||||
async_router.hpp
|
||||
base/astar_algorithm.hpp
|
||||
base/astar_progress.cpp
|
||||
base/astar_progress.hpp
|
||||
base/astar_vertex_data.hpp
|
||||
base/astar_weight.hpp
|
||||
base/bfs.hpp
|
||||
base/followed_polyline.cpp
|
||||
base/followed_polyline.hpp
|
||||
base/routing_result.hpp
|
||||
base/small_list.hpp
|
||||
base/small_list.cpp
|
||||
lanes/lane_info.cpp
|
||||
lanes/lane_info.hpp
|
||||
lanes/lane_way.cpp
|
||||
lanes/lane_way.hpp
|
||||
lanes/lanes_parser.cpp
|
||||
lanes/lanes_parser.hpp
|
||||
lanes/lanes_recommendation.cpp
|
||||
lanes/lanes_recommendation.hpp
|
||||
car_directions.cpp
|
||||
car_directions.hpp
|
||||
checkpoint_predictor.cpp
|
||||
checkpoint_predictor.hpp
|
||||
checkpoints.cpp
|
||||
checkpoints.hpp
|
||||
city_roads.cpp
|
||||
city_roads.hpp
|
||||
city_roads_serialization.hpp
|
||||
coding.hpp
|
||||
cross_border_graph.cpp
|
||||
cross_border_graph.hpp
|
||||
cross_mwm_connector.cpp
|
||||
cross_mwm_connector.hpp
|
||||
cross_mwm_connector_serialization.hpp
|
||||
cross_mwm_graph.cpp
|
||||
cross_mwm_graph.hpp
|
||||
cross_mwm_ids.hpp
|
||||
cross_mwm_index_graph.hpp
|
||||
data_source.hpp
|
||||
directions_engine.cpp
|
||||
directions_engine.hpp
|
||||
directions_engine_helpers.cpp
|
||||
directions_engine_helpers.hpp
|
||||
dummy_world_graph.hpp
|
||||
edge_estimator.cpp
|
||||
edge_estimator.hpp
|
||||
fake_edges_container.hpp
|
||||
fake_ending.cpp
|
||||
fake_ending.hpp
|
||||
fake_feature_ids.hpp
|
||||
fake_graph.cpp
|
||||
fake_graph.hpp
|
||||
fake_vertex.hpp
|
||||
features_road_graph.cpp
|
||||
features_road_graph.hpp
|
||||
following_info.hpp
|
||||
geometry.cpp
|
||||
geometry.hpp
|
||||
guides_connections.cpp
|
||||
guides_connections.hpp
|
||||
guides_graph.cpp
|
||||
guides_graph.hpp
|
||||
index_graph.cpp
|
||||
index_graph.hpp
|
||||
index_graph_loader.cpp
|
||||
index_graph_loader.hpp
|
||||
index_graph_serialization.cpp
|
||||
index_graph_serialization.hpp
|
||||
index_graph_starter.cpp
|
||||
index_graph_starter.hpp
|
||||
index_graph_starter_joints.hpp
|
||||
index_road_graph.cpp
|
||||
index_road_graph.hpp
|
||||
index_router.cpp
|
||||
index_router.hpp
|
||||
joint.cpp
|
||||
joint.hpp
|
||||
joint_index.cpp
|
||||
joint_index.hpp
|
||||
joint_segment.cpp
|
||||
joint_segment.hpp
|
||||
junction_visitor.cpp
|
||||
junction_visitor.hpp
|
||||
latlon_with_altitude.cpp
|
||||
latlon_with_altitude.hpp
|
||||
leaps_graph.cpp
|
||||
leaps_graph.hpp
|
||||
leaps_postprocessor.cpp
|
||||
leaps_postprocessor.hpp
|
||||
loaded_path_segment.hpp
|
||||
maxspeeds.cpp
|
||||
maxspeeds.hpp
|
||||
maxspeeds_serialization.cpp
|
||||
maxspeeds_serialization.hpp
|
||||
mwm_hierarchy_handler.cpp
|
||||
mwm_hierarchy_handler.hpp
|
||||
nearest_edge_finder.cpp
|
||||
nearest_edge_finder.hpp
|
||||
opening_hours_serdes.cpp
|
||||
opening_hours_serdes.hpp
|
||||
pedestrian_directions.cpp
|
||||
pedestrian_directions.hpp
|
||||
position_accumulator.cpp
|
||||
position_accumulator.hpp
|
||||
regions_router.cpp
|
||||
regions_router.hpp
|
||||
regions_sparse_graph.cpp
|
||||
regions_sparse_graph.hpp
|
||||
restriction_loader.cpp
|
||||
restriction_loader.hpp
|
||||
restrictions_serialization.cpp
|
||||
restrictions_serialization.hpp
|
||||
road_access.cpp
|
||||
road_access.hpp
|
||||
road_access_serialization.cpp
|
||||
road_access_serialization.hpp
|
||||
road_penalty.cpp
|
||||
road_penalty.hpp
|
||||
road_penalty_serialization.hpp
|
||||
road_graph.cpp
|
||||
road_graph.hpp
|
||||
road_index.cpp
|
||||
road_index.hpp
|
||||
road_point.hpp
|
||||
route.cpp
|
||||
route.hpp
|
||||
route_point.hpp
|
||||
route_weight.cpp
|
||||
route_weight.hpp
|
||||
router.cpp
|
||||
router.hpp
|
||||
router_delegate.cpp
|
||||
router_delegate.hpp
|
||||
routing_callbacks.hpp
|
||||
routing_exceptions.hpp
|
||||
routing_helpers.cpp
|
||||
routing_helpers.hpp
|
||||
routing_options.cpp
|
||||
routing_options.hpp
|
||||
routing_result_graph.hpp
|
||||
routing_session.cpp
|
||||
routing_session.hpp
|
||||
routing_settings.cpp
|
||||
routing_settings.hpp
|
||||
ruler_router.cpp
|
||||
ruler_router.hpp
|
||||
segment.cpp
|
||||
segment.hpp
|
||||
segmented_route.cpp
|
||||
segmented_route.hpp
|
||||
single_vehicle_world_graph.cpp
|
||||
single_vehicle_world_graph.hpp
|
||||
speed_camera.cpp
|
||||
speed_camera.hpp
|
||||
speed_camera_manager.cpp
|
||||
speed_camera_manager.hpp
|
||||
speed_camera_prohibition.cpp
|
||||
speed_camera_prohibition.hpp
|
||||
speed_camera_ser_des.cpp
|
||||
speed_camera_ser_des.hpp
|
||||
traffic_stash.cpp
|
||||
traffic_stash.hpp
|
||||
transit_graph.cpp
|
||||
transit_graph.hpp
|
||||
transit_graph_loader.cpp
|
||||
transit_graph_loader.hpp
|
||||
transit_info.hpp
|
||||
transit_world_graph.cpp
|
||||
transit_world_graph.hpp
|
||||
turn_candidate.hpp
|
||||
turns.cpp
|
||||
turns.hpp
|
||||
turns_generator_utils.cpp
|
||||
turns_generator_utils.hpp
|
||||
turns_generator.cpp
|
||||
turns_generator.hpp
|
||||
turns_notification_manager.cpp
|
||||
turns_notification_manager.hpp
|
||||
turns_sound_settings.cpp
|
||||
turns_sound_settings.hpp
|
||||
turns_tts_text.cpp
|
||||
turns_tts_text.hpp
|
||||
turns_tts_text_i18n.cpp
|
||||
turns_tts_text_i18n.hpp
|
||||
vehicle_mask.cpp
|
||||
vehicle_mask.hpp
|
||||
world_graph.cpp
|
||||
world_graph.hpp
|
||||
)
|
||||
|
||||
omim_add_library(${PROJECT_NAME} ${SRC})
|
||||
|
||||
target_link_libraries(${PROJECT_NAME}
|
||||
base
|
||||
routing_common
|
||||
platform
|
||||
traffic
|
||||
transit
|
||||
opening_hours
|
||||
)
|
||||
|
||||
if (PLATFORM_DESKTOP)
|
||||
add_subdirectory(routing_quality)
|
||||
add_subdirectory(routes_builder)
|
||||
endif()
|
||||
|
||||
omim_add_test_subdirectory(routing_benchmarks)
|
||||
omim_add_test_subdirectory(routing_consistency_tests)
|
||||
omim_add_test_subdirectory(routing_integration_tests)
|
||||
omim_add_test_subdirectory(routing_tests)
|
||||
73
libs/routing/absent_regions_finder.cpp
Normal file
73
libs/routing/absent_regions_finder.cpp
Normal file
|
|
@ -0,0 +1,73 @@
|
|||
#include "routing/absent_regions_finder.hpp"
|
||||
#include "routing/regions_router.hpp"
|
||||
|
||||
namespace routing
|
||||
{
|
||||
AbsentRegionsFinder::AbsentRegionsFinder(CountryFileGetterFn const & countryFileGetter,
|
||||
LocalFileCheckerFn const & localFileChecker,
|
||||
std::shared_ptr<NumMwmIds> numMwmIds, DataSource & dataSource)
|
||||
: m_countryFileGetterFn(countryFileGetter)
|
||||
, m_localFileCheckerFn(localFileChecker)
|
||||
, m_numMwmIds(std::move(numMwmIds))
|
||||
, m_dataSource(dataSource)
|
||||
{
|
||||
CHECK(m_countryFileGetterFn, ());
|
||||
CHECK(m_localFileCheckerFn, ());
|
||||
}
|
||||
|
||||
void AbsentRegionsFinder::GenerateAbsentRegions(Checkpoints const & checkpoints, RouterDelegate const & delegate)
|
||||
{
|
||||
if (m_routerThread)
|
||||
{
|
||||
m_routerThread->Cancel();
|
||||
m_routerThread.reset();
|
||||
}
|
||||
|
||||
if (AreCheckpointsInSameMwm(checkpoints))
|
||||
return;
|
||||
|
||||
std::unique_ptr<RegionsRouter> router =
|
||||
std::make_unique<RegionsRouter>(m_countryFileGetterFn, m_numMwmIds, m_dataSource, delegate, checkpoints);
|
||||
|
||||
// iOS can't reuse threads. So we need to recreate the thread.
|
||||
m_routerThread = std::make_unique<threads::Thread>();
|
||||
m_routerThread->Create(std::move(router));
|
||||
}
|
||||
|
||||
void AbsentRegionsFinder::GetAbsentRegions(std::set<std::string> & regions)
|
||||
{
|
||||
regions.clear();
|
||||
GetAllRegions(regions);
|
||||
|
||||
for (auto i = regions.begin(); i != regions.end();)
|
||||
if (m_localFileCheckerFn(*i))
|
||||
i = regions.erase(i);
|
||||
else
|
||||
++i;
|
||||
}
|
||||
|
||||
void AbsentRegionsFinder::GetAllRegions(std::set<std::string> & countries)
|
||||
{
|
||||
countries.clear();
|
||||
|
||||
if (!m_routerThread)
|
||||
return;
|
||||
|
||||
m_routerThread->Join();
|
||||
|
||||
for (auto const & mwmName : m_routerThread->GetRoutineAs<RegionsRouter>()->GetMwmNames())
|
||||
if (!mwmName.empty())
|
||||
countries.emplace(mwmName);
|
||||
|
||||
m_routerThread.reset();
|
||||
}
|
||||
|
||||
bool AbsentRegionsFinder::AreCheckpointsInSameMwm(Checkpoints const & checkpoints) const
|
||||
{
|
||||
for (size_t i = 0; i < checkpoints.GetNumSubroutes(); ++i)
|
||||
if (m_countryFileGetterFn(checkpoints.GetPoint(i)) != m_countryFileGetterFn(checkpoints.GetPoint(i + 1)))
|
||||
return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
} // namespace routing
|
||||
43
libs/routing/absent_regions_finder.hpp
Normal file
43
libs/routing/absent_regions_finder.hpp
Normal file
|
|
@ -0,0 +1,43 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/regions_decl.hpp"
|
||||
#include "routing/router_delegate.hpp"
|
||||
|
||||
#include "base/thread.hpp"
|
||||
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <set>
|
||||
#include <string>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
using LocalFileCheckerFn = std::function<bool(std::string const &)>;
|
||||
|
||||
// Encapsulates generation of mwm names of absent regions needed for building the route between
|
||||
// |checkpoints|. For this purpose the new thread is used.
|
||||
class AbsentRegionsFinder
|
||||
{
|
||||
public:
|
||||
AbsentRegionsFinder(CountryFileGetterFn const & countryFileGetter, LocalFileCheckerFn const & localFileChecker,
|
||||
std::shared_ptr<NumMwmIds> numMwmIds, DataSource & dataSource);
|
||||
|
||||
// Creates new thread |m_routerThread| and starts routing in it.
|
||||
void GenerateAbsentRegions(Checkpoints const & checkpoints, RouterDelegate const & delegate);
|
||||
// Waits for the routing thread |m_routerThread| to finish and returns results from it.
|
||||
void GetAllRegions(std::set<std::string> & countries);
|
||||
// Waits for the results from GetAllRegions() and returns only regions absent on the device.
|
||||
void GetAbsentRegions(std::set<std::string> & absentCountries);
|
||||
|
||||
private:
|
||||
bool AreCheckpointsInSameMwm(Checkpoints const & checkpoints) const;
|
||||
|
||||
CountryFileGetterFn const m_countryFileGetterFn;
|
||||
LocalFileCheckerFn const m_localFileCheckerFn;
|
||||
|
||||
std::shared_ptr<NumMwmIds> m_numMwmIds;
|
||||
DataSource & m_dataSource;
|
||||
|
||||
std::unique_ptr<threads::Thread> m_routerThread;
|
||||
};
|
||||
} // namespace routing
|
||||
371
libs/routing/async_router.cpp
Normal file
371
libs/routing/async_router.cpp
Normal file
|
|
@ -0,0 +1,371 @@
|
|||
#include "routing/async_router.hpp"
|
||||
|
||||
#include "geometry/mercator.hpp"
|
||||
|
||||
#include "base/logging.hpp"
|
||||
#include "base/macros.hpp"
|
||||
#include "base/timer.hpp"
|
||||
|
||||
#include <functional>
|
||||
#include <mutex>
|
||||
#include <utility>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
using std::lock_guard, std::unique_lock;
|
||||
// ----------------------------------------------------------------------------------------------------------------------------
|
||||
|
||||
AsyncRouter::RouterDelegateProxy::RouterDelegateProxy(ReadyCallbackOwnership const & onReady,
|
||||
NeedMoreMapsCallback const & onNeedMoreMaps,
|
||||
RemoveRouteCallback const & onRemoveRoute,
|
||||
PointCheckCallback const & onPointCheck,
|
||||
ProgressCallback const & onProgress, uint32_t timeoutSec)
|
||||
: m_onReadyOwnership(onReady)
|
||||
, m_onNeedMoreMaps(onNeedMoreMaps)
|
||||
, m_onRemoveRoute(onRemoveRoute)
|
||||
, m_onPointCheck(onPointCheck)
|
||||
, m_onProgress(onProgress)
|
||||
{
|
||||
m_delegate.Reset();
|
||||
m_delegate.SetPointCheckCallback(std::bind(&RouterDelegateProxy::OnPointCheck, this, std::placeholders::_1));
|
||||
m_delegate.SetProgressCallback(std::bind(&RouterDelegateProxy::OnProgress, this, std::placeholders::_1));
|
||||
m_delegate.SetTimeout(timeoutSec);
|
||||
}
|
||||
|
||||
void AsyncRouter::RouterDelegateProxy::OnReady(std::shared_ptr<Route> route, RouterResultCode resultCode)
|
||||
{
|
||||
if (!m_onReadyOwnership)
|
||||
return;
|
||||
{
|
||||
lock_guard l(m_guard);
|
||||
if (m_delegate.IsCancelled())
|
||||
return;
|
||||
}
|
||||
m_onReadyOwnership(std::move(route), resultCode);
|
||||
}
|
||||
|
||||
void AsyncRouter::RouterDelegateProxy::OnNeedMoreMaps(uint64_t routeId, std::set<std::string> const & absentCounties)
|
||||
{
|
||||
if (!m_onNeedMoreMaps)
|
||||
return;
|
||||
{
|
||||
lock_guard l(m_guard);
|
||||
if (m_delegate.IsCancelled())
|
||||
return;
|
||||
}
|
||||
m_onNeedMoreMaps(routeId, absentCounties);
|
||||
}
|
||||
|
||||
void AsyncRouter::RouterDelegateProxy::OnRemoveRoute(RouterResultCode resultCode)
|
||||
{
|
||||
if (!m_onRemoveRoute)
|
||||
return;
|
||||
{
|
||||
lock_guard l(m_guard);
|
||||
if (m_delegate.IsCancelled())
|
||||
return;
|
||||
}
|
||||
m_onRemoveRoute(resultCode);
|
||||
}
|
||||
|
||||
void AsyncRouter::RouterDelegateProxy::Cancel()
|
||||
{
|
||||
lock_guard l(m_guard);
|
||||
m_delegate.Cancel();
|
||||
}
|
||||
|
||||
bool AsyncRouter::FindClosestProjectionToRoad(m2::PointD const & point, m2::PointD const & direction, double radius,
|
||||
EdgeProj & proj)
|
||||
{
|
||||
return m_router->FindClosestProjectionToRoad(point, direction, radius, proj);
|
||||
}
|
||||
|
||||
void AsyncRouter::RouterDelegateProxy::OnProgress(float progress)
|
||||
{
|
||||
ProgressCallback onProgress = nullptr;
|
||||
|
||||
{
|
||||
lock_guard l(m_guard);
|
||||
if (!m_onProgress)
|
||||
return;
|
||||
|
||||
if (m_delegate.IsCancelled())
|
||||
return;
|
||||
|
||||
onProgress = m_onProgress;
|
||||
GetPlatform().RunTask(Platform::Thread::Gui, [onProgress, progress]() { onProgress(progress); });
|
||||
}
|
||||
}
|
||||
|
||||
void AsyncRouter::RouterDelegateProxy::OnPointCheck(ms::LatLon const & pt)
|
||||
{
|
||||
#ifdef SHOW_ROUTE_DEBUG_MARKS
|
||||
PointCheckCallback onPointCheck = nullptr;
|
||||
m2::PointD point;
|
||||
{
|
||||
lock_guard<mutex> l(m_guard);
|
||||
CHECK(m_onPointCheck, ());
|
||||
|
||||
if (m_delegate.IsCancelled())
|
||||
return;
|
||||
|
||||
onPointCheck = m_onPointCheck;
|
||||
point = mercator::FromLatLon(pt);
|
||||
}
|
||||
|
||||
GetPlatform().RunTask(Platform::Thread::Gui, [onPointCheck, point]() { onPointCheck(point); });
|
||||
#else
|
||||
UNUSED_VALUE(pt);
|
||||
#endif
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------------------------------
|
||||
|
||||
AsyncRouter::AsyncRouter(PointCheckCallback const & pointCheckCallback)
|
||||
: m_threadExit(false)
|
||||
, m_hasRequest(false)
|
||||
, m_clearState(false)
|
||||
, m_pointCheckCallback(pointCheckCallback)
|
||||
{
|
||||
m_thread = threads::SimpleThread(&AsyncRouter::ThreadFunc, this);
|
||||
}
|
||||
|
||||
AsyncRouter::~AsyncRouter()
|
||||
{
|
||||
{
|
||||
unique_lock ul(m_guard);
|
||||
|
||||
ResetDelegate();
|
||||
|
||||
m_threadExit = true;
|
||||
m_threadCondVar.notify_one();
|
||||
}
|
||||
|
||||
m_thread.join();
|
||||
}
|
||||
|
||||
void AsyncRouter::SetRouter(std::unique_ptr<IRouter> && router, std::unique_ptr<AbsentRegionsFinder> && finder)
|
||||
{
|
||||
unique_lock ul(m_guard);
|
||||
|
||||
ResetDelegate();
|
||||
|
||||
m_router = std::move(router);
|
||||
m_absentRegionsFinder = std::move(finder);
|
||||
}
|
||||
|
||||
void AsyncRouter::CalculateRoute(Checkpoints const & checkpoints, m2::PointD const & direction, bool adjustToPrevRoute,
|
||||
ReadyCallbackOwnership const & readyCallback,
|
||||
NeedMoreMapsCallback const & needMoreMapsCallback,
|
||||
RemoveRouteCallback const & removeRouteCallback,
|
||||
ProgressCallback const & progressCallback, uint32_t timeoutSec)
|
||||
{
|
||||
unique_lock ul(m_guard);
|
||||
|
||||
m_checkpoints = checkpoints;
|
||||
m_startDirection = direction;
|
||||
m_adjustToPrevRoute = adjustToPrevRoute;
|
||||
|
||||
ResetDelegate();
|
||||
|
||||
m_delegateProxy = std::make_shared<RouterDelegateProxy>(readyCallback, needMoreMapsCallback, removeRouteCallback,
|
||||
m_pointCheckCallback, progressCallback, timeoutSec);
|
||||
|
||||
m_hasRequest = true;
|
||||
m_threadCondVar.notify_one();
|
||||
}
|
||||
|
||||
void AsyncRouter::SetGuidesTracks(GuidesTracks && guides)
|
||||
{
|
||||
unique_lock ul(m_guard);
|
||||
m_guides = std::move(guides);
|
||||
}
|
||||
|
||||
void AsyncRouter::ClearState()
|
||||
{
|
||||
unique_lock ul(m_guard);
|
||||
|
||||
m_clearState = true;
|
||||
m_threadCondVar.notify_one();
|
||||
|
||||
ResetDelegate();
|
||||
}
|
||||
|
||||
// static
|
||||
void AsyncRouter::LogCode(RouterResultCode code, double const elapsedSec)
|
||||
{
|
||||
switch (code)
|
||||
{
|
||||
case RouterResultCode::StartPointNotFound: LOG(LWARNING, ("Can't find start or end node")); break;
|
||||
case RouterResultCode::EndPointNotFound: LOG(LWARNING, ("Can't find end point node")); break;
|
||||
case RouterResultCode::PointsInDifferentMWM: LOG(LWARNING, ("Points are in different MWMs")); break;
|
||||
case RouterResultCode::RouteNotFound: LOG(LWARNING, ("Route not found")); break;
|
||||
case RouterResultCode::RouteFileNotExist: LOG(LWARNING, ("There is no routing file")); break;
|
||||
case RouterResultCode::NeedMoreMaps:
|
||||
LOG(LINFO, ("Routing can find a better way with additional maps, elapsed seconds:", elapsedSec));
|
||||
break;
|
||||
case RouterResultCode::Cancelled: LOG(LINFO, ("Route calculation cancelled, elapsed seconds:", elapsedSec)); break;
|
||||
case RouterResultCode::NoError: LOG(LINFO, ("Route found, elapsed seconds:", elapsedSec)); break;
|
||||
case RouterResultCode::NoCurrentPosition: LOG(LINFO, ("No current position")); break;
|
||||
case RouterResultCode::InconsistentMWMandRoute: LOG(LINFO, ("Inconsistent mwm and route")); break;
|
||||
case RouterResultCode::InternalError: LOG(LINFO, ("Internal error")); break;
|
||||
case RouterResultCode::FileTooOld: LOG(LINFO, ("File too old")); break;
|
||||
case RouterResultCode::IntermediatePointNotFound: LOG(LWARNING, ("Can't find intermediate point node")); break;
|
||||
case RouterResultCode::TransitRouteNotFoundNoNetwork:
|
||||
LOG(LWARNING, ("No transit route is found because there's no transit network in the mwm of "
|
||||
"the route point"));
|
||||
break;
|
||||
case RouterResultCode::TransitRouteNotFoundTooLongPedestrian:
|
||||
LOG(LWARNING, ("No transit route is found because pedestrian way is too long"));
|
||||
break;
|
||||
case RouterResultCode::RouteNotFoundRedressRouteError:
|
||||
LOG(LWARNING, ("Route not found because of a redress route error"));
|
||||
break;
|
||||
case RouterResultCode::HasWarnings: LOG(LINFO, ("Route has warnings, elapsed seconds:", elapsedSec)); break;
|
||||
}
|
||||
}
|
||||
|
||||
void AsyncRouter::ResetDelegate()
|
||||
{
|
||||
if (m_delegateProxy)
|
||||
{
|
||||
m_delegateProxy->Cancel();
|
||||
m_delegateProxy.reset();
|
||||
}
|
||||
}
|
||||
|
||||
void AsyncRouter::ThreadFunc()
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
{
|
||||
unique_lock ul(m_guard);
|
||||
m_threadCondVar.wait(ul, [this]() { return m_threadExit || m_hasRequest || m_clearState; });
|
||||
|
||||
if (m_clearState && m_router)
|
||||
{
|
||||
m_router->ClearState();
|
||||
m_clearState = false;
|
||||
}
|
||||
|
||||
if (m_threadExit)
|
||||
break;
|
||||
|
||||
if (!m_hasRequest)
|
||||
continue;
|
||||
}
|
||||
|
||||
CalculateRoute();
|
||||
}
|
||||
}
|
||||
|
||||
void AsyncRouter::CalculateRoute()
|
||||
{
|
||||
Checkpoints checkpoints;
|
||||
std::shared_ptr<RouterDelegateProxy> delegateProxy;
|
||||
m2::PointD startDirection;
|
||||
bool adjustToPrevRoute = false;
|
||||
std::shared_ptr<AbsentRegionsFinder> absentRegionsFinder;
|
||||
std::shared_ptr<IRouter> router;
|
||||
uint64_t routeId = 0;
|
||||
std::string routerName;
|
||||
|
||||
{
|
||||
unique_lock ul(m_guard);
|
||||
|
||||
bool hasRequest = m_hasRequest;
|
||||
m_hasRequest = false;
|
||||
if (!hasRequest)
|
||||
return;
|
||||
if (!m_router)
|
||||
return;
|
||||
if (!m_delegateProxy)
|
||||
return;
|
||||
|
||||
checkpoints = m_checkpoints;
|
||||
startDirection = m_startDirection;
|
||||
adjustToPrevRoute = m_adjustToPrevRoute;
|
||||
delegateProxy = m_delegateProxy;
|
||||
router = m_router;
|
||||
absentRegionsFinder = m_absentRegionsFinder;
|
||||
routeId = ++m_routeCounter;
|
||||
routerName = router->GetName();
|
||||
router->SetGuides(std::move(m_guides));
|
||||
m_guides.clear();
|
||||
}
|
||||
|
||||
auto route = std::make_shared<Route>(router->GetName(), routeId);
|
||||
RouterResultCode code;
|
||||
|
||||
base::Timer timer;
|
||||
double elapsedSec = 0.0;
|
||||
|
||||
try
|
||||
{
|
||||
LOG(LINFO, ("Calculating the route of direct length", checkpoints.GetSummaryLengthBetweenPointsMeters(),
|
||||
"m. checkpoints:", checkpoints, "startDirection:", startDirection, "router name:", router->GetName()));
|
||||
|
||||
if (absentRegionsFinder)
|
||||
absentRegionsFinder->GenerateAbsentRegions(checkpoints, delegateProxy->GetDelegate());
|
||||
|
||||
// Run basic request.
|
||||
code = router->CalculateRoute(checkpoints, startDirection, adjustToPrevRoute, delegateProxy->GetDelegate(), *route);
|
||||
router->SetGuides({});
|
||||
elapsedSec = timer.ElapsedSeconds(); // routing time
|
||||
LogCode(code, elapsedSec);
|
||||
LOG(LINFO, ("ETA:", route->GetTotalTimeSec(), "sec."));
|
||||
}
|
||||
catch (RootException const & e)
|
||||
{
|
||||
code = RouterResultCode::InternalError;
|
||||
LOG(LERROR, ("Exception happened while calculating route:", e.Msg()));
|
||||
// Note. After call of this method |route| should be used only on ui thread.
|
||||
// And |route| should stop using on routing background thread, in this method.
|
||||
GetPlatform().RunTask(Platform::Thread::Gui,
|
||||
[delegateProxy, route, code]() { delegateProxy->OnReady(route, code); });
|
||||
return;
|
||||
}
|
||||
|
||||
// Draw route without waiting network latency.
|
||||
if (code == RouterResultCode::NoError)
|
||||
{
|
||||
// Note. After call of this method |route| should be used only on ui thread.
|
||||
// And |route| should stop using on routing background thread, in this method.
|
||||
GetPlatform().RunTask(Platform::Thread::Gui,
|
||||
[delegateProxy, route, code]() { delegateProxy->OnReady(route, code); });
|
||||
}
|
||||
|
||||
bool const needAbsentRegions = (code != RouterResultCode::Cancelled && route->GetRouterId() != "ruler-router");
|
||||
|
||||
std::set<std::string> absent;
|
||||
if (needAbsentRegions)
|
||||
{
|
||||
if (absentRegionsFinder)
|
||||
absentRegionsFinder->GetAbsentRegions(absent);
|
||||
|
||||
absent.insert(route->GetAbsentCountries().cbegin(), route->GetAbsentCountries().cend());
|
||||
if (!absent.empty())
|
||||
{
|
||||
code = RouterResultCode::NeedMoreMaps;
|
||||
LOG(LDEBUG, ("Absent regions:", absent));
|
||||
}
|
||||
}
|
||||
|
||||
elapsedSec = timer.ElapsedSeconds(); // routing time + absents fetch time
|
||||
LogCode(code, elapsedSec);
|
||||
|
||||
// Call callback only if we have some new data.
|
||||
if (code != RouterResultCode::NoError)
|
||||
{
|
||||
if (code == RouterResultCode::NeedMoreMaps)
|
||||
{
|
||||
GetPlatform().RunTask(Platform::Thread::Gui,
|
||||
[delegateProxy, routeId, absent]() { delegateProxy->OnNeedMoreMaps(routeId, absent); });
|
||||
}
|
||||
else
|
||||
{
|
||||
GetPlatform().RunTask(Platform::Thread::Gui, [delegateProxy, code]() { delegateProxy->OnRemoveRoute(code); });
|
||||
}
|
||||
}
|
||||
}
|
||||
} // namespace routing
|
||||
125
libs/routing/async_router.hpp
Normal file
125
libs/routing/async_router.hpp
Normal file
|
|
@ -0,0 +1,125 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/absent_regions_finder.hpp"
|
||||
#include "routing/checkpoints.hpp"
|
||||
#include "routing/route.hpp"
|
||||
#include "routing/router.hpp"
|
||||
#include "routing/router_delegate.hpp"
|
||||
#include "routing/routing_callbacks.hpp"
|
||||
|
||||
#include "platform/platform.hpp"
|
||||
|
||||
#include "base/thread.hpp"
|
||||
|
||||
#include <condition_variable>
|
||||
#include <cstdint>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <set>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
|
||||
/// Dispatches a route calculation on a worker thread
|
||||
class AsyncRouter final
|
||||
{
|
||||
public:
|
||||
/// AsyncRouter is a wrapper class to run routing routines in the different thread
|
||||
AsyncRouter(PointCheckCallback const & pointCheckCallback);
|
||||
~AsyncRouter();
|
||||
|
||||
/// Sets a synchronous router, current route calculation will be cancelled
|
||||
/// @param router pointer to a router implementation
|
||||
/// @param finder pointer to a router for generated absent wmwms.
|
||||
void SetRouter(std::unique_ptr<IRouter> && router, std::unique_ptr<AbsentRegionsFinder> && finder);
|
||||
|
||||
/// Main method to calculate new route from startPt to finalPt with start direction
|
||||
/// Processed result will be passed to callback. Callback will be called at the GUI thread.
|
||||
///
|
||||
/// @param checkpoints start, finish and intermadiate points
|
||||
/// @param direction start direction for routers with high cost of the turnarounds
|
||||
/// @param adjustToPrevRoute adjust route to the previous one if possible
|
||||
/// @param readyCallback function to return routing result
|
||||
/// @param progressCallback function to update the router progress
|
||||
/// @param timeoutSec timeout to cancel routing. 0 is infinity.
|
||||
// @TODO(bykoianko) Gather |readyCallback|, |needMoreMapsCallback| and |removeRouteCallback|
|
||||
// to one delegate. No need to add |progressCallback| to the delegate.
|
||||
void CalculateRoute(Checkpoints const & checkpoints, m2::PointD const & direction, bool adjustToPrevRoute,
|
||||
ReadyCallbackOwnership const & readyCallback, NeedMoreMapsCallback const & needMoreMapsCallback,
|
||||
RemoveRouteCallback const & removeRouteCallback, ProgressCallback const & progressCallback,
|
||||
uint32_t timeoutSec = RouterDelegate::kNoTimeout);
|
||||
|
||||
void SetGuidesTracks(GuidesTracks && guides);
|
||||
/// Interrupt routing and clear buffers
|
||||
void ClearState();
|
||||
|
||||
bool FindClosestProjectionToRoad(m2::PointD const & point, m2::PointD const & direction, double radius,
|
||||
EdgeProj & proj);
|
||||
|
||||
private:
|
||||
/// Worker thread function
|
||||
void ThreadFunc();
|
||||
|
||||
/// This function is called in worker thread
|
||||
void CalculateRoute();
|
||||
void ResetDelegate();
|
||||
static void LogCode(RouterResultCode code, double const elapsedSec);
|
||||
|
||||
/// Blocks callbacks when routing has been cancelled
|
||||
class RouterDelegateProxy
|
||||
{
|
||||
public:
|
||||
RouterDelegateProxy(ReadyCallbackOwnership const & onReady, NeedMoreMapsCallback const & onNeedMoreMaps,
|
||||
RemoveRouteCallback const & onRemoveRoute, PointCheckCallback const & onPointCheck,
|
||||
ProgressCallback const & onProgress, uint32_t timeoutSec);
|
||||
|
||||
void OnReady(std::shared_ptr<Route> route, RouterResultCode resultCode);
|
||||
void OnNeedMoreMaps(uint64_t routeId, std::set<std::string> const & absentCounties);
|
||||
void OnRemoveRoute(RouterResultCode resultCode);
|
||||
void Cancel();
|
||||
|
||||
RouterDelegate const & GetDelegate() const { return m_delegate; }
|
||||
|
||||
private:
|
||||
void OnProgress(float progress);
|
||||
void OnPointCheck(ms::LatLon const & pt);
|
||||
|
||||
std::mutex m_guard;
|
||||
ReadyCallbackOwnership const m_onReadyOwnership;
|
||||
// |m_onNeedMoreMaps| may be called after |m_onReadyOwnership| if
|
||||
// - it's possible to build route only if to load some maps
|
||||
// - there's a faster route, but it's necessary to load some more maps to build it
|
||||
NeedMoreMapsCallback const m_onNeedMoreMaps;
|
||||
RemoveRouteCallback const m_onRemoveRoute;
|
||||
PointCheckCallback const m_onPointCheck;
|
||||
ProgressCallback const m_onProgress;
|
||||
RouterDelegate m_delegate;
|
||||
};
|
||||
|
||||
private:
|
||||
std::mutex m_guard;
|
||||
|
||||
/// Thread which executes routing calculation
|
||||
threads::SimpleThread m_thread;
|
||||
std::condition_variable m_threadCondVar;
|
||||
bool m_threadExit;
|
||||
bool m_hasRequest;
|
||||
|
||||
/// Current request parameters
|
||||
bool m_clearState = false;
|
||||
Checkpoints m_checkpoints;
|
||||
GuidesTracks m_guides;
|
||||
|
||||
m2::PointD m_startDirection = m2::PointD::Zero();
|
||||
bool m_adjustToPrevRoute = false;
|
||||
std::shared_ptr<RouterDelegateProxy> m_delegateProxy;
|
||||
std::shared_ptr<AbsentRegionsFinder> m_absentRegionsFinder;
|
||||
std::shared_ptr<IRouter> m_router;
|
||||
|
||||
PointCheckCallback const m_pointCheckCallback;
|
||||
uint64_t m_routeCounter = 0;
|
||||
};
|
||||
} // namespace routing
|
||||
820
libs/routing/base/astar_algorithm.hpp
Normal file
820
libs/routing/base/astar_algorithm.hpp
Normal file
|
|
@ -0,0 +1,820 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/base/astar_graph.hpp"
|
||||
#include "routing/base/astar_vertex_data.hpp"
|
||||
#include "routing/base/astar_weight.hpp"
|
||||
#include "routing/base/routing_result.hpp"
|
||||
|
||||
#include "base/assert.hpp"
|
||||
#include "base/cancellable.hpp"
|
||||
#include "base/logging.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <functional>
|
||||
#include <iostream>
|
||||
#include <map>
|
||||
#include <optional>
|
||||
#include <queue>
|
||||
#include <type_traits>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "3party/skarupke/bytell_hash_map.hpp"
|
||||
|
||||
namespace routing
|
||||
{
|
||||
namespace astar
|
||||
{
|
||||
|
||||
struct DefaultVisitor
|
||||
{
|
||||
template <class State, class Vertex>
|
||||
void operator()(State const &, Vertex const &) const
|
||||
{}
|
||||
};
|
||||
|
||||
struct DefaultLengthChecker
|
||||
{
|
||||
template <class Weight>
|
||||
bool operator()(Weight const &) const
|
||||
{
|
||||
return true;
|
||||
}
|
||||
};
|
||||
} // namespace astar
|
||||
|
||||
template <typename Vertex, typename Edge, typename Weight>
|
||||
class AStarAlgorithm
|
||||
{
|
||||
public:
|
||||
using Graph = AStarGraph<Vertex, Edge, Weight>;
|
||||
|
||||
enum class Result
|
||||
{
|
||||
OK,
|
||||
NoPath,
|
||||
Cancelled
|
||||
};
|
||||
|
||||
friend std::ostream & operator<<(std::ostream & os, Result const & result)
|
||||
{
|
||||
switch (result)
|
||||
{
|
||||
case Result::OK: os << "OK"; break;
|
||||
case Result::NoPath: os << "NoPath"; break;
|
||||
case Result::Cancelled: os << "Cancelled"; break;
|
||||
}
|
||||
return os;
|
||||
}
|
||||
|
||||
struct ParamsBase
|
||||
{
|
||||
ParamsBase(Graph & graph, Vertex const & startVertex, Vertex const & finalVertex,
|
||||
base::Cancellable const & cancellable)
|
||||
: m_graph(graph)
|
||||
, m_startVertex(startVertex)
|
||||
, m_finalVertex(finalVertex)
|
||||
, m_cancellable(cancellable)
|
||||
{}
|
||||
|
||||
Graph & m_graph;
|
||||
Weight const m_weightEpsilon = m_graph.GetAStarWeightEpsilon();
|
||||
Vertex const m_startVertex;
|
||||
// Used for FindPath, FindPathBidirectional.
|
||||
Vertex const m_finalVertex;
|
||||
// Used for AdjustRoute.
|
||||
base::Cancellable const & m_cancellable;
|
||||
std::function<bool(Weight, Weight)> m_badReducedWeight = [](Weight, Weight) { return true; };
|
||||
};
|
||||
|
||||
// |LengthChecker| callback used to check path length from start/finish to the edge (including the
|
||||
// edge itself) before adding the edge to AStar queue. Can be used to clip some path which does
|
||||
// not meet restrictions.
|
||||
template <typename Visitor = astar::DefaultVisitor, typename LengthChecker = astar::DefaultLengthChecker>
|
||||
struct Params : public ParamsBase
|
||||
{
|
||||
Params(Graph & graph, Vertex const & startVertex, Vertex const & finalVertex, base::Cancellable const & cancellable,
|
||||
Visitor && onVisitedVertexCallback = astar::DefaultVisitor(),
|
||||
LengthChecker && checkLengthCallback = astar::DefaultLengthChecker())
|
||||
: ParamsBase(graph, startVertex, finalVertex, cancellable)
|
||||
, m_onVisitedVertexCallback(std::forward<Visitor>(onVisitedVertexCallback))
|
||||
, m_checkLengthCallback(std::forward<LengthChecker>(checkLengthCallback))
|
||||
{}
|
||||
|
||||
Visitor m_onVisitedVertexCallback;
|
||||
LengthChecker const m_checkLengthCallback;
|
||||
};
|
||||
|
||||
template <typename LengthChecker = astar::DefaultLengthChecker>
|
||||
struct ParamsForTests : public ParamsBase
|
||||
{
|
||||
ParamsForTests(Graph & graph, Vertex const & startVertex, Vertex const & finalVertex,
|
||||
LengthChecker && checkLengthCallback = astar::DefaultLengthChecker())
|
||||
: ParamsBase(graph, startVertex, finalVertex, m_dummy)
|
||||
, m_checkLengthCallback(std::forward<LengthChecker>(checkLengthCallback))
|
||||
{}
|
||||
|
||||
astar::DefaultVisitor const m_onVisitedVertexCallback{};
|
||||
LengthChecker const m_checkLengthCallback;
|
||||
|
||||
private:
|
||||
base::Cancellable const m_dummy;
|
||||
};
|
||||
|
||||
class Context final
|
||||
{
|
||||
public:
|
||||
Context(Graph & graph) : m_graph(graph) { m_graph.SetAStarParents(true /* forward */, m_parents); }
|
||||
|
||||
~Context() { m_graph.DropAStarParents(); }
|
||||
|
||||
void Clear()
|
||||
{
|
||||
m_distanceMap.clear();
|
||||
m_parents.clear();
|
||||
}
|
||||
|
||||
bool HasDistance(Vertex const & vertex) const { return m_distanceMap.find(vertex) != m_distanceMap.cend(); }
|
||||
|
||||
Weight GetDistance(Vertex const & vertex) const
|
||||
{
|
||||
auto const & it = m_distanceMap.find(vertex);
|
||||
if (it == m_distanceMap.cend())
|
||||
return kInfiniteDistance;
|
||||
|
||||
return it->second;
|
||||
}
|
||||
|
||||
void SetDistance(Vertex const & vertex, Weight const & distance) { m_distanceMap[vertex] = distance; }
|
||||
|
||||
void SetParent(Vertex const & parent, Vertex const & child) { m_parents[parent] = child; }
|
||||
|
||||
bool HasParent(Vertex const & child) const { return m_parents.count(child) != 0; }
|
||||
|
||||
Vertex const & GetParent(Vertex const & child) const
|
||||
{
|
||||
auto const it = m_parents.find(child);
|
||||
CHECK(it != m_parents.cend(), ("Can not find parent of child:", child));
|
||||
return it->second;
|
||||
}
|
||||
|
||||
typename Graph::Parents & GetParents() { return m_parents; }
|
||||
|
||||
void ReconstructPath(Vertex const & v, std::vector<Vertex> & path) const;
|
||||
|
||||
private:
|
||||
Graph & m_graph;
|
||||
ska::bytell_hash_map<Vertex, Weight> m_distanceMap;
|
||||
typename Graph::Parents m_parents;
|
||||
};
|
||||
|
||||
// VisitVertex returns true: wave will continue
|
||||
// VisitVertex returns false: wave will stop
|
||||
template <typename VisitVertex, typename AdjustEdgeWeight, typename FilterStates, typename ReducedToRealLength>
|
||||
void PropagateWave(Graph & graph, Vertex const & startVertex, VisitVertex && visitVertex,
|
||||
AdjustEdgeWeight && adjustEdgeWeight, FilterStates && filterStates,
|
||||
ReducedToRealLength && reducedToRealLength, Context & context) const;
|
||||
|
||||
template <typename VisitVertex>
|
||||
void PropagateWave(Graph & graph, Vertex const & startVertex, VisitVertex && visitVertex, Context & context) const;
|
||||
|
||||
template <typename P>
|
||||
Result FindPath(P & params, RoutingResult<Vertex, Weight> & result) const;
|
||||
|
||||
/// Fetch routes until \a emitter returns false.
|
||||
template <class P, class Emitter>
|
||||
Result FindPathBidirectionalEx(P & params, Emitter && emitter) const;
|
||||
|
||||
template <class P>
|
||||
Result FindPathBidirectional(P & params, RoutingResult<Vertex, Weight> & result) const
|
||||
{
|
||||
return FindPathBidirectionalEx(params, [&result](RoutingResult<Vertex, Weight> && res)
|
||||
{
|
||||
// Fetch first (best) route and stop.
|
||||
result = std::move(res);
|
||||
return true;
|
||||
});
|
||||
}
|
||||
|
||||
// Adjust route to the previous one.
|
||||
// Expects |params.m_checkLengthCallback| to check wave propagation limit.
|
||||
template <typename P>
|
||||
typename AStarAlgorithm<Vertex, Edge, Weight>::Result AdjustRoute(P & params, std::vector<Edge> const & prevRoute,
|
||||
RoutingResult<Vertex, Weight> & result) const;
|
||||
|
||||
private:
|
||||
// Periodicity of switching a wave of bidirectional algorithm.
|
||||
static uint32_t constexpr kQueueSwitchPeriod = 128;
|
||||
|
||||
// Precision of comparison weights.
|
||||
static Weight constexpr kZeroDistance = GetAStarWeightZero<Weight>();
|
||||
static Weight constexpr kInfiniteDistance = GetAStarWeightMax<Weight>();
|
||||
|
||||
class PeriodicPollCancellable final
|
||||
{
|
||||
public:
|
||||
PeriodicPollCancellable(base::Cancellable const & cancellable) : m_cancellable(cancellable) {}
|
||||
|
||||
bool IsCancelled()
|
||||
{
|
||||
// Periodicity of checking is cancellable cancelled.
|
||||
uint32_t constexpr kPeriod = 128;
|
||||
return count++ % kPeriod == 0 && m_cancellable.IsCancelled();
|
||||
}
|
||||
|
||||
private:
|
||||
base::Cancellable const & m_cancellable;
|
||||
uint32_t count = 0;
|
||||
};
|
||||
|
||||
// State is what is going to be put in the priority queue. See the
|
||||
// comment for FindPath for more information.
|
||||
struct State
|
||||
{
|
||||
State(Vertex const & vertex, Weight const & distance, Weight const & heuristic)
|
||||
: vertex(vertex)
|
||||
, distance(distance)
|
||||
, heuristic(heuristic)
|
||||
{}
|
||||
State(Vertex const & vertex, Weight const & distance) : State(vertex, distance, Weight()) {}
|
||||
|
||||
inline bool operator>(State const & rhs) const { return distance > rhs.distance; }
|
||||
|
||||
Vertex vertex;
|
||||
Weight distance;
|
||||
Weight heuristic;
|
||||
};
|
||||
|
||||
// BidirectionalStepContext keeps all the information that is needed to
|
||||
// search starting from one of the two directions. Its main
|
||||
// purpose is to make the code that changes directions more readable.
|
||||
struct BidirectionalStepContext
|
||||
{
|
||||
using Parents = typename Graph::Parents;
|
||||
|
||||
BidirectionalStepContext(bool forward, Vertex const & startVertex, Vertex const & finalVertex, Graph & graph)
|
||||
: forward(forward)
|
||||
, startVertex(startVertex)
|
||||
, finalVertex(finalVertex)
|
||||
, graph(graph)
|
||||
{
|
||||
bestVertex = forward ? startVertex : finalVertex;
|
||||
pS = ConsistentHeuristic(bestVertex);
|
||||
graph.SetAStarParents(forward, parent);
|
||||
}
|
||||
|
||||
~BidirectionalStepContext() { graph.DropAStarParents(); }
|
||||
|
||||
Weight TopDistance() const
|
||||
{
|
||||
ASSERT(!queue.empty(), ());
|
||||
return bestDistance.at(queue.top().vertex);
|
||||
}
|
||||
|
||||
// p_f(v) = 0.5*(π_f(v) - π_r(v))
|
||||
// p_r(v) = 0.5*(π_r(v) - π_f(v))
|
||||
// p_r(v) + p_f(v) = const. This condition is called consistence.
|
||||
//
|
||||
// Note. Adding constant terms to p_f(v) or p_r(v) does
|
||||
// not violate the consistence so a common choice is
|
||||
// p_f(v) = 0.5*(π_f(v) - π_r(v)) + 0.5*π_r(t)
|
||||
// p_r(v) = 0.5*(π_r(v) - π_f(v)) + 0.5*π_f(s)
|
||||
// which leads to p_f(t) = 0 and p_r(s) = 0.
|
||||
// However, with constants set to zero understanding
|
||||
// particular routes when debugging turned out to be easier.
|
||||
Weight ConsistentHeuristic(Vertex const & v) const
|
||||
{
|
||||
auto const piF = graph.HeuristicCostEstimate(v, finalVertex);
|
||||
auto const piR = graph.HeuristicCostEstimate(v, startVertex);
|
||||
if (forward)
|
||||
{
|
||||
/// @todo careful: with this "return" here and below in the Backward case
|
||||
/// the heuristic becomes inconsistent but still seems to work.
|
||||
/// return HeuristicCostEstimate(v, finalVertex);
|
||||
return 0.5 * (piF - piR);
|
||||
}
|
||||
else
|
||||
{
|
||||
// return HeuristicCostEstimate(v, startVertex);
|
||||
return 0.5 * (piR - piF);
|
||||
}
|
||||
}
|
||||
|
||||
bool ExistsStateWithBetterDistance(State const & state, Weight const & eps = Weight(0.0)) const
|
||||
{
|
||||
auto const it = bestDistance.find(state.vertex);
|
||||
return it != bestDistance.end() && state.distance > it->second - eps;
|
||||
}
|
||||
|
||||
void UpdateDistance(State const & state) { bestDistance.insert_or_assign(state.vertex, state.distance); }
|
||||
|
||||
std::optional<Weight> GetDistance(Vertex const & vertex) const
|
||||
{
|
||||
auto const it = bestDistance.find(vertex);
|
||||
return it != bestDistance.cend() ? std::optional<Weight>(it->second) : std::nullopt;
|
||||
}
|
||||
|
||||
void UpdateParent(Vertex const & to, Vertex const & from) { parent.insert_or_assign(to, from); }
|
||||
|
||||
void GetAdjacencyList(State const & state, typename Graph::EdgeListT & adj)
|
||||
{
|
||||
auto const realDistance = state.distance + pS - state.heuristic;
|
||||
astar::VertexData const data(state.vertex, realDistance);
|
||||
if (forward)
|
||||
graph.GetOutgoingEdgesList(data, adj);
|
||||
else
|
||||
graph.GetIngoingEdgesList(data, adj);
|
||||
}
|
||||
|
||||
Parents & GetParents() { return parent; }
|
||||
|
||||
std::optional<Vertex> GetParent(Vertex const & vertex) const
|
||||
{
|
||||
auto const it = parent.find(vertex);
|
||||
return it != parent.cend() ? std::optional<Vertex>(it->second) : std::nullopt;
|
||||
}
|
||||
|
||||
bool const forward;
|
||||
Vertex const & startVertex;
|
||||
Vertex const & finalVertex;
|
||||
Graph & graph;
|
||||
|
||||
std::priority_queue<State, std::vector<State>, std::greater<State>> queue;
|
||||
ska::bytell_hash_map<Vertex, Weight> bestDistance;
|
||||
Parents parent;
|
||||
Vertex bestVertex;
|
||||
|
||||
Weight pS;
|
||||
};
|
||||
|
||||
static void ReconstructPath(Vertex const & v, typename BidirectionalStepContext::Parents const & parent,
|
||||
std::vector<Vertex> & path);
|
||||
static void ReconstructPathBidirectional(Vertex const & v, Vertex const & w,
|
||||
typename BidirectionalStepContext::Parents const & parentV,
|
||||
typename BidirectionalStepContext::Parents const & parentW,
|
||||
std::vector<Vertex> & path);
|
||||
};
|
||||
|
||||
template <typename Vertex, typename Edge, typename Weight>
|
||||
constexpr Weight AStarAlgorithm<Vertex, Edge, Weight>::kInfiniteDistance;
|
||||
template <typename Vertex, typename Edge, typename Weight>
|
||||
constexpr Weight AStarAlgorithm<Vertex, Edge, Weight>::kZeroDistance;
|
||||
|
||||
template <typename Vertex, typename Edge, typename Weight>
|
||||
template <typename VisitVertex, typename AdjustEdgeWeight, typename FilterStates, typename ReducedToFullLength>
|
||||
void AStarAlgorithm<Vertex, Edge, Weight>::PropagateWave(Graph & graph, Vertex const & startVertex,
|
||||
VisitVertex && visitVertex,
|
||||
AdjustEdgeWeight && adjustEdgeWeight,
|
||||
FilterStates && filterStates,
|
||||
ReducedToFullLength && reducedToFullLength,
|
||||
AStarAlgorithm<Vertex, Edge, Weight>::Context & context) const
|
||||
{
|
||||
auto const epsilon = graph.GetAStarWeightEpsilon();
|
||||
|
||||
context.Clear();
|
||||
|
||||
std::priority_queue<State, std::vector<State>, std::greater<State>> queue;
|
||||
|
||||
context.SetDistance(startVertex, kZeroDistance);
|
||||
queue.push(State(startVertex, kZeroDistance));
|
||||
|
||||
typename Graph::EdgeListT adj;
|
||||
|
||||
while (!queue.empty())
|
||||
{
|
||||
State const stateV = queue.top();
|
||||
queue.pop();
|
||||
|
||||
if (stateV.distance > context.GetDistance(stateV.vertex))
|
||||
continue;
|
||||
|
||||
if (!visitVertex(stateV.vertex))
|
||||
return;
|
||||
|
||||
astar::VertexData const vertexData(stateV.vertex, reducedToFullLength(stateV));
|
||||
graph.GetOutgoingEdgesList(vertexData, adj);
|
||||
for (auto const & edge : adj)
|
||||
{
|
||||
State stateW(edge.GetTarget(), kZeroDistance);
|
||||
if (stateV.vertex == stateW.vertex)
|
||||
continue;
|
||||
|
||||
auto const edgeWeight = adjustEdgeWeight(stateV.vertex, edge);
|
||||
auto const newReducedDist = stateV.distance + edgeWeight;
|
||||
|
||||
if (newReducedDist >= context.GetDistance(stateW.vertex) - epsilon)
|
||||
continue;
|
||||
|
||||
stateW.distance = newReducedDist;
|
||||
|
||||
if (!filterStates(stateW))
|
||||
continue;
|
||||
|
||||
context.SetDistance(stateW.vertex, newReducedDist);
|
||||
context.SetParent(stateW.vertex, stateV.vertex);
|
||||
queue.push(stateW);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
template <typename Vertex, typename Edge, typename Weight>
|
||||
template <typename VisitVertex>
|
||||
void AStarAlgorithm<Vertex, Edge, Weight>::PropagateWave(Graph & graph, Vertex const & startVertex,
|
||||
VisitVertex && visitVertex,
|
||||
AStarAlgorithm<Vertex, Edge, Weight>::Context & context) const
|
||||
{
|
||||
auto const adjustEdgeWeight = [](Vertex const & /* vertex */, Edge const & edge) { return edge.GetWeight(); };
|
||||
auto const filterStates = [](State const & /* state */) { return true; };
|
||||
auto const reducedToRealLength = [](State const & state) { return state.distance; };
|
||||
PropagateWave(graph, startVertex, visitVertex, adjustEdgeWeight, filterStates, reducedToRealLength, context);
|
||||
}
|
||||
|
||||
// This implementation is based on the view that the A* algorithm
|
||||
// is equivalent to Dijkstra's algorithm that is run on a reweighted
|
||||
// version of the graph. If an edge (v, w) has length l(v, w), its reduced
|
||||
// cost is l_r(v, w) = l(v, w) + pi(w) - pi(v), where pi() is any function
|
||||
// that ensures l_r(v, w) >= 0 for every edge. We set pi() to calculate
|
||||
// the shortest possible distance to a goal node, and this is a common
|
||||
// heuristic that people use in A*.
|
||||
//
|
||||
// For a detailed explanation of the reweighting idea refer to David Eppstein's
|
||||
// post "Reweighting a graph for faster shortest paths" at
|
||||
// https://11011110.github.io/blog/2008/04/03/reweighting-graph-for.html
|
||||
//
|
||||
// For more information on A* refer to
|
||||
// http://research.microsoft.com/pubs/154937/soda05.pdf
|
||||
// http://www.cs.princeton.edu/courses/archive/spr06/cos423/Handouts/EPP%20shortest%20path%20algorithms.pdf
|
||||
|
||||
template <typename Vertex, typename Edge, typename Weight>
|
||||
template <typename P>
|
||||
typename AStarAlgorithm<Vertex, Edge, Weight>::Result AStarAlgorithm<Vertex, Edge, Weight>::FindPath(
|
||||
P & params, RoutingResult<Vertex, Weight> & result) const
|
||||
{
|
||||
auto const epsilon = params.m_weightEpsilon;
|
||||
|
||||
result.Clear();
|
||||
|
||||
auto & graph = params.m_graph;
|
||||
auto const & finalVertex = params.m_finalVertex;
|
||||
auto const & startVertex = params.m_startVertex;
|
||||
|
||||
Context context(graph);
|
||||
PeriodicPollCancellable periodicCancellable(params.m_cancellable);
|
||||
Result resultCode = Result::NoPath;
|
||||
|
||||
auto const heuristicDiff = [&](Vertex const & vertexFrom, Vertex const & vertexTo)
|
||||
{ return graph.HeuristicCostEstimate(vertexFrom, finalVertex) - graph.HeuristicCostEstimate(vertexTo, finalVertex); };
|
||||
|
||||
auto const fullToReducedLength = [&](Vertex const & vertexFrom, Vertex const & vertexTo, Weight const length)
|
||||
{ return length - heuristicDiff(vertexFrom, vertexTo); };
|
||||
|
||||
auto const reducedToFullLength = [&](Vertex const & vertexFrom, Vertex const & vertexTo, Weight const reducedLength)
|
||||
{ return reducedLength + heuristicDiff(vertexFrom, vertexTo); };
|
||||
|
||||
auto visitVertex = [&](Vertex const & vertex)
|
||||
{
|
||||
if (periodicCancellable.IsCancelled())
|
||||
{
|
||||
resultCode = Result::Cancelled;
|
||||
return false;
|
||||
}
|
||||
|
||||
params.m_onVisitedVertexCallback(vertex, finalVertex);
|
||||
|
||||
if (vertex == finalVertex)
|
||||
{
|
||||
resultCode = Result::OK;
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
};
|
||||
|
||||
auto const adjustEdgeWeight = [&](Vertex const & vertexV, Edge const & edge)
|
||||
{
|
||||
auto const reducedWeight = fullToReducedLength(vertexV, edge.GetTarget(), edge.GetWeight());
|
||||
|
||||
CHECK_GREATER_OR_EQUAL(reducedWeight, -epsilon, ("Invariant violated."));
|
||||
|
||||
return std::max(reducedWeight, kZeroDistance);
|
||||
};
|
||||
|
||||
auto const reducedToRealLength = [&](State const & state)
|
||||
{ return reducedToFullLength(startVertex, state.vertex, state.distance); };
|
||||
|
||||
auto const filterStates = [&](State const & state)
|
||||
{ return params.m_checkLengthCallback(reducedToRealLength(state)); };
|
||||
|
||||
PropagateWave(graph, startVertex, visitVertex, adjustEdgeWeight, filterStates, reducedToRealLength, context);
|
||||
|
||||
if (resultCode == Result::OK)
|
||||
{
|
||||
context.ReconstructPath(finalVertex, result.m_path);
|
||||
result.m_distance = reducedToFullLength(startVertex, finalVertex, context.GetDistance(finalVertex));
|
||||
|
||||
if (!params.m_checkLengthCallback(result.m_distance))
|
||||
resultCode = Result::NoPath;
|
||||
}
|
||||
|
||||
return resultCode;
|
||||
}
|
||||
|
||||
template <typename Vertex, typename Edge, typename Weight>
|
||||
template <class P, class Emitter>
|
||||
typename AStarAlgorithm<Vertex, Edge, Weight>::Result AStarAlgorithm<Vertex, Edge, Weight>::FindPathBidirectionalEx(
|
||||
P & params, Emitter && emitter) const
|
||||
{
|
||||
auto const epsilon = params.m_weightEpsilon;
|
||||
auto & graph = params.m_graph;
|
||||
auto const & finalVertex = params.m_finalVertex;
|
||||
auto const & startVertex = params.m_startVertex;
|
||||
|
||||
BidirectionalStepContext forward(true /* forward */, startVertex, finalVertex, graph);
|
||||
BidirectionalStepContext backward(false /* forward */, startVertex, finalVertex, graph);
|
||||
|
||||
auto & forwardParents = forward.GetParents();
|
||||
auto & backwardParents = backward.GetParents();
|
||||
|
||||
bool foundAnyPath = false;
|
||||
Weight bestPathReducedLength = kZeroDistance;
|
||||
Weight bestPathRealLength = kZeroDistance;
|
||||
|
||||
forward.UpdateDistance(State(startVertex, kZeroDistance));
|
||||
forward.queue.push(State(startVertex, kZeroDistance, forward.ConsistentHeuristic(startVertex)));
|
||||
|
||||
backward.UpdateDistance(State(finalVertex, kZeroDistance));
|
||||
backward.queue.push(State(finalVertex, kZeroDistance, backward.ConsistentHeuristic(finalVertex)));
|
||||
|
||||
// To use the search code both for backward and forward directions
|
||||
// we keep the pointers to everything related to the search in the
|
||||
// 'current' and 'next' directions. Swapping these pointers indicates
|
||||
// changing the end we are searching from.
|
||||
BidirectionalStepContext * cur = &forward;
|
||||
BidirectionalStepContext * nxt = &backward;
|
||||
|
||||
auto const EmitResult = [cur, nxt, &bestPathRealLength, &emitter]()
|
||||
{
|
||||
// No problem if length check fails, but we still emit the result.
|
||||
// Happens with "transit" route because of length, haven't seen with regular car route.
|
||||
// ASSERT(params.m_checkLengthCallback(bestPathRealLength), ());
|
||||
|
||||
RoutingResult<Vertex, Weight> result;
|
||||
ReconstructPathBidirectional(cur->bestVertex, nxt->bestVertex, cur->parent, nxt->parent, result.m_path);
|
||||
result.m_distance = bestPathRealLength;
|
||||
if (!cur->forward)
|
||||
reverse(result.m_path.begin(), result.m_path.end());
|
||||
|
||||
return emitter(std::move(result));
|
||||
};
|
||||
|
||||
typename Graph::EdgeListT adj;
|
||||
|
||||
// It is not necessary to check emptiness for both queues here
|
||||
// because if we have not found a path by the time one of the
|
||||
// queues is exhausted, we never will.
|
||||
uint32_t steps = 0;
|
||||
PeriodicPollCancellable periodicCancellable(params.m_cancellable);
|
||||
|
||||
while (!cur->queue.empty() && !nxt->queue.empty())
|
||||
{
|
||||
++steps;
|
||||
|
||||
if (periodicCancellable.IsCancelled())
|
||||
return Result::Cancelled;
|
||||
|
||||
if (steps % kQueueSwitchPeriod == 0)
|
||||
std::swap(cur, nxt);
|
||||
|
||||
if (foundAnyPath)
|
||||
{
|
||||
auto const curTop = cur->TopDistance();
|
||||
auto const nxtTop = nxt->TopDistance();
|
||||
|
||||
// The intuition behind this is that we cannot obtain a path shorter
|
||||
// than the left side of the inequality because that is how any path we find
|
||||
// will look like (see comment for curPathReducedLength below).
|
||||
// We do not yet have the proof that we will not miss a good path by doing so.
|
||||
|
||||
// The shortest reduced path corresponds to the shortest real path
|
||||
// because the heuristic we use are consistent.
|
||||
// It would be a mistake to make a decision based on real path lengths because
|
||||
// several top states in a priority queue may have equal reduced path lengths and
|
||||
// different real path lengths.
|
||||
|
||||
if (curTop + nxtTop >= bestPathReducedLength - epsilon)
|
||||
{
|
||||
if (EmitResult())
|
||||
return Result::OK;
|
||||
else
|
||||
foundAnyPath = false;
|
||||
}
|
||||
}
|
||||
|
||||
State const stateV = cur->queue.top();
|
||||
cur->queue.pop();
|
||||
|
||||
if (cur->ExistsStateWithBetterDistance(stateV))
|
||||
continue;
|
||||
|
||||
auto const endV = cur->forward ? cur->finalVertex : cur->startVertex;
|
||||
params.m_onVisitedVertexCallback(std::make_pair(stateV, cur), endV);
|
||||
|
||||
cur->GetAdjacencyList(stateV, adj);
|
||||
auto const & pV = stateV.heuristic;
|
||||
for (auto const & edge : adj)
|
||||
{
|
||||
State stateW(edge.GetTarget(), kZeroDistance);
|
||||
|
||||
if (stateV.vertex == stateW.vertex)
|
||||
continue;
|
||||
|
||||
auto const weight = edge.GetWeight();
|
||||
auto const pW = cur->ConsistentHeuristic(stateW.vertex);
|
||||
auto const reducedWeight = weight + pW - pV;
|
||||
|
||||
if (reducedWeight < -epsilon && params.m_badReducedWeight(reducedWeight, std::max(pW, pV)))
|
||||
{
|
||||
// Break in Debug, log in Release and safe continue: std::max(reducedWeight, kZeroDistance).
|
||||
LOG(LERROR,
|
||||
("Invariant violated for:", "v =", stateV.vertex, "w =", stateW.vertex, "reduced weight =", reducedWeight));
|
||||
}
|
||||
|
||||
stateW.distance = stateV.distance + std::max(reducedWeight, kZeroDistance);
|
||||
|
||||
auto const fullLength = weight + stateV.distance + cur->pS - pV;
|
||||
if (!params.m_checkLengthCallback(fullLength))
|
||||
continue;
|
||||
|
||||
if (cur->ExistsStateWithBetterDistance(stateW, epsilon))
|
||||
continue;
|
||||
|
||||
stateW.heuristic = pW;
|
||||
cur->UpdateDistance(stateW);
|
||||
cur->UpdateParent(stateW.vertex, stateV.vertex);
|
||||
|
||||
if (auto op = nxt->GetDistance(stateW.vertex); op)
|
||||
{
|
||||
auto const & distW = *op;
|
||||
// Reduced length that the path we've just found has in the original graph:
|
||||
// find the reduced length of the path's parts in the reduced forward and backward graphs.
|
||||
auto const curPathReducedLength = stateW.distance + distW;
|
||||
// No epsilon here: it is ok to overshoot slightly.
|
||||
if ((!foundAnyPath || bestPathReducedLength > curPathReducedLength) &&
|
||||
graph.AreWavesConnectible(forwardParents, stateW.vertex, backwardParents))
|
||||
{
|
||||
bestPathReducedLength = curPathReducedLength;
|
||||
|
||||
bestPathRealLength = stateV.distance + weight + distW;
|
||||
bestPathRealLength += cur->pS - pV;
|
||||
bestPathRealLength += nxt->pS - nxt->ConsistentHeuristic(stateW.vertex);
|
||||
|
||||
foundAnyPath = true;
|
||||
cur->bestVertex = stateV.vertex;
|
||||
nxt->bestVertex = stateW.vertex;
|
||||
}
|
||||
}
|
||||
|
||||
if (stateW.vertex != endV)
|
||||
cur->queue.push(stateW);
|
||||
}
|
||||
}
|
||||
|
||||
if (foundAnyPath)
|
||||
{
|
||||
(void)EmitResult();
|
||||
return Result::OK;
|
||||
}
|
||||
|
||||
return Result::NoPath;
|
||||
}
|
||||
|
||||
template <typename Vertex, typename Edge, typename Weight>
|
||||
template <typename P>
|
||||
typename AStarAlgorithm<Vertex, Edge, Weight>::Result AStarAlgorithm<Vertex, Edge, Weight>::AdjustRoute(
|
||||
P & params, std::vector<Edge> const & prevRoute, RoutingResult<Vertex, Weight> & result) const
|
||||
{
|
||||
auto & graph = params.m_graph;
|
||||
auto const & startVertex = params.m_startVertex;
|
||||
CHECK(!prevRoute.empty(), ());
|
||||
|
||||
result.Clear();
|
||||
|
||||
bool wasCancelled = false;
|
||||
auto minDistance = kInfiniteDistance;
|
||||
Vertex returnVertex;
|
||||
|
||||
std::map<Vertex, Weight> remainingDistances;
|
||||
auto remainingDistance = kZeroDistance;
|
||||
|
||||
for (auto it = prevRoute.crbegin(); it != prevRoute.crend(); ++it)
|
||||
{
|
||||
remainingDistances[it->GetTarget()] = remainingDistance;
|
||||
remainingDistance += it->GetWeight();
|
||||
}
|
||||
|
||||
Context context(graph);
|
||||
PeriodicPollCancellable periodicCancellable(params.m_cancellable);
|
||||
|
||||
auto visitVertex = [&](Vertex const & vertex)
|
||||
{
|
||||
if (periodicCancellable.IsCancelled())
|
||||
{
|
||||
wasCancelled = true;
|
||||
return false;
|
||||
}
|
||||
|
||||
params.m_onVisitedVertexCallback(startVertex, vertex);
|
||||
|
||||
auto it = remainingDistances.find(vertex);
|
||||
if (it != remainingDistances.cend())
|
||||
{
|
||||
auto const fullDistance = context.GetDistance(vertex) + it->second;
|
||||
if (fullDistance < minDistance)
|
||||
{
|
||||
minDistance = fullDistance;
|
||||
returnVertex = vertex;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
};
|
||||
|
||||
auto const adjustEdgeWeight = [](Vertex const & /* vertex */, Edge const & edge) { return edge.GetWeight(); };
|
||||
|
||||
auto const filterStates = [&](State const & state) { return params.m_checkLengthCallback(state.distance); };
|
||||
|
||||
auto const reducedToRealLength = [&](State const & state) { return state.distance; };
|
||||
|
||||
PropagateWave(graph, startVertex, visitVertex, adjustEdgeWeight, filterStates, reducedToRealLength, context);
|
||||
if (wasCancelled)
|
||||
return Result::Cancelled;
|
||||
|
||||
if (minDistance == kInfiniteDistance)
|
||||
return Result::NoPath;
|
||||
|
||||
context.ReconstructPath(returnVertex, result.m_path);
|
||||
|
||||
// Append remaining route.
|
||||
bool found = false;
|
||||
for (size_t i = 0; i < prevRoute.size(); ++i)
|
||||
{
|
||||
if (prevRoute[i].GetTarget() == returnVertex)
|
||||
{
|
||||
for (size_t j = i + 1; j < prevRoute.size(); ++j)
|
||||
result.m_path.push_back(prevRoute[j].GetTarget());
|
||||
|
||||
found = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
CHECK(found, ("Can't find", returnVertex, ", prev:", prevRoute.size(), ", adjust:", result.m_path.size()));
|
||||
|
||||
auto const & it = remainingDistances.find(returnVertex);
|
||||
CHECK(it != remainingDistances.end(), ());
|
||||
result.m_distance = context.GetDistance(returnVertex) + it->second;
|
||||
return Result::OK;
|
||||
}
|
||||
|
||||
// static
|
||||
template <typename Vertex, typename Edge, typename Weight>
|
||||
void AStarAlgorithm<Vertex, Edge, Weight>::ReconstructPath(Vertex const & v,
|
||||
typename BidirectionalStepContext::Parents const & parent,
|
||||
std::vector<Vertex> & path)
|
||||
{
|
||||
path.clear();
|
||||
Vertex cur = v;
|
||||
while (true)
|
||||
{
|
||||
path.push_back(cur);
|
||||
auto const it = parent.find(cur);
|
||||
if (it == parent.end())
|
||||
break;
|
||||
cur = it->second;
|
||||
}
|
||||
|
||||
std::reverse(path.begin(), path.end());
|
||||
}
|
||||
|
||||
// static
|
||||
template <typename Vertex, typename Edge, typename Weight>
|
||||
void AStarAlgorithm<Vertex, Edge, Weight>::ReconstructPathBidirectional(
|
||||
Vertex const & v, Vertex const & w, typename BidirectionalStepContext::Parents const & parentV,
|
||||
typename BidirectionalStepContext::Parents const & parentW, std::vector<Vertex> & path)
|
||||
{
|
||||
std::vector<Vertex> pathV;
|
||||
ReconstructPath(v, parentV, pathV);
|
||||
std::vector<Vertex> pathW;
|
||||
ReconstructPath(w, parentW, pathW);
|
||||
path.clear();
|
||||
path.reserve(pathV.size() + pathW.size());
|
||||
path.insert(path.end(), pathV.begin(), pathV.end());
|
||||
path.insert(path.end(), pathW.rbegin(), pathW.rend());
|
||||
}
|
||||
|
||||
template <typename Vertex, typename Edge, typename Weight>
|
||||
void AStarAlgorithm<Vertex, Edge, Weight>::Context::ReconstructPath(Vertex const & v, std::vector<Vertex> & path) const
|
||||
{
|
||||
AStarAlgorithm<Vertex, Edge, Weight>::ReconstructPath(v, m_parents, path);
|
||||
}
|
||||
} // namespace routing
|
||||
63
libs/routing/base/astar_graph.hpp
Normal file
63
libs/routing/base/astar_graph.hpp
Normal file
|
|
@ -0,0 +1,63 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/base/astar_vertex_data.hpp"
|
||||
#include "routing/base/astar_weight.hpp"
|
||||
#include "routing/base/small_list.hpp"
|
||||
|
||||
#include "base/buffer_vector.hpp"
|
||||
|
||||
#include <map>
|
||||
#include <vector>
|
||||
|
||||
#include "3party/skarupke/bytell_hash_map.hpp"
|
||||
|
||||
namespace routing
|
||||
{
|
||||
template <typename VertexType, typename EdgeType, typename WeightType>
|
||||
class AStarGraph
|
||||
{
|
||||
public:
|
||||
using Vertex = VertexType;
|
||||
using Edge = EdgeType;
|
||||
using Weight = WeightType;
|
||||
|
||||
using Parents = ska::bytell_hash_map<Vertex, Vertex>;
|
||||
|
||||
using EdgeListT = SmallList<Edge>;
|
||||
|
||||
virtual Weight HeuristicCostEstimate(Vertex const & from, Vertex const & to) = 0;
|
||||
|
||||
virtual void GetOutgoingEdgesList(astar::VertexData<Vertex, Weight> const & vertexData, EdgeListT & edges) = 0;
|
||||
virtual void GetIngoingEdgesList(astar::VertexData<Vertex, Weight> const & vertexData, EdgeListT & edges) = 0;
|
||||
|
||||
virtual void SetAStarParents(bool forward, Parents & parents);
|
||||
virtual void DropAStarParents();
|
||||
virtual bool AreWavesConnectible(Parents & forwardParents, Vertex const & commonVertex, Parents & backwardParents);
|
||||
|
||||
virtual Weight GetAStarWeightEpsilon();
|
||||
|
||||
virtual ~AStarGraph() = default;
|
||||
};
|
||||
|
||||
template <typename VertexType, typename EdgeType, typename WeightType>
|
||||
void AStarGraph<VertexType, EdgeType, WeightType>::SetAStarParents(bool /* forward */, Parents & /* parents */)
|
||||
{}
|
||||
|
||||
template <typename VertexType, typename EdgeType, typename WeightType>
|
||||
void AStarGraph<VertexType, EdgeType, WeightType>::DropAStarParents()
|
||||
{}
|
||||
|
||||
template <typename VertexType, typename EdgeType, typename WeightType>
|
||||
bool AStarGraph<VertexType, EdgeType, WeightType>::AreWavesConnectible(AStarGraph::Parents & /* forwardParents */,
|
||||
Vertex const & /* commonVertex */,
|
||||
AStarGraph::Parents & /* backwardParents */)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
template <typename VertexType, typename EdgeType, typename WeightType>
|
||||
WeightType AStarGraph<VertexType, EdgeType, WeightType>::GetAStarWeightEpsilon()
|
||||
{
|
||||
return routing::GetAStarWeightEpsilon<WeightType>();
|
||||
}
|
||||
} // namespace routing
|
||||
132
libs/routing/base/astar_progress.cpp
Normal file
132
libs/routing/base/astar_progress.cpp
Normal file
|
|
@ -0,0 +1,132 @@
|
|||
#include "routing/base/astar_progress.hpp"
|
||||
|
||||
#include "coding/point_coding.hpp"
|
||||
|
||||
#include "base/assert.hpp"
|
||||
#include "base/math.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <iterator>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
|
||||
double AStarSubProgress::CalcDistance(ms::LatLon const & from, ms::LatLon const & to)
|
||||
{
|
||||
// Use very simple, naive and fast distance for progress, because the honest ms::DistanceOnEarth
|
||||
// is very time-consuming and *on top* of routing calculation, taking into account frequent progress calls.
|
||||
// We even added distance cache into RoadGeometry.
|
||||
return fabs(from.m_lon - to.m_lon) + fabs(from.m_lat - to.m_lat);
|
||||
}
|
||||
|
||||
AStarSubProgress::AStarSubProgress(ms::LatLon const & start, ms::LatLon const & finish, double contributionCoef)
|
||||
: m_contributionCoef(contributionCoef)
|
||||
, m_startPoint(start)
|
||||
, m_finishPoint(finish)
|
||||
{
|
||||
ASSERT_GREATER(m_contributionCoef, 0.0, ());
|
||||
|
||||
m_fullDistance = CalcDistance(start, finish);
|
||||
m_forwardDistance = m_fullDistance;
|
||||
m_backwardDistance = m_fullDistance;
|
||||
}
|
||||
|
||||
AStarSubProgress::AStarSubProgress(double contributionCoef) : m_contributionCoef(contributionCoef)
|
||||
{
|
||||
ASSERT_NOT_EQUAL(m_contributionCoef, 0.0, ());
|
||||
}
|
||||
|
||||
double AStarSubProgress::UpdateProgress(ms::LatLon const & current, ms::LatLon const & finish)
|
||||
{
|
||||
// to avoid 0/0
|
||||
if (m_fullDistance < kMwmPointAccuracy)
|
||||
return m_currentProgress;
|
||||
|
||||
double const dist = CalcDistance(current, finish);
|
||||
double & toUpdate = finish == m_finishPoint ? m_forwardDistance : m_backwardDistance;
|
||||
|
||||
toUpdate = std::min(toUpdate, dist);
|
||||
|
||||
double part = 2.0 - (m_forwardDistance + m_backwardDistance) / m_fullDistance;
|
||||
part = math::Clamp(part, 0.0, 1.0);
|
||||
double const newProgress = m_contributionCoef * part;
|
||||
|
||||
m_currentProgress = std::max(newProgress, m_currentProgress);
|
||||
return m_currentProgress;
|
||||
}
|
||||
|
||||
double AStarSubProgress::UpdateProgress(double subSubProgressValue)
|
||||
{
|
||||
return m_currentProgress + m_contributionCoef * subSubProgressValue;
|
||||
}
|
||||
|
||||
void AStarSubProgress::Flush(double progress)
|
||||
{
|
||||
m_currentProgress += m_contributionCoef * progress;
|
||||
}
|
||||
|
||||
double AStarSubProgress::GetMaxContribution() const
|
||||
{
|
||||
return m_contributionCoef;
|
||||
}
|
||||
|
||||
// AStarProgress -------------------------------------------------------------------------
|
||||
|
||||
// static
|
||||
double const AStarProgress::kMaxPercent = 99.0;
|
||||
|
||||
AStarProgress::AStarProgress()
|
||||
{
|
||||
m_subProgresses.emplace_back(AStarSubProgress(kMaxPercent / 100.0));
|
||||
}
|
||||
|
||||
AStarProgress::~AStarProgress()
|
||||
{
|
||||
CHECK(std::next(m_subProgresses.begin()) == m_subProgresses.end(), ());
|
||||
}
|
||||
|
||||
void AStarProgress::AppendSubProgress(AStarSubProgress const & subProgress)
|
||||
{
|
||||
m_subProgresses.emplace_back(subProgress);
|
||||
}
|
||||
|
||||
void AStarProgress::DropLastSubProgress()
|
||||
{
|
||||
CHECK(!m_subProgresses.empty(), ());
|
||||
m_subProgresses.pop_back();
|
||||
}
|
||||
|
||||
void AStarProgress::PushAndDropLastSubProgress()
|
||||
{
|
||||
CHECK_GREATER(m_subProgresses.size(), 1, ());
|
||||
auto prevLast = std::prev(std::prev(m_subProgresses.end()));
|
||||
|
||||
prevLast->Flush(m_subProgresses.back().GetMaxContribution());
|
||||
DropLastSubProgress();
|
||||
}
|
||||
|
||||
double AStarProgress::UpdateProgress(ms::LatLon const & current, ms::LatLon const & end)
|
||||
{
|
||||
double const newProgress = UpdateProgressImpl(m_subProgresses.begin(), current, end) * 100.0;
|
||||
m_lastPercentValue = std::max(m_lastPercentValue, newProgress);
|
||||
|
||||
ASSERT(m_lastPercentValue < kMaxPercent || AlmostEqualAbs(m_lastPercentValue, kMaxPercent, 1e-5 /* eps */),
|
||||
(m_lastPercentValue, kMaxPercent));
|
||||
|
||||
m_lastPercentValue = std::min(m_lastPercentValue, kMaxPercent);
|
||||
return m_lastPercentValue;
|
||||
}
|
||||
|
||||
double AStarProgress::GetLastPercent() const
|
||||
{
|
||||
return m_lastPercentValue;
|
||||
}
|
||||
|
||||
double AStarProgress::UpdateProgressImpl(ListItem subProgress, ms::LatLon const & current, ms::LatLon const & end)
|
||||
{
|
||||
if (std::next(subProgress) != m_subProgresses.end())
|
||||
return subProgress->UpdateProgress(UpdateProgressImpl(std::next(subProgress), current, end));
|
||||
|
||||
return subProgress->UpdateProgress(current, end);
|
||||
}
|
||||
} // namespace routing
|
||||
63
libs/routing/base/astar_progress.hpp
Normal file
63
libs/routing/base/astar_progress.hpp
Normal file
|
|
@ -0,0 +1,63 @@
|
|||
#pragma once
|
||||
|
||||
#include "geometry/latlon.hpp"
|
||||
|
||||
#include <list>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
class AStarSubProgress
|
||||
{
|
||||
public:
|
||||
AStarSubProgress(ms::LatLon const & start, ms::LatLon const & finish, double contributionCoef);
|
||||
|
||||
explicit AStarSubProgress(double contributionCoef);
|
||||
|
||||
double UpdateProgress(ms::LatLon const & current, ms::LatLon const & finish);
|
||||
/// \brief Some |AStarSubProgress| could have their own subProgresses (next item in
|
||||
/// AStarProgress::m_subProgresses after current), in order to return true progress
|
||||
/// back to the upper level of subProgresses, we should do progress backpropagation of
|
||||
/// subProgress of current subProgress - |subSubProgressValue|
|
||||
double UpdateProgress(double subSubProgressValue);
|
||||
|
||||
void Flush(double progress);
|
||||
double GetMaxContribution() const;
|
||||
|
||||
private:
|
||||
static double CalcDistance(ms::LatLon const & from, ms::LatLon const & to);
|
||||
|
||||
double m_currentProgress = 0.0;
|
||||
double m_contributionCoef = 0.0;
|
||||
double m_fullDistance = 0.0;
|
||||
double m_forwardDistance = 0.0;
|
||||
double m_backwardDistance = 0.0;
|
||||
|
||||
ms::LatLon m_startPoint;
|
||||
ms::LatLon m_finishPoint;
|
||||
};
|
||||
|
||||
class AStarProgress
|
||||
{
|
||||
public:
|
||||
static double const kMaxPercent;
|
||||
|
||||
AStarProgress();
|
||||
~AStarProgress();
|
||||
|
||||
double GetLastPercent() const;
|
||||
void PushAndDropLastSubProgress();
|
||||
void DropLastSubProgress();
|
||||
void AppendSubProgress(AStarSubProgress const & subProgress);
|
||||
double UpdateProgress(ms::LatLon const & current, ms::LatLon const & end);
|
||||
|
||||
private:
|
||||
using ListItem = std::list<AStarSubProgress>::iterator;
|
||||
|
||||
double UpdateProgressImpl(ListItem subProgress, ms::LatLon const & current, ms::LatLon const & end);
|
||||
|
||||
// This value is in range: [0, 1].
|
||||
double m_lastPercentValue = 0.0;
|
||||
|
||||
std::list<AStarSubProgress> m_subProgresses;
|
||||
};
|
||||
} // namespace routing
|
||||
15
libs/routing/base/astar_vertex_data.hpp
Normal file
15
libs/routing/base/astar_vertex_data.hpp
Normal file
|
|
@ -0,0 +1,15 @@
|
|||
#pragma once
|
||||
|
||||
namespace routing::astar
|
||||
{
|
||||
template <typename Vertex, typename Weight>
|
||||
struct VertexData
|
||||
{
|
||||
static VertexData Zero() { return VertexData(Vertex(), Weight()); }
|
||||
|
||||
VertexData(Vertex const & vertex, Weight const & realDistance) : m_vertex(vertex), m_realDistance(realDistance) {}
|
||||
|
||||
Vertex m_vertex;
|
||||
Weight m_realDistance;
|
||||
};
|
||||
} // namespace routing::astar
|
||||
40
libs/routing/base/astar_weight.hpp
Normal file
40
libs/routing/base/astar_weight.hpp
Normal file
|
|
@ -0,0 +1,40 @@
|
|||
#pragma once
|
||||
|
||||
#include "base/assert.hpp"
|
||||
|
||||
#include <limits>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
template <typename Weight>
|
||||
constexpr Weight GetAStarWeightZero();
|
||||
|
||||
template <>
|
||||
constexpr double GetAStarWeightZero<double>()
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
// Precision of comparison weights.
|
||||
template <typename Weight>
|
||||
constexpr Weight GetAStarWeightEpsilon()
|
||||
{
|
||||
UNREACHABLE();
|
||||
}
|
||||
|
||||
template <>
|
||||
constexpr double GetAStarWeightEpsilon<double>()
|
||||
{
|
||||
return 1e-6;
|
||||
}
|
||||
|
||||
template <typename Weight>
|
||||
constexpr Weight GetAStarWeightMax();
|
||||
|
||||
template <>
|
||||
constexpr double GetAStarWeightMax<double>()
|
||||
{
|
||||
return std::numeric_limits<double>::max();
|
||||
}
|
||||
|
||||
} // namespace routing
|
||||
96
libs/routing/base/bfs.hpp
Normal file
96
libs/routing/base/bfs.hpp
Normal file
|
|
@ -0,0 +1,96 @@
|
|||
#pragma once
|
||||
|
||||
#include "base/scope_guard.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <functional>
|
||||
#include <map>
|
||||
#include <queue>
|
||||
#include <set>
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
template <typename Graph>
|
||||
class BFS
|
||||
{
|
||||
public:
|
||||
using Vertex = typename Graph::Vertex;
|
||||
using Edge = typename Graph::Edge;
|
||||
using Weight = typename Graph::Weight;
|
||||
|
||||
struct State
|
||||
{
|
||||
State(Vertex const & v, Vertex const & p) : m_vertex(v), m_parent(p) {}
|
||||
|
||||
Vertex m_vertex;
|
||||
Vertex m_parent;
|
||||
};
|
||||
|
||||
explicit BFS(Graph & graph) : m_graph(graph) {}
|
||||
|
||||
void Run(Vertex const & start, bool isOutgoing, std::function<bool(State const &)> && onVisitCallback);
|
||||
|
||||
std::vector<Vertex> ReconstructPath(Vertex from, bool reverse);
|
||||
|
||||
private:
|
||||
Graph & m_graph;
|
||||
std::map<Vertex, Vertex> m_parents;
|
||||
};
|
||||
|
||||
template <typename Graph>
|
||||
void BFS<Graph>::Run(Vertex const & start, bool isOutgoing, std::function<bool(State const &)> && onVisitCallback)
|
||||
{
|
||||
m_parents.clear();
|
||||
|
||||
m_parents.emplace(start, start);
|
||||
SCOPE_GUARD(removeStart, [&]() { m_parents.erase(start); });
|
||||
|
||||
std::queue<Vertex> queue;
|
||||
queue.emplace(start);
|
||||
|
||||
typename Graph::EdgeListT edges;
|
||||
while (!queue.empty())
|
||||
{
|
||||
Vertex const current = queue.front();
|
||||
queue.pop();
|
||||
|
||||
m_graph.GetEdgesList(current, isOutgoing, edges);
|
||||
|
||||
for (auto const & edge : edges)
|
||||
{
|
||||
Vertex const & child = edge.GetTarget();
|
||||
if (m_parents.count(child) != 0)
|
||||
continue;
|
||||
|
||||
State const state(child, current);
|
||||
if (!onVisitCallback(state))
|
||||
continue;
|
||||
|
||||
m_parents.emplace(child, current);
|
||||
queue.emplace(child);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
template <typename Graph>
|
||||
auto BFS<Graph>::ReconstructPath(Vertex from, bool reverse) -> std::vector<Vertex>
|
||||
{
|
||||
std::vector<Vertex> result;
|
||||
auto it = m_parents.find(from);
|
||||
while (it != m_parents.end())
|
||||
{
|
||||
result.emplace_back(from);
|
||||
from = it->second;
|
||||
it = m_parents.find(from);
|
||||
}
|
||||
|
||||
// Here stored path in inverse order (from end to begin).
|
||||
result.emplace_back(from);
|
||||
|
||||
if (!reverse)
|
||||
std::reverse(result.begin(), result.end());
|
||||
|
||||
return result;
|
||||
}
|
||||
} // namespace routing
|
||||
240
libs/routing/base/followed_polyline.cpp
Normal file
240
libs/routing/base/followed_polyline.cpp
Normal file
|
|
@ -0,0 +1,240 @@
|
|||
#include "followed_polyline.hpp"
|
||||
|
||||
#include "base/assert.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <limits>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
using namespace std;
|
||||
using Iter = routing::FollowedPolyline::Iter;
|
||||
|
||||
Iter FollowedPolyline::Begin() const
|
||||
{
|
||||
ASSERT(IsValid(), ());
|
||||
return Iter(m_poly.Front(), 0);
|
||||
}
|
||||
|
||||
Iter FollowedPolyline::End() const
|
||||
{
|
||||
ASSERT(IsValid(), ());
|
||||
return Iter(m_poly.Back(), m_poly.GetSize() - 1);
|
||||
}
|
||||
|
||||
Iter FollowedPolyline::GetIterToIndex(size_t index) const
|
||||
{
|
||||
ASSERT(IsValid(), ());
|
||||
ASSERT_LESS(index, m_poly.GetSize(), ());
|
||||
|
||||
return Iter(m_poly.GetPoint(index), index);
|
||||
}
|
||||
|
||||
double FollowedPolyline::GetDistanceM(Iter const & it1, Iter const & it2) const
|
||||
{
|
||||
ASSERT(IsValid(), ());
|
||||
ASSERT(it1.IsValid() && it2.IsValid(), ());
|
||||
ASSERT_LESS_OR_EQUAL(it1.m_ind, it2.m_ind, ());
|
||||
ASSERT_LESS(it1.m_ind, m_poly.GetSize(), ());
|
||||
ASSERT_LESS(it2.m_ind, m_poly.GetSize(), ());
|
||||
|
||||
if (it1.m_ind == it2.m_ind)
|
||||
return mercator::DistanceOnEarth(it1.m_pt, it2.m_pt);
|
||||
|
||||
return (mercator::DistanceOnEarth(it1.m_pt, m_poly.GetPoint(it1.m_ind + 1)) + m_segDistance[it2.m_ind - 1] -
|
||||
m_segDistance[it1.m_ind] + mercator::DistanceOnEarth(m_poly.GetPoint(it2.m_ind), it2.m_pt));
|
||||
}
|
||||
|
||||
double FollowedPolyline::GetTotalDistanceMeters() const
|
||||
{
|
||||
if (!IsValid())
|
||||
{
|
||||
ASSERT(IsValid(), ());
|
||||
return 0;
|
||||
}
|
||||
return m_segDistance.back();
|
||||
}
|
||||
|
||||
double FollowedPolyline::GetDistanceFromStartMeters() const
|
||||
{
|
||||
if (!IsValid() || !m_current.IsValid())
|
||||
{
|
||||
ASSERT(IsValid(), ());
|
||||
ASSERT(m_current.IsValid(), ());
|
||||
return 0;
|
||||
}
|
||||
|
||||
return (m_current.m_ind > 0 ? m_segDistance[m_current.m_ind - 1] : 0.0) +
|
||||
mercator::DistanceOnEarth(m_current.m_pt, m_poly.GetPoint(m_current.m_ind));
|
||||
}
|
||||
|
||||
double FollowedPolyline::GetDistanceToEndMeters() const
|
||||
{
|
||||
return GetTotalDistanceMeters() - GetDistanceFromStartMeters();
|
||||
}
|
||||
|
||||
void FollowedPolyline::Swap(FollowedPolyline & rhs)
|
||||
{
|
||||
m_poly.Swap(rhs.m_poly);
|
||||
m_segDistance.swap(rhs.m_segDistance);
|
||||
m_segProj.swap(rhs.m_segProj);
|
||||
swap(m_current, rhs.m_current);
|
||||
swap(m_nextCheckpointIndex, rhs.m_nextCheckpointIndex);
|
||||
}
|
||||
|
||||
void FollowedPolyline::Update()
|
||||
{
|
||||
size_t n = m_poly.GetSize();
|
||||
ASSERT_GREATER(n, 1, ());
|
||||
--n;
|
||||
|
||||
m_segDistance.clear();
|
||||
m_segDistance.reserve(n);
|
||||
m_segProj.clear();
|
||||
m_segProj.reserve(n);
|
||||
|
||||
double dist = 0.0;
|
||||
for (size_t i = 0; i < n; ++i)
|
||||
{
|
||||
m2::PointD const & p1 = m_poly.GetPoint(i);
|
||||
m2::PointD const & p2 = m_poly.GetPoint(i + 1);
|
||||
|
||||
dist += mercator::DistanceOnEarth(p1, p2);
|
||||
|
||||
m_segDistance.emplace_back(dist);
|
||||
m_segProj.emplace_back(p1, p2);
|
||||
}
|
||||
|
||||
m_current = Iter(m_poly.Front(), 0);
|
||||
}
|
||||
|
||||
bool FollowedPolyline::IsFakeSegment(size_t index) const
|
||||
{
|
||||
return binary_search(m_fakeSegmentIndexes.begin(), m_fakeSegmentIndexes.end(), index);
|
||||
}
|
||||
|
||||
template <class DistanceFn>
|
||||
Iter FollowedPolyline::GetBestProjection(m2::RectD const & posRect, DistanceFn const & distFn) const
|
||||
{
|
||||
CHECK_EQUAL(m_segProj.size() + 1, m_poly.GetSize(), ());
|
||||
// At first trying to find a projection to two closest route segments of route which is close
|
||||
// enough to |posRect| center. If |m_current| is right before intermediate point we can get |closestIter|
|
||||
// right after intermediate point (in next subroute).
|
||||
size_t const hoppingBorderIdx = min(m_segProj.size(), m_current.m_ind + 2);
|
||||
Iter const closestIter = GetClosestProjectionInInterval(posRect, distFn, m_current.m_ind, hoppingBorderIdx);
|
||||
if (closestIter.IsValid())
|
||||
return closestIter;
|
||||
|
||||
// If a projection to the two closest route segments is not found tries to find projection to other route
|
||||
// segments of current subroute.
|
||||
return GetClosestProjectionInInterval(posRect, distFn, hoppingBorderIdx, m_nextCheckpointIndex);
|
||||
}
|
||||
|
||||
Iter FollowedPolyline::GetBestMatchingProjection(m2::RectD const & posRect) const
|
||||
{
|
||||
CHECK_EQUAL(m_segProj.size() + 1, m_poly.GetSize(), ());
|
||||
// At first trying to find a projection to two closest route segments of route which is close
|
||||
// enough to |posRect| center. If |m_current| is right before intermediate point we can get |iter|
|
||||
// right after intermediate point (in next subroute).
|
||||
size_t const hoppingBorderIdx = min(m_nextCheckpointIndex, min(m_segProj.size(), m_current.m_ind + 3));
|
||||
auto const iter = GetClosestMatchingProjectionInInterval(posRect, m_current.m_ind, hoppingBorderIdx);
|
||||
if (iter.IsValid())
|
||||
return iter;
|
||||
// If a projection to the 3 closest route segments is not found tries to find projection to other route
|
||||
// segments of current subroute.
|
||||
return GetClosestMatchingProjectionInInterval(posRect, hoppingBorderIdx, m_nextCheckpointIndex);
|
||||
}
|
||||
|
||||
bool FollowedPolyline::UpdateMatchingProjection(m2::RectD const & posRect)
|
||||
{
|
||||
ASSERT(m_current.IsValid(), ());
|
||||
ASSERT_LESS(m_current.m_ind, m_poly.GetSize() - 1, ());
|
||||
|
||||
auto const iter = GetBestMatchingProjection(posRect);
|
||||
|
||||
if (iter.IsValid())
|
||||
{
|
||||
m_current = iter;
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
Iter FollowedPolyline::UpdateProjection(m2::RectD const & posRect)
|
||||
{
|
||||
ASSERT(m_current.IsValid(), ());
|
||||
ASSERT_LESS(m_current.m_ind, m_poly.GetSize() - 1, ());
|
||||
|
||||
Iter res;
|
||||
m2::PointD const currPos = posRect.Center();
|
||||
res = GetBestProjection(posRect, [&](Iter const & it) { return mercator::DistanceOnEarth(it.m_pt, currPos); });
|
||||
|
||||
if (res.IsValid())
|
||||
m_current = res;
|
||||
return res;
|
||||
}
|
||||
|
||||
void FollowedPolyline::SetFakeSegmentIndexes(vector<size_t> && fakeSegmentIndexes)
|
||||
{
|
||||
ASSERT(is_sorted(fakeSegmentIndexes.cbegin(), fakeSegmentIndexes.cend()), ());
|
||||
m_fakeSegmentIndexes = std::move(fakeSegmentIndexes);
|
||||
}
|
||||
|
||||
double FollowedPolyline::GetDistFromCurPointToRoutePointMerc() const
|
||||
{
|
||||
if (!m_current.IsValid())
|
||||
return 0.0;
|
||||
|
||||
return m_poly.GetPoint(m_current.m_ind).Length(m_current.m_pt);
|
||||
}
|
||||
|
||||
double FollowedPolyline::GetDistFromCurPointToRoutePointMeters() const
|
||||
{
|
||||
if (!m_current.IsValid())
|
||||
return 0.0;
|
||||
|
||||
return mercator::DistanceOnEarth(m_poly.GetPoint(m_current.m_ind), m_current.m_pt);
|
||||
}
|
||||
|
||||
void FollowedPolyline::GetCurrentDirectionPoint(m2::PointD & pt, double toleranceM) const
|
||||
{
|
||||
ASSERT(IsValid(), ());
|
||||
size_t currentIndex = min(m_current.m_ind + 1, m_poly.GetSize() - 1);
|
||||
m2::PointD point = m_poly.GetPoint(currentIndex);
|
||||
for (; currentIndex < m_poly.GetSize() - 1; point = m_poly.GetPoint(++currentIndex))
|
||||
if (mercator::DistanceOnEarth(point, m_current.m_pt) > toleranceM)
|
||||
break;
|
||||
|
||||
pt = point;
|
||||
}
|
||||
|
||||
Iter FollowedPolyline::GetClosestMatchingProjectionInInterval(m2::RectD const & posRect, size_t startIdx,
|
||||
size_t endIdx) const
|
||||
{
|
||||
CHECK_LESS_OR_EQUAL(endIdx, m_segProj.size(), ());
|
||||
CHECK_LESS_OR_EQUAL(startIdx, endIdx, ());
|
||||
|
||||
Iter nearestIter;
|
||||
double minDist = std::numeric_limits<double>::max();
|
||||
|
||||
m2::PointD const currPos = posRect.Center();
|
||||
|
||||
for (size_t i = startIdx; i < endIdx; ++i)
|
||||
{
|
||||
m2::PointD const pt = m_segProj[i].ClosestPointTo(currPos);
|
||||
|
||||
if (!posRect.IsPointInside(pt))
|
||||
continue;
|
||||
|
||||
double const dp = mercator::DistanceOnEarth(pt, currPos);
|
||||
if (dp >= minDist)
|
||||
continue;
|
||||
|
||||
nearestIter = Iter(pt, i);
|
||||
minDist = dp;
|
||||
}
|
||||
|
||||
return nearestIter;
|
||||
}
|
||||
} // namespace routing
|
||||
152
libs/routing/base/followed_polyline.hpp
Normal file
152
libs/routing/base/followed_polyline.hpp
Normal file
|
|
@ -0,0 +1,152 @@
|
|||
#pragma once
|
||||
|
||||
#include "geometry/mercator.hpp"
|
||||
|
||||
#include "geometry/point2d.hpp"
|
||||
#include "geometry/polyline2d.hpp"
|
||||
#include "geometry/rect2d.hpp"
|
||||
|
||||
#include <cstddef>
|
||||
#include <limits>
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
class FollowedPolyline
|
||||
{
|
||||
public:
|
||||
FollowedPolyline() = default;
|
||||
|
||||
template <typename Iter>
|
||||
FollowedPolyline(Iter begin, Iter end) : m_poly(begin, end)
|
||||
{
|
||||
Update();
|
||||
// Initially we do not have intermediate points. Next checkpoint is finish.
|
||||
m_nextCheckpointIndex = m_segProj.size();
|
||||
}
|
||||
|
||||
void SetNextCheckpointIndex(size_t index) { m_nextCheckpointIndex = index; }
|
||||
|
||||
void Swap(FollowedPolyline & rhs);
|
||||
|
||||
void Append(FollowedPolyline const & poly)
|
||||
{
|
||||
m_poly.Append(poly.m_poly);
|
||||
Update();
|
||||
}
|
||||
|
||||
void PopBack()
|
||||
{
|
||||
m_poly.PopBack();
|
||||
Update();
|
||||
}
|
||||
|
||||
bool IsValid() const { return (m_current.IsValid() && m_poly.GetSize() > 1); }
|
||||
m2::PolylineD const & GetPolyline() const { return m_poly; }
|
||||
|
||||
std::vector<double> const & GetSegDistanceMeters() const { return m_segDistance; }
|
||||
double GetTotalDistanceMeters() const;
|
||||
double GetDistanceFromStartMeters() const;
|
||||
double GetDistanceToEndMeters() const;
|
||||
double GetDistFromCurPointToRoutePointMerc() const;
|
||||
double GetDistFromCurPointToRoutePointMeters() const;
|
||||
|
||||
/*! \brief Return next navigation point for direction widgets.
|
||||
* Returns first geometry point from the polyline after your location if it is farther then
|
||||
* toleranceM.
|
||||
*/
|
||||
void GetCurrentDirectionPoint(m2::PointD & pt, double toleranceM) const;
|
||||
|
||||
struct Iter
|
||||
{
|
||||
static size_t constexpr kInvalidIndex = std::numeric_limits<size_t>::max();
|
||||
|
||||
Iter() = default;
|
||||
Iter(m2::PointD pt, size_t ind) : m_pt(pt), m_ind(ind) {}
|
||||
|
||||
bool IsValid() const { return m_ind != kInvalidIndex; }
|
||||
|
||||
m2::PointD m_pt;
|
||||
size_t m_ind = kInvalidIndex;
|
||||
};
|
||||
|
||||
Iter GetCurrentIter() const { return m_current; }
|
||||
|
||||
double GetDistanceM(Iter const & it1, Iter const & it2) const;
|
||||
|
||||
/// \brief Sets indexes of all unmatching segments on route.
|
||||
void SetFakeSegmentIndexes(std::vector<size_t> && fakeSegmentIndexes);
|
||||
|
||||
/// \brief Updates projection to the closest matched segment if it's possible.
|
||||
bool UpdateMatchingProjection(m2::RectD const & posRect);
|
||||
|
||||
Iter UpdateProjection(m2::RectD const & posRect);
|
||||
|
||||
Iter Begin() const;
|
||||
Iter End() const;
|
||||
Iter GetIterToIndex(size_t index) const;
|
||||
|
||||
/// \brief Calculates projection of center of |posRect| to the polyline.
|
||||
/// \param posRect Only projection inside the rect is considered.
|
||||
/// \param distFn A method which is used to calculate the destination between points.
|
||||
/// \param startIdx Start segment index in |m_segProj|.
|
||||
/// \param endIdx The index after the last one in |m_segProj|.
|
||||
/// \returns iterator which contains projection point and projection segment index.
|
||||
template <typename DistanceFn>
|
||||
Iter GetClosestProjectionInInterval(m2::RectD const & posRect, DistanceFn const & distFn, size_t startIdx,
|
||||
size_t endIdx) const
|
||||
{
|
||||
CHECK_LESS_OR_EQUAL(endIdx, m_segProj.size(), ());
|
||||
CHECK_LESS_OR_EQUAL(startIdx, endIdx, ());
|
||||
|
||||
Iter res;
|
||||
double minDist = std::numeric_limits<double>::max();
|
||||
|
||||
for (size_t i = startIdx; i < endIdx; ++i)
|
||||
{
|
||||
m2::PointD const & pt = m_segProj[i].ClosestPointTo(posRect.Center());
|
||||
|
||||
if (!posRect.IsPointInside(pt))
|
||||
continue;
|
||||
|
||||
Iter it(pt, i);
|
||||
double const dp = distFn(it);
|
||||
if (dp < minDist)
|
||||
{
|
||||
res = it;
|
||||
minDist = dp;
|
||||
}
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
Iter GetClosestMatchingProjectionInInterval(m2::RectD const & posRect, size_t startIdx, size_t endIdx) const;
|
||||
|
||||
bool IsFakeSegment(size_t index) const;
|
||||
|
||||
private:
|
||||
/// \returns iterator to the best projection of center of |posRect| to the |m_poly|.
|
||||
/// If there's a good projection of center of |posRect| to two closest segments of |m_poly|
|
||||
/// after |m_current| the iterator corresponding of the projection is returned.
|
||||
/// Otherwise returns a projection to closest point of route.
|
||||
template <typename DistanceFn>
|
||||
Iter GetBestProjection(m2::RectD const & posRect, DistanceFn const & distFn) const;
|
||||
|
||||
Iter GetBestMatchingProjection(m2::RectD const & posRect) const;
|
||||
|
||||
void Update();
|
||||
|
||||
m2::PolylineD m_poly;
|
||||
/// Indexes of all unmatching segments on route.
|
||||
std::vector<size_t> m_fakeSegmentIndexes;
|
||||
|
||||
/// Iterator with the current position. Position sets with UpdateProjection methods.
|
||||
Iter m_current;
|
||||
size_t m_nextCheckpointIndex;
|
||||
/// Precalculated info for fast projection finding.
|
||||
std::vector<m2::ParametrizedSegment<m2::PointD>> m_segProj;
|
||||
/// Accumulated cache of segments length in meters.
|
||||
std::vector<double> m_segDistance;
|
||||
};
|
||||
} // namespace routing
|
||||
30
libs/routing/base/routing_result.hpp
Normal file
30
libs/routing/base/routing_result.hpp
Normal file
|
|
@ -0,0 +1,30 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/base/astar_weight.hpp"
|
||||
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
template <typename Vertex, typename Weight>
|
||||
struct RoutingResult final
|
||||
{
|
||||
RoutingResult() : m_distance(GetAStarWeightZero<Weight>()) {}
|
||||
|
||||
void Clear()
|
||||
{
|
||||
m_path.clear();
|
||||
m_distance = GetAStarWeightZero<Weight>();
|
||||
}
|
||||
|
||||
bool Empty() const { return m_path.empty(); }
|
||||
|
||||
std::vector<Vertex> m_path;
|
||||
Weight m_distance;
|
||||
|
||||
struct LessWeight
|
||||
{
|
||||
bool operator()(RoutingResult const & l, RoutingResult const & r) const { return l.m_distance < r.m_distance; }
|
||||
};
|
||||
};
|
||||
} // namespace routing
|
||||
28
libs/routing/base/small_list.cpp
Normal file
28
libs/routing/base/small_list.cpp
Normal file
|
|
@ -0,0 +1,28 @@
|
|||
#include "small_list.hpp"
|
||||
|
||||
#include "base/logging.hpp"
|
||||
|
||||
namespace routing
|
||||
{
|
||||
namespace impl
|
||||
{
|
||||
|
||||
std::map<char const *, Statistics::Info> Statistics::s_map;
|
||||
|
||||
void Statistics::Add(char const * name, size_t val)
|
||||
{
|
||||
auto & e = s_map[name];
|
||||
++e.m_count;
|
||||
e.m_sum += val;
|
||||
e.m_max = std::max(val, e.m_max);
|
||||
}
|
||||
|
||||
void Statistics::Dump()
|
||||
{
|
||||
for (auto const & e : s_map)
|
||||
LOG(LINFO, (e.first, ": count = ", e.second.m_count, "; max = ", e.second.m_max,
|
||||
"; avg = ", e.second.m_sum / double(e.second.m_count)));
|
||||
}
|
||||
|
||||
} // namespace impl
|
||||
} // namespace routing
|
||||
58
libs/routing/base/small_list.hpp
Normal file
58
libs/routing/base/small_list.hpp
Normal file
|
|
@ -0,0 +1,58 @@
|
|||
#pragma once
|
||||
#include "base/buffer_vector.hpp"
|
||||
|
||||
#include <map>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
namespace impl
|
||||
{
|
||||
|
||||
class Statistics
|
||||
{
|
||||
struct Info
|
||||
{
|
||||
size_t m_sum = 0;
|
||||
size_t m_count = 0;
|
||||
size_t m_max = 0;
|
||||
};
|
||||
|
||||
static std::map<char const *, Info> s_map;
|
||||
|
||||
public:
|
||||
static void Add(char const * name, size_t val);
|
||||
static void Dump();
|
||||
};
|
||||
|
||||
} // namespace impl
|
||||
|
||||
template <class T>
|
||||
class SmallList : public buffer_vector<T, 8>
|
||||
{
|
||||
using BaseT = buffer_vector<T, 8>;
|
||||
|
||||
public:
|
||||
using BaseT::BaseT;
|
||||
|
||||
/// @todo Use in Generator only.
|
||||
/*
|
||||
void clear()
|
||||
{
|
||||
impl::Statistics::Add(typeid(T).name(), BaseT::size());
|
||||
BaseT::clear();
|
||||
}
|
||||
|
||||
~SmallList()
|
||||
{
|
||||
impl::Statistics::Add(typeid(T).name(), BaseT::size());
|
||||
}
|
||||
*/
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
inline std::string DebugPrint(SmallList<T> const & v)
|
||||
{
|
||||
return DebugPrint(static_cast<buffer_vector<T, 8> const &>(v));
|
||||
}
|
||||
|
||||
} // namespace routing
|
||||
600
libs/routing/car_directions.cpp
Normal file
600
libs/routing/car_directions.cpp
Normal file
|
|
@ -0,0 +1,600 @@
|
|||
#include "routing/car_directions.hpp"
|
||||
|
||||
#include "routing/lanes/lanes_recommendation.hpp"
|
||||
#include "routing/turns.hpp"
|
||||
#include "routing/turns_generator.hpp"
|
||||
#include "routing/turns_generator_utils.hpp"
|
||||
|
||||
#include "geometry/angles.hpp"
|
||||
|
||||
namespace routing
|
||||
{
|
||||
using namespace std;
|
||||
using namespace turns;
|
||||
using namespace ftypes;
|
||||
|
||||
CarDirectionsEngine::CarDirectionsEngine(MwmDataSource & dataSource, shared_ptr<NumMwmIds> numMwmIds)
|
||||
: DirectionsEngine(dataSource, std::move(numMwmIds))
|
||||
{}
|
||||
|
||||
void CarDirectionsEngine::FixupTurns(vector<RouteSegment> & routeSegments)
|
||||
{
|
||||
FixupCarTurns(routeSegments);
|
||||
}
|
||||
|
||||
void FixupCarTurns(vector<RouteSegment> & routeSegments)
|
||||
{
|
||||
double constexpr kMergeDistMeters = 15.0;
|
||||
// For turns that are not EnterRoundAbout/ExitRoundAbout exitNum is always equal to zero.
|
||||
// If a turn is EnterRoundAbout exitNum is a number of turns between two junctions:
|
||||
// (1) the route enters to the roundabout;
|
||||
// (2) the route leaves the roundabout;
|
||||
uint32_t exitNum = 0;
|
||||
size_t constexpr kInvalidEnter = numeric_limits<size_t>::max();
|
||||
size_t enterRoundAbout = kInvalidEnter;
|
||||
|
||||
for (size_t idx = 0; idx < routeSegments.size(); ++idx)
|
||||
{
|
||||
auto & t = routeSegments[idx].GetTurn();
|
||||
if (t.IsTurnNone())
|
||||
continue;
|
||||
|
||||
if (enterRoundAbout != kInvalidEnter && t.m_turn != CarDirection::StayOnRoundAbout &&
|
||||
t.m_turn != CarDirection::LeaveRoundAbout && t.m_turn != CarDirection::ReachedYourDestination)
|
||||
{
|
||||
ASSERT(false,
|
||||
("Only StayOnRoundAbout, LeaveRoundAbout or ReachedYourDestination are expected after EnterRoundAbout."));
|
||||
exitNum = 0;
|
||||
enterRoundAbout = kInvalidEnter;
|
||||
}
|
||||
else if (t.m_turn == CarDirection::EnterRoundAbout)
|
||||
{
|
||||
ASSERT(enterRoundAbout == kInvalidEnter,
|
||||
("It's not expected to find new EnterRoundAbout until previous EnterRoundAbout was leaved."));
|
||||
enterRoundAbout = idx;
|
||||
ASSERT(exitNum == 0, ("exitNum is reset at start and after LeaveRoundAbout."));
|
||||
exitNum = t.m_exitNum; // Normally it is 0, but sometimes it can be 1.
|
||||
}
|
||||
else if (t.m_turn == CarDirection::StayOnRoundAbout)
|
||||
{
|
||||
++exitNum;
|
||||
routeSegments[idx].ClearTurn();
|
||||
continue;
|
||||
}
|
||||
else if (t.m_turn == CarDirection::LeaveRoundAbout)
|
||||
{
|
||||
// It's possible for car to be on roundabout without entering it
|
||||
// if route calculation started at roundabout (e.g. if user made full turn on roundabout).
|
||||
if (enterRoundAbout != kInvalidEnter)
|
||||
routeSegments[enterRoundAbout].SetTurnExits(exitNum + 1);
|
||||
routeSegments[idx].SetTurnExits(exitNum + 1); // For LeaveRoundAbout turn.
|
||||
enterRoundAbout = kInvalidEnter;
|
||||
exitNum = 0;
|
||||
}
|
||||
|
||||
// Merging turns which are closed to each other under some circumstance.
|
||||
// distance(turnsDir[idx - 1].m_index, turnsDir[idx].m_index) < kMergeDistMeters
|
||||
// means the distance in meters between the former turn (idx - 1)
|
||||
// and the current turn (idx).
|
||||
if (idx > 0 && IsStayOnRoad(routeSegments[idx - 1].GetTurn().m_turn) && IsLeftOrRightTurn(t.m_turn))
|
||||
{
|
||||
auto const & junction = routeSegments[idx].GetJunction();
|
||||
auto const & prevJunction = routeSegments[idx - 1].GetJunction();
|
||||
if (mercator::DistanceOnEarth(junction.GetPoint(), prevJunction.GetPoint()) < kMergeDistMeters)
|
||||
routeSegments[idx - 1].ClearTurn();
|
||||
}
|
||||
}
|
||||
turns::lanes::SelectRecommendedLanes(routeSegments);
|
||||
}
|
||||
|
||||
void GetTurnDirectionBasic(IRoutingResult const & result, size_t const outgoingSegmentIndex,
|
||||
NumMwmIds const & numMwmIds, RoutingSettings const & vehicleSettings, TurnItem & turn);
|
||||
|
||||
size_t CarDirectionsEngine::GetTurnDirection(IRoutingResult const & result, size_t const outgoingSegmentIndex,
|
||||
NumMwmIds const & numMwmIds, RoutingSettings const & vehicleSettings,
|
||||
TurnItem & turnItem)
|
||||
{
|
||||
if (outgoingSegmentIndex == result.GetSegments().size())
|
||||
{
|
||||
turnItem.m_turn = CarDirection::ReachedYourDestination;
|
||||
return 0;
|
||||
}
|
||||
|
||||
// This is for jump from initial point to start of the route. No direction is given.
|
||||
/// @todo Sometimes results of GetPossibleTurns are empty, sometimes are invalid.
|
||||
/// The best will be to fix GetPossibleTurns(). It will allow us to use following approach.
|
||||
/// E.g. Google Maps until you reach the destination will guide you to go to the left or to the right of the first
|
||||
/// road.
|
||||
if (outgoingSegmentIndex == 2) // The same as turnItem.m_index == 2.
|
||||
return 0;
|
||||
|
||||
size_t skipTurnSegments = CheckUTurnOnRoute(result, outgoingSegmentIndex, numMwmIds, vehicleSettings, turnItem);
|
||||
|
||||
if (turnItem.m_turn == CarDirection::None)
|
||||
GetTurnDirectionBasic(result, outgoingSegmentIndex, numMwmIds, vehicleSettings, turnItem);
|
||||
|
||||
// Lane information.
|
||||
if (turnItem.m_turn != CarDirection::None)
|
||||
{
|
||||
auto const & loadedSegments = result.GetSegments();
|
||||
auto const & ingoingSegment = loadedSegments[outgoingSegmentIndex - 1];
|
||||
turnItem.m_lanes = ingoingSegment.m_lanes;
|
||||
}
|
||||
|
||||
return skipTurnSegments;
|
||||
}
|
||||
|
||||
/*!
|
||||
* \brief Calculates a turn instruction if the ingoing edge or (and) the outgoing edge belongs to a
|
||||
* roundabout.
|
||||
* \return Returns one of the following results:
|
||||
* - TurnDirection::EnterRoundAbout if the ingoing edge does not belong to a roundabout
|
||||
* and the outgoing edge belongs to a roundabout.
|
||||
* - TurnDirection::StayOnRoundAbout if the ingoing edge and the outgoing edge belong to a
|
||||
* roundabout
|
||||
* and there is a reasonalbe way to leave the junction besides the outgoing edge.
|
||||
* This function does not return TurnDirection::StayOnRoundAbout for small ways to leave the
|
||||
* roundabout.
|
||||
* - TurnDirection::NoTurn if the ingoing edge and the outgoing edge belong to a roundabout
|
||||
* (a) and there is a single way (outgoing edge) to leave the junction.
|
||||
* (b) and there is a way(s) besides outgoing edge to leave the junction (the roundabout)
|
||||
* but it is (they are) relevantly small.
|
||||
*/
|
||||
CarDirection GetRoundaboutDirectionBasic(bool isIngoingEdgeRoundabout, bool isOutgoingEdgeRoundabout,
|
||||
bool isMultiTurnJunction, bool keepTurnByHighwayClass)
|
||||
{
|
||||
if (isIngoingEdgeRoundabout && isOutgoingEdgeRoundabout)
|
||||
{
|
||||
if (isMultiTurnJunction)
|
||||
return keepTurnByHighwayClass ? CarDirection::StayOnRoundAbout : CarDirection::None;
|
||||
return CarDirection::None;
|
||||
}
|
||||
|
||||
if (!isIngoingEdgeRoundabout && isOutgoingEdgeRoundabout)
|
||||
return CarDirection::EnterRoundAbout;
|
||||
|
||||
if (isIngoingEdgeRoundabout && !isOutgoingEdgeRoundabout)
|
||||
return CarDirection::LeaveRoundAbout;
|
||||
|
||||
ASSERT(false, ());
|
||||
return CarDirection::None;
|
||||
}
|
||||
|
||||
/*!
|
||||
* \brief Returns false when other possible turns leads to service roads;
|
||||
*/
|
||||
bool KeepRoundaboutTurnByHighwayClass(TurnCandidates const & possibleTurns, TurnInfo const & turnInfo,
|
||||
NumMwmIds const & numMwmIds)
|
||||
{
|
||||
Segment firstOutgoingSegment;
|
||||
turnInfo.m_outgoing->m_segmentRange.GetFirstSegment(numMwmIds, firstOutgoingSegment);
|
||||
|
||||
for (auto const & t : possibleTurns.candidates)
|
||||
{
|
||||
if (t.m_segment == firstOutgoingSegment)
|
||||
continue;
|
||||
// For roundabouts of Tertiary and less significant class count every road.
|
||||
// For more significant roundabouts - ignore service roads (driveway, parking_aisle).
|
||||
if (turnInfo.m_outgoing->m_highwayClass >= HighwayClass::Tertiary || t.m_highwayClass != HighwayClass::ServiceMinor)
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
CarDirection GetRoundaboutDirection(TurnInfo const & turnInfo, TurnCandidates const & nodes,
|
||||
NumMwmIds const & numMwmIds)
|
||||
{
|
||||
bool const keepTurnByHighwayClass = KeepRoundaboutTurnByHighwayClass(nodes, turnInfo, numMwmIds);
|
||||
return GetRoundaboutDirectionBasic(turnInfo.m_ingoing->m_onRoundabout, turnInfo.m_outgoing->m_onRoundabout,
|
||||
nodes.candidates.size() > 1, keepTurnByHighwayClass);
|
||||
}
|
||||
|
||||
/// \brief Return |CarDirection::ExitHighwayToRight| or |CarDirection::ExitHighwayToLeft|
|
||||
/// or return |CarDirection::None| if no exit is detected.
|
||||
/// \note The function makes a decision about turn based on geometry of the route and turn
|
||||
/// candidates, so it works correctly for both left and right hand traffic.
|
||||
CarDirection TryToGetExitDirection(TurnCandidates const & possibleTurns, TurnInfo const & turnInfo,
|
||||
Segment const & firstOutgoingSeg, CarDirection const intermediateDirection)
|
||||
{
|
||||
if (!possibleTurns.isCandidatesAngleValid)
|
||||
return CarDirection::None;
|
||||
|
||||
if (!IsHighway(turnInfo.m_ingoing->m_highwayClass, turnInfo.m_ingoing->m_isLink))
|
||||
return CarDirection::None;
|
||||
|
||||
if (!turnInfo.m_outgoing->m_isLink &&
|
||||
!(IsSmallRoad(turnInfo.m_outgoing->m_highwayClass) && IsGoStraightOrSlightTurn(intermediateDirection)))
|
||||
return CarDirection::None;
|
||||
|
||||
// At this point it is known that the route goes form a highway to a link road or to a small road
|
||||
// which has a slight angle with the highway.
|
||||
|
||||
// Considering cases when the route goes from a highway to a link or a small road.
|
||||
// Checking all turn candidates (sorted by their angles) and looking for the road which is a
|
||||
// continuation of the ingoing segment. If the continuation is on the right hand of the route
|
||||
// it's an exit to the left. If the continuation is on the left hand of the route
|
||||
// it's an exit to the right.
|
||||
// Note. The angle which is used for sorting turn candidates in |possibleTurns.candidates|
|
||||
// is a counterclockwise angle between the ingoing route edge and corresponding candidate.
|
||||
// For left turns the angle is less than zero and for right ones it is more than zero.
|
||||
bool isCandidateBeforeOutgoing = true;
|
||||
bool isHighwayCandidateBeforeOutgoing = true;
|
||||
size_t highwayCandidateNumber = 0;
|
||||
|
||||
for (auto const & c : possibleTurns.candidates)
|
||||
{
|
||||
if (c.m_segment == firstOutgoingSeg)
|
||||
{
|
||||
isCandidateBeforeOutgoing = false;
|
||||
continue;
|
||||
}
|
||||
|
||||
if (IsHighway(c.m_highwayClass, c.m_isLink))
|
||||
{
|
||||
++highwayCandidateNumber;
|
||||
if (highwayCandidateNumber >= 2)
|
||||
return CarDirection::None; // There are two or more highway candidates from the junction.
|
||||
isHighwayCandidateBeforeOutgoing = isCandidateBeforeOutgoing;
|
||||
}
|
||||
}
|
||||
if (highwayCandidateNumber == 1)
|
||||
return isHighwayCandidateBeforeOutgoing ? CarDirection::ExitHighwayToRight : CarDirection::ExitHighwayToLeft;
|
||||
return CarDirection::None;
|
||||
}
|
||||
|
||||
// Min difference of route and alternative turn abs angles in degrees
|
||||
// to ignore alternative turn (when route direction is GoStraight).
|
||||
double constexpr kMinAbsAngleDiffForStraightRoute = 25.0;
|
||||
|
||||
// Min difference of route and alternative turn abs angles in degrees
|
||||
// to ignore alternative turn (when route direction is bigger and GoStraight).
|
||||
double constexpr kMinAbsAngleDiffForBigStraightRoute = 5;
|
||||
|
||||
// Min difference of route and alternative turn abs angles in degrees
|
||||
// to ignore this alternative candidate (when alternative road is the same or smaller).
|
||||
double constexpr kMinAbsAngleDiffForSameOrSmallerRoad = 35.0;
|
||||
|
||||
/// * \returns true when
|
||||
/// * - the route leads from one big road to another one;
|
||||
/// * - and the other possible turns lead to small roads or these turns too sharp.
|
||||
/// * Returns false otherwise.
|
||||
/// \param routeDirection is route direction.
|
||||
/// \param routeAngle is route angle.
|
||||
/// \param turnCandidates is all possible ways out from a junction except uturn.
|
||||
/// \param turnInfo is information about ingoing and outgoing segments of the route.
|
||||
bool CanDiscardTurnByHighwayClassOrAngles(CarDirection const routeDirection, double const routeAngle,
|
||||
vector<TurnCandidate> const & turnCandidates, TurnInfo const & turnInfo,
|
||||
NumMwmIds const & numMwmIds)
|
||||
{
|
||||
if (!IsGoStraightOrSlightTurn(routeDirection))
|
||||
return false;
|
||||
|
||||
if (turnCandidates.size() < 2)
|
||||
return true;
|
||||
|
||||
HighwayClass outgoingRouteRoadClass = turnInfo.m_outgoing->m_highwayClass;
|
||||
HighwayClass ingoingRouteRoadClass = turnInfo.m_ingoing->m_highwayClass;
|
||||
|
||||
Segment firstOutgoingSegment;
|
||||
turnInfo.m_outgoing->m_segmentRange.GetFirstSegment(numMwmIds, firstOutgoingSegment);
|
||||
|
||||
for (auto const & t : turnCandidates)
|
||||
{
|
||||
// Let's consider all outgoing segments except for route outgoing segment.
|
||||
if (t.m_segment == firstOutgoingSegment)
|
||||
continue;
|
||||
|
||||
// Min difference of route and alternative turn abs angles in degrees
|
||||
// to ignore this alternative candidate (when route goes from non-link to link).
|
||||
double constexpr kMinAbsAngleDiffForGoLink = 70.0;
|
||||
|
||||
// If route goes from non-link to link and there is not too sharp candidate then turn can't be discarded.
|
||||
if (!turnInfo.m_ingoing->m_isLink && turnInfo.m_outgoing->m_isLink &&
|
||||
abs(t.m_angle) < abs(routeAngle) + kMinAbsAngleDiffForGoLink)
|
||||
return false;
|
||||
|
||||
HighwayClass candidateRoadClass = t.m_highwayClass;
|
||||
|
||||
// If outgoing route road is significantly larger than candidate, the candidate can be ignored.
|
||||
if (CalcDiffRoadClasses(outgoingRouteRoadClass, candidateRoadClass) <= kMinHighwayClassDiff)
|
||||
continue;
|
||||
|
||||
// If outgoing route's road is significantly larger than candidate's service road, the candidate can be ignored.
|
||||
if (CalcDiffRoadClasses(outgoingRouteRoadClass, candidateRoadClass) <= kMinHighwayClassDiffForService &&
|
||||
(candidateRoadClass == HighwayClass::Service || candidateRoadClass == HighwayClass::ServiceMinor))
|
||||
continue;
|
||||
|
||||
// If igngoing and outgoing edges are not links
|
||||
// and outgoing route road is the same or large than ingoing
|
||||
// then candidate-link can be ignored.
|
||||
if (!turnInfo.m_ingoing->m_isLink && !turnInfo.m_outgoing->m_isLink &&
|
||||
CalcDiffRoadClasses(outgoingRouteRoadClass, ingoingRouteRoadClass) <= 0 && t.m_isLink)
|
||||
continue;
|
||||
|
||||
// If alternative cadidate's road size is the same or smaller
|
||||
// and it's angle is sharp enough compared to the route it can be ignored.
|
||||
if (CalcDiffRoadClasses(outgoingRouteRoadClass, candidateRoadClass) <= 0 &&
|
||||
abs(t.m_angle) > abs(routeAngle) + kMinAbsAngleDiffForSameOrSmallerRoad)
|
||||
continue;
|
||||
|
||||
if (routeDirection == CarDirection::GoStraight)
|
||||
{
|
||||
// If alternative cadidate's road size is smaller
|
||||
// and it's angle is not very close to the route's one - it can be ignored.
|
||||
if (CalcDiffRoadClasses(outgoingRouteRoadClass, candidateRoadClass) < 0 &&
|
||||
abs(t.m_angle) > abs(routeAngle) + kMinAbsAngleDiffForBigStraightRoute)
|
||||
continue;
|
||||
|
||||
// If outgoing route road is the same or large than ingoing
|
||||
// and candidate's angle is sharp enough compared to the route it can be ignored.
|
||||
if (CalcDiffRoadClasses(outgoingRouteRoadClass, ingoingRouteRoadClass) <= 0 &&
|
||||
abs(t.m_angle) > abs(routeAngle) + kMinAbsAngleDiffForStraightRoute)
|
||||
continue;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/*!
|
||||
* \brief Corrects |turn.m_turn| if |turn.m_turn == CarDirection::GoStraight|.
|
||||
* If the other way is not sharp enough, turn.m_turn is set to |turnToSet|.
|
||||
*/
|
||||
void CorrectGoStraight(TurnCandidate const & notRouteCandidate, double const routeAngle, CarDirection const & turnToSet,
|
||||
TurnItem & turn)
|
||||
{
|
||||
if (turn.m_turn != CarDirection::GoStraight)
|
||||
return;
|
||||
|
||||
// Correct turn if alternative cadidate's angle is not sharp enough compared to the route.
|
||||
if (abs(notRouteCandidate.m_angle) < abs(routeAngle) + kMinAbsAngleDiffForStraightRoute)
|
||||
{
|
||||
LOG(LDEBUG, ("Turn: ", turn.m_index, " GoStraight correction."));
|
||||
turn.m_turn = turnToSet;
|
||||
}
|
||||
}
|
||||
|
||||
// If the route goes along the rightmost or the leftmost way among available ones:
|
||||
// 1. RightmostDirection or LeftmostDirection is selected
|
||||
// 2. If the turn direction is |CarDirection::GoStraight|
|
||||
// and there's another not sharp enough turn
|
||||
// GoStraight is corrected to TurnSlightRight/TurnSlightLeft
|
||||
// to avoid ambiguity for GoStraight direction: 2 or more almost straight turns.
|
||||
void CorrectRightmostAndLeftmost(vector<TurnCandidate> const & turnCandidates, Segment const & firstOutgoingSeg,
|
||||
double const turnAngle, TurnItem & turn)
|
||||
{
|
||||
// turnCandidates are sorted by angle from leftmost to rightmost.
|
||||
// Normally no duplicates should be found. But if they are present we can't identify the leftmost/rightmost by order.
|
||||
if (adjacent_find(turnCandidates.begin(), turnCandidates.end(), base::EqualsBy(&TurnCandidate::m_angle)) !=
|
||||
turnCandidates.end())
|
||||
{
|
||||
LOG(LWARNING, ("nodes.candidates are not expected to have same m_angle."));
|
||||
return;
|
||||
}
|
||||
|
||||
double constexpr kMaxAbsAngleConsideredLeftOrRightMost = 90;
|
||||
|
||||
// Go from left to right to findout if the route goes through the leftmost candidate and fixes can be applied.
|
||||
// Other candidates which are sharper than kMaxAbsAngleConsideredLeftOrRightMost are ignored.
|
||||
for (auto candidate = turnCandidates.begin(); candidate != turnCandidates.end(); ++candidate)
|
||||
{
|
||||
if (candidate->m_segment == firstOutgoingSeg && candidate + 1 != turnCandidates.end())
|
||||
{
|
||||
// The route goes along the leftmost candidate.
|
||||
turn.m_turn = LeftmostDirection(turnAngle);
|
||||
if (IntermediateDirection(turnAngle) != turn.m_turn)
|
||||
LOG(LDEBUG, ("Turn: ", turn.m_index, " LeftmostDirection correction."));
|
||||
// Compare with the next candidate to the right.
|
||||
CorrectGoStraight(*(candidate + 1), candidate->m_angle, CarDirection::TurnSlightLeft, turn);
|
||||
break;
|
||||
}
|
||||
// Check if this candidate is considered as leftmost as not too sharp.
|
||||
// If yes - this candidate is leftmost, not route's one.
|
||||
if (candidate->m_angle > -kMaxAbsAngleConsideredLeftOrRightMost)
|
||||
break;
|
||||
}
|
||||
// Go from right to left to findout if the route goes through the rightmost candidate anf fixes can be applied.
|
||||
// Other candidates which are sharper than kMaxAbsAngleConsideredLeftOrRightMost are ignored.
|
||||
for (auto candidate = turnCandidates.rbegin(); candidate != turnCandidates.rend(); ++candidate)
|
||||
{
|
||||
if (candidate->m_segment == firstOutgoingSeg && candidate + 1 != turnCandidates.rend())
|
||||
{
|
||||
// The route goes along the rightmost candidate.
|
||||
turn.m_turn = RightmostDirection(turnAngle);
|
||||
if (IntermediateDirection(turnAngle) != turn.m_turn)
|
||||
LOG(LDEBUG, ("Turn: ", turn.m_index, " RighmostDirection correction."));
|
||||
// Compare with the next candidate to the left.
|
||||
CorrectGoStraight(*(candidate + 1), candidate->m_angle, CarDirection::TurnSlightRight, turn);
|
||||
break;
|
||||
}
|
||||
// Check if this candidate is considered as rightmost as not too sharp.
|
||||
// If yes - this candidate is rightmost, not route's one.
|
||||
if (candidate->m_angle < kMaxAbsAngleConsideredLeftOrRightMost)
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void GetTurnDirectionBasic(IRoutingResult const & result, size_t const outgoingSegmentIndex,
|
||||
NumMwmIds const & numMwmIds, RoutingSettings const & vehicleSettings, TurnItem & turn)
|
||||
{
|
||||
TurnInfo turnInfo;
|
||||
if (!GetTurnInfo(result, outgoingSegmentIndex, vehicleSettings, turnInfo))
|
||||
return;
|
||||
|
||||
turn.m_turn = CarDirection::None;
|
||||
|
||||
ASSERT_GREATER(turnInfo.m_ingoing->m_path.size(), 1, ());
|
||||
TurnCandidates nodes;
|
||||
size_t ingoingCount;
|
||||
m2::PointD const junctionPoint = turnInfo.m_ingoing->m_path.back().GetPoint();
|
||||
result.GetPossibleTurns(turnInfo.m_ingoing->m_segmentRange, junctionPoint, ingoingCount, nodes);
|
||||
if (nodes.isCandidatesAngleValid)
|
||||
{
|
||||
ASSERT(is_sorted(nodes.candidates.begin(), nodes.candidates.end(), base::LessBy(&TurnCandidate::m_angle)),
|
||||
("Turn candidates should be sorted by its angle field."));
|
||||
}
|
||||
|
||||
/// @todo Proper handling of isCandidatesAngleValid == False, when we don't have angles of candidates.
|
||||
|
||||
if (nodes.candidates.size() == 0)
|
||||
return;
|
||||
|
||||
// No turns are needed on transported road.
|
||||
if (turnInfo.m_ingoing->m_highwayClass == HighwayClass::Transported &&
|
||||
turnInfo.m_outgoing->m_highwayClass == HighwayClass::Transported)
|
||||
return;
|
||||
|
||||
Segment firstOutgoingSeg;
|
||||
if (!turnInfo.m_outgoing->m_segmentRange.GetFirstSegment(numMwmIds, firstOutgoingSeg))
|
||||
return;
|
||||
|
||||
CorrectCandidatesSegmentByOutgoing(turnInfo, firstOutgoingSeg, nodes);
|
||||
|
||||
RemoveUTurnCandidate(turnInfo, numMwmIds, nodes.candidates);
|
||||
auto const & turnCandidates = nodes.candidates;
|
||||
|
||||
// Check for enter or exit to/from roundabout.
|
||||
if (turnInfo.m_ingoing->m_onRoundabout || turnInfo.m_outgoing->m_onRoundabout)
|
||||
{
|
||||
turn.m_turn = GetRoundaboutDirection(turnInfo, nodes, numMwmIds);
|
||||
// If there is 1 or more exits (nodes.candidates > 1) right at the enter it should be counted. Issue #3027.
|
||||
if (turn.m_turn == CarDirection::EnterRoundAbout && nodes.candidates.size() > 1)
|
||||
turn.m_exitNum = 1;
|
||||
return;
|
||||
}
|
||||
|
||||
double const turnAngle = CalcTurnAngle(result, outgoingSegmentIndex, numMwmIds, vehicleSettings);
|
||||
|
||||
CarDirection const intermediateDirection = IntermediateDirection(turnAngle);
|
||||
|
||||
// Checking for exits from highways.
|
||||
turn.m_turn = TryToGetExitDirection(nodes, turnInfo, firstOutgoingSeg, intermediateDirection);
|
||||
if (turn.m_turn != CarDirection::None)
|
||||
return;
|
||||
|
||||
if (turnCandidates.size() == 1)
|
||||
{
|
||||
// ASSERT_EQUAL(turnCandidates.front().m_segment, firstOutgoingSeg, ());
|
||||
|
||||
if (IsGoStraightOrSlightTurn(intermediateDirection))
|
||||
return;
|
||||
// IngoingCount includes ingoing segment and reversed outgoing (if it is not one-way).
|
||||
// If any other one is present, turn (not straight or slight) is kept to prevent user from going to oneway
|
||||
// alternative.
|
||||
|
||||
/// @todo Min abs angle of ingoing ones should be considered. If it's much bigger than route angle - ignore ingoing
|
||||
/// ones. Now this data is not available from IRoutingResult::GetPossibleTurns().
|
||||
if (ingoingCount <= 1 + (turnInfo.m_outgoing->m_isOneWay ? 0 : 1))
|
||||
return;
|
||||
}
|
||||
|
||||
// This angle is calculated using only 1 segment back and forward, not like turnAngle.
|
||||
double turnOneSegmentAngle = CalcOneSegmentTurnAngle(turnInfo);
|
||||
|
||||
// To not discard some disputable turns let's use max by modulus from turnOneSegmentAngle and turnAngle.
|
||||
// It's natural since angles of turnCandidates are calculated in IRoutingResult::GetPossibleTurns()
|
||||
// according to CalcOneSegmentTurnAngle logic. And to be safe turnAngle is used too.
|
||||
double turnAngleToCompare = turnAngle;
|
||||
if (turnOneSegmentAngle <= 0 && turnAngle <= 0)
|
||||
turnAngleToCompare = min(turnOneSegmentAngle, turnAngle);
|
||||
else if (turnOneSegmentAngle >= 0 && turnAngle >= 0)
|
||||
turnAngleToCompare = max(turnOneSegmentAngle, turnAngle);
|
||||
else if (abs(turnOneSegmentAngle) > 10)
|
||||
LOG(LWARNING, ("Significant angles are expected to have the same sign."));
|
||||
|
||||
if (CanDiscardTurnByHighwayClassOrAngles(intermediateDirection, turnAngleToCompare, turnCandidates, turnInfo,
|
||||
numMwmIds))
|
||||
return;
|
||||
|
||||
turn.m_turn = intermediateDirection;
|
||||
|
||||
if (turnCandidates.size() >= 2 && nodes.isCandidatesAngleValid)
|
||||
CorrectRightmostAndLeftmost(turnCandidates, firstOutgoingSeg, turnAngle, turn);
|
||||
}
|
||||
|
||||
size_t CheckUTurnOnRoute(IRoutingResult const & result, size_t const outgoingSegmentIndex, NumMwmIds const & numMwmIds,
|
||||
RoutingSettings const & vehicleSettings, TurnItem & turn)
|
||||
{
|
||||
size_t constexpr kUTurnLookAhead = 3;
|
||||
double constexpr kUTurnHeadingSensitivity = math::pi / 10.0;
|
||||
auto const & segments = result.GetSegments();
|
||||
|
||||
// In this function we process the turn between the previous and the current
|
||||
// segments. So we need a shift to get the previous segment.
|
||||
ASSERT_GREATER(segments.size(), 1, ());
|
||||
ASSERT_GREATER(outgoingSegmentIndex, 0, ());
|
||||
ASSERT_GREATER(segments.size(), outgoingSegmentIndex, ());
|
||||
auto const & masterSegment = segments[outgoingSegmentIndex - 1];
|
||||
if (masterSegment.m_path.size() < 2)
|
||||
return 0;
|
||||
|
||||
// Roundabout is not the UTurn.
|
||||
if (masterSegment.m_onRoundabout)
|
||||
return 0;
|
||||
for (size_t i = 0; i < kUTurnLookAhead && i + outgoingSegmentIndex < segments.size(); ++i)
|
||||
{
|
||||
auto const & checkedSegment = segments[outgoingSegmentIndex + i];
|
||||
if (checkedSegment.m_path.size() < 2)
|
||||
return 0;
|
||||
|
||||
if (checkedSegment.m_roadNameInfo.m_name == masterSegment.m_roadNameInfo.m_name &&
|
||||
checkedSegment.m_highwayClass == masterSegment.m_highwayClass &&
|
||||
checkedSegment.m_isLink == masterSegment.m_isLink && !checkedSegment.m_onRoundabout)
|
||||
{
|
||||
auto const & path = masterSegment.m_path;
|
||||
auto const & pointBeforeTurn = path[path.size() - 2];
|
||||
auto const & turnPoint = path[path.size() - 1];
|
||||
auto const & pointAfterTurn = checkedSegment.m_path[1];
|
||||
// Same segment UTurn case.
|
||||
if (i == 0)
|
||||
{
|
||||
// TODO Fix direction calculation.
|
||||
// Warning! We can not determine UTurn direction in single edge case. So we use UTurnLeft.
|
||||
// We decided to add driving rules (left-right sided driving) to mwm header.
|
||||
if (pointBeforeTurn == pointAfterTurn && turnPoint != pointBeforeTurn)
|
||||
{
|
||||
turn.m_turn = CarDirection::UTurnLeft;
|
||||
return 1;
|
||||
}
|
||||
// Wide UTurn must have link in it's middle.
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Avoid the UTurn on unnamed roads inside the rectangle based distinct.
|
||||
if (checkedSegment.m_roadNameInfo.m_name.empty())
|
||||
return 0;
|
||||
|
||||
// Avoid returning to the same edge after uturn somewere else.
|
||||
if (pointBeforeTurn == pointAfterTurn)
|
||||
return 0;
|
||||
|
||||
m2::PointD const v1 = turnPoint.GetPoint() - pointBeforeTurn.GetPoint();
|
||||
m2::PointD const v2 = pointAfterTurn.GetPoint() - checkedSegment.m_path[0].GetPoint();
|
||||
|
||||
auto angle = ang::TwoVectorsAngle(m2::PointD::Zero(), v1, v2);
|
||||
|
||||
if (!AlmostEqualAbs(angle, math::pi, kUTurnHeadingSensitivity))
|
||||
return 0;
|
||||
|
||||
// Determine turn direction.
|
||||
m2::PointD const junctionPoint = masterSegment.m_path.back().GetPoint();
|
||||
|
||||
m2::PointD const ingoingPoint =
|
||||
GetPointForTurn(result, outgoingSegmentIndex, numMwmIds, vehicleSettings.m_maxIngoingPointsCount,
|
||||
vehicleSettings.m_minIngoingDistMeters, false /* forward */);
|
||||
m2::PointD const outgoingPoint =
|
||||
GetPointForTurn(result, outgoingSegmentIndex, numMwmIds, vehicleSettings.m_maxOutgoingPointsCount,
|
||||
vehicleSettings.m_minOutgoingDistMeters, true /* forward */);
|
||||
|
||||
if (PiMinusTwoVectorsAngle(junctionPoint, ingoingPoint, outgoingPoint) < 0)
|
||||
turn.m_turn = CarDirection::UTurnLeft;
|
||||
else
|
||||
turn.m_turn = CarDirection::UTurnRight;
|
||||
return i + 1;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
} // namespace routing
|
||||
56
libs/routing/car_directions.hpp
Normal file
56
libs/routing/car_directions.hpp
Normal file
|
|
@ -0,0 +1,56 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/directions_engine.hpp"
|
||||
|
||||
#include "routing/route.hpp"
|
||||
#include "routing_common/num_mwm_id.hpp"
|
||||
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
|
||||
class CarDirectionsEngine : public DirectionsEngine
|
||||
{
|
||||
public:
|
||||
CarDirectionsEngine(MwmDataSource & dataSource, std::shared_ptr<NumMwmIds> numMwmIds);
|
||||
|
||||
protected:
|
||||
virtual size_t GetTurnDirection(turns::IRoutingResult const & result, size_t const outgoingSegmentIndex,
|
||||
NumMwmIds const & numMwmIds, RoutingSettings const & vehicleSettings,
|
||||
turns::TurnItem & turn);
|
||||
virtual void FixupTurns(std::vector<RouteSegment> & routeSegments);
|
||||
};
|
||||
|
||||
void FixupCarTurns(std::vector<RouteSegment> & routeSegments);
|
||||
|
||||
/*!
|
||||
* \brief Finds an U-turn that starts from master segment and returns how many segments it lasts.
|
||||
* \returns an index in |segments| that has the opposite direction with master segment
|
||||
* (|segments[currentSegment - 1]|) and 0 if there is no UTurn.
|
||||
* \warning |currentSegment| must be greater than 0.
|
||||
*/
|
||||
size_t CheckUTurnOnRoute(turns::IRoutingResult const & result, size_t const outgoingSegmentIndex,
|
||||
NumMwmIds const & numMwmIds, RoutingSettings const & vehicleSettings, turns::TurnItem & turn);
|
||||
|
||||
/*!
|
||||
* \brief Calculates a turn instruction if the ingoing edge or (and) the outgoing edge belongs to a
|
||||
* roundabout.
|
||||
* \return Returns one of the following results:
|
||||
* - TurnDirection::EnterRoundAbout if the ingoing edge does not belong to a roundabout
|
||||
* and the outgoing edge belongs to a roundabout.
|
||||
* - TurnDirection::StayOnRoundAbout if the ingoing edge and the outgoing edge belong to a
|
||||
* roundabout
|
||||
* and there is a reasonalbe way to leave the junction besides the outgoing edge.
|
||||
* This function does not return TurnDirection::StayOnRoundAbout for small ways to leave the
|
||||
* roundabout.
|
||||
* - TurnDirection::NoTurn if the ingoing edge and the outgoing edge belong to a roundabout
|
||||
* (a) and there is a single way (outgoing edge) to leave the junction.
|
||||
* (b) and there is a way(s) besides outgoing edge to leave the junction (the roundabout)
|
||||
* but it is (they are) relevantly small.
|
||||
*/
|
||||
turns::CarDirection GetRoundaboutDirectionBasic(bool isIngoingEdgeRoundabout, bool isOutgoingEdgeRoundabout,
|
||||
bool isMultiTurnJunction, bool keepTurnByHighwayClass);
|
||||
|
||||
} // namespace routing
|
||||
52
libs/routing/checkpoint_predictor.cpp
Normal file
52
libs/routing/checkpoint_predictor.cpp
Normal file
|
|
@ -0,0 +1,52 @@
|
|||
#include "routing/checkpoint_predictor.hpp"
|
||||
|
||||
#include "geometry/mercator.hpp"
|
||||
|
||||
#include "base/stl_helpers.hpp"
|
||||
|
||||
#include <limits>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
using namespace std;
|
||||
|
||||
// static
|
||||
double CheckpointPredictor::CalculateDeltaMeters(m2::PointD const & from, m2::PointD const & to,
|
||||
m2::PointD const & between)
|
||||
{
|
||||
double const directDist = mercator::DistanceOnEarth(from, to);
|
||||
double const distThroughPoint = mercator::DistanceOnEarth(from, between) + mercator::DistanceOnEarth(between, to);
|
||||
return distThroughPoint - directDist;
|
||||
}
|
||||
|
||||
size_t CheckpointPredictor::PredictPosition(vector<m2::PointD> const & points, m2::PointD const & point) const
|
||||
{
|
||||
double constexpr kInvalidDistance = numeric_limits<double>::max();
|
||||
double minDeltaMeters = kInvalidDistance;
|
||||
size_t minDeltaIdx = 0;
|
||||
// Checkpoints include start, all the intermediate points and finish.
|
||||
size_t const checkpointNum = points.size() + 2 /* for start and finish points */;
|
||||
for (size_t i = 0; i + 1 != checkpointNum; ++i)
|
||||
{
|
||||
double const delta = CalculateDeltaMeters(GetCheckpoint(points, i), GetCheckpoint(points, i + 1), point);
|
||||
if (minDeltaMeters > delta)
|
||||
{
|
||||
minDeltaMeters = delta;
|
||||
minDeltaIdx = i;
|
||||
}
|
||||
}
|
||||
|
||||
CHECK_NOT_EQUAL(minDeltaMeters, kInvalidDistance, ());
|
||||
return minDeltaIdx;
|
||||
}
|
||||
|
||||
m2::PointD const & CheckpointPredictor::GetCheckpoint(vector<m2::PointD> const & points, size_t index) const
|
||||
{
|
||||
if (index == 0)
|
||||
return m_start;
|
||||
if (index <= points.size())
|
||||
return points[index - 1];
|
||||
CHECK_EQUAL(index, points.size() + 1, ());
|
||||
return m_finish;
|
||||
}
|
||||
} // namespace routing
|
||||
41
libs/routing/checkpoint_predictor.hpp
Normal file
41
libs/routing/checkpoint_predictor.hpp
Normal file
|
|
@ -0,0 +1,41 @@
|
|||
#pragma once
|
||||
|
||||
#include "geometry/point2d.hpp"
|
||||
|
||||
#include <cstdint>
|
||||
#include <iterator>
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
/// \brief This class is responsable for prediction of the order of intermediate route points.
|
||||
/// According to current implementation to find the best place for an adding intermediate points
|
||||
/// this class inserts an added intermediate point between points at vector
|
||||
/// {<route start>, <former intermediate points>, <route finish>}, calculates the length
|
||||
/// of the broken line in meters, and finds the sequence of points which gets the shortest length.
|
||||
class CheckpointPredictor final
|
||||
{
|
||||
friend class CheckpointPredictorTest;
|
||||
|
||||
public:
|
||||
CheckpointPredictor(m2::PointD const & start, m2::PointD const & finish) : m_start(start), m_finish(finish) {}
|
||||
|
||||
/// \returns difference between distance |from|->|between| + |between|->|to| and distance |from|->|to|.
|
||||
static double CalculateDeltaMeters(m2::PointD const & from, m2::PointD const & to, m2::PointD const & between);
|
||||
|
||||
/// \brief finds the best position for |point| between |points| to minimize the length
|
||||
/// of broken line of |m_start|, |points| and |m_finish| with inserted |point| between them.
|
||||
/// \param points is a sequence of points on the map.
|
||||
/// \param point is a point to be inserted between |points|.
|
||||
/// \returns zero based index of a line segment between two points at |points|.
|
||||
size_t PredictPosition(std::vector<m2::PointD> const & points, m2::PointD const & point) const;
|
||||
|
||||
private:
|
||||
/// \returns checkpoint by its |index|.
|
||||
/// \note checkpoints is a sequence of points: |m_start|, |points|, |m_finish|.
|
||||
m2::PointD const & GetCheckpoint(std::vector<m2::PointD> const & points, size_t index) const;
|
||||
|
||||
m2::PointD const m_start;
|
||||
m2::PointD const m_finish;
|
||||
};
|
||||
} // namespace routing
|
||||
64
libs/routing/checkpoints.cpp
Normal file
64
libs/routing/checkpoints.cpp
Normal file
|
|
@ -0,0 +1,64 @@
|
|||
#include "routing/checkpoints.hpp"
|
||||
|
||||
#include "geometry/mercator.hpp"
|
||||
|
||||
#include "base/assert.hpp"
|
||||
|
||||
#include <iomanip>
|
||||
#include <sstream>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
Checkpoints::Checkpoints(std::vector<m2::PointD> && points) : m_points(std::move(points))
|
||||
{
|
||||
CHECK_GREATER_OR_EQUAL(m_points.size(), 2, ("Checkpoints should at least contain start and finish"));
|
||||
}
|
||||
|
||||
void Checkpoints::SetPointFrom(m2::PointD const & point)
|
||||
{
|
||||
ASSERT_LESS(m_passedIdx, m_points.size(), ());
|
||||
m_points[m_passedIdx] = point;
|
||||
}
|
||||
|
||||
m2::PointD const & Checkpoints::GetPoint(size_t pointIdx) const
|
||||
{
|
||||
ASSERT_LESS(pointIdx, m_points.size(), ());
|
||||
return m_points[pointIdx];
|
||||
}
|
||||
|
||||
m2::PointD const & Checkpoints::GetPointFrom() const
|
||||
{
|
||||
CHECK(!IsFinished(), ());
|
||||
return GetPoint(m_passedIdx);
|
||||
}
|
||||
|
||||
m2::PointD const & Checkpoints::GetPointTo() const
|
||||
{
|
||||
CHECK(!IsFinished(), ());
|
||||
return GetPoint(m_passedIdx + 1);
|
||||
}
|
||||
|
||||
void Checkpoints::PassNextPoint()
|
||||
{
|
||||
CHECK(!IsFinished(), ());
|
||||
++m_passedIdx;
|
||||
}
|
||||
double Checkpoints::GetSummaryLengthBetweenPointsMeters() const
|
||||
{
|
||||
double dist = 0.0;
|
||||
for (size_t i = 1; i < m_points.size(); ++i)
|
||||
dist += mercator::DistanceOnEarth(m_points[i - 1], m_points[i]);
|
||||
|
||||
return dist;
|
||||
}
|
||||
|
||||
std::string DebugPrint(Checkpoints const & checkpoints)
|
||||
{
|
||||
std::ostringstream out;
|
||||
out << "Checkpoints(";
|
||||
for (auto const & pt : checkpoints.GetPoints())
|
||||
out << DebugPrint(mercator::ToLatLon(pt)) << "; ";
|
||||
out << "passed: " << checkpoints.GetPassedIdx() << ")";
|
||||
return out.str();
|
||||
}
|
||||
} // namespace routing
|
||||
44
libs/routing/checkpoints.hpp
Normal file
44
libs/routing/checkpoints.hpp
Normal file
|
|
@ -0,0 +1,44 @@
|
|||
#pragma once
|
||||
|
||||
#include "geometry/point2d.hpp"
|
||||
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
class Checkpoints final
|
||||
{
|
||||
public:
|
||||
Checkpoints() = default;
|
||||
Checkpoints(m2::PointD const & start, m2::PointD const & finish) : m_points({start, finish}) {}
|
||||
explicit Checkpoints(std::vector<m2::PointD> && points);
|
||||
|
||||
m2::PointD const & GetStart() const { return m_points.front(); }
|
||||
m2::PointD const & GetFinish() const { return m_points.back(); }
|
||||
std::vector<m2::PointD> const & GetPoints() const { return m_points; }
|
||||
size_t GetPassedIdx() const { return m_passedIdx; }
|
||||
|
||||
void SetPointFrom(m2::PointD const & point);
|
||||
m2::PointD const & GetPoint(size_t pointIdx) const;
|
||||
m2::PointD const & GetPointFrom() const;
|
||||
m2::PointD const & GetPointTo() const;
|
||||
|
||||
size_t GetNumSubroutes() const { return m_points.size() - 1; }
|
||||
bool IsFinished() const { return m_passedIdx >= GetNumSubroutes(); }
|
||||
|
||||
double GetSummaryLengthBetweenPointsMeters() const;
|
||||
|
||||
void PassNextPoint();
|
||||
|
||||
private:
|
||||
// m_points contains start, finish and intermediate points.
|
||||
std::vector<m2::PointD> m_points = {m2::PointD::Zero(), m2::PointD::Zero()};
|
||||
// m_passedIdx is the index of the checkpoint by which the user passed by.
|
||||
// By default, user has passed 0, it is start point.
|
||||
size_t m_passedIdx = 0;
|
||||
};
|
||||
|
||||
std::string DebugPrint(Checkpoints const & checkpoints);
|
||||
} // namespace routing
|
||||
44
libs/routing/city_roads.cpp
Normal file
44
libs/routing/city_roads.cpp
Normal file
|
|
@ -0,0 +1,44 @@
|
|||
#include "routing/city_roads.hpp"
|
||||
|
||||
#include "routing/city_roads_serialization.hpp"
|
||||
|
||||
#include "indexer/data_source.hpp"
|
||||
|
||||
#include "base/logging.hpp"
|
||||
|
||||
#include <utility>
|
||||
|
||||
#include "defines.hpp"
|
||||
|
||||
namespace routing
|
||||
{
|
||||
bool CityRoads::IsCityRoad(uint32_t fid) const
|
||||
{
|
||||
return fid < m_cityRoads.size() ? m_cityRoads[fid] : false;
|
||||
}
|
||||
|
||||
void CityRoads::Load(ReaderT const & reader)
|
||||
{
|
||||
ReaderSource<ReaderT> src(reader);
|
||||
CityRoadsSerializer::Deserialize(src, m_cityRoadsRegion, m_cityRoads);
|
||||
}
|
||||
|
||||
std::unique_ptr<CityRoads> LoadCityRoads(MwmSet::MwmHandle const & handle)
|
||||
{
|
||||
auto const * value = handle.GetValue();
|
||||
CHECK(value, ());
|
||||
|
||||
try
|
||||
{
|
||||
auto cityRoads = std::make_unique<CityRoads>();
|
||||
if (value->m_cont.IsExist(CITY_ROADS_FILE_TAG))
|
||||
cityRoads->Load(value->m_cont.GetReader(CITY_ROADS_FILE_TAG));
|
||||
return cityRoads;
|
||||
}
|
||||
catch (Reader::Exception const & e)
|
||||
{
|
||||
LOG(LERROR, ("File", value->GetCountryFileName(), "Error while reading", CITY_ROADS_FILE_TAG, "section.", e.Msg()));
|
||||
return std::make_unique<CityRoads>();
|
||||
}
|
||||
}
|
||||
} // namespace routing
|
||||
34
libs/routing/city_roads.hpp
Normal file
34
libs/routing/city_roads.hpp
Normal file
|
|
@ -0,0 +1,34 @@
|
|||
#pragma once
|
||||
|
||||
#include "indexer/mwm_set.hpp"
|
||||
|
||||
#include "coding/memory_region.hpp"
|
||||
#include "coding/reader.hpp"
|
||||
|
||||
#include <memory>
|
||||
|
||||
#include "3party/succinct/elias_fano.hpp"
|
||||
|
||||
namespace routing
|
||||
{
|
||||
class CityRoads
|
||||
{
|
||||
public:
|
||||
bool HaveCityRoads() const { return m_cityRoads.size() > 0; }
|
||||
/// \returns true if |fid| is a feature id of a road (for cars, bicycles or pedestrians) in city
|
||||
/// or town.
|
||||
/// \note if there's no section with city roads returns false. That means for maps without
|
||||
/// city roads section features are considered as features outside cities.
|
||||
bool IsCityRoad(uint32_t fid) const;
|
||||
|
||||
using ReaderT = ModelReaderPtr;
|
||||
void Load(ReaderT const & reader);
|
||||
|
||||
private:
|
||||
std::unique_ptr<CopiedMemoryRegion> m_cityRoadsRegion;
|
||||
// |m_cityRoads| contains true for feature ids which are roads in cities.
|
||||
succinct::elias_fano m_cityRoads;
|
||||
};
|
||||
|
||||
std::unique_ptr<CityRoads> LoadCityRoads(MwmSet::MwmHandle const & handle);
|
||||
} // namespace routing
|
||||
94
libs/routing/city_roads_serialization.hpp
Normal file
94
libs/routing/city_roads_serialization.hpp
Normal file
|
|
@ -0,0 +1,94 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/city_roads.hpp"
|
||||
|
||||
#include "coding/succinct_mapper.hpp"
|
||||
#include "coding/writer.hpp"
|
||||
|
||||
#include "base/assert.hpp"
|
||||
#include "base/logging.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#include "3party/succinct/elias_fano.hpp"
|
||||
|
||||
namespace routing
|
||||
{
|
||||
struct CityRoadsHeader
|
||||
{
|
||||
template <typename Sink>
|
||||
void Serialize(Sink & sink) const
|
||||
{
|
||||
WriteToSink(sink, m_version);
|
||||
WriteToSink(sink, m_endianness);
|
||||
WriteToSink(sink, m_dataSize);
|
||||
}
|
||||
|
||||
template <typename Source>
|
||||
void Deserialize(Source & src)
|
||||
{
|
||||
m_version = ReadPrimitiveFromSource<uint16_t>(src);
|
||||
m_endianness = ReadPrimitiveFromSource<uint16_t>(src);
|
||||
m_dataSize = ReadPrimitiveFromSource<uint32_t>(src);
|
||||
}
|
||||
|
||||
uint16_t m_version = 0;
|
||||
// Field |m_endianness| is reserved for endianness of the section.
|
||||
uint16_t m_endianness = 0;
|
||||
uint32_t m_dataSize = 0;
|
||||
};
|
||||
|
||||
static_assert(sizeof(CityRoadsHeader) == 8, "Wrong header size of city_roads section.");
|
||||
|
||||
class CityRoadsSerializer
|
||||
{
|
||||
public:
|
||||
CityRoadsSerializer() = delete;
|
||||
|
||||
template <typename Sink>
|
||||
static void Serialize(Sink & sink, std::vector<uint32_t> && cityRoadFeatureIds)
|
||||
{
|
||||
CityRoadsHeader header;
|
||||
auto const startOffset = sink.Pos();
|
||||
header.Serialize(sink);
|
||||
|
||||
std::sort(cityRoadFeatureIds.begin(), cityRoadFeatureIds.end());
|
||||
CHECK(adjacent_find(cityRoadFeatureIds.cbegin(), cityRoadFeatureIds.cend()) == cityRoadFeatureIds.cend(),
|
||||
("City road feature ids should be unique."));
|
||||
succinct::elias_fano::elias_fano_builder builder(cityRoadFeatureIds.back() + 1, cityRoadFeatureIds.size());
|
||||
for (auto fid : cityRoadFeatureIds)
|
||||
builder.push_back(fid);
|
||||
|
||||
coding::FreezeVisitor<Writer> visitor(sink);
|
||||
succinct::elias_fano(&builder).map(visitor);
|
||||
|
||||
auto const endOffset = sink.Pos();
|
||||
header.m_dataSize = static_cast<uint32_t>(endOffset - startOffset - sizeof(CityRoadsHeader));
|
||||
|
||||
sink.Seek(startOffset);
|
||||
header.Serialize(sink);
|
||||
sink.Seek(endOffset);
|
||||
|
||||
LOG(LINFO, ("Serialized", cityRoadFeatureIds.size(), "road feature ids in cities. Size:", endOffset - startOffset,
|
||||
"bytes."));
|
||||
}
|
||||
|
||||
template <typename Source>
|
||||
static void Deserialize(Source & src, std::unique_ptr<CopiedMemoryRegion> & cityRoadsRegion,
|
||||
succinct::elias_fano & cityRoads)
|
||||
{
|
||||
CityRoadsHeader header;
|
||||
header.Deserialize(src);
|
||||
CHECK_EQUAL(header.m_version, 0, ());
|
||||
|
||||
std::vector<uint8_t> data(header.m_dataSize);
|
||||
src.Read(data.data(), data.size());
|
||||
cityRoadsRegion = std::make_unique<CopiedMemoryRegion>(std::move(data));
|
||||
coding::MapVisitor visitor(cityRoadsRegion->ImmutableData());
|
||||
cityRoads.map(visitor);
|
||||
}
|
||||
};
|
||||
} // namespace routing
|
||||
110
libs/routing/coding.hpp
Normal file
110
libs/routing/coding.hpp
Normal file
|
|
@ -0,0 +1,110 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/routing_exceptions.hpp"
|
||||
|
||||
#include "coding/bit_streams.hpp"
|
||||
#include "coding/elias_coder.hpp"
|
||||
|
||||
#include "base/assert.hpp"
|
||||
#include "base/bits.hpp"
|
||||
#include "base/checked_cast.hpp"
|
||||
#include "base/macros.hpp"
|
||||
|
||||
#include <limits>
|
||||
#include <type_traits>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
template <typename T, typename Source>
|
||||
T ReadDelta(BitReader<Source> & reader)
|
||||
{
|
||||
static_assert(std::is_unsigned<T>::value, "T should be an unsigned type");
|
||||
uint64_t const decoded = coding::DeltaCoder::Decode(reader);
|
||||
if (decoded > std::numeric_limits<T>::max())
|
||||
MYTHROW(CorruptedDataException, ("Decoded value", decoded, "out of limit", std::numeric_limits<T>::max()));
|
||||
|
||||
return static_cast<T>(decoded);
|
||||
}
|
||||
|
||||
template <typename T, typename Sink>
|
||||
void WriteDelta(BitWriter<Sink> & writer, T value)
|
||||
{
|
||||
static_assert(std::is_unsigned<T>::value, "T should be an unsigned type");
|
||||
ASSERT_GREATER(value, 0, ());
|
||||
|
||||
bool const success = coding::DeltaCoder::Encode(writer, static_cast<uint64_t>(value));
|
||||
ASSERT(success, ());
|
||||
UNUSED_VALUE(success);
|
||||
}
|
||||
|
||||
template <typename T, typename Source>
|
||||
T ReadGamma(BitReader<Source> & reader)
|
||||
{
|
||||
static_assert(std::is_unsigned<T>::value, "T should be an unsigned type");
|
||||
uint64_t const decoded = coding::GammaCoder::Decode(reader);
|
||||
if (decoded > std::numeric_limits<T>::max())
|
||||
MYTHROW(CorruptedDataException, ("Decoded value", decoded, "out of limit", std::numeric_limits<T>::max()));
|
||||
|
||||
return static_cast<T>(decoded);
|
||||
}
|
||||
|
||||
template <typename T, typename Sink>
|
||||
void WriteGamma(BitWriter<Sink> & writer, T value)
|
||||
{
|
||||
static_assert(std::is_unsigned<T>::value, "T should be an unsigned type");
|
||||
ASSERT_GREATER(value, 0, ());
|
||||
|
||||
bool const success = coding::GammaCoder::Encode(writer, static_cast<uint64_t>(value));
|
||||
ASSERT(success, ());
|
||||
UNUSED_VALUE(success);
|
||||
}
|
||||
|
||||
// C++ standard says:
|
||||
// if the value can't be represented in the destination unsigned type,
|
||||
// the result is implementation-defined.
|
||||
//
|
||||
// ModularCast makes unambiguous conversion from unsigned value to signed.
|
||||
// The resulting value is the least signed integer congruent to the source integer
|
||||
// (modulo 2^n where n is the number of bits used to represent the unsigned type)
|
||||
template <typename Unsigned, typename Signed = std::make_signed_t<Unsigned>>
|
||||
Signed ModularCast(Unsigned value)
|
||||
{
|
||||
static_assert(std::is_unsigned<Unsigned>::value, "T should be an unsigned type");
|
||||
|
||||
if (value <= static_cast<Unsigned>(std::numeric_limits<Signed>::max()))
|
||||
return static_cast<Signed>(value);
|
||||
|
||||
auto constexpr minSignedT = std::numeric_limits<Signed>::min();
|
||||
return static_cast<Signed>(value - static_cast<Unsigned>(minSignedT)) + minSignedT;
|
||||
}
|
||||
|
||||
// Encodes current as delta compared with prev.
|
||||
template <typename T, typename UnsignedT = std::make_unsigned_t<T>>
|
||||
UnsignedT EncodeZigZagDelta(T prev, T current)
|
||||
{
|
||||
static_assert(std::is_integral<T>::value, "T should be an integral type");
|
||||
|
||||
auto const unsignedPrev = static_cast<UnsignedT>(prev);
|
||||
auto const unsignedCurrent = static_cast<UnsignedT>(current);
|
||||
auto originalDelta = ModularCast(static_cast<UnsignedT>(unsignedCurrent - unsignedPrev));
|
||||
static_assert(std::is_same<decltype(originalDelta), std::make_signed_t<T>>::value,
|
||||
"It's expected that ModuleCast returns SignedT");
|
||||
|
||||
auto encodedDelta = bits::ZigZagEncode(originalDelta);
|
||||
static_assert(std::is_same<decltype(encodedDelta), UnsignedT>::value,
|
||||
"It's expected that bits::ZigZagEncode returns UnsignedT");
|
||||
return encodedDelta;
|
||||
}
|
||||
|
||||
// Reverse function for EncodeZigZagDelta.
|
||||
template <typename T, typename UnsignedT = std::make_unsigned_t<T>>
|
||||
T DecodeZigZagDelta(T prev, UnsignedT delta)
|
||||
{
|
||||
static_assert(std::is_integral<T>::value, "T should be an integral type");
|
||||
|
||||
auto decoded = bits::ZigZagDecode(delta);
|
||||
static_assert(std::is_same<decltype(decoded), std::make_signed_t<T>>::value,
|
||||
"It's expected that bits::ZigZagDecode returns SignedT");
|
||||
return prev + static_cast<T>(decoded);
|
||||
}
|
||||
} // namespace routing
|
||||
57
libs/routing/cross_border_graph.cpp
Normal file
57
libs/routing/cross_border_graph.cpp
Normal file
|
|
@ -0,0 +1,57 @@
|
|||
#include "routing/cross_border_graph.hpp"
|
||||
|
||||
#include "geometry/mercator.hpp"
|
||||
|
||||
namespace routing
|
||||
{
|
||||
void CrossBorderGraph::AddCrossBorderSegment(RegionSegmentId segId, CrossBorderSegment const & segment)
|
||||
{
|
||||
m_segments.emplace(segId, segment);
|
||||
|
||||
auto addEndingToMwms = [&](CrossBorderSegmentEnding const & ending)
|
||||
{
|
||||
auto [it, inserted] = m_mwms.emplace(ending.m_numMwmId, std::vector<RegionSegmentId>{segId});
|
||||
if (!inserted)
|
||||
it->second.push_back(segId);
|
||||
};
|
||||
|
||||
addEndingToMwms(segment.m_start);
|
||||
addEndingToMwms(segment.m_end);
|
||||
}
|
||||
|
||||
CrossBorderSegmentEnding::CrossBorderSegmentEnding(m2::PointD const & point, NumMwmId const & mwmId)
|
||||
: m_point(mercator::ToLatLon(point), geometry::kDefaultAltitudeMeters)
|
||||
, m_numMwmId(mwmId)
|
||||
{}
|
||||
|
||||
CrossBorderSegmentEnding::CrossBorderSegmentEnding(ms::LatLon const & point, NumMwmId const & mwmId)
|
||||
: m_point(point, 0)
|
||||
, m_numMwmId(mwmId)
|
||||
{}
|
||||
|
||||
CrossBorderGraphSerializer::Header::Header(CrossBorderGraph const & graph, uint32_t version)
|
||||
: m_numRegions(static_cast<uint32_t>(graph.m_mwms.size()))
|
||||
, m_numRoads(static_cast<uint32_t>(graph.m_segments.size()))
|
||||
, m_version(version)
|
||||
{}
|
||||
|
||||
// static
|
||||
uint32_t CrossBorderGraphSerializer::Hash(std::string const & s)
|
||||
{
|
||||
// We use RSHash (Real Simple Hash) variation. RSHash is a standard hash calculating function
|
||||
// described in Robert Sedgwick's "Algorithms in C". The difference between this implementation
|
||||
// and the original algorithm is that we return hash, not (hash & & 0x7FFFFFFF).
|
||||
uint32_t constexpr b = 378551;
|
||||
uint32_t a = 63689;
|
||||
|
||||
uint32_t hash = 0;
|
||||
|
||||
for (auto c : s)
|
||||
{
|
||||
hash = hash * a + c;
|
||||
a *= b;
|
||||
}
|
||||
|
||||
return hash;
|
||||
}
|
||||
} // namespace routing
|
||||
239
libs/routing/cross_border_graph.hpp
Normal file
239
libs/routing/cross_border_graph.hpp
Normal file
|
|
@ -0,0 +1,239 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/latlon_with_altitude.hpp"
|
||||
|
||||
#include "routing_common/num_mwm_id.hpp"
|
||||
|
||||
#include "coding/point_coding.hpp"
|
||||
#include "coding/reader.hpp"
|
||||
#include "coding/varint.hpp"
|
||||
#include "coding/write_to_sink.hpp"
|
||||
|
||||
#include "geometry/latlon.hpp"
|
||||
#include "geometry/point2d.hpp"
|
||||
|
||||
#include "base/assert.hpp"
|
||||
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
#include <set>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
|
||||
using RegionSegmentId = uint32_t;
|
||||
|
||||
struct CrossBorderSegmentEnding
|
||||
{
|
||||
CrossBorderSegmentEnding() = default;
|
||||
CrossBorderSegmentEnding(m2::PointD const & point, NumMwmId const & mwmId);
|
||||
CrossBorderSegmentEnding(ms::LatLon const & point, NumMwmId const & mwmId);
|
||||
|
||||
LatLonWithAltitude m_point;
|
||||
NumMwmId m_numMwmId = std::numeric_limits<NumMwmId>::max();
|
||||
};
|
||||
|
||||
struct CrossBorderSegment
|
||||
{
|
||||
CrossBorderSegmentEnding m_start;
|
||||
CrossBorderSegmentEnding m_end;
|
||||
|
||||
double m_weight = 0.0;
|
||||
};
|
||||
|
||||
using CrossBorderSegments = std::unordered_map<RegionSegmentId, CrossBorderSegment>;
|
||||
using MwmIdToSegmentIds = std::unordered_map<NumMwmId, std::vector<RegionSegmentId>>;
|
||||
|
||||
struct CrossBorderGraph
|
||||
{
|
||||
void AddCrossBorderSegment(RegionSegmentId segId, CrossBorderSegment const & segment);
|
||||
|
||||
CrossBorderSegments m_segments;
|
||||
MwmIdToSegmentIds m_mwms;
|
||||
};
|
||||
|
||||
class CrossBorderGraphSerializer
|
||||
{
|
||||
public:
|
||||
CrossBorderGraphSerializer() = delete;
|
||||
|
||||
template <class Sink>
|
||||
static void Serialize(CrossBorderGraph const & graph, Sink & sink, std::shared_ptr<NumMwmIds> numMwmIds);
|
||||
|
||||
template <class Source>
|
||||
static void Deserialize(CrossBorderGraph & graph, Source & src, std::shared_ptr<NumMwmIds> numMwmIds);
|
||||
|
||||
private:
|
||||
static uint32_t constexpr kVersion = 1;
|
||||
static double constexpr kDouble2Int = 1.0E6;
|
||||
|
||||
struct Header
|
||||
{
|
||||
Header() = default;
|
||||
explicit Header(CrossBorderGraph const & graph, uint32_t version = kVersion);
|
||||
|
||||
template <class Sink>
|
||||
void Serialize(Sink & sink) const;
|
||||
|
||||
template <class Source>
|
||||
void Deserialize(Source & src);
|
||||
|
||||
uint32_t m_numRegions = 0;
|
||||
uint32_t m_numRoads = 0;
|
||||
|
||||
uint32_t m_version = kVersion;
|
||||
};
|
||||
|
||||
static size_t constexpr kBitsForDouble = 30;
|
||||
|
||||
static uint32_t Hash(std::string const & s);
|
||||
};
|
||||
|
||||
// static
|
||||
template <class Sink>
|
||||
void CrossBorderGraphSerializer::Serialize(CrossBorderGraph const & graph, Sink & sink,
|
||||
std::shared_ptr<NumMwmIds> numMwmIds)
|
||||
{
|
||||
Header header(graph);
|
||||
header.Serialize(sink);
|
||||
|
||||
std::set<uint32_t> mwmNameHashes;
|
||||
|
||||
for (auto it = graph.m_mwms.begin(); it != graph.m_mwms.end(); ++it)
|
||||
{
|
||||
auto const & mwmId = it->first;
|
||||
std::string const & mwmName = numMwmIds->GetFile(mwmId).GetName();
|
||||
auto const hash = Hash(mwmName);
|
||||
|
||||
// Triggering of this check during the maps build means that the mwm set has been changed and
|
||||
// current hash function Hash(mwmName) no longer suits it and should be replaced.
|
||||
CHECK(mwmNameHashes.emplace(hash).second, (mwmId, mwmName, hash));
|
||||
}
|
||||
|
||||
CHECK_LESS(mwmNameHashes.size(), std::numeric_limits<NumMwmId>::max(), ());
|
||||
|
||||
for (auto hash : mwmNameHashes)
|
||||
WriteToSink(sink, hash);
|
||||
|
||||
auto writeSegEnding = [&](CrossBorderSegmentEnding const & ending)
|
||||
{
|
||||
auto const & coord = ending.m_point.GetLatLon();
|
||||
WriteToSink(sink, DoubleToUint32(coord.m_lat, ms::LatLon::kMinLat, ms::LatLon::kMaxLat, kBitsForDouble));
|
||||
WriteToSink(sink, DoubleToUint32(coord.m_lon, ms::LatLon::kMinLon, ms::LatLon::kMaxLon, kBitsForDouble));
|
||||
|
||||
auto const & mwmNameHash = Hash(numMwmIds->GetFile(ending.m_numMwmId).GetName());
|
||||
auto it = mwmNameHashes.find(mwmNameHash);
|
||||
CHECK(it != mwmNameHashes.end(), (ending.m_numMwmId, mwmNameHash));
|
||||
|
||||
NumMwmId const id = std::distance(mwmNameHashes.begin(), it);
|
||||
WriteToSink(sink, id);
|
||||
};
|
||||
|
||||
for (auto const & [segId, seg] : graph.m_segments)
|
||||
{
|
||||
WriteVarUint(sink, segId);
|
||||
|
||||
WriteVarUint(sink, static_cast<uint64_t>(std::lround(seg.m_weight * kDouble2Int)));
|
||||
|
||||
writeSegEnding(seg.m_start);
|
||||
writeSegEnding(seg.m_end);
|
||||
}
|
||||
}
|
||||
|
||||
// static
|
||||
template <class Source>
|
||||
void CrossBorderGraphSerializer::Deserialize(CrossBorderGraph & graph, Source & src,
|
||||
std::shared_ptr<NumMwmIds> numMwmIds)
|
||||
{
|
||||
Header header;
|
||||
header.Deserialize(src);
|
||||
|
||||
graph.m_mwms.reserve(header.m_numRegions);
|
||||
graph.m_segments.reserve(header.m_numRoads);
|
||||
|
||||
std::map<uint32_t, NumMwmId> hashToMwmId;
|
||||
|
||||
numMwmIds->ForEachId([&](NumMwmId id)
|
||||
{
|
||||
std::string const & region = numMwmIds->GetFile(id).GetName();
|
||||
uint32_t const mwmNameHash = Hash(region);
|
||||
// Triggering of this check in runtime means that the latest built mwm set differs from
|
||||
// the previous one ("classic" mwm set which is constant for many years). The solution is to
|
||||
// replace current hash function Hash() and rebuild World.mwm.
|
||||
CHECK(hashToMwmId.emplace(mwmNameHash, id).second, (id, region, mwmNameHash));
|
||||
});
|
||||
|
||||
std::set<uint32_t> mwmNameHashes;
|
||||
|
||||
for (size_t i = 0; i < header.m_numRegions; ++i)
|
||||
{
|
||||
auto const mwmNameHash = ReadPrimitiveFromSource<uint32_t>(src);
|
||||
mwmNameHashes.emplace(mwmNameHash);
|
||||
}
|
||||
|
||||
auto readSegEnding = [&](CrossBorderSegmentEnding & ending)
|
||||
{
|
||||
double const lat = Uint32ToDouble(ReadPrimitiveFromSource<uint32_t>(src), ms::LatLon::kMinLat, ms::LatLon::kMaxLat,
|
||||
kBitsForDouble);
|
||||
double const lon = Uint32ToDouble(ReadPrimitiveFromSource<uint32_t>(src), ms::LatLon::kMinLon, ms::LatLon::kMaxLon,
|
||||
kBitsForDouble);
|
||||
ending.m_point = LatLonWithAltitude(ms::LatLon(lat, lon), geometry::kDefaultAltitudeMeters);
|
||||
|
||||
NumMwmId index = ReadPrimitiveFromSource<uint16_t>(src);
|
||||
CHECK_LESS(index, mwmNameHashes.size(), ());
|
||||
|
||||
auto it = mwmNameHashes.begin();
|
||||
std::advance(it, index);
|
||||
|
||||
auto const & mwmHash = *it;
|
||||
auto itHash = hashToMwmId.find(mwmHash);
|
||||
CHECK(itHash != hashToMwmId.end(), (mwmHash));
|
||||
|
||||
ending.m_numMwmId = itHash->second;
|
||||
};
|
||||
|
||||
for (size_t i = 0; i < header.m_numRoads; ++i)
|
||||
{
|
||||
CrossBorderSegment seg;
|
||||
RegionSegmentId segId;
|
||||
|
||||
if (header.m_version == 0)
|
||||
{
|
||||
segId = ReadPrimitiveFromSource<RegionSegmentId>(src);
|
||||
seg.m_weight = static_cast<double>(ReadPrimitiveFromSource<uint32_t>(src));
|
||||
}
|
||||
else
|
||||
{
|
||||
segId = ReadVarUint<RegionSegmentId>(src);
|
||||
seg.m_weight = static_cast<double>(ReadVarUint<uint64_t>(src));
|
||||
seg.m_weight /= kDouble2Int;
|
||||
}
|
||||
|
||||
readSegEnding(seg.m_start);
|
||||
readSegEnding(seg.m_end);
|
||||
|
||||
graph.AddCrossBorderSegment(segId, seg);
|
||||
}
|
||||
}
|
||||
|
||||
template <class Sink>
|
||||
void CrossBorderGraphSerializer::Header::Serialize(Sink & sink) const
|
||||
{
|
||||
WriteToSink(sink, m_version);
|
||||
WriteToSink(sink, m_numRegions);
|
||||
WriteToSink(sink, m_numRoads);
|
||||
}
|
||||
|
||||
template <class Source>
|
||||
void CrossBorderGraphSerializer::Header::Deserialize(Source & src)
|
||||
{
|
||||
m_version = ReadPrimitiveFromSource<uint32_t>(src);
|
||||
m_numRegions = ReadPrimitiveFromSource<uint32_t>(src);
|
||||
m_numRoads = ReadPrimitiveFromSource<uint32_t>(src);
|
||||
}
|
||||
} // namespace routing
|
||||
19
libs/routing/cross_mwm_connector.cpp
Normal file
19
libs/routing/cross_mwm_connector.cpp
Normal file
|
|
@ -0,0 +1,19 @@
|
|||
#include "routing/cross_mwm_connector.hpp"
|
||||
|
||||
namespace routing
|
||||
{
|
||||
namespace connector
|
||||
{
|
||||
std::string DebugPrint(WeightsLoadState state)
|
||||
{
|
||||
switch (state)
|
||||
{
|
||||
case WeightsLoadState::Unknown: return "Unknown";
|
||||
case WeightsLoadState::ReadyToLoad: return "ReadyToLoad";
|
||||
case WeightsLoadState::NotExists: return "NotExists";
|
||||
case WeightsLoadState::Loaded: return "Loaded";
|
||||
}
|
||||
UNREACHABLE();
|
||||
}
|
||||
} // namespace connector
|
||||
} // namespace routing
|
||||
323
libs/routing/cross_mwm_connector.hpp
Normal file
323
libs/routing/cross_mwm_connector.hpp
Normal file
|
|
@ -0,0 +1,323 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/cross_mwm_ids.hpp"
|
||||
#include "routing/segment.hpp"
|
||||
|
||||
#include "routing/base/small_list.hpp"
|
||||
|
||||
#include "coding/map_uint32_to_val.hpp"
|
||||
#include "coding/sparse_vector.hpp"
|
||||
|
||||
#include "base/assert.hpp"
|
||||
#include "base/buffer_vector.hpp"
|
||||
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
namespace connector
|
||||
{
|
||||
double constexpr kNoRoute = 0.0;
|
||||
|
||||
/// @todo Can we make cross-mwm Weight in minutes and store it as uint16_t?
|
||||
/// Will get 2x less memory without quality loss (minutes is fine granularity for cross-mwm routing).
|
||||
using Weight = uint32_t;
|
||||
Weight constexpr kNoRouteStored = 0;
|
||||
|
||||
enum class WeightsLoadState
|
||||
{
|
||||
Unknown,
|
||||
NotExists,
|
||||
ReadyToLoad,
|
||||
Loaded
|
||||
};
|
||||
|
||||
std::string DebugPrint(WeightsLoadState state);
|
||||
} // namespace connector
|
||||
|
||||
/// @param CrossMwmId Encoded OSM feature (way) ID that should be equal and unique in all MWMs.
|
||||
template <typename CrossMwmId>
|
||||
class CrossMwmConnector final
|
||||
{
|
||||
public:
|
||||
/// Should initialize with some valid mwm id here not to conflict with @see JointSegment::IsFake().
|
||||
explicit CrossMwmConnector(NumMwmId mwmId = kGeneratorMwmId) : m_mwmId(mwmId) {}
|
||||
|
||||
template <class FnT>
|
||||
void ForEachTransitSegmentId(uint32_t featureId, FnT && fn) const
|
||||
{
|
||||
auto it = std::lower_bound(m_transitions.begin(), m_transitions.end(), Key{featureId, 0}, LessKT());
|
||||
while (it != m_transitions.end() && it->first.m_featureId == featureId)
|
||||
{
|
||||
if (fn(it->first.m_segmentIdx))
|
||||
break;
|
||||
++it;
|
||||
}
|
||||
}
|
||||
|
||||
bool IsTransition(Segment const & segment, bool isOutgoing) const
|
||||
{
|
||||
Key const key(segment.GetFeatureId(), segment.GetSegmentIdx());
|
||||
auto const it = std::lower_bound(m_transitions.begin(), m_transitions.end(), key, LessKT());
|
||||
if (it == m_transitions.end() || !(it->first == key))
|
||||
return false;
|
||||
|
||||
auto const & transition = it->second;
|
||||
if (transition.m_oneWay && !segment.IsForward())
|
||||
return false;
|
||||
|
||||
// Note. If |isOutgoing| == true |segment| should be an exit transition segment
|
||||
// (|isEnter| == false) to be a transition segment.
|
||||
// Otherwise |segment| should be an enter transition segment (|isEnter| == true)
|
||||
// to be a transition segment. If not, |segment| is not a transition segment.
|
||||
// Please see documentation on CrossMwmGraph::IsTransition() method for details.
|
||||
bool const isEnter = (segment.IsForward() == transition.m_forwardIsEnter);
|
||||
return isEnter != isOutgoing;
|
||||
}
|
||||
|
||||
CrossMwmId const & GetCrossMwmId(Segment const & segment) const { return GetTransition(segment).m_crossMwmId; }
|
||||
|
||||
/// @return {} if there is no transition for such cross mwm id.
|
||||
std::optional<Segment> GetTransition(CrossMwmId const & crossMwmId, uint32_t segmentIdx, bool isEnter) const
|
||||
{
|
||||
auto const fIt = m_crossMwmIdToFeatureId.find(crossMwmId);
|
||||
if (fIt == m_crossMwmIdToFeatureId.cend())
|
||||
return {};
|
||||
|
||||
uint32_t const featureId = fIt->second;
|
||||
|
||||
Transition const * transition = GetTransition(featureId, segmentIdx);
|
||||
if (transition == nullptr)
|
||||
{
|
||||
/// @todo By VNG: Workaround until cross-mwm transitions generator investigation.
|
||||
/// https://github.com/organicmaps/organicmaps/issues/1736
|
||||
/// Actually, the fix is valid, because transition features can have segment = 1 when leaving MWM
|
||||
/// and segment = 2 when entering MWM due to *not precise* packed MWM borders.
|
||||
if (isEnter)
|
||||
transition = GetTransition(featureId, ++segmentIdx);
|
||||
else if (segmentIdx > 0)
|
||||
transition = GetTransition(featureId, --segmentIdx);
|
||||
|
||||
if (transition == nullptr)
|
||||
return {};
|
||||
}
|
||||
|
||||
ASSERT_EQUAL(transition->m_crossMwmId, crossMwmId, ("fId:", featureId, ", segId:", segmentIdx));
|
||||
bool const isForward = transition->m_forwardIsEnter == isEnter;
|
||||
if (transition->m_oneWay && !isForward)
|
||||
return {};
|
||||
|
||||
return Segment(m_mwmId, featureId, segmentIdx, isForward);
|
||||
}
|
||||
|
||||
using EdgeListT = SmallList<SegmentEdge>;
|
||||
|
||||
template <class FnT>
|
||||
void ForEachEnter(FnT && fn) const
|
||||
{
|
||||
for (auto const & [key, transit] : m_transitions)
|
||||
{
|
||||
if (transit.m_forwardIsEnter)
|
||||
fn(transit.m_enterIdx, Segment(m_mwmId, key.m_featureId, key.m_segmentIdx, true));
|
||||
|
||||
if (!transit.m_oneWay && !transit.m_forwardIsEnter)
|
||||
fn(transit.m_enterIdx, Segment(m_mwmId, key.m_featureId, key.m_segmentIdx, false));
|
||||
}
|
||||
}
|
||||
|
||||
template <class FnT>
|
||||
void ForEachExit(FnT && fn) const
|
||||
{
|
||||
for (auto const & [key, transit] : m_transitions)
|
||||
{
|
||||
if (!transit.m_forwardIsEnter)
|
||||
fn(transit.m_exitIdx, Segment(m_mwmId, key.m_featureId, key.m_segmentIdx, true));
|
||||
|
||||
if (!transit.m_oneWay && transit.m_forwardIsEnter)
|
||||
fn(transit.m_exitIdx, Segment(m_mwmId, key.m_featureId, key.m_segmentIdx, false));
|
||||
}
|
||||
}
|
||||
|
||||
void GetOutgoingEdgeList(Segment const & segment, EdgeListT & edges) const
|
||||
{
|
||||
auto const enterIdx = GetTransition(segment).m_enterIdx;
|
||||
ForEachExit([enterIdx, this, &edges](uint32_t exitIdx, Segment const & s)
|
||||
{ AddEdge(s, enterIdx, exitIdx, edges); });
|
||||
}
|
||||
|
||||
void GetIngoingEdgeList(Segment const & segment, EdgeListT & edges) const
|
||||
{
|
||||
auto const exitIdx = GetTransition(segment).m_exitIdx;
|
||||
ForEachEnter([exitIdx, this, &edges](uint32_t enterIdx, Segment const & s)
|
||||
{ AddEdge(s, enterIdx, exitIdx, edges); });
|
||||
}
|
||||
|
||||
RouteWeight GetWeightSure(Segment const & from, Segment const & to) const
|
||||
{
|
||||
auto const weight = GetWeight(GetTransition(from).m_enterIdx, GetTransition(to).m_exitIdx);
|
||||
ASSERT(weight != connector::kNoRouteStored, ());
|
||||
return RouteWeight::FromCrossMwmWeight(weight);
|
||||
}
|
||||
|
||||
uint32_t GetNumEnters() const { return m_entersCount; }
|
||||
uint32_t GetNumExits() const { return m_exitsCount; }
|
||||
|
||||
bool HasWeights() const { return !m_weights.Empty(); }
|
||||
bool IsEmpty() const { return m_entersCount == 0 && m_exitsCount == 0; }
|
||||
|
||||
bool WeightsWereLoaded() const
|
||||
{
|
||||
switch (m_weights.m_loadState)
|
||||
{
|
||||
case connector::WeightsLoadState::Unknown:
|
||||
case connector::WeightsLoadState::ReadyToLoad: return false;
|
||||
case connector::WeightsLoadState::NotExists:
|
||||
case connector::WeightsLoadState::Loaded: return true;
|
||||
}
|
||||
UNREACHABLE();
|
||||
}
|
||||
|
||||
uint32_t GetWeightIndex(uint32_t enterIdx, uint32_t exitIdx) const
|
||||
{
|
||||
ASSERT_LESS(enterIdx, m_entersCount, ());
|
||||
ASSERT_LESS(exitIdx, m_exitsCount, ());
|
||||
return base::asserted_cast<uint32_t>(size_t(enterIdx) * m_exitsCount + exitIdx);
|
||||
}
|
||||
|
||||
using WeightT = connector::Weight;
|
||||
WeightT GetWeight(uint32_t enterIdx, uint32_t exitIdx) const
|
||||
{
|
||||
WeightT weight;
|
||||
return (m_weights.Get(GetWeightIndex(enterIdx, exitIdx), weight) ? weight : connector::kNoRouteStored);
|
||||
}
|
||||
|
||||
size_t GetMemorySize() const
|
||||
{
|
||||
return (m_transitions.capacity() * sizeof(KeyTransitionT) +
|
||||
m_crossMwmIdToFeatureId.size() * sizeof(typename MwmID2FeatureIDMapT::value_type) +
|
||||
m_weights.GetMemorySize());
|
||||
}
|
||||
|
||||
private:
|
||||
template <class T>
|
||||
friend class CrossMwmConnectorBuilder;
|
||||
|
||||
struct Key
|
||||
{
|
||||
Key(uint32_t featureId, uint32_t segmentIdx) : m_featureId(featureId), m_segmentIdx(segmentIdx) {}
|
||||
|
||||
bool operator==(Key const & key) const
|
||||
{
|
||||
return (m_featureId == key.m_featureId && m_segmentIdx == key.m_segmentIdx);
|
||||
}
|
||||
|
||||
bool operator<(Key const & key) const
|
||||
{
|
||||
if (m_featureId == key.m_featureId)
|
||||
return m_segmentIdx < key.m_segmentIdx;
|
||||
return m_featureId < key.m_featureId;
|
||||
}
|
||||
|
||||
uint32_t m_featureId = 0;
|
||||
uint32_t m_segmentIdx = 0;
|
||||
};
|
||||
|
||||
struct Transition
|
||||
{
|
||||
Transition(uint32_t enterIdx, uint32_t exitIdx, CrossMwmId crossMwmId, bool oneWay, bool forwardIsEnter)
|
||||
: m_enterIdx(enterIdx)
|
||||
, m_exitIdx(exitIdx)
|
||||
, m_crossMwmId(crossMwmId)
|
||||
, m_oneWay(oneWay)
|
||||
, m_forwardIsEnter(forwardIsEnter)
|
||||
{}
|
||||
|
||||
uint32_t m_enterIdx;
|
||||
uint32_t m_exitIdx;
|
||||
CrossMwmId m_crossMwmId;
|
||||
|
||||
// false - Transition represents both forward and backward segments with same featureId, segmentIdx.
|
||||
bool m_oneWay : 1;
|
||||
// true - forward segment is enter to mwm, enter means: m_backPoint is outside mwm borders, m_frontPoint is inside.
|
||||
bool m_forwardIsEnter : 1;
|
||||
};
|
||||
|
||||
void AddEdge(Segment const & segment, uint32_t enterIdx, uint32_t exitIdx, EdgeListT & edges) const
|
||||
{
|
||||
auto const weight = GetWeight(enterIdx, exitIdx);
|
||||
if (weight != connector::kNoRouteStored)
|
||||
edges.emplace_back(segment, RouteWeight::FromCrossMwmWeight(weight));
|
||||
}
|
||||
|
||||
Transition const * GetTransition(uint32_t featureId, uint32_t segmentIdx) const
|
||||
{
|
||||
Key key(featureId, segmentIdx);
|
||||
auto const it = std::lower_bound(m_transitions.begin(), m_transitions.end(), key, LessKT());
|
||||
if (it == m_transitions.end() || !(it->first == key))
|
||||
return nullptr;
|
||||
return &(it->second);
|
||||
}
|
||||
|
||||
Transition const & GetTransition(Segment const & segment) const
|
||||
{
|
||||
Transition const * tr = GetTransition(segment.GetFeatureId(), segment.GetSegmentIdx());
|
||||
CHECK(tr, (segment));
|
||||
return *tr;
|
||||
}
|
||||
|
||||
NumMwmId m_mwmId;
|
||||
uint32_t m_entersCount = 0;
|
||||
uint32_t m_exitsCount = 0;
|
||||
|
||||
using KeyTransitionT = std::pair<Key, Transition>;
|
||||
struct LessKT
|
||||
{
|
||||
bool operator()(KeyTransitionT const & l, KeyTransitionT const & r) const { return l.first < r.first; }
|
||||
bool operator()(KeyTransitionT const & l, Key const & r) const { return l.first < r; }
|
||||
bool operator()(Key const & l, KeyTransitionT const & r) const { return l < r.first; }
|
||||
};
|
||||
std::vector<KeyTransitionT> m_transitions;
|
||||
|
||||
using MwmID2FeatureIDMapT = std::unordered_map<CrossMwmId, uint32_t, connector::HashKey>;
|
||||
MwmID2FeatureIDMapT m_crossMwmIdToFeatureId;
|
||||
|
||||
// Weight is the time required for the route to pass edge, measured in seconds rounded upwards.
|
||||
struct Weights
|
||||
{
|
||||
connector::WeightsLoadState m_loadState = connector::WeightsLoadState::Unknown;
|
||||
uint64_t m_offset = 0;
|
||||
WeightT m_granularity = 0;
|
||||
uint16_t m_version;
|
||||
|
||||
coding::SparseVector<WeightT> m_v1;
|
||||
|
||||
std::unique_ptr<MapUint32ToValue<WeightT>> m_v2;
|
||||
std::unique_ptr<Reader> m_reader;
|
||||
|
||||
bool Empty() const
|
||||
{
|
||||
if (m_version < 2)
|
||||
return m_v1.Empty();
|
||||
else
|
||||
return m_v2 == nullptr;
|
||||
}
|
||||
|
||||
bool Get(uint32_t idx, WeightT & weight) const
|
||||
{
|
||||
if (m_version < 2)
|
||||
if (m_v1.Has(idx))
|
||||
{
|
||||
weight = m_v1.Get(idx);
|
||||
return true;
|
||||
}
|
||||
else
|
||||
return false;
|
||||
else
|
||||
return m_v2->Get(idx, weight);
|
||||
}
|
||||
|
||||
} m_weights;
|
||||
};
|
||||
} // namespace routing
|
||||
637
libs/routing/cross_mwm_connector_serialization.hpp
Normal file
637
libs/routing/cross_mwm_connector_serialization.hpp
Normal file
|
|
@ -0,0 +1,637 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/coding.hpp"
|
||||
#include "routing/cross_mwm_connector.hpp"
|
||||
#include "routing/cross_mwm_ids.hpp"
|
||||
#include "routing/fake_feature_ids.hpp"
|
||||
#include "routing/routing_exceptions.hpp"
|
||||
#include "routing/vehicle_mask.hpp"
|
||||
|
||||
#include "transit/transit_types.hpp"
|
||||
|
||||
#include "coding/bit_streams.hpp"
|
||||
#include "coding/geometry_coding.hpp"
|
||||
#include "coding/reader.hpp"
|
||||
#include "coding/write_to_sink.hpp"
|
||||
#include "coding/writer.hpp"
|
||||
|
||||
#include "base/checked_cast.hpp"
|
||||
#include "base/geo_object_id.hpp"
|
||||
#include "base/macros.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <limits>
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
namespace connector
|
||||
{
|
||||
static uint8_t constexpr kOsmIdBits = 64;
|
||||
static uint8_t constexpr kStopIdBits = 64;
|
||||
static uint8_t constexpr kLineIdBits = 32;
|
||||
|
||||
inline uint32_t constexpr CalcBitsPerTransitId()
|
||||
{
|
||||
return 2 * kStopIdBits + kLineIdBits;
|
||||
}
|
||||
|
||||
template <typename CrossMwmId>
|
||||
uint32_t constexpr GetFeaturesOffset() noexcept;
|
||||
template <>
|
||||
uint32_t constexpr GetFeaturesOffset<base::GeoObjectId>() noexcept
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
template <>
|
||||
uint32_t constexpr GetFeaturesOffset<TransitId>() noexcept
|
||||
{
|
||||
return FakeFeatureIds::kTransitGraphFeaturesStart;
|
||||
}
|
||||
} // namespace connector
|
||||
|
||||
/// Builder class for deserialization.
|
||||
template <typename CrossMwmId>
|
||||
class CrossMwmConnectorBuilder
|
||||
{
|
||||
protected:
|
||||
using ConnectorT = CrossMwmConnector<CrossMwmId>;
|
||||
ConnectorT & m_c;
|
||||
|
||||
// For some connectors we may need to shift features with some offset.
|
||||
// For example for versions and transit section compatibility we number transit features
|
||||
// starting from 0 in mwm and shift them with |m_featureNumerationOffset| in runtime.
|
||||
uint32_t m_featureNumerationOffset = 0;
|
||||
|
||||
public:
|
||||
explicit CrossMwmConnectorBuilder(ConnectorT & c) : m_c(c) {}
|
||||
|
||||
/// Called only in cross-mwm-graph when deserialization connectors.
|
||||
void ApplyNumerationOffset() { m_featureNumerationOffset = connector::GetFeaturesOffset<CrossMwmId>(); }
|
||||
|
||||
void AddTransition(CrossMwmId const & crossMwmId, uint32_t featureId, uint32_t segmentIdx, bool oneWay,
|
||||
bool forwardIsEnter)
|
||||
{
|
||||
featureId += m_featureNumerationOffset;
|
||||
|
||||
typename ConnectorT::Transition transition(m_c.m_entersCount, m_c.m_exitsCount, crossMwmId, oneWay, forwardIsEnter);
|
||||
|
||||
if (forwardIsEnter)
|
||||
++m_c.m_entersCount;
|
||||
else
|
||||
++m_c.m_exitsCount;
|
||||
|
||||
if (!oneWay)
|
||||
{
|
||||
if (forwardIsEnter)
|
||||
++m_c.m_exitsCount;
|
||||
else
|
||||
++m_c.m_entersCount;
|
||||
}
|
||||
|
||||
m_c.m_transitions.emplace_back(typename ConnectorT::Key(featureId, segmentIdx), transition);
|
||||
m_c.m_crossMwmIdToFeatureId.emplace(crossMwmId, featureId);
|
||||
}
|
||||
|
||||
protected:
|
||||
template <class GetTransition>
|
||||
void FillTransitions(size_t count, VehicleType requiredVehicle, GetTransition && getter)
|
||||
{
|
||||
auto const vhMask = GetVehicleMask(requiredVehicle);
|
||||
|
||||
m_c.m_transitions.reserve(count);
|
||||
for (size_t i = 0; i < count; ++i)
|
||||
AddTransition(getter(i), vhMask);
|
||||
|
||||
// Sort by FeatureID to make lower_bound queries.
|
||||
std::sort(m_c.m_transitions.begin(), m_c.m_transitions.end(), typename ConnectorT::LessKT());
|
||||
}
|
||||
|
||||
class Transition final
|
||||
{
|
||||
public:
|
||||
Transition() = default;
|
||||
|
||||
Transition(CrossMwmId const & crossMwmId, uint32_t featureId, uint32_t segmentIdx, VehicleMask roadMask,
|
||||
VehicleMask oneWayMask, bool forwardIsEnter)
|
||||
: m_crossMwmId(crossMwmId)
|
||||
, m_featureId(featureId)
|
||||
, m_segmentIdx(segmentIdx)
|
||||
, m_roadMask(roadMask)
|
||||
, m_oneWayMask(oneWayMask)
|
||||
, m_forwardIsEnter(forwardIsEnter)
|
||||
{}
|
||||
|
||||
template <class Sink>
|
||||
static void WriteCrossMwmId(base::GeoObjectId const & id, uint8_t bits, BitWriter<Sink> & w)
|
||||
{
|
||||
CHECK_LESS_OR_EQUAL(bits, connector::kOsmIdBits, ());
|
||||
w.WriteAtMost64Bits(id.GetEncodedId(), bits);
|
||||
}
|
||||
|
||||
template <class Sink>
|
||||
static void WriteCrossMwmId(connector::TransitId const & id, uint8_t bitsPerCrossMwmId, BitWriter<Sink> & w)
|
||||
{
|
||||
CHECK_EQUAL(bitsPerCrossMwmId, connector::CalcBitsPerTransitId(), ("Wrong TransitId size."));
|
||||
w.WriteAtMost64Bits(id.m_stop1Id, connector::kStopIdBits);
|
||||
w.WriteAtMost64Bits(id.m_stop2Id, connector::kStopIdBits);
|
||||
w.WriteAtMost32Bits(id.m_lineId, connector::kLineIdBits);
|
||||
}
|
||||
|
||||
template <class Sink>
|
||||
void Serialize(uint32_t bitsPerCrossMwmId, uint8_t bitsPerMask, Sink & sink) const
|
||||
{
|
||||
// TODO (@gmoryes):
|
||||
// We do not use back and front point of segment in code, so we just write
|
||||
// zero to this 128 bits. Need to remove this data from mwm section.
|
||||
WriteVarUint<uint64_t>(sink, 0);
|
||||
WriteVarUint<uint64_t>(sink, 0);
|
||||
|
||||
BitWriter<Sink> writer(sink);
|
||||
WriteCrossMwmId(m_crossMwmId, bitsPerCrossMwmId, writer);
|
||||
WriteDelta(writer, m_featureId + 1);
|
||||
WriteDelta(writer, m_segmentIdx + 1);
|
||||
writer.WriteAtMost32Bits(base::asserted_cast<uint32_t>(m_roadMask), bitsPerMask);
|
||||
writer.WriteAtMost32Bits(base::asserted_cast<uint32_t>(m_oneWayMask), bitsPerMask);
|
||||
writer.Write(m_forwardIsEnter ? 0 : 1, 1);
|
||||
}
|
||||
|
||||
template <class Source>
|
||||
static void ReadCrossMwmId(uint8_t bitsPerCrossMwmId, BitReader<Source> & reader, connector::TransitId & readed)
|
||||
{
|
||||
CHECK_EQUAL(bitsPerCrossMwmId, connector::CalcBitsPerTransitId(), ("Wrong TransitId size."));
|
||||
readed.m_stop1Id = reader.ReadAtMost64Bits(connector::kStopIdBits);
|
||||
readed.m_stop2Id = reader.ReadAtMost64Bits(connector::kStopIdBits);
|
||||
readed.m_lineId = reader.ReadAtMost32Bits(connector::kLineIdBits);
|
||||
}
|
||||
|
||||
template <class Source>
|
||||
static void ReadCrossMwmId(uint8_t bits, BitReader<Source> & reader, base::GeoObjectId & osmId)
|
||||
{
|
||||
CHECK_LESS_OR_EQUAL(bits, connector::kOsmIdBits, ());
|
||||
// We lost data about transition type after compression (look at CalcBitsPerCrossMwmId method)
|
||||
// but we used Way in routing, so suggest that it is Osm Way.
|
||||
osmId = base::MakeOsmWay(reader.ReadAtMost64Bits(bits));
|
||||
}
|
||||
|
||||
template <class Source>
|
||||
void Deserialize(uint32_t bitsPerCrossMwmId, uint8_t bitsPerMask, Source & src)
|
||||
{
|
||||
// TODO (@gmoryes)
|
||||
// We do not use back and front point of segment in code, so we just skip this 128 bits.
|
||||
// Need to remove this data from mwm section.
|
||||
UNUSED_VALUE(ReadVarUint<uint64_t>(src));
|
||||
UNUSED_VALUE(ReadVarUint<uint64_t>(src));
|
||||
|
||||
BitReader<Source> reader(src);
|
||||
ReadCrossMwmId(bitsPerCrossMwmId, reader, m_crossMwmId);
|
||||
m_featureId = ReadDelta<decltype(m_featureId)>(reader) - 1;
|
||||
m_segmentIdx = ReadDelta<decltype(m_segmentIdx)>(reader) - 1;
|
||||
m_roadMask = base::asserted_cast<VehicleMask>(reader.ReadAtMost32Bits(bitsPerMask));
|
||||
m_oneWayMask = base::asserted_cast<VehicleMask>(reader.ReadAtMost32Bits(bitsPerMask));
|
||||
m_forwardIsEnter = reader.Read(1) == 0;
|
||||
}
|
||||
|
||||
CrossMwmId const & GetCrossMwmId() const { return m_crossMwmId; }
|
||||
uint32_t GetFeatureId() const { return m_featureId; }
|
||||
uint32_t GetSegmentIdx() const { return m_segmentIdx; }
|
||||
bool ForwardIsEnter() const { return m_forwardIsEnter; }
|
||||
VehicleMask GetRoadMask() const { return m_roadMask; }
|
||||
VehicleMask GetOneWayMask() const { return m_oneWayMask; }
|
||||
|
||||
private:
|
||||
CrossMwmId m_crossMwmId{};
|
||||
uint32_t m_featureId = 0;
|
||||
uint32_t m_segmentIdx = 0;
|
||||
VehicleMask m_roadMask = 0;
|
||||
VehicleMask m_oneWayMask = 0;
|
||||
bool m_forwardIsEnter = false;
|
||||
};
|
||||
|
||||
void CheckBitsPerCrossMwmId(uint32_t bitsPerCrossMwmId) const;
|
||||
|
||||
public:
|
||||
void DeserializeTransitions(VehicleType requiredVehicle, FilesContainerR::TReader & reader)
|
||||
{
|
||||
DeserializeTransitions(requiredVehicle, *(reader.GetPtr()));
|
||||
}
|
||||
|
||||
template <class Reader>
|
||||
void DeserializeTransitions(VehicleType requiredVehicle, Reader & reader)
|
||||
{
|
||||
CHECK(m_c.m_weights.m_loadState == connector::WeightsLoadState::Unknown, ());
|
||||
|
||||
NonOwningReaderSource src(reader);
|
||||
Header header;
|
||||
header.Deserialize(src);
|
||||
CheckBitsPerCrossMwmId(header.GetBitsPerCrossMwmId());
|
||||
|
||||
uint64_t weightsOffset = src.Pos() + header.GetSizeTransitions();
|
||||
auto const numTransitions = base::checked_cast<size_t>(header.GetNumTransitions());
|
||||
|
||||
FillTransitions(numTransitions, requiredVehicle, [&header, &src](size_t)
|
||||
{
|
||||
Transition transition;
|
||||
transition.Deserialize(header.GetBitsPerCrossMwmId(), header.GetBitsPerMask(), src);
|
||||
return transition;
|
||||
});
|
||||
|
||||
if (src.Pos() != weightsOffset)
|
||||
{
|
||||
MYTHROW(CorruptedDataException, ("Wrong position", src.Pos(), "after decoding transitions, expected:",
|
||||
weightsOffset, "size:", header.GetSizeTransitions()));
|
||||
}
|
||||
|
||||
for (Section const & section : header.GetSections())
|
||||
{
|
||||
if (section.GetVehicleType() != requiredVehicle)
|
||||
{
|
||||
weightsOffset += section.GetSize();
|
||||
continue;
|
||||
}
|
||||
|
||||
size_t const numEnters = m_c.GetNumEnters();
|
||||
size_t const numExits = m_c.GetNumExits();
|
||||
|
||||
if (base::checked_cast<size_t>(section.GetNumEnters()) != numEnters)
|
||||
{
|
||||
MYTHROW(CorruptedDataException,
|
||||
("Mismatch enters number, section:", section.GetNumEnters(), ", connector:", numEnters));
|
||||
}
|
||||
|
||||
if (base::checked_cast<size_t>(section.GetNumExits()) != numExits)
|
||||
{
|
||||
MYTHROW(CorruptedDataException,
|
||||
("Mismatch exits number, section:", section.GetNumExits(), ", connector:", numExits));
|
||||
}
|
||||
|
||||
m_c.m_weights.m_offset = weightsOffset;
|
||||
m_c.m_weights.m_granularity = header.GetGranularity();
|
||||
m_c.m_weights.m_version = header.GetVersion();
|
||||
m_c.m_weights.m_loadState = connector::WeightsLoadState::ReadyToLoad;
|
||||
return;
|
||||
}
|
||||
|
||||
m_c.m_weights.m_loadState = connector::WeightsLoadState::NotExists;
|
||||
}
|
||||
|
||||
void DeserializeWeights(FilesContainerR::TReader & reader) { DeserializeWeights(*(reader.GetPtr())); }
|
||||
|
||||
/// @param[in] reader Initialized reader for the whole section (makes Skip inside).
|
||||
template <class Reader>
|
||||
void DeserializeWeights(Reader & reader)
|
||||
{
|
||||
CHECK(m_c.m_weights.m_loadState == connector::WeightsLoadState::ReadyToLoad, ());
|
||||
CHECK_GREATER(m_c.m_weights.m_granularity, 0, ());
|
||||
|
||||
size_t const amount = m_c.GetNumEnters() * m_c.GetNumExits();
|
||||
|
||||
if (m_c.m_weights.m_version < 2)
|
||||
{
|
||||
NonOwningReaderSource src(reader);
|
||||
src.Skip(m_c.m_weights.m_offset);
|
||||
|
||||
// Do reserve memory here to avoid a lot of reallocations.
|
||||
// SparseVector will shrink final vector if needed.
|
||||
coding::SparseVectorBuilder<Weight> builder(amount);
|
||||
BitReader bitReader(src);
|
||||
|
||||
Weight prev = 1;
|
||||
for (size_t i = 0; i < amount; ++i)
|
||||
if (bitReader.Read(1) != kNoRouteBit)
|
||||
{
|
||||
Weight const delta = ReadDelta<Weight>(bitReader) - 1;
|
||||
Weight const current = DecodeZigZagDelta(prev, delta);
|
||||
builder.PushValue(current * m_c.m_weights.m_granularity);
|
||||
prev = current;
|
||||
}
|
||||
else
|
||||
builder.PushEmpty();
|
||||
|
||||
m_c.m_weights.m_v1 = builder.Build();
|
||||
}
|
||||
else
|
||||
{
|
||||
m_c.m_weights.m_reader = reader.CreateSubReader(m_c.m_weights.m_offset, reader.Size() - m_c.m_weights.m_offset);
|
||||
m_c.m_weights.m_v2 = MapUint32ToValue<Weight>::Load(
|
||||
*(m_c.m_weights.m_reader),
|
||||
[granularity = m_c.m_weights.m_granularity](NonOwningReaderSource & source, uint32_t blockSize,
|
||||
std::vector<Weight> & values)
|
||||
{
|
||||
values.resize(blockSize);
|
||||
|
||||
uint32_t prev = ReadVarUint<uint32_t>(source);
|
||||
values[0] = granularity * prev;
|
||||
|
||||
for (size_t i = 1; i < blockSize && source.Size() > 0; ++i)
|
||||
{
|
||||
prev += ReadVarInt<int32_t>(source);
|
||||
values[i] = granularity * prev;
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
m_c.m_weights.m_loadState = connector::WeightsLoadState::Loaded;
|
||||
}
|
||||
|
||||
protected:
|
||||
bool AddTransition(Transition const & transition, VehicleMask requiredMask)
|
||||
{
|
||||
if ((transition.GetRoadMask() & requiredMask) == 0)
|
||||
return false;
|
||||
|
||||
bool const isOneWay = (transition.GetOneWayMask() & requiredMask) != 0;
|
||||
AddTransition(transition.GetCrossMwmId(), transition.GetFeatureId(), transition.GetSegmentIdx(), isOneWay,
|
||||
transition.ForwardIsEnter());
|
||||
return true;
|
||||
}
|
||||
|
||||
using Weight = connector::Weight;
|
||||
using WeightsLoadState = connector::WeightsLoadState;
|
||||
|
||||
// 0 - initial version
|
||||
// 1 - removed dummy GeometryCodingParams
|
||||
// 2 - store weights as MapUint32ToValue
|
||||
static uint32_t constexpr kLastVersion = 2;
|
||||
static uint8_t constexpr kNoRouteBit = 0;
|
||||
static uint8_t constexpr kRouteBit = 1;
|
||||
|
||||
// Weight is rounded up to the next multiple of kGranularity before being stored in the section.
|
||||
// kGranularity is measured in seconds as well as Weight.
|
||||
// Increasing kGranularity will decrease the section's size.
|
||||
static Weight constexpr kGranularity = 4;
|
||||
static_assert(kGranularity > 0, "kGranularity should be > 0");
|
||||
|
||||
class Section final
|
||||
{
|
||||
public:
|
||||
Section() = default;
|
||||
|
||||
Section(uint64_t size, uint32_t numEnters, uint32_t numExits, VehicleType vehicleType)
|
||||
: m_size(size)
|
||||
, m_numEnters(numEnters)
|
||||
, m_numExits(numExits)
|
||||
, m_vehicleType(vehicleType)
|
||||
{}
|
||||
|
||||
template <class Sink>
|
||||
void Serialize(Sink & sink) const
|
||||
{
|
||||
WriteToSink(sink, m_size);
|
||||
WriteToSink(sink, m_numEnters);
|
||||
WriteToSink(sink, m_numExits);
|
||||
WriteToSink(sink, static_cast<uint8_t>(m_vehicleType));
|
||||
}
|
||||
|
||||
template <class Source>
|
||||
void Deserialize(Source & src)
|
||||
{
|
||||
m_size = ReadPrimitiveFromSource<decltype(m_size)>(src);
|
||||
m_numEnters = ReadPrimitiveFromSource<decltype(m_numEnters)>(src);
|
||||
m_numExits = ReadPrimitiveFromSource<decltype(m_numExits)>(src);
|
||||
m_vehicleType = static_cast<VehicleType>(ReadPrimitiveFromSource<uint8_t>(src));
|
||||
}
|
||||
|
||||
uint64_t GetSize() const { return m_size; }
|
||||
uint32_t GetNumEnters() const { return m_numEnters; }
|
||||
uint32_t GetNumExits() const { return m_numExits; }
|
||||
VehicleType GetVehicleType() const { return m_vehicleType; }
|
||||
|
||||
private:
|
||||
uint64_t m_size = 0;
|
||||
uint32_t m_numEnters = 0;
|
||||
uint32_t m_numExits = 0;
|
||||
VehicleType m_vehicleType = VehicleType::Pedestrian;
|
||||
};
|
||||
|
||||
class Header final
|
||||
{
|
||||
public:
|
||||
Header() = default;
|
||||
|
||||
Header(uint32_t numTransitions, uint64_t sizeTransitions, uint32_t bitsPerCrossMwmId, uint8_t bitsPerMask)
|
||||
: m_numTransitions(numTransitions)
|
||||
, m_sizeTransitions(sizeTransitions)
|
||||
, m_bitsPerCrossMwmId(bitsPerCrossMwmId)
|
||||
, m_bitsPerMask(bitsPerMask)
|
||||
{}
|
||||
|
||||
template <class Sink>
|
||||
void Serialize(Sink & sink) const
|
||||
{
|
||||
WriteToSink(sink, m_version);
|
||||
WriteToSink(sink, m_numTransitions);
|
||||
WriteToSink(sink, m_sizeTransitions);
|
||||
WriteToSink(sink, m_granularity);
|
||||
WriteToSink(sink, m_bitsPerCrossMwmId);
|
||||
WriteToSink(sink, m_bitsPerMask);
|
||||
|
||||
WriteToSink(sink, base::checked_cast<uint32_t>(m_sections.size()));
|
||||
for (Section const & section : m_sections)
|
||||
section.Serialize(sink);
|
||||
}
|
||||
|
||||
template <class Source>
|
||||
void Deserialize(Source & src)
|
||||
{
|
||||
m_version = ReadPrimitiveFromSource<decltype(m_version)>(src);
|
||||
if (m_version > kLastVersion)
|
||||
{
|
||||
MYTHROW(CorruptedDataException,
|
||||
("Unknown cross mwm section version ", m_version, ", current version ", kLastVersion));
|
||||
}
|
||||
|
||||
m_numTransitions = ReadPrimitiveFromSource<decltype(m_numTransitions)>(src);
|
||||
m_sizeTransitions = ReadPrimitiveFromSource<decltype(m_sizeTransitions)>(src);
|
||||
m_granularity = ReadPrimitiveFromSource<decltype(m_granularity)>(src);
|
||||
if (m_version == 0)
|
||||
serial::GeometryCodingParams().Load(src);
|
||||
m_bitsPerCrossMwmId = ReadPrimitiveFromSource<decltype(m_bitsPerCrossMwmId)>(src);
|
||||
m_bitsPerMask = ReadPrimitiveFromSource<decltype(m_bitsPerMask)>(src);
|
||||
|
||||
auto const sectionsSize = ReadPrimitiveFromSource<uint32_t>(src);
|
||||
m_sections.resize(base::checked_cast<size_t>(sectionsSize));
|
||||
for (Section & section : m_sections)
|
||||
section.Deserialize(src);
|
||||
}
|
||||
|
||||
void AddSection(Section const & section) { m_sections.push_back(section); }
|
||||
|
||||
uint32_t GetVersion() const { return m_version; }
|
||||
uint32_t GetNumTransitions() const { return m_numTransitions; }
|
||||
uint64_t GetSizeTransitions() const { return m_sizeTransitions; }
|
||||
Weight GetGranularity() const { return m_granularity; }
|
||||
uint8_t GetBitsPerCrossMwmId() const { return m_bitsPerCrossMwmId; }
|
||||
uint8_t GetBitsPerMask() const { return m_bitsPerMask; }
|
||||
std::vector<Section> const & GetSections() const { return m_sections; }
|
||||
|
||||
private:
|
||||
uint32_t m_version = kLastVersion;
|
||||
uint32_t m_numTransitions = 0;
|
||||
uint64_t m_sizeTransitions = 0;
|
||||
Weight m_granularity = kGranularity;
|
||||
uint32_t m_bitsPerCrossMwmId = 0;
|
||||
uint8_t m_bitsPerMask = 0;
|
||||
std::vector<Section> m_sections;
|
||||
};
|
||||
};
|
||||
|
||||
template <>
|
||||
inline void CrossMwmConnectorBuilder<base::GeoObjectId>::CheckBitsPerCrossMwmId(uint32_t bitsPerCrossMwmId) const
|
||||
{
|
||||
CHECK_LESS_OR_EQUAL(bitsPerCrossMwmId, connector::kOsmIdBits, ());
|
||||
}
|
||||
|
||||
template <>
|
||||
inline void CrossMwmConnectorBuilder<connector::TransitId>::CheckBitsPerCrossMwmId(uint32_t bitsPerCrossMwmId) const
|
||||
{
|
||||
CHECK_EQUAL(bitsPerCrossMwmId, connector::CalcBitsPerTransitId(), ());
|
||||
}
|
||||
|
||||
/// Builder class for generator and tests.
|
||||
template <typename CrossMwmId>
|
||||
class CrossMwmConnectorBuilderEx : public CrossMwmConnectorBuilder<CrossMwmId>
|
||||
{
|
||||
using BaseT = CrossMwmConnectorBuilder<CrossMwmId>;
|
||||
|
||||
private:
|
||||
uint32_t CalcBitsPerCrossMwmId() const;
|
||||
|
||||
void WriteTransitions(int32_t bitsPerOsmId, uint8_t bitsPerMask, std::vector<uint8_t> & buffer) const
|
||||
{
|
||||
MemWriter<std::vector<uint8_t>> memWriter(buffer);
|
||||
|
||||
for (auto const & transition : m_transitions)
|
||||
transition.Serialize(bitsPerOsmId, bitsPerMask, memWriter);
|
||||
}
|
||||
|
||||
static uint16_t constexpr kBlockSize = 256;
|
||||
|
||||
using Weight = typename BaseT::Weight;
|
||||
using IdxWeightT = std::pair<uint32_t, Weight>;
|
||||
|
||||
void WriteWeights(std::vector<uint8_t> & buffer) const
|
||||
{
|
||||
MapUint32ToValueBuilder<Weight> builder;
|
||||
for (auto const & w : m_weights)
|
||||
builder.Put(w.first, w.second);
|
||||
|
||||
MemWriter writer(buffer);
|
||||
builder.Freeze(writer, [](auto & writer, auto beg, auto end)
|
||||
{
|
||||
auto const NextStoredValue = [&beg]() { return (*beg++ + BaseT::kGranularity - 1) / BaseT::kGranularity; };
|
||||
|
||||
Weight prev = NextStoredValue();
|
||||
WriteVarUint(writer, prev);
|
||||
while (beg != end)
|
||||
{
|
||||
Weight const storedWeight = NextStoredValue();
|
||||
WriteVarInt(writer, static_cast<int32_t>(storedWeight) - static_cast<int32_t>(prev));
|
||||
prev = storedWeight;
|
||||
}
|
||||
}, kBlockSize);
|
||||
}
|
||||
|
||||
public:
|
||||
CrossMwmConnectorBuilderEx() : BaseT(m_connector) {}
|
||||
|
||||
void AddTransition(CrossMwmId const & crossMwmId, uint32_t featureId, uint32_t segmentIdx, VehicleMask roadMask,
|
||||
VehicleMask oneWayMask, bool forwardIsEnter)
|
||||
{
|
||||
m_transitions.emplace_back(crossMwmId, featureId, segmentIdx, roadMask, oneWayMask, forwardIsEnter);
|
||||
}
|
||||
|
||||
size_t GetTransitionsCount() const { return m_transitions.size(); }
|
||||
|
||||
template <class Sink>
|
||||
void Serialize(Sink & sink)
|
||||
{
|
||||
uint32_t const bitsPerCrossMwmId = CalcBitsPerCrossMwmId();
|
||||
auto const bitsPerMask = uint8_t(VehicleType::Count);
|
||||
std::vector<uint8_t> transitionsBuf;
|
||||
WriteTransitions(bitsPerCrossMwmId, bitsPerMask, transitionsBuf);
|
||||
|
||||
typename BaseT::Header header(base::checked_cast<uint32_t>(m_transitions.size()),
|
||||
base::checked_cast<uint64_t>(transitionsBuf.size()), bitsPerCrossMwmId, bitsPerMask);
|
||||
|
||||
std::vector<uint8_t> weightsBuf;
|
||||
if (!m_weights.empty())
|
||||
{
|
||||
std::sort(m_weights.begin(), m_weights.end(), base::LessBy(&IdxWeightT::first));
|
||||
WriteWeights(weightsBuf);
|
||||
|
||||
header.AddSection(typename BaseT::Section(weightsBuf.size(), m_connector.GetNumEnters(),
|
||||
m_connector.GetNumExits(), m_vehicleType));
|
||||
}
|
||||
|
||||
// Use buffer serialization above, because BaseT::Header is not plain (vector<Section>)
|
||||
// and we are not able to calculate its final size.
|
||||
header.Serialize(sink);
|
||||
sink.Write(transitionsBuf.data(), transitionsBuf.size());
|
||||
sink.Write(weightsBuf.data(), weightsBuf.size());
|
||||
}
|
||||
|
||||
typename BaseT::ConnectorT const & PrepareConnector(VehicleType requiredVehicle)
|
||||
{
|
||||
m_vehicleType = requiredVehicle;
|
||||
BaseT::FillTransitions(m_transitions.size(), m_vehicleType, [this](size_t i) { return m_transitions[i]; });
|
||||
return m_connector;
|
||||
}
|
||||
|
||||
template <class CalcWeight>
|
||||
void FillWeights(CalcWeight && calcWeight)
|
||||
{
|
||||
CHECK(m_vehicleType != VehicleType::Count, ("PrepareConnector should be called"));
|
||||
|
||||
m_weights.reserve(m_connector.GetNumEnters() * m_connector.GetNumExits());
|
||||
m_connector.ForEachEnter([&](uint32_t enterIdx, Segment const & enter)
|
||||
{
|
||||
m_connector.ForEachExit([&](uint32_t exitIdx, Segment const & exit)
|
||||
{
|
||||
auto const w = calcWeight(enter, exit);
|
||||
CHECK_LESS(w, std::numeric_limits<Weight>::max(), ());
|
||||
|
||||
// Edges weights should be >= astar heuristic, so use std::ceil.
|
||||
if (w != connector::kNoRoute)
|
||||
m_weights.emplace_back(m_connector.GetWeightIndex(enterIdx, exitIdx), static_cast<Weight>(std::ceil(w)));
|
||||
});
|
||||
});
|
||||
}
|
||||
|
||||
/// Used in tests only.
|
||||
void SetAndWriteWeights(std::vector<IdxWeightT> && weights, std::vector<uint8_t> & buffer)
|
||||
{
|
||||
m_weights = std::move(weights);
|
||||
std::sort(m_weights.begin(), m_weights.end(), base::LessBy(&IdxWeightT::first));
|
||||
WriteWeights(buffer);
|
||||
}
|
||||
|
||||
private:
|
||||
// All accumulated transitions with road-mask inside.
|
||||
std::vector<typename BaseT::Transition> m_transitions;
|
||||
|
||||
// Weights for the current prepared connector. Used for VehicleType::Car only now.
|
||||
typename BaseT::ConnectorT m_connector;
|
||||
std::vector<IdxWeightT> m_weights;
|
||||
VehicleType m_vehicleType = VehicleType::Count;
|
||||
};
|
||||
|
||||
template <>
|
||||
inline uint32_t CrossMwmConnectorBuilderEx<base::GeoObjectId>::CalcBitsPerCrossMwmId() const
|
||||
{
|
||||
uint64_t serial = 0;
|
||||
for (auto const & transition : m_transitions)
|
||||
serial = std::max(serial, transition.GetCrossMwmId().GetSerialId());
|
||||
|
||||
// Note that we lose base::GeoObjectId::Type bits here, remember about it in ReadCrossMwmId method.
|
||||
return bits::NumUsedBits(serial);
|
||||
}
|
||||
|
||||
template <>
|
||||
inline uint32_t CrossMwmConnectorBuilderEx<connector::TransitId>::CalcBitsPerCrossMwmId() const
|
||||
{
|
||||
return connector::CalcBitsPerTransitId();
|
||||
}
|
||||
|
||||
} // namespace routing
|
||||
252
libs/routing/cross_mwm_graph.cpp
Normal file
252
libs/routing/cross_mwm_graph.cpp
Normal file
|
|
@ -0,0 +1,252 @@
|
|||
#include "routing/cross_mwm_graph.hpp"
|
||||
|
||||
#include "routing/data_source.hpp"
|
||||
#include "routing/routing_exceptions.hpp"
|
||||
#include "routing/transit_graph.hpp"
|
||||
|
||||
#include "base/assert.hpp"
|
||||
#include "base/stl_helpers.hpp"
|
||||
|
||||
#include "defines.hpp"
|
||||
|
||||
namespace routing
|
||||
{
|
||||
using namespace std;
|
||||
|
||||
CrossMwmGraph::CrossMwmGraph(shared_ptr<NumMwmIds> numMwmIds, shared_ptr<m4::Tree<NumMwmId>> numMwmTree,
|
||||
VehicleType vehicleType, CountryRectFn const & countryRectFn, MwmDataSource & dataSource)
|
||||
: m_dataSource(dataSource)
|
||||
, m_numMwmIds(numMwmIds)
|
||||
, m_numMwmTree(numMwmTree)
|
||||
, m_countryRectFn(countryRectFn)
|
||||
, m_crossMwmIndexGraph(m_dataSource, vehicleType)
|
||||
, m_crossMwmTransitGraph(m_dataSource, VehicleType::Transit)
|
||||
{
|
||||
CHECK(m_numMwmIds, ());
|
||||
CHECK_NOT_EQUAL(vehicleType, VehicleType::Transit, ());
|
||||
}
|
||||
|
||||
bool CrossMwmGraph::IsTransition(Segment const & s, bool isOutgoing)
|
||||
{
|
||||
if (TransitGraph::IsTransitSegment(s))
|
||||
return TransitCrossMwmSectionExists(s.GetMwmId()) ? m_crossMwmTransitGraph.IsTransition(s, isOutgoing) : false;
|
||||
|
||||
return CrossMwmSectionExists(s.GetMwmId()) ? m_crossMwmIndexGraph.IsTransition(s, isOutgoing) : false;
|
||||
}
|
||||
|
||||
bool CrossMwmGraph::GetAllLoadedNeighbors(NumMwmId numMwmId, vector<NumMwmId> & neighbors)
|
||||
{
|
||||
CHECK(m_numMwmTree, ());
|
||||
m2::RectD const rect = m_countryRectFn(m_numMwmIds->GetFile(numMwmId).GetName());
|
||||
|
||||
bool allNeighborsHaveCrossMwmSection = true;
|
||||
m_numMwmTree->ForEachInRect(rect, [&](NumMwmId id)
|
||||
{
|
||||
if (id == numMwmId)
|
||||
return;
|
||||
MwmStatus const status = GetCrossMwmStatus(id);
|
||||
if (status == MwmStatus::NotLoaded)
|
||||
return;
|
||||
|
||||
if (status == MwmStatus::NoSection)
|
||||
{
|
||||
allNeighborsHaveCrossMwmSection = false;
|
||||
LOG(LWARNING, ("No cross-mwm-section for", m_numMwmIds->GetFile(id)));
|
||||
}
|
||||
|
||||
neighbors.push_back(id);
|
||||
});
|
||||
|
||||
return allNeighborsHaveCrossMwmSection;
|
||||
}
|
||||
|
||||
void CrossMwmGraph::DeserializeTransitions(vector<NumMwmId> const & mwmIds)
|
||||
{
|
||||
for (auto mwmId : mwmIds)
|
||||
m_crossMwmIndexGraph.LoadCrossMwmConnectorWithTransitions(mwmId);
|
||||
}
|
||||
|
||||
void CrossMwmGraph::DeserializeTransitTransitions(vector<NumMwmId> const & mwmIds)
|
||||
{
|
||||
for (auto mwmId : mwmIds)
|
||||
{
|
||||
// Some mwms may not have cross mwm transit section.
|
||||
if (TransitCrossMwmSectionExists(mwmId))
|
||||
m_crossMwmTransitGraph.LoadCrossMwmConnectorWithTransitions(mwmId);
|
||||
}
|
||||
}
|
||||
|
||||
void CrossMwmGraph::GetTwins(Segment const & s, bool isOutgoing, vector<Segment> & twins)
|
||||
{
|
||||
ASSERT(IsTransition(s, isOutgoing), (s, isOutgoing));
|
||||
// Note. There's an extremely rare case when a segment is ingoing and outgoing at the same time.
|
||||
// |twins| is not filled for such cases. For details please see a note in
|
||||
// CrossMwmGraph::GetOutgoingEdgeList().
|
||||
if (IsTransition(s, !isOutgoing))
|
||||
return;
|
||||
|
||||
twins.clear();
|
||||
|
||||
// If you got ASSERTs here, check that m_numMwmIds and m_numMwmTree are initialized with valid
|
||||
// country MWMs only, without World*, minsk-pass, or any other test MWMs.
|
||||
// This may happen with ill-formed routing integration tests.
|
||||
#ifdef DEBUG
|
||||
auto const & file = m_numMwmIds->GetFile(s.GetMwmId());
|
||||
#endif
|
||||
|
||||
vector<NumMwmId> neighbors;
|
||||
bool const allNeighborsHaveCrossMwmSection = GetAllLoadedNeighbors(s.GetMwmId(), neighbors);
|
||||
ASSERT(allNeighborsHaveCrossMwmSection, (file));
|
||||
|
||||
MwmStatus const currentMwmStatus = GetCrossMwmStatus(s.GetMwmId());
|
||||
ASSERT_EQUAL(currentMwmStatus, MwmStatus::SectionExists, (file));
|
||||
|
||||
if (TransitGraph::IsTransitSegment(s) && TransitCrossMwmSectionExists(s.GetMwmId()))
|
||||
{
|
||||
DeserializeTransitTransitions(neighbors);
|
||||
m_crossMwmTransitGraph.GetTwinsByCrossMwmId(s, isOutgoing, neighbors, twins);
|
||||
}
|
||||
else if (allNeighborsHaveCrossMwmSection && currentMwmStatus == MwmStatus::SectionExists)
|
||||
{
|
||||
DeserializeTransitions(neighbors);
|
||||
m_crossMwmIndexGraph.GetTwinsByCrossMwmId(s, isOutgoing, neighbors, twins);
|
||||
}
|
||||
else
|
||||
{
|
||||
ASSERT(false, ("All country MWMs should have cross-mwm-section", file));
|
||||
return;
|
||||
}
|
||||
|
||||
for (Segment const & t : twins)
|
||||
CHECK_NOT_EQUAL(s.GetMwmId(), t.GetMwmId(), ());
|
||||
}
|
||||
|
||||
void CrossMwmGraph::GetOutgoingEdgeList(Segment const & enter, EdgeListT & edges)
|
||||
{
|
||||
ASSERT(IsTransition(enter, false /* isEnter */),
|
||||
("The segment is not a transition segment. IsTransition(", enter, ", false) returns false."));
|
||||
|
||||
// Is not supported yet.
|
||||
/*
|
||||
if (TransitGraph::IsTransitSegment(enter))
|
||||
{
|
||||
if (TransitCrossMwmSectionExists(enter.GetMwmId()))
|
||||
m_crossMwmTransitGraph.GetOutgoingEdgeList(enter, edges);
|
||||
return;
|
||||
}
|
||||
*/
|
||||
|
||||
if (CrossMwmSectionExists(enter.GetMwmId()))
|
||||
m_crossMwmIndexGraph.GetOutgoingEdgeList(enter, edges);
|
||||
}
|
||||
|
||||
void CrossMwmGraph::GetIngoingEdgeList(Segment const & exit, EdgeListT & edges)
|
||||
{
|
||||
ASSERT(IsTransition(exit, true /* isEnter */),
|
||||
("The segment is not a transition segment. IsTransition(", exit, ", true) returns false."));
|
||||
|
||||
// Is not supported yet.
|
||||
/*
|
||||
if (TransitGraph::IsTransitSegment(enter))
|
||||
{
|
||||
if (TransitCrossMwmSectionExists(enter.GetMwmId()))
|
||||
m_crossMwmTransitGraph.GetIngoingEdgeList(enter, edges);
|
||||
return;
|
||||
}
|
||||
*/
|
||||
|
||||
if (CrossMwmSectionExists(exit.GetMwmId()))
|
||||
m_crossMwmIndexGraph.GetIngoingEdgeList(exit, edges);
|
||||
}
|
||||
|
||||
RouteWeight CrossMwmGraph::GetWeightSure(Segment const & from, Segment const & to)
|
||||
{
|
||||
ASSERT_EQUAL(from.GetMwmId(), to.GetMwmId(), ());
|
||||
ASSERT(CrossMwmSectionExists(from.GetMwmId()), ());
|
||||
return m_crossMwmIndexGraph.GetWeightSure(from, to);
|
||||
}
|
||||
|
||||
// void CrossMwmGraph::Clear()
|
||||
//{
|
||||
// m_crossMwmIndexGraph.Clear();
|
||||
// m_crossMwmTransitGraph.Clear();
|
||||
// }
|
||||
|
||||
void CrossMwmGraph::Purge()
|
||||
{
|
||||
m_crossMwmIndexGraph.Purge();
|
||||
m_crossMwmTransitGraph.Purge();
|
||||
}
|
||||
|
||||
void CrossMwmGraph::GetTwinFeature(Segment const & segment, bool isOutgoing, vector<Segment> & twins)
|
||||
{
|
||||
m_crossMwmIndexGraph.ForEachTransitSegmentId(segment.GetMwmId(), segment.GetFeatureId(),
|
||||
[&](uint32_t transitSegmentId)
|
||||
{
|
||||
Segment const transitSegment(segment.GetMwmId(), segment.GetFeatureId(), transitSegmentId, segment.IsForward());
|
||||
|
||||
if (IsTransition(transitSegment, isOutgoing))
|
||||
{
|
||||
// Get twins and exit.
|
||||
GetTwins(transitSegment, isOutgoing, twins);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
});
|
||||
}
|
||||
|
||||
CrossMwmGraph::MwmStatus CrossMwmGraph::GetMwmStatus(NumMwmId numMwmId, string const & sectionName) const
|
||||
{
|
||||
switch (m_dataSource.GetSectionStatus(numMwmId, sectionName))
|
||||
{
|
||||
case MwmDataSource::MwmNotLoaded: return MwmStatus::NotLoaded;
|
||||
case MwmDataSource::SectionExists: return MwmStatus::SectionExists;
|
||||
case MwmDataSource::NoSection: return MwmStatus::NoSection;
|
||||
}
|
||||
CHECK(false, ("Unreachable"));
|
||||
return MwmStatus::NoSection;
|
||||
}
|
||||
|
||||
CrossMwmGraph::MwmStatus CrossMwmGraph::GetCrossMwmStatus(NumMwmId numMwmId) const
|
||||
{
|
||||
if (m_crossMwmIndexGraph.InCache(numMwmId))
|
||||
return MwmStatus::SectionExists;
|
||||
return GetMwmStatus(numMwmId, CROSS_MWM_FILE_TAG);
|
||||
}
|
||||
|
||||
CrossMwmGraph::MwmStatus CrossMwmGraph::GetTransitCrossMwmStatus(NumMwmId numMwmId) const
|
||||
{
|
||||
if (m_crossMwmTransitGraph.InCache(numMwmId))
|
||||
return MwmStatus::SectionExists;
|
||||
return GetMwmStatus(numMwmId, TRANSIT_CROSS_MWM_FILE_TAG);
|
||||
}
|
||||
|
||||
bool CrossMwmGraph::CrossMwmSectionExists(NumMwmId numMwmId) const
|
||||
{
|
||||
CrossMwmGraph::MwmStatus const status = GetCrossMwmStatus(numMwmId);
|
||||
if (status == MwmStatus::NotLoaded)
|
||||
MYTHROW(RoutingException, ("Mwm", m_numMwmIds->GetFile(numMwmId), "cannot be loaded."));
|
||||
|
||||
return status == MwmStatus::SectionExists;
|
||||
}
|
||||
|
||||
bool CrossMwmGraph::TransitCrossMwmSectionExists(NumMwmId numMwmId) const
|
||||
{
|
||||
CrossMwmGraph::MwmStatus const status = GetTransitCrossMwmStatus(numMwmId);
|
||||
if (status == MwmStatus::NotLoaded)
|
||||
MYTHROW(RoutingException, ("Mwm", m_numMwmIds->GetFile(numMwmId), "cannot be loaded."));
|
||||
|
||||
return status == MwmStatus::SectionExists;
|
||||
}
|
||||
|
||||
string DebugPrint(CrossMwmGraph::MwmStatus status)
|
||||
{
|
||||
switch (status)
|
||||
{
|
||||
case CrossMwmGraph::MwmStatus::NotLoaded: return "CrossMwmGraph::NotLoaded";
|
||||
case CrossMwmGraph::MwmStatus::SectionExists: return "CrossMwmGraph::SectionExists";
|
||||
case CrossMwmGraph::MwmStatus::NoSection: return "CrossMwmGraph::NoSection";
|
||||
}
|
||||
return string("Unknown CrossMwmGraph::MwmStatus.");
|
||||
}
|
||||
} // namespace routing
|
||||
129
libs/routing/cross_mwm_graph.hpp
Normal file
129
libs/routing/cross_mwm_graph.hpp
Normal file
|
|
@ -0,0 +1,129 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/cross_mwm_ids.hpp"
|
||||
#include "routing/cross_mwm_index_graph.hpp"
|
||||
#include "routing/regions_decl.hpp"
|
||||
#include "routing/segment.hpp"
|
||||
#include "routing/vehicle_mask.hpp"
|
||||
|
||||
#include "routing_common/num_mwm_id.hpp"
|
||||
#include "routing_common/vehicle_model.hpp"
|
||||
|
||||
#include "geometry/tree4d.hpp"
|
||||
|
||||
#include "base/geo_object_id.hpp"
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
class MwmDataSource;
|
||||
|
||||
/// \brief Getting information for cross mwm routing.
|
||||
class CrossMwmGraph final
|
||||
{
|
||||
public:
|
||||
enum class MwmStatus
|
||||
{
|
||||
NotLoaded,
|
||||
SectionExists,
|
||||
NoSection,
|
||||
};
|
||||
|
||||
CrossMwmGraph(std::shared_ptr<NumMwmIds> numMwmIds, std::shared_ptr<m4::Tree<NumMwmId>> numMwmTree,
|
||||
VehicleType vehicleType, CountryRectFn const & countryRectFn, MwmDataSource & dataSource);
|
||||
|
||||
/// \brief Transition segment is a segment which is crossed by mwm border. That means
|
||||
/// start and finish of such segment have to lie in different mwms. If a segment is
|
||||
/// crossed by mwm border but its start and finish lie in the same mwm it's not
|
||||
/// a transition segment.
|
||||
/// For most cases there is only one transition segment for a transition feature.
|
||||
/// Transition segment at the picture below is marked as *===>*.
|
||||
/// Transition feature is a feature which contains one or more transition segments.
|
||||
/// Transition features at the picture below is marked as *===>*------>*.
|
||||
/// Every transition feature is duplicated in two neighbouring mwms.
|
||||
/// The duplicate is called "twin" at the picture below.
|
||||
/// For most cases a transition feature contains only one transition segment.
|
||||
/// -------MWM0---------------MWM1----------------MWM2-------------
|
||||
/// | | | |
|
||||
/// | F0 in MWM0 *===>*------>* *===>*------>* F2 of MWM1 |
|
||||
/// | Twin in MWM1 *===>*------>* *===>*------>* Twin in MWM2 |
|
||||
/// | | | |
|
||||
/// | F1 in MWM0 *===>*---->* *===>*--->* F3 of MWM1 |
|
||||
/// | Twin in MWM1 *===>*---->* *===>*--->* Twin in MWM2 |
|
||||
/// | | | |
|
||||
/// ---------------------------------------------------------------
|
||||
/// There are two kinds of transition segments:
|
||||
/// * enter transition segments are enters to their mwms;
|
||||
/// * exit transition segments are exits from their mwms;
|
||||
/// \returns true if |s| is an exit (|isOutgoing| == true) or an enter (|isOutgoing| == false)
|
||||
/// transition segment.
|
||||
bool IsTransition(Segment const & s, bool isOutgoing);
|
||||
|
||||
/// \brief Fills |twins| with duplicates of |s| transition segment in neighbouring mwm.
|
||||
/// For most cases there is only one twin for |s|.
|
||||
/// If |isOutgoing| == true |s| should be an exit transition segment and
|
||||
/// the method fills |twins| with appropriate enter transition segments.
|
||||
/// If |isOutgoing| == false |s| should be an enter transition segment and
|
||||
/// the method fills |twins| with appropriate exit transition segments.
|
||||
/// \note GetTwins(s, isOutgoing, ...) shall be called only if IsTransition(s, isOutgoing) returns
|
||||
/// true.
|
||||
/// \note GetTwins(s, isOutgoing, twins) fills |twins| only if mwm contained |twins| has been
|
||||
/// downloaded.
|
||||
/// If not, |twins| could be emply after a GetTwins(...) call.
|
||||
void GetTwins(Segment const & s, bool isOutgoing, std::vector<Segment> & twins);
|
||||
|
||||
using EdgeListT = CrossMwmIndexGraph<base::GeoObjectId>::EdgeListT;
|
||||
|
||||
/// \brief Fills |edges| with edges outgoing from |s|.
|
||||
/// |s| should be an enter transition segment, |edges| is filled with all edges starting from |s|
|
||||
/// and ending at all reachable exit transition segments of the mwm of |s|.
|
||||
/// Weight of each edge is equal to weight of the route form |s| to |SegmentEdge::m_target|.
|
||||
/// Getting ingoing edges is not supported because we do not have enough information
|
||||
/// to calculate |segment| weight.
|
||||
void GetOutgoingEdgeList(Segment const & s, EdgeListT & edges);
|
||||
void GetIngoingEdgeList(Segment const & s, EdgeListT & edges);
|
||||
|
||||
RouteWeight GetWeightSure(Segment const & from, Segment const & to);
|
||||
|
||||
// void Clear();
|
||||
void Purge();
|
||||
|
||||
template <class FnT>
|
||||
void ForEachTransition(NumMwmId numMwmId, bool isEnter, FnT && fn)
|
||||
{
|
||||
CHECK(CrossMwmSectionExists(numMwmId), ("Should be used in LeapsOnly mode only"));
|
||||
return m_crossMwmIndexGraph.ForEachTransition(numMwmId, isEnter, fn);
|
||||
}
|
||||
|
||||
/// \brief Checks whether feature where |segment| is placed is a cross mwm connector.
|
||||
/// If yes twin-segments are saved to |twins|.
|
||||
void GetTwinFeature(Segment const & segment, bool isOutgoing, std::vector<Segment> & twins);
|
||||
|
||||
private:
|
||||
MwmStatus GetMwmStatus(NumMwmId numMwmId, std::string const & sectionName) const;
|
||||
MwmStatus GetCrossMwmStatus(NumMwmId numMwmId) const;
|
||||
MwmStatus GetTransitCrossMwmStatus(NumMwmId numMwmId) const;
|
||||
bool CrossMwmSectionExists(NumMwmId numMwmId) const;
|
||||
bool TransitCrossMwmSectionExists(NumMwmId numMwmId) const;
|
||||
|
||||
/// \brief Fills |neighbors| with number mwm id of all loaded neighbors of |numMwmId| and
|
||||
/// sets |allNeighborsHaveCrossMwmSection| to true if all loaded neighbors have cross mwm section
|
||||
/// and to false otherwise.
|
||||
bool GetAllLoadedNeighbors(NumMwmId numMwmId, std::vector<NumMwmId> & neighbors);
|
||||
/// \brief Deserizlize transitions for mwm with |ids|.
|
||||
void DeserializeTransitions(std::vector<NumMwmId> const & mwmIds);
|
||||
void DeserializeTransitTransitions(std::vector<NumMwmId> const & mwmIds);
|
||||
|
||||
MwmDataSource & m_dataSource;
|
||||
std::shared_ptr<NumMwmIds> m_numMwmIds;
|
||||
std::shared_ptr<m4::Tree<NumMwmId>> m_numMwmTree;
|
||||
CountryRectFn const & m_countryRectFn;
|
||||
CrossMwmIndexGraph<base::GeoObjectId> m_crossMwmIndexGraph;
|
||||
CrossMwmIndexGraph<connector::TransitId> m_crossMwmTransitGraph;
|
||||
};
|
||||
|
||||
std::string DebugPrint(CrossMwmGraph::MwmStatus status);
|
||||
} // namespace routing
|
||||
65
libs/routing/cross_mwm_ids.hpp
Normal file
65
libs/routing/cross_mwm_ids.hpp
Normal file
|
|
@ -0,0 +1,65 @@
|
|||
#pragma once
|
||||
|
||||
#include "transit/transit_types.hpp"
|
||||
|
||||
#include "base/geo_object_id.hpp"
|
||||
#include "base/newtype.hpp"
|
||||
#include "base/visitor.hpp"
|
||||
|
||||
#include <cstdint>
|
||||
#include <functional>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
namespace connector
|
||||
{
|
||||
// Identifier to find a border edge in neighbouring mwm while cross mwm transition
|
||||
// for transit routing.
|
||||
struct TransitId
|
||||
{
|
||||
DECLARE_VISITOR_AND_DEBUG_PRINT(TransitId, visitor(m_stop1Id, "stop1_id"), visitor(m_stop2Id, "stop2_id"),
|
||||
visitor(m_lineId, "line_id"))
|
||||
|
||||
TransitId() = default;
|
||||
TransitId(transit::StopId stop1Id, transit::StopId stop2Id, transit::LineId lineId)
|
||||
: m_stop1Id(stop1Id)
|
||||
, m_stop2Id(stop2Id)
|
||||
, m_lineId(lineId)
|
||||
{}
|
||||
|
||||
bool operator==(TransitId const & rhs) const
|
||||
{
|
||||
return m_stop1Id == rhs.m_stop1Id && m_stop2Id == rhs.m_stop2Id && m_lineId == rhs.m_lineId;
|
||||
}
|
||||
|
||||
bool operator!=(TransitId const & rhs) const { return !(rhs == *this); }
|
||||
|
||||
bool operator<(TransitId const & rhs) const
|
||||
{
|
||||
if (m_stop1Id != rhs.m_stop1Id)
|
||||
return m_stop1Id < rhs.m_stop1Id;
|
||||
if (m_stop2Id != rhs.m_stop2Id)
|
||||
return m_stop2Id < rhs.m_stop2Id;
|
||||
return m_lineId < rhs.m_lineId;
|
||||
}
|
||||
|
||||
bool operator>(TransitId const & rhs) const { return rhs < *this; }
|
||||
bool operator<=(TransitId const & rhs) const { return !(rhs < *this); }
|
||||
bool operator>=(TransitId const & rhs) const { return !(*this < rhs); }
|
||||
|
||||
transit::StopId m_stop1Id = transit::kInvalidStopId;
|
||||
transit::StopId m_stop2Id = transit::kInvalidStopId;
|
||||
transit::LineId m_lineId = transit::kInvalidLineId;
|
||||
};
|
||||
|
||||
struct HashKey
|
||||
{
|
||||
size_t operator()(base::GeoObjectId const & key) const { return std::hash<base::GeoObjectId>()(key); }
|
||||
|
||||
size_t operator()(TransitId const & key) const
|
||||
{
|
||||
return std::hash<uint64_t>()(key.m_stop1Id ^ key.m_stop2Id ^ static_cast<uint64_t>(key.m_lineId));
|
||||
}
|
||||
};
|
||||
} // namespace connector
|
||||
} // namespace routing
|
||||
274
libs/routing/cross_mwm_index_graph.hpp
Normal file
274
libs/routing/cross_mwm_index_graph.hpp
Normal file
|
|
@ -0,0 +1,274 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/cross_mwm_connector.hpp"
|
||||
#include "routing/cross_mwm_connector_serialization.hpp"
|
||||
#include "routing/data_source.hpp"
|
||||
#include "routing/segment.hpp"
|
||||
#include "routing/vehicle_mask.hpp"
|
||||
|
||||
#include "routing_common/vehicle_model.hpp"
|
||||
|
||||
#include "geometry/point2d.hpp"
|
||||
|
||||
#include "coding/files_container.hpp"
|
||||
#include "coding/point_coding.hpp"
|
||||
#include "coding/reader.hpp"
|
||||
|
||||
#include "indexer/data_source.hpp"
|
||||
|
||||
#include "base/logging.hpp"
|
||||
|
||||
#include <cstdint>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <type_traits>
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
namespace connector
|
||||
{
|
||||
template <typename CrossMwmId>
|
||||
inline FilesContainerR::TReader GetReader(FilesContainerR const & cont)
|
||||
{
|
||||
return cont.GetReader(CROSS_MWM_FILE_TAG);
|
||||
}
|
||||
|
||||
template <>
|
||||
inline FilesContainerR::TReader GetReader<TransitId>(FilesContainerR const & cont)
|
||||
{
|
||||
return cont.GetReader(TRANSIT_CROSS_MWM_FILE_TAG);
|
||||
}
|
||||
|
||||
template <typename CrossMwmId>
|
||||
void AssertConnectorIsFound(NumMwmId neighbor, bool isConnectorFound)
|
||||
{
|
||||
CHECK(isConnectorFound, ("Connector for mwm with number mwm id", neighbor, "was not deserialized."));
|
||||
}
|
||||
|
||||
template <>
|
||||
inline void AssertConnectorIsFound<TransitId>(NumMwmId /* neighbor */, bool /* isConnectorFound */)
|
||||
{}
|
||||
} // namespace connector
|
||||
|
||||
template <typename CrossMwmId>
|
||||
class CrossMwmIndexGraph final
|
||||
{
|
||||
public:
|
||||
using ReaderSourceFile = ReaderSource<FilesContainerR::TReader>;
|
||||
|
||||
CrossMwmIndexGraph(MwmDataSource & dataSource, VehicleType vehicleType)
|
||||
: m_dataSource(dataSource)
|
||||
, m_vehicleType(vehicleType)
|
||||
{}
|
||||
|
||||
bool IsTransition(Segment const & s, bool isOutgoing)
|
||||
{
|
||||
CrossMwmConnector<CrossMwmId> const & c = GetCrossMwmConnectorWithTransitions(s.GetMwmId());
|
||||
return c.IsTransition(s, isOutgoing);
|
||||
}
|
||||
|
||||
template <class FnT>
|
||||
void ForEachTransitSegmentId(NumMwmId numMwmId, uint32_t featureId, FnT && fn)
|
||||
{
|
||||
GetCrossMwmConnectorWithTransitions(numMwmId).ForEachTransitSegmentId(featureId, fn);
|
||||
}
|
||||
|
||||
/// \brief Fills |twins| based on transitions defined in cross_mwm section.
|
||||
/// \note In cross_mwm section transitions are defined by osm ids of theirs features.
|
||||
/// This method fills |twins| with all available twins iff all neighbor MWMs of |s| have cross_mwm section.
|
||||
void GetTwinsByCrossMwmId(Segment const & s, bool isOutgoing, std::vector<NumMwmId> const & neighbors,
|
||||
std::vector<Segment> & twins)
|
||||
{
|
||||
auto const & crossMwmId = GetCrossMwmConnectorWithTransitions(s.GetMwmId()).GetCrossMwmId(s);
|
||||
|
||||
for (NumMwmId const neighbor : neighbors)
|
||||
{
|
||||
auto const it = m_connectors.find(neighbor);
|
||||
// In case of TransitId, a connector for a mwm with number id |neighbor| may not be found
|
||||
// if mwm with such id does not contain corresponding transit_cross_mwm section.
|
||||
// It may happen in case of obsolete mwms.
|
||||
// Note. Actually it is assumed that connectors always must be found for car routing case.
|
||||
// That means mwm without cross_mwm section is not supported.
|
||||
connector::AssertConnectorIsFound<CrossMwmId>(neighbor, it != m_connectors.cend());
|
||||
if (it == m_connectors.cend())
|
||||
continue;
|
||||
|
||||
CrossMwmConnector<CrossMwmId> const & connector = it->second;
|
||||
// Note. Last parameter in the method below (isEnter) should be set to |isOutgoing|.
|
||||
// If |isOutgoing| == true |s| should be an exit transition segment and the method below searches enters
|
||||
// and the last parameter (|isEnter|) should be set to true.
|
||||
// If |isOutgoing| == false |s| should be an enter transition segment and the method below searches exits
|
||||
// and the last parameter (|isEnter|) should be set to false.
|
||||
auto const twinSeg = connector.GetTransition(crossMwmId, s.GetSegmentIdx(), isOutgoing);
|
||||
if (!twinSeg)
|
||||
continue;
|
||||
|
||||
// Twins should have the same direction, because we assume that twins are the same segments
|
||||
// with one difference - they are in the different mwms. Twins could have not same direction
|
||||
// in case of different mwms versions. For example |s| is cross mwm Enter from elder version
|
||||
// and somebody moved points of ways such that |twinSeg| became cross mwm Exit from newer
|
||||
// version. Because of that |twinSeg.IsForward()| will differ from |s.IsForward()|.
|
||||
if (twinSeg->IsForward() != s.IsForward())
|
||||
continue;
|
||||
|
||||
// Checks twins for equality if they are from different mwm versions.
|
||||
// There are same in common, but in case of different version of mwms
|
||||
// their's geometry can differ from each other. Because of this we can not
|
||||
// build the route, because we fail in astar_algorithm.hpp CHECK(invariant) sometimes.
|
||||
if (IsTwinSegmentsEqual(s, *twinSeg))
|
||||
twins.push_back(*twinSeg);
|
||||
else
|
||||
LOG(LDEBUG, ("Bad cross mwm feature, differ in geometry. Current:", s, ", twin:", *twinSeg));
|
||||
}
|
||||
}
|
||||
|
||||
using EdgeListT = typename CrossMwmConnector<CrossMwmId>::EdgeListT;
|
||||
|
||||
void GetOutgoingEdgeList(Segment const & s, EdgeListT & edges)
|
||||
{
|
||||
CrossMwmConnector<CrossMwmId> const & c = GetCrossMwmConnectorWithWeights(s.GetMwmId());
|
||||
c.GetOutgoingEdgeList(s, edges);
|
||||
}
|
||||
|
||||
void GetIngoingEdgeList(Segment const & s, EdgeListT & edges)
|
||||
{
|
||||
CrossMwmConnector<CrossMwmId> const & c = GetCrossMwmConnectorWithWeights(s.GetMwmId());
|
||||
c.GetIngoingEdgeList(s, edges);
|
||||
}
|
||||
|
||||
RouteWeight GetWeightSure(Segment const & from, Segment const & to)
|
||||
{
|
||||
ASSERT_EQUAL(from.GetMwmId(), to.GetMwmId(), ());
|
||||
CrossMwmConnector<CrossMwmId> const & c = GetCrossMwmConnectorWithWeights(from.GetMwmId());
|
||||
return c.GetWeightSure(from, to);
|
||||
}
|
||||
|
||||
// void Clear()
|
||||
// {
|
||||
// m_connectors.clear();
|
||||
// }
|
||||
void Purge()
|
||||
{
|
||||
ConnectersMapT tmp;
|
||||
tmp.swap(m_connectors);
|
||||
}
|
||||
|
||||
bool InCache(NumMwmId numMwmId) const { return m_connectors.count(numMwmId) != 0; }
|
||||
|
||||
CrossMwmConnector<CrossMwmId> const & GetCrossMwmConnectorWithTransitions(NumMwmId numMwmId)
|
||||
{
|
||||
auto const it = m_connectors.find(numMwmId);
|
||||
if (it != m_connectors.cend())
|
||||
return it->second;
|
||||
|
||||
return Deserialize(numMwmId, [this](CrossMwmConnectorBuilder<CrossMwmId> & builder, auto & src)
|
||||
{ builder.DeserializeTransitions(m_vehicleType, src); });
|
||||
}
|
||||
|
||||
void LoadCrossMwmConnectorWithTransitions(NumMwmId numMwmId) { GetCrossMwmConnectorWithTransitions(numMwmId); }
|
||||
|
||||
template <class FnT>
|
||||
void ForEachTransition(NumMwmId numMwmId, bool isEnter, FnT && fn)
|
||||
{
|
||||
auto const & connector = GetCrossMwmConnectorWithTransitions(numMwmId);
|
||||
|
||||
auto const wrapper = [&fn](uint32_t, Segment const & s) { fn(s); };
|
||||
return isEnter ? connector.ForEachEnter(wrapper) : connector.ForEachExit(wrapper);
|
||||
}
|
||||
|
||||
private:
|
||||
std::vector<m2::PointD> GetFeaturePointsBySegment(MwmSet::MwmId const & mwmId, Segment const & segment)
|
||||
{
|
||||
auto ft = m_dataSource.GetFeature({mwmId, segment.GetFeatureId()});
|
||||
ft->ParseGeometry(FeatureType::BEST_GEOMETRY);
|
||||
|
||||
size_t const count = ft->GetPointsCount();
|
||||
std::vector<m2::PointD> geometry;
|
||||
geometry.reserve(count);
|
||||
for (uint32_t i = 0; i < count; ++i)
|
||||
geometry.emplace_back(ft->GetPoint(i));
|
||||
|
||||
return geometry;
|
||||
}
|
||||
|
||||
/// \brief Checks that segments from different MWMs are really equal.
|
||||
/// Compare point by point in case of different MWM versions.
|
||||
bool IsTwinSegmentsEqual(Segment const & s1, Segment const & s2)
|
||||
{
|
||||
// Do not check for transit graph.
|
||||
if (!s1.IsRealSegment() || !s2.IsRealSegment())
|
||||
return true;
|
||||
|
||||
static_assert(
|
||||
std::is_same<CrossMwmId, base::GeoObjectId>::value || std::is_same<CrossMwmId, connector::TransitId>::value,
|
||||
"Be careful of usage other ids here. "
|
||||
"Make sure, there is not crash with your new CrossMwmId");
|
||||
|
||||
ASSERT_NOT_EQUAL(s1.GetMwmId(), s2.GetMwmId(), ());
|
||||
|
||||
auto const mwmId1 = m_dataSource.GetMwmId(s1.GetMwmId());
|
||||
ASSERT(mwmId1.IsAlive(), (s1));
|
||||
auto const mwmId2 = m_dataSource.GetMwmId(s2.GetMwmId());
|
||||
ASSERT(mwmId2.IsAlive(), (s2));
|
||||
|
||||
if (mwmId1.GetInfo()->GetVersion() == mwmId2.GetInfo()->GetVersion())
|
||||
return true;
|
||||
|
||||
auto const geo1 = GetFeaturePointsBySegment(mwmId1, s1);
|
||||
auto const geo2 = GetFeaturePointsBySegment(mwmId2, s2);
|
||||
|
||||
if (geo1.size() != geo2.size())
|
||||
return false;
|
||||
|
||||
for (uint32_t i = 0; i < geo1.size(); ++i)
|
||||
if (!AlmostEqualAbs(geo1[i], geo2[i], kMwmPointAccuracy))
|
||||
return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
CrossMwmConnector<CrossMwmId> const & GetCrossMwmConnectorWithWeights(NumMwmId numMwmId)
|
||||
{
|
||||
auto const & c = GetCrossMwmConnectorWithTransitions(numMwmId);
|
||||
if (c.WeightsWereLoaded())
|
||||
return c;
|
||||
|
||||
return Deserialize(
|
||||
numMwmId, [](CrossMwmConnectorBuilder<CrossMwmId> & builder, auto & src) { builder.DeserializeWeights(src); });
|
||||
}
|
||||
|
||||
/// \brief Deserializes connectors for an mwm with |numMwmId|.
|
||||
/// \param fn is a function implementing deserialization.
|
||||
/// \note Each CrossMwmConnector contained in |m_connectors| may be deserialized in two stages.
|
||||
/// The first one is transition deserialization and the second is weight deserialization.
|
||||
/// Transition deserialization is much faster and used more often.
|
||||
template <typename Fn>
|
||||
CrossMwmConnector<CrossMwmId> const & Deserialize(NumMwmId numMwmId, Fn && fn)
|
||||
{
|
||||
MwmValue const & mwmValue = m_dataSource.GetMwmValue(numMwmId);
|
||||
|
||||
auto it = m_connectors.emplace(numMwmId, CrossMwmConnector<CrossMwmId>(numMwmId)).first;
|
||||
|
||||
CrossMwmConnectorBuilder<CrossMwmId> builder(it->second);
|
||||
builder.ApplyNumerationOffset();
|
||||
|
||||
auto reader = connector::GetReader<CrossMwmId>(mwmValue.m_cont);
|
||||
fn(builder, reader);
|
||||
return it->second;
|
||||
}
|
||||
|
||||
MwmDataSource & m_dataSource;
|
||||
VehicleType m_vehicleType;
|
||||
|
||||
/// \note |m_connectors| contains cache with transition segments and leap edges.
|
||||
/// Each mwm in |m_connectors| may be in two conditions:
|
||||
/// * with loaded transition segments (after a call to
|
||||
/// CrossMwmConnectorSerializer::DeserializeTransitions())
|
||||
/// * with loaded transition segments and with loaded weights
|
||||
/// (after a call to CrossMwmConnectorSerializer::DeserializeTransitions()
|
||||
/// and CrossMwmConnectorSerializer::DeserializeWeights())
|
||||
using ConnectersMapT = std::map<NumMwmId, CrossMwmConnector<CrossMwmId>>;
|
||||
ConnectersMapT m_connectors;
|
||||
};
|
||||
} // namespace routing
|
||||
138
libs/routing/data_source.hpp
Normal file
138
libs/routing/data_source.hpp
Normal file
|
|
@ -0,0 +1,138 @@
|
|||
#pragma once
|
||||
#include "routing/routing_exceptions.hpp"
|
||||
|
||||
#include "routing_common/num_mwm_id.hpp"
|
||||
|
||||
#include "indexer/data_source.hpp"
|
||||
|
||||
#include "base/lru_cache.hpp"
|
||||
|
||||
#include <map>
|
||||
#include <unordered_map>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
// Main purpose is to take and hold MwmHandle-s here (readers and caches).
|
||||
// Routing works in a separate threads and doesn't interfere within route calculation process.
|
||||
/// @note Should add additional thread's caches in case of concurrent routing process.
|
||||
class MwmDataSource
|
||||
{
|
||||
DataSource & m_dataSource;
|
||||
std::shared_ptr<NumMwmIds> m_numMwmIDs;
|
||||
std::unordered_map<NumMwmId, MwmSet::MwmHandle> m_handles;
|
||||
|
||||
// Used for FeaturesRoadGraph in openlr only.
|
||||
std::map<MwmSet::MwmId, MwmSet::MwmHandle> m_handles2;
|
||||
|
||||
// Cache is important for Cross-Mwm routing, where we need at least 2 sources simultaneously.
|
||||
LruCache<MwmSet::MwmId, std::unique_ptr<FeatureSource>> m_featureSources{4};
|
||||
|
||||
MwmSet::MwmHandle const * GetHandleSafe(NumMwmId numMwmId)
|
||||
{
|
||||
ASSERT(m_numMwmIDs, ());
|
||||
auto it = m_handles.find(numMwmId);
|
||||
if (it == m_handles.end())
|
||||
{
|
||||
auto handle = m_dataSource.GetMwmHandleByCountryFile(m_numMwmIDs->GetFile(numMwmId));
|
||||
if (!handle.IsAlive())
|
||||
return nullptr;
|
||||
|
||||
it = m_handles.emplace(numMwmId, std::move(handle)).first;
|
||||
}
|
||||
return &(it->second);
|
||||
}
|
||||
|
||||
public:
|
||||
/// @param[in] numMwmIDs Can be null, if won't call NumMwmId functions.
|
||||
MwmDataSource(DataSource & dataSource, std::shared_ptr<NumMwmIds> numMwmIDs)
|
||||
: m_dataSource(dataSource)
|
||||
, m_numMwmIDs(std::move(numMwmIDs))
|
||||
{}
|
||||
|
||||
void FreeHandles()
|
||||
{
|
||||
m_featureSources.Clear();
|
||||
m_handles.clear();
|
||||
m_handles2.clear();
|
||||
}
|
||||
|
||||
bool IsLoaded(platform::CountryFile const & file) const { return m_dataSource.IsLoaded(file); }
|
||||
|
||||
enum SectionStatus
|
||||
{
|
||||
MwmNotLoaded,
|
||||
SectionExists,
|
||||
NoSection,
|
||||
};
|
||||
|
||||
MwmSet::MwmHandle const & GetHandle(NumMwmId numMwmId)
|
||||
{
|
||||
ASSERT(m_numMwmIDs, ());
|
||||
auto it = m_handles.find(numMwmId);
|
||||
if (it == m_handles.end())
|
||||
{
|
||||
auto const file = m_numMwmIDs->GetFile(numMwmId);
|
||||
auto handle = m_dataSource.GetMwmHandleByCountryFile(file);
|
||||
if (!handle.IsAlive())
|
||||
MYTHROW(RoutingException, ("Mwm", file, "cannot be loaded."));
|
||||
|
||||
it = m_handles.emplace(numMwmId, std::move(handle)).first;
|
||||
}
|
||||
return it->second;
|
||||
}
|
||||
|
||||
MwmValue const & GetMwmValue(NumMwmId numMwmId) { return *GetHandle(numMwmId).GetValue(); }
|
||||
|
||||
SectionStatus GetSectionStatus(NumMwmId numMwmId, std::string const & section)
|
||||
{
|
||||
auto const * handle = GetHandleSafe(numMwmId);
|
||||
if (!handle)
|
||||
return MwmNotLoaded;
|
||||
return handle->GetValue()->m_cont.IsExist(section) ? SectionExists : NoSection;
|
||||
}
|
||||
|
||||
MwmSet::MwmId GetMwmId(NumMwmId numMwmId) const
|
||||
{
|
||||
ASSERT(m_numMwmIDs, ());
|
||||
return m_dataSource.GetMwmIdByCountryFile(m_numMwmIDs->GetFile(numMwmId));
|
||||
}
|
||||
|
||||
template <class FnT>
|
||||
void ForEachStreet(FnT && fn, m2::RectD const & rect)
|
||||
{
|
||||
m_dataSource.ForEachInRect(fn, rect, scales::GetUpperScale());
|
||||
}
|
||||
|
||||
MwmSet::MwmHandle const & GetHandle(MwmSet::MwmId const & mwmId)
|
||||
{
|
||||
if (m_numMwmIDs)
|
||||
{
|
||||
return GetHandle(m_numMwmIDs->GetId(mwmId.GetInfo()->GetLocalFile().GetCountryFile()));
|
||||
}
|
||||
else
|
||||
{
|
||||
auto it = m_handles2.find(mwmId);
|
||||
if (it == m_handles2.end())
|
||||
{
|
||||
auto handle = m_dataSource.GetMwmHandleById(mwmId);
|
||||
if (!handle.IsAlive())
|
||||
MYTHROW(RoutingException, ("Mwm", mwmId.GetInfo()->GetCountryName(), "cannot be loaded."));
|
||||
|
||||
it = m_handles2.emplace(mwmId, std::move(handle)).first;
|
||||
}
|
||||
return it->second;
|
||||
}
|
||||
}
|
||||
|
||||
std::unique_ptr<FeatureType> GetFeature(FeatureID const & id)
|
||||
{
|
||||
bool found = false;
|
||||
auto & ptr = m_featureSources.Find(id.m_mwmId, found);
|
||||
if (!found)
|
||||
ptr = m_dataSource.CreateFeatureSource(GetHandle(id.m_mwmId));
|
||||
|
||||
/// @todo Should we also retrieve "modified" features here?
|
||||
return ptr->GetOriginalFeature(id.m_index);
|
||||
}
|
||||
};
|
||||
} // namespace routing
|
||||
365
libs/routing/directions_engine.cpp
Normal file
365
libs/routing/directions_engine.cpp
Normal file
|
|
@ -0,0 +1,365 @@
|
|||
#include "routing/directions_engine.hpp"
|
||||
|
||||
#include "routing/data_source.hpp"
|
||||
#include "routing/fake_feature_ids.hpp"
|
||||
#include "routing/lanes/lanes_parser.hpp"
|
||||
#include "routing/routing_helpers.hpp"
|
||||
#include "routing/turns.hpp"
|
||||
|
||||
#include "indexer/ftypes_matcher.hpp"
|
||||
|
||||
#include "coding/string_utf8_multilang.hpp"
|
||||
|
||||
#include "geometry/point2d.hpp"
|
||||
|
||||
namespace routing
|
||||
{
|
||||
using namespace ftypes;
|
||||
using namespace routing::turns;
|
||||
using namespace std;
|
||||
|
||||
namespace
|
||||
{
|
||||
bool IsFakeFeature(uint32_t featureId)
|
||||
{
|
||||
return routing::FakeFeatureIds::IsGuidesFeature(featureId) || routing::FakeFeatureIds::IsTransitFeature(featureId);
|
||||
}
|
||||
|
||||
feature::Metadata::EType GetLanesMetadataTag(FeatureType & ft, bool isForward)
|
||||
{
|
||||
auto directionTag =
|
||||
isForward ? feature::Metadata::FMD_TURN_LANES_FORWARD : feature::Metadata::FMD_TURN_LANES_BACKWARD;
|
||||
if (ft.HasMetadata(directionTag))
|
||||
return directionTag;
|
||||
return feature::Metadata::FMD_TURN_LANES;
|
||||
}
|
||||
|
||||
void LoadLanes(LoadedPathSegment & pathSegment, FeatureType & ft, bool isForward)
|
||||
{
|
||||
auto tag = GetLanesMetadataTag(ft, isForward);
|
||||
pathSegment.m_lanes = lanes::ParseLanes(ft.GetMetadata(tag));
|
||||
}
|
||||
} // namespace
|
||||
|
||||
DirectionsEngine::DirectionsEngine(MwmDataSource & dataSource, std::shared_ptr<NumMwmIds> numMwmIds)
|
||||
: m_dataSource(dataSource)
|
||||
, m_numMwmIds(numMwmIds)
|
||||
, m_linkChecker(IsLinkChecker::Instance())
|
||||
, m_roundAboutChecker(IsRoundAboutChecker::Instance())
|
||||
, m_onewayChecker(IsOneWayChecker::Instance())
|
||||
{
|
||||
CHECK(m_numMwmIds, ());
|
||||
}
|
||||
|
||||
void DirectionsEngine::Clear()
|
||||
{
|
||||
m_adjacentEdges.clear();
|
||||
m_pathSegments.clear();
|
||||
}
|
||||
|
||||
unique_ptr<FeatureType> DirectionsEngine::GetFeature(FeatureID const & featureId)
|
||||
{
|
||||
if (IsFakeFeature(featureId.m_index))
|
||||
return nullptr;
|
||||
return m_dataSource.GetFeature(featureId);
|
||||
}
|
||||
|
||||
void DirectionsEngine::LoadPathAttributes(FeatureID const & featureId, LoadedPathSegment & pathSegment, bool isForward)
|
||||
{
|
||||
if (!featureId.IsValid())
|
||||
return;
|
||||
|
||||
auto ft = GetFeature(featureId);
|
||||
if (!ft)
|
||||
return;
|
||||
|
||||
feature::TypesHolder types(*ft);
|
||||
|
||||
LoadLanes(pathSegment, *ft, isForward);
|
||||
|
||||
pathSegment.m_highwayClass = GetHighwayClass(types);
|
||||
ASSERT(pathSegment.m_highwayClass != HighwayClass::Undefined, (featureId));
|
||||
|
||||
pathSegment.m_isLink = m_linkChecker(types);
|
||||
pathSegment.m_onRoundabout = m_roundAboutChecker(types);
|
||||
pathSegment.m_isOneWay = m_onewayChecker(types);
|
||||
|
||||
pathSegment.m_roadNameInfo.m_isLink = pathSegment.m_isLink;
|
||||
pathSegment.m_roadNameInfo.m_junction_ref = ft->GetMetadata(feature::Metadata::FMD_JUNCTION_REF);
|
||||
pathSegment.m_roadNameInfo.m_destination_ref = ft->GetMetadata(feature::Metadata::FMD_DESTINATION_REF);
|
||||
pathSegment.m_roadNameInfo.m_destination = ft->GetMetadata(feature::Metadata::FMD_DESTINATION);
|
||||
/// @todo Should make some better parsing here (@see further use in GetFullRoadName).
|
||||
pathSegment.m_roadNameInfo.m_ref = ft->GetRef();
|
||||
pathSegment.m_roadNameInfo.m_name = ft->GetName(StringUtf8Multilang::kDefaultCode);
|
||||
}
|
||||
|
||||
void DirectionsEngine::GetSegmentRangeAndAdjacentEdges(IRoadGraph::EdgeListT const & outgoingEdges, Edge const & inEdge,
|
||||
uint32_t startSegId, uint32_t endSegId,
|
||||
SegmentRange & segmentRange, TurnCandidates & outgoingTurns)
|
||||
{
|
||||
outgoingTurns.isCandidatesAngleValid = true;
|
||||
outgoingTurns.candidates.reserve(outgoingEdges.size());
|
||||
segmentRange = SegmentRange(inEdge.GetFeatureId(), startSegId, endSegId, inEdge.IsForward(), inEdge.GetStartPoint(),
|
||||
inEdge.GetEndPoint());
|
||||
CHECK(segmentRange.IsCorrect(), ());
|
||||
m2::PointD const & ingoingPoint = inEdge.GetStartJunction().GetPoint();
|
||||
m2::PointD const & junctionPoint = inEdge.GetEndJunction().GetPoint();
|
||||
|
||||
for (auto const & edge : outgoingEdges)
|
||||
{
|
||||
if (edge.IsFake())
|
||||
continue;
|
||||
|
||||
auto ft = GetFeature(edge.GetFeatureId());
|
||||
if (!ft)
|
||||
continue;
|
||||
|
||||
feature::TypesHolder types(*ft);
|
||||
|
||||
auto const highwayClass = GetHighwayClass(types);
|
||||
ASSERT(highwayClass != HighwayClass::Undefined, (edge.PrintLatLon()));
|
||||
|
||||
double angle = 0;
|
||||
|
||||
if (inEdge.GetFeatureId().m_mwmId == edge.GetFeatureId().m_mwmId)
|
||||
{
|
||||
ASSERT_LESS(mercator::DistanceOnEarth(junctionPoint, edge.GetStartJunction().GetPoint()),
|
||||
turns::kFeaturesNearTurnMeters, ());
|
||||
angle =
|
||||
math::RadToDeg(turns::PiMinusTwoVectorsAngle(junctionPoint, ingoingPoint, edge.GetEndJunction().GetPoint()));
|
||||
}
|
||||
else
|
||||
{
|
||||
// Note. In case of crossing mwm border
|
||||
// (inEdge.GetFeatureId().m_mwmId != edge.GetFeatureId().m_mwmId)
|
||||
// twins of inEdge.GetFeatureId() are considered as outgoing features.
|
||||
// In this case that turn candidate angle is invalid and
|
||||
// should not be used for turn generation.
|
||||
outgoingTurns.isCandidatesAngleValid = false;
|
||||
}
|
||||
|
||||
outgoingTurns.candidates.emplace_back(angle, ConvertEdgeToSegment(*m_numMwmIds, edge), highwayClass,
|
||||
m_linkChecker(types));
|
||||
}
|
||||
|
||||
if (outgoingTurns.isCandidatesAngleValid)
|
||||
sort(outgoingTurns.candidates.begin(), outgoingTurns.candidates.end(), base::LessBy(&TurnCandidate::m_angle));
|
||||
}
|
||||
|
||||
void DirectionsEngine::FillPathSegmentsAndAdjacentEdgesMap(IndexRoadGraph const & graph,
|
||||
vector<geometry::PointWithAltitude> const & path,
|
||||
IRoadGraph::EdgeVector const & routeEdges,
|
||||
base::Cancellable const & cancellable)
|
||||
{
|
||||
size_t const pathSize = path.size();
|
||||
CHECK_GREATER(pathSize, 1, ());
|
||||
CHECK_EQUAL(routeEdges.size() + 1, pathSize, ());
|
||||
|
||||
// Filling |m_adjacentEdges|.
|
||||
auto constexpr kInvalidSegId = numeric_limits<uint32_t>::max();
|
||||
// |startSegId| is a value to keep start segment id of a new instance of LoadedPathSegment.
|
||||
uint32_t startSegId = kInvalidSegId;
|
||||
|
||||
vector<geometry::PointWithAltitude> prevJunctions;
|
||||
vector<Segment> prevSegments;
|
||||
for (size_t i = 1; i < pathSize; ++i)
|
||||
{
|
||||
if (cancellable.IsCancelled())
|
||||
return;
|
||||
|
||||
auto const & currJunction = path[i];
|
||||
bool const isCurrJunctionFinish = (i + 1 == pathSize);
|
||||
|
||||
IRoadGraph::EdgeListT outgoingEdges, ingoingEdges;
|
||||
if (!isCurrJunctionFinish)
|
||||
graph.GetOutgoingEdges(currJunction, outgoingEdges);
|
||||
graph.GetIngoingEdges(currJunction, ingoingEdges);
|
||||
|
||||
Edge const & inEdge = routeEdges[i - 1];
|
||||
uint32_t const inSegId = inEdge.GetSegId();
|
||||
|
||||
if (startSegId == kInvalidSegId)
|
||||
startSegId = inSegId;
|
||||
|
||||
prevJunctions.push_back(path[i - 1]);
|
||||
prevSegments.push_back(ConvertEdgeToSegment(*m_numMwmIds, inEdge));
|
||||
|
||||
// inEdge.FeatureId may be invalid in case of adding fake features. It happens for example near starts and finishes.
|
||||
if (!isCurrJunctionFinish && inEdge.GetFeatureId().IsValid() &&
|
||||
!IsJoint(ingoingEdges, outgoingEdges, inEdge, routeEdges[i]))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
CHECK_EQUAL(prevJunctions.size(), static_cast<size_t>(abs(int(inSegId) - int(startSegId)) + 1), ());
|
||||
|
||||
prevJunctions.push_back(currJunction);
|
||||
|
||||
AdjacentEdges adjacentEdges(ingoingEdges.size());
|
||||
SegmentRange segmentRange;
|
||||
GetSegmentRangeAndAdjacentEdges(outgoingEdges, inEdge, startSegId, inSegId, segmentRange,
|
||||
adjacentEdges.m_outgoingTurns);
|
||||
|
||||
LoadedPathSegment pathSegment;
|
||||
pathSegment.m_segmentRange = segmentRange;
|
||||
// |prevSegments| contains segments which corresponds to road edges between joints. In case of a
|
||||
// fake edge a fake segment is created.
|
||||
CHECK_EQUAL(prevSegments.size() + 1, prevJunctions.size(), ());
|
||||
pathSegment.m_path = std::move(prevJunctions);
|
||||
pathSegment.m_segments = std::move(prevSegments);
|
||||
|
||||
LoadPathAttributes(segmentRange.GetFeature(), pathSegment, inEdge.IsForward());
|
||||
|
||||
if (!segmentRange.IsEmpty())
|
||||
{
|
||||
/// @todo By VNG: Here was mostly investigational CHECK.
|
||||
/// Entry already exists, when start-end points are on the same fake segments.
|
||||
|
||||
// bool const isEmpty = adjacentEdges.m_outgoingTurns.candidates.empty();
|
||||
// CHECK(m_adjacentEdges.emplace(segmentRange, std::move(adjacentEdges)).second || isEmpty, ());
|
||||
m_adjacentEdges.emplace(segmentRange, std::move(adjacentEdges));
|
||||
}
|
||||
|
||||
m_pathSegments.push_back(std::move(pathSegment));
|
||||
|
||||
prevJunctions.clear();
|
||||
prevSegments.clear();
|
||||
startSegId = kInvalidSegId;
|
||||
}
|
||||
}
|
||||
|
||||
bool DirectionsEngine::Generate(IndexRoadGraph const & graph, vector<geometry::PointWithAltitude> const & path,
|
||||
base::Cancellable const & cancellable, vector<RouteSegment> & routeSegments)
|
||||
{
|
||||
CHECK(m_numMwmIds, ());
|
||||
|
||||
m_adjacentEdges.clear();
|
||||
m_pathSegments.clear();
|
||||
|
||||
CHECK_NOT_EQUAL(m_vehicleType, VehicleType::Count, (m_vehicleType));
|
||||
|
||||
if (m_vehicleType == VehicleType::Transit)
|
||||
{
|
||||
auto const & segments = graph.GetRouteSegments();
|
||||
uint32_t const segsCount = base::asserted_cast<uint32_t>(segments.size());
|
||||
for (uint32_t i = 0; i < segsCount; ++i)
|
||||
{
|
||||
TurnItem turn;
|
||||
if (i == segsCount - 1)
|
||||
turn = TurnItem(segsCount, turns::PedestrianDirection::ReachedYourDestination);
|
||||
routeSegments.emplace_back(segments[i], turn, path[i + 1], RouteSegment::RoadNameInfo());
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
if (path.size() <= 1)
|
||||
return false;
|
||||
|
||||
IndexRoadGraph::EdgeVector routeEdges;
|
||||
graph.GetRouteEdges(routeEdges);
|
||||
|
||||
if (routeEdges.empty())
|
||||
return false;
|
||||
|
||||
if (cancellable.IsCancelled())
|
||||
return false;
|
||||
|
||||
FillPathSegmentsAndAdjacentEdgesMap(graph, path, routeEdges, cancellable);
|
||||
|
||||
if (cancellable.IsCancelled())
|
||||
return false;
|
||||
|
||||
MakeTurnAnnotation(routeEdges, routeSegments);
|
||||
|
||||
CHECK_EQUAL(routeSegments.size(), routeEdges.size(), ());
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/*!
|
||||
* \brief Compute turn and time estimation structs for the abstract route result.
|
||||
* \param result abstract routing result to annotate.
|
||||
* \param delegate Routing callbacks delegate.
|
||||
* \param points Storage for unpacked points of the path.
|
||||
* \param turnsDir output turns annotation storage.
|
||||
* \param streets output street names along the path.
|
||||
* \param segments route segments.
|
||||
* \return routing operation result code.
|
||||
*/
|
||||
|
||||
// Normally loadedSegments structure is:
|
||||
// - Start point. Fake loop LoadedPathSegment with 1 segment of zero length.
|
||||
// - Straight jump from start point to the beginning of real route. LoadedPathSegment with 1 segment.
|
||||
// - Real route. N LoadedPathSegments, each with arbitrary amount of segments. N >= 1.
|
||||
// - Straight jump from the end of real route to finish point. LoadedPathSegment with 1 segment.
|
||||
// - Finish point. Fake loop LoadedPathSegment with 1 segment of zero length.
|
||||
// So minimal amount of segments is 5.
|
||||
|
||||
// Resulting structure of turnsDir:
|
||||
// No Turn for 0th segment (no ingoing). m_index == 0.
|
||||
// No Turn for 1st segment (ingoing fake loop) - at start point. m_index == 1.
|
||||
// No Turn for 2nd (ingoing == jump) - at beginning of real route. m_index == 2.
|
||||
// Possible turn for next N-1 segments. m_index >= 3.
|
||||
// No Turn for (2 + N + 1)th segment (outgoing jump) - at finish point. m_index = 3 + M.
|
||||
// No Turn for (2 + N + 2)th segment (outgoing fake loop) - at finish point. m_index == 4 + M.
|
||||
// Added ReachedYourDestination - at finish point. m_index == 4 + M.
|
||||
// Where M - total amount of all segments from all LoadedPathSegments (size of |segments|).
|
||||
// So minimum m_index of ReachedYourDestination is 5 (real route with single segment),
|
||||
// and minimal |turnsDir| is - single ReachedYourDestination with m_index == 5.
|
||||
|
||||
void DirectionsEngine::MakeTurnAnnotation(IndexRoadGraph::EdgeVector const & routeEdges,
|
||||
vector<RouteSegment> & routeSegments)
|
||||
{
|
||||
CHECK_GREATER_OR_EQUAL(routeEdges.size(), 2, ());
|
||||
|
||||
RoutingEngineResult result(routeEdges, m_adjacentEdges, m_pathSegments);
|
||||
|
||||
routeSegments.reserve(routeEdges.size());
|
||||
|
||||
RoutingSettings const vehicleSettings = GetRoutingSettings(m_vehicleType);
|
||||
|
||||
auto const & loadedSegments = result.GetSegments(); // the same as m_pathSegments
|
||||
|
||||
// First point of first loadedSegment is ignored. This is the reason for:
|
||||
// ASSERT_EQUAL(loadedSegments.front().m_path.back(), loadedSegments.front().m_path.front(), ());
|
||||
|
||||
size_t skipTurnSegments = 0;
|
||||
for (size_t idxLoadedSegment = 0; idxLoadedSegment < loadedSegments.size(); ++idxLoadedSegment)
|
||||
{
|
||||
auto const & loadedSegment = loadedSegments[idxLoadedSegment];
|
||||
|
||||
CHECK(loadedSegment.IsValid(), ());
|
||||
CHECK_GREATER_OR_EQUAL(loadedSegment.m_segments.size(), 1, ());
|
||||
CHECK_EQUAL(loadedSegment.m_segments.size() + 1, loadedSegment.m_path.size(), ());
|
||||
|
||||
auto rni = loadedSegment.m_roadNameInfo;
|
||||
|
||||
for (size_t i = 0; i < loadedSegment.m_segments.size() - 1; ++i)
|
||||
{
|
||||
auto const & junction = loadedSegment.m_path[i + 1];
|
||||
routeSegments.emplace_back(loadedSegment.m_segments[i], TurnItem(), junction, rni);
|
||||
if (i == 0)
|
||||
rni = {"", "", "", "", "", loadedSegment.m_isLink};
|
||||
}
|
||||
|
||||
// For the last segment of current loadedSegment put info about turn
|
||||
// from current loadedSegment to the next one.
|
||||
TurnItem turnItem;
|
||||
if (skipTurnSegments == 0)
|
||||
{
|
||||
turnItem.m_index = base::asserted_cast<uint32_t>(routeSegments.size() + 1);
|
||||
skipTurnSegments = GetTurnDirection(result, idxLoadedSegment + 1, *m_numMwmIds, vehicleSettings, turnItem);
|
||||
}
|
||||
else
|
||||
--skipTurnSegments;
|
||||
|
||||
routeSegments.emplace_back(loadedSegment.m_segments.back(), turnItem, loadedSegment.m_path.back(), rni);
|
||||
}
|
||||
|
||||
// ASSERT_EQUAL(routeSegments.front().GetJunction(), result.GetStartPoint(), ());
|
||||
// ASSERT_EQUAL(routeSegments.back().GetJunction(), result.GetEndPoint(), ());
|
||||
|
||||
FixupTurns(routeSegments);
|
||||
}
|
||||
|
||||
} // namespace routing
|
||||
81
libs/routing/directions_engine.hpp
Normal file
81
libs/routing/directions_engine.hpp
Normal file
|
|
@ -0,0 +1,81 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/directions_engine_helpers.hpp"
|
||||
#include "routing/index_road_graph.hpp"
|
||||
#include "routing/route.hpp"
|
||||
#include "routing/vehicle_mask.hpp"
|
||||
|
||||
#include "indexer/feature.hpp"
|
||||
|
||||
#include "geometry/point_with_altitude.hpp"
|
||||
|
||||
#include "base/cancellable.hpp"
|
||||
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
namespace turns
|
||||
{
|
||||
class IRoutingResult;
|
||||
struct TurnItem;
|
||||
} // namespace turns
|
||||
|
||||
enum class RouterResultCode;
|
||||
class MwmDataSource;
|
||||
|
||||
class DirectionsEngine
|
||||
{
|
||||
public:
|
||||
DirectionsEngine(MwmDataSource & dataSource, std::shared_ptr<NumMwmIds> numMwmIds);
|
||||
virtual ~DirectionsEngine() = default;
|
||||
|
||||
// @TODO(bykoianko) Method Generate() should fill
|
||||
// vector<RouteSegment> instead of corresponding arguments.
|
||||
/// \brief Generates all args which are passed by reference.
|
||||
/// \param path is points of the route. It should not be empty.
|
||||
/// \returns true if fields passed by reference are filled correctly and false otherwise.
|
||||
bool Generate(IndexRoadGraph const & graph, std::vector<geometry::PointWithAltitude> const & path,
|
||||
base::Cancellable const & cancellable, std::vector<RouteSegment> & routeSegments);
|
||||
void Clear();
|
||||
|
||||
void SetVehicleType(VehicleType const & vehicleType) { m_vehicleType = vehicleType; }
|
||||
|
||||
protected:
|
||||
/*!
|
||||
* \brief GetTurnDirection makes a primary decision about turns on the route.
|
||||
* \param outgoingSegmentIndex index of an outgoing segments in vector result.GetSegments().
|
||||
* \param turn is used for keeping the result of turn calculation.
|
||||
*/
|
||||
virtual size_t GetTurnDirection(turns::IRoutingResult const & result, size_t const outgoingSegmentIndex,
|
||||
NumMwmIds const & numMwmIds, RoutingSettings const & vehicleSettings,
|
||||
turns::TurnItem & turn) = 0;
|
||||
virtual void FixupTurns(std::vector<RouteSegment> & routeSegments) = 0;
|
||||
std::unique_ptr<FeatureType> GetFeature(FeatureID const & featureId);
|
||||
void LoadPathAttributes(FeatureID const & featureId, LoadedPathSegment & pathSegment, bool isForward);
|
||||
void GetSegmentRangeAndAdjacentEdges(IRoadGraph::EdgeListT const & outgoingEdges, Edge const & inEdge,
|
||||
uint32_t startSegId, uint32_t endSegId, SegmentRange & segmentRange,
|
||||
turns::TurnCandidates & outgoingTurns);
|
||||
/// \brief The method gathers sequence of segments according to IsJoint() method
|
||||
/// and fills |m_adjacentEdges| and |m_pathSegments|.
|
||||
void FillPathSegmentsAndAdjacentEdgesMap(IndexRoadGraph const & graph,
|
||||
std::vector<geometry::PointWithAltitude> const & path,
|
||||
IRoadGraph::EdgeVector const & routeEdges,
|
||||
base::Cancellable const & cancellable);
|
||||
|
||||
AdjacentEdgesMap m_adjacentEdges;
|
||||
TUnpackedPathSegments m_pathSegments;
|
||||
|
||||
MwmDataSource & m_dataSource;
|
||||
std::shared_ptr<NumMwmIds> m_numMwmIds;
|
||||
VehicleType m_vehicleType = VehicleType::Count;
|
||||
|
||||
private:
|
||||
void MakeTurnAnnotation(IndexRoadGraph::EdgeVector const & routeEdges, std::vector<RouteSegment> & routeSegments);
|
||||
|
||||
ftypes::IsLinkChecker const & m_linkChecker;
|
||||
ftypes::IsRoundAboutChecker const & m_roundAboutChecker;
|
||||
ftypes::IsOneWayChecker const & m_onewayChecker;
|
||||
};
|
||||
} // namespace routing
|
||||
92
libs/routing/directions_engine_helpers.cpp
Normal file
92
libs/routing/directions_engine_helpers.cpp
Normal file
|
|
@ -0,0 +1,92 @@
|
|||
#include "routing/directions_engine_helpers.hpp"
|
||||
|
||||
#include "geometry/mercator.hpp"
|
||||
|
||||
namespace routing
|
||||
{
|
||||
bool AdjacentEdges::IsAlmostEqual(AdjacentEdges const & rhs) const
|
||||
{
|
||||
return m_outgoingTurns.IsAlmostEqual(rhs.m_outgoingTurns) && m_ingoingTurnsCount == rhs.m_ingoingTurnsCount;
|
||||
}
|
||||
|
||||
RoutingEngineResult::RoutingEngineResult(IRoadGraph::EdgeVector const & routeEdges,
|
||||
AdjacentEdgesMap const & adjacentEdges,
|
||||
TUnpackedPathSegments const & pathSegments)
|
||||
: m_routeEdges(routeEdges)
|
||||
, m_adjacentEdges(adjacentEdges)
|
||||
, m_pathSegments(pathSegments)
|
||||
, m_routeLength(0)
|
||||
{
|
||||
for (auto const & edge : routeEdges)
|
||||
m_routeLength += mercator::DistanceOnEarth(edge.GetStartJunction().GetPoint(), edge.GetEndJunction().GetPoint());
|
||||
}
|
||||
|
||||
void RoutingEngineResult::GetPossibleTurns(SegmentRange const & segmentRange, m2::PointD const & junctionPoint,
|
||||
size_t & ingoingCount, turns::TurnCandidates & outgoingTurns) const
|
||||
{
|
||||
CHECK(!segmentRange.IsEmpty(),
|
||||
("SegmentRange presents a fake feature.", "junctionPoint:", mercator::ToLatLon(junctionPoint)));
|
||||
|
||||
ingoingCount = 0;
|
||||
outgoingTurns.candidates.clear();
|
||||
|
||||
auto const adjacentEdges = m_adjacentEdges.find(segmentRange);
|
||||
if (adjacentEdges == m_adjacentEdges.cend())
|
||||
{
|
||||
ASSERT(false, (segmentRange));
|
||||
return;
|
||||
}
|
||||
|
||||
ingoingCount = adjacentEdges->second.m_ingoingTurnsCount;
|
||||
outgoingTurns = adjacentEdges->second.m_outgoingTurns;
|
||||
}
|
||||
|
||||
geometry::PointWithAltitude RoutingEngineResult::GetStartPoint() const
|
||||
{
|
||||
CHECK(!m_routeEdges.empty(), ());
|
||||
return m_routeEdges.front().GetStartJunction();
|
||||
}
|
||||
|
||||
geometry::PointWithAltitude RoutingEngineResult::GetEndPoint() const
|
||||
{
|
||||
CHECK(!m_routeEdges.empty(), ());
|
||||
return m_routeEdges.back().GetEndJunction();
|
||||
}
|
||||
|
||||
bool IsJoint(IRoadGraph::EdgeListT const & ingoingEdges, IRoadGraph::EdgeListT const & outgoingEdges,
|
||||
Edge const & ingoingRouteEdge, Edge const & outgoingRouteEdge)
|
||||
{
|
||||
// When feature id is changed at a junction this junction should be considered as a joint.
|
||||
//
|
||||
// If a feature id is not changed at a junction but the junction has some ingoing or outgoing
|
||||
// edges with different feature ids, the junction should be considered as a joint.
|
||||
//
|
||||
// If a feature id is not changed at a junction and all ingoing and outgoing edges of the junction
|
||||
// has the same feature id, the junction still may be considered as a joint. It happens in case of
|
||||
// self intersected features. For example:
|
||||
// *--Seg3--*
|
||||
// | |
|
||||
// Seg4 Seg2
|
||||
// | |
|
||||
// *--Seg0--*--Seg1--*
|
||||
// The common point of segments 0, 1 and 4 should be considered as a joint.
|
||||
|
||||
if (ingoingRouteEdge.GetFeatureId() != outgoingRouteEdge.GetFeatureId())
|
||||
return true;
|
||||
|
||||
FeatureID const & featureId = ingoingRouteEdge.GetFeatureId();
|
||||
uint32_t const segOut = outgoingRouteEdge.GetSegId();
|
||||
for (Edge const & e : ingoingEdges)
|
||||
if (e.GetFeatureId() != featureId || abs(static_cast<int32_t>(segOut - e.GetSegId())) != 1)
|
||||
return true;
|
||||
|
||||
uint32_t const segIn = ingoingRouteEdge.GetSegId();
|
||||
for (Edge const & e : outgoingEdges)
|
||||
{
|
||||
// It's necessary to compare segments for cases when |featureId| is a loop.
|
||||
if (e.GetFeatureId() != featureId || abs(static_cast<int32_t>(segIn - e.GetSegId())) != 1)
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
} // namespace routing
|
||||
54
libs/routing/directions_engine_helpers.hpp
Normal file
54
libs/routing/directions_engine_helpers.hpp
Normal file
|
|
@ -0,0 +1,54 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/loaded_path_segment.hpp"
|
||||
#include "routing/road_graph.hpp"
|
||||
#include "routing/routing_result_graph.hpp"
|
||||
#include "routing/turn_candidate.hpp"
|
||||
#include "routing/turns.hpp"
|
||||
|
||||
#include "geometry/point2d.hpp"
|
||||
|
||||
namespace routing
|
||||
{
|
||||
struct AdjacentEdges
|
||||
{
|
||||
explicit AdjacentEdges(size_t ingoingTurnsCount = 0) : m_ingoingTurnsCount(ingoingTurnsCount) {}
|
||||
bool IsAlmostEqual(AdjacentEdges const & rhs) const;
|
||||
|
||||
turns::TurnCandidates m_outgoingTurns;
|
||||
size_t m_ingoingTurnsCount;
|
||||
};
|
||||
|
||||
using AdjacentEdgesMap = std::map<SegmentRange, AdjacentEdges>;
|
||||
|
||||
class RoutingEngineResult : public turns::IRoutingResult
|
||||
{
|
||||
public:
|
||||
RoutingEngineResult(IRoadGraph::EdgeVector const & routeEdges, AdjacentEdgesMap const & adjacentEdges,
|
||||
TUnpackedPathSegments const & pathSegments);
|
||||
|
||||
// turns::IRoutingResult overrides:
|
||||
TUnpackedPathSegments const & GetSegments() const override { return m_pathSegments; }
|
||||
|
||||
void GetPossibleTurns(SegmentRange const & segmentRange, m2::PointD const & junctionPoint, size_t & ingoingCount,
|
||||
turns::TurnCandidates & outgoingTurns) const override;
|
||||
|
||||
double GetPathLength() const override { return m_routeLength; }
|
||||
geometry::PointWithAltitude GetStartPoint() const override;
|
||||
geometry::PointWithAltitude GetEndPoint() const override;
|
||||
|
||||
private:
|
||||
IRoadGraph::EdgeVector const & m_routeEdges;
|
||||
AdjacentEdgesMap const & m_adjacentEdges;
|
||||
TUnpackedPathSegments const & m_pathSegments;
|
||||
double m_routeLength;
|
||||
};
|
||||
|
||||
/// \brief This method should be called for an internal junction of the route with corresponding
|
||||
/// |ingoingEdges|, |outgoingEdges|, |ingoingRouteEdge| and |outgoingRouteEdge|.
|
||||
/// \returns false if the junction is an internal point of feature segment and can be considered as
|
||||
/// a part of LoadedPathSegment and returns true if the junction should be considered as a beginning
|
||||
/// of a new LoadedPathSegment.
|
||||
bool IsJoint(IRoadGraph::EdgeListT const & ingoingEdges, IRoadGraph::EdgeListT const & outgoingEdges,
|
||||
Edge const & ingoingRouteEdge, Edge const & outgoingRouteEdge);
|
||||
} // namespace routing
|
||||
84
libs/routing/dummy_world_graph.hpp
Normal file
84
libs/routing/dummy_world_graph.hpp
Normal file
|
|
@ -0,0 +1,84 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/latlon_with_altitude.hpp"
|
||||
#include "routing/world_graph.hpp"
|
||||
|
||||
#include "geometry/distance_on_sphere.hpp"
|
||||
#include "geometry/latlon.hpp"
|
||||
|
||||
#include "base/assert.hpp"
|
||||
|
||||
#include <functional>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
// This is dummy class inherited from WorldGraph. Some of its overridden methods should never be
|
||||
// called. This class is used in RegionsRouter as a lightweight replacement of
|
||||
// SingleVehicleWorldGraph.
|
||||
class DummyWorldGraph final : public WorldGraph
|
||||
{
|
||||
public:
|
||||
using WorldGraph::GetEdgeList;
|
||||
|
||||
void GetEdgeList(astar::VertexData<Segment, RouteWeight> const & vertexData, bool isOutgoing, bool useRoutingOptions,
|
||||
bool useAccessConditional, SegmentEdgeListT & edges) override
|
||||
{
|
||||
UNREACHABLE();
|
||||
}
|
||||
|
||||
void GetEdgeList(astar::VertexData<JointSegment, RouteWeight> const & vertexData, Segment const & segment,
|
||||
bool isOutgoing, bool useAccessConditional, JointEdgeListT & edges,
|
||||
WeightListT & parentWeights) override
|
||||
{
|
||||
UNREACHABLE();
|
||||
}
|
||||
|
||||
bool CheckLength(RouteWeight const & weight, double startToFinishDistanceM) const override { return true; }
|
||||
|
||||
LatLonWithAltitude const & GetJunction(Segment const & segment, bool front) override { UNREACHABLE(); }
|
||||
|
||||
ms::LatLon const & GetPoint(Segment const & segment, bool front) override { UNREACHABLE(); }
|
||||
|
||||
bool IsOneWay(NumMwmId mwmId, uint32_t featureId) override { UNREACHABLE(); }
|
||||
|
||||
bool IsPassThroughAllowed(NumMwmId mwmId, uint32_t featureId) override { UNREACHABLE(); }
|
||||
|
||||
void ClearCachedGraphs() override { UNREACHABLE(); }
|
||||
|
||||
void SetMode(WorldGraphMode mode) override {}
|
||||
|
||||
WorldGraphMode GetMode() const override { return WorldGraphMode::NoLeaps; }
|
||||
|
||||
RouteWeight HeuristicCostEstimate(ms::LatLon const & from, ms::LatLon const & to) override
|
||||
{
|
||||
return RouteWeight(ms::DistanceOnEarth(from, to));
|
||||
}
|
||||
|
||||
RouteWeight CalcSegmentWeight(Segment const & segment, EdgeEstimator::Purpose purpose) override { UNREACHABLE(); }
|
||||
|
||||
RouteWeight CalcLeapWeight(ms::LatLon const & from, ms::LatLon const & to, NumMwmId mwmId) const override
|
||||
{
|
||||
UNREACHABLE();
|
||||
}
|
||||
|
||||
RouteWeight CalcOffroadWeight(ms::LatLon const & from, ms::LatLon const & to,
|
||||
EdgeEstimator::Purpose purpose) const override
|
||||
{
|
||||
return RouteWeight(ms::DistanceOnEarth(from, to));
|
||||
}
|
||||
|
||||
double CalculateETA(Segment const & from, Segment const & to) override { UNREACHABLE(); }
|
||||
|
||||
double CalculateETAWithoutPenalty(Segment const & segment) override { UNREACHABLE(); }
|
||||
|
||||
IndexGraph & GetIndexGraph(NumMwmId numMwmId) override { UNREACHABLE(); }
|
||||
|
||||
void GetTwinsInner(Segment const & segment, bool isOutgoing, std::vector<Segment> & twins) override
|
||||
{
|
||||
CHECK(false, ());
|
||||
}
|
||||
};
|
||||
} // namespace routing
|
||||
847
libs/routing/edge_estimator.cpp
Normal file
847
libs/routing/edge_estimator.cpp
Normal file
|
|
@ -0,0 +1,847 @@
|
|||
#include "routing/edge_estimator.hpp"
|
||||
#include "routing/geometry.hpp"
|
||||
#include "routing/latlon_with_altitude.hpp"
|
||||
#include "routing/routing_helpers.hpp"
|
||||
#include "routing/traffic_stash.hpp"
|
||||
|
||||
#include "traffic/speed_groups.hpp"
|
||||
|
||||
#include "geometry/distance_on_sphere.hpp"
|
||||
#include "geometry/point_with_altitude.hpp"
|
||||
|
||||
#include "coding/csv_reader.hpp"
|
||||
|
||||
#include "base/assert.hpp"
|
||||
#include "base/logging.hpp"
|
||||
#include "platform/platform.hpp"
|
||||
|
||||
namespace routing
|
||||
{
|
||||
using namespace std;
|
||||
using namespace traffic;
|
||||
using measurement_utils::KmphToMps;
|
||||
|
||||
namespace
|
||||
{
|
||||
geometry::Altitude constexpr kMountainSicknessAltitudeM = 2500;
|
||||
|
||||
double TimeBetweenSec(ms::LatLon const & from, ms::LatLon const & to, double speedMpS)
|
||||
{
|
||||
ASSERT_GREATER(speedMpS, 0.0, ("from:", from, "to:", to));
|
||||
|
||||
double const distanceM = ms::DistanceOnEarth(from, to);
|
||||
return distanceM / speedMpS;
|
||||
}
|
||||
|
||||
double CalcTrafficFactor(SpeedGroup speedGroup)
|
||||
{
|
||||
if (speedGroup == SpeedGroup::TempBlock)
|
||||
{
|
||||
// impossible driving factor
|
||||
return 1.0E4;
|
||||
}
|
||||
|
||||
double const percentage = 0.01 * kSpeedGroupThresholdPercentage[static_cast<size_t>(speedGroup)];
|
||||
ASSERT_GREATER(percentage, 0.0, (speedGroup));
|
||||
return 1.0 / percentage;
|
||||
}
|
||||
|
||||
double GetSpeedMpS(EdgeEstimator::Purpose purpose, Segment const & segment, RoadGeometry const & road)
|
||||
{
|
||||
SpeedKMpH const & speed = road.GetSpeed(segment.IsForward());
|
||||
double const speedMpS = KmphToMps(purpose == EdgeEstimator::Purpose::Weight ? speed.m_weight : speed.m_eta);
|
||||
ASSERT_GREATER(speedMpS, 0.0, (segment));
|
||||
return speedMpS;
|
||||
}
|
||||
|
||||
bool IsTransit(std::optional<HighwayType> type)
|
||||
{
|
||||
return type && (type == HighwayType::RouteFerry || type == HighwayType::RouteShuttleTrain);
|
||||
}
|
||||
|
||||
template <class CalcSpeed>
|
||||
double CalcClimbSegment(EdgeEstimator::Purpose purpose, Segment const & segment, RoadGeometry const & road,
|
||||
CalcSpeed && calcSpeed)
|
||||
{
|
||||
double const distance = road.GetDistance(segment.GetSegmentIdx());
|
||||
double speedMpS = GetSpeedMpS(purpose, segment, road);
|
||||
|
||||
static double constexpr kSmallDistanceM = 1; // we have altitude threshold is 0.5m
|
||||
if (distance > kSmallDistanceM && !IsTransit(road.GetHighwayType()))
|
||||
{
|
||||
LatLonWithAltitude const & from = road.GetJunction(segment.GetPointId(false /* front */));
|
||||
LatLonWithAltitude const & to = road.GetJunction(segment.GetPointId(true /* front */));
|
||||
|
||||
ASSERT(to.GetAltitude() != geometry::kInvalidAltitude && from.GetAltitude() != geometry::kInvalidAltitude, ());
|
||||
auto const altitudeDiff = to.GetAltitude() - from.GetAltitude();
|
||||
|
||||
if (altitudeDiff != 0)
|
||||
{
|
||||
speedMpS = calcSpeed(speedMpS, altitudeDiff / distance, to.GetAltitude());
|
||||
ASSERT_GREATER(speedMpS, 0.0, (segment));
|
||||
}
|
||||
}
|
||||
|
||||
return distance / speedMpS;
|
||||
}
|
||||
} // namespace
|
||||
|
||||
double GetPedestrianClimbPenalty(EdgeEstimator::Purpose purpose, double tangent, geometry::Altitude altitudeM)
|
||||
{
|
||||
double constexpr kMinPenalty = 1.0;
|
||||
// Descent penalty is less then the ascent penalty.
|
||||
double const impact = tangent >= 0.0 ? 1.0 : 0.35;
|
||||
|
||||
if (altitudeM >= kMountainSicknessAltitudeM)
|
||||
return kMinPenalty + (10.0 + (altitudeM - kMountainSicknessAltitudeM) * 10.0 / 1500.0) * fabs(tangent) * impact;
|
||||
|
||||
// Use magic constant from this table: https://en.wikipedia.org/wiki/Tobler's_hiking_function#Sample_values
|
||||
// Tobler's returns unusually big values for bigger tangent.
|
||||
// See Australia_Mountains_Downlhill test.
|
||||
if (purpose == EdgeEstimator::Purpose::Weight || fabs(tangent) > 1.19)
|
||||
{
|
||||
tangent = fabs(tangent);
|
||||
// Some thoughts about gradient and foot walking: https://gre-kow.livejournal.com/26916.html
|
||||
// 3cm diff with avg foot length 60cm is imperceptible (see Hungary_UseFootways).
|
||||
double constexpr kTangentThreshold = 3.0 / 60.0;
|
||||
if (tangent < kTangentThreshold)
|
||||
return kMinPenalty;
|
||||
|
||||
// ETA coefficients are calculated in https://github.com/mapsme/omim-scripts/pull/21
|
||||
auto const penalty = purpose == EdgeEstimator::Purpose::Weight ? 5.0 * tangent + 7.0 * tangent * tangent
|
||||
: 3.01 * tangent + 3.54 * tangent * tangent;
|
||||
|
||||
return kMinPenalty + penalty * impact;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Use Tobler’s Hiking Function for ETA like more comprehensive. See France_Uphill_Downlhill test.
|
||||
// Why not in Weight? See Crimea_Altitude_Mountains test.
|
||||
// https://mtntactical.com/research/yet-calculating-movement-uneven-terrain/
|
||||
// Returns factor: W(0) / W(tangent).
|
||||
return exp(-3.5 * (0.05 - fabs(tangent + 0.05)));
|
||||
}
|
||||
}
|
||||
|
||||
double GetBicycleClimbPenalty(EdgeEstimator::Purpose purpose, double tangent, geometry::Altitude altitudeM)
|
||||
{
|
||||
double constexpr kMinPenalty = 1.0;
|
||||
double const impact = tangent >= 0.0 ? 1.0 : 0.35;
|
||||
|
||||
if (altitudeM >= kMountainSicknessAltitudeM)
|
||||
return kMinPenalty + 50.0 * fabs(tangent) * impact;
|
||||
|
||||
// By VNG: This approach is strange at least because it always returns penalty > 1 (even for downhill)
|
||||
/*
|
||||
tangent = fabs(tangent);
|
||||
// ETA coefficients are calculated in https://github.com/mapsme/omim-scripts/pull/22
|
||||
auto const penalty = purpose == EdgeEstimator::Purpose::Weight
|
||||
? 10.0 * tangent + 26.0 * tangent * tangent
|
||||
: 8.8 * tangent + 6.51 * tangent * tangent;
|
||||
|
||||
return kMinPenalty + penalty * impact;
|
||||
*/
|
||||
|
||||
// https://web.tecnico.ulisboa.pt/~rosamfelix/gis/declives/SpeedSlopeFactor.html
|
||||
double const slope = tangent * 100;
|
||||
|
||||
double factor;
|
||||
if (slope < -30)
|
||||
factor = 1.5;
|
||||
else if (slope < 0)
|
||||
{
|
||||
// Min factor (max speed) will be at slope = -13.
|
||||
factor = 1 + 2 * 0.7 / 13.0 * slope + 0.7 / 169 * slope * slope;
|
||||
}
|
||||
else if (slope <= 20)
|
||||
factor = 1 + slope * slope / 49;
|
||||
else
|
||||
factor = 10.0;
|
||||
return factor;
|
||||
}
|
||||
|
||||
double GetCarClimbPenalty(EdgeEstimator::Purpose, double, geometry::Altitude)
|
||||
{
|
||||
return 1.0;
|
||||
}
|
||||
|
||||
// EdgeEstimator -----------------------------------------------------------------------------------
|
||||
EdgeEstimator::EdgeEstimator(VehicleType vehicleType, double maxWeightSpeedKMpH, SpeedKMpH const & offroadSpeedKMpH,
|
||||
DataSource * /*dataSourcePtr*/, std::shared_ptr<NumMwmIds> /*numMwmIds*/)
|
||||
: m_vehicleType(vehicleType)
|
||||
, m_maxWeightSpeedMpS(KmphToMps(maxWeightSpeedKMpH))
|
||||
, m_offroadSpeedKMpH(offroadSpeedKMpH)
|
||||
|
||||
//, m_dataSourcePtr(dataSourcePtr)
|
||||
//, m_numMwmIds(numMwmIds)
|
||||
{
|
||||
CHECK_GREATER(m_offroadSpeedKMpH.m_weight, 0.0, ());
|
||||
CHECK_GREATER(m_offroadSpeedKMpH.m_eta, 0.0, ());
|
||||
CHECK_GREATER_OR_EQUAL(m_maxWeightSpeedMpS, KmphToMps(m_offroadSpeedKMpH.m_weight), ());
|
||||
|
||||
if (m_offroadSpeedKMpH.m_eta != kNotUsed)
|
||||
CHECK_GREATER_OR_EQUAL(m_maxWeightSpeedMpS, KmphToMps(m_offroadSpeedKMpH.m_eta), ());
|
||||
|
||||
LOG(LINFO, ("Loading turn penalties for vehicle type:", static_cast<int>(vehicleType)));
|
||||
|
||||
struct TurnPenaltyMatrix
|
||||
{
|
||||
int road;
|
||||
VehicleType vehicleType;
|
||||
double penalty;
|
||||
};
|
||||
|
||||
struct TurnPenalty
|
||||
{
|
||||
HighwayType fromRoadType;
|
||||
HighwayType toRoadType;
|
||||
VehicleType vehicleType;
|
||||
double penalty;
|
||||
};
|
||||
|
||||
#define N 288
|
||||
|
||||
static auto constexpr kTurnPenaltyMatrix = []
|
||||
{
|
||||
array<TurnPenalty, N> constexpr kTable = {{
|
||||
{HighwayType::HighwayLivingStreet, HighwayType::HighwayLivingStreet, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayLivingStreet, HighwayType::HighwayPrimary, VehicleType::Car, 0.09},
|
||||
{HighwayType::HighwayLivingStreet, HighwayType::HighwayPrimaryLink, VehicleType::Car, 0.09},
|
||||
{HighwayType::HighwayLivingStreet, HighwayType::HighwayResidential, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayLivingStreet, HighwayType::HighwaySecondary, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwayLivingStreet, HighwayType::HighwaySecondaryLink, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwayLivingStreet, HighwayType::HighwayService, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayLivingStreet, HighwayType::HighwayTertiary, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayLivingStreet, HighwayType::HighwayTertiaryLink, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayLivingStreet, HighwayType::HighwayTrunk, VehicleType::Car, 0.09},
|
||||
{HighwayType::HighwayLivingStreet, HighwayType::HighwayTrunkLink, VehicleType::Car, 0.09},
|
||||
{HighwayType::HighwayLivingStreet, HighwayType::HighwayUnclassified, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayPrimary, HighwayType::HighwayLivingStreet, VehicleType::Car, 0.11},
|
||||
{HighwayType::HighwayPrimary, HighwayType::HighwayPrimary, VehicleType::Car, 0.06},
|
||||
{HighwayType::HighwayPrimary, HighwayType::HighwayPrimaryLink, VehicleType::Car, 0.06},
|
||||
{HighwayType::HighwayPrimary, HighwayType::HighwayResidential, VehicleType::Car, 0.1},
|
||||
{HighwayType::HighwayPrimary, HighwayType::HighwaySecondary, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayPrimary, HighwayType::HighwaySecondaryLink, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayPrimary, HighwayType::HighwayService, VehicleType::Car, 0.1},
|
||||
{HighwayType::HighwayPrimary, HighwayType::HighwayTertiary, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwayPrimary, HighwayType::HighwayTertiaryLink, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayPrimary, HighwayType::HighwayTrunk, VehicleType::Car, 0.04},
|
||||
{HighwayType::HighwayPrimary, HighwayType::HighwayTrunkLink, VehicleType::Car, 0.04},
|
||||
{HighwayType::HighwayPrimary, HighwayType::HighwayUnclassified, VehicleType::Car, 0.1},
|
||||
{HighwayType::HighwayPrimaryLink, HighwayType::HighwayLivingStreet, VehicleType::Car, 0.1},
|
||||
{HighwayType::HighwayPrimaryLink, HighwayType::HighwayPrimary, VehicleType::Car, 0.06},
|
||||
{HighwayType::HighwayPrimaryLink, HighwayType::HighwayPrimaryLink, VehicleType::Car, 0.06},
|
||||
{HighwayType::HighwayPrimaryLink, HighwayType::HighwayResidential, VehicleType::Car, 0.1},
|
||||
{HighwayType::HighwayPrimaryLink, HighwayType::HighwaySecondary, VehicleType::Car, 0.06},
|
||||
{HighwayType::HighwayPrimaryLink, HighwayType::HighwaySecondaryLink, VehicleType::Car, 0.06},
|
||||
{HighwayType::HighwayPrimaryLink, HighwayType::HighwayService, VehicleType::Car, 0.09},
|
||||
{HighwayType::HighwayPrimaryLink, HighwayType::HighwayTertiary, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwayPrimaryLink, HighwayType::HighwayTertiaryLink, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwayPrimaryLink, HighwayType::HighwayTrunk, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayPrimaryLink, HighwayType::HighwayTrunkLink, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayPrimaryLink, HighwayType::HighwayUnclassified, VehicleType::Car, 0.1},
|
||||
{HighwayType::HighwayResidential, HighwayType::HighwayLivingStreet, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayResidential, HighwayType::HighwayPrimary, VehicleType::Car, 0.09},
|
||||
{HighwayType::HighwayResidential, HighwayType::HighwayPrimaryLink, VehicleType::Car, 0.09},
|
||||
{HighwayType::HighwayResidential, HighwayType::HighwayResidential, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwayResidential, HighwayType::HighwaySecondary, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwayResidential, HighwayType::HighwaySecondaryLink, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwayResidential, HighwayType::HighwayService, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayResidential, HighwayType::HighwayTertiary, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayResidential, HighwayType::HighwayTertiaryLink, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayResidential, HighwayType::HighwayTrunk, VehicleType::Car, 0.09},
|
||||
{HighwayType::HighwayResidential, HighwayType::HighwayTrunkLink, VehicleType::Car, 0.09},
|
||||
{HighwayType::HighwayResidential, HighwayType::HighwayUnclassified, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwaySecondary, HighwayType::HighwayLivingStreet, VehicleType::Car, 0.1},
|
||||
{HighwayType::HighwaySecondary, HighwayType::HighwayPrimary, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwaySecondary, HighwayType::HighwayPrimaryLink, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwaySecondary, HighwayType::HighwayResidential, VehicleType::Car, 0.1},
|
||||
{HighwayType::HighwaySecondary, HighwayType::HighwaySecondary, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwaySecondary, HighwayType::HighwaySecondaryLink, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwaySecondary, HighwayType::HighwayService, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwaySecondary, HighwayType::HighwayTertiary, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwaySecondary, HighwayType::HighwayTertiaryLink, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwaySecondary, HighwayType::HighwayTrunk, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwaySecondary, HighwayType::HighwayTrunkLink, VehicleType::Car, 0.06},
|
||||
{HighwayType::HighwaySecondary, HighwayType::HighwayUnclassified, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwaySecondaryLink, HighwayType::HighwayLivingStreet, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwaySecondaryLink, HighwayType::HighwayPrimary, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwaySecondaryLink, HighwayType::HighwayPrimaryLink, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwaySecondaryLink, HighwayType::HighwayResidential, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwaySecondaryLink, HighwayType::HighwaySecondary, VehicleType::Car, 0.05},
|
||||
{HighwayType::HighwaySecondaryLink, HighwayType::HighwaySecondaryLink, VehicleType::Car, 0.05},
|
||||
{HighwayType::HighwaySecondaryLink, HighwayType::HighwayService, VehicleType::Car, 0.06},
|
||||
{HighwayType::HighwaySecondaryLink, HighwayType::HighwayTertiary, VehicleType::Car, 0.05},
|
||||
{HighwayType::HighwaySecondaryLink, HighwayType::HighwayTertiaryLink, VehicleType::Car, 0.05},
|
||||
{HighwayType::HighwaySecondaryLink, HighwayType::HighwayTrunk, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwaySecondaryLink, HighwayType::HighwayTrunkLink, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwaySecondaryLink, HighwayType::HighwayUnclassified, VehicleType::Car, 0.06},
|
||||
{HighwayType::HighwayService, HighwayType::HighwayLivingStreet, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayService, HighwayType::HighwayPrimary, VehicleType::Car, 0.09},
|
||||
{HighwayType::HighwayService, HighwayType::HighwayPrimaryLink, VehicleType::Car, 0.09},
|
||||
{HighwayType::HighwayService, HighwayType::HighwayResidential, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayService, HighwayType::HighwaySecondary, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayService, HighwayType::HighwaySecondaryLink, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayService, HighwayType::HighwayService, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayService, HighwayType::HighwayTertiary, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayService, HighwayType::HighwayTertiaryLink, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayService, HighwayType::HighwayTrunk, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayService, HighwayType::HighwayTrunkLink, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayService, HighwayType::HighwayUnclassified, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayTertiary, HighwayType::HighwayLivingStreet, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwayTertiary, HighwayType::HighwayPrimary, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayTertiary, HighwayType::HighwayPrimaryLink, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayTertiary, HighwayType::HighwayResidential, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwayTertiary, HighwayType::HighwaySecondary, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayTertiary, HighwayType::HighwaySecondaryLink, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayTertiary, HighwayType::HighwayService, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayTertiary, HighwayType::HighwayTertiary, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayTertiary, HighwayType::HighwayTertiaryLink, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayTertiary, HighwayType::HighwayTrunk, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayTertiary, HighwayType::HighwayTrunkLink, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayTertiary, HighwayType::HighwayUnclassified, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwayTertiaryLink, HighwayType::HighwayLivingStreet, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayTertiaryLink, HighwayType::HighwayPrimary, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayTertiaryLink, HighwayType::HighwayPrimaryLink, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayTertiaryLink, HighwayType::HighwayResidential, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayTertiaryLink, HighwayType::HighwaySecondary, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayTertiaryLink, HighwayType::HighwaySecondaryLink, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayTertiaryLink, HighwayType::HighwayService, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayTertiaryLink, HighwayType::HighwayTertiary, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayTertiaryLink, HighwayType::HighwayTertiaryLink, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayTertiaryLink, HighwayType::HighwayTrunk, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayTertiaryLink, HighwayType::HighwayTrunkLink, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayTertiaryLink, HighwayType::HighwayUnclassified, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayTrunk, HighwayType::HighwayLivingStreet, VehicleType::Car, 0.1},
|
||||
{HighwayType::HighwayTrunk, HighwayType::HighwayPrimary, VehicleType::Car, 0.05},
|
||||
{HighwayType::HighwayTrunk, HighwayType::HighwayPrimaryLink, VehicleType::Car, 0.03},
|
||||
{HighwayType::HighwayTrunk, HighwayType::HighwayResidential, VehicleType::Car, 0.09},
|
||||
{HighwayType::HighwayTrunk, HighwayType::HighwaySecondary, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwayTrunk, HighwayType::HighwaySecondaryLink, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwayTrunk, HighwayType::HighwayService, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwayTrunk, HighwayType::HighwayTertiary, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwayTrunk, HighwayType::HighwayTertiaryLink, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwayTrunk, HighwayType::HighwayTrunk, VehicleType::Car, 0.01},
|
||||
{HighwayType::HighwayTrunk, HighwayType::HighwayTrunkLink, VehicleType::Car, 0.01},
|
||||
{HighwayType::HighwayTrunk, HighwayType::HighwayUnclassified, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwayTrunkLink, HighwayType::HighwayLivingStreet, VehicleType::Car, 0.11},
|
||||
{HighwayType::HighwayTrunkLink, HighwayType::HighwayPrimary, VehicleType::Car, 0.04},
|
||||
{HighwayType::HighwayTrunkLink, HighwayType::HighwayPrimaryLink, VehicleType::Car, 0.04},
|
||||
{HighwayType::HighwayTrunkLink, HighwayType::HighwayResidential, VehicleType::Car, 0.1},
|
||||
{HighwayType::HighwayTrunkLink, HighwayType::HighwaySecondary, VehicleType::Car, 0.04},
|
||||
{HighwayType::HighwayTrunkLink, HighwayType::HighwaySecondaryLink, VehicleType::Car, 0.04},
|
||||
{HighwayType::HighwayTrunkLink, HighwayType::HighwayService, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayTrunkLink, HighwayType::HighwayTertiary, VehicleType::Car, 0.06},
|
||||
{HighwayType::HighwayTrunkLink, HighwayType::HighwayTertiaryLink, VehicleType::Car, 0.06},
|
||||
{HighwayType::HighwayTrunkLink, HighwayType::HighwayTrunk, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayTrunkLink, HighwayType::HighwayTrunkLink, VehicleType::Car, 0.02},
|
||||
{HighwayType::HighwayTrunkLink, HighwayType::HighwayUnclassified, VehicleType::Car, 0.1},
|
||||
{HighwayType::HighwayUnclassified, HighwayType::HighwayLivingStreet, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayUnclassified, HighwayType::HighwayPrimary, VehicleType::Car, 0.09},
|
||||
{HighwayType::HighwayUnclassified, HighwayType::HighwayPrimaryLink, VehicleType::Car, 0.09},
|
||||
{HighwayType::HighwayUnclassified, HighwayType::HighwayResidential, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayUnclassified, HighwayType::HighwaySecondary, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwayUnclassified, HighwayType::HighwaySecondaryLink, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwayUnclassified, HighwayType::HighwayService, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwayUnclassified, HighwayType::HighwayTertiary, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayUnclassified, HighwayType::HighwayTertiaryLink, VehicleType::Car, 0.07},
|
||||
{HighwayType::HighwayUnclassified, HighwayType::HighwayTrunk, VehicleType::Car, 0.09},
|
||||
{HighwayType::HighwayUnclassified, HighwayType::HighwayTrunkLink, VehicleType::Car, 0.09},
|
||||
{HighwayType::HighwayUnclassified, HighwayType::HighwayUnclassified, VehicleType::Car, 0.08},
|
||||
{HighwayType::HighwayLivingStreet, HighwayType::HighwayLivingStreet, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayLivingStreet, HighwayType::HighwayPrimary, VehicleType::Bicycle, 0.09},
|
||||
{HighwayType::HighwayLivingStreet, HighwayType::HighwayPrimaryLink, VehicleType::Bicycle, 0.09},
|
||||
{HighwayType::HighwayLivingStreet, HighwayType::HighwayResidential, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayLivingStreet, HighwayType::HighwaySecondary, VehicleType::Bicycle, 0.08},
|
||||
{HighwayType::HighwayLivingStreet, HighwayType::HighwaySecondaryLink, VehicleType::Bicycle, 0.08},
|
||||
{HighwayType::HighwayLivingStreet, HighwayType::HighwayService, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayLivingStreet, HighwayType::HighwayTertiary, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayLivingStreet, HighwayType::HighwayTertiaryLink, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayLivingStreet, HighwayType::HighwayTrunk, VehicleType::Bicycle, 0.09},
|
||||
{HighwayType::HighwayLivingStreet, HighwayType::HighwayTrunkLink, VehicleType::Bicycle, 0.09},
|
||||
{HighwayType::HighwayLivingStreet, HighwayType::HighwayUnclassified, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayPrimary, HighwayType::HighwayLivingStreet, VehicleType::Bicycle, 0.11},
|
||||
{HighwayType::HighwayPrimary, HighwayType::HighwayPrimary, VehicleType::Bicycle, 0.06},
|
||||
{HighwayType::HighwayPrimary, HighwayType::HighwayPrimaryLink, VehicleType::Bicycle, 0.06},
|
||||
{HighwayType::HighwayPrimary, HighwayType::HighwayResidential, VehicleType::Bicycle, 0.1},
|
||||
{HighwayType::HighwayPrimary, HighwayType::HighwaySecondary, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayPrimary, HighwayType::HighwaySecondaryLink, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayPrimary, HighwayType::HighwayService, VehicleType::Bicycle, 0.1},
|
||||
{HighwayType::HighwayPrimary, HighwayType::HighwayTertiary, VehicleType::Bicycle, 0.08},
|
||||
{HighwayType::HighwayPrimary, HighwayType::HighwayTertiaryLink, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayPrimary, HighwayType::HighwayTrunk, VehicleType::Bicycle, 0.04},
|
||||
{HighwayType::HighwayPrimary, HighwayType::HighwayTrunkLink, VehicleType::Bicycle, 0.04},
|
||||
{HighwayType::HighwayPrimary, HighwayType::HighwayUnclassified, VehicleType::Bicycle, 0.1},
|
||||
{HighwayType::HighwayPrimaryLink, HighwayType::HighwayLivingStreet, VehicleType::Bicycle, 0.1},
|
||||
{HighwayType::HighwayPrimaryLink, HighwayType::HighwayPrimary, VehicleType::Bicycle, 0.06},
|
||||
{HighwayType::HighwayPrimaryLink, HighwayType::HighwayPrimaryLink, VehicleType::Bicycle, 0.06},
|
||||
{HighwayType::HighwayPrimaryLink, HighwayType::HighwayResidential, VehicleType::Bicycle, 0.1},
|
||||
{HighwayType::HighwayPrimaryLink, HighwayType::HighwaySecondary, VehicleType::Bicycle, 0.06},
|
||||
{HighwayType::HighwayPrimaryLink, HighwayType::HighwaySecondaryLink, VehicleType::Bicycle, 0.06},
|
||||
{HighwayType::HighwayPrimaryLink, HighwayType::HighwayService, VehicleType::Bicycle, 0.09},
|
||||
{HighwayType::HighwayPrimaryLink, HighwayType::HighwayTertiary, VehicleType::Bicycle, 0.08},
|
||||
{HighwayType::HighwayPrimaryLink, HighwayType::HighwayTertiaryLink, VehicleType::Bicycle, 0.08},
|
||||
{HighwayType::HighwayPrimaryLink, HighwayType::HighwayTrunk, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayPrimaryLink, HighwayType::HighwayTrunkLink, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayPrimaryLink, HighwayType::HighwayUnclassified, VehicleType::Bicycle, 0.1},
|
||||
{HighwayType::HighwayResidential, HighwayType::HighwayLivingStreet, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayResidential, HighwayType::HighwayPrimary, VehicleType::Bicycle, 0.09},
|
||||
{HighwayType::HighwayResidential, HighwayType::HighwayPrimaryLink, VehicleType::Bicycle, 0.09},
|
||||
{HighwayType::HighwayResidential, HighwayType::HighwayResidential, VehicleType::Bicycle, 0.08},
|
||||
{HighwayType::HighwayResidential, HighwayType::HighwaySecondary, VehicleType::Bicycle, 0.08},
|
||||
{HighwayType::HighwayResidential, HighwayType::HighwaySecondaryLink, VehicleType::Bicycle, 0.08},
|
||||
{HighwayType::HighwayResidential, HighwayType::HighwayService, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayResidential, HighwayType::HighwayTertiary, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayResidential, HighwayType::HighwayTertiaryLink, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayResidential, HighwayType::HighwayTrunk, VehicleType::Bicycle, 0.09},
|
||||
{HighwayType::HighwayResidential, HighwayType::HighwayTrunkLink, VehicleType::Bicycle, 0.09},
|
||||
{HighwayType::HighwayResidential, HighwayType::HighwayUnclassified, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwaySecondary, HighwayType::HighwayLivingStreet, VehicleType::Bicycle, 0.1},
|
||||
{HighwayType::HighwaySecondary, HighwayType::HighwayPrimary, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwaySecondary, HighwayType::HighwayPrimaryLink, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwaySecondary, HighwayType::HighwayResidential, VehicleType::Bicycle, 0.1},
|
||||
{HighwayType::HighwaySecondary, HighwayType::HighwaySecondary, VehicleType::Bicycle, 0.08},
|
||||
{HighwayType::HighwaySecondary, HighwayType::HighwaySecondaryLink, VehicleType::Bicycle, 0.08},
|
||||
{HighwayType::HighwaySecondary, HighwayType::HighwayService, VehicleType::Bicycle, 0.08},
|
||||
{HighwayType::HighwaySecondary, HighwayType::HighwayTertiary, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwaySecondary, HighwayType::HighwayTertiaryLink, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwaySecondary, HighwayType::HighwayTrunk, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwaySecondary, HighwayType::HighwayTrunkLink, VehicleType::Bicycle, 0.06},
|
||||
{HighwayType::HighwaySecondary, HighwayType::HighwayUnclassified, VehicleType::Bicycle, 0.08},
|
||||
{HighwayType::HighwaySecondaryLink, HighwayType::HighwayLivingStreet, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwaySecondaryLink, HighwayType::HighwayPrimary, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwaySecondaryLink, HighwayType::HighwayPrimaryLink, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwaySecondaryLink, HighwayType::HighwayResidential, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwaySecondaryLink, HighwayType::HighwaySecondary, VehicleType::Bicycle, 0.05},
|
||||
{HighwayType::HighwaySecondaryLink, HighwayType::HighwaySecondaryLink, VehicleType::Bicycle, 0.05},
|
||||
{HighwayType::HighwaySecondaryLink, HighwayType::HighwayService, VehicleType::Bicycle, 0.06},
|
||||
{HighwayType::HighwaySecondaryLink, HighwayType::HighwayTertiary, VehicleType::Bicycle, 0.05},
|
||||
{HighwayType::HighwaySecondaryLink, HighwayType::HighwayTertiaryLink, VehicleType::Bicycle, 0.05},
|
||||
{HighwayType::HighwaySecondaryLink, HighwayType::HighwayTrunk, VehicleType::Bicycle, 0.08},
|
||||
{HighwayType::HighwaySecondaryLink, HighwayType::HighwayTrunkLink, VehicleType::Bicycle, 0.08},
|
||||
{HighwayType::HighwaySecondaryLink, HighwayType::HighwayUnclassified, VehicleType::Bicycle, 0.06},
|
||||
{HighwayType::HighwayService, HighwayType::HighwayLivingStreet, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayService, HighwayType::HighwayPrimary, VehicleType::Bicycle, 0.09},
|
||||
{HighwayType::HighwayService, HighwayType::HighwayPrimaryLink, VehicleType::Bicycle, 0.09},
|
||||
{HighwayType::HighwayService, HighwayType::HighwayResidential, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayService, HighwayType::HighwaySecondary, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayService, HighwayType::HighwaySecondaryLink, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayService, HighwayType::HighwayService, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayService, HighwayType::HighwayTertiary, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayService, HighwayType::HighwayTertiaryLink, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayService, HighwayType::HighwayTrunk, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayService, HighwayType::HighwayTrunkLink, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayService, HighwayType::HighwayUnclassified, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayTertiary, HighwayType::HighwayLivingStreet, VehicleType::Bicycle, 0.08},
|
||||
{HighwayType::HighwayTertiary, HighwayType::HighwayPrimary, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayTertiary, HighwayType::HighwayPrimaryLink, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayTertiary, HighwayType::HighwayResidential, VehicleType::Bicycle, 0.08},
|
||||
{HighwayType::HighwayTertiary, HighwayType::HighwaySecondary, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayTertiary, HighwayType::HighwaySecondaryLink, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayTertiary, HighwayType::HighwayService, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayTertiary, HighwayType::HighwayTertiary, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayTertiary, HighwayType::HighwayTertiaryLink, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayTertiary, HighwayType::HighwayTrunk, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayTertiary, HighwayType::HighwayTrunkLink, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayTertiary, HighwayType::HighwayUnclassified, VehicleType::Bicycle, 0.08},
|
||||
{HighwayType::HighwayTertiaryLink, HighwayType::HighwayLivingStreet, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayTertiaryLink, HighwayType::HighwayPrimary, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayTertiaryLink, HighwayType::HighwayPrimaryLink, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayTertiaryLink, HighwayType::HighwayResidential, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayTertiaryLink, HighwayType::HighwaySecondary, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayTertiaryLink, HighwayType::HighwaySecondaryLink, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayTertiaryLink, HighwayType::HighwayService, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayTertiaryLink, HighwayType::HighwayTertiary, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayTertiaryLink, HighwayType::HighwayTertiaryLink, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayTertiaryLink, HighwayType::HighwayTrunk, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayTertiaryLink, HighwayType::HighwayTrunkLink, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayTertiaryLink, HighwayType::HighwayUnclassified, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayTrunk, HighwayType::HighwayLivingStreet, VehicleType::Bicycle, 0.1},
|
||||
{HighwayType::HighwayTrunk, HighwayType::HighwayPrimary, VehicleType::Bicycle, 0.05},
|
||||
{HighwayType::HighwayTrunk, HighwayType::HighwayPrimaryLink, VehicleType::Bicycle, 0.03},
|
||||
{HighwayType::HighwayTrunk, HighwayType::HighwayResidential, VehicleType::Bicycle, 0.09},
|
||||
{HighwayType::HighwayTrunk, HighwayType::HighwaySecondary, VehicleType::Bicycle, 0.08},
|
||||
{HighwayType::HighwayTrunk, HighwayType::HighwaySecondaryLink, VehicleType::Bicycle, 0.08},
|
||||
{HighwayType::HighwayTrunk, HighwayType::HighwayService, VehicleType::Bicycle, 0.08},
|
||||
{HighwayType::HighwayTrunk, HighwayType::HighwayTertiary, VehicleType::Bicycle, 0.08},
|
||||
{HighwayType::HighwayTrunk, HighwayType::HighwayTertiaryLink, VehicleType::Bicycle, 0.08},
|
||||
{HighwayType::HighwayTrunk, HighwayType::HighwayTrunk, VehicleType::Bicycle, 0.01},
|
||||
{HighwayType::HighwayTrunk, HighwayType::HighwayTrunkLink, VehicleType::Bicycle, 0.01},
|
||||
{HighwayType::HighwayTrunk, HighwayType::HighwayUnclassified, VehicleType::Bicycle, 0.08},
|
||||
{HighwayType::HighwayTrunkLink, HighwayType::HighwayLivingStreet, VehicleType::Bicycle, 0.11},
|
||||
{HighwayType::HighwayTrunkLink, HighwayType::HighwayPrimary, VehicleType::Bicycle, 0.04},
|
||||
{HighwayType::HighwayTrunkLink, HighwayType::HighwayPrimaryLink, VehicleType::Bicycle, 0.04},
|
||||
{HighwayType::HighwayTrunkLink, HighwayType::HighwayResidential, VehicleType::Bicycle, 0.1},
|
||||
{HighwayType::HighwayTrunkLink, HighwayType::HighwaySecondary, VehicleType::Bicycle, 0.04},
|
||||
{HighwayType::HighwayTrunkLink, HighwayType::HighwaySecondaryLink, VehicleType::Bicycle, 0.04},
|
||||
{HighwayType::HighwayTrunkLink, HighwayType::HighwayService, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayTrunkLink, HighwayType::HighwayTertiary, VehicleType::Bicycle, 0.06},
|
||||
{HighwayType::HighwayTrunkLink, HighwayType::HighwayTertiaryLink, VehicleType::Bicycle, 0.06},
|
||||
{HighwayType::HighwayTrunkLink, HighwayType::HighwayTrunk, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayTrunkLink, HighwayType::HighwayTrunkLink, VehicleType::Bicycle, 0.02},
|
||||
{HighwayType::HighwayTrunkLink, HighwayType::HighwayUnclassified, VehicleType::Bicycle, 0.1},
|
||||
{HighwayType::HighwayUnclassified, HighwayType::HighwayLivingStreet, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayUnclassified, HighwayType::HighwayPrimary, VehicleType::Bicycle, 0.09},
|
||||
{HighwayType::HighwayUnclassified, HighwayType::HighwayPrimaryLink, VehicleType::Bicycle, 0.09},
|
||||
{HighwayType::HighwayUnclassified, HighwayType::HighwayResidential, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayUnclassified, HighwayType::HighwaySecondary, VehicleType::Bicycle, 0.08},
|
||||
{HighwayType::HighwayUnclassified, HighwayType::HighwaySecondaryLink, VehicleType::Bicycle, 0.08},
|
||||
{HighwayType::HighwayUnclassified, HighwayType::HighwayService, VehicleType::Bicycle, 0.08},
|
||||
{HighwayType::HighwayUnclassified, HighwayType::HighwayTertiary, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayUnclassified, HighwayType::HighwayTertiaryLink, VehicleType::Bicycle, 0.07},
|
||||
{HighwayType::HighwayUnclassified, HighwayType::HighwayTrunk, VehicleType::Bicycle, 0.09},
|
||||
{HighwayType::HighwayUnclassified, HighwayType::HighwayTrunkLink, VehicleType::Bicycle, 0.09},
|
||||
{HighwayType::HighwayUnclassified, HighwayType::HighwayUnclassified, VehicleType::Bicycle, 0.08},
|
||||
}};
|
||||
|
||||
array<TurnPenaltyMatrix, N> result{};
|
||||
|
||||
for (size_t i = 0; i < N; ++i)
|
||||
result[i] = {static_cast<int>(kTable[i].fromRoadType) * 65535 + static_cast<int>(kTable[i].toRoadType),
|
||||
kTable[i].vehicleType, kTable[i].penalty};
|
||||
|
||||
return result;
|
||||
}();
|
||||
|
||||
double totalPenalty = 0;
|
||||
|
||||
for (TurnPenaltyMatrix const & row : kTurnPenaltyMatrix)
|
||||
{
|
||||
if (row.vehicleType != vehicleType)
|
||||
continue;
|
||||
|
||||
m_turnPenaltyMap[row.road] += row.penalty;
|
||||
totalPenalty += row.penalty;
|
||||
}
|
||||
|
||||
if (!m_turnPenaltyMap.empty())
|
||||
{
|
||||
m_defaultPenalty = totalPenalty / m_turnPenaltyMap.size();
|
||||
LOG(LINFO, ("Loaded", m_turnPenaltyMap.size(), "turn penalties with default:", m_defaultPenalty));
|
||||
}
|
||||
else
|
||||
{
|
||||
LOG(LWARNING, ("No turn penalties loaded for vehicle type:", static_cast<int>(vehicleType)));
|
||||
}
|
||||
}
|
||||
|
||||
double EdgeEstimator::CalcHeuristic(ms::LatLon const & from, ms::LatLon const & to) const
|
||||
{
|
||||
// For the correct A*, we should use maximum _possible_ speed here, including:
|
||||
// default model, feature stored, unlimited autobahn, ferry or rail transit.
|
||||
return TimeBetweenSec(from, to, m_maxWeightSpeedMpS);
|
||||
}
|
||||
|
||||
double EdgeEstimator::ComputeDefaultLeapWeightSpeed() const
|
||||
{
|
||||
// 1.76 factor was computed as an average ratio of escape/enter speed to max MWM speed across all MWMs.
|
||||
// return m_maxWeightSpeedMpS / 1.76;
|
||||
|
||||
// By VNG: Current m_maxWeightSpeedMpS is > 120 km/h, so estimating speed was > 60km/h
|
||||
// for start/end fake edges by straight line! I strongly believe that this is very! optimistic.
|
||||
// Set speed to 57.5km/h (16m/s):
|
||||
// - lower bound Russia_MoscowDesnogorsk (https://github.com/organicmaps/organicmaps/issues/1071)
|
||||
// - upper bound RussiaSmolenskRussiaMoscowTimeTest
|
||||
return 16.0;
|
||||
}
|
||||
|
||||
/*
|
||||
double EdgeEstimator::LoadLeapWeightSpeed(NumMwmId mwmId)
|
||||
{
|
||||
double leapWeightSpeed = ComputeDefaultLeapWeightSpeed();
|
||||
|
||||
if (m_dataSourcePtr)
|
||||
{
|
||||
MwmSet::MwmHandle handle =
|
||||
m_dataSourcePtr->GetMwmHandleByCountryFile(m_numMwmIds->GetFile(mwmId));
|
||||
if (!handle.IsAlive())
|
||||
MYTHROW(RoutingException, ("Mwm", m_numMwmIds->GetFile(mwmId), "cannot be loaded."));
|
||||
|
||||
if (handle.GetInfo())
|
||||
leapWeightSpeed = handle.GetInfo()->GetRegionData().GetLeapWeightSpeed(leapWeightSpeed);
|
||||
}
|
||||
|
||||
if (leapWeightSpeed > m_maxWeightSpeedMpS)
|
||||
leapWeightSpeed = m_maxWeightSpeedMpS;
|
||||
|
||||
return leapWeightSpeed;
|
||||
}
|
||||
*/
|
||||
|
||||
double EdgeEstimator::GetLeapWeightSpeed(NumMwmId /*mwmId*/)
|
||||
{
|
||||
double defaultSpeed = ComputeDefaultLeapWeightSpeed();
|
||||
|
||||
/// @todo By VNG: We don't have LEAP_SPEEDS_FILE to assign RegionData::SetLeapWeightSpeed
|
||||
/// unique for each MWM, so this is useless now. And what about possible races here?
|
||||
// if (mwmId != kFakeNumMwmId)
|
||||
// {
|
||||
// auto [speedIt, inserted] = m_leapWeightSpeedMpS.emplace(mwmId, defaultSpeed);
|
||||
// if (inserted)
|
||||
// speedIt->second = LoadLeapWeightSpeed(mwmId);
|
||||
// return speedIt->second;
|
||||
// }
|
||||
|
||||
return defaultSpeed;
|
||||
}
|
||||
|
||||
double EdgeEstimator::CalcLeapWeight(ms::LatLon const & from, ms::LatLon const & to, NumMwmId mwmId)
|
||||
{
|
||||
return TimeBetweenSec(from, to, GetLeapWeightSpeed(mwmId));
|
||||
}
|
||||
|
||||
double EdgeEstimator::GetMaxWeightSpeedMpS() const
|
||||
{
|
||||
return m_maxWeightSpeedMpS;
|
||||
}
|
||||
|
||||
double EdgeEstimator::CalcOffroad(ms::LatLon const & from, ms::LatLon const & to, Purpose purpose) const
|
||||
{
|
||||
auto const offroadSpeedKMpH = purpose == Purpose::Weight ? m_offroadSpeedKMpH.m_weight : m_offroadSpeedKMpH.m_eta;
|
||||
if (offroadSpeedKMpH == kNotUsed)
|
||||
return 0.0;
|
||||
|
||||
return TimeBetweenSec(from, to, KmphToMps(offroadSpeedKMpH));
|
||||
}
|
||||
|
||||
// PedestrianEstimator -----------------------------------------------------------------------------
|
||||
class PedestrianEstimator final : public EdgeEstimator
|
||||
{
|
||||
public:
|
||||
PedestrianEstimator(double maxWeightSpeedKMpH, SpeedKMpH const & offroadSpeedKMpH)
|
||||
: EdgeEstimator(VehicleType::Pedestrian, maxWeightSpeedKMpH, offroadSpeedKMpH)
|
||||
{}
|
||||
|
||||
// EdgeEstimator overrides:
|
||||
double GetUTurnPenalty(Purpose /* purpose */) const override { return 0.0 /* seconds */; }
|
||||
|
||||
double GetTurnPenalty(Purpose purpose, double angle, RoadGeometry const & from_road, RoadGeometry const & to_road,
|
||||
bool is_left_hand_traffic = false) const override
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
double GetFerryLandingPenalty(Purpose purpose) const override
|
||||
{
|
||||
switch (purpose)
|
||||
{
|
||||
case Purpose::Weight: return 10 * 60; // seconds
|
||||
case Purpose::ETA: return 8 * 60; // seconds
|
||||
}
|
||||
UNREACHABLE();
|
||||
}
|
||||
|
||||
double CalcSegmentWeight(Segment const & segment, RoadGeometry const & road, Purpose purpose) const override
|
||||
{
|
||||
return CalcClimbSegment(purpose, segment, road,
|
||||
[purpose](double speedMpS, double tangent, geometry::Altitude altitude)
|
||||
{ return speedMpS / GetPedestrianClimbPenalty(purpose, tangent, altitude); });
|
||||
}
|
||||
};
|
||||
|
||||
// BicycleEstimator --------------------------------------------------------------------------------
|
||||
class BicycleEstimator final : public EdgeEstimator
|
||||
{
|
||||
public:
|
||||
BicycleEstimator(double maxWeightSpeedKMpH, SpeedKMpH const & offroadSpeedKMpH)
|
||||
: EdgeEstimator(VehicleType::Bicycle, maxWeightSpeedKMpH, offroadSpeedKMpH)
|
||||
{}
|
||||
|
||||
// EdgeEstimator overrides:
|
||||
double GetUTurnPenalty(Purpose /* purpose */) const override { return 20.0 /* seconds */; }
|
||||
|
||||
double GetTurnPenalty(Purpose purpose, double angle, RoadGeometry const & from_road, RoadGeometry const & to_road,
|
||||
bool is_left_hand_traffic = false) const override;
|
||||
|
||||
double GetFerryLandingPenalty(Purpose purpose) const override
|
||||
{
|
||||
switch (purpose)
|
||||
{
|
||||
case Purpose::Weight: return 10 * 60; // seconds
|
||||
case Purpose::ETA: return 8 * 60; // seconds
|
||||
}
|
||||
UNREACHABLE();
|
||||
}
|
||||
|
||||
double CalcSegmentWeight(Segment const & segment, RoadGeometry const & road, Purpose purpose) const override
|
||||
{
|
||||
return CalcClimbSegment(purpose, segment, road,
|
||||
[purpose, this](double speedMpS, double tangent, geometry::Altitude altitude)
|
||||
{
|
||||
auto const factor = GetBicycleClimbPenalty(purpose, tangent, altitude);
|
||||
ASSERT_GREATER(factor, 0.0, ());
|
||||
|
||||
/// @todo Take out "bad" bicycle road (path, track, footway, ...) check into BicycleModel?
|
||||
static double constexpr badBicycleRoadSpeed = KmphToMps(9);
|
||||
if (speedMpS <= badBicycleRoadSpeed)
|
||||
{
|
||||
if (factor > 1)
|
||||
speedMpS /= factor;
|
||||
}
|
||||
else if (factor > 1)
|
||||
{
|
||||
// Calculate uphill speed according to the average bicycle speed, because "good-roads" like
|
||||
// residential, secondary, cycleway are "equal-low-speed" uphill and road type doesn't matter.
|
||||
static double constexpr avgBicycleSpeed = KmphToMps(20);
|
||||
double const upperBound = avgBicycleSpeed / factor;
|
||||
if (speedMpS > upperBound)
|
||||
{
|
||||
// Add small weight to distinguish roads by class (10 is a max factor value).
|
||||
speedMpS = upperBound + (purpose == Purpose::Weight ? speedMpS / (10 * avgBicycleSpeed) : 0);
|
||||
}
|
||||
}
|
||||
else
|
||||
speedMpS /= factor;
|
||||
|
||||
return std::min(speedMpS, GetMaxWeightSpeedMpS());
|
||||
});
|
||||
}
|
||||
};
|
||||
|
||||
double BicycleEstimator::GetTurnPenalty(Purpose purpose, double angle, RoadGeometry const & from_road,
|
||||
RoadGeometry const & to_road, bool is_left_hand_traffic) const
|
||||
{
|
||||
auto penalty = m_defaultPenalty;
|
||||
|
||||
if (from_road.GetHighwayType().has_value() && to_road.GetHighwayType().has_value())
|
||||
{
|
||||
int const from_road_idx = static_cast<int>(from_road.GetHighwayType().value());
|
||||
int const to_road_idx = static_cast<int>(to_road.GetHighwayType().value());
|
||||
auto const pen = m_turnPenaltyMap.find(from_road_idx * 65535 + to_road_idx);
|
||||
if (pen != m_turnPenaltyMap.end())
|
||||
penalty = pen->second;
|
||||
}
|
||||
|
||||
// Determine if turn crosses traffic based on driving side
|
||||
// For bicycles, crossing traffic is less dangerous than for cars but still a factor
|
||||
bool turn_crosses_traffic;
|
||||
if (is_left_hand_traffic)
|
||||
turn_crosses_traffic = angle < 0;
|
||||
else
|
||||
turn_crosses_traffic = angle > 0;
|
||||
|
||||
// Penalty multiplier for bicycles crossing traffic
|
||||
auto const extra_penalty = turn_crosses_traffic ? 2.0 : 1.0;
|
||||
auto const result = fabs(angle) * penalty * extra_penalty;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// CarEstimator ------------------------------------------------------------------------------------
|
||||
class CarEstimator final : public EdgeEstimator
|
||||
{
|
||||
public:
|
||||
CarEstimator(DataSource * dataSourcePtr, std::shared_ptr<NumMwmIds> numMwmIds, shared_ptr<TrafficStash> trafficStash,
|
||||
double maxWeightSpeedKMpH, SpeedKMpH const & offroadSpeedKMpH)
|
||||
: EdgeEstimator(VehicleType::Car, maxWeightSpeedKMpH, offroadSpeedKMpH, dataSourcePtr, numMwmIds)
|
||||
, m_trafficStash(std::move(trafficStash))
|
||||
{}
|
||||
|
||||
// EdgeEstimator overrides:
|
||||
double CalcSegmentWeight(Segment const & segment, RoadGeometry const & road, Purpose purpose) const override;
|
||||
double GetUTurnPenalty(Purpose /* purpose */) const override;
|
||||
double GetTurnPenalty(Purpose purpose, double angle, RoadGeometry const & from_road, RoadGeometry const & to_road,
|
||||
bool is_left_hand_traffic = false) const override;
|
||||
double GetFerryLandingPenalty(Purpose purpose) const override
|
||||
{
|
||||
switch (purpose)
|
||||
{
|
||||
case Purpose::Weight: return 20 * 60; // seconds
|
||||
case Purpose::ETA: return 20 * 60; // seconds
|
||||
}
|
||||
UNREACHABLE();
|
||||
}
|
||||
|
||||
private:
|
||||
std::shared_ptr<TrafficStash> m_trafficStash;
|
||||
};
|
||||
|
||||
double CarEstimator::GetUTurnPenalty(Purpose /* purpose */) const
|
||||
{
|
||||
// Adds 2 minutes penalty for U-turn. The value is quite arbitrary
|
||||
// and needs to be properly selected after a number of real-world
|
||||
// experiments.
|
||||
return 2 * 60; // seconds
|
||||
}
|
||||
|
||||
double CarEstimator::GetTurnPenalty(Purpose purpose, double angle, RoadGeometry const & from_road,
|
||||
RoadGeometry const & to_road, bool is_left_hand_traffic) const
|
||||
{
|
||||
auto penalty = m_defaultPenalty;
|
||||
|
||||
if (from_road.GetHighwayType().has_value() && to_road.GetHighwayType().has_value())
|
||||
{
|
||||
int const from_road_idx = static_cast<int>(from_road.GetHighwayType().value());
|
||||
int const to_road_idx = static_cast<int>(to_road.GetHighwayType().value());
|
||||
auto const pen = m_turnPenaltyMap.find(from_road_idx * 65535 + to_road_idx);
|
||||
if (pen != m_turnPenaltyMap.end())
|
||||
penalty = pen->second;
|
||||
}
|
||||
|
||||
// Determine if turn crosses traffic based on driving side
|
||||
// @TODO We should really account for oneway roads etc.
|
||||
|
||||
bool turn_crosses_traffic;
|
||||
if (is_left_hand_traffic)
|
||||
turn_crosses_traffic = angle < 0;
|
||||
else
|
||||
turn_crosses_traffic = angle > 0;
|
||||
|
||||
// Twice as long to turn across traffic than not to
|
||||
auto const extra_penalty = turn_crosses_traffic ? 2.0 : 1.0;
|
||||
auto const result = fabs(angle) * penalty * extra_penalty;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
double CarEstimator::CalcSegmentWeight(Segment const & segment, RoadGeometry const & road, Purpose purpose) const
|
||||
{
|
||||
double result = road.GetDistance(segment.GetSegmentIdx()) / GetSpeedMpS(purpose, segment, road);
|
||||
|
||||
if (m_trafficStash)
|
||||
{
|
||||
SpeedGroup const speedGroup = m_trafficStash->GetSpeedGroup(segment);
|
||||
ASSERT_LESS(speedGroup, SpeedGroup::Count, ());
|
||||
double const trafficFactor = CalcTrafficFactor(speedGroup);
|
||||
result *= trafficFactor;
|
||||
if (speedGroup != SpeedGroup::Unknown && speedGroup != SpeedGroup::G5)
|
||||
{
|
||||
// Current time estimation are too optimistic.
|
||||
// Need more accurate tuning: traffic lights, traffic jams, road models and so on.
|
||||
// Add some penalty to make estimation more realistic.
|
||||
/// @todo Make accurate tuning, remove penalty.
|
||||
result *= 1.8;
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// EdgeEstimator -----------------------------------------------------------------------------------
|
||||
// static
|
||||
shared_ptr<EdgeEstimator> EdgeEstimator::Create(VehicleType vehicleType, double maxWeighSpeedKMpH,
|
||||
SpeedKMpH const & offroadSpeedKMpH,
|
||||
shared_ptr<TrafficStash> trafficStash, DataSource * dataSourcePtr,
|
||||
std::shared_ptr<NumMwmIds> numMwmIds)
|
||||
{
|
||||
switch (vehicleType)
|
||||
{
|
||||
case VehicleType::Pedestrian:
|
||||
case VehicleType::Transit: return make_shared<PedestrianEstimator>(maxWeighSpeedKMpH, offroadSpeedKMpH);
|
||||
case VehicleType::Bicycle: return make_shared<BicycleEstimator>(maxWeighSpeedKMpH, offroadSpeedKMpH);
|
||||
case VehicleType::Car:
|
||||
return make_shared<CarEstimator>(dataSourcePtr, numMwmIds, trafficStash, maxWeighSpeedKMpH, offroadSpeedKMpH);
|
||||
case VehicleType::Count: CHECK(false, ("Can't create EdgeEstimator for", vehicleType)); return nullptr;
|
||||
}
|
||||
UNREACHABLE();
|
||||
}
|
||||
|
||||
// static
|
||||
shared_ptr<EdgeEstimator> EdgeEstimator::Create(VehicleType vehicleType, VehicleModelInterface const & vehicleModel,
|
||||
shared_ptr<TrafficStash> trafficStash, DataSource * dataSourcePtr,
|
||||
std::shared_ptr<NumMwmIds> numMwmIds)
|
||||
{
|
||||
return Create(vehicleType, vehicleModel.GetMaxWeightSpeed(), vehicleModel.GetOffroadSpeed(), trafficStash,
|
||||
dataSourcePtr, numMwmIds);
|
||||
}
|
||||
} // namespace routing
|
||||
86
libs/routing/edge_estimator.hpp
Normal file
86
libs/routing/edge_estimator.hpp
Normal file
|
|
@ -0,0 +1,86 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/segment.hpp"
|
||||
#include "routing/vehicle_mask.hpp"
|
||||
|
||||
#include "routing_common/num_mwm_id.hpp"
|
||||
#include "routing_common/vehicle_model.hpp"
|
||||
|
||||
#include "geometry/latlon.hpp"
|
||||
#include "geometry/point_with_altitude.hpp"
|
||||
|
||||
#include <memory>
|
||||
#include <unordered_map>
|
||||
|
||||
class DataSource;
|
||||
|
||||
namespace routing
|
||||
{
|
||||
class RoadGeometry;
|
||||
class TrafficStash;
|
||||
|
||||
class EdgeEstimator
|
||||
{
|
||||
public:
|
||||
enum class Purpose
|
||||
{
|
||||
Weight,
|
||||
ETA
|
||||
};
|
||||
|
||||
EdgeEstimator(VehicleType vehicleType, double maxWeightSpeedKMpH, SpeedKMpH const & offroadSpeedKMpH,
|
||||
DataSource * dataSourcePtr = nullptr, std::shared_ptr<NumMwmIds> numMwmIds = nullptr);
|
||||
virtual ~EdgeEstimator() = default;
|
||||
|
||||
double CalcHeuristic(ms::LatLon const & from, ms::LatLon const & to) const;
|
||||
// Estimates time in seconds it takes to go from point |from| to point |to| along a leap (fake)
|
||||
// edge |from|-|to| using real features.
|
||||
// Note 1. The result of the method should be used if it's necessary to add a leap (fake) edge
|
||||
// (|from|, |to|) in road graph.
|
||||
// Note 2. The result of the method should be less or equal to CalcHeuristic(|from|, |to|).
|
||||
// Note 3. It's assumed here that CalcLeapWeight(p1, p2) == CalcLeapWeight(p2, p1).
|
||||
double CalcLeapWeight(ms::LatLon const & from, ms::LatLon const & to, NumMwmId mwmId = kFakeNumMwmId);
|
||||
|
||||
double GetMaxWeightSpeedMpS() const;
|
||||
|
||||
// Estimates time in seconds it takes to go from point |from| to point |to| along direct fake edge.
|
||||
double CalcOffroad(ms::LatLon const & from, ms::LatLon const & to, Purpose purpose) const;
|
||||
|
||||
virtual double CalcSegmentWeight(Segment const & segment, RoadGeometry const & road, Purpose purpose) const = 0;
|
||||
virtual double GetUTurnPenalty(Purpose purpose) const = 0;
|
||||
virtual double GetTurnPenalty(Purpose purpose, double angle, RoadGeometry const & from_road,
|
||||
RoadGeometry const & to_road, bool is_left_hand_traffic = false) const = 0;
|
||||
virtual double GetFerryLandingPenalty(Purpose purpose) const = 0;
|
||||
|
||||
static std::shared_ptr<EdgeEstimator> Create(VehicleType vehicleType, double maxWeighSpeedKMpH,
|
||||
SpeedKMpH const & offroadSpeedKMpH,
|
||||
std::shared_ptr<TrafficStash> trafficStash, DataSource * dataSourcePtr,
|
||||
std::shared_ptr<NumMwmIds> numMwmIds);
|
||||
|
||||
static std::shared_ptr<EdgeEstimator> Create(VehicleType vehicleType, VehicleModelInterface const & vehicleModel,
|
||||
std::shared_ptr<TrafficStash> trafficStash, DataSource * dataSourcePtr,
|
||||
std::shared_ptr<NumMwmIds> numMwmIds);
|
||||
|
||||
protected:
|
||||
VehicleType const m_vehicleType;
|
||||
double m_defaultPenalty = 0.0;
|
||||
std::unordered_map<int, double> m_turnPenaltyMap;
|
||||
|
||||
private:
|
||||
double const m_maxWeightSpeedMpS;
|
||||
SpeedKMpH const m_offroadSpeedKMpH;
|
||||
|
||||
// DataSource * m_dataSourcePtr;
|
||||
// std::shared_ptr<NumMwmIds> m_numMwmIds;
|
||||
// std::unordered_map<NumMwmId, double> m_leapWeightSpeedMpS;
|
||||
|
||||
double ComputeDefaultLeapWeightSpeed() const;
|
||||
double GetLeapWeightSpeed(NumMwmId mwmId);
|
||||
// double LoadLeapWeightSpeed(NumMwmId mwmId);
|
||||
};
|
||||
|
||||
double GetPedestrianClimbPenalty(EdgeEstimator::Purpose purpose, double tangent, geometry::Altitude altitudeM);
|
||||
double GetBicycleClimbPenalty(EdgeEstimator::Purpose purpose, double tangent, geometry::Altitude altitudeM);
|
||||
double GetCarClimbPenalty(EdgeEstimator::Purpose purpose, double tangent, geometry::Altitude altitudeM);
|
||||
|
||||
} // namespace routing
|
||||
38
libs/routing/fake_edges_container.hpp
Normal file
38
libs/routing/fake_edges_container.hpp
Normal file
|
|
@ -0,0 +1,38 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/fake_graph.hpp"
|
||||
#include "routing/fake_vertex.hpp"
|
||||
#include "routing/index_graph_starter.hpp"
|
||||
#include "routing/segment.hpp"
|
||||
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <limits>
|
||||
#include <utility>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
class FakeEdgesContainer final
|
||||
{
|
||||
friend class IndexGraphStarter;
|
||||
|
||||
public:
|
||||
FakeEdgesContainer(IndexGraphStarter && starter)
|
||||
: m_finish(std::move(starter.m_finish))
|
||||
, m_fake(std::move(starter.m_fake))
|
||||
{}
|
||||
|
||||
uint32_t GetNumFakeEdges() const
|
||||
{
|
||||
// Maximal number of fake segments in fake graph is numeric_limits<uint32_t>::max()
|
||||
// because segment idx type is uint32_t.
|
||||
CHECK_LESS_OR_EQUAL(m_fake.GetSize(), std::numeric_limits<uint32_t>::max(), ());
|
||||
return static_cast<uint32_t>(m_fake.GetSize());
|
||||
}
|
||||
|
||||
private:
|
||||
// Finish ending.
|
||||
IndexGraphStarter::Ending m_finish;
|
||||
FakeGraph m_fake;
|
||||
};
|
||||
} // namespace routing
|
||||
85
libs/routing/fake_ending.cpp
Normal file
85
libs/routing/fake_ending.cpp
Normal file
|
|
@ -0,0 +1,85 @@
|
|||
#include "routing/fake_ending.hpp"
|
||||
|
||||
#include "routing/index_graph.hpp"
|
||||
#include "routing/world_graph.hpp"
|
||||
|
||||
#include "geometry/distance_on_sphere.hpp"
|
||||
#include "geometry/mercator.hpp"
|
||||
#include "geometry/parametrized_segment.hpp"
|
||||
|
||||
#include "base/math.hpp"
|
||||
|
||||
#include <tuple>
|
||||
#include <utility>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
using namespace routing;
|
||||
using namespace std;
|
||||
|
||||
LatLonWithAltitude CalcProjectionToSegment(LatLonWithAltitude const & begin, LatLonWithAltitude const & end,
|
||||
m2::PointD const & point)
|
||||
{
|
||||
m2::ParametrizedSegment<m2::PointD> segment(mercator::FromLatLon(begin.GetLatLon()),
|
||||
mercator::FromLatLon(end.GetLatLon()));
|
||||
|
||||
auto const projectedPoint = segment.ClosestPointTo(point);
|
||||
auto const distBeginToEnd = ms::DistanceOnEarth(begin.GetLatLon(), end.GetLatLon());
|
||||
|
||||
auto const projectedLatLon = mercator::ToLatLon(projectedPoint);
|
||||
|
||||
double constexpr kEpsMeters = 2.0;
|
||||
if (AlmostEqualAbs(distBeginToEnd, 0.0, kEpsMeters))
|
||||
return LatLonWithAltitude(projectedLatLon, begin.GetAltitude());
|
||||
|
||||
auto const distBeginToProjection = ms::DistanceOnEarth(begin.GetLatLon(), projectedLatLon);
|
||||
|
||||
auto const altitude =
|
||||
begin.GetAltitude() + (end.GetAltitude() - begin.GetAltitude()) * distBeginToProjection / distBeginToEnd;
|
||||
return LatLonWithAltitude(projectedLatLon, altitude);
|
||||
}
|
||||
|
||||
bool Projection::operator==(Projection const & other) const
|
||||
{
|
||||
return tie(m_segment, m_isOneWay, m_segmentFront, m_segmentBack, m_junction) ==
|
||||
tie(other.m_segment, other.m_isOneWay, other.m_segmentFront, other.m_segmentBack, other.m_junction);
|
||||
}
|
||||
|
||||
FakeEnding MakeFakeEnding(vector<Segment> const & segments, m2::PointD const & point, WorldGraph & graph)
|
||||
{
|
||||
FakeEnding ending;
|
||||
double averageAltitude = 0.0;
|
||||
|
||||
for (size_t i = 0; i < segments.size(); ++i)
|
||||
{
|
||||
auto const & segment = segments[i];
|
||||
|
||||
bool const oneWay = graph.IsOneWay(segment.GetMwmId(), segment.GetFeatureId());
|
||||
auto const & frontJunction = graph.GetJunction(segment, true /* front */);
|
||||
auto const & backJunction = graph.GetJunction(segment, false /* front */);
|
||||
auto const & projectedJunction = CalcProjectionToSegment(backJunction, frontJunction, point);
|
||||
|
||||
ending.m_projections.emplace_back(segment, oneWay, frontJunction, backJunction, projectedJunction);
|
||||
|
||||
averageAltitude = (i * averageAltitude + projectedJunction.GetAltitude()) / (i + 1);
|
||||
}
|
||||
|
||||
ending.m_originJunction =
|
||||
LatLonWithAltitude(mercator::ToLatLon(point), static_cast<geometry::Altitude>(averageAltitude));
|
||||
return ending;
|
||||
}
|
||||
|
||||
FakeEnding MakeFakeEnding(Segment const & segment, m2::PointD const & point, IndexGraph & graph)
|
||||
{
|
||||
auto const & road = graph.GetRoadGeometry(segment.GetFeatureId());
|
||||
bool const oneWay = road.IsOneWay();
|
||||
auto const & frontJunction = road.GetJunction(segment.GetPointId(true /* front */));
|
||||
auto const & backJunction = road.GetJunction(segment.GetPointId(false /* front */));
|
||||
auto const & projectedJunction = CalcProjectionToSegment(backJunction, frontJunction, point);
|
||||
|
||||
FakeEnding ending;
|
||||
ending.m_originJunction = LatLonWithAltitude(mercator::ToLatLon(point), projectedJunction.GetAltitude());
|
||||
ending.m_projections.emplace_back(segment, oneWay, frontJunction, backJunction, projectedJunction);
|
||||
return ending;
|
||||
}
|
||||
} // namespace routing
|
||||
48
libs/routing/fake_ending.hpp
Normal file
48
libs/routing/fake_ending.hpp
Normal file
|
|
@ -0,0 +1,48 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/latlon_with_altitude.hpp"
|
||||
#include "routing/road_graph.hpp"
|
||||
#include "routing/segment.hpp"
|
||||
|
||||
#include "geometry/latlon.hpp"
|
||||
#include "geometry/point2d.hpp"
|
||||
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
class EdgeEstimator;
|
||||
class IndexGraph;
|
||||
class WorldGraph;
|
||||
|
||||
struct Projection final
|
||||
{
|
||||
Projection(Segment const & segment, bool isOneWay, LatLonWithAltitude const & segmentFront,
|
||||
LatLonWithAltitude const & segmentBack, LatLonWithAltitude const & junction)
|
||||
: m_segment(segment)
|
||||
, m_isOneWay(isOneWay)
|
||||
, m_segmentFront(segmentFront)
|
||||
, m_segmentBack(segmentBack)
|
||||
, m_junction(junction)
|
||||
{}
|
||||
bool operator==(Projection const & other) const;
|
||||
|
||||
Segment m_segment;
|
||||
bool m_isOneWay = false;
|
||||
LatLonWithAltitude m_segmentFront;
|
||||
LatLonWithAltitude m_segmentBack;
|
||||
LatLonWithAltitude m_junction;
|
||||
};
|
||||
|
||||
struct FakeEnding final
|
||||
{
|
||||
LatLonWithAltitude m_originJunction;
|
||||
std::vector<Projection> m_projections;
|
||||
};
|
||||
|
||||
FakeEnding MakeFakeEnding(std::vector<Segment> const & segments, m2::PointD const & point, WorldGraph & graph);
|
||||
FakeEnding MakeFakeEnding(Segment const & segment, m2::PointD const & point, IndexGraph & graph);
|
||||
|
||||
LatLonWithAltitude CalcProjectionToSegment(LatLonWithAltitude const & begin, LatLonWithAltitude const & end,
|
||||
m2::PointD const & point);
|
||||
} // namespace routing
|
||||
48
libs/routing/fake_feature_ids.hpp
Normal file
48
libs/routing/fake_feature_ids.hpp
Normal file
|
|
@ -0,0 +1,48 @@
|
|||
#pragma once
|
||||
|
||||
#include "indexer/fake_feature_ids.hpp"
|
||||
|
||||
#include <cstdint>
|
||||
#include <limits>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
struct FakeFeatureIds
|
||||
{
|
||||
static bool IsTransitFeature(uint32_t id)
|
||||
{
|
||||
return id >= kTransitGraphFeaturesStart && id != kIndexGraphStarterId &&
|
||||
!feature::FakeFeatureIds::IsEditorCreatedFeature(id);
|
||||
}
|
||||
|
||||
static bool IsGuidesFeature(uint32_t id)
|
||||
{
|
||||
return id >= kGuidesGraphFeaturesStart && id < kTransitGraphFeaturesStart;
|
||||
}
|
||||
|
||||
static uint32_t constexpr kIndexGraphStarterId = std::numeric_limits<uint32_t>::max();
|
||||
// It's important that |kGuidesGraphFeaturesStart| is greater than maximum number of real road
|
||||
// feature id. Also transit and guides fake feature id should not overlap with real feature id
|
||||
// and editor feature ids.
|
||||
static uint32_t constexpr k28BitsOffset = 0xfffffff;
|
||||
static uint32_t constexpr k24BitsOffset = 0xffffff;
|
||||
static uint32_t constexpr kTransitGraphFeaturesStart = std::numeric_limits<uint32_t>::max() - k28BitsOffset;
|
||||
static uint32_t constexpr kGuidesGraphFeaturesStart = kTransitGraphFeaturesStart - k24BitsOffset;
|
||||
};
|
||||
|
||||
static_assert(feature::FakeFeatureIds::kEditorCreatedFeaturesStart > FakeFeatureIds::kTransitGraphFeaturesStart,
|
||||
"routing::FakeFeatureIds::kTransitGraphFeaturesStart or "
|
||||
"feature::FakeFeatureIds::kEditorCreatedFeaturesStart was changed. "
|
||||
"Interval for transit fake features may be too small.");
|
||||
|
||||
static_assert(FakeFeatureIds::kTransitGraphFeaturesStart > FakeFeatureIds::kGuidesGraphFeaturesStart,
|
||||
"routing::FakeFeatureIds::kTransitGraphFeaturesStart or "
|
||||
"feature::FakeFeatureIds::kGuidesGraphFeaturesStart was changed. "
|
||||
"Interval for guides fake features may be too small.");
|
||||
|
||||
static_assert(feature::FakeFeatureIds::kEditorCreatedFeaturesStart - FakeFeatureIds::kTransitGraphFeaturesStart >=
|
||||
FakeFeatureIds::k24BitsOffset - feature::FakeFeatureIds::k20BitsOffset,
|
||||
"routing::FakeFeatureIds::kTransitGraphFeaturesStart or "
|
||||
"feature::FakeFeatureIds::kEditorCreatedFeaturesStart was changed. "
|
||||
"Interval for transit fake features may be too small.");
|
||||
} // namespace routing
|
||||
198
libs/routing/fake_graph.cpp
Normal file
198
libs/routing/fake_graph.cpp
Normal file
|
|
@ -0,0 +1,198 @@
|
|||
#include "routing/fake_graph.hpp"
|
||||
|
||||
#include "routing/fake_feature_ids.hpp"
|
||||
#include "routing/latlon_with_altitude.hpp"
|
||||
|
||||
#include "base/assert.hpp"
|
||||
#include "base/stl_helpers.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
void FakeGraph::AddStandaloneVertex(Segment const & newSegment, FakeVertex const & newVertex)
|
||||
{
|
||||
m_segmentToVertex[newSegment] = newVertex;
|
||||
m_vertexToSegment[newVertex] = newSegment;
|
||||
}
|
||||
|
||||
void FakeGraph::AddVertex(Segment const & existentSegment, Segment const & newSegment, FakeVertex const & newVertex,
|
||||
bool isOutgoing, bool isPartOfReal, Segment const & real)
|
||||
{
|
||||
AddStandaloneVertex(newSegment, newVertex);
|
||||
auto const & segmentFrom = isOutgoing ? existentSegment : newSegment;
|
||||
auto const & segmentTo = isOutgoing ? newSegment : existentSegment;
|
||||
m_outgoing[segmentFrom].insert(segmentTo);
|
||||
m_ingoing[segmentTo].insert(segmentFrom);
|
||||
if (isPartOfReal)
|
||||
{
|
||||
m_realToFake[real].insert(newSegment);
|
||||
m_fakeToReal[newSegment] = real;
|
||||
}
|
||||
}
|
||||
|
||||
void FakeGraph::AddConnection(Segment const & from, Segment const & to)
|
||||
{
|
||||
ASSERT(m_segmentToVertex.find(from) != m_segmentToVertex.end(), ("Segment", from, "does not exist in fake graph."));
|
||||
ASSERT(m_segmentToVertex.find(to) != m_segmentToVertex.end(), ("Segment", to, "does not exist in fake graph."));
|
||||
m_outgoing[from].insert(to);
|
||||
m_ingoing[to].insert(from);
|
||||
}
|
||||
|
||||
void FakeGraph::Append(FakeGraph const & rhs)
|
||||
{
|
||||
std::map<Segment, FakeVertex> intersection;
|
||||
typename std::map<Segment, FakeVertex>::iterator intersectionIt(intersection.begin());
|
||||
|
||||
std::set_intersection(m_segmentToVertex.begin(), m_segmentToVertex.end(), rhs.m_segmentToVertex.begin(),
|
||||
rhs.m_segmentToVertex.end(), std::inserter(intersection, intersectionIt),
|
||||
base::LessBy(&std::pair<Segment, FakeVertex>::first));
|
||||
|
||||
size_t countEqual = 0;
|
||||
for (auto const & segmentVertexPair : intersection)
|
||||
{
|
||||
auto const it = m_fakeToReal.find(segmentVertexPair.first);
|
||||
if (it == m_fakeToReal.cend() || !FakeFeatureIds::IsGuidesFeature(it->second.GetFeatureId()))
|
||||
countEqual++;
|
||||
}
|
||||
|
||||
CHECK_EQUAL(countEqual, 0, ("Fake segments are not unique."));
|
||||
|
||||
m_segmentToVertex.insert(rhs.m_segmentToVertex.begin(), rhs.m_segmentToVertex.end());
|
||||
m_vertexToSegment.insert(rhs.m_vertexToSegment.begin(), rhs.m_vertexToSegment.end());
|
||||
|
||||
for (auto const & kv : rhs.m_outgoing)
|
||||
m_outgoing[kv.first].insert(kv.second.begin(), kv.second.end());
|
||||
|
||||
for (auto const & kv : rhs.m_ingoing)
|
||||
m_ingoing[kv.first].insert(kv.second.begin(), kv.second.end());
|
||||
|
||||
for (auto const & kv : rhs.m_realToFake)
|
||||
m_realToFake[kv.first].insert(kv.second.begin(), kv.second.end());
|
||||
|
||||
m_fakeToReal.insert(rhs.m_fakeToReal.begin(), rhs.m_fakeToReal.end());
|
||||
}
|
||||
|
||||
FakeVertex const & FakeGraph::GetVertex(Segment const & segment) const
|
||||
{
|
||||
auto const it = m_segmentToVertex.find(segment);
|
||||
CHECK(it != m_segmentToVertex.end(), ("Vertex for invalid fake segment requested."));
|
||||
return it->second;
|
||||
}
|
||||
|
||||
std::set<Segment> const & FakeGraph::GetEdges(Segment const & segment, bool isOutgoing) const
|
||||
{
|
||||
auto const & adjacentEdges = isOutgoing ? m_outgoing : m_ingoing;
|
||||
|
||||
auto const it = adjacentEdges.find(segment);
|
||||
if (it != adjacentEdges.end())
|
||||
return it->second;
|
||||
|
||||
return kEmptySet;
|
||||
}
|
||||
|
||||
size_t FakeGraph::GetSize() const
|
||||
{
|
||||
return m_segmentToVertex.size();
|
||||
}
|
||||
|
||||
std::set<Segment> const & FakeGraph::GetFake(Segment const & real) const
|
||||
{
|
||||
auto const it = m_realToFake.find(real);
|
||||
if (it != m_realToFake.end())
|
||||
return it->second;
|
||||
|
||||
return kEmptySet;
|
||||
}
|
||||
|
||||
bool FakeGraph::FindReal(Segment const & fake, Segment & real) const
|
||||
{
|
||||
auto const it = m_fakeToReal.find(fake);
|
||||
if (it == m_fakeToReal.end())
|
||||
return false;
|
||||
|
||||
real = it->second;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool FakeGraph::FindSegment(FakeVertex const & vertex, Segment & segment) const
|
||||
{
|
||||
auto const it = m_vertexToSegment.find(vertex);
|
||||
if (it == m_vertexToSegment.end())
|
||||
return false;
|
||||
|
||||
segment = it->second;
|
||||
return true;
|
||||
}
|
||||
|
||||
void FakeGraph::ConnectLoopToGuideSegments(FakeVertex const & loop, Segment const & guidesSegment,
|
||||
LatLonWithAltitude const & guidesSegmentFrom,
|
||||
LatLonWithAltitude const & guidesSegmentTo,
|
||||
std::vector<std::pair<FakeVertex, Segment>> const & partsOfReal)
|
||||
{
|
||||
auto itLoop = m_vertexToSegment.find(loop);
|
||||
CHECK(itLoop != m_vertexToSegment.end(), (loop));
|
||||
|
||||
auto const & loopSegment = itLoop->second;
|
||||
auto const & loopPoint = loop.GetPointTo();
|
||||
|
||||
auto const backwardReal = Segment(guidesSegment.GetMwmId(), guidesSegment.GetFeatureId(),
|
||||
guidesSegment.GetSegmentIdx(), !guidesSegment.IsForward());
|
||||
|
||||
ConnectLoopToExistentPartsOfReal(loop, guidesSegment, backwardReal);
|
||||
|
||||
for (auto & [newVertex, newSegment] : partsOfReal)
|
||||
{
|
||||
auto const [it, inserted] = m_vertexToSegment.emplace(newVertex, newSegment);
|
||||
Segment const & segment = it->second;
|
||||
|
||||
Segment const & directedReal = (newVertex.GetPointFrom() == guidesSegmentTo.GetLatLon() ||
|
||||
newVertex.GetPointTo() == guidesSegmentFrom.GetLatLon())
|
||||
? backwardReal
|
||||
: guidesSegment;
|
||||
|
||||
m_realToFake[directedReal].insert(segment);
|
||||
|
||||
if (inserted)
|
||||
{
|
||||
m_fakeToReal[segment] = directedReal;
|
||||
m_segmentToVertex[segment] = newVertex;
|
||||
m_vertexToSegment[newVertex] = segment;
|
||||
}
|
||||
|
||||
CHECK((newVertex.GetPointFrom() == loopPoint || newVertex.GetPointTo() == loopPoint), (newVertex, loopPoint));
|
||||
|
||||
auto const & to = (newVertex.GetPointTo() == loopPoint) ? loopSegment : segment;
|
||||
auto const & from = (newVertex.GetPointFrom() == loopPoint) ? loopSegment : segment;
|
||||
|
||||
m_ingoing[to].insert(from);
|
||||
m_outgoing[from].insert(to);
|
||||
}
|
||||
}
|
||||
|
||||
void FakeGraph::ConnectLoopToExistentPartsOfReal(FakeVertex const & loop, Segment const & guidesSegment,
|
||||
Segment const & directedGuidesSegment)
|
||||
{
|
||||
auto const & loopSegment = m_vertexToSegment[loop];
|
||||
auto const & loopPoint = loop.GetPointTo();
|
||||
|
||||
for (auto const & real : {guidesSegment, directedGuidesSegment})
|
||||
{
|
||||
for (auto const & partOfReal : GetFake(real))
|
||||
{
|
||||
auto const & partOfRealVertex = m_segmentToVertex[partOfReal];
|
||||
if (partOfRealVertex.GetPointTo() == loopPoint)
|
||||
{
|
||||
m_outgoing[partOfReal].insert(loopSegment);
|
||||
m_ingoing[loopSegment].insert(partOfReal);
|
||||
}
|
||||
|
||||
if (partOfRealVertex.GetPointFrom() == loopPoint)
|
||||
{
|
||||
m_outgoing[loopSegment].insert(partOfReal);
|
||||
m_ingoing[partOfReal].insert(loopSegment);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
} // namespace routing
|
||||
64
libs/routing/fake_graph.hpp
Normal file
64
libs/routing/fake_graph.hpp
Normal file
|
|
@ -0,0 +1,64 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/fake_vertex.hpp"
|
||||
#include "routing/segment.hpp"
|
||||
|
||||
#include <map>
|
||||
#include <set>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
class FakeGraph
|
||||
{
|
||||
public:
|
||||
// Adds vertex with no connections.
|
||||
void AddStandaloneVertex(Segment const & newSegment, FakeVertex const & newVertex);
|
||||
// Adds vertex. Connects newSegment to existentSegment. Adds ingoing and
|
||||
// outgoing edges, fills segment to vertex mapping. Fills real to fake and fake to real
|
||||
// mapping if isPartOfReal is true.
|
||||
void AddVertex(Segment const & existentSegment, Segment const & newSegment, FakeVertex const & newVertex,
|
||||
bool isOutgoing, bool isPartOfReal, Segment const & real);
|
||||
// Adds connection from existent fake segment |from| to existent fake segment |to|
|
||||
void AddConnection(Segment const & from, Segment const & to);
|
||||
// Merges |rhs| into this.
|
||||
void Append(FakeGraph const & rhs);
|
||||
|
||||
// Returns Vertex which corresponds to |segment|. Segment must be a part of the fake graph.
|
||||
FakeVertex const & GetVertex(Segment const & segment) const;
|
||||
// Returns outgoing/ingoing edges set for specified segment.
|
||||
std::set<Segment> const & GetEdges(Segment const & segment, bool isOutgoing) const;
|
||||
size_t GetSize() const;
|
||||
std::set<Segment> const & GetFake(Segment const & real) const;
|
||||
|
||||
bool FindReal(Segment const & fake, Segment & real) const;
|
||||
bool FindSegment(FakeVertex const & vertex, Segment & segment) const;
|
||||
|
||||
// Connects loop to the PartOfReal segments on the Guides track.
|
||||
void ConnectLoopToGuideSegments(FakeVertex const & loop, Segment const & guidesSegment,
|
||||
LatLonWithAltitude const & guidesSegmentFrom,
|
||||
LatLonWithAltitude const & guidesSegmentTo,
|
||||
std::vector<std::pair<FakeVertex, Segment>> const & partsOfReal);
|
||||
|
||||
private:
|
||||
void ConnectLoopToExistentPartsOfReal(FakeVertex const & loop, Segment const & guidesSegment,
|
||||
Segment const & directedGuidesSegment);
|
||||
|
||||
// Key is fake segment, value is set of outgoing fake segments.
|
||||
std::map<Segment, std::set<Segment>> m_outgoing;
|
||||
// Key is fake segment, value is set of ingoing fake segments.
|
||||
std::map<Segment, std::set<Segment>> m_ingoing;
|
||||
// Key is fake segment, value is fake vertex which corresponds fake segment.
|
||||
std::map<Segment, FakeVertex> m_segmentToVertex;
|
||||
// Key is fake vertex, value is fake segment which corresponds fake vertex.
|
||||
std::map<FakeVertex, Segment> m_vertexToSegment;
|
||||
// Key is fake segment of type PartOfReal, value is corresponding real segment.
|
||||
std::map<Segment, Segment> m_fakeToReal;
|
||||
// Key is real segment, value is set of fake segments with type PartOfReal
|
||||
// which are parts of this real segment.
|
||||
std::map<Segment, std::set<Segment>> m_realToFake;
|
||||
// To return empty set by const reference.
|
||||
std::set<Segment> const kEmptySet = std::set<Segment>();
|
||||
};
|
||||
} // namespace routing
|
||||
76
libs/routing/fake_vertex.hpp
Normal file
76
libs/routing/fake_vertex.hpp
Normal file
|
|
@ -0,0 +1,76 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/latlon_with_altitude.hpp"
|
||||
#include "routing/road_graph.hpp"
|
||||
|
||||
#include "routing_common/num_mwm_id.hpp"
|
||||
|
||||
#include "geometry/latlon.hpp"
|
||||
|
||||
#include "base/visitor.hpp"
|
||||
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <tuple>
|
||||
#include <utility>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
class FakeVertex final
|
||||
{
|
||||
public:
|
||||
enum class Type
|
||||
{
|
||||
PureFake,
|
||||
PartOfReal,
|
||||
};
|
||||
|
||||
FakeVertex(NumMwmId numMwmId, LatLonWithAltitude const & from, LatLonWithAltitude const & to, Type type)
|
||||
: m_numMwmId(numMwmId)
|
||||
, m_from(from)
|
||||
, m_to(to)
|
||||
, m_type(type)
|
||||
{}
|
||||
|
||||
FakeVertex() = default;
|
||||
|
||||
bool operator==(FakeVertex const & rhs) const
|
||||
{
|
||||
return std::tie(m_numMwmId, m_from, m_to, m_type) == std::tie(rhs.m_numMwmId, rhs.m_from, rhs.m_to, rhs.m_type);
|
||||
}
|
||||
|
||||
bool operator<(FakeVertex const & rhs) const
|
||||
{
|
||||
return std::tie(m_type, m_from, m_to, m_numMwmId) < std::tie(rhs.m_type, rhs.m_from, rhs.m_to, rhs.m_numMwmId);
|
||||
}
|
||||
|
||||
LatLonWithAltitude const & GetJunctionFrom() const { return m_from; }
|
||||
ms::LatLon const & GetPointFrom() const { return m_from.GetLatLon(); }
|
||||
LatLonWithAltitude const & GetJunctionTo() const { return m_to; }
|
||||
ms::LatLon const & GetPointTo() const { return m_to.GetLatLon(); }
|
||||
Type GetType() const { return m_type; }
|
||||
|
||||
DECLARE_VISITOR(visitor(m_numMwmId, "m_numMwmId"), visitor(m_from, "m_from"), visitor(m_to, "m_to"),
|
||||
visitor(m_type, "m_type"))
|
||||
DECLARE_DEBUG_PRINT(FakeVertex)
|
||||
|
||||
private:
|
||||
// Note. It's important to know which mwm contains the FakeVertex if it is located
|
||||
// near an mwm borders along road features which are duplicated.
|
||||
NumMwmId m_numMwmId = kFakeNumMwmId;
|
||||
LatLonWithAltitude m_from;
|
||||
LatLonWithAltitude m_to;
|
||||
Type m_type = Type::PureFake;
|
||||
};
|
||||
|
||||
inline std::string DebugPrint(FakeVertex::Type type)
|
||||
{
|
||||
switch (type)
|
||||
{
|
||||
case FakeVertex::Type::PureFake: return "PureFake";
|
||||
case FakeVertex::Type::PartOfReal: return "PartOfReal";
|
||||
}
|
||||
CHECK(false, ("Unreachable"));
|
||||
return "UnknownFakeVertexType";
|
||||
}
|
||||
} // namespace routing
|
||||
320
libs/routing/features_road_graph.cpp
Normal file
320
libs/routing/features_road_graph.cpp
Normal file
|
|
@ -0,0 +1,320 @@
|
|||
#include "routing/features_road_graph.hpp"
|
||||
|
||||
#include "routing/data_source.hpp"
|
||||
#include "routing/nearest_edge_finder.hpp"
|
||||
#include "routing/routing_helpers.hpp"
|
||||
|
||||
#include "routing_common/vehicle_model.hpp"
|
||||
|
||||
#include "coding/point_coding.hpp"
|
||||
|
||||
#include <limits>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
using namespace std;
|
||||
|
||||
namespace
|
||||
{
|
||||
uint32_t constexpr kPowOfTwoForFeatureCacheSize = 10; // cache contains 2 ^ kPowOfTwoForFeatureCacheSize elements
|
||||
|
||||
double constexpr kMwmRoadCrossingRadiusMeters = 2.0;
|
||||
|
||||
auto constexpr kInvalidSpeedKMPH = numeric_limits<double>::max();
|
||||
} // namespace
|
||||
|
||||
double GetRoadCrossingRadiusMeters()
|
||||
{
|
||||
return kMwmRoadCrossingRadiusMeters;
|
||||
}
|
||||
|
||||
FeaturesRoadGraphBase::CrossCountryVehicleModel::CrossCountryVehicleModel(VehicleModelFactoryPtrT modelFactory)
|
||||
: m_modelFactory(modelFactory)
|
||||
, m_maxSpeed(m_modelFactory->GetVehicleModel()->GetMaxWeightSpeed())
|
||||
, m_offroadSpeedKMpH(m_modelFactory->GetVehicleModel()->GetOffroadSpeed())
|
||||
{}
|
||||
|
||||
SpeedKMpH FeaturesRoadGraphBase::CrossCountryVehicleModel::GetSpeed(FeatureType & f,
|
||||
SpeedParams const & speedParams) const
|
||||
{
|
||||
return GetVehicleModel(f.GetID())->GetSpeed(FeatureTypes(f), speedParams);
|
||||
}
|
||||
|
||||
std::optional<HighwayType> FeaturesRoadGraphBase::CrossCountryVehicleModel::GetHighwayType(FeatureType & f) const
|
||||
{
|
||||
return GetVehicleModel(f.GetID())->GetHighwayType(FeatureTypes(f));
|
||||
}
|
||||
|
||||
SpeedKMpH const & FeaturesRoadGraphBase::CrossCountryVehicleModel::GetOffroadSpeed() const
|
||||
{
|
||||
return m_offroadSpeedKMpH;
|
||||
}
|
||||
|
||||
bool FeaturesRoadGraphBase::CrossCountryVehicleModel::IsOneWay(FeatureType & f) const
|
||||
{
|
||||
return GetVehicleModel(f.GetID())->IsOneWay(FeatureTypes(f));
|
||||
}
|
||||
|
||||
bool FeaturesRoadGraphBase::CrossCountryVehicleModel::IsRoad(FeatureType & f) const
|
||||
{
|
||||
return GetVehicleModel(f.GetID())->IsRoad(FeatureTypes(f));
|
||||
}
|
||||
|
||||
bool FeaturesRoadGraphBase::CrossCountryVehicleModel::IsPassThroughAllowed(FeatureType & f) const
|
||||
{
|
||||
return GetVehicleModel(f.GetID())->IsPassThroughAllowed(FeatureTypes(f));
|
||||
}
|
||||
|
||||
VehicleModelInterface * FeaturesRoadGraphBase::CrossCountryVehicleModel::GetVehicleModel(
|
||||
FeatureID const & featureId) const
|
||||
{
|
||||
auto itr = m_cache.find(featureId.m_mwmId);
|
||||
if (itr != m_cache.end())
|
||||
return itr->second.get();
|
||||
|
||||
auto vehicleModel = m_modelFactory->GetVehicleModelForCountry(featureId.m_mwmId.GetInfo()->GetCountryName());
|
||||
|
||||
ASSERT(vehicleModel, ());
|
||||
ASSERT_EQUAL(m_maxSpeed, vehicleModel->GetMaxWeightSpeed(), ());
|
||||
|
||||
itr = m_cache.emplace(featureId.m_mwmId, std::move(vehicleModel)).first;
|
||||
return itr->second.get();
|
||||
}
|
||||
|
||||
void FeaturesRoadGraphBase::CrossCountryVehicleModel::Clear()
|
||||
{
|
||||
m_cache.clear();
|
||||
}
|
||||
|
||||
IRoadGraph::RoadInfo & FeaturesRoadGraphBase::RoadInfoCache::Find(FeatureID const & featureId, bool & found)
|
||||
{
|
||||
std::lock_guard lock(m_mutexCache);
|
||||
auto res = m_cache.emplace(featureId.m_mwmId, TMwmFeatureCache());
|
||||
if (res.second)
|
||||
res.first->second.Init(kPowOfTwoForFeatureCacheSize);
|
||||
return res.first->second.Find(featureId.m_index, found);
|
||||
}
|
||||
|
||||
void FeaturesRoadGraph::RoadInfoCache::Clear()
|
||||
{
|
||||
std::lock_guard lock(m_mutexCache);
|
||||
m_cache.clear();
|
||||
}
|
||||
|
||||
FeaturesRoadGraphBase::FeaturesRoadGraphBase(MwmDataSource & dataSource, IRoadGraph::Mode mode,
|
||||
shared_ptr<VehicleModelFactoryInterface> vehicleModelFactory)
|
||||
: m_dataSource(dataSource)
|
||||
, m_mode(mode)
|
||||
, m_vehicleModel(vehicleModelFactory)
|
||||
{}
|
||||
|
||||
class CrossFeaturesLoader
|
||||
{
|
||||
public:
|
||||
CrossFeaturesLoader(FeaturesRoadGraphBase const & graph, IRoadGraph::ICrossEdgesLoader & edgesLoader)
|
||||
: m_graph(graph)
|
||||
, m_edgesLoader(edgesLoader)
|
||||
{}
|
||||
|
||||
void operator()(FeatureType & ft)
|
||||
{
|
||||
if (!m_graph.IsRoad(ft))
|
||||
return;
|
||||
|
||||
FeatureID const & featureId = ft.GetID();
|
||||
|
||||
IRoadGraph::RoadInfo const & roadInfo = m_graph.GetCachedRoadInfo(featureId, ft, kInvalidSpeedKMPH);
|
||||
CHECK_EQUAL(roadInfo.m_speedKMPH, kInvalidSpeedKMPH, ());
|
||||
|
||||
m_edgesLoader(featureId, roadInfo.m_junctions, roadInfo.m_bidirectional);
|
||||
}
|
||||
|
||||
private:
|
||||
FeaturesRoadGraphBase const & m_graph;
|
||||
IRoadGraph::ICrossEdgesLoader & m_edgesLoader;
|
||||
};
|
||||
|
||||
void FeaturesRoadGraphBase::ForEachFeatureClosestToCross(m2::PointD const & cross,
|
||||
ICrossEdgesLoader & edgesLoader) const
|
||||
{
|
||||
CrossFeaturesLoader featuresLoader(*this, edgesLoader);
|
||||
m2::RectD const rect = mercator::RectByCenterXYAndSizeInMeters(cross, kMwmRoadCrossingRadiusMeters);
|
||||
m_dataSource.ForEachStreet(featuresLoader, rect);
|
||||
}
|
||||
|
||||
void FeaturesRoadGraphBase::FindClosestEdges(m2::RectD const & rect, uint32_t count,
|
||||
vector<pair<Edge, geometry::PointWithAltitude>> & vicinities) const
|
||||
{
|
||||
NearestEdgeFinder finder(rect.Center(), nullptr /* IsEdgeProjGood */);
|
||||
|
||||
m_dataSource.ForEachStreet([&](FeatureType & ft)
|
||||
{
|
||||
if (!m_vehicleModel.IsRoad(ft))
|
||||
return;
|
||||
|
||||
FeatureID const & featureId = ft.GetID();
|
||||
|
||||
IRoadGraph::RoadInfo const & roadInfo = GetCachedRoadInfo(featureId, ft, kInvalidSpeedKMPH);
|
||||
finder.AddInformationSource(IRoadGraph::FullRoadInfo(featureId, roadInfo));
|
||||
}, rect);
|
||||
|
||||
finder.MakeResult(vicinities, count);
|
||||
}
|
||||
|
||||
vector<IRoadGraph::FullRoadInfo> FeaturesRoadGraphBase::FindRoads(m2::RectD const & rect,
|
||||
IsGoodFeatureFn const & isGoodFeature) const
|
||||
{
|
||||
vector<IRoadGraph::FullRoadInfo> roads;
|
||||
|
||||
m_dataSource.ForEachStreet([&](FeatureType & ft)
|
||||
{
|
||||
if (!m_vehicleModel.IsRoad(ft))
|
||||
return;
|
||||
|
||||
FeatureID const & featureId = ft.GetID();
|
||||
if (isGoodFeature && !isGoodFeature(featureId))
|
||||
return;
|
||||
|
||||
// DataSource::ForEachInRect() gives not only features inside |rect| but some other features
|
||||
// which lie close to the rect. Removes all the features which don't cross |rect|.
|
||||
auto const & roadInfo = GetCachedRoadInfo(featureId, ft, kInvalidSpeedKMPH);
|
||||
if (!RectCoversPolyline(roadInfo.m_junctions, rect))
|
||||
return;
|
||||
|
||||
roads.emplace_back(featureId, roadInfo);
|
||||
}, rect);
|
||||
|
||||
return roads;
|
||||
}
|
||||
|
||||
void FeaturesRoadGraphBase::GetFeatureTypes(FeatureID const & featureId, feature::TypesHolder & types) const
|
||||
{
|
||||
auto ft = m_dataSource.GetFeature(featureId);
|
||||
if (!ft)
|
||||
return;
|
||||
|
||||
ASSERT_EQUAL(ft->GetGeomType(), feature::GeomType::Line, ());
|
||||
types = feature::TypesHolder(*ft);
|
||||
}
|
||||
|
||||
void FeaturesRoadGraphBase::GetJunctionTypes(geometry::PointWithAltitude const & junction,
|
||||
feature::TypesHolder & types) const
|
||||
{
|
||||
types = feature::TypesHolder();
|
||||
|
||||
m2::PointD const & cross = junction.GetPoint();
|
||||
m2::RectD const rect = mercator::RectByCenterXYAndSizeInMeters(cross, kMwmRoadCrossingRadiusMeters);
|
||||
|
||||
m_dataSource.ForEachStreet([&](FeatureType & ft)
|
||||
{
|
||||
if (types.Empty() && ft.GetGeomType() == feature::GeomType::Point &&
|
||||
AlmostEqualAbs(ft.GetCenter(), cross, kMwmPointAccuracy))
|
||||
{
|
||||
types = feature::TypesHolder(ft);
|
||||
}
|
||||
}, rect);
|
||||
}
|
||||
|
||||
IRoadGraph::Mode FeaturesRoadGraphBase::GetMode() const
|
||||
{
|
||||
return m_mode;
|
||||
}
|
||||
|
||||
void FeaturesRoadGraphBase::ClearState()
|
||||
{
|
||||
m_cache.Clear();
|
||||
m_vehicleModel.Clear();
|
||||
}
|
||||
|
||||
bool FeaturesRoadGraphBase::IsRoad(FeatureType & ft) const
|
||||
{
|
||||
return m_vehicleModel.IsRoad(ft);
|
||||
}
|
||||
|
||||
bool FeaturesRoadGraphBase::IsOneWay(FeatureType & ft) const
|
||||
{
|
||||
return m_vehicleModel.IsOneWay(ft);
|
||||
}
|
||||
|
||||
double FeaturesRoadGraphBase::GetSpeedKMpHFromFt(FeatureType & ft, SpeedParams const & speedParams) const
|
||||
{
|
||||
return m_vehicleModel.GetSpeed(ft, speedParams).m_weight;
|
||||
}
|
||||
|
||||
void FeaturesRoadGraphBase::ExtractRoadInfo(FeatureID const & featureId, FeatureType & ft, double speedKMpH,
|
||||
RoadInfo & ri) const
|
||||
{
|
||||
ri.m_speedKMPH = speedKMpH;
|
||||
ri.m_bidirectional = !IsOneWay(ft);
|
||||
|
||||
ft.ParseGeometry(FeatureType::BEST_GEOMETRY);
|
||||
size_t const pointsCount = ft.GetPointsCount();
|
||||
|
||||
geometry::Altitudes altitudes;
|
||||
auto const loader = GetAltitudesLoader(featureId.m_mwmId);
|
||||
if (loader)
|
||||
altitudes = loader->GetAltitudes(featureId.m_index, pointsCount);
|
||||
else
|
||||
altitudes = geometry::Altitudes(pointsCount, geometry::kDefaultAltitudeMeters);
|
||||
|
||||
CHECK_EQUAL(altitudes.size(), pointsCount, ("GetAltitudes for", featureId, "returns wrong altitudes:", altitudes));
|
||||
|
||||
ri.m_junctions.resize(pointsCount);
|
||||
for (size_t i = 0; i < pointsCount; ++i)
|
||||
{
|
||||
auto & pt = ri.m_junctions[i];
|
||||
pt.SetPoint(ft.GetPoint(i));
|
||||
pt.SetAltitude(altitudes[i]);
|
||||
}
|
||||
}
|
||||
|
||||
IRoadGraph::RoadInfo const & FeaturesRoadGraphBase::GetCachedRoadInfo(FeatureID const & featureId,
|
||||
SpeedParams const & speedParams) const
|
||||
{
|
||||
bool found = false;
|
||||
RoadInfo & ri = m_cache.Find(featureId, found);
|
||||
|
||||
if (found)
|
||||
return ri;
|
||||
|
||||
auto ft = m_dataSource.GetFeature(featureId);
|
||||
if (!ft)
|
||||
return ri;
|
||||
|
||||
ASSERT_EQUAL(ft->GetGeomType(), feature::GeomType::Line, ());
|
||||
|
||||
ExtractRoadInfo(featureId, *ft, GetSpeedKMpHFromFt(*ft, speedParams), ri);
|
||||
return ri;
|
||||
}
|
||||
|
||||
IRoadGraph::RoadInfo const & FeaturesRoadGraphBase::GetCachedRoadInfo(FeatureID const & featureId, FeatureType & ft,
|
||||
double speedKMPH) const
|
||||
{
|
||||
bool found = false;
|
||||
RoadInfo & ri = m_cache.Find(featureId, found);
|
||||
if (found)
|
||||
return ri;
|
||||
|
||||
ASSERT_EQUAL(featureId, ft.GetID(), ());
|
||||
ExtractRoadInfo(featureId, ft, speedKMPH, ri);
|
||||
return ri;
|
||||
}
|
||||
|
||||
feature::AltitudeLoaderCached * FeaturesRoadGraph::GetAltitudesLoader(MwmSet::MwmId const & mwmId) const
|
||||
{
|
||||
auto it = m_altitudes.find(mwmId);
|
||||
if (it == m_altitudes.end())
|
||||
{
|
||||
auto const & handle = m_dataSource.GetHandle(mwmId);
|
||||
it = m_altitudes.emplace(mwmId, *handle.GetValue()).first;
|
||||
}
|
||||
return &(it->second);
|
||||
}
|
||||
|
||||
void FeaturesRoadGraph::ClearState()
|
||||
{
|
||||
FeaturesRoadGraphBase::ClearState();
|
||||
m_altitudes.clear();
|
||||
}
|
||||
|
||||
} // namespace routing
|
||||
146
libs/routing/features_road_graph.hpp
Normal file
146
libs/routing/features_road_graph.hpp
Normal file
|
|
@ -0,0 +1,146 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/road_graph.hpp"
|
||||
|
||||
#include "routing_common/vehicle_model.hpp"
|
||||
|
||||
#include "indexer/altitude_loader.hpp"
|
||||
#include "indexer/feature_data.hpp"
|
||||
#include "indexer/mwm_set.hpp"
|
||||
|
||||
#include "geometry/point2d.hpp"
|
||||
#include "geometry/point_with_altitude.hpp"
|
||||
|
||||
#include "base/cache.hpp"
|
||||
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
class FeatureType;
|
||||
|
||||
namespace routing
|
||||
{
|
||||
class MwmDataSource;
|
||||
|
||||
class FeaturesRoadGraphBase : public IRoadGraph
|
||||
{
|
||||
protected:
|
||||
using VehicleModelFactoryPtrT = std::shared_ptr<VehicleModelFactoryInterface>;
|
||||
|
||||
private:
|
||||
class CrossCountryVehicleModel
|
||||
{
|
||||
using FeatureTypes = VehicleModelInterface::FeatureTypes;
|
||||
|
||||
public:
|
||||
explicit CrossCountryVehicleModel(VehicleModelFactoryPtrT modelFactory);
|
||||
|
||||
// VehicleModelInterface overrides:
|
||||
SpeedKMpH GetSpeed(FeatureType & f, SpeedParams const & speedParams) const;
|
||||
std::optional<HighwayType> GetHighwayType(FeatureType & f) const;
|
||||
double GetMaxWeightSpeed() const { return m_maxSpeed; }
|
||||
SpeedKMpH const & GetOffroadSpeed() const;
|
||||
bool IsOneWay(FeatureType & f) const;
|
||||
bool IsRoad(FeatureType & f) const;
|
||||
bool IsPassThroughAllowed(FeatureType & f) const;
|
||||
|
||||
void Clear();
|
||||
|
||||
private:
|
||||
VehicleModelInterface * GetVehicleModel(FeatureID const & featureId) const;
|
||||
|
||||
VehicleModelFactoryPtrT const m_modelFactory;
|
||||
double const m_maxSpeed;
|
||||
SpeedKMpH const m_offroadSpeedKMpH;
|
||||
|
||||
mutable std::map<MwmSet::MwmId, std::shared_ptr<VehicleModelInterface>> m_cache;
|
||||
};
|
||||
|
||||
class RoadInfoCache
|
||||
{
|
||||
public:
|
||||
RoadInfo & Find(FeatureID const & featureId, bool & found);
|
||||
|
||||
void Clear();
|
||||
|
||||
private:
|
||||
using TMwmFeatureCache = base::Cache<uint32_t, RoadInfo>;
|
||||
|
||||
std::mutex m_mutexCache;
|
||||
std::map<MwmSet::MwmId, TMwmFeatureCache> m_cache;
|
||||
};
|
||||
|
||||
public:
|
||||
static double constexpr kClosestEdgesRadiusM = 150.0;
|
||||
|
||||
FeaturesRoadGraphBase(MwmDataSource & dataSource, IRoadGraph::Mode mode, VehicleModelFactoryPtrT modelFactory);
|
||||
|
||||
static int GetStreetReadScale();
|
||||
|
||||
/// @name IRoadGraph overrides
|
||||
/// @{
|
||||
void ForEachFeatureClosestToCross(m2::PointD const & cross, ICrossEdgesLoader & edgesLoader) const override;
|
||||
void FindClosestEdges(m2::RectD const & rect, uint32_t count,
|
||||
std::vector<std::pair<Edge, geometry::PointWithAltitude>> & vicinities) const override;
|
||||
std::vector<IRoadGraph::FullRoadInfo> FindRoads(m2::RectD const & rect,
|
||||
IsGoodFeatureFn const & isGoodFeature) const override;
|
||||
void GetFeatureTypes(FeatureID const & featureId, feature::TypesHolder & types) const override;
|
||||
void GetJunctionTypes(geometry::PointWithAltitude const & junction, feature::TypesHolder & types) const override;
|
||||
IRoadGraph::Mode GetMode() const override;
|
||||
void ClearState() override;
|
||||
/// @}
|
||||
|
||||
bool IsRoad(FeatureType & ft) const;
|
||||
|
||||
protected:
|
||||
MwmDataSource & m_dataSource;
|
||||
|
||||
virtual feature::AltitudeLoaderBase * GetAltitudesLoader(MwmSet::MwmId const & mwmId) const
|
||||
{
|
||||
// Don't retrieve altitudes here because FeaturesRoadGraphBase is used in IndexRouter for
|
||||
// IndexRouter::FindClosestProjectionToRoad and IndexRouter::FindBestEdges only.
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
private:
|
||||
friend class CrossFeaturesLoader;
|
||||
|
||||
bool IsOneWay(FeatureType & ft) const;
|
||||
double GetSpeedKMpHFromFt(FeatureType & ft, SpeedParams const & speedParams) const;
|
||||
|
||||
// Searches a feature RoadInfo in the cache, and if does not find then
|
||||
// loads feature from the index and takes speed for the feature from the vehicle model.
|
||||
RoadInfo const & GetCachedRoadInfo(FeatureID const & featureId, SpeedParams const & speedParams) const;
|
||||
// Searches a feature RoadInfo in the cache, and if does not find then takes passed feature and speed.
|
||||
// This version is used to prevent redundant feature loading when feature speed is known.
|
||||
RoadInfo const & GetCachedRoadInfo(FeatureID const & featureId, FeatureType & ft, double speedKMPH) const;
|
||||
void ExtractRoadInfo(FeatureID const & featureId, FeatureType & ft, double speedKMpH, RoadInfo & ri) const;
|
||||
|
||||
IRoadGraph::Mode const m_mode;
|
||||
mutable RoadInfoCache m_cache;
|
||||
mutable CrossCountryVehicleModel m_vehicleModel;
|
||||
};
|
||||
|
||||
class FeaturesRoadGraph : public FeaturesRoadGraphBase
|
||||
{
|
||||
mutable std::map<MwmSet::MwmId, feature::AltitudeLoaderCached> m_altitudes;
|
||||
|
||||
public:
|
||||
FeaturesRoadGraph(MwmDataSource & dataSource, IRoadGraph::Mode mode, VehicleModelFactoryPtrT modelFactory)
|
||||
: FeaturesRoadGraphBase(dataSource, mode, modelFactory)
|
||||
{}
|
||||
|
||||
protected:
|
||||
feature::AltitudeLoaderCached * GetAltitudesLoader(MwmSet::MwmId const & mwmId) const override;
|
||||
void ClearState() override;
|
||||
};
|
||||
|
||||
// @returns a distance d such as that for a given point p any edge
|
||||
// with start point s such as that |s - p| < d, and edge is considered outgouing from p.
|
||||
// Symmetrically for ingoing edges.
|
||||
double GetRoadCrossingRadiusMeters();
|
||||
} // namespace routing
|
||||
67
libs/routing/following_info.hpp
Normal file
67
libs/routing/following_info.hpp
Normal file
|
|
@ -0,0 +1,67 @@
|
|||
#pragma once
|
||||
|
||||
#include "platform/distance.hpp"
|
||||
|
||||
#include "routing/lanes/lane_info.hpp"
|
||||
#include "routing/turns.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
class FollowingInfo
|
||||
{
|
||||
public:
|
||||
FollowingInfo()
|
||||
: m_turn(turns::CarDirection::None)
|
||||
, m_nextTurn(turns::CarDirection::None)
|
||||
, m_exitNum(0)
|
||||
, m_time(0)
|
||||
, m_completionPercent(0)
|
||||
, m_pedestrianTurn(turns::PedestrianDirection::None)
|
||||
{}
|
||||
|
||||
bool IsValid() const { return m_distToTarget.IsValid(); }
|
||||
|
||||
/// @name Formatted covered distance.
|
||||
platform::Distance m_distToTarget;
|
||||
|
||||
/// @name Formatted distance to the next turn.
|
||||
//@{
|
||||
platform::Distance m_distToTurn;
|
||||
turns::CarDirection m_turn;
|
||||
/// Turn after m_turn. Returns NoTurn if there is no turns after.
|
||||
turns::CarDirection m_nextTurn;
|
||||
uint32_t m_exitNum;
|
||||
//@}
|
||||
int m_time;
|
||||
/// Contains lane information on the edge before the turn.
|
||||
turns::lanes::LanesInfo m_lanes;
|
||||
// m_turnNotifications contains information about the next turn notifications.
|
||||
// If there is nothing to pronounce m_turnNotifications is empty.
|
||||
// If there is something to pronounce the size of m_turnNotifications may be one or even more
|
||||
// depends on the number of notifications to prononce.
|
||||
std::vector<std::string> m_turnNotifications;
|
||||
// Current street name. May be empty.
|
||||
std::string m_currentStreetName;
|
||||
// The next street name. May be empty.
|
||||
std::string m_nextStreetName;
|
||||
// The next next street name. May be empty.
|
||||
std::string m_nextNextStreetName;
|
||||
|
||||
// Percentage of the route completion.
|
||||
double m_completionPercent;
|
||||
|
||||
/// @name Pedestrian direction information
|
||||
//@{
|
||||
turns::PedestrianDirection m_pedestrianTurn;
|
||||
//@}
|
||||
|
||||
// Current speed limit in meters per second.
|
||||
// If no info about speed limit then m_speedLimitMps < 0.
|
||||
double m_speedLimitMps = -1.0;
|
||||
};
|
||||
} // namespace routing
|
||||
306
libs/routing/geometry.cpp
Normal file
306
libs/routing/geometry.cpp
Normal file
|
|
@ -0,0 +1,306 @@
|
|||
#include "routing/geometry.hpp"
|
||||
|
||||
#include "routing/city_roads.hpp"
|
||||
#include "routing/maxspeeds.hpp"
|
||||
|
||||
#include "indexer/altitude_loader.hpp"
|
||||
#include "indexer/feature.hpp"
|
||||
#include "indexer/feature_source.hpp"
|
||||
|
||||
#include "geometry/distance_on_sphere.hpp"
|
||||
#include "geometry/mercator.hpp"
|
||||
|
||||
#include "base/assert.hpp"
|
||||
#include "base/string_utils.hpp"
|
||||
|
||||
#include <string>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
using namespace std;
|
||||
|
||||
double CalcFerryDurationHours(string_view durationHours, double roadLenKm)
|
||||
{
|
||||
// Look for more info: https://confluence.mail.ru/display/MAPSME/Ferries
|
||||
// Shortly: the coefs were received from statistic about ferries with durations in OSM.
|
||||
double constexpr kIntercept = 0.2490726747447476;
|
||||
/// @todo This constant means that average ferry speed is 1/0.02 = 50km/h OMG!
|
||||
double constexpr kSlope = 0.02078913;
|
||||
|
||||
if (durationHours.empty())
|
||||
return kIntercept + kSlope * roadLenKm;
|
||||
|
||||
double durationH = 0.0;
|
||||
CHECK(strings::to_double(durationHours, durationH), (durationHours));
|
||||
|
||||
// See: https://confluence.mail.ru/download/attachments/249123157/image2019-8-22_16-15-53.png
|
||||
// Shortly: we drop some points: (x: lengthKm, y: durationH), that are upper or lower these two lines.
|
||||
double constexpr kUpperBoundIntercept = 4.0;
|
||||
double constexpr kUpperBoundSlope = 0.037;
|
||||
if (kUpperBoundIntercept + kUpperBoundSlope * roadLenKm - durationH < 0)
|
||||
return kIntercept + kSlope * roadLenKm;
|
||||
|
||||
double constexpr kLowerBoundIntercept = -2.0;
|
||||
double constexpr kLowerBoundSlope = 0.015;
|
||||
if (kLowerBoundIntercept + kLowerBoundSlope * roadLenKm - durationH > 0)
|
||||
return kIntercept + kSlope * roadLenKm;
|
||||
|
||||
return durationH;
|
||||
}
|
||||
|
||||
class RoadAttrsGetter
|
||||
{
|
||||
public:
|
||||
void Load(FilesContainerR const & cont)
|
||||
{
|
||||
if (cont.IsExist(CITY_ROADS_FILE_TAG))
|
||||
m_cityRoads.Load(cont.GetReader(CITY_ROADS_FILE_TAG));
|
||||
|
||||
if (cont.IsExist(MAXSPEEDS_FILE_TAG))
|
||||
m_maxSpeeds.Load(cont.GetReader(MAXSPEEDS_FILE_TAG));
|
||||
}
|
||||
|
||||
public:
|
||||
Maxspeeds m_maxSpeeds;
|
||||
CityRoads m_cityRoads;
|
||||
};
|
||||
|
||||
namespace
|
||||
{
|
||||
class GeometryLoaderImpl final : public GeometryLoader
|
||||
{
|
||||
public:
|
||||
GeometryLoaderImpl(MwmSet::MwmHandle const & handle, VehicleModelPtrT const & vehicleModel, bool loadAltitudes)
|
||||
: m_vehicleModel(vehicleModel)
|
||||
, m_source(handle)
|
||||
, m_altitudeLoader(*handle.GetValue())
|
||||
, m_loadAltitudes(loadAltitudes)
|
||||
{
|
||||
m_attrsGetter.Load(handle.GetValue()->m_cont);
|
||||
}
|
||||
|
||||
void Load(uint32_t featureId, RoadGeometry & road) override
|
||||
{
|
||||
auto feature = m_source.GetOriginalFeature(featureId);
|
||||
feature->ParseGeometry(FeatureType::BEST_GEOMETRY);
|
||||
|
||||
geometry::Altitudes altitudes;
|
||||
if (m_loadAltitudes)
|
||||
altitudes = m_altitudeLoader.GetAltitudes(featureId, feature->GetPointsCount());
|
||||
|
||||
road.Load(*m_vehicleModel, *feature, altitudes.empty() ? nullptr : &altitudes, m_attrsGetter);
|
||||
}
|
||||
|
||||
SpeedInUnits GetSavedMaxspeed(uint32_t featureId, bool forward) override
|
||||
{
|
||||
auto const speed = m_attrsGetter.m_maxSpeeds.GetMaxspeed(featureId);
|
||||
return {speed.GetSpeedInUnits(forward), speed.GetUnits()};
|
||||
}
|
||||
|
||||
private:
|
||||
VehicleModelPtrT m_vehicleModel;
|
||||
RoadAttrsGetter m_attrsGetter;
|
||||
FeatureSource m_source;
|
||||
feature::AltitudeLoaderBase m_altitudeLoader;
|
||||
bool const m_loadAltitudes;
|
||||
};
|
||||
|
||||
class FileGeometryLoader final : public GeometryLoader
|
||||
{
|
||||
public:
|
||||
FileGeometryLoader(string const & fileName, VehicleModelPtrT const & vehicleModel)
|
||||
: m_featuresVector(fileName)
|
||||
, m_vehicleModel(vehicleModel)
|
||||
{
|
||||
m_attrsGetter.Load(m_featuresVector.GetContainer());
|
||||
}
|
||||
|
||||
void Load(uint32_t featureId, RoadGeometry & road) override
|
||||
{
|
||||
auto feature = m_featuresVector.GetVector().GetByIndex(featureId);
|
||||
CHECK(feature, ());
|
||||
feature->SetID({{}, featureId});
|
||||
feature->ParseGeometry(FeatureType::BEST_GEOMETRY);
|
||||
|
||||
// Note. If FileGeometryLoader is used for generation cross mwm section for bicycle or
|
||||
// pedestrian routing |altitudes| should be used here.
|
||||
road.Load(*m_vehicleModel, *feature, nullptr /* altitudes */, m_attrsGetter);
|
||||
}
|
||||
|
||||
private:
|
||||
FeaturesVectorTest m_featuresVector;
|
||||
RoadAttrsGetter m_attrsGetter;
|
||||
VehicleModelPtrT m_vehicleModel;
|
||||
};
|
||||
} // namespace
|
||||
|
||||
// RoadGeometry ------------------------------------------------------------------------------------
|
||||
RoadGeometry::RoadGeometry(bool oneWay, double weightSpeedKMpH, double etaSpeedKMpH, Points const & points)
|
||||
: m_forwardSpeed{weightSpeedKMpH, etaSpeedKMpH}
|
||||
, m_backwardSpeed(m_forwardSpeed)
|
||||
, m_isOneWay(oneWay)
|
||||
, m_valid(true)
|
||||
, m_isPassThroughAllowed(false)
|
||||
, m_inCity(false)
|
||||
{
|
||||
ASSERT_GREATER(weightSpeedKMpH, 0.0, ());
|
||||
ASSERT_GREATER(etaSpeedKMpH, 0.0, ());
|
||||
|
||||
size_t const count = points.size();
|
||||
ASSERT_GREATER(count, 1, ());
|
||||
|
||||
m_junctions.reserve(count);
|
||||
for (auto const & point : points)
|
||||
m_junctions.emplace_back(mercator::ToLatLon(point), geometry::kDefaultAltitudeMeters);
|
||||
|
||||
m_distances.resize(count - 1, -1);
|
||||
}
|
||||
|
||||
void RoadGeometry::Load(VehicleModelInterface const & vehicleModel, FeatureType & feature,
|
||||
geometry::Altitudes const * altitudes, RoadAttrsGetter & attrs)
|
||||
{
|
||||
size_t const count = feature.GetPointsCount();
|
||||
CHECK_GREATER(count, 1, ());
|
||||
CHECK(altitudes == nullptr || altitudes->size() == count, ());
|
||||
|
||||
feature::TypesHolder types(feature);
|
||||
m_highwayType = vehicleModel.GetHighwayType(types);
|
||||
|
||||
m_valid = vehicleModel.IsRoad(types);
|
||||
m_isOneWay = vehicleModel.IsOneWay(types);
|
||||
m_isPassThroughAllowed = vehicleModel.IsPassThroughAllowed(types);
|
||||
|
||||
uint32_t const fID = feature.GetID().m_index;
|
||||
m_inCity = attrs.m_cityRoads.IsCityRoad(fID);
|
||||
|
||||
SpeedParams params(attrs.m_maxSpeeds.GetMaxspeed(fID),
|
||||
m_highwayType ? attrs.m_maxSpeeds.GetDefaultSpeed(m_inCity, *m_highwayType) : kInvalidSpeed,
|
||||
m_inCity);
|
||||
params.m_forward = true;
|
||||
m_forwardSpeed = vehicleModel.GetSpeed(types, params);
|
||||
params.m_forward = false;
|
||||
m_backwardSpeed = vehicleModel.GetSpeed(types, params);
|
||||
|
||||
auto const & optionsClassfier = RoutingOptionsClassifier::Instance();
|
||||
for (uint32_t type : types)
|
||||
if (auto const it = optionsClassfier.Get(type))
|
||||
m_routingOptions.Add(*it);
|
||||
|
||||
m_junctions.clear();
|
||||
m_junctions.reserve(count);
|
||||
for (size_t i = 0; i < count; ++i)
|
||||
{
|
||||
auto const ll = mercator::ToLatLon(feature.GetPoint(i));
|
||||
m_junctions.emplace_back(ll, altitudes ? (*altitudes)[i] : geometry::kDefaultAltitudeMeters);
|
||||
|
||||
#ifdef DEBUG
|
||||
// I'd like to check these big jumps manually, if any.
|
||||
if (altitudes && i > 0)
|
||||
{
|
||||
// Since we store integer altitudes, 1 is a possible error for 2 points.
|
||||
geometry::Altitude constexpr kError = 1;
|
||||
|
||||
auto const altDiff = (*altitudes)[i] - (*altitudes)[i - 1];
|
||||
auto const absDiff = abs(altDiff) - kError;
|
||||
if (absDiff > 0)
|
||||
{
|
||||
double const dist = ms::DistanceOnEarth(m_junctions[i - 1].GetLatLon(), m_junctions[i].GetLatLon());
|
||||
if (absDiff / dist >= 1.0)
|
||||
LOG(LWARNING, ("Altitudes jump:", altDiff, "/", dist, m_junctions[i - 1], m_junctions[i]));
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
m_distances.resize(count - 1, -1);
|
||||
|
||||
bool const isFerry = m_routingOptions.Has(RoutingOptions::Road::Ferry);
|
||||
/// @todo Add RouteShuttleTrain into RoutingOptions?
|
||||
if (isFerry || (m_highwayType && *m_highwayType == HighwayType::RouteShuttleTrain))
|
||||
{
|
||||
// Skip shuttle train calculation without duration.
|
||||
auto const durationMeta = feature.GetMetadata(feature::Metadata::FMD_DURATION);
|
||||
if (isFerry || !durationMeta.empty())
|
||||
{
|
||||
/// @todo Also process "interval" OSM tag (without additional boarding penalties).
|
||||
// https://github.com/organicmaps/organicmaps/issues/3695
|
||||
|
||||
auto const roadLenKm = GetRoadLengthM() / 1000.0;
|
||||
double const durationH = CalcFerryDurationHours(durationMeta, roadLenKm);
|
||||
CHECK(!AlmostEqualAbs(durationH, 0.0, 1e-5), (durationH));
|
||||
|
||||
if (roadLenKm != 0.0)
|
||||
{
|
||||
double const speed = roadLenKm / durationH;
|
||||
ASSERT_LESS_OR_EQUAL(speed, vehicleModel.GetMaxWeightSpeed(), (roadLenKm, durationH, fID));
|
||||
m_forwardSpeed = m_backwardSpeed = SpeedKMpH(speed);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (m_valid)
|
||||
ASSERT(m_forwardSpeed.IsValid() && m_backwardSpeed.IsValid(), (feature.DebugString()));
|
||||
}
|
||||
|
||||
double RoadGeometry::GetDistance(uint32_t idx) const
|
||||
{
|
||||
if (m_distances[idx] < 0)
|
||||
m_distances[idx] = ms::DistanceOnEarth(m_junctions[idx].GetLatLon(), m_junctions[idx + 1].GetLatLon());
|
||||
return m_distances[idx];
|
||||
}
|
||||
|
||||
SpeedKMpH const & RoadGeometry::GetSpeed(bool forward) const
|
||||
{
|
||||
return forward ? m_forwardSpeed : m_backwardSpeed;
|
||||
}
|
||||
|
||||
double RoadGeometry::GetRoadLengthM() const
|
||||
{
|
||||
uint32_t const count = GetPointsCount();
|
||||
double lenM = 0.0;
|
||||
|
||||
if (count > 0)
|
||||
for (uint32_t i = 0; i < count - 1; ++i)
|
||||
lenM += GetDistance(i);
|
||||
|
||||
return lenM;
|
||||
}
|
||||
|
||||
// Geometry ----------------------------------------------------------------------------------------
|
||||
Geometry::Geometry(unique_ptr<GeometryLoader> loader, size_t roadsCacheSize) : m_loader(std::move(loader))
|
||||
{
|
||||
CHECK(m_loader, ());
|
||||
|
||||
m_featureIdToRoad = make_unique<RoutingCacheT>(
|
||||
roadsCacheSize, [this](uint32_t featureId, RoadGeometry & road) { m_loader->Load(featureId, road); });
|
||||
}
|
||||
|
||||
RoadGeometry const & Geometry::GetRoad(uint32_t featureId)
|
||||
{
|
||||
ASSERT(m_featureIdToRoad, ());
|
||||
ASSERT(m_loader, ());
|
||||
|
||||
return m_featureIdToRoad->GetValue(featureId);
|
||||
}
|
||||
|
||||
SpeedInUnits GeometryLoader::GetSavedMaxspeed(uint32_t featureId, bool forward)
|
||||
{
|
||||
UNREACHABLE();
|
||||
}
|
||||
|
||||
// static
|
||||
unique_ptr<GeometryLoader> GeometryLoader::Create(MwmSet::MwmHandle const & handle,
|
||||
VehicleModelPtrT const & vehicleModel, bool loadAltitudes)
|
||||
{
|
||||
CHECK(handle.IsAlive(), ());
|
||||
CHECK(vehicleModel, ());
|
||||
return make_unique<GeometryLoaderImpl>(handle, vehicleModel, loadAltitudes);
|
||||
}
|
||||
|
||||
// static
|
||||
unique_ptr<GeometryLoader> GeometryLoader::CreateFromFile(string const & fileName,
|
||||
VehicleModelPtrT const & vehicleModel)
|
||||
{
|
||||
CHECK(vehicleModel, ());
|
||||
return make_unique<FileGeometryLoader>(fileName, vehicleModel);
|
||||
}
|
||||
} // namespace routing
|
||||
156
libs/routing/geometry.hpp
Normal file
156
libs/routing/geometry.hpp
Normal file
|
|
@ -0,0 +1,156 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/latlon_with_altitude.hpp"
|
||||
#include "routing/road_point.hpp"
|
||||
#include "routing/routing_options.hpp"
|
||||
|
||||
#include "routing_common/vehicle_model.hpp"
|
||||
|
||||
#include "indexer/mwm_set.hpp"
|
||||
|
||||
#include "geometry/latlon.hpp"
|
||||
|
||||
#include "base/fifo_cache.hpp"
|
||||
|
||||
#include <memory>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
|
||||
#include "3party/skarupke/bytell_hash_map.hpp"
|
||||
|
||||
class DataSource;
|
||||
|
||||
namespace routing
|
||||
{
|
||||
// @TODO(bykoianko) Consider setting cache size based on available memory.
|
||||
// Maximum road geometry cache size in items.
|
||||
size_t constexpr kRoadsCacheSize = 10000;
|
||||
|
||||
class RoadAttrsGetter;
|
||||
|
||||
class RoadGeometry final
|
||||
{
|
||||
public:
|
||||
RoadGeometry() : m_isOneWay(false), m_valid(false), m_isPassThroughAllowed(false), m_inCity(false) {}
|
||||
|
||||
/// Used in tests.
|
||||
using Points = std::vector<m2::PointD>;
|
||||
RoadGeometry(bool oneWay, double weightSpeedKMpH, double etaSpeedKMpH, Points const & points);
|
||||
|
||||
/// @param[in] altitudes May be nullptr.
|
||||
void Load(VehicleModelInterface const & vehicleModel, FeatureType & feature, geometry::Altitudes const * altitudes,
|
||||
RoadAttrsGetter & attrs);
|
||||
|
||||
SpeedKMpH const & GetSpeed(bool forward) const;
|
||||
std::optional<HighwayType> GetHighwayType() const { return m_highwayType; }
|
||||
bool IsOneWay() const { return m_isOneWay; }
|
||||
bool IsPassThroughAllowed() const { return m_isPassThroughAllowed; }
|
||||
bool IsInCity() const { return m_inCity; }
|
||||
|
||||
LatLonWithAltitude const & GetJunction(uint32_t junctionId) const
|
||||
{
|
||||
ASSERT_LESS(junctionId, m_junctions.size(), ());
|
||||
return m_junctions[junctionId];
|
||||
}
|
||||
|
||||
double GetDistance(uint32_t segmendIdx) const;
|
||||
double GetRoadLengthM() const;
|
||||
|
||||
ms::LatLon const & GetPoint(uint32_t pointId) const { return GetJunction(pointId).GetLatLon(); }
|
||||
|
||||
uint32_t GetPointsCount() const { return static_cast<uint32_t>(m_junctions.size()); }
|
||||
|
||||
// Note. It's possible that car_model was changed after the map was built.
|
||||
// For example, the map from 12.2016 contained highway=pedestrian
|
||||
// in car_model but this type of highways is removed as of 01.2017.
|
||||
// In such cases RoadGeometry is not valid.
|
||||
bool IsValid() const { return m_valid; }
|
||||
|
||||
bool IsEndPointId(uint32_t pointId) const
|
||||
{
|
||||
ASSERT_LESS(pointId, m_junctions.size(), ());
|
||||
return pointId == 0 || pointId + 1 == GetPointsCount();
|
||||
}
|
||||
|
||||
void SetPassThroughAllowedForTests(bool passThroughAllowed) { m_isPassThroughAllowed = passThroughAllowed; }
|
||||
|
||||
bool SuitableForOptions(RoutingOptions avoidRoutingOptions) const
|
||||
{
|
||||
return (avoidRoutingOptions.GetOptions() & m_routingOptions.GetOptions()) == 0;
|
||||
}
|
||||
|
||||
RoutingOptions GetRoutingOptions() const { return m_routingOptions; }
|
||||
|
||||
private:
|
||||
std::vector<LatLonWithAltitude> m_junctions;
|
||||
mutable std::vector<double> m_distances; ///< as cache, @see GetDistance()
|
||||
|
||||
SpeedKMpH m_forwardSpeed;
|
||||
SpeedKMpH m_backwardSpeed;
|
||||
std::optional<HighwayType> m_highwayType;
|
||||
RoutingOptions m_routingOptions;
|
||||
bool m_isOneWay : 1;
|
||||
bool m_valid : 1;
|
||||
bool m_isPassThroughAllowed : 1;
|
||||
bool m_inCity : 1;
|
||||
};
|
||||
|
||||
class GeometryLoader
|
||||
{
|
||||
public:
|
||||
virtual ~GeometryLoader() = default;
|
||||
|
||||
virtual void Load(uint32_t featureId, RoadGeometry & road) = 0;
|
||||
|
||||
/// Used in client-app only for the final route preparation.
|
||||
virtual SpeedInUnits GetSavedMaxspeed(uint32_t featureId, bool forward);
|
||||
|
||||
using VehicleModelPtrT = std::shared_ptr<VehicleModelInterface>;
|
||||
|
||||
/// @param[in] handle should be alive, its caller responsibility to check it.
|
||||
static std::unique_ptr<GeometryLoader> Create(MwmSet::MwmHandle const & handle, VehicleModelPtrT const & vehicleModel,
|
||||
bool loadAltitudes);
|
||||
|
||||
/// This is for stand-alone work.
|
||||
/// Use in generator_tool and unit tests.
|
||||
static std::unique_ptr<GeometryLoader> CreateFromFile(std::string const & filePath,
|
||||
VehicleModelPtrT const & vehicleModel);
|
||||
};
|
||||
|
||||
/// \brief This class supports loading geometry of roads for routing.
|
||||
/// \note Loaded information about road geometry is kept in a fixed-size cache |m_featureIdToRoad|.
|
||||
/// On the other hand methods GetRoad() and GetPoint() return geometry information by reference.
|
||||
/// The reference may be invalid after the next call of GetRoad() or GetPoint() because the cache
|
||||
/// item which is referred by returned reference may be evicted. It's done for performance reasons.
|
||||
/// \note The cache |m_featureIdToRoad| is used for road geometry for single-directional
|
||||
/// and bidirectional A*. According to tests it's faster to use one cache for both directions
|
||||
/// in bidirectional A* case than two separate caches, one for each direction (one for each A* wave).
|
||||
class Geometry final
|
||||
{
|
||||
public:
|
||||
Geometry() = default;
|
||||
/// \brief Geometry constructor
|
||||
/// \param roadsCacheSize in-memory geometry elements count limit
|
||||
Geometry(std::unique_ptr<GeometryLoader> loader, size_t roadsCacheSize = kRoadsCacheSize);
|
||||
|
||||
/// \note The reference returned by the method is valid until the next call of GetRoad()
|
||||
/// of GetPoint() methods.
|
||||
RoadGeometry const & GetRoad(uint32_t featureId);
|
||||
|
||||
/// \note The reference returned by the method is valid until the next call of GetRoad()
|
||||
/// of GetPoint() methods.
|
||||
ms::LatLon const & GetPoint(RoadPoint const & rp) { return GetRoad(rp.GetFeatureId()).GetPoint(rp.GetPointId()); }
|
||||
|
||||
SpeedInUnits GetSavedMaxspeed(uint32_t featureId, bool forward)
|
||||
{
|
||||
return m_loader->GetSavedMaxspeed(featureId, forward);
|
||||
}
|
||||
|
||||
private:
|
||||
/// @todo Use LRU cache?
|
||||
using RoutingCacheT = FifoCache<uint32_t, RoadGeometry, ska::bytell_hash_map<uint32_t, RoadGeometry>>;
|
||||
|
||||
std::unique_ptr<GeometryLoader> m_loader;
|
||||
std::unique_ptr<RoutingCacheT> m_featureIdToRoad;
|
||||
};
|
||||
} // namespace routing
|
||||
302
libs/routing/guides_connections.cpp
Normal file
302
libs/routing/guides_connections.cpp
Normal file
|
|
@ -0,0 +1,302 @@
|
|||
#include "routing/guides_connections.hpp"
|
||||
|
||||
#include "geometry/mercator.hpp"
|
||||
#include "geometry/parametrized_segment.hpp"
|
||||
|
||||
namespace routing
|
||||
{
|
||||
namespace
|
||||
{
|
||||
// We consider only really close points to be attached to the track.
|
||||
double constexpr kMaxDistToTrackPointM = 50.0;
|
||||
|
||||
// For points further from track then |kEqDistToTrackPointM| we try to attach them to the OSM
|
||||
// graph.
|
||||
double constexpr kEqDistToTrackPointM = 20.0;
|
||||
|
||||
// It the checkpoint is further from the guides track than |kMaxDistToTrackForSkippingM|, we skip
|
||||
// this track completely. For time optimization purposes only.
|
||||
double constexpr kMaxDistToTrackForSkippingM = 100'000.0;
|
||||
} // namespace
|
||||
|
||||
CheckpointTrackProj::CheckpointTrackProj(kml::MarkGroupId guideId, size_t trackIdx, size_t trackPointIdx,
|
||||
geometry::PointWithAltitude const & projectedPoint,
|
||||
double distToProjectedPointM)
|
||||
: m_guideId(guideId)
|
||||
, m_trackIdx(trackIdx)
|
||||
, m_trackPointIdx(trackPointIdx)
|
||||
, m_projectedPoint(projectedPoint)
|
||||
, m_distToProjectedPointM(distToProjectedPointM)
|
||||
{}
|
||||
|
||||
std::pair<geometry::PointWithAltitude, double> GetProjectionAndDistOnSegment(
|
||||
m2::PointD const & point, geometry::PointWithAltitude const & startPath,
|
||||
geometry::PointWithAltitude const & endPath)
|
||||
{
|
||||
m2::PointD const projection =
|
||||
m2::ParametrizedSegment<m2::PointD>(startPath.GetPoint(), endPath.GetPoint()).ClosestPointTo(point);
|
||||
double const distM = mercator::DistanceOnEarth(projection, point);
|
||||
return std::make_pair(geometry::PointWithAltitude(projection, 0 /* altitude */), distM);
|
||||
}
|
||||
|
||||
bool GuidesConnections::IsActive() const
|
||||
{
|
||||
return !m_allTracks.empty();
|
||||
}
|
||||
|
||||
std::vector<ConnectionToOsm> GuidesConnections::GetOsmConnections(size_t checkpointIdx) const
|
||||
{
|
||||
auto it = m_connectionsToOsm.find(checkpointIdx);
|
||||
if (it == m_connectionsToOsm.end())
|
||||
return {};
|
||||
return it->second;
|
||||
}
|
||||
|
||||
void GuidesConnections::UpdateOsmConnections(size_t checkpointIdx, std::vector<ConnectionToOsm> const & links)
|
||||
{
|
||||
auto const it = m_connectionsToOsm.find(checkpointIdx);
|
||||
CHECK(it != m_connectionsToOsm.cend(), (checkpointIdx));
|
||||
it->second.clear();
|
||||
for (auto const & link : links)
|
||||
if (!link.m_fakeEnding.m_projections.empty())
|
||||
it->second.push_back(link);
|
||||
if (it->second.empty())
|
||||
m_connectionsToOsm.erase(it);
|
||||
}
|
||||
|
||||
GuidesConnections::GuidesConnections(GuidesTracks const & guides) : m_allTracks(guides) {}
|
||||
|
||||
void GuidesConnections::PullCheckpointsToTracks(std::vector<m2::PointD> const & checkpoints)
|
||||
{
|
||||
for (size_t checkpointIdx = 0; checkpointIdx < checkpoints.size(); ++checkpointIdx)
|
||||
{
|
||||
for (auto const & [guideId, tracks] : m_allTracks)
|
||||
{
|
||||
for (size_t trackIdx = 0; trackIdx < tracks.size(); ++trackIdx)
|
||||
{
|
||||
CHECK(!tracks[trackIdx].empty(), (trackIdx));
|
||||
for (size_t pointIdx = 0; pointIdx < tracks[trackIdx].size() - 1; ++pointIdx)
|
||||
{
|
||||
auto const [checkpointProj, distM] = GetProjectionAndDistOnSegment(
|
||||
checkpoints[checkpointIdx], tracks[trackIdx][pointIdx], tracks[trackIdx][pointIdx + 1]);
|
||||
|
||||
// Skip too far track.
|
||||
if (distM > kMaxDistToTrackForSkippingM)
|
||||
break;
|
||||
|
||||
// We consider only really close points to be attached to the track.
|
||||
if (distM > kMaxDistToTrackPointM)
|
||||
continue;
|
||||
|
||||
CheckpointTrackProj const curProj(guideId, trackIdx, pointIdx, checkpointProj, distM);
|
||||
auto const [it, inserted] = m_checkpointsOnTracks.emplace(checkpointIdx, curProj);
|
||||
|
||||
if (!inserted && it->second.m_distToProjectedPointM > distM)
|
||||
it->second = curProj;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void GuidesConnections::AddTerminalGuidePoint(size_t checkpointIdx, size_t neighbourIdx, m2::PointD const & curPoint)
|
||||
{
|
||||
auto const it = m_checkpointsOnTracks.find(neighbourIdx);
|
||||
CHECK(it != m_checkpointsOnTracks.cend(), (neighbourIdx));
|
||||
|
||||
auto const & neighbour = it->second;
|
||||
auto const guideId = neighbour.m_guideId;
|
||||
auto const trackIdx = neighbour.m_trackIdx;
|
||||
auto const & track = m_allTracks[guideId][trackIdx];
|
||||
|
||||
CHECK_GREATER(track.size(), 1,
|
||||
("checkpointIdx:", checkpointIdx, "neighbourIdx:", neighbourIdx, "trackIdx:", trackIdx));
|
||||
|
||||
// Connect start checkpoint to the starting point of the track.
|
||||
if (checkpointIdx == 0)
|
||||
{
|
||||
double const distToStartM = mercator::DistanceOnEarth(curPoint, track.front().GetPoint());
|
||||
m_checkpointsOnTracks[checkpointIdx] =
|
||||
CheckpointTrackProj(guideId, trackIdx, 0 /* trackPointIdx */, track.front() /* proj */, distToStartM);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
// Connect finish checkpoint to the finish point of the track.
|
||||
double const distToSFinishM = mercator::DistanceOnEarth(curPoint, track.back().GetPoint());
|
||||
m_checkpointsOnTracks[checkpointIdx] = CheckpointTrackProj(guideId, trackIdx, track.size() - 2 /* trackPointIdx */,
|
||||
track.back() /* proj */, distToSFinishM);
|
||||
}
|
||||
|
||||
bool GuidesConnections::IsCheckpointAttached(size_t checkpointIdx) const
|
||||
{
|
||||
return m_checkpointsOnTracks.find(checkpointIdx) != m_checkpointsOnTracks.end();
|
||||
}
|
||||
|
||||
std::vector<size_t> GetNeighbourIntermediatePoints(size_t checkpointIdx, size_t checkpointsCount)
|
||||
{
|
||||
CHECK_GREATER(checkpointsCount, 2, (checkpointsCount));
|
||||
std::vector<size_t> neighbours;
|
||||
// We add left intermediate point:
|
||||
if (checkpointIdx > 1)
|
||||
neighbours.push_back(checkpointIdx - 1);
|
||||
// We add right intermediate point:
|
||||
if (checkpointIdx < checkpointsCount - 2)
|
||||
neighbours.push_back(checkpointIdx + 1);
|
||||
return neighbours;
|
||||
}
|
||||
|
||||
bool GuidesConnections::FitsForDirectLinkToGuide(size_t checkpointIdx, size_t checkpointsCount) const
|
||||
{
|
||||
auto it = m_checkpointsOnTracks.find(checkpointIdx);
|
||||
CHECK(it != m_checkpointsOnTracks.end(), (checkpointIdx));
|
||||
bool const checkpointIsFar = it->second.m_distToProjectedPointM > kEqDistToTrackPointM;
|
||||
if (checkpointIsFar)
|
||||
return false;
|
||||
|
||||
// If checkpoint lies on the track but its neighbour intermediate checkpoint does not, we should
|
||||
// connect this checkpoint to the OSM graph.
|
||||
if (checkpointsCount <= 2)
|
||||
return true;
|
||||
|
||||
for (auto const neighbourIdx : GetNeighbourIntermediatePoints(checkpointIdx, checkpointsCount))
|
||||
if (m_checkpointsOnTracks.find(neighbourIdx) == m_checkpointsOnTracks.end())
|
||||
return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void GuidesConnections::PullAdditionalCheckpointsToTracks(std::vector<m2::PointD> const & checkpoints)
|
||||
{
|
||||
for (size_t i : {size_t(0), checkpoints.size() - 1})
|
||||
{
|
||||
// Skip already connected to the tracks checkpoints.
|
||||
if (IsCheckpointAttached(i))
|
||||
continue;
|
||||
|
||||
// Neighbour point of this terminal point should be on the track.
|
||||
size_t neighbourIdx = i == 0 ? i + 1 : i - 1;
|
||||
|
||||
if (!IsCheckpointAttached(neighbourIdx))
|
||||
continue;
|
||||
|
||||
AddTerminalGuidePoint(i, neighbourIdx, checkpoints[i]);
|
||||
}
|
||||
}
|
||||
|
||||
bool GuidesConnections::IsAttached() const
|
||||
{
|
||||
return !m_checkpointsFakeEndings.empty();
|
||||
}
|
||||
|
||||
FakeEnding GuidesConnections::GetFakeEnding(size_t checkpointIdx) const
|
||||
{
|
||||
if (IsAttached())
|
||||
{
|
||||
auto it = m_checkpointsFakeEndings.find(checkpointIdx);
|
||||
if (it != m_checkpointsFakeEndings.end())
|
||||
return it->second;
|
||||
}
|
||||
return FakeEnding();
|
||||
}
|
||||
|
||||
GuidesGraph const & GuidesConnections::GetGuidesGraph() const
|
||||
{
|
||||
return m_graph;
|
||||
}
|
||||
|
||||
void GuidesConnections::OverwriteFakeEnding(size_t checkpointIdx, FakeEnding const & newFakeEnding)
|
||||
{
|
||||
m_checkpointsFakeEndings[checkpointIdx] = newFakeEnding;
|
||||
}
|
||||
|
||||
// static
|
||||
void GuidesConnections::ExtendFakeEndingProjections(FakeEnding const & srcFakeEnding, FakeEnding & dstFakeEnding)
|
||||
{
|
||||
dstFakeEnding.m_originJunction = srcFakeEnding.m_originJunction;
|
||||
|
||||
for (auto const & proj : srcFakeEnding.m_projections)
|
||||
if (!base::IsExist(dstFakeEnding.m_projections, proj))
|
||||
dstFakeEnding.m_projections.push_back(proj);
|
||||
}
|
||||
|
||||
NumMwmId GuidesConnections::GetMwmId() const
|
||||
{
|
||||
return m_graph.GetMwmId();
|
||||
}
|
||||
|
||||
void GuidesConnections::SetGuidesGraphParams(NumMwmId mwmId, double maxSpeed)
|
||||
{
|
||||
m_graph = GuidesGraph(maxSpeed, mwmId);
|
||||
}
|
||||
|
||||
void GuidesConnections::ConnectToGuidesGraph(std::vector<m2::PointD> const & checkpoints)
|
||||
{
|
||||
PullCheckpointsToTracks(checkpoints);
|
||||
|
||||
PullAdditionalCheckpointsToTracks(checkpoints);
|
||||
|
||||
std::map<std::pair<kml::MarkGroupId, size_t>, Segment> addedTracks;
|
||||
|
||||
for (auto const & [checkpointIdx, proj] : m_checkpointsOnTracks)
|
||||
{
|
||||
auto const & checkpoint = checkpoints[checkpointIdx];
|
||||
auto const & checkpointProj = proj.m_projectedPoint;
|
||||
|
||||
Segment segmentOnTrack;
|
||||
auto const [it, insertedSegment] =
|
||||
addedTracks.emplace(std::make_pair(proj.m_guideId, proj.m_trackIdx), segmentOnTrack);
|
||||
if (insertedSegment)
|
||||
{
|
||||
CHECK(!m_allTracks[proj.m_guideId][proj.m_trackIdx].empty(),
|
||||
("checkpointIdx:", checkpointIdx, "guideId:", proj.m_guideId, "trackIdx:", proj.m_trackIdx));
|
||||
|
||||
segmentOnTrack = m_graph.AddTrack(m_allTracks[proj.m_guideId][proj.m_trackIdx], proj.m_trackPointIdx);
|
||||
it->second = segmentOnTrack;
|
||||
}
|
||||
else
|
||||
{
|
||||
segmentOnTrack = m_graph.FindSegment(it->second, proj.m_trackPointIdx);
|
||||
}
|
||||
FakeEnding const newEnding = m_graph.MakeFakeEnding(segmentOnTrack, checkpoint, checkpointProj);
|
||||
auto const [itEnding, inserted] = m_checkpointsFakeEndings.emplace(checkpointIdx, newEnding);
|
||||
|
||||
CHECK(inserted, (checkpointIdx));
|
||||
bool const fitsForOsmLink = !FitsForDirectLinkToGuide(checkpointIdx, checkpoints.size());
|
||||
|
||||
if (fitsForOsmLink)
|
||||
AddConnectionToOsm(checkpointIdx, segmentOnTrack, checkpointProj, true /* fromCheckpoint */);
|
||||
|
||||
// Connect to OSM start and finish points of the track.
|
||||
auto const & firstPointOnTrack = m_allTracks[proj.m_guideId][proj.m_trackIdx][0];
|
||||
if (!(fitsForOsmLink && firstPointOnTrack == checkpointProj))
|
||||
{
|
||||
auto const & firstSegmentOnTrack = m_graph.FindSegment(segmentOnTrack, 0);
|
||||
AddConnectionToOsm(checkpointIdx, firstSegmentOnTrack, firstPointOnTrack, false /* fromCheckpoint */);
|
||||
}
|
||||
|
||||
auto const & lastPointIdx = m_allTracks[proj.m_guideId][proj.m_trackIdx].size() - 1;
|
||||
auto const & lastPointOnTrack = m_allTracks[proj.m_guideId][proj.m_trackIdx][lastPointIdx];
|
||||
if (!(fitsForOsmLink && lastPointOnTrack == checkpointProj))
|
||||
{
|
||||
auto const & lastSegmentOnTrack = m_graph.FindSegment(segmentOnTrack, lastPointIdx - 1);
|
||||
AddConnectionToOsm(checkpointIdx, lastSegmentOnTrack, lastPointOnTrack, false /* fromCheckpoint */);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void GuidesConnections::AddConnectionToOsm(size_t checkpointIdx, Segment const & real,
|
||||
geometry::PointWithAltitude const & loop, bool fromCheckpoint)
|
||||
{
|
||||
LatLonWithAltitude const loopPoint(mercator::ToLatLon(loop.GetPoint()), loop.GetAltitude());
|
||||
|
||||
ConnectionToOsm link;
|
||||
link.m_loopVertex = FakeVertex(kFakeNumMwmId, loopPoint, loopPoint, FakeVertex::Type::PureFake);
|
||||
link.m_realSegment = real;
|
||||
link.m_projectedPoint = loop;
|
||||
link.m_fromCheckpoint = fromCheckpoint;
|
||||
std::tie(link.m_realFrom, link.m_realTo) = m_graph.GetFromTo(real);
|
||||
|
||||
m_connectionsToOsm[checkpointIdx].push_back(link);
|
||||
}
|
||||
} // namespace routing
|
||||
132
libs/routing/guides_connections.hpp
Normal file
132
libs/routing/guides_connections.hpp
Normal file
|
|
@ -0,0 +1,132 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/edge_estimator.hpp"
|
||||
#include "routing/fake_ending.hpp"
|
||||
#include "routing/fake_vertex.hpp"
|
||||
#include "routing/guides_graph.hpp"
|
||||
#include "routing/router.hpp"
|
||||
#include "routing/segment.hpp"
|
||||
|
||||
#include "kml/type_utils.hpp"
|
||||
|
||||
#include "geometry/point2d.hpp"
|
||||
#include "geometry/point_with_altitude.hpp"
|
||||
|
||||
#include <cstdint>
|
||||
#include <map>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
// Information needed to attach guide track to the OSM segments via fake edges.
|
||||
struct ConnectionToOsm
|
||||
{
|
||||
// Fake ending connecting |m_projectedPoint| on the tracks segment |m_realSegment| to OSM.
|
||||
FakeEnding m_fakeEnding;
|
||||
// Loop vertex for |m_projectedPoint|.
|
||||
FakeVertex m_loopVertex;
|
||||
// Segment on the track to which the |m_loopVertex| should be attached via |m_partsOfReal|.
|
||||
Segment m_realSegment;
|
||||
// Terminal points of |m_realSegment|.
|
||||
LatLonWithAltitude m_realFrom;
|
||||
LatLonWithAltitude m_realTo;
|
||||
// Vertexes and corresponding PartOfReal segments connecting |m_loopVertex| and |m_realSegment|.
|
||||
std::vector<std::pair<FakeVertex, Segment>> m_partsOfReal;
|
||||
// Projection of the checkpoint to the track.
|
||||
geometry::PointWithAltitude m_projectedPoint;
|
||||
bool m_fromCheckpoint = false;
|
||||
};
|
||||
|
||||
// Information about checkpoint projection to the nearest guides track.
|
||||
struct CheckpointTrackProj
|
||||
{
|
||||
CheckpointTrackProj() = default;
|
||||
CheckpointTrackProj(kml::MarkGroupId guideId, size_t trackIdx, size_t trackPointIdx,
|
||||
geometry::PointWithAltitude const & projectedPoint, double distToProjectedPointM);
|
||||
// Guide id.
|
||||
kml::MarkGroupId m_guideId = 0;
|
||||
// Index of the track belonging to |m_guideId|.
|
||||
size_t m_trackIdx = 0;
|
||||
// Index of the nearest segment 'from' or 'to' point on the track to the checkpoint.
|
||||
size_t m_trackPointIdx = 0;
|
||||
// Projection of the checkpoint to the track.
|
||||
geometry::PointWithAltitude m_projectedPoint;
|
||||
// Distance between the checkpoint and |m_projectedPoint|
|
||||
double m_distToProjectedPointM = 0;
|
||||
};
|
||||
|
||||
using Projections = std::map<size_t, CheckpointTrackProj>;
|
||||
using IterProjections = Projections::iterator;
|
||||
|
||||
// Prepares guides tracks for attaching to the roads graph.
|
||||
class GuidesConnections
|
||||
{
|
||||
public:
|
||||
GuidesConnections() = default;
|
||||
GuidesConnections(GuidesTracks const & guides);
|
||||
|
||||
// Sets mwm id and speed values for guides graph.
|
||||
void SetGuidesGraphParams(NumMwmId mwmId, double maxSpeed);
|
||||
|
||||
// Finds closest guides tracks for checkpoints, fills guides graph.
|
||||
void ConnectToGuidesGraph(std::vector<m2::PointD> const & checkpoints);
|
||||
|
||||
// Overwrites osm connections for checkpoint by its index |checkpointIdx|.
|
||||
void UpdateOsmConnections(size_t checkpointIdx, std::vector<ConnectionToOsm> const & links);
|
||||
|
||||
// Set |newFakeEnding| for checkpoint.
|
||||
void OverwriteFakeEnding(size_t checkpointIdx, FakeEnding const & newFakeEnding);
|
||||
|
||||
// Merge existing |srcFakeEnding| with |dstFakeEnding|.
|
||||
static void ExtendFakeEndingProjections(FakeEnding const & srcFakeEnding, FakeEnding & dstFakeEnding);
|
||||
|
||||
// Returns guides graph |m_graph| for index graph starter.
|
||||
GuidesGraph const & GetGuidesGraph() const;
|
||||
|
||||
// Returns all connections to the OSM graph relevant to checkpoint with |checkpointIdx| index.
|
||||
std::vector<ConnectionToOsm> GetOsmConnections(size_t checkpointIdx) const;
|
||||
|
||||
// Checks if checkpoint is close enough to some track.
|
||||
bool FitsForDirectLinkToGuide(size_t checkpointIdx, size_t checkpointsCount) const;
|
||||
|
||||
// Checks if checkpoint is considered to be attached to some track.
|
||||
bool IsCheckpointAttached(size_t checkpointIdx) const;
|
||||
|
||||
// Checks if GuidesConnections instance is active.
|
||||
bool IsActive() const;
|
||||
|
||||
// Checks if some guides tracks in GuidesConnections instance are attached to the checkpoints.
|
||||
bool IsAttached() const;
|
||||
|
||||
// Returns mwm id linked to all tracks in the guides graph.
|
||||
NumMwmId GetMwmId() const;
|
||||
|
||||
// Returns fake ending associated with checkpoint by its index |checkpointIdx|.
|
||||
FakeEnding GetFakeEnding(size_t checkpointIdx) const;
|
||||
|
||||
private:
|
||||
// Fills |ConnectionToOsm| for checkpoints for its further attachment to the roads graph.
|
||||
void AddConnectionToOsm(size_t checkpointIdx, Segment const & real, geometry::PointWithAltitude const & loop,
|
||||
bool fromCheckpoint);
|
||||
|
||||
// Attaches checkpoints to the nearest guides tracks if possible.
|
||||
void PullCheckpointsToTracks(std::vector<m2::PointD> const & checkpoints);
|
||||
|
||||
// Attaches terminal point on the track to the terminal checkpoint: start to start;
|
||||
// finish - to finish.
|
||||
void AddTerminalGuidePoint(size_t checkpointIdx, size_t neighbourIdx, m2::PointD const & curPoint);
|
||||
|
||||
// Attaches neighbour terminal checkpoints for those which are already attached.
|
||||
void PullAdditionalCheckpointsToTracks(std::vector<m2::PointD> const & checkpoints);
|
||||
|
||||
GuidesTracks m_allTracks;
|
||||
Projections m_checkpointsOnTracks;
|
||||
|
||||
// Maps with checkpoint indexes as keys.
|
||||
std::map<size_t, FakeEnding> m_checkpointsFakeEndings;
|
||||
std::map<size_t, std::vector<ConnectionToOsm>> m_connectionsToOsm;
|
||||
|
||||
GuidesGraph m_graph;
|
||||
};
|
||||
} // namespace routing
|
||||
156
libs/routing/guides_graph.cpp
Normal file
156
libs/routing/guides_graph.cpp
Normal file
|
|
@ -0,0 +1,156 @@
|
|||
#include "routing/guides_graph.hpp"
|
||||
|
||||
#include "routing/fake_feature_ids.hpp"
|
||||
|
||||
#include "geometry/distance_on_sphere.hpp"
|
||||
#include "geometry/mercator.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <limits>
|
||||
#include <tuple>
|
||||
|
||||
namespace
|
||||
{
|
||||
double constexpr kZeroAltitude = 0.0;
|
||||
} // namespace
|
||||
|
||||
namespace routing
|
||||
{
|
||||
bool GuideSegmentCompare::operator()(Segment const & lhs, Segment const & rhs) const
|
||||
{
|
||||
if (lhs.GetFeatureId() != rhs.GetFeatureId())
|
||||
return lhs.GetFeatureId() < rhs.GetFeatureId();
|
||||
|
||||
return lhs.GetSegmentIdx() < rhs.GetSegmentIdx();
|
||||
}
|
||||
|
||||
GuidesGraph::GuidesGraph(double maxSpeedMpS, NumMwmId mwmId)
|
||||
: m_maxSpeedMpS(maxSpeedMpS)
|
||||
, m_mwmId(mwmId)
|
||||
, m_curGuidesFeatrueId(FakeFeatureIds::kGuidesGraphFeaturesStart)
|
||||
{
|
||||
CHECK_NOT_EQUAL(m_maxSpeedMpS, 0.0, ());
|
||||
}
|
||||
|
||||
double GuidesGraph::CalcSegmentTimeS(LatLonWithAltitude const & point1, LatLonWithAltitude const & point2) const
|
||||
{
|
||||
double const distM = ms::DistanceOnEarth(point1.GetLatLon(), point2.GetLatLon());
|
||||
double const weight = distM / m_maxSpeedMpS;
|
||||
return weight;
|
||||
}
|
||||
|
||||
void GuidesGraph::GetEdgeList(Segment const & segment, bool isOutgoing, EdgeListT & edges,
|
||||
RouteWeight const & prevWeight) const
|
||||
{
|
||||
auto const it = m_segments.find(segment);
|
||||
CHECK(it != m_segments.end(), (segment));
|
||||
auto const & segmentOnTrack = it->first;
|
||||
|
||||
IterOnTrack itOnTrack;
|
||||
|
||||
if (segment.IsForward() == isOutgoing)
|
||||
{
|
||||
Segment nextSegment(segmentOnTrack.GetMwmId(), segmentOnTrack.GetFeatureId(), segmentOnTrack.GetSegmentIdx() + 1,
|
||||
segmentOnTrack.IsForward());
|
||||
itOnTrack = m_segments.find(nextSegment);
|
||||
if (itOnTrack == m_segments.end())
|
||||
return;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (it->first.GetSegmentIdx() == 0)
|
||||
return;
|
||||
|
||||
Segment prevSegment(segmentOnTrack.GetMwmId(), segmentOnTrack.GetFeatureId(), segmentOnTrack.GetSegmentIdx() - 1,
|
||||
segmentOnTrack.IsForward());
|
||||
itOnTrack = m_segments.find(prevSegment);
|
||||
CHECK(itOnTrack != m_segments.end(), (segment));
|
||||
}
|
||||
auto const & neighbour = itOnTrack->first;
|
||||
Segment const resSegment(neighbour.GetMwmId(), neighbour.GetFeatureId(), neighbour.GetSegmentIdx(),
|
||||
segment.IsForward());
|
||||
|
||||
auto const weight = isOutgoing ? RouteWeight(itOnTrack->second.m_weight) : prevWeight;
|
||||
edges.emplace_back(resSegment, weight);
|
||||
}
|
||||
|
||||
LatLonWithAltitude const & GuidesGraph::GetJunction(Segment const & segment, bool front) const
|
||||
{
|
||||
auto const it = m_segments.find(segment);
|
||||
CHECK(it != m_segments.end(), (segment));
|
||||
return segment.IsForward() == front ? it->second.m_pointLast : it->second.m_pointFirst;
|
||||
}
|
||||
|
||||
NumMwmId GuidesGraph::GetMwmId() const
|
||||
{
|
||||
return m_mwmId;
|
||||
}
|
||||
|
||||
Segment GuidesGraph::AddTrack(std::vector<geometry::PointWithAltitude> const & guideTrack, size_t requiredSegmentIdx)
|
||||
{
|
||||
uint32_t segmentIdx = 0;
|
||||
Segment segment;
|
||||
|
||||
for (size_t i = 0; i < guideTrack.size() - 1; ++i)
|
||||
{
|
||||
GuideSegmentData data;
|
||||
Segment curSegment(m_mwmId, m_curGuidesFeatrueId, segmentIdx++, true /* forward */);
|
||||
if (i == requiredSegmentIdx)
|
||||
segment = curSegment;
|
||||
|
||||
data.m_pointFirst = LatLonWithAltitude(mercator::ToLatLon(guideTrack[i].GetPoint()), guideTrack[i].GetAltitude());
|
||||
data.m_pointLast =
|
||||
LatLonWithAltitude(mercator::ToLatLon(guideTrack[i + 1].GetPoint()), guideTrack[i + 1].GetAltitude());
|
||||
|
||||
data.m_weight = CalcSegmentTimeS(data.m_pointFirst, data.m_pointLast);
|
||||
|
||||
auto const [it, inserted] = m_segments.emplace(curSegment, data);
|
||||
CHECK(inserted, (curSegment));
|
||||
}
|
||||
++m_curGuidesFeatrueId;
|
||||
return segment;
|
||||
}
|
||||
|
||||
Segment GuidesGraph::FindSegment(Segment const & segment, size_t segmentIdx) const
|
||||
{
|
||||
auto const it = m_segments.find(segment);
|
||||
CHECK(it != m_segments.end(), (segment, segmentIdx));
|
||||
Segment segmentOnTrack(it->first.GetMwmId(), it->first.GetFeatureId(), static_cast<uint32_t>(segmentIdx),
|
||||
it->first.IsForward());
|
||||
|
||||
auto const itByIndex = m_segments.find(segmentOnTrack);
|
||||
CHECK(itByIndex != m_segments.end(), (segment, segmentIdx));
|
||||
return itByIndex->first;
|
||||
}
|
||||
|
||||
RouteWeight GuidesGraph::CalcSegmentWeight(Segment const & segment) const
|
||||
{
|
||||
auto const it = m_segments.find(segment);
|
||||
CHECK(it != m_segments.end(), (segment));
|
||||
|
||||
return RouteWeight(it->second.m_weight);
|
||||
}
|
||||
|
||||
FakeEnding GuidesGraph::MakeFakeEnding(Segment const & segment, m2::PointD const & point,
|
||||
geometry::PointWithAltitude const & projection) const
|
||||
{
|
||||
auto const & frontJunction = GetJunction(segment, true /* front */);
|
||||
auto const & backJunction = GetJunction(segment, false /* front */);
|
||||
|
||||
FakeEnding ending;
|
||||
ending.m_projections.emplace_back(
|
||||
segment, false /* isOneWay */, frontJunction, backJunction,
|
||||
LatLonWithAltitude(mercator::ToLatLon(projection.GetPoint()), projection.GetAltitude()) /* junction */);
|
||||
|
||||
ending.m_originJunction =
|
||||
LatLonWithAltitude(mercator::ToLatLon(point), static_cast<geometry::Altitude>(kZeroAltitude));
|
||||
return ending;
|
||||
}
|
||||
|
||||
std::pair<LatLonWithAltitude, LatLonWithAltitude> GuidesGraph::GetFromTo(Segment const & segment) const
|
||||
{
|
||||
auto const & from = GetJunction(segment, false /* front */);
|
||||
auto const & to = GetJunction(segment, true /* front */);
|
||||
return std::make_pair(from, to);
|
||||
}
|
||||
} // namespace routing
|
||||
69
libs/routing/guides_graph.hpp
Normal file
69
libs/routing/guides_graph.hpp
Normal file
|
|
@ -0,0 +1,69 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/fake_ending.hpp"
|
||||
#include "routing/latlon_with_altitude.hpp"
|
||||
#include "routing/segment.hpp"
|
||||
|
||||
#include "routing/base/small_list.hpp"
|
||||
|
||||
#include "routing_common/num_mwm_id.hpp"
|
||||
|
||||
#include "geometry/point2d.hpp"
|
||||
|
||||
#include <cstdint>
|
||||
#include <map>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
// Segment of the track in the guides graph.
|
||||
struct GuideSegmentData
|
||||
{
|
||||
LatLonWithAltitude m_pointFirst;
|
||||
LatLonWithAltitude m_pointLast;
|
||||
double m_weight = 0.0;
|
||||
};
|
||||
|
||||
// Custom comparison optimized for Guides segments: they all have the same |m_mwmId| and
|
||||
// |m_forward| is always true.
|
||||
struct GuideSegmentCompare
|
||||
{
|
||||
bool operator()(Segment const & lhs, Segment const & rhs) const;
|
||||
};
|
||||
|
||||
using GuideSegments = std::map<Segment, GuideSegmentData, GuideSegmentCompare>;
|
||||
using IterOnTrack = GuideSegments::const_iterator;
|
||||
|
||||
// Graph used in IndexGraphStarter for building routes through the guides tracks.
|
||||
class GuidesGraph
|
||||
{
|
||||
public:
|
||||
GuidesGraph() = default;
|
||||
explicit GuidesGraph(double maxSpeedMpS, NumMwmId mwmId);
|
||||
|
||||
Segment AddTrack(std::vector<geometry::PointWithAltitude> const & guideTrack, size_t requiredSegmentIdx);
|
||||
|
||||
FakeEnding MakeFakeEnding(Segment const & segment, m2::PointD const & point,
|
||||
geometry::PointWithAltitude const & projection) const;
|
||||
|
||||
using EdgeListT = SmallList<SegmentEdge>;
|
||||
void GetEdgeList(Segment const & segment, bool isOutgoing, EdgeListT & edges, RouteWeight const & prevWeight) const;
|
||||
routing::LatLonWithAltitude const & GetJunction(Segment const & segment, bool front) const;
|
||||
RouteWeight CalcSegmentWeight(Segment const & segment) const;
|
||||
Segment FindSegment(Segment const & segment, size_t segmentIdx) const;
|
||||
|
||||
// Returns back and front points of the segment. Segment belongs to one of the guides tracks.
|
||||
std::pair<LatLonWithAltitude, LatLonWithAltitude> GetFromTo(Segment const & segment) const;
|
||||
|
||||
NumMwmId GetMwmId() const;
|
||||
|
||||
private:
|
||||
double CalcSegmentTimeS(LatLonWithAltitude const & point1, LatLonWithAltitude const & point2) const;
|
||||
|
||||
GuideSegments m_segments;
|
||||
double m_maxSpeedMpS = 0.0;
|
||||
NumMwmId m_mwmId = 0;
|
||||
uint32_t m_curGuidesFeatrueId = 0;
|
||||
};
|
||||
} // namespace routing
|
||||
565
libs/routing/index_graph.cpp
Normal file
565
libs/routing/index_graph.cpp
Normal file
|
|
@ -0,0 +1,565 @@
|
|||
#include "routing/index_graph.hpp"
|
||||
|
||||
#include "routing/restrictions_serialization.hpp"
|
||||
#include "routing/routing_options.hpp"
|
||||
#include "routing/world_graph.hpp"
|
||||
|
||||
#include "platform/settings.hpp"
|
||||
|
||||
#include "base/assert.hpp"
|
||||
#include "base/checked_cast.hpp"
|
||||
#include "base/exception.hpp"
|
||||
#include "base/timer.hpp"
|
||||
|
||||
#include "geometry/distance_on_sphere.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <limits>
|
||||
#include <utility>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
using namespace base;
|
||||
using namespace std;
|
||||
|
||||
bool IsUTurn(Segment const & u, Segment const & v)
|
||||
{
|
||||
return u.GetFeatureId() == v.GetFeatureId() && u.GetSegmentIdx() == v.GetSegmentIdx() &&
|
||||
u.IsForward() != v.IsForward();
|
||||
}
|
||||
|
||||
bool IsBoarding(bool prevIsFerry, bool nextIsFerry)
|
||||
{
|
||||
return !prevIsFerry && nextIsFerry;
|
||||
}
|
||||
|
||||
IndexGraph::IndexGraph(shared_ptr<Geometry> geometry, shared_ptr<EdgeEstimator> estimator,
|
||||
RoutingOptions routingOptions, feature::RegionData const * regionData)
|
||||
: m_geometry(std::move(geometry))
|
||||
, m_estimator(std::move(estimator))
|
||||
, m_avoidRoutingOptions(routingOptions)
|
||||
{
|
||||
CHECK(m_geometry, ());
|
||||
CHECK(m_estimator, ());
|
||||
if (regionData)
|
||||
{
|
||||
auto const driving_side = regionData->Get(feature::RegionData::RD_DRIVING);
|
||||
m_isLeftHandTraffic = (driving_side == "l");
|
||||
}
|
||||
else
|
||||
{
|
||||
m_isLeftHandTraffic = false;
|
||||
}
|
||||
}
|
||||
|
||||
bool IndexGraph::IsJoint(RoadPoint const & roadPoint) const
|
||||
{
|
||||
return m_roadIndex.GetJointId(roadPoint) != Joint::kInvalidId;
|
||||
}
|
||||
|
||||
bool IndexGraph::IsJointOrEnd(Segment const & segment, bool fromStart) const
|
||||
{
|
||||
if (IsJoint(segment.GetRoadPoint(fromStart)))
|
||||
return true;
|
||||
|
||||
// For features, that ends out of mwm. In this case |m_graph.IsJoint| returns false, but we should
|
||||
// think, that it's Joint anyway.
|
||||
uint32_t const pointId = segment.GetPointId(fromStart);
|
||||
if (pointId == 0)
|
||||
return true;
|
||||
|
||||
uint32_t const pointsNumber = GetRoadGeometry(segment.GetFeatureId()).GetPointsCount();
|
||||
return pointId + 1 == pointsNumber;
|
||||
}
|
||||
|
||||
void IndexGraph::GetEdgeList(astar::VertexData<Segment, RouteWeight> const & vertexData, bool isOutgoing,
|
||||
bool useRoutingOptions, SegmentEdgeListT & edges, Parents<Segment> const & parents) const
|
||||
{
|
||||
GetEdgeListImpl(vertexData, isOutgoing, useRoutingOptions, true /* useAccessConditional */, edges, parents);
|
||||
}
|
||||
|
||||
void IndexGraph::GetEdgeList(Segment const & segment, bool isOutgoing, bool useRoutingOptions, SegmentEdgeListT & edges,
|
||||
Parents<Segment> const & parents) const
|
||||
{
|
||||
GetEdgeListImpl({segment, Weight(0.0)} /* vertexData */, isOutgoing, useRoutingOptions,
|
||||
false /* useAccessConditional */, edges, parents);
|
||||
}
|
||||
|
||||
void IndexGraph::GetEdgeListImpl(astar::VertexData<Segment, RouteWeight> const & vertexData, bool isOutgoing,
|
||||
bool useRoutingOptions, bool useAccessConditional, SegmentEdgeListT & edges,
|
||||
Parents<Segment> const & parents) const
|
||||
{
|
||||
auto const & segment = vertexData.m_vertex;
|
||||
|
||||
RoadPoint const roadPoint = segment.GetRoadPoint(isOutgoing);
|
||||
Joint::Id const jointId = m_roadIndex.GetJointId(roadPoint);
|
||||
|
||||
if (jointId != Joint::kInvalidId)
|
||||
{
|
||||
m_jointIndex.ForEachPoint(jointId, [&](RoadPoint const & rp)
|
||||
{ GetNeighboringEdges(vertexData, rp, isOutgoing, useRoutingOptions, edges, parents, useAccessConditional); });
|
||||
}
|
||||
else
|
||||
{
|
||||
GetNeighboringEdges(vertexData, roadPoint, isOutgoing, useRoutingOptions, edges, parents, useAccessConditional);
|
||||
}
|
||||
}
|
||||
|
||||
void IndexGraph::GetLastPointsForJoint(SegmentListT const & children, bool isOutgoing, PointIdListT & lastPoints) const
|
||||
{
|
||||
ASSERT(lastPoints.empty(), ());
|
||||
|
||||
lastPoints.reserve(children.size());
|
||||
for (auto const & child : children)
|
||||
{
|
||||
uint32_t const startPointId = child.GetPointId(!isOutgoing /* front */);
|
||||
uint32_t const pointsNumber = GetRoadGeometry(child.GetFeatureId()).GetPointsCount();
|
||||
CHECK_LESS(startPointId, pointsNumber, (child));
|
||||
|
||||
uint32_t endPointId;
|
||||
// child.IsForward() == isOutgoing
|
||||
// This is the direction, indicating the end of the road,
|
||||
// where we should go for building JointSegment.
|
||||
// You can retrieve such result if bust possible options of |child.IsForward()| and |isOutgoing|.
|
||||
bool forward = child.IsForward() == isOutgoing;
|
||||
if (IsRoad(child.GetFeatureId()))
|
||||
{
|
||||
endPointId = GetRoad(child.GetFeatureId()).FindNeighbor(startPointId, forward, pointsNumber).second;
|
||||
}
|
||||
else
|
||||
{
|
||||
// child.GetFeatureId() can be not road in this case:
|
||||
// -->--> { -->-->--> } -->
|
||||
// Where { ... } - borders of mwm
|
||||
// Here one feature, which enter from mwm A to mwm B and then exit from B to A.
|
||||
// And in mwm B no other feature cross this one, those feature is in geometry
|
||||
// and absent in roadIndex.
|
||||
// In this case we just return endPointNumber.
|
||||
endPointId = forward ? pointsNumber - 1 : 0;
|
||||
}
|
||||
|
||||
lastPoints.push_back(endPointId);
|
||||
}
|
||||
}
|
||||
|
||||
void IndexGraph::GetEdgeList(astar::VertexData<JointSegment, RouteWeight> const & parentVertexData,
|
||||
Segment const & parent, bool isOutgoing, JointEdgeListT & edges,
|
||||
WeightListT & parentWeights, Parents<JointSegment> const & parents) const
|
||||
{
|
||||
GetEdgeListImpl(parentVertexData, parent, isOutgoing, true /* useAccessConditional */, edges, parentWeights, parents);
|
||||
}
|
||||
|
||||
void IndexGraph::GetEdgeList(JointSegment const & parentJoint, Segment const & parent, bool isOutgoing,
|
||||
JointEdgeListT & edges, WeightListT & parentWeights,
|
||||
Parents<JointSegment> const & parents) const
|
||||
{
|
||||
GetEdgeListImpl({parentJoint, Weight(0.0)}, parent, isOutgoing, false /* useAccessConditional */, edges,
|
||||
parentWeights, parents);
|
||||
}
|
||||
|
||||
void IndexGraph::GetEdgeListImpl(astar::VertexData<JointSegment, RouteWeight> const & parentVertexData,
|
||||
Segment const & parent, bool isOutgoing, bool useAccessConditional,
|
||||
JointEdgeListT & edges, WeightListT & parentWeights,
|
||||
Parents<JointSegment> const & parents) const
|
||||
{
|
||||
SegmentListT possibleChildren;
|
||||
GetSegmentCandidateForJoint(parent, isOutgoing, possibleChildren);
|
||||
|
||||
PointIdListT lastPoints;
|
||||
GetLastPointsForJoint(possibleChildren, isOutgoing, lastPoints);
|
||||
|
||||
ReconstructJointSegment(parentVertexData, parent, possibleChildren, lastPoints, isOutgoing, edges, parentWeights,
|
||||
parents);
|
||||
}
|
||||
|
||||
optional<JointEdge> IndexGraph::GetJointEdgeByLastPoint(Segment const & parent, Segment const & firstChild,
|
||||
bool isOutgoing, uint32_t lastPoint) const
|
||||
{
|
||||
SegmentListT const possibleChildren = {firstChild};
|
||||
PointIdListT const lastPoints = {lastPoint};
|
||||
|
||||
JointEdgeListT edges;
|
||||
WeightListT parentWeights;
|
||||
Parents<JointSegment> emptyParents;
|
||||
ReconstructJointSegment(astar::VertexData<JointSegment, RouteWeight>::Zero(), parent, possibleChildren, lastPoints,
|
||||
isOutgoing, edges, parentWeights, emptyParents);
|
||||
|
||||
CHECK_LESS_OR_EQUAL(edges.size(), 1, ());
|
||||
if (edges.size() == 1)
|
||||
return {edges.back()};
|
||||
|
||||
return {};
|
||||
}
|
||||
|
||||
void IndexGraph::Build(uint32_t numJoints)
|
||||
{
|
||||
m_jointIndex.Build(m_roadIndex, numJoints);
|
||||
}
|
||||
|
||||
void IndexGraph::Import(vector<Joint> const & joints)
|
||||
{
|
||||
m_roadIndex.Import(joints);
|
||||
CHECK_LESS_OR_EQUAL(joints.size(), numeric_limits<uint32_t>::max(), ());
|
||||
Build(checked_cast<uint32_t>(joints.size()));
|
||||
}
|
||||
|
||||
void IndexGraph::SetRestrictions(RestrictionVec && restrictions)
|
||||
{
|
||||
m_restrictionsForward.clear();
|
||||
m_restrictionsBackward.clear();
|
||||
|
||||
base::HighResTimer timer;
|
||||
for (auto const & restriction : restrictions)
|
||||
{
|
||||
ASSERT(!restriction.empty(), ());
|
||||
|
||||
auto & forward = m_restrictionsForward[restriction.back()];
|
||||
forward.emplace_back(restriction.begin(), prev(restriction.end()));
|
||||
reverse(forward.back().begin(), forward.back().end());
|
||||
|
||||
m_restrictionsBackward[restriction.front()].emplace_back(next(restriction.begin()), restriction.end());
|
||||
}
|
||||
|
||||
LOG(LDEBUG, ("Restrictions are loaded in:", timer.ElapsedMilliseconds(), "ms"));
|
||||
}
|
||||
|
||||
void IndexGraph::SetUTurnRestrictions(vector<RestrictionUTurn> && noUTurnRestrictions)
|
||||
{
|
||||
for (auto const & noUTurn : noUTurnRestrictions)
|
||||
if (noUTurn.m_viaIsFirstPoint)
|
||||
m_noUTurnRestrictions[noUTurn.m_featureId].m_atTheBegin = true;
|
||||
else
|
||||
m_noUTurnRestrictions[noUTurn.m_featureId].m_atTheEnd = true;
|
||||
}
|
||||
|
||||
void IndexGraph::SetRoadAccess(RoadAccess && roadAccess)
|
||||
{
|
||||
m_roadAccess = std::move(roadAccess);
|
||||
m_roadAccess.SetCurrentTimeGetter(m_currentTimeGetter);
|
||||
}
|
||||
|
||||
void IndexGraph::SetRoadPenalty(RoadPenalty && roadPenalty)
|
||||
{
|
||||
m_roadPenalty = std::move(roadPenalty);
|
||||
}
|
||||
|
||||
void IndexGraph::GetNeighboringEdges(astar::VertexData<Segment, RouteWeight> const & fromVertexData,
|
||||
RoadPoint const & rp, bool isOutgoing, bool useRoutingOptions,
|
||||
SegmentEdgeListT & edges, Parents<Segment> const & parents,
|
||||
bool useAccessConditional) const
|
||||
{
|
||||
RoadGeometry const & road = GetRoadGeometry(rp.GetFeatureId());
|
||||
|
||||
if (!road.IsValid())
|
||||
return;
|
||||
|
||||
if (useRoutingOptions && !road.SuitableForOptions(m_avoidRoutingOptions))
|
||||
return;
|
||||
|
||||
bool const bidirectional = !road.IsOneWay();
|
||||
auto const & from = fromVertexData.m_vertex;
|
||||
if ((isOutgoing || bidirectional) && rp.GetPointId() + 1 < road.GetPointsCount())
|
||||
{
|
||||
GetNeighboringEdge(fromVertexData, Segment(from.GetMwmId(), rp.GetFeatureId(), rp.GetPointId(), isOutgoing),
|
||||
isOutgoing, edges, parents, useAccessConditional);
|
||||
}
|
||||
|
||||
if ((!isOutgoing || bidirectional) && rp.GetPointId() > 0)
|
||||
{
|
||||
GetNeighboringEdge(fromVertexData, Segment(from.GetMwmId(), rp.GetFeatureId(), rp.GetPointId() - 1, !isOutgoing),
|
||||
isOutgoing, edges, parents, useAccessConditional);
|
||||
}
|
||||
}
|
||||
|
||||
void IndexGraph::GetSegmentCandidateForRoadPoint(RoadPoint const & rp, NumMwmId numMwmId, bool isOutgoing,
|
||||
SegmentListT & children) const
|
||||
{
|
||||
RoadGeometry const & road = GetRoadGeometry(rp.GetFeatureId());
|
||||
if (!road.IsValid())
|
||||
return;
|
||||
|
||||
if (!road.SuitableForOptions(m_avoidRoutingOptions))
|
||||
return;
|
||||
|
||||
bool const bidirectional = !road.IsOneWay();
|
||||
auto const pointId = rp.GetPointId();
|
||||
|
||||
if ((isOutgoing || bidirectional) && pointId + 1 < road.GetPointsCount())
|
||||
children.emplace_back(numMwmId, rp.GetFeatureId(), pointId, isOutgoing);
|
||||
|
||||
if ((!isOutgoing || bidirectional) && pointId > 0)
|
||||
children.emplace_back(numMwmId, rp.GetFeatureId(), pointId - 1, !isOutgoing);
|
||||
}
|
||||
|
||||
void IndexGraph::GetSegmentCandidateForJoint(Segment const & parent, bool isOutgoing, SegmentListT & children) const
|
||||
{
|
||||
RoadPoint const roadPoint = parent.GetRoadPoint(isOutgoing);
|
||||
Joint::Id const jointId = m_roadIndex.GetJointId(roadPoint);
|
||||
|
||||
if (jointId == Joint::kInvalidId)
|
||||
return;
|
||||
|
||||
m_jointIndex.ForEachPoint(jointId, [&](RoadPoint const & rp)
|
||||
{ GetSegmentCandidateForRoadPoint(rp, parent.GetMwmId(), isOutgoing, children); });
|
||||
}
|
||||
|
||||
/// \brief Prolongs segments from |parent| to |firstChildren| directions in order to
|
||||
/// create JointSegments.
|
||||
/// \param |firstChildren| - vector of neighbouring segments from parent.
|
||||
/// \param |lastPointIds| - vector of the end numbers of road points for |firstChildren|.
|
||||
/// \param |jointEdges| - the result vector with JointEdges.
|
||||
/// \param |parentWeights| - see |IndexGraphStarterJoints::GetEdgeList| method about this argument.
|
||||
/// Shortly - in case of |isOutgoing| == false, method saves here the weights
|
||||
/// from parent to firstChildren.
|
||||
void IndexGraph::ReconstructJointSegment(astar::VertexData<JointSegment, RouteWeight> const & parentVertexData,
|
||||
Segment const & parent, SegmentListT const & firstChildren,
|
||||
PointIdListT const & lastPointIds, bool isOutgoing,
|
||||
JointEdgeListT & jointEdges, WeightListT & parentWeights,
|
||||
Parents<JointSegment> const & parents) const
|
||||
{
|
||||
CHECK_EQUAL(firstChildren.size(), lastPointIds.size(), ());
|
||||
|
||||
auto const & weightTimeToParent = parentVertexData.m_realDistance;
|
||||
auto const & parentJoint = parentVertexData.m_vertex;
|
||||
for (size_t i = 0; i < firstChildren.size(); ++i)
|
||||
{
|
||||
auto const & firstChild = firstChildren[i];
|
||||
auto const lastPointId = lastPointIds[i];
|
||||
|
||||
uint32_t currentPointId = firstChild.GetPointId(!isOutgoing /* front */);
|
||||
CHECK_NOT_EQUAL(currentPointId, lastPointId,
|
||||
("Invariant violated, can not build JointSegment,"
|
||||
"started and ended in the same point."));
|
||||
|
||||
auto const increment = [currentPointId, lastPointId](uint32_t pointId)
|
||||
{ return currentPointId < lastPointId ? pointId + 1 : pointId - 1; };
|
||||
|
||||
if (IsAccessNoForSure(firstChild.GetFeatureId(), weightTimeToParent, true /* useAccessConditional */))
|
||||
continue;
|
||||
|
||||
if (IsAccessNoForSure(parent.GetRoadPoint(isOutgoing), weightTimeToParent, true /* useAccessConditional */))
|
||||
continue;
|
||||
|
||||
if (IsUTurn(parent, firstChild) && IsUTurnAndRestricted(parent, firstChild, isOutgoing))
|
||||
continue;
|
||||
|
||||
if (IsRestricted(parentJoint, parent.GetFeatureId(), firstChild.GetFeatureId(), isOutgoing, parents))
|
||||
continue;
|
||||
|
||||
RouteWeight summaryWeight;
|
||||
// Check current JointSegment for bad road access between segments.
|
||||
RoadPoint rp = firstChild.GetRoadPoint(isOutgoing);
|
||||
uint32_t start = currentPointId;
|
||||
bool noRoadAccess = false;
|
||||
do
|
||||
{
|
||||
// This is optimization: we calculate accesses of road points before calculating weight of
|
||||
// segments between these road points. Because of that we make fewer calculations when some
|
||||
// points have RoadAccess::Type::No.
|
||||
// And using |weightTimeToParent| is not fair in fact, because we should calculate weight
|
||||
// until this |rp|. But we assume that segments have small length and inaccuracy will not
|
||||
// affect user.
|
||||
if (IsAccessNoForSure(rp, weightTimeToParent, true /* useAccessConditional */))
|
||||
{
|
||||
noRoadAccess = true;
|
||||
break;
|
||||
}
|
||||
|
||||
start = increment(start);
|
||||
rp.SetPointId(start);
|
||||
}
|
||||
while (start != lastPointId);
|
||||
|
||||
if (noRoadAccess)
|
||||
continue;
|
||||
|
||||
bool forward = currentPointId < lastPointId;
|
||||
Segment current = firstChild;
|
||||
Segment prev = parent;
|
||||
|
||||
do
|
||||
{
|
||||
RouteWeight const weight =
|
||||
CalculateEdgeWeight(EdgeEstimator::Purpose::Weight, isOutgoing, prev, current, weightTimeToParent);
|
||||
|
||||
if (isOutgoing || prev != parent)
|
||||
summaryWeight += weight;
|
||||
|
||||
if (prev == parent)
|
||||
parentWeights.emplace_back(weight);
|
||||
|
||||
prev = current;
|
||||
current.Next(forward);
|
||||
currentPointId = increment(currentPointId);
|
||||
}
|
||||
while (currentPointId != lastPointId);
|
||||
|
||||
jointEdges.emplace_back(isOutgoing ? JointSegment(firstChild, prev) : JointSegment(prev, firstChild),
|
||||
summaryWeight);
|
||||
}
|
||||
}
|
||||
|
||||
void IndexGraph::GetNeighboringEdge(astar::VertexData<Segment, RouteWeight> const & fromVertexData, Segment const & to,
|
||||
bool isOutgoing, SegmentEdgeListT & edges, Parents<Segment> const & parents,
|
||||
bool useAccessConditional) const
|
||||
{
|
||||
auto const & from = fromVertexData.m_vertex;
|
||||
auto const & weightToFrom = fromVertexData.m_realDistance;
|
||||
|
||||
if (IsUTurn(from, to) && IsUTurnAndRestricted(from, to, isOutgoing))
|
||||
return;
|
||||
|
||||
if (IsRestricted(from, from.GetFeatureId(), to.GetFeatureId(), isOutgoing, parents))
|
||||
return;
|
||||
|
||||
if (IsAccessNoForSure(to.GetFeatureId(), weightToFrom, useAccessConditional))
|
||||
return;
|
||||
|
||||
RoadPoint const rp = from.GetRoadPoint(isOutgoing);
|
||||
if (IsAccessNoForSure(rp, weightToFrom, useAccessConditional))
|
||||
return;
|
||||
|
||||
auto const weight = CalculateEdgeWeight(EdgeEstimator::Purpose::Weight, isOutgoing, from, to, weightToFrom);
|
||||
|
||||
edges.emplace_back(to, weight);
|
||||
}
|
||||
|
||||
IndexGraph::PenaltyData IndexGraph::GetRoadPenaltyData(Segment const & segment) const
|
||||
{
|
||||
auto const & road = GetRoadGeometry(segment.GetFeatureId());
|
||||
return {road.IsPassThroughAllowed(), road.GetRoutingOptions().Has(RoutingOptions::Road::Ferry)};
|
||||
}
|
||||
|
||||
RouteWeight IndexGraph::GetPenalties(EdgeEstimator::Purpose purpose, Segment const & u, Segment const & v,
|
||||
optional<RouteWeight> const & prevWeight) const
|
||||
{
|
||||
auto const & fromPenaltyData = GetRoadPenaltyData(u);
|
||||
auto const & toPenaltyData = GetRoadPenaltyData(v);
|
||||
// Route crosses border of pass-through/non-pass-through area if |u| and |v| have different
|
||||
// pass through restrictions.
|
||||
int8_t const passThroughPenalty = fromPenaltyData.m_passThroughAllowed == toPenaltyData.m_passThroughAllowed ? 0 : 1;
|
||||
|
||||
int8_t accessPenalty = 0;
|
||||
int8_t accessConditionalPenalties = 0;
|
||||
|
||||
if (u.GetFeatureId() != v.GetFeatureId())
|
||||
{
|
||||
// We do not distinguish between RoadAccess::Type::Private and RoadAccess::Type::Destination for
|
||||
// now.
|
||||
auto const [fromAccess, fromConfidence] = prevWeight ? m_roadAccess.GetAccess(u.GetFeatureId(), *prevWeight)
|
||||
: m_roadAccess.GetAccessWithoutConditional(u.GetFeatureId());
|
||||
|
||||
auto const [toAccess, toConfidence] = prevWeight ? m_roadAccess.GetAccess(v.GetFeatureId(), *prevWeight)
|
||||
: m_roadAccess.GetAccessWithoutConditional(v.GetFeatureId());
|
||||
|
||||
if (fromConfidence == RoadAccess::Confidence::Sure && toConfidence == RoadAccess::Confidence::Sure)
|
||||
{
|
||||
bool const fromAccessAllowed = fromAccess == RoadAccess::Type::Yes;
|
||||
bool const toAccessAllowed = toAccess == RoadAccess::Type::Yes;
|
||||
// Route crosses border of access=yes/access={private, destination} area if |u| and |v| have
|
||||
// different access restrictions.
|
||||
accessPenalty = fromAccessAllowed == toAccessAllowed ? 0 : 1;
|
||||
}
|
||||
else if (toConfidence == RoadAccess::Confidence::Maybe)
|
||||
{
|
||||
accessConditionalPenalties = 1;
|
||||
}
|
||||
}
|
||||
|
||||
// RoadPoint between u and v is front of u.
|
||||
auto const rp = u.GetRoadPoint(true /* front */);
|
||||
auto const [rpAccessType, rpConfidence] =
|
||||
prevWeight ? m_roadAccess.GetAccess(rp, *prevWeight) : m_roadAccess.GetAccessWithoutConditional(rp);
|
||||
// Get penalty from new penalty system if MWM supports it
|
||||
auto penalty = m_roadPenalty.GetPenalty(rp);
|
||||
uint16_t penaltyTime = 0;
|
||||
if (penalty)
|
||||
penaltyTime = penalty->m_timeSeconds;
|
||||
|
||||
switch (rpConfidence)
|
||||
{
|
||||
case RoadAccess::Confidence::Sure:
|
||||
{
|
||||
if (rpAccessType != RoadAccess::Type::Yes)
|
||||
accessPenalty = 1;
|
||||
break;
|
||||
}
|
||||
case RoadAccess::Confidence::Maybe:
|
||||
{
|
||||
accessConditionalPenalties = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
double weightPenalty = 0.0;
|
||||
if (IsUTurn(u, v))
|
||||
weightPenalty += m_estimator->GetUTurnPenalty(purpose);
|
||||
|
||||
if (IsBoarding(fromPenaltyData.m_isFerry, toPenaltyData.m_isFerry))
|
||||
weightPenalty += m_estimator->GetFerryLandingPenalty(purpose);
|
||||
|
||||
return {penaltyTime + weightPenalty /* weight */, passThroughPenalty, accessPenalty, accessConditionalPenalties,
|
||||
0.0 /* transitTime */};
|
||||
}
|
||||
|
||||
WorldGraphMode IndexGraph::GetMode() const
|
||||
{
|
||||
return WorldGraphMode::Undefined;
|
||||
}
|
||||
|
||||
bool IndexGraph::IsUTurnAndRestricted(Segment const & parent, Segment const & child, bool isOutgoing) const
|
||||
{
|
||||
ASSERT(IsUTurn(parent, child), ());
|
||||
|
||||
uint32_t const featureId = parent.GetFeatureId();
|
||||
uint32_t const turnPoint = parent.GetPointId(isOutgoing);
|
||||
auto const & roadGeometry = GetRoadGeometry(featureId);
|
||||
|
||||
RoadPoint const rp = parent.GetRoadPoint(isOutgoing);
|
||||
if (m_roadIndex.GetJointId(rp) == Joint::kInvalidId && !roadGeometry.IsEndPointId(turnPoint))
|
||||
return true;
|
||||
|
||||
auto const it = m_noUTurnRestrictions.find(featureId);
|
||||
if (it == m_noUTurnRestrictions.cend())
|
||||
return false;
|
||||
|
||||
auto const & uTurn = it->second;
|
||||
if (uTurn.m_atTheBegin && turnPoint == 0)
|
||||
return true;
|
||||
|
||||
uint32_t const n = roadGeometry.GetPointsCount();
|
||||
ASSERT_GREATER_OR_EQUAL(n, 1, ());
|
||||
return uTurn.m_atTheEnd && turnPoint == n - 1;
|
||||
}
|
||||
|
||||
RouteWeight IndexGraph::CalculateEdgeWeight(EdgeEstimator::Purpose purpose, bool isOutgoing, Segment const & from,
|
||||
Segment const & to,
|
||||
std::optional<RouteWeight const> const & prevWeight) const
|
||||
{
|
||||
auto const & to_segment = isOutgoing ? to : from;
|
||||
auto const & from_segment = isOutgoing ? from : to;
|
||||
auto const & to_road = GetRoadGeometry(to_segment.GetFeatureId());
|
||||
auto const weight = RouteWeight(m_estimator->CalcSegmentWeight(to_segment, to_road, purpose));
|
||||
auto const penalties = GetPenalties(purpose, isOutgoing ? from : to, isOutgoing ? to : from, prevWeight);
|
||||
auto const turn_penalty = getTurnPenalty(purpose, from_segment, to_segment);
|
||||
return weight + penalties + turn_penalty;
|
||||
}
|
||||
|
||||
RouteWeight IndexGraph::getTurnPenalty(EdgeEstimator::Purpose purpose, Segment const & from, Segment const & to) const
|
||||
{
|
||||
if (from.GetFeatureId() == to.GetFeatureId())
|
||||
return {0, 0, 0, 0, 0};
|
||||
auto const & to_road = GetRoadGeometry(to.GetFeatureId());
|
||||
auto const & from_road = GetRoadGeometry(from.GetFeatureId());
|
||||
auto v1 = ms::ToVector(GetPoint(from, true)) - ms::ToVector(GetPoint(from, false));
|
||||
auto v2 = ms::ToVector(GetPoint(to, true)) - ms::ToVector(GetPoint(to, false));
|
||||
auto const dotLen = v1.Length() * v2.Length();
|
||||
if (dotLen == 0)
|
||||
return {0, 0, 0, 0, 0};
|
||||
auto normal = ms::ToVector(GetPoint(from, true)) + ms::ToVector(GetPoint(to, false));
|
||||
normal = normal / normal.Length();
|
||||
auto const signed_angle =
|
||||
math::RadToDeg(atan2(m3::DotProduct(m3::CrossProduct(v1, v2), normal), m3::DotProduct(v1, v2)));
|
||||
return {m_estimator->GetTurnPenalty(purpose, signed_angle, from_road, to_road, m_isLeftHandTraffic), 0, 0, 0, 0};
|
||||
}
|
||||
} // namespace routing
|
||||
295
libs/routing/index_graph.hpp
Normal file
295
libs/routing/index_graph.hpp
Normal file
|
|
@ -0,0 +1,295 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/base/astar_graph.hpp"
|
||||
#include "routing/base/astar_vertex_data.hpp"
|
||||
|
||||
#include "routing/edge_estimator.hpp"
|
||||
#include "routing/geometry.hpp"
|
||||
#include "routing/joint.hpp"
|
||||
#include "routing/joint_index.hpp"
|
||||
#include "routing/joint_segment.hpp"
|
||||
#include "routing/restrictions_serialization.hpp"
|
||||
#include "routing/road_access.hpp"
|
||||
#include "routing/road_index.hpp"
|
||||
#include "routing/road_penalty.hpp"
|
||||
#include "routing/road_point.hpp"
|
||||
#include "routing/routing_options.hpp"
|
||||
#include "routing/segment.hpp"
|
||||
|
||||
#include "indexer/feature_meta.hpp"
|
||||
|
||||
#include "geometry/point2d.hpp"
|
||||
|
||||
#include <memory>
|
||||
#include <optional>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
bool IsUTurn(Segment const & u, Segment const & v);
|
||||
|
||||
enum class WorldGraphMode;
|
||||
|
||||
class IndexGraph final
|
||||
{
|
||||
public:
|
||||
// AStarAlgorithm types aliases:
|
||||
using Vertex = Segment;
|
||||
using Edge = SegmentEdge;
|
||||
using Weight = RouteWeight;
|
||||
|
||||
template <typename VertexType>
|
||||
using Parents = typename AStarGraph<VertexType, void, void>::Parents;
|
||||
|
||||
using Restrictions = std::unordered_map<uint32_t, std::vector<std::vector<uint32_t>>>;
|
||||
|
||||
using SegmentEdgeListT = SmallList<SegmentEdge>;
|
||||
using JointEdgeListT = SmallList<JointEdge>;
|
||||
using WeightListT = SmallList<RouteWeight>;
|
||||
using SegmentListT = SmallList<Segment>;
|
||||
using PointIdListT = SmallList<uint32_t>;
|
||||
|
||||
IndexGraph() = default;
|
||||
IndexGraph(std::shared_ptr<Geometry> geometry, std::shared_ptr<EdgeEstimator> estimator,
|
||||
RoutingOptions routingOptions = RoutingOptions(), feature::RegionData const * regionData = nullptr);
|
||||
|
||||
// Put outgoing (or ingoing) egdes for segment to the 'edges' vector.
|
||||
void GetEdgeList(astar::VertexData<Segment, RouteWeight> const & vertexData, bool isOutgoing, bool useRoutingOptions,
|
||||
SegmentEdgeListT & edges, Parents<Segment> const & parents = {}) const;
|
||||
void GetEdgeList(Segment const & segment, bool isOutgoing, bool useRoutingOptions, SegmentEdgeListT & edges,
|
||||
Parents<Segment> const & parents = {}) const;
|
||||
|
||||
void GetEdgeList(astar::VertexData<JointSegment, RouteWeight> const & parentVertexData, Segment const & parent,
|
||||
bool isOutgoing, JointEdgeListT & edges, WeightListT & parentWeights,
|
||||
Parents<JointSegment> const & parents) const;
|
||||
void GetEdgeList(JointSegment const & parentJoint, Segment const & parent, bool isOutgoing, JointEdgeListT & edges,
|
||||
WeightListT & parentWeights, Parents<JointSegment> const & parents) const;
|
||||
|
||||
std::optional<JointEdge> GetJointEdgeByLastPoint(Segment const & parent, Segment const & firstChild, bool isOutgoing,
|
||||
uint32_t lastPoint) const;
|
||||
|
||||
Joint::Id GetJointId(RoadPoint const & rp) const { return m_roadIndex.GetJointId(rp); }
|
||||
|
||||
bool IsRoad(uint32_t featureId) const { return m_roadIndex.IsRoad(featureId); }
|
||||
RoadJointIds const & GetRoad(uint32_t featureId) const { return m_roadIndex.GetRoad(featureId); }
|
||||
RoadGeometry const & GetRoadGeometry(uint32_t featureId) const { return m_geometry->GetRoad(featureId); }
|
||||
|
||||
Geometry & GetGeometry() const { return *m_geometry; }
|
||||
|
||||
RoadAccess::Type GetAccessType(Segment const & segment) const
|
||||
{
|
||||
return m_roadAccess.GetAccessWithoutConditional(segment.GetFeatureId()).first;
|
||||
}
|
||||
|
||||
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 Build(uint32_t numJoints);
|
||||
void Import(std::vector<Joint> const & joints);
|
||||
|
||||
void SetRestrictions(RestrictionVec && restrictions);
|
||||
void SetUTurnRestrictions(std::vector<RestrictionUTurn> && noUTurnRestrictions);
|
||||
void SetRoadAccess(RoadAccess && roadAccess);
|
||||
void SetRoadPenalty(RoadPenalty && roadPenalty);
|
||||
|
||||
void PushFromSerializer(Joint::Id jointId, RoadPoint const & rp) { m_roadIndex.PushFromSerializer(jointId, rp); }
|
||||
|
||||
template <typename F>
|
||||
void ForEachRoad(F && f) const
|
||||
{
|
||||
m_roadIndex.ForEachRoad(std::forward<F>(f));
|
||||
}
|
||||
|
||||
template <typename F>
|
||||
void ForEachPoint(Joint::Id jointId, F && f) const
|
||||
{
|
||||
m_jointIndex.ForEachPoint(jointId, std::forward<F>(f));
|
||||
}
|
||||
|
||||
bool IsJoint(RoadPoint const & roadPoint) const;
|
||||
bool IsJointOrEnd(Segment const & segment, bool fromStart) const;
|
||||
void GetLastPointsForJoint(SegmentListT const & children, bool isOutgoing, PointIdListT & lastPoints) const;
|
||||
|
||||
WorldGraphMode GetMode() const;
|
||||
ms::LatLon const & GetPoint(Segment const & segment, bool front) const
|
||||
{
|
||||
return GetRoadGeometry(segment.GetFeatureId()).GetPoint(segment.GetPointId(front));
|
||||
}
|
||||
|
||||
/// \brief Check, that we can go to |currentFeatureId|.
|
||||
/// We pass |parentFeatureId| and don't use |parent.GetFeatureId()| because
|
||||
/// |parent| can be fake sometimes but |parentFeatureId| is almost non-fake.
|
||||
template <typename ParentVertex>
|
||||
bool IsRestricted(ParentVertex const & parent, uint32_t parentFeatureId, uint32_t currentFeatureId, bool isOutgoing,
|
||||
Parents<ParentVertex> const & parents) const;
|
||||
|
||||
bool IsUTurnAndRestricted(Segment const & parent, Segment const & child, bool isOutgoing) const;
|
||||
|
||||
/// @param[in] isOutgoing true, when movig from -> to, false otherwise.
|
||||
/// @param[in] prevWeight used for fetching access:conditional.
|
||||
/// I suppose :) its time when user will be at the end of |from| (|to| if \a isOutgoing == false) segment.
|
||||
/// @return Transition weight + |to| (|from| if \a isOutgoing == false) segment's weight.
|
||||
RouteWeight CalculateEdgeWeight(EdgeEstimator::Purpose purpose, bool isOutgoing, Segment const & from,
|
||||
Segment const & to,
|
||||
std::optional<RouteWeight const> const & prevWeight = std::nullopt) const;
|
||||
|
||||
template <typename T>
|
||||
void SetCurrentTimeGetter(T && t)
|
||||
{
|
||||
m_currentTimeGetter = std::forward<T>(t);
|
||||
}
|
||||
|
||||
private:
|
||||
void GetEdgeListImpl(astar::VertexData<Segment, RouteWeight> const & vertexData, bool isOutgoing,
|
||||
bool useRoutingOptions, bool useAccessConditional, SegmentEdgeListT & edges,
|
||||
Parents<Segment> const & parents) const;
|
||||
|
||||
void GetEdgeListImpl(astar::VertexData<JointSegment, RouteWeight> const & parentVertexData, Segment const & parent,
|
||||
bool isOutgoing, bool useAccessConditional, JointEdgeListT & edges, WeightListT & parentWeights,
|
||||
Parents<JointSegment> const & parents) const;
|
||||
|
||||
void GetNeighboringEdges(astar::VertexData<Segment, RouteWeight> const & fromVertexData, RoadPoint const & rp,
|
||||
bool isOutgoing, bool useRoutingOptions, SegmentEdgeListT & edges,
|
||||
Parents<Segment> const & parents, bool useAccessConditional) const;
|
||||
void GetNeighboringEdge(astar::VertexData<Segment, RouteWeight> const & fromVertexData, Segment const & to,
|
||||
bool isOutgoing, SegmentEdgeListT & edges, Parents<Segment> const & parents,
|
||||
bool useAccessConditional) const;
|
||||
|
||||
struct PenaltyData
|
||||
{
|
||||
PenaltyData(bool passThroughAllowed, bool isFerry) : m_passThroughAllowed(passThroughAllowed), m_isFerry(isFerry) {}
|
||||
|
||||
bool m_passThroughAllowed;
|
||||
bool m_isFerry;
|
||||
};
|
||||
|
||||
PenaltyData GetRoadPenaltyData(Segment const & segment) const;
|
||||
|
||||
/// \brief Calculates penalties for moving from |u| to |v|.
|
||||
/// \param |prevWeight| uses for fetching access:conditional. In fact it is time when user
|
||||
/// will be at |u|. This time is based on start time of route building and weight of calculated
|
||||
/// path until |u|.
|
||||
RouteWeight GetPenalties(EdgeEstimator::Purpose purpose, Segment const & u, Segment const & v,
|
||||
std::optional<RouteWeight> const & prevWeight) const;
|
||||
|
||||
void GetSegmentCandidateForRoadPoint(RoadPoint const & rp, NumMwmId numMwmId, bool isOutgoing,
|
||||
SegmentListT & children) const;
|
||||
void GetSegmentCandidateForJoint(Segment const & parent, bool isOutgoing, SegmentListT & children) const;
|
||||
void ReconstructJointSegment(astar::VertexData<JointSegment, RouteWeight> const & parentVertexData,
|
||||
Segment const & parent, SegmentListT const & firstChildren,
|
||||
PointIdListT const & lastPointIds, bool isOutgoing, JointEdgeListT & jointEdges,
|
||||
WeightListT & parentWeights, Parents<JointSegment> const & parents) const;
|
||||
|
||||
template <typename AccessPositionType>
|
||||
bool IsAccessNoForSure(AccessPositionType const & accessPositionType, RouteWeight const & weight,
|
||||
bool useAccessConditional) const;
|
||||
|
||||
std::shared_ptr<Geometry> m_geometry;
|
||||
std::shared_ptr<EdgeEstimator> m_estimator;
|
||||
RoadIndex m_roadIndex;
|
||||
JointIndex m_jointIndex;
|
||||
Restrictions m_restrictionsForward;
|
||||
Restrictions m_restrictionsBackward;
|
||||
// u_turn can be in both sides of feature.
|
||||
struct UTurnEnding
|
||||
{
|
||||
bool m_atTheBegin = false;
|
||||
bool m_atTheEnd = false;
|
||||
};
|
||||
// Stored featureId and it's UTurnEnding, which shows where is
|
||||
// u_turn restriction is placed - at the beginning or at the ending of feature.
|
||||
//
|
||||
// If m_noUTurnRestrictions.count(featureId) == 0, that means, that there are no any
|
||||
// no_u_turn restriction at the feature with id = featureId.
|
||||
std::unordered_map<uint32_t, UTurnEnding> m_noUTurnRestrictions;
|
||||
RoadAccess m_roadAccess;
|
||||
RoadPenalty m_roadPenalty;
|
||||
RoutingOptions m_avoidRoutingOptions;
|
||||
bool m_isLeftHandTraffic;
|
||||
|
||||
std::function<time_t()> m_currentTimeGetter = []() { return GetCurrentTimestamp(); };
|
||||
RouteWeight getTurnPenalty(EdgeEstimator::Purpose purpose, Segment const & from, Segment const & to) const;
|
||||
};
|
||||
|
||||
template <typename AccessPositionType>
|
||||
bool IndexGraph::IsAccessNoForSure(AccessPositionType const & accessPositionType, RouteWeight const & weight,
|
||||
bool useAccessConditional) const
|
||||
{
|
||||
auto const [accessType, confidence] = useAccessConditional
|
||||
? m_roadAccess.GetAccess(accessPositionType, weight)
|
||||
: m_roadAccess.GetAccessWithoutConditional(accessPositionType);
|
||||
return accessType == RoadAccess::Type::No && confidence == RoadAccess::Confidence::Sure;
|
||||
}
|
||||
|
||||
template <typename ParentVertex>
|
||||
bool IndexGraph::IsRestricted(ParentVertex const & parent, uint32_t parentFeatureId, uint32_t currentFeatureId,
|
||||
bool isOutgoing, Parents<ParentVertex> const & parents) const
|
||||
{
|
||||
if (parentFeatureId == currentFeatureId)
|
||||
return false;
|
||||
|
||||
auto const & restrictions = isOutgoing ? m_restrictionsForward : m_restrictionsBackward;
|
||||
auto const it = restrictions.find(currentFeatureId);
|
||||
if (it == restrictions.cend())
|
||||
return false;
|
||||
|
||||
std::vector<ParentVertex> parentsFromCurrent;
|
||||
// Finds the first featureId from parents, that differ from |p.GetFeatureId()|.
|
||||
auto const appendNextParent = [&parents](ParentVertex const & p, auto & parentsVector)
|
||||
{
|
||||
uint32_t prevFeatureId = p.GetFeatureId();
|
||||
uint32_t curFeatureId = prevFeatureId;
|
||||
|
||||
auto nextParent = parents.end();
|
||||
auto * curParent = &p;
|
||||
while (curFeatureId == prevFeatureId)
|
||||
{
|
||||
auto const parentIt = parents.find(*curParent);
|
||||
if (parentIt == parents.cend())
|
||||
return false;
|
||||
|
||||
curFeatureId = parentIt->second.GetFeatureId();
|
||||
nextParent = parentIt;
|
||||
curParent = &nextParent->second;
|
||||
}
|
||||
|
||||
ASSERT(nextParent != parents.end(), ());
|
||||
parentsVector.emplace_back(nextParent->second);
|
||||
return true;
|
||||
};
|
||||
|
||||
for (std::vector<uint32_t> const & restriction : it->second)
|
||||
{
|
||||
bool const prevIsParent = restriction[0] == parentFeatureId;
|
||||
if (!prevIsParent)
|
||||
continue;
|
||||
|
||||
if (restriction.size() == 1)
|
||||
return true;
|
||||
|
||||
// If parents are empty we process only two feature restrictions.
|
||||
if (parents.empty())
|
||||
continue;
|
||||
|
||||
if (!appendNextParent(parent, parentsFromCurrent))
|
||||
continue;
|
||||
|
||||
for (size_t i = 1; i < restriction.size(); ++i)
|
||||
{
|
||||
ASSERT_GREATER_OR_EQUAL(i, 1, ("Unexpected overflow."));
|
||||
if (i - 1 == parentsFromCurrent.size() && !appendNextParent(parentsFromCurrent.back(), parentsFromCurrent))
|
||||
break;
|
||||
|
||||
if (parentsFromCurrent.back().GetFeatureId() != restriction[i])
|
||||
break;
|
||||
|
||||
if (i + 1 == restriction.size())
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
} // namespace routing
|
||||
295
libs/routing/index_graph_loader.cpp
Normal file
295
libs/routing/index_graph_loader.cpp
Normal file
|
|
@ -0,0 +1,295 @@
|
|||
#include "routing/index_graph_loader.hpp"
|
||||
|
||||
#include "routing/data_source.hpp"
|
||||
#include "routing/index_graph_serialization.hpp"
|
||||
#include "routing/restriction_loader.hpp"
|
||||
#include "routing/road_access.hpp"
|
||||
#include "routing/road_access_serialization.hpp"
|
||||
#include "routing/road_penalty.hpp"
|
||||
#include "routing/road_penalty_serialization.hpp"
|
||||
#include "routing/route.hpp"
|
||||
#include "routing/speed_camera_ser_des.hpp"
|
||||
|
||||
#include "coding/files_container.hpp"
|
||||
|
||||
#include "base/assert.hpp"
|
||||
#include "base/timer.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <map>
|
||||
#include <unordered_map>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
namespace
|
||||
{
|
||||
using namespace std;
|
||||
|
||||
class IndexGraphLoaderImpl final : public IndexGraphLoader
|
||||
{
|
||||
public:
|
||||
IndexGraphLoaderImpl(VehicleType vehicleType, bool loadAltitudes,
|
||||
shared_ptr<VehicleModelFactoryInterface> vehicleModelFactory,
|
||||
shared_ptr<EdgeEstimator> estimator, MwmDataSource & dataSource,
|
||||
RoutingOptions routingOptions = RoutingOptions())
|
||||
: m_vehicleType(vehicleType)
|
||||
, m_loadAltitudes(loadAltitudes)
|
||||
, m_dataSource(dataSource)
|
||||
, m_vehicleModelFactory(std::move(vehicleModelFactory))
|
||||
, m_estimator(std::move(estimator))
|
||||
, m_avoidRoutingOptions(routingOptions)
|
||||
{
|
||||
CHECK(m_vehicleModelFactory, ());
|
||||
CHECK(m_estimator, ());
|
||||
}
|
||||
|
||||
// IndexGraphLoader overrides:
|
||||
IndexGraph & GetIndexGraph(NumMwmId numMwmId) override;
|
||||
Geometry & GetGeometry(NumMwmId numMwmId) override;
|
||||
vector<RouteSegment::SpeedCamera> GetSpeedCameraInfo(Segment const & segment) override;
|
||||
void Clear() override;
|
||||
|
||||
private:
|
||||
using GeometryPtrT = shared_ptr<Geometry>;
|
||||
GeometryPtrT CreateGeometry(NumMwmId numMwmId);
|
||||
using GraphPtrT = unique_ptr<IndexGraph>;
|
||||
GraphPtrT CreateIndexGraph(NumMwmId numMwmId, GeometryPtrT & geometry);
|
||||
|
||||
VehicleType m_vehicleType;
|
||||
bool m_loadAltitudes;
|
||||
MwmDataSource & m_dataSource;
|
||||
shared_ptr<VehicleModelFactoryInterface> m_vehicleModelFactory;
|
||||
shared_ptr<EdgeEstimator> m_estimator;
|
||||
|
||||
struct GraphAttrs
|
||||
{
|
||||
GeometryPtrT m_geometry;
|
||||
// May be nullptr, because it has "lazy" loading.
|
||||
GraphPtrT m_graph;
|
||||
};
|
||||
unordered_map<NumMwmId, GraphAttrs> m_graphs;
|
||||
|
||||
unordered_map<NumMwmId, SpeedCamerasMapT> m_cachedCameras;
|
||||
SpeedCamerasMapT const & ReceiveSpeedCamsFromMwm(NumMwmId numMwmId);
|
||||
|
||||
RoutingOptions m_avoidRoutingOptions;
|
||||
std::function<time_t()> m_currentTimeGetter = [time = GetCurrentTimestamp()]() { return time; };
|
||||
};
|
||||
|
||||
IndexGraph & IndexGraphLoaderImpl::GetIndexGraph(NumMwmId numMwmId)
|
||||
{
|
||||
auto res = m_graphs.try_emplace(numMwmId, GraphAttrs());
|
||||
if (res.second || res.first->second.m_graph == nullptr)
|
||||
{
|
||||
// Create graph using (or initializing) existing geometry.
|
||||
res.first->second.m_graph = CreateIndexGraph(numMwmId, res.first->second.m_geometry);
|
||||
}
|
||||
return *(res.first->second.m_graph);
|
||||
}
|
||||
|
||||
Geometry & IndexGraphLoaderImpl::GetGeometry(NumMwmId numMwmId)
|
||||
{
|
||||
auto res = m_graphs.try_emplace(numMwmId, GraphAttrs());
|
||||
if (res.second)
|
||||
{
|
||||
// Create geometry only, graph stays nullptr.
|
||||
res.first->second.m_geometry = CreateGeometry(numMwmId);
|
||||
}
|
||||
return *(res.first->second.m_geometry);
|
||||
}
|
||||
|
||||
SpeedCamerasMapT const & IndexGraphLoaderImpl::ReceiveSpeedCamsFromMwm(NumMwmId numMwmId)
|
||||
{
|
||||
auto res = m_cachedCameras.try_emplace(numMwmId, SpeedCamerasMapT{});
|
||||
if (res.second)
|
||||
(void)ReadSpeedCamsFromMwm(m_dataSource.GetMwmValue(numMwmId), res.first->second);
|
||||
return res.first->second;
|
||||
}
|
||||
|
||||
vector<RouteSegment::SpeedCamera> IndexGraphLoaderImpl::GetSpeedCameraInfo(Segment const & segment)
|
||||
{
|
||||
SpeedCamerasMapT const & camerasMap = ReceiveSpeedCamsFromMwm(segment.GetMwmId());
|
||||
auto it = camerasMap.find({segment.GetFeatureId(), segment.GetSegmentIdx()});
|
||||
if (it == camerasMap.end())
|
||||
return {};
|
||||
|
||||
auto cameras = it->second;
|
||||
base::SortUnique(cameras, std::less<RouteSegment::SpeedCamera>(),
|
||||
[](auto const & l, auto const & r) { return l.EqualCoef(r); });
|
||||
|
||||
// Cameras stored from beginning to ending of segment. So if we go at segment in backward direction,
|
||||
// we should read cameras in reverse sequence too.
|
||||
if (!segment.IsForward())
|
||||
std::reverse(cameras.begin(), cameras.end());
|
||||
|
||||
return cameras;
|
||||
}
|
||||
|
||||
IndexGraphLoaderImpl::GraphPtrT IndexGraphLoaderImpl::CreateIndexGraph(NumMwmId numMwmId, GeometryPtrT & geometry)
|
||||
{
|
||||
MwmSet::MwmHandle const & handle = m_dataSource.GetHandle(numMwmId);
|
||||
MwmValue const * value = handle.GetValue();
|
||||
|
||||
try
|
||||
{
|
||||
base::Timer timer;
|
||||
|
||||
if (!geometry)
|
||||
{
|
||||
auto vehicleModel = m_vehicleModelFactory->GetVehicleModelForCountry(value->GetCountryFileName());
|
||||
geometry = make_shared<Geometry>(GeometryLoader::Create(handle, std::move(vehicleModel), m_loadAltitudes));
|
||||
}
|
||||
|
||||
auto graph = make_unique<IndexGraph>(geometry, m_estimator, m_avoidRoutingOptions, &value->GetRegionData());
|
||||
graph->SetCurrentTimeGetter(m_currentTimeGetter);
|
||||
DeserializeIndexGraph(*value, m_vehicleType, *graph);
|
||||
|
||||
LOG(LINFO,
|
||||
(ROUTING_FILE_TAG, "section for", value->GetCountryFileName(), "loaded in", timer.ElapsedSeconds(), "seconds"));
|
||||
return graph;
|
||||
}
|
||||
catch (RootException const & ex)
|
||||
{
|
||||
LOG(LERROR, ("Error reading graph for", value->m_file));
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
IndexGraphLoaderImpl::GeometryPtrT IndexGraphLoaderImpl::CreateGeometry(NumMwmId numMwmId)
|
||||
{
|
||||
MwmSet::MwmHandle const & handle = m_dataSource.GetHandle(numMwmId);
|
||||
MwmValue const * value = handle.GetValue();
|
||||
|
||||
auto vehicleModel = m_vehicleModelFactory->GetVehicleModelForCountry(value->GetCountryFileName());
|
||||
return make_shared<Geometry>(GeometryLoader::Create(handle, std::move(vehicleModel), m_loadAltitudes));
|
||||
}
|
||||
|
||||
void IndexGraphLoaderImpl::Clear()
|
||||
{
|
||||
m_graphs.clear();
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
bool ReadSpeedCamsFromMwm(MwmValue const & mwmValue, SpeedCamerasMapT & camerasMap)
|
||||
{
|
||||
try
|
||||
{
|
||||
auto reader = mwmValue.m_cont.GetReader(CAMERAS_INFO_FILE_TAG);
|
||||
ReaderSource src(reader);
|
||||
DeserializeSpeedCamsFromMwm(src, camerasMap);
|
||||
return true;
|
||||
}
|
||||
catch (Reader::OpenException const &)
|
||||
{
|
||||
LOG(LWARNING, (CAMERAS_INFO_FILE_TAG "section not found in", mwmValue.GetCountryFileName()));
|
||||
}
|
||||
catch (Reader::Exception const & e)
|
||||
{
|
||||
LOG(LERROR, ("Error while reading", CAMERAS_INFO_FILE_TAG, "section in", mwmValue.GetCountryFileName(), ":", e.Msg()));
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool ReadRoadAccessFromMwm(MwmValue const & mwmValue, VehicleType vehicleType, RoadAccess & roadAccess)
|
||||
{
|
||||
try
|
||||
{
|
||||
auto const reader = mwmValue.m_cont.GetReader(ROAD_ACCESS_FILE_TAG);
|
||||
ReaderSource src(reader);
|
||||
RoadAccessSerializer::Deserialize(src, vehicleType, roadAccess);
|
||||
return true;
|
||||
}
|
||||
catch (Reader::OpenException const &)
|
||||
{
|
||||
LOG(LWARNING, (ROAD_ACCESS_FILE_TAG, "section not found in", mwmValue.GetCountryFileName()));
|
||||
}
|
||||
catch (Reader::Exception const & e)
|
||||
{
|
||||
LOG(LERROR, ("Error while reading", ROAD_ACCESS_FILE_TAG, "section in", mwmValue.GetCountryFileName(), ":", e.Msg()));
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool ReadRoadPenaltyFromMwm(MwmValue const & mwmValue, VehicleType vehicleType, RoadPenalty & roadPenalty)
|
||||
{
|
||||
try
|
||||
{
|
||||
auto const reader = mwmValue.m_cont.GetReader(ROAD_PENALTY_FILE_TAG);
|
||||
ReaderSource src(reader);
|
||||
|
||||
// Read number of vehicle types
|
||||
uint32_t numVehicleTypes = ReadPrimitiveFromSource<uint32_t>(src);
|
||||
CHECK_EQUAL(numVehicleTypes, static_cast<uint32_t>(VehicleType::Count), ());
|
||||
|
||||
// Skip to the correct vehicle type
|
||||
for (uint32_t i = 0; i < static_cast<uint32_t>(vehicleType); ++i)
|
||||
{
|
||||
RoadPenalty dummy;
|
||||
RoadPenaltySerializer::Deserialize(src, dummy, static_cast<VehicleType>(i));
|
||||
}
|
||||
|
||||
// Read the penalty data for this vehicle type
|
||||
RoadPenaltySerializer::Deserialize(src, roadPenalty, vehicleType);
|
||||
return true;
|
||||
}
|
||||
catch (Reader::OpenException const &)
|
||||
{
|
||||
// This is expected for older mwm files - not an error
|
||||
LOG(LINFO, (ROAD_PENALTY_FILE_TAG, "section not found in", mwmValue.GetCountryFileName(), "- using legacy penalty system"));
|
||||
}
|
||||
catch (Reader::Exception const & e)
|
||||
{
|
||||
LOG(LERROR, ("Error while reading", ROAD_PENALTY_FILE_TAG, "section in", mwmValue.GetCountryFileName(), ":", e.Msg()));
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// static
|
||||
unique_ptr<IndexGraphLoader> IndexGraphLoader::Create(VehicleType vehicleType, bool loadAltitudes,
|
||||
shared_ptr<VehicleModelFactoryInterface> vehicleModelFactory,
|
||||
shared_ptr<EdgeEstimator> estimator, MwmDataSource & dataSource,
|
||||
RoutingOptions routingOptions)
|
||||
{
|
||||
return make_unique<IndexGraphLoaderImpl>(vehicleType, loadAltitudes, vehicleModelFactory, estimator, dataSource,
|
||||
routingOptions);
|
||||
}
|
||||
|
||||
void DeserializeIndexGraph(MwmValue const & mwmValue, VehicleType vehicleType, IndexGraph & graph)
|
||||
{
|
||||
FilesContainerR::TReader reader(mwmValue.m_cont.GetReader(ROUTING_FILE_TAG));
|
||||
ReaderSource<FilesContainerR::TReader> src(reader);
|
||||
|
||||
IndexGraphSerializer::Deserialize(graph, src, GetVehicleMask(vehicleType));
|
||||
|
||||
// Do not load restrictions (relation type = restriction) for pedestrian routing.
|
||||
// https://wiki.openstreetmap.org/wiki/Relation:restriction
|
||||
/// @todo OSM has 49 (April 2022) restriction:foot relations. We should use them someday,
|
||||
/// starting from generator and saving like access, according to the vehicleType.
|
||||
ASSERT(vehicleType != VehicleType::Transit, ());
|
||||
if (vehicleType != VehicleType::Pedestrian)
|
||||
{
|
||||
RestrictionLoader restrictionLoader(mwmValue, graph);
|
||||
if (restrictionLoader.HasRestrictions())
|
||||
{
|
||||
graph.SetRestrictions(restrictionLoader.StealRestrictions());
|
||||
graph.SetUTurnRestrictions(restrictionLoader.StealNoUTurnRestrictions());
|
||||
}
|
||||
}
|
||||
|
||||
RoadAccess roadAccess;
|
||||
if (ReadRoadAccessFromMwm(mwmValue, vehicleType, roadAccess))
|
||||
graph.SetRoadAccess(std::move(roadAccess));
|
||||
|
||||
RoadPenalty roadPenalty;
|
||||
if (ReadRoadPenaltyFromMwm(mwmValue, vehicleType, roadPenalty))
|
||||
graph.SetRoadPenalty(std::move(roadPenalty));
|
||||
}
|
||||
|
||||
uint32_t DeserializeIndexGraphNumRoads(MwmValue const & mwmValue, VehicleType vehicleType)
|
||||
{
|
||||
FilesContainerR::TReader reader(mwmValue.m_cont.GetReader(ROUTING_FILE_TAG));
|
||||
ReaderSource<FilesContainerR::TReader> src(reader);
|
||||
return IndexGraphSerializer::DeserializeNumRoads(src, GetVehicleMask(vehicleType));
|
||||
}
|
||||
} // namespace routing
|
||||
45
libs/routing/index_graph_loader.hpp
Normal file
45
libs/routing/index_graph_loader.hpp
Normal file
|
|
@ -0,0 +1,45 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/edge_estimator.hpp"
|
||||
#include "routing/index_graph.hpp"
|
||||
#include "routing/route.hpp"
|
||||
#include "routing/speed_camera_ser_des.hpp"
|
||||
#include "routing/vehicle_mask.hpp"
|
||||
|
||||
#include "routing_common/num_mwm_id.hpp"
|
||||
#include "routing_common/vehicle_model.hpp"
|
||||
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
class MwmValue;
|
||||
|
||||
namespace routing
|
||||
{
|
||||
class MwmDataSource;
|
||||
|
||||
class IndexGraphLoader
|
||||
{
|
||||
public:
|
||||
virtual ~IndexGraphLoader() = default;
|
||||
|
||||
virtual IndexGraph & GetIndexGraph(NumMwmId mwmId) = 0;
|
||||
virtual Geometry & GetGeometry(NumMwmId numMwmId) = 0;
|
||||
|
||||
// Because several cameras can lie on one segment we return vector of them.
|
||||
virtual std::vector<RouteSegment::SpeedCamera> GetSpeedCameraInfo(Segment const & segment) = 0;
|
||||
virtual void Clear() = 0;
|
||||
|
||||
static std::unique_ptr<IndexGraphLoader> Create(VehicleType vehicleType, bool loadAltitudes,
|
||||
std::shared_ptr<VehicleModelFactoryInterface> vehicleModelFactory,
|
||||
std::shared_ptr<EdgeEstimator> estimator, MwmDataSource & dataSource,
|
||||
RoutingOptions routingOptions = RoutingOptions());
|
||||
};
|
||||
|
||||
void DeserializeIndexGraph(MwmValue const & mwmValue, VehicleType vehicleType, IndexGraph & graph);
|
||||
|
||||
uint32_t DeserializeIndexGraphNumRoads(MwmValue const & mwmValue, VehicleType vehicleType);
|
||||
|
||||
bool ReadRoadAccessFromMwm(MwmValue const & mwmValue, VehicleType vehicleType, RoadAccess & roadAccess);
|
||||
bool ReadSpeedCamsFromMwm(MwmValue const & mwmValue, SpeedCamerasMapT & camerasMap);
|
||||
} // namespace routing
|
||||
124
libs/routing/index_graph_serialization.cpp
Normal file
124
libs/routing/index_graph_serialization.cpp
Normal file
|
|
@ -0,0 +1,124 @@
|
|||
#include "routing/index_graph_serialization.hpp"
|
||||
|
||||
namespace routing
|
||||
{
|
||||
// static
|
||||
uint8_t constexpr IndexGraphSerializer::kLastVersion;
|
||||
uint32_t constexpr IndexGraphSerializer::JointsFilter::kEmptyEntry;
|
||||
uint32_t constexpr IndexGraphSerializer::JointsFilter::kPushedEntry;
|
||||
|
||||
// IndexGraphSerializer::SectionSerializer ---------------------------------------------------------
|
||||
void IndexGraphSerializer::SectionSerializer::PreSerialize(IndexGraph const & graph,
|
||||
std::unordered_map<uint32_t, VehicleMask> const & masks,
|
||||
JointIdEncoder & jointEncoder)
|
||||
{
|
||||
m_buffer.clear();
|
||||
MemWriter<std::vector<uint8_t>> memWriter(m_buffer);
|
||||
BitWriter<MemWriter<std::vector<uint8_t>>> writer(memWriter);
|
||||
|
||||
// -1 for uint32_t is some confusing, but it allows process first iteration in the common way.
|
||||
// Gamma coder can't write 0, so init prevFeatureId = -1 in case of first featureId == 0.
|
||||
// It works because uint32_t is residual ring type.
|
||||
uint32_t prevFeatureId = -1;
|
||||
|
||||
for (uint32_t const featureId : m_featureIds)
|
||||
{
|
||||
RoadJointIds const & road = graph.GetRoad(featureId);
|
||||
WriteGamma(writer, featureId - prevFeatureId);
|
||||
WriteGamma(writer, ConvertJointsNumber(road.GetJointsNumber()));
|
||||
|
||||
uint32_t prevPointId = -1;
|
||||
road.ForEachJoint([&](uint32_t pointId, Joint::Id jointId)
|
||||
{
|
||||
WriteGamma(writer, pointId - prevPointId);
|
||||
jointEncoder.Write(jointId, writer);
|
||||
prevPointId = pointId;
|
||||
});
|
||||
|
||||
prevFeatureId = featureId;
|
||||
}
|
||||
}
|
||||
|
||||
// IndexGraphSerializer::JointsFilter --------------------------------------------------------------
|
||||
void IndexGraphSerializer::JointsFilter::Push(Joint::Id jointIdInFile, RoadPoint const & rp)
|
||||
{
|
||||
CHECK_LESS(jointIdInFile, m_entries.size(), ());
|
||||
|
||||
auto & entry = m_entries[jointIdInFile];
|
||||
switch (entry.first)
|
||||
{
|
||||
case kEmptyEntry:
|
||||
// Keep RoadPoint here until second RoadPoint with same Joint::Id comes.
|
||||
// If second point does not come, it is redundant Joint that should be filtered.
|
||||
entry.first = rp.GetFeatureId();
|
||||
entry.second.pointId = rp.GetPointId();
|
||||
break;
|
||||
case kPushedEntry: m_graph.PushFromSerializer(entry.second.jointId, rp); break;
|
||||
default:
|
||||
m_graph.PushFromSerializer(m_count, RoadPoint(entry.first /* featureId */, entry.second.pointId));
|
||||
m_graph.PushFromSerializer(m_count, rp);
|
||||
entry.first = kPushedEntry;
|
||||
entry.second.jointId = m_count;
|
||||
++m_count;
|
||||
}
|
||||
}
|
||||
|
||||
// IndexGraphSerializer ----------------------------------------------------------------------------
|
||||
// static
|
||||
VehicleMask IndexGraphSerializer::GetRoadMask(std::unordered_map<uint32_t, VehicleMask> const & masks,
|
||||
uint32_t featureId)
|
||||
{
|
||||
auto const & it = masks.find(featureId);
|
||||
CHECK(it != masks.cend(), ("Can't find vehicle mask for feature", featureId));
|
||||
return it->second;
|
||||
}
|
||||
|
||||
// Gamma Coder encodes value 1 with 1 bit, value 2 with 3 bits.
|
||||
// The vast majority of roads contains 2 joints: endpoints connections.
|
||||
// Swap values 1, 2 to encode most of joints numbers with 1 bit.
|
||||
//
|
||||
// static
|
||||
uint32_t IndexGraphSerializer::ConvertJointsNumber(uint32_t jointsNumber)
|
||||
{
|
||||
switch (jointsNumber)
|
||||
{
|
||||
case 1: return 2;
|
||||
case 2: return 1;
|
||||
default: return jointsNumber;
|
||||
}
|
||||
}
|
||||
|
||||
// static
|
||||
void IndexGraphSerializer::PrepareSectionSerializers(IndexGraph const & graph,
|
||||
std::unordered_map<uint32_t, VehicleMask> const & masks,
|
||||
std::vector<SectionSerializer> & serializers)
|
||||
{
|
||||
size_t maskToIndex[kNumVehicleMasks] = {};
|
||||
// Car routing is most used routing: put car sections first.
|
||||
// It would be a bit faster read them for car model.
|
||||
for (size_t step = 0; step < 2; ++step)
|
||||
{
|
||||
for (VehicleMask mask = 1; mask < kNumVehicleMasks; ++mask)
|
||||
{
|
||||
bool const hasCar = (mask & kCarMask) != 0;
|
||||
if ((step == 0) == hasCar)
|
||||
{
|
||||
CHECK_EQUAL(maskToIndex[mask], 0, ("Mask", mask, "already has serializer"));
|
||||
maskToIndex[mask] = serializers.size();
|
||||
serializers.emplace_back(mask /* SectionSerializer(mask) */);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
graph.ForEachRoad([&](uint32_t featureId, RoadJointIds const & /* road */)
|
||||
{
|
||||
VehicleMask const mask = GetRoadMask(masks, featureId);
|
||||
SectionSerializer & serializer = serializers[maskToIndex[mask]];
|
||||
CHECK_EQUAL(serializer.GetMask(), mask, ());
|
||||
serializer.AddRoad(featureId);
|
||||
});
|
||||
|
||||
for (SectionSerializer & serializer : serializers)
|
||||
serializer.SortRoads();
|
||||
}
|
||||
} // namespace routing
|
||||
389
libs/routing/index_graph_serialization.hpp
Normal file
389
libs/routing/index_graph_serialization.hpp
Normal file
|
|
@ -0,0 +1,389 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/coding.hpp"
|
||||
#include "routing/index_graph.hpp"
|
||||
#include "routing/joint.hpp"
|
||||
#include "routing/routing_exceptions.hpp"
|
||||
#include "routing/vehicle_mask.hpp"
|
||||
|
||||
#include "coding/bit_streams.hpp"
|
||||
#include "coding/reader.hpp"
|
||||
#include "coding/write_to_sink.hpp"
|
||||
|
||||
#include "base/checked_cast.hpp"
|
||||
|
||||
#include <limits>
|
||||
#include <type_traits>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
class IndexGraphSerializer final
|
||||
{
|
||||
public:
|
||||
IndexGraphSerializer() = delete;
|
||||
|
||||
template <class Sink>
|
||||
static void Serialize(IndexGraph const & graph, std::unordered_map<uint32_t, VehicleMask> const & masks, Sink & sink)
|
||||
{
|
||||
Header header(graph);
|
||||
JointIdEncoder jointEncoder;
|
||||
|
||||
std::vector<SectionSerializer> serializers;
|
||||
PrepareSectionSerializers(graph, masks, serializers);
|
||||
|
||||
for (SectionSerializer & serializer : serializers)
|
||||
{
|
||||
Joint::Id const begin = jointEncoder.GetCount();
|
||||
serializer.PreSerialize(graph, masks, jointEncoder);
|
||||
header.AddSection({
|
||||
serializer.GetBufferSize(),
|
||||
static_cast<uint32_t>(serializer.GetNumRoads()),
|
||||
begin,
|
||||
jointEncoder.GetCount(),
|
||||
serializer.GetMask(),
|
||||
});
|
||||
}
|
||||
|
||||
header.Serialize(sink);
|
||||
for (SectionSerializer & section : serializers)
|
||||
section.Flush(sink);
|
||||
}
|
||||
|
||||
template <class Source>
|
||||
static void Deserialize(IndexGraph & graph, Source & src, VehicleMask requiredMask)
|
||||
{
|
||||
Header header;
|
||||
header.Deserialize(src);
|
||||
|
||||
JointsFilter jointsFilter(graph, header.GetNumJoints());
|
||||
|
||||
for (uint32_t i = 0; i < header.GetNumSections(); ++i)
|
||||
{
|
||||
Section const & section = header.GetSection(i);
|
||||
VehicleMask const mask = section.GetMask();
|
||||
|
||||
if (!(mask & requiredMask))
|
||||
{
|
||||
src.Skip(section.GetSize());
|
||||
continue;
|
||||
}
|
||||
|
||||
JointIdDecoder jointIdDecoder(section.GetBeginJointId());
|
||||
BitReader<Source> reader(src);
|
||||
uint64_t const expectedEndPos = src.Pos() + section.GetSize();
|
||||
|
||||
// -1 for uint32_t is some confusing, but it allows process first iteration in the common way.
|
||||
// Delta coder can't write 0, so init prevFeatureId = -1 in case of first featureId == 0.
|
||||
// It works because uint32_t is residual ring type.
|
||||
uint32_t featureId = -1;
|
||||
for (uint32_t i = 0; i < section.GetNumRoads(); ++i)
|
||||
{
|
||||
uint32_t const featureDelta = ReadGamma<uint32_t>(reader);
|
||||
featureId += featureDelta;
|
||||
|
||||
uint32_t const jointsNumber = ConvertJointsNumber(ReadGamma<uint32_t>(reader));
|
||||
|
||||
// See comment above about -1.
|
||||
uint32_t pointId = -1;
|
||||
|
||||
for (uint32_t j = 0; j < jointsNumber; ++j)
|
||||
{
|
||||
auto const pointDelta = ReadGamma<uint32_t>(reader);
|
||||
pointId += pointDelta;
|
||||
Joint::Id const jointId = jointIdDecoder.Read(reader);
|
||||
if (jointId >= section.GetEndJointId())
|
||||
{
|
||||
MYTHROW(CorruptedDataException, ("Invalid jointId =", jointId, ", end =", section.GetEndJointId(),
|
||||
", mask =", mask, ", pointId =", pointId, ", featureId =", featureId));
|
||||
}
|
||||
|
||||
jointsFilter.Push(jointId, RoadPoint(featureId, pointId));
|
||||
}
|
||||
}
|
||||
|
||||
if (jointIdDecoder.GetCount() != section.GetEndJointId())
|
||||
{
|
||||
MYTHROW(CorruptedDataException, ("Invalid decoder count =", jointIdDecoder.GetCount(),
|
||||
", expected =", section.GetEndJointId(), ", mask =", mask));
|
||||
}
|
||||
|
||||
if (src.Pos() != expectedEndPos)
|
||||
{
|
||||
MYTHROW(CorruptedDataException, ("Wrong position", src.Pos(), "after decoding section", mask, "expected",
|
||||
expectedEndPos, "section size =", section.GetSize()));
|
||||
}
|
||||
}
|
||||
|
||||
graph.Build(jointsFilter.GetCount());
|
||||
}
|
||||
|
||||
template <class Source>
|
||||
static uint32_t DeserializeNumRoads(Source & src, VehicleMask requiredMask)
|
||||
{
|
||||
Header header;
|
||||
header.Deserialize(src);
|
||||
|
||||
uint32_t numRoads = 0;
|
||||
for (uint32_t i = 0; i < header.GetNumSections(); ++i)
|
||||
{
|
||||
Section const & section = header.GetSection(i);
|
||||
VehicleMask const mask = section.GetMask();
|
||||
|
||||
if (mask & requiredMask)
|
||||
numRoads += section.GetNumRoads();
|
||||
|
||||
src.Skip(section.GetSize());
|
||||
}
|
||||
|
||||
return numRoads;
|
||||
}
|
||||
|
||||
private:
|
||||
static uint8_t constexpr kLastVersion = 0;
|
||||
static uint8_t constexpr kNewJointIdBit = 0;
|
||||
static uint8_t constexpr kRepeatJointIdBit = 1;
|
||||
|
||||
class Section final
|
||||
{
|
||||
public:
|
||||
Section() = default;
|
||||
|
||||
Section(uint64_t size, uint32_t numRoads, Joint::Id beginJointId, Joint::Id endJointId, VehicleMask mask)
|
||||
: m_size(size)
|
||||
, m_numRoads(numRoads)
|
||||
, m_beginJointId(beginJointId)
|
||||
, m_endJointId(endJointId)
|
||||
, m_mask(mask)
|
||||
{}
|
||||
|
||||
template <class Sink>
|
||||
void Serialize(Sink & sink) const
|
||||
{
|
||||
WriteToSink(sink, m_size);
|
||||
WriteToSink(sink, m_numRoads);
|
||||
WriteToSink(sink, m_beginJointId);
|
||||
WriteToSink(sink, m_endJointId);
|
||||
WriteToSink(sink, m_mask);
|
||||
}
|
||||
|
||||
template <class Source>
|
||||
void Deserialize(Source & src)
|
||||
{
|
||||
m_size = ReadPrimitiveFromSource<decltype(m_size)>(src);
|
||||
m_numRoads = ReadPrimitiveFromSource<decltype(m_numRoads)>(src);
|
||||
m_beginJointId = ReadPrimitiveFromSource<decltype(m_beginJointId)>(src);
|
||||
m_endJointId = ReadPrimitiveFromSource<decltype(m_endJointId)>(src);
|
||||
m_mask = ReadPrimitiveFromSource<decltype(m_mask)>(src);
|
||||
}
|
||||
|
||||
uint64_t GetSize() const { return m_size; }
|
||||
uint32_t GetNumRoads() const { return m_numRoads; }
|
||||
Joint::Id GetBeginJointId() const { return m_beginJointId; }
|
||||
Joint::Id GetEndJointId() const { return m_endJointId; }
|
||||
VehicleMask GetMask() const { return m_mask; }
|
||||
|
||||
private:
|
||||
uint64_t m_size = 0;
|
||||
uint32_t m_numRoads = 0;
|
||||
Joint::Id m_beginJointId = Joint::kInvalidId;
|
||||
Joint::Id m_endJointId = Joint::kInvalidId;
|
||||
VehicleMask m_mask = 0;
|
||||
};
|
||||
|
||||
class Header final
|
||||
{
|
||||
public:
|
||||
Header() = default;
|
||||
|
||||
explicit Header(IndexGraph const & graph) : m_numRoads(graph.GetNumRoads()), m_numJoints(graph.GetNumJoints()) {}
|
||||
|
||||
template <class Sink>
|
||||
void Serialize(Sink & sink) const
|
||||
{
|
||||
WriteToSink(sink, m_version);
|
||||
WriteToSink(sink, m_numRoads);
|
||||
WriteToSink(sink, m_numJoints);
|
||||
|
||||
WriteToSink(sink, static_cast<uint32_t>(m_sections.size()));
|
||||
for (Section const & section : m_sections)
|
||||
section.Serialize(sink);
|
||||
}
|
||||
|
||||
template <class Source>
|
||||
void Deserialize(Source & src)
|
||||
{
|
||||
m_version = ReadPrimitiveFromSource<decltype(m_version)>(src);
|
||||
if (m_version != kLastVersion)
|
||||
{
|
||||
MYTHROW(CorruptedDataException,
|
||||
("Unknown index graph version ", int(m_version), ", current version ", int(kLastVersion)));
|
||||
}
|
||||
|
||||
m_numRoads = ReadPrimitiveFromSource<decltype(m_numRoads)>(src);
|
||||
m_numJoints = ReadPrimitiveFromSource<decltype(m_numJoints)>(src);
|
||||
|
||||
auto const sectionsSize = ReadPrimitiveFromSource<uint32_t>(src);
|
||||
m_sections.resize(sectionsSize);
|
||||
for (Section & section : m_sections)
|
||||
section.Deserialize(src);
|
||||
}
|
||||
|
||||
uint32_t GetNumRoads() const { return m_numRoads; }
|
||||
Joint::Id GetNumJoints() const { return m_numJoints; }
|
||||
uint32_t GetNumSections() const { return base::asserted_cast<uint32_t>(m_sections.size()); }
|
||||
|
||||
Section const & GetSection(size_t index) const
|
||||
{
|
||||
CHECK_LESS(index, m_sections.size(), ());
|
||||
return m_sections[index];
|
||||
}
|
||||
|
||||
void AddSection(Section const & section) { m_sections.push_back(section); }
|
||||
|
||||
private:
|
||||
uint8_t m_version = kLastVersion;
|
||||
uint32_t m_numRoads = 0;
|
||||
Joint::Id m_numJoints = 0;
|
||||
std::vector<Section> m_sections;
|
||||
};
|
||||
|
||||
class JointIdEncoder final
|
||||
{
|
||||
public:
|
||||
template <class Sink>
|
||||
void Write(Joint::Id originalId, BitWriter<Sink> & writer)
|
||||
{
|
||||
auto const & it = m_convertedIds.find(originalId);
|
||||
if (it != m_convertedIds.end())
|
||||
{
|
||||
Joint::Id const convertedId = it->second;
|
||||
CHECK_LESS(convertedId, m_count,
|
||||
("Duplicate joint id should be less than count, convertedId =", convertedId, ", count =", m_count,
|
||||
", originalId =", originalId));
|
||||
writer.Write(kRepeatJointIdBit, 1);
|
||||
// m_count - convertedId is less than convertedId in general.
|
||||
// So write this delta to save file space.
|
||||
WriteDelta(writer, m_count - convertedId);
|
||||
return;
|
||||
}
|
||||
|
||||
m_convertedIds[originalId] = m_count;
|
||||
writer.Write(kNewJointIdBit, 1);
|
||||
++m_count;
|
||||
}
|
||||
|
||||
Joint::Id GetCount() const { return m_count; }
|
||||
|
||||
private:
|
||||
Joint::Id m_count = 0;
|
||||
std::unordered_map<Joint::Id, Joint::Id> m_convertedIds;
|
||||
};
|
||||
|
||||
class JointIdDecoder final
|
||||
{
|
||||
public:
|
||||
// Joint::Id count is incrementing along entire file.
|
||||
// But deserializer skips some sections, therefore we should recover counter at each section
|
||||
// start.
|
||||
JointIdDecoder(Joint::Id startId) : m_count(startId) {}
|
||||
|
||||
template <class Source>
|
||||
Joint::Id Read(BitReader<Source> & reader)
|
||||
{
|
||||
uint8_t const bit = reader.Read(1);
|
||||
if (bit == kRepeatJointIdBit)
|
||||
{
|
||||
auto const delta = ReadDelta<uint32_t>(reader);
|
||||
if (delta > m_count)
|
||||
MYTHROW(CorruptedDataException, ("Joint id delta", delta, "> count =", m_count));
|
||||
|
||||
return m_count - delta;
|
||||
}
|
||||
|
||||
return m_count++;
|
||||
}
|
||||
|
||||
Joint::Id GetCount() const { return m_count; }
|
||||
|
||||
private:
|
||||
Joint::Id m_count;
|
||||
};
|
||||
|
||||
// JointsFilter has two goals:
|
||||
//
|
||||
// 1. Deserialization skips some sections.
|
||||
// Therefore one skips some joint ids from a continuous series of numbers [0, numJoints).
|
||||
// JointsFilter converts loaded joint ids to a new continuous series of numbers [0,
|
||||
// numLoadedJoints).
|
||||
//
|
||||
// 2. Some joints connect roads from different models.
|
||||
// E.g. 2 roads joint: all vehicles road + pedestrian road.
|
||||
// If one loads car roads only, pedestrian roads should be skipped,
|
||||
// so joint would have only point from all vehicles road.
|
||||
// JointsFilter filters such redundant joints.
|
||||
class JointsFilter final
|
||||
{
|
||||
public:
|
||||
JointsFilter(IndexGraph & graph, Joint::Id numJoints) : m_graph(graph)
|
||||
{
|
||||
m_entries.assign(numJoints, {kEmptyEntry, {0}});
|
||||
}
|
||||
|
||||
void Push(Joint::Id jointIdInFile, RoadPoint const & rp);
|
||||
Joint::Id GetCount() const { return m_count; }
|
||||
|
||||
private:
|
||||
static auto constexpr kEmptyEntry = std::numeric_limits<uint32_t>::max();
|
||||
static auto constexpr kPushedEntry = std::numeric_limits<uint32_t>::max() - 1;
|
||||
|
||||
// Joints number is large.
|
||||
// Therefore point id and joint id are stored in same space to save some memory.
|
||||
union Point
|
||||
{
|
||||
uint32_t pointId;
|
||||
Joint::Id jointId;
|
||||
};
|
||||
|
||||
static_assert(std::is_integral<Joint::Id>::value, "Joint::Id should be integral type");
|
||||
|
||||
IndexGraph & m_graph;
|
||||
Joint::Id m_count = 0;
|
||||
std::vector<std::pair<uint32_t, Point>> m_entries;
|
||||
};
|
||||
|
||||
class SectionSerializer final
|
||||
{
|
||||
public:
|
||||
explicit SectionSerializer(VehicleMask mask) : m_mask(mask) {}
|
||||
|
||||
size_t GetBufferSize() const { return m_buffer.size(); }
|
||||
VehicleMask GetMask() const { return m_mask; }
|
||||
size_t GetNumRoads() const { return m_featureIds.size(); }
|
||||
|
||||
void AddRoad(uint32_t featureId) { m_featureIds.push_back(featureId); }
|
||||
void SortRoads() { sort(m_featureIds.begin(), m_featureIds.end()); }
|
||||
void PreSerialize(IndexGraph const & graph, std::unordered_map<uint32_t, VehicleMask> const & masks,
|
||||
JointIdEncoder & jointEncoder);
|
||||
|
||||
template <class Sink>
|
||||
void Flush(Sink & sink)
|
||||
{
|
||||
sink.Write(m_buffer.data(), m_buffer.size());
|
||||
m_buffer.clear();
|
||||
}
|
||||
|
||||
private:
|
||||
VehicleMask const m_mask;
|
||||
std::vector<uint32_t> m_featureIds;
|
||||
std::vector<uint8_t> m_buffer;
|
||||
};
|
||||
|
||||
static VehicleMask GetRoadMask(std::unordered_map<uint32_t, VehicleMask> const & masks, uint32_t featureId);
|
||||
static uint32_t ConvertJointsNumber(uint32_t jointsNumber);
|
||||
static void PrepareSectionSerializers(IndexGraph const & graph,
|
||||
std::unordered_map<uint32_t, VehicleMask> const & masks,
|
||||
std::vector<SectionSerializer> & sections);
|
||||
};
|
||||
} // namespace routing
|
||||
598
libs/routing/index_graph_starter.cpp
Normal file
598
libs/routing/index_graph_starter.cpp
Normal file
|
|
@ -0,0 +1,598 @@
|
|||
#include "routing/index_graph_starter.hpp"
|
||||
|
||||
#include "routing/fake_edges_container.hpp"
|
||||
#include "routing/regions_sparse_graph.hpp"
|
||||
|
||||
#include "routing_common/num_mwm_id.hpp"
|
||||
|
||||
#include "coding/point_coding.hpp"
|
||||
|
||||
#include "geometry/distance_on_sphere.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <map>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
using namespace std;
|
||||
|
||||
// IndexGraphStarter::Ending -----------------------------------------------------------------------
|
||||
void IndexGraphStarter::Ending::FillMwmIds()
|
||||
{
|
||||
for (auto const & segment : m_real)
|
||||
m_mwmIds.emplace(segment.GetMwmId());
|
||||
}
|
||||
|
||||
// IndexGraphStarter -------------------------------------------------------------------------------
|
||||
// static
|
||||
void IndexGraphStarter::CheckValidRoute(vector<Segment> const & segments)
|
||||
{
|
||||
// Valid route contains at least 3 segments:
|
||||
// start fake, finish fake and at least one normal nearest segment.
|
||||
CHECK_GREATER_OR_EQUAL(segments.size(), 3, ());
|
||||
CHECK(IsFakeSegment(segments.front()), ());
|
||||
CHECK(IsFakeSegment(segments.back()), ());
|
||||
}
|
||||
|
||||
IndexGraphStarter::IndexGraphStarter(FakeEnding const & startEnding, FakeEnding const & finishEnding,
|
||||
uint32_t fakeNumerationStart, bool strictForward, WorldGraph & graph)
|
||||
: m_graph(graph)
|
||||
{
|
||||
m_fakeNumerationStart = fakeNumerationStart;
|
||||
|
||||
m_start.m_id = m_fakeNumerationStart;
|
||||
AddStart(startEnding, finishEnding, strictForward);
|
||||
m_finish.m_id = m_fakeNumerationStart;
|
||||
AddFinish(finishEnding, startEnding);
|
||||
|
||||
m_otherEndings.push_back(startEnding);
|
||||
m_otherEndings.push_back(finishEnding);
|
||||
|
||||
auto const startPoint = GetPoint(GetStartSegment(), false /* front */);
|
||||
auto const finishPoint = GetPoint(GetFinishSegment(), true /* front */);
|
||||
m_startToFinishDistanceM = ms::DistanceOnEarth(startPoint, finishPoint);
|
||||
}
|
||||
|
||||
void IndexGraphStarter::Append(FakeEdgesContainer const & container)
|
||||
{
|
||||
m_finish = container.m_finish;
|
||||
m_fake.Append(container.m_fake);
|
||||
|
||||
// It's important to calculate distance after m_fake.Append() because
|
||||
// we don't have finish segment in fake graph before m_fake.Append().
|
||||
auto const startPoint = GetPoint(GetStartSegment(), false /* front */);
|
||||
auto const finishPoint = GetPoint(GetFinishSegment(), true /* front */);
|
||||
m_startToFinishDistanceM = ms::DistanceOnEarth(startPoint, finishPoint);
|
||||
m_fakeNumerationStart += container.m_fake.GetSize();
|
||||
}
|
||||
|
||||
void IndexGraphStarter::SetGuides(GuidesGraph const & guides)
|
||||
{
|
||||
m_guides = guides;
|
||||
}
|
||||
|
||||
void IndexGraphStarter::SetRegionsGraphMode(std::shared_ptr<RegionsSparseGraph> regionsSparseGraph)
|
||||
{
|
||||
m_regionsGraph = std::move(regionsSparseGraph);
|
||||
m_graph.SetRegionsGraphMode(true);
|
||||
}
|
||||
|
||||
LatLonWithAltitude const & IndexGraphStarter::GetStartJunction() const
|
||||
{
|
||||
auto const & startSegment = GetStartSegment();
|
||||
return m_fake.GetVertex(startSegment).GetJunctionFrom();
|
||||
}
|
||||
|
||||
LatLonWithAltitude const & IndexGraphStarter::GetFinishJunction() const
|
||||
{
|
||||
auto const & finishSegment = GetFinishSegment();
|
||||
return m_fake.GetVertex(finishSegment).GetJunctionTo();
|
||||
}
|
||||
|
||||
bool IndexGraphStarter::ConvertToReal(Segment & segment) const
|
||||
{
|
||||
return IsFakeSegment(segment) ? m_fake.FindReal(segment, segment) : true;
|
||||
}
|
||||
|
||||
LatLonWithAltitude const & IndexGraphStarter::GetJunction(Segment const & segment, bool front) const
|
||||
{
|
||||
if (IsRegionsGraphMode() && !IsFakeSegment(segment))
|
||||
return m_regionsGraph->GetJunction(segment, front);
|
||||
|
||||
if (IsGuidesSegment(segment))
|
||||
return m_guides.GetJunction(segment, front);
|
||||
|
||||
if (!IsFakeSegment(segment))
|
||||
return m_graph.GetJunction(segment, front);
|
||||
|
||||
auto const & vertex = m_fake.GetVertex(segment);
|
||||
return front ? vertex.GetJunctionTo() : vertex.GetJunctionFrom();
|
||||
}
|
||||
|
||||
LatLonWithAltitude const & IndexGraphStarter::GetRouteJunction(vector<Segment> const & segments,
|
||||
size_t pointIndex) const
|
||||
{
|
||||
CHECK(!segments.empty(), ());
|
||||
CHECK_LESS_OR_EQUAL(pointIndex, segments.size(),
|
||||
("Point with index", pointIndex, "does not exist in route with size", segments.size()));
|
||||
if (pointIndex == segments.size())
|
||||
return GetJunction(segments[pointIndex - 1], true /* front */);
|
||||
return GetJunction(segments[pointIndex], false);
|
||||
}
|
||||
|
||||
ms::LatLon const & IndexGraphStarter::GetPoint(Segment const & segment, bool front) const
|
||||
{
|
||||
return GetJunction(segment, front).GetLatLon();
|
||||
}
|
||||
|
||||
bool IndexGraphStarter::IsRoutingOptionsGood(Segment const & segment) const
|
||||
{
|
||||
return m_graph.IsRoutingOptionsGood(segment);
|
||||
}
|
||||
|
||||
RoutingOptions IndexGraphStarter::GetRoutingOptions(Segment const & segment) const
|
||||
{
|
||||
if (segment.IsRealSegment())
|
||||
return m_graph.GetRoutingOptions(segment);
|
||||
|
||||
Segment real;
|
||||
if (!m_fake.FindReal(segment, real))
|
||||
return {};
|
||||
|
||||
return m_graph.GetRoutingOptions(real);
|
||||
}
|
||||
|
||||
set<NumMwmId> IndexGraphStarter::GetMwms() const
|
||||
{
|
||||
set<NumMwmId> mwms;
|
||||
mwms.insert(m_start.m_mwmIds.begin(), m_start.m_mwmIds.end());
|
||||
mwms.insert(m_finish.m_mwmIds.begin(), m_finish.m_mwmIds.end());
|
||||
return mwms;
|
||||
}
|
||||
|
||||
bool IndexGraphStarter::CheckLength(RouteWeight const & weight)
|
||||
{
|
||||
// Avoid transit through living neighbourhoods.
|
||||
// Assume that start or end belongs to the living neighbourhood (HasNoPassThroughAllowed),
|
||||
// if at least one of the projections is non-pass-through road (service, living_street).
|
||||
// Interesting example here: https://github.com/organicmaps/organicmaps/issues/1008
|
||||
|
||||
// Allow pass-through <-> non-pass-through zone changes if start or end belongs
|
||||
// to no-pass-through zone (have to leave or arrive at living neighbourhood).
|
||||
|
||||
/// @todo By VNG: Additional 2 changes to relax this restriction. Or should enchance this mechanics to allow
|
||||
/// pass through say living_street into lower class road (track/road/unclassified?).
|
||||
/// This additional penalty will be included in RouteWeight::GetIntegratedWeight().
|
||||
/// @see RussiaMoscowNotCrossingTollRoadTest
|
||||
|
||||
int8_t const changesAllowed =
|
||||
2 + (HasNoPassThroughAllowed(m_start) ? 1 : 0) + (HasNoPassThroughAllowed(m_finish) ? 1 : 0);
|
||||
|
||||
return weight.GetNumPassThroughChanges() <= changesAllowed && m_graph.CheckLength(weight, m_startToFinishDistanceM);
|
||||
}
|
||||
|
||||
void IndexGraphStarter::GetEdgesList(astar::VertexData<Vertex, Weight> const & vertexData, bool isOutgoing,
|
||||
bool useAccessConditional, EdgeListT & edges) const
|
||||
{
|
||||
edges.clear();
|
||||
CHECK_NOT_EQUAL(m_graph.GetMode(), WorldGraphMode::LeapsOnly, ());
|
||||
|
||||
auto const & segment = vertexData.m_vertex;
|
||||
// Weight used only when isOutgoing = false for passing to m_guides and placing to |edges|.
|
||||
RouteWeight ingoingSegmentWeight;
|
||||
if (!isOutgoing)
|
||||
ingoingSegmentWeight = CalcSegmentWeight(segment, EdgeEstimator::Purpose::Weight);
|
||||
|
||||
if (IsFakeSegment(segment))
|
||||
{
|
||||
Segment real;
|
||||
if (m_fake.FindReal(segment, real))
|
||||
{
|
||||
bool const haveSameFront = GetJunction(segment, true /* front */) == GetJunction(real, true);
|
||||
bool const haveSameBack = GetJunction(segment, false /* front */) == GetJunction(real, false);
|
||||
if ((isOutgoing && haveSameFront) || (!isOutgoing && haveSameBack))
|
||||
{
|
||||
if (IsGuidesSegment(real))
|
||||
{
|
||||
m_guides.GetEdgeList(real, isOutgoing, edges, ingoingSegmentWeight);
|
||||
}
|
||||
else if (IsRegionsGraphMode())
|
||||
{
|
||||
m_regionsGraph->GetEdgeList(real, isOutgoing, edges, GetJunction(segment, true /* front */).GetLatLon());
|
||||
}
|
||||
else
|
||||
{
|
||||
astar::VertexData const replacedFakeSegment(real, vertexData.m_realDistance);
|
||||
m_graph.GetEdgeList(replacedFakeSegment, isOutgoing, true /* useRoutingOptions */, useAccessConditional,
|
||||
edges);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (auto const & s : m_fake.GetEdges(segment, isOutgoing))
|
||||
edges.emplace_back(s, isOutgoing ? CalcSegmentWeight(s, EdgeEstimator::Purpose::Weight) : ingoingSegmentWeight);
|
||||
}
|
||||
else if (IsGuidesSegment(segment))
|
||||
{
|
||||
m_guides.GetEdgeList(segment, isOutgoing, edges, ingoingSegmentWeight);
|
||||
}
|
||||
else if (IsRegionsGraphMode())
|
||||
{
|
||||
m_regionsGraph->GetEdgeList(segment, isOutgoing, edges, GetJunction(segment, true /* front */).GetLatLon());
|
||||
}
|
||||
else
|
||||
{
|
||||
m_graph.GetEdgeList(vertexData, isOutgoing, true /* useRoutingOptions */, useAccessConditional, edges);
|
||||
}
|
||||
|
||||
AddFakeEdges(segment, isOutgoing, edges);
|
||||
}
|
||||
|
||||
RouteWeight IndexGraphStarter::CalcGuidesSegmentWeight(Segment const & segment, EdgeEstimator::Purpose purpose) const
|
||||
{
|
||||
if (purpose == EdgeEstimator::Purpose::Weight)
|
||||
return m_guides.CalcSegmentWeight(segment);
|
||||
|
||||
auto [from, to] = m_guides.GetFromTo(segment);
|
||||
return m_graph.CalcOffroadWeight(from.GetLatLon(), to.GetLatLon(), purpose);
|
||||
}
|
||||
|
||||
RouteWeight IndexGraphStarter::CalcSegmentWeight(Segment const & segment, EdgeEstimator::Purpose purpose) const
|
||||
{
|
||||
if (IsGuidesSegment(segment))
|
||||
return CalcGuidesSegmentWeight(segment, purpose);
|
||||
|
||||
if (IsRegionsGraphMode() && !IsFakeSegment(segment))
|
||||
return m_regionsGraph->CalcSegmentWeight(segment);
|
||||
|
||||
if (!IsFakeSegment(segment))
|
||||
return m_graph.CalcSegmentWeight(segment, purpose);
|
||||
|
||||
auto const & vertex = m_fake.GetVertex(segment);
|
||||
Segment real;
|
||||
if (m_fake.FindReal(segment, real))
|
||||
{
|
||||
double const partLen = ms::DistanceOnEarth(vertex.GetPointFrom(), vertex.GetPointTo());
|
||||
|
||||
if (IsRegionsGraphMode())
|
||||
return RouteWeight(partLen);
|
||||
|
||||
double const fullLen = ms::DistanceOnEarth(GetPoint(real, false), GetPoint(real, true));
|
||||
// Note 1. |fullLen| == 0.0 in case of Segment(s) with the same ends.
|
||||
// Note 2. There is the following logic behind |return 0.0 * m_graph.CalcSegmentWeight(real, ...);|:
|
||||
// it's necessary to return a instance of the structure |RouteWeight| with zero wight.
|
||||
// Theoretically it may be differ from |RouteWeight(0)| because some road access block
|
||||
// may be kept in it and it is up to |RouteWeight| to know how to multiply by zero.
|
||||
|
||||
Weight weight;
|
||||
if (IsGuidesSegment(real))
|
||||
weight = CalcGuidesSegmentWeight(real, purpose);
|
||||
else
|
||||
weight = m_graph.CalcSegmentWeight(real, purpose);
|
||||
if (fullLen == 0.0)
|
||||
return 0.0 * weight;
|
||||
|
||||
return (partLen / fullLen) * weight;
|
||||
}
|
||||
|
||||
return m_graph.CalcOffroadWeight(vertex.GetPointFrom(), vertex.GetPointTo(), purpose);
|
||||
}
|
||||
|
||||
double IndexGraphStarter::CalculateETA(Segment const & from, Segment const & to) const
|
||||
{
|
||||
// We don't distinguish fake segment weight and fake segment transit time.
|
||||
if (IsFakeSegment(to))
|
||||
return CalcSegmentWeight(to, EdgeEstimator::Purpose::ETA).GetWeight();
|
||||
|
||||
if (IsFakeSegment(from))
|
||||
return CalculateETAWithoutPenalty(to);
|
||||
|
||||
if (IsGuidesSegment(from) || IsGuidesSegment(to))
|
||||
{
|
||||
double res = 0.0;
|
||||
if (IsGuidesSegment(from))
|
||||
res += CalcGuidesSegmentWeight(from, EdgeEstimator::Purpose::ETA).GetWeight();
|
||||
else
|
||||
res += CalculateETAWithoutPenalty(from);
|
||||
|
||||
if (IsGuidesSegment(to))
|
||||
res += CalcGuidesSegmentWeight(to, EdgeEstimator::Purpose::ETA).GetWeight();
|
||||
else
|
||||
res += CalculateETAWithoutPenalty(to);
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
if (IsRegionsGraphMode())
|
||||
return m_regionsGraph->CalcSegmentWeight(from).GetWeight() + m_regionsGraph->CalcSegmentWeight(to).GetWeight();
|
||||
|
||||
return m_graph.CalculateETA(from, to);
|
||||
}
|
||||
|
||||
double IndexGraphStarter::CalculateETAWithoutPenalty(Segment const & segment) const
|
||||
{
|
||||
if (IsFakeSegment(segment))
|
||||
return CalcSegmentWeight(segment, EdgeEstimator::Purpose::ETA).GetWeight();
|
||||
|
||||
if (IsGuidesSegment(segment))
|
||||
return CalcGuidesSegmentWeight(segment, EdgeEstimator::Purpose::ETA).GetWeight();
|
||||
|
||||
if (IsRegionsGraphMode())
|
||||
return m_regionsGraph->CalcSegmentWeight(segment).GetWeight();
|
||||
|
||||
return m_graph.CalculateETAWithoutPenalty(segment);
|
||||
}
|
||||
|
||||
void IndexGraphStarter::AddEnding(FakeEnding const & thisEnding)
|
||||
{
|
||||
Segment const dummy = Segment();
|
||||
|
||||
map<Segment, vector<LatLonWithAltitude>> otherSegments;
|
||||
for (auto const & ending : m_otherEndings)
|
||||
{
|
||||
for (auto const & p : ending.m_projections)
|
||||
{
|
||||
otherSegments[p.m_segment].push_back(p.m_junction);
|
||||
// We use |otherEnding| to generate proper fake edges in case both endings have projections
|
||||
// to the same segment. Direction of p.m_segment does not matter.
|
||||
otherSegments[p.m_segment.GetReversed()].push_back(p.m_junction);
|
||||
}
|
||||
}
|
||||
|
||||
// Add pure fake vertex
|
||||
auto const fakeSegment = GetFakeSegmentAndIncr();
|
||||
FakeVertex fakeVertex(kFakeNumMwmId, thisEnding.m_originJunction, thisEnding.m_originJunction,
|
||||
FakeVertex::Type::PureFake);
|
||||
m_fake.AddStandaloneVertex(fakeSegment, fakeVertex);
|
||||
for (bool isStart : {true, false})
|
||||
{
|
||||
for (auto const & projection : thisEnding.m_projections)
|
||||
{
|
||||
// Add projection edges
|
||||
auto const projectionSegment = GetFakeSegmentAndIncr();
|
||||
FakeVertex projectionVertex(
|
||||
projection.m_segment.GetMwmId(), isStart ? thisEnding.m_originJunction : projection.m_junction,
|
||||
isStart ? projection.m_junction : thisEnding.m_originJunction, FakeVertex::Type::PureFake);
|
||||
m_fake.AddVertex(fakeSegment, projectionSegment, projectionVertex, isStart /* isOutgoing */,
|
||||
false /* isPartOfReal */, dummy /* realSegment */);
|
||||
|
||||
// Add fake parts of real
|
||||
auto frontJunction = projection.m_segmentFront;
|
||||
auto backJunction = projection.m_segmentBack;
|
||||
|
||||
// Check whether we have projections to same real segment from both endings.
|
||||
auto const it = otherSegments.find(projection.m_segment);
|
||||
if (it != otherSegments.end())
|
||||
{
|
||||
ASSERT(!it->second.empty(), ());
|
||||
|
||||
LatLonWithAltitude otherJunction;
|
||||
double distBackToOther = 1.0E8;
|
||||
for (auto const & coord : it->second)
|
||||
{
|
||||
double const curDist = ms::DistanceOnEarth(backJunction.GetLatLon(), coord.GetLatLon());
|
||||
if (curDist < distBackToOther)
|
||||
{
|
||||
distBackToOther = curDist;
|
||||
otherJunction = coord;
|
||||
}
|
||||
}
|
||||
|
||||
double const distBackToThis = ms::DistanceOnEarth(backJunction.GetLatLon(), projection.m_junction.GetLatLon());
|
||||
|
||||
if (distBackToThis < distBackToOther)
|
||||
frontJunction = otherJunction;
|
||||
else if (distBackToOther < distBackToThis)
|
||||
backJunction = otherJunction;
|
||||
else
|
||||
frontJunction = backJunction = otherJunction;
|
||||
}
|
||||
|
||||
FakeVertex forwardPartOfReal(projection.m_segment.GetMwmId(), isStart ? projection.m_junction : backJunction,
|
||||
isStart ? frontJunction : projection.m_junction, FakeVertex::Type::PartOfReal);
|
||||
Segment fakeForwardSegment;
|
||||
if (!m_fake.FindSegment(forwardPartOfReal, fakeForwardSegment))
|
||||
fakeForwardSegment = GetFakeSegmentAndIncr();
|
||||
m_fake.AddVertex(projectionSegment, fakeForwardSegment, forwardPartOfReal, isStart /* isOutgoing */,
|
||||
true /* isPartOfReal */, projection.m_segment);
|
||||
|
||||
if (!projection.m_isOneWay)
|
||||
{
|
||||
auto const backwardSegment = projection.m_segment.GetReversed();
|
||||
FakeVertex backwardPartOfReal(backwardSegment.GetMwmId(), isStart ? projection.m_junction : frontJunction,
|
||||
isStart ? backJunction : projection.m_junction, FakeVertex::Type::PartOfReal);
|
||||
Segment fakeBackwardSegment;
|
||||
if (!m_fake.FindSegment(backwardPartOfReal, fakeBackwardSegment))
|
||||
fakeBackwardSegment = GetFakeSegmentAndIncr();
|
||||
m_fake.AddVertex(projectionSegment, fakeBackwardSegment, backwardPartOfReal, isStart /* isOutgoing */,
|
||||
true /* isPartOfReal */, backwardSegment);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
m_otherEndings.push_back(thisEnding);
|
||||
}
|
||||
|
||||
void IndexGraphStarter::AddEnding(FakeEnding const & thisEnding, FakeEnding const & otherEnding, bool isStart,
|
||||
bool strictForward)
|
||||
{
|
||||
Segment const dummy = Segment();
|
||||
|
||||
map<Segment, LatLonWithAltitude> otherSegments;
|
||||
for (auto const & p : otherEnding.m_projections)
|
||||
{
|
||||
otherSegments[p.m_segment] = p.m_junction;
|
||||
// We use |otherEnding| to generate proper fake edges in case both endings have projections
|
||||
// to the same segment. Direction of p.m_segment does not matter.
|
||||
otherSegments[p.m_segment.GetReversed()] = p.m_junction;
|
||||
}
|
||||
|
||||
// Add pure fake vertex
|
||||
auto const fakeSegment = GetFakeSegmentAndIncr();
|
||||
FakeVertex fakeVertex(kFakeNumMwmId, thisEnding.m_originJunction, thisEnding.m_originJunction,
|
||||
FakeVertex::Type::PureFake);
|
||||
m_fake.AddStandaloneVertex(fakeSegment, fakeVertex);
|
||||
for (auto const & projection : thisEnding.m_projections)
|
||||
{
|
||||
if (isStart)
|
||||
m_start.m_real.insert(projection.m_segment);
|
||||
else
|
||||
m_finish.m_real.insert(projection.m_segment);
|
||||
|
||||
// Add projection edges
|
||||
auto const projectionSegment = GetFakeSegmentAndIncr();
|
||||
FakeVertex projectionVertex(
|
||||
projection.m_segment.GetMwmId(), isStart ? thisEnding.m_originJunction : projection.m_junction,
|
||||
isStart ? projection.m_junction : thisEnding.m_originJunction, FakeVertex::Type::PureFake);
|
||||
m_fake.AddVertex(fakeSegment, projectionSegment, projectionVertex, isStart /* isOutgoing */,
|
||||
false /* isPartOfReal */, dummy /* realSegment */);
|
||||
|
||||
// Add fake parts of real
|
||||
auto frontJunction = projection.m_segmentFront;
|
||||
auto backJunction = projection.m_segmentBack;
|
||||
|
||||
// Check whether we have projections to same real segment from both endings.
|
||||
auto const it = otherSegments.find(projection.m_segment);
|
||||
if (it != otherSegments.end())
|
||||
{
|
||||
auto const & otherJunction = it->second;
|
||||
auto const distBackToThis = ms::DistanceOnEarth(backJunction.GetLatLon(), projection.m_junction.GetLatLon());
|
||||
auto const distBackToOther = ms::DistanceOnEarth(backJunction.GetLatLon(), otherJunction.GetLatLon());
|
||||
if (distBackToThis < distBackToOther)
|
||||
frontJunction = otherJunction;
|
||||
else if (distBackToOther < distBackToThis)
|
||||
backJunction = otherJunction;
|
||||
else
|
||||
frontJunction = backJunction = otherJunction;
|
||||
}
|
||||
|
||||
FakeVertex forwardPartOfReal(projection.m_segment.GetMwmId(), isStart ? projection.m_junction : backJunction,
|
||||
isStart ? frontJunction : projection.m_junction, FakeVertex::Type::PartOfReal);
|
||||
Segment fakeForwardSegment;
|
||||
if (!m_fake.FindSegment(forwardPartOfReal, fakeForwardSegment))
|
||||
fakeForwardSegment = GetFakeSegmentAndIncr();
|
||||
m_fake.AddVertex(projectionSegment, fakeForwardSegment, forwardPartOfReal, isStart /* isOutgoing */,
|
||||
true /* isPartOfReal */, projection.m_segment);
|
||||
|
||||
if (!strictForward && !projection.m_isOneWay)
|
||||
{
|
||||
auto const backwardSegment = projection.m_segment.GetReversed();
|
||||
FakeVertex backwardPartOfReal(backwardSegment.GetMwmId(), isStart ? projection.m_junction : frontJunction,
|
||||
isStart ? backJunction : projection.m_junction, FakeVertex::Type::PartOfReal);
|
||||
Segment fakeBackwardSegment;
|
||||
if (!m_fake.FindSegment(backwardPartOfReal, fakeBackwardSegment))
|
||||
fakeBackwardSegment = GetFakeSegmentAndIncr();
|
||||
m_fake.AddVertex(projectionSegment, fakeBackwardSegment, backwardPartOfReal, isStart /* isOutgoing */,
|
||||
true /* isPartOfReal */, backwardSegment);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void IndexGraphStarter::ConnectLoopToGuideSegments(FakeVertex const & loop, Segment const & realSegment,
|
||||
LatLonWithAltitude realFrom, LatLonWithAltitude realTo,
|
||||
vector<pair<FakeVertex, Segment>> const & partsOfReal)
|
||||
{
|
||||
m_fake.ConnectLoopToGuideSegments(loop, realSegment, realFrom, realTo, partsOfReal);
|
||||
}
|
||||
|
||||
HighwayCategory IndexGraphStarter::GetHighwayCategory(Segment seg) const
|
||||
{
|
||||
if (IsFakeSegment(seg))
|
||||
{
|
||||
Segment real;
|
||||
if (m_fake.FindReal(seg, real))
|
||||
seg = real;
|
||||
else
|
||||
return HighwayCategory::Unknown;
|
||||
}
|
||||
|
||||
auto const hwType = m_graph.GetIndexGraph(seg.GetMwmId()).GetRoadGeometry(seg.GetFeatureId()).GetHighwayType();
|
||||
if (!hwType)
|
||||
return HighwayCategory::Unknown;
|
||||
|
||||
switch (*hwType)
|
||||
{
|
||||
case HighwayType::HighwayMotorway:
|
||||
case HighwayType::HighwayMotorwayLink:
|
||||
case HighwayType::HighwayTrunk:
|
||||
case HighwayType::HighwayTrunkLink: return HighwayCategory::Major;
|
||||
case HighwayType::HighwayPrimary:
|
||||
case HighwayType::HighwayPrimaryLink: return HighwayCategory::Primary;
|
||||
case HighwayType::HighwaySecondary:
|
||||
case HighwayType::HighwaySecondaryLink:
|
||||
case HighwayType::HighwayTertiary:
|
||||
case HighwayType::HighwayTertiaryLink: return HighwayCategory::Usual;
|
||||
case HighwayType::RouteFerry:
|
||||
case HighwayType::RouteShuttleTrain: return HighwayCategory::Transit;
|
||||
default: return HighwayCategory::Minor;
|
||||
}
|
||||
}
|
||||
|
||||
void IndexGraphStarter::AddStart(FakeEnding const & startEnding, FakeEnding const & finishEnding, bool strictForward)
|
||||
{
|
||||
AddEnding(startEnding, finishEnding, true /* isStart */, strictForward);
|
||||
m_start.FillMwmIds();
|
||||
}
|
||||
|
||||
void IndexGraphStarter::AddFinish(FakeEnding const & finishEnding, FakeEnding const & startEnding)
|
||||
{
|
||||
AddEnding(finishEnding, startEnding, false /* isStart */, false /* strictForward */);
|
||||
m_finish.FillMwmIds();
|
||||
}
|
||||
|
||||
void IndexGraphStarter::AddFakeEdges(Segment const & segment, bool isOutgoing, EdgeListT & edges) const
|
||||
{
|
||||
EdgeListT fakeEdges;
|
||||
for (auto const & edge : edges)
|
||||
{
|
||||
for (auto const & s : m_fake.GetFake(edge.GetTarget()))
|
||||
{
|
||||
// |segment| |s|
|
||||
// *------------>*----------->
|
||||
bool const sIsOutgoing = GetJunction(segment, true /* front */) == GetJunction(s, false /* front */);
|
||||
|
||||
// |s| |segment|
|
||||
// *------------>*----------->
|
||||
bool const sIsIngoing = GetJunction(s, true /* front */) == GetJunction(segment, false /* front */);
|
||||
|
||||
if ((isOutgoing && sIsOutgoing) || (!isOutgoing && sIsIngoing))
|
||||
{
|
||||
// For ingoing edges we use source weight which is the same for |s| and for |edge| and is
|
||||
// already calculated.
|
||||
fakeEdges.emplace_back(s, isOutgoing ? CalcSegmentWeight(s, EdgeEstimator::Purpose::Weight) : edge.GetWeight());
|
||||
}
|
||||
}
|
||||
}
|
||||
edges.append(fakeEdges.begin(), fakeEdges.end());
|
||||
}
|
||||
|
||||
bool IndexGraphStarter::HasNoPassThroughAllowed(Ending const & ending) const
|
||||
{
|
||||
if (IsRegionsGraphMode())
|
||||
return false;
|
||||
|
||||
return any_of(ending.m_real.cbegin(), ending.m_real.cend(), [this](Segment const & s)
|
||||
{
|
||||
if (IsGuidesSegment(s))
|
||||
return false;
|
||||
|
||||
return !m_graph.IsPassThroughAllowed(s.GetMwmId(), s.GetFeatureId());
|
||||
});
|
||||
}
|
||||
|
||||
Segment IndexGraphStarter::GetFakeSegmentAndIncr()
|
||||
{
|
||||
CHECK_LESS(m_fakeNumerationStart, numeric_limits<uint32_t>::max(), ());
|
||||
return GetFakeSegment(m_fakeNumerationStart++);
|
||||
}
|
||||
|
||||
RouteWeight IndexGraphStarter::GetAStarWeightEpsilon()
|
||||
{
|
||||
// Epsilon for double calculations.
|
||||
auto constexpr kEps = RouteWeight(1e-6);
|
||||
// We store points with |kMwmPointAccuracy|. In case of cross mwm routing we couldn't
|
||||
// distinguish the point geometry changing in |kMwmPointAccuracy| radius of the same segments from
|
||||
// mwms with different versions. So let's use such epsilon to maintain the A* invariant.
|
||||
return kEps + m_graph.HeuristicCostEstimate(ms::LatLon(0.0, 0.0), ms::LatLon(0.0, kMwmPointAccuracy));
|
||||
}
|
||||
} // namespace routing
|
||||
245
libs/routing/index_graph_starter.hpp
Normal file
245
libs/routing/index_graph_starter.hpp
Normal file
|
|
@ -0,0 +1,245 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/base/astar_graph.hpp"
|
||||
#include "routing/base/astar_vertex_data.hpp"
|
||||
#include "routing/fake_ending.hpp"
|
||||
#include "routing/fake_feature_ids.hpp"
|
||||
#include "routing/fake_graph.hpp"
|
||||
#include "routing/fake_vertex.hpp"
|
||||
#include "routing/guides_graph.hpp"
|
||||
#include "routing/index_graph.hpp"
|
||||
#include "routing/latlon_with_altitude.hpp"
|
||||
#include "routing/route_weight.hpp"
|
||||
#include "routing/segment.hpp"
|
||||
#include "routing/world_graph.hpp"
|
||||
|
||||
#include "routing_common/num_mwm_id.hpp"
|
||||
|
||||
#include <memory>
|
||||
#include <set>
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
class FakeEdgesContainer;
|
||||
class RegionsSparseGraph;
|
||||
|
||||
// Group highway types on categories (classes) to use in leaps candidates filtering.
|
||||
enum class HighwayCategory : uint8_t
|
||||
{
|
||||
// Do not change order!
|
||||
Major,
|
||||
Primary,
|
||||
Usual,
|
||||
Minor,
|
||||
Transit,
|
||||
Unknown
|
||||
};
|
||||
|
||||
// IndexGraphStarter adds fake start and finish vertices for AStarAlgorithm.
|
||||
class IndexGraphStarter : public AStarGraph<IndexGraph::Vertex, IndexGraph::Edge, IndexGraph::Weight>
|
||||
{
|
||||
public:
|
||||
template <typename VertexType>
|
||||
using Parents = IndexGraph::Parents<VertexType>;
|
||||
|
||||
using JointEdgeListT = IndexGraph::JointEdgeListT;
|
||||
using WeightListT = IndexGraph::WeightListT;
|
||||
|
||||
friend class FakeEdgesContainer;
|
||||
|
||||
static void CheckValidRoute(std::vector<Segment> const & segments);
|
||||
|
||||
static bool IsFakeSegment(Segment const & segment) { return segment.IsFakeCreated(); }
|
||||
|
||||
static bool IsGuidesSegment(Segment const & segment)
|
||||
{
|
||||
return FakeFeatureIds::IsGuidesFeature(segment.GetFeatureId());
|
||||
}
|
||||
|
||||
// strictForward flag specifies which parts of real segment should be placed from the start
|
||||
// vertex. true: place exactly one fake edge to the m_segment indicated with m_forward. false:
|
||||
// place two fake edges to the m_segment with both directions.
|
||||
IndexGraphStarter(FakeEnding const & startEnding, FakeEnding const & finishEnding, uint32_t fakeNumerationStart,
|
||||
bool strictForward, WorldGraph & graph);
|
||||
|
||||
void Append(FakeEdgesContainer const & container);
|
||||
|
||||
void SetGuides(GuidesGraph const & guides);
|
||||
|
||||
void SetRegionsGraphMode(std::shared_ptr<RegionsSparseGraph> regionsSparseGraph);
|
||||
bool IsRegionsGraphMode() const { return m_regionsGraph != nullptr; }
|
||||
|
||||
WorldGraph & GetGraph() const { return m_graph; }
|
||||
WorldGraphMode GetMode() const { return m_graph.GetMode(); }
|
||||
LatLonWithAltitude const & GetStartJunction() const;
|
||||
LatLonWithAltitude const & GetFinishJunction() const;
|
||||
Segment GetStartSegment() const { return GetFakeSegment(m_start.m_id); }
|
||||
Segment GetFinishSegment() const { return GetFakeSegment(m_finish.m_id); }
|
||||
// If segment is real returns true and does not modify segment.
|
||||
// If segment is part of real converts it to real and returns true.
|
||||
// Otherwise returns false and does not modify segment.
|
||||
bool ConvertToReal(Segment & segment) const;
|
||||
LatLonWithAltitude const & GetJunction(Segment const & segment, bool front) const;
|
||||
LatLonWithAltitude const & GetRouteJunction(std::vector<Segment> const & route, size_t pointIndex) const;
|
||||
ms::LatLon const & GetPoint(Segment const & segment, bool front) const;
|
||||
|
||||
bool IsRoutingOptionsGood(Segment const & segment) const;
|
||||
RoutingOptions GetRoutingOptions(Segment const & segment) const;
|
||||
|
||||
uint32_t GetNumFakeSegments() const { return base::checked_cast<uint32_t>(m_fake.GetSize()); }
|
||||
|
||||
std::set<NumMwmId> GetMwms() const;
|
||||
std::set<NumMwmId> const & GetStartMwms() const { return m_start.m_mwmIds; }
|
||||
std::set<NumMwmId> const & GetFinishMwms() const { return m_finish.m_mwmIds; }
|
||||
|
||||
// Checks whether |weight| meets non-pass-through crossing restrictions according to placement of
|
||||
// start and finish in pass-through/non-pass-through area and number of non-pass-through crosses.
|
||||
bool CheckLength(RouteWeight const & weight);
|
||||
|
||||
void GetEdgeList(astar::VertexData<JointSegment, Weight> const & parentVertexData, Segment const & segment,
|
||||
bool isOutgoing, JointEdgeListT & edges, WeightListT & parentWeights) const
|
||||
{
|
||||
return m_graph.GetEdgeList(parentVertexData, segment, isOutgoing, true /* useAccessConditional */, edges,
|
||||
parentWeights);
|
||||
}
|
||||
|
||||
// AStarGraph overridings:
|
||||
// @{
|
||||
void GetOutgoingEdgesList(astar::VertexData<Vertex, Weight> const & vertexData, EdgeListT & edges) override
|
||||
{
|
||||
GetEdgesList(vertexData, true /* isOutgoing */, true /* useAccessConditional */, edges);
|
||||
}
|
||||
|
||||
void GetIngoingEdgesList(astar::VertexData<Vertex, Weight> const & vertexData, EdgeListT & edges) override
|
||||
{
|
||||
GetEdgesList(vertexData, false /* isOutgoing */, true /* useAccessConditional */, edges);
|
||||
}
|
||||
|
||||
RouteWeight HeuristicCostEstimate(Vertex const & from, Vertex const & to) override
|
||||
{
|
||||
return m_graph.HeuristicCostEstimate(GetPoint(from, true /* front */), GetPoint(to, true /* front */));
|
||||
}
|
||||
|
||||
void SetAStarParents(bool forward, Parents<Segment> & parents) override { m_graph.SetAStarParents(forward, parents); }
|
||||
|
||||
void DropAStarParents() override { m_graph.DropAStarParents(); }
|
||||
|
||||
bool AreWavesConnectible(Parents<Segment> & forwardParents, Vertex const & commonVertex,
|
||||
Parents<Segment> & backwardParents) override
|
||||
{
|
||||
return m_graph.AreWavesConnectible(forwardParents, commonVertex, backwardParents);
|
||||
}
|
||||
|
||||
RouteWeight GetAStarWeightEpsilon() override;
|
||||
// @}
|
||||
|
||||
void GetEdgesList(Vertex const & vertex, bool isOutgoing, EdgeListT & edges) const
|
||||
{
|
||||
GetEdgesList({vertex, Weight(0.0)}, isOutgoing, false /* useAccessConditional */, edges);
|
||||
}
|
||||
|
||||
RouteWeight HeuristicCostEstimate(Vertex const & from, ms::LatLon const & to) const
|
||||
{
|
||||
return m_graph.HeuristicCostEstimate(GetPoint(from, true /* front */), to);
|
||||
}
|
||||
|
||||
RouteWeight CalcSegmentWeight(Segment const & segment, EdgeEstimator::Purpose purpose) const;
|
||||
RouteWeight CalcGuidesSegmentWeight(Segment const & segment, EdgeEstimator::Purpose purpose) const;
|
||||
double CalculateETA(Segment const & from, Segment const & to) const;
|
||||
double CalculateETAWithoutPenalty(Segment const & segment) const;
|
||||
|
||||
/// @name For compatibility with IndexGraphStarterJoints.
|
||||
/// @{
|
||||
void SetAStarParents(bool forward, Parents<JointSegment> & parents) { m_graph.SetAStarParents(forward, parents); }
|
||||
|
||||
bool AreWavesConnectible(Parents<JointSegment> & forwardParents, JointSegment const & commonVertex,
|
||||
Parents<JointSegment> & backwardParents,
|
||||
WorldGraph::FakeConverterT const & fakeFeatureConverter)
|
||||
{
|
||||
return m_graph.AreWavesConnectible(forwardParents, commonVertex, backwardParents, fakeFeatureConverter);
|
||||
}
|
||||
|
||||
bool IsJoint(Segment const & segment, bool fromStart)
|
||||
{
|
||||
return GetGraph().GetIndexGraph(segment.GetMwmId()).IsJoint(segment.GetRoadPoint(fromStart));
|
||||
}
|
||||
|
||||
bool IsJointOrEnd(Segment const & segment, bool fromStart)
|
||||
{
|
||||
return GetGraph().GetIndexGraph(segment.GetMwmId()).IsJointOrEnd(segment, fromStart);
|
||||
}
|
||||
|
||||
RouteWeight GetCrossBorderPenalty(NumMwmId mwmId1, NumMwmId mwmId2)
|
||||
{
|
||||
return GetGraph().GetCrossBorderPenalty(mwmId1, mwmId2);
|
||||
}
|
||||
/// @}
|
||||
|
||||
// Start or finish ending information.
|
||||
struct Ending
|
||||
{
|
||||
void FillMwmIds();
|
||||
// Fake segment id.
|
||||
uint32_t m_id = 0;
|
||||
// Real segments connected to the ending.
|
||||
std::set<Segment> m_real;
|
||||
// Mwm ids of connected segments to the ending.
|
||||
std::set<NumMwmId> m_mwmIds;
|
||||
};
|
||||
|
||||
uint32_t GetFakeNumerationStart() const { return m_fakeNumerationStart; }
|
||||
|
||||
// Creates fake edges for guides fake ending and adds them to the fake graph.
|
||||
void AddEnding(FakeEnding const & thisEnding);
|
||||
|
||||
void ConnectLoopToGuideSegments(FakeVertex const & loop, Segment const & realSegment, LatLonWithAltitude realFrom,
|
||||
LatLonWithAltitude realTo,
|
||||
std::vector<std::pair<FakeVertex, Segment>> const & partsOfReal);
|
||||
|
||||
HighwayCategory GetHighwayCategory(Segment seg) const;
|
||||
|
||||
private:
|
||||
// Creates fake edges for fake ending and adds it to fake graph. |otherEnding| is used to
|
||||
// generate proper fake edges in case both endings have projections to the same segment.
|
||||
void AddEnding(FakeEnding const & thisEnding, FakeEnding const & otherEnding, bool isStart, bool strictForward);
|
||||
|
||||
static Segment GetFakeSegment(uint32_t segmentIdx)
|
||||
{
|
||||
// We currently ignore |isForward| and use FakeGraph to get ingoing/outgoing.
|
||||
// But all fake segments are oneway and placement of segment head and tail
|
||||
// correspond forward direction.
|
||||
return Segment(kFakeNumMwmId, FakeFeatureIds::kIndexGraphStarterId, segmentIdx, true /* isForward */);
|
||||
}
|
||||
|
||||
Segment GetFakeSegmentAndIncr();
|
||||
|
||||
void GetEdgesList(astar::VertexData<Vertex, Weight> const & vertexData, bool isOutgoing, bool useAccessConditional,
|
||||
EdgeListT & edges) const;
|
||||
|
||||
void AddStart(FakeEnding const & startEnding, FakeEnding const & finishEnding, bool strictForward);
|
||||
void AddFinish(FakeEnding const & finishEnding, FakeEnding const & startEnding);
|
||||
|
||||
// Adds fake edges of type PartOfReal which correspond real edges from |edges| and are connected
|
||||
// to |segment|
|
||||
void AddFakeEdges(Segment const & segment, bool isOutgoing, EdgeListT & edges) const;
|
||||
|
||||
// Checks whether ending belongs to non-pass-through zone (service, living street, etc).
|
||||
bool HasNoPassThroughAllowed(Ending const & ending) const;
|
||||
|
||||
WorldGraph & m_graph;
|
||||
// Start segment id
|
||||
Ending m_start;
|
||||
// Finish segment id
|
||||
Ending m_finish;
|
||||
double m_startToFinishDistanceM;
|
||||
FakeGraph m_fake;
|
||||
GuidesGraph m_guides;
|
||||
uint32_t m_fakeNumerationStart;
|
||||
|
||||
std::vector<FakeEnding> m_otherEndings;
|
||||
|
||||
// Field for routing in mode for finding all route mwms.
|
||||
std::shared_ptr<RegionsSparseGraph> m_regionsGraph = nullptr;
|
||||
};
|
||||
} // namespace routing
|
||||
651
libs/routing/index_graph_starter_joints.hpp
Normal file
651
libs/routing/index_graph_starter_joints.hpp
Normal file
|
|
@ -0,0 +1,651 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/base/astar_graph.hpp"
|
||||
#include "routing/base/astar_vertex_data.hpp"
|
||||
|
||||
#include "routing/joint_segment.hpp"
|
||||
#include "routing/segment.hpp"
|
||||
|
||||
#include "geometry/latlon.hpp"
|
||||
|
||||
#include "base/assert.hpp"
|
||||
|
||||
#include "3party/skarupke/bytell_hash_map.hpp" // needed despite of IDE warning
|
||||
|
||||
#include <algorithm>
|
||||
#include <map>
|
||||
#include <optional>
|
||||
#include <queue>
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
enum class WorldGraphMode;
|
||||
|
||||
template <typename Graph>
|
||||
class IndexGraphStarterJoints : public AStarGraph<JointSegment, JointEdge, RouteWeight>
|
||||
{
|
||||
public:
|
||||
explicit IndexGraphStarterJoints(Graph & graph) : m_graph(graph) {}
|
||||
IndexGraphStarterJoints(Graph & graph, Segment const & startSegment, Segment const & endSegment);
|
||||
|
||||
IndexGraphStarterJoints(Graph & graph, Segment const & startSegment);
|
||||
|
||||
void Init(Segment const & startSegment, Segment const & endSegment);
|
||||
|
||||
ms::LatLon const & GetPoint(JointSegment const & jointSegment, bool start) const
|
||||
{
|
||||
return m_graph.GetPoint(GetSegmentFromJoint(jointSegment, start), jointSegment.IsForward());
|
||||
}
|
||||
JointSegment const & GetStartJoint() const { return m_startJoint; }
|
||||
JointSegment const & GetFinishJoint() const { return m_endJoint; }
|
||||
|
||||
// AStarGraph overridings
|
||||
// @{
|
||||
RouteWeight HeuristicCostEstimate(Vertex const & from, Vertex const & to) override;
|
||||
|
||||
void GetOutgoingEdgesList(astar::VertexData<Vertex, Weight> const & vertexData, EdgeListT & edges) override
|
||||
{
|
||||
GetEdgeList(vertexData, true /* isOutgoing */, edges);
|
||||
}
|
||||
|
||||
void GetIngoingEdgesList(astar::VertexData<Vertex, Weight> const & vertexData, EdgeListT & edges) override
|
||||
{
|
||||
GetEdgeList(vertexData, false /* isOutgoing */, edges);
|
||||
}
|
||||
|
||||
void SetAStarParents(bool forward, Parents & parents) override { m_graph.SetAStarParents(forward, parents); }
|
||||
|
||||
void DropAStarParents() override { m_graph.DropAStarParents(); }
|
||||
|
||||
bool AreWavesConnectible(Parents & forwardParents, Vertex const & commonVertex, Parents & backwardParents) override
|
||||
{
|
||||
auto const converter = [&](JointSegment & vertex)
|
||||
{
|
||||
if (!vertex.IsFake())
|
||||
return;
|
||||
|
||||
auto const & fake = GetFakeSegmentSure(vertex);
|
||||
auto const & first = fake.GetSegment(true /* start */);
|
||||
if (first.IsRealSegment())
|
||||
vertex.AssignID(first);
|
||||
else
|
||||
vertex.AssignID(fake.GetSegment(false /* start */));
|
||||
};
|
||||
|
||||
return m_graph.AreWavesConnectible(forwardParents, commonVertex, backwardParents, converter);
|
||||
}
|
||||
|
||||
RouteWeight GetAStarWeightEpsilon() override { return m_graph.GetAStarWeightEpsilon(); }
|
||||
// @}
|
||||
|
||||
WorldGraphMode GetMode() const { return m_graph.GetMode(); }
|
||||
|
||||
/// \brief Reconstructs JointSegment by segment after building the route.
|
||||
std::vector<Segment> ReconstructJoint(JointSegment const & joint);
|
||||
|
||||
void Reset();
|
||||
|
||||
static bool IsRealSegment(Segment const & segment) { return !segment.IsFakeCreated(); }
|
||||
|
||||
Segment const & GetSegmentOfFakeJoint(JointSegment const & joint, bool start) const
|
||||
{
|
||||
return GetFakeSegmentSure(joint).GetSegment(start);
|
||||
}
|
||||
Segment GetSegmentFromJoint(JointSegment const & joint, bool start) const
|
||||
{
|
||||
return joint.IsFake() ? GetSegmentOfFakeJoint(joint, start) : joint.GetSegment(start);
|
||||
}
|
||||
|
||||
private:
|
||||
static auto constexpr kInvalidId = JointSegment::kInvalidSegmentId;
|
||||
static auto constexpr kInvisibleEndId = kInvalidId - 1;
|
||||
static auto constexpr kInvisibleStartId = kInvalidId - 2;
|
||||
|
||||
struct FakeJointSegment
|
||||
{
|
||||
FakeJointSegment() = default;
|
||||
FakeJointSegment(Segment const & start, Segment const & end) : m_start(start), m_end(end) {}
|
||||
|
||||
Segment const & GetSegment(bool start) const { return start ? m_start : m_end; }
|
||||
|
||||
Segment m_start;
|
||||
Segment m_end;
|
||||
};
|
||||
|
||||
void InitEnding(Segment const & ending, bool start);
|
||||
|
||||
void AddFakeJoints(Segment const & segment, bool isOutgoing, EdgeListT & edges);
|
||||
|
||||
void GetEdgeList(astar::VertexData<Vertex, Weight> const & vertexData, bool isOutgoing, EdgeListT & edges);
|
||||
|
||||
JointSegment CreateFakeJoint(Segment const & from, Segment const & to, uint32_t featureId = kInvalidFeatureId);
|
||||
|
||||
bool IsJoint(Segment const & segment, bool fromStart) const { return m_graph.IsJoint(segment, fromStart); }
|
||||
|
||||
bool IsJointOrEnd(Segment const & segment, bool fromStart) const { return m_graph.IsJointOrEnd(segment, fromStart); }
|
||||
|
||||
using WeightListT = typename Graph::WeightListT;
|
||||
bool FillEdgesAndParentsWeights(astar::VertexData<Vertex, Weight> const & vertexData, bool isOutgoing,
|
||||
size_t & firstFakeId, EdgeListT & edges, WeightListT & parentWeights);
|
||||
|
||||
std::optional<Segment> GetParentSegment(JointSegment const & vertex, bool isOutgoing, EdgeListT & edges);
|
||||
|
||||
/// \brief Makes BFS from |startSegment| in direction |fromStart| and find the closest segments
|
||||
/// which end RoadPoints are joints. Thus we build fake joint segments graph.
|
||||
EdgeListT FindFirstJoints(Segment const & startSegment, bool fromStart);
|
||||
|
||||
JointSegment CreateInvisibleJoint(Segment const & segment, bool start);
|
||||
|
||||
bool IsInvisible(JointSegment const & jointSegment) const
|
||||
{
|
||||
static_assert(kInvisibleStartId < kInvisibleEndId && kInvisibleEndId != kInvalidId,
|
||||
"FakeID's are crossing in JointSegment.");
|
||||
|
||||
return jointSegment.GetStartSegmentId() == jointSegment.GetEndSegmentId() &&
|
||||
(jointSegment.GetStartSegmentId() == kInvisibleStartId ||
|
||||
jointSegment.GetStartSegmentId() == kInvisibleEndId) &&
|
||||
jointSegment.GetStartSegmentId() != kInvalidId;
|
||||
}
|
||||
|
||||
FakeJointSegment const & GetFakeSegmentSure(JointSegment const & key) const
|
||||
{
|
||||
auto const it = m_fakeJointSegments.find(key);
|
||||
ASSERT(it != m_fakeJointSegments.end(), (key));
|
||||
return it->second;
|
||||
}
|
||||
|
||||
Graph & m_graph;
|
||||
|
||||
// Fake start and end joints.
|
||||
JointSegment m_startJoint;
|
||||
JointSegment m_endJoint;
|
||||
|
||||
Segment m_startSegment;
|
||||
Segment m_endSegment;
|
||||
|
||||
ms::LatLon m_startPoint;
|
||||
ms::LatLon m_endPoint;
|
||||
|
||||
// See comments in |GetEdgeList()| about |m_savedWeight|.
|
||||
ska::bytell_hash_map<Vertex, Weight> m_savedWeight;
|
||||
|
||||
// JointSegment consists of two segments of one feature.
|
||||
// FakeJointSegment consists of two segments of different features.
|
||||
// So we create an invalid JointSegment (see |ToFake()| method), that
|
||||
// converts to FakeJointSegments. This std::map is converter.
|
||||
ska::flat_hash_map<JointSegment, FakeJointSegment> m_fakeJointSegments;
|
||||
|
||||
struct ReconstructedPath
|
||||
{
|
||||
ReconstructedPath() = default;
|
||||
ReconstructedPath(std::vector<Segment> && path, bool fromStart) : m_fromStart(fromStart), m_path(std::move(path)) {}
|
||||
|
||||
bool m_fromStart = true;
|
||||
std::vector<Segment> m_path;
|
||||
};
|
||||
|
||||
std::map<JointSegment, ReconstructedPath> m_reconstructedFakeJoints;
|
||||
|
||||
// List of JointEdges that are outgoing from start.
|
||||
EdgeListT m_startOutEdges;
|
||||
// List of incoming to finish.
|
||||
EdgeListT m_endOutEdges;
|
||||
|
||||
uint32_t m_fakeId = 0;
|
||||
bool m_init = false;
|
||||
};
|
||||
|
||||
template <typename Graph>
|
||||
IndexGraphStarterJoints<Graph>::IndexGraphStarterJoints(Graph & graph, Segment const & startSegment,
|
||||
Segment const & endSegment)
|
||||
: m_graph(graph)
|
||||
, m_startSegment(startSegment)
|
||||
, m_endSegment(endSegment)
|
||||
{
|
||||
Init(m_startSegment, m_endSegment);
|
||||
}
|
||||
|
||||
template <typename Graph>
|
||||
IndexGraphStarterJoints<Graph>::IndexGraphStarterJoints(Graph & graph, Segment const & startSegment)
|
||||
: m_graph(graph)
|
||||
, m_startSegment(startSegment)
|
||||
{
|
||||
InitEnding(startSegment, true /* start */);
|
||||
|
||||
m_endSegment = Segment();
|
||||
m_endJoint = JointSegment();
|
||||
m_endPoint = ms::LatLon();
|
||||
|
||||
m_init = true;
|
||||
}
|
||||
|
||||
template <typename Graph>
|
||||
void IndexGraphStarterJoints<Graph>::Init(Segment const & startSegment, Segment const & endSegment)
|
||||
{
|
||||
InitEnding(startSegment, true /* start */);
|
||||
InitEnding(endSegment, false /* start */);
|
||||
|
||||
m_init = true;
|
||||
}
|
||||
|
||||
template <typename Graph>
|
||||
void IndexGraphStarterJoints<Graph>::InitEnding(Segment const & ending, bool start)
|
||||
{
|
||||
auto & segment = start ? m_startSegment : m_endSegment;
|
||||
segment = ending;
|
||||
|
||||
auto & point = start ? m_startPoint : m_endPoint;
|
||||
point = m_graph.GetPoint(ending, true /* front */);
|
||||
|
||||
auto & endingJoint = start ? m_startJoint : m_endJoint;
|
||||
if (IsRealSegment(ending))
|
||||
{
|
||||
endingJoint = CreateInvisibleJoint(ending, start);
|
||||
}
|
||||
else
|
||||
{
|
||||
auto const & loopSegment = start ? m_graph.GetStartSegment() : m_graph.GetFinishSegment();
|
||||
endingJoint = CreateFakeJoint(loopSegment, loopSegment);
|
||||
}
|
||||
|
||||
m_reconstructedFakeJoints[endingJoint] = ReconstructedPath({ending}, start);
|
||||
|
||||
auto & outEdges = start ? m_startOutEdges : m_endOutEdges;
|
||||
outEdges = FindFirstJoints(ending, start);
|
||||
|
||||
if (!start)
|
||||
{
|
||||
m_savedWeight[m_endJoint] = Weight(0.0);
|
||||
for (auto const & edge : m_endOutEdges)
|
||||
m_savedWeight[edge.GetTarget()] = edge.GetWeight();
|
||||
}
|
||||
}
|
||||
|
||||
template <typename Graph>
|
||||
RouteWeight IndexGraphStarterJoints<Graph>::HeuristicCostEstimate(JointSegment const & from, JointSegment const & to)
|
||||
{
|
||||
ASSERT(to == m_startJoint || to == m_endJoint, ("Invariant violated."));
|
||||
|
||||
Segment fromSegment;
|
||||
if (from.IsFake() || IsInvisible(from))
|
||||
{
|
||||
auto const it = m_reconstructedFakeJoints.find(from);
|
||||
CHECK(it != m_reconstructedFakeJoints.end(), ("No such fake joint:", from));
|
||||
fromSegment = it->second.m_path.back();
|
||||
}
|
||||
else
|
||||
{
|
||||
fromSegment = from.GetSegment(false /* start */);
|
||||
}
|
||||
|
||||
return (to == m_endJoint) ? m_graph.HeuristicCostEstimate(fromSegment, m_endPoint)
|
||||
: m_graph.HeuristicCostEstimate(fromSegment, m_startPoint);
|
||||
}
|
||||
|
||||
template <typename Graph>
|
||||
std::vector<Segment> IndexGraphStarterJoints<Graph>::ReconstructJoint(JointSegment const & joint)
|
||||
{
|
||||
// We have invisible JointSegments, which are come from start to start or end to end.
|
||||
// They need just for generic algorithm working. So we skip such objects.
|
||||
if (IsInvisible(joint))
|
||||
return {};
|
||||
|
||||
// In case of a fake vertex we return its prebuilt path.
|
||||
if (joint.IsFake())
|
||||
{
|
||||
auto const it = m_reconstructedFakeJoints.find(joint);
|
||||
CHECK(it != m_reconstructedFakeJoints.cend(), ("No such fake joint:", joint));
|
||||
|
||||
auto path = it->second.m_path;
|
||||
ASSERT(!path.empty(), ());
|
||||
if (path.front() == m_startSegment && path.back() == m_endSegment)
|
||||
path.pop_back();
|
||||
|
||||
return path;
|
||||
}
|
||||
|
||||
// Otherwise just reconstruct segment consequently.
|
||||
std::vector<Segment> subpath;
|
||||
|
||||
Segment currentSegment = joint.GetSegment(true /* start */);
|
||||
Segment lastSegment = joint.GetSegment(false /* start */);
|
||||
|
||||
bool const forward = currentSegment.GetSegmentIdx() < lastSegment.GetSegmentIdx();
|
||||
while (currentSegment != lastSegment)
|
||||
{
|
||||
subpath.emplace_back(currentSegment);
|
||||
currentSegment.Next(forward);
|
||||
}
|
||||
|
||||
subpath.emplace_back(lastSegment);
|
||||
return subpath;
|
||||
}
|
||||
|
||||
template <typename Graph>
|
||||
void IndexGraphStarterJoints<Graph>::AddFakeJoints(Segment const & segment, bool isOutgoing, EdgeListT & edges)
|
||||
{
|
||||
// If |isOutgoing| is true, we need real segments, that are real parts
|
||||
// of fake joints, entered to finish and vice versa.
|
||||
EdgeListT const & endings = isOutgoing ? m_endOutEdges : m_startOutEdges;
|
||||
|
||||
for (auto const & edge : endings)
|
||||
{
|
||||
// The one end of FakeJointSegment is start/finish and the opposite end is real segment.
|
||||
// So we check, whether |segment| is equal to the real segment of FakeJointSegment.
|
||||
// If yes, that means, that we can go from |segment| to start/finish.
|
||||
auto const it = m_fakeJointSegments.find(edge.GetTarget());
|
||||
if (it == m_fakeJointSegments.end())
|
||||
continue;
|
||||
|
||||
Segment const & firstSegment = it->second.GetSegment(isOutgoing /* start */);
|
||||
if (firstSegment == segment)
|
||||
{
|
||||
edges.emplace_back(edge);
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
template <typename Graph>
|
||||
std::optional<Segment> IndexGraphStarterJoints<Graph>::GetParentSegment(JointSegment const & vertex, bool isOutgoing,
|
||||
EdgeListT & edges)
|
||||
{
|
||||
std::optional<Segment> parentSegment;
|
||||
bool const opposite = !isOutgoing;
|
||||
if (vertex.IsFake())
|
||||
{
|
||||
FakeJointSegment const & fakeJointSegment = GetFakeSegmentSure(vertex);
|
||||
|
||||
auto const & endSegment = isOutgoing ? m_endSegment : m_startSegment;
|
||||
parentSegment = fakeJointSegment.GetSegment(opposite /* start */);
|
||||
|
||||
// This is case when we can build route from start to finish without real segment, only fake.
|
||||
// It happens when start and finish are close to each other.
|
||||
// If we want A* stop, we should add |endJoint| to its queue, then A* will see the vertex: |endJoint|
|
||||
// where it has already been and stop working.
|
||||
if (parentSegment == endSegment)
|
||||
{
|
||||
auto const & endJoint = isOutgoing ? m_endJoint : m_startJoint;
|
||||
if (isOutgoing)
|
||||
{
|
||||
static auto constexpr kZeroWeight = RouteWeight(0.0);
|
||||
edges.emplace_back(endJoint, kZeroWeight);
|
||||
}
|
||||
else
|
||||
{
|
||||
auto const it = m_savedWeight.find(vertex);
|
||||
CHECK(it != m_savedWeight.cend(), ("Can not find weight for:", vertex));
|
||||
|
||||
Weight const & weight = it->second;
|
||||
edges.emplace_back(endJoint, weight);
|
||||
}
|
||||
return {};
|
||||
}
|
||||
|
||||
#ifdef DEBUG
|
||||
auto const & startSegment = isOutgoing ? m_startSegment : m_endSegment;
|
||||
ASSERT_EQUAL(fakeJointSegment.GetSegment(!opposite /* start */), startSegment, ());
|
||||
#endif // DEBUG
|
||||
}
|
||||
else
|
||||
{
|
||||
parentSegment = vertex.GetSegment(opposite /* start */);
|
||||
}
|
||||
|
||||
return parentSegment;
|
||||
}
|
||||
|
||||
template <typename Graph>
|
||||
bool IndexGraphStarterJoints<Graph>::FillEdgesAndParentsWeights(astar::VertexData<Vertex, Weight> const & vertexData,
|
||||
bool isOutgoing, size_t & firstFakeId,
|
||||
EdgeListT & edges, WeightListT & parentWeights)
|
||||
{
|
||||
auto const & vertex = vertexData.m_vertex;
|
||||
// Case of fake start or finish joints.
|
||||
// Note: startJoint and finishJoint are just loops
|
||||
// from start to start or end to end vertex.
|
||||
if (vertex == GetStartJoint())
|
||||
{
|
||||
edges.append(m_startOutEdges.begin(), m_startOutEdges.end());
|
||||
parentWeights.append(edges.size(), Weight(0.0));
|
||||
firstFakeId = edges.size();
|
||||
}
|
||||
else if (vertex == GetFinishJoint())
|
||||
{
|
||||
edges.append(m_endOutEdges.begin(), m_endOutEdges.end());
|
||||
// If vertex is FinishJoint, parentWeight is equal to zero, because the first vertex is zero-weight loop.
|
||||
parentWeights.append(edges.size(), Weight(0.0));
|
||||
}
|
||||
else
|
||||
{
|
||||
auto const optional = GetParentSegment(vertex, isOutgoing, edges);
|
||||
if (!optional)
|
||||
return false;
|
||||
|
||||
Segment const & parentSegment = *optional;
|
||||
m_graph.GetEdgeList(vertexData, parentSegment, isOutgoing, edges, parentWeights);
|
||||
|
||||
firstFakeId = edges.size();
|
||||
for (size_t i = 0; i < firstFakeId; ++i)
|
||||
{
|
||||
size_t const prevSize = edges.size();
|
||||
|
||||
AddFakeJoints(GetSegmentFromJoint(edges[i].GetTarget(), isOutgoing), isOutgoing, edges);
|
||||
|
||||
// If we add fake edge, we should add new parentWeight as "child[i] -> parent".
|
||||
// Because fake edge and current edge (jointEdges[i]) have the same first
|
||||
// segments (differ only the ends), so we add to |parentWeights| the same
|
||||
// value: parentWeights[i].
|
||||
if (edges.size() != prevSize)
|
||||
parentWeights.push_back(parentWeights[i]);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
template <typename Graph>
|
||||
void IndexGraphStarterJoints<Graph>::GetEdgeList(astar::VertexData<Vertex, Weight> const & vertexData, bool isOutgoing,
|
||||
EdgeListT & edges)
|
||||
{
|
||||
CHECK(m_init, ("IndexGraphStarterJoints was not initialized."));
|
||||
|
||||
edges.clear();
|
||||
|
||||
// This vector needs for backward A* search. Assume, we have parent and child_1, child_2.
|
||||
// In this case we will save next weights:
|
||||
// 1) from child_1 to parent.
|
||||
// 2) from child_2 to parent.
|
||||
// That needs for correct edges weights calculation.
|
||||
WeightListT parentWeights;
|
||||
|
||||
size_t firstFakeId = 0;
|
||||
if (!FillEdgesAndParentsWeights(vertexData, isOutgoing, firstFakeId, edges, parentWeights))
|
||||
return;
|
||||
|
||||
auto const & vertex = vertexData.m_vertex;
|
||||
if (!isOutgoing)
|
||||
{
|
||||
// |parentSegment| is parent-vertex from which we search children.
|
||||
// For correct weight calculation we should get weight of JointSegment, that
|
||||
// ends in |parentSegment| and add |parentWeight[i]| to the saved value.
|
||||
auto const it = m_savedWeight.find(vertex);
|
||||
CHECK(it != m_savedWeight.cend(), ("Can not find weight for:", vertex));
|
||||
|
||||
Weight const weight = it->second;
|
||||
for (size_t i = 0; i < edges.size(); ++i)
|
||||
{
|
||||
// Saving weight of current edges for returning in the next iterations.
|
||||
auto & w = edges[i].GetWeight();
|
||||
auto const & t = edges[i].GetTarget();
|
||||
|
||||
// By VNG: Revert to the MM original code, since Cross-MWM borders penalty is assigned in the end of this
|
||||
// function.
|
||||
/// @todo I still have doubts on how we "fetch" weight for ingoing edges with m_savedWeight.
|
||||
m_savedWeight[t] = w;
|
||||
|
||||
// For parent JointSegment we know its weight without last segment, because for each child
|
||||
// it will differ (child_1 => parent != child_2 => parent), but (!) we save this weight in
|
||||
// |parentWeights[]|. So the weight of an ith edge is a cached "weight of parent JointSegment" +
|
||||
// "parentWeight[i]".
|
||||
w = weight + parentWeights[i];
|
||||
}
|
||||
|
||||
// Delete useless weight of parent JointSegment.
|
||||
m_savedWeight.erase(vertex);
|
||||
}
|
||||
else
|
||||
{
|
||||
// This needs for correct weights calculation of FakeJointSegments during forward A* search.
|
||||
for (size_t i = firstFakeId; i < edges.size(); ++i)
|
||||
edges[i].GetWeight() += parentWeights[i];
|
||||
}
|
||||
|
||||
auto const vertexMwmId = vertex.GetMwmId();
|
||||
if (vertexMwmId != kFakeNumMwmId)
|
||||
{
|
||||
/// @todo By VNG: Main problem with current (borders penalty) approach and bidirectional A* is that:
|
||||
/// a weight of v1->v2 transition moving forward (v1 outgoing) should be the same as
|
||||
/// a weight of v1->v2 transition moving backward (v2 ingoing). This is impossible in current (m_savedWeight)
|
||||
/// logic, so I moved Cross-MWM penalty into separate block here after _all_ weights calculations.
|
||||
|
||||
for (auto & e : edges)
|
||||
{
|
||||
auto const targetMwmId = e.GetTarget().GetMwmId();
|
||||
if (targetMwmId != kFakeNumMwmId && vertexMwmId != targetMwmId)
|
||||
e.GetWeight() += m_graph.GetCrossBorderPenalty(vertexMwmId, targetMwmId);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
template <typename Graph>
|
||||
JointSegment IndexGraphStarterJoints<Graph>::CreateFakeJoint(Segment const & from, Segment const & to,
|
||||
uint32_t featureId /* = kInvalidFeatureId*/)
|
||||
{
|
||||
JointSegment const result = JointSegment::MakeFake(m_fakeId++, featureId);
|
||||
m_fakeJointSegments.emplace(result, FakeJointSegment(from, to));
|
||||
return result;
|
||||
}
|
||||
|
||||
template <typename Graph>
|
||||
typename IndexGraphStarterJoints<Graph>::EdgeListT IndexGraphStarterJoints<Graph>::FindFirstJoints(
|
||||
Segment const & startSegment, bool fromStart)
|
||||
{
|
||||
Segment const & endSegment = fromStart ? m_endSegment : m_startSegment;
|
||||
|
||||
std::queue<Segment> queue;
|
||||
queue.emplace(startSegment);
|
||||
|
||||
std::map<Segment, Segment> parent;
|
||||
std::map<Segment, RouteWeight> weight;
|
||||
EdgeListT result;
|
||||
|
||||
auto const reconstructPath = [&parent, &startSegment](Segment current, bool forward)
|
||||
{
|
||||
std::vector<Segment> path;
|
||||
path.emplace_back(current);
|
||||
|
||||
while (current != startSegment)
|
||||
{
|
||||
current = parent[current];
|
||||
path.emplace_back(current);
|
||||
}
|
||||
|
||||
if (forward)
|
||||
std::reverse(path.begin(), path.end());
|
||||
return path;
|
||||
};
|
||||
|
||||
uint32_t firstFeatureId = kInvalidFeatureId;
|
||||
auto const addFake = [&](Segment const & segment, Segment const & beforeConvert)
|
||||
{
|
||||
JointSegment fakeJoint;
|
||||
fakeJoint =
|
||||
fromStart ? CreateFakeJoint(startSegment, segment, firstFeatureId) : CreateFakeJoint(segment, startSegment);
|
||||
result.emplace_back(fakeJoint, weight[beforeConvert]);
|
||||
|
||||
std::vector<Segment> path = reconstructPath(beforeConvert, fromStart);
|
||||
m_reconstructedFakeJoints.emplace(fakeJoint, ReconstructedPath(std::move(path), fromStart));
|
||||
};
|
||||
|
||||
auto const isEndOfSegment = [&](Segment const & fake, Segment const & segment, bool fromStart)
|
||||
{
|
||||
bool const hasSameFront = m_graph.GetPoint(fake, true /* front */) == m_graph.GetPoint(segment, true);
|
||||
|
||||
bool const hasSameBack = m_graph.GetPoint(fake, false /* front */) == m_graph.GetPoint(segment, false);
|
||||
|
||||
return (fromStart && hasSameFront) || (!fromStart && hasSameBack);
|
||||
};
|
||||
|
||||
while (!queue.empty())
|
||||
{
|
||||
Segment segment = queue.front();
|
||||
queue.pop();
|
||||
Segment beforeConvert = segment;
|
||||
|
||||
bool const isRealPart =
|
||||
!IsRealSegment(segment) && m_graph.ConvertToReal(segment) && isEndOfSegment(beforeConvert, segment, fromStart);
|
||||
|
||||
// Get first real feature id and assign it below into future fake joint, that will pass over this feature.
|
||||
// Its important for IndexGraph::IsRestricted. See https://github.com/organicmaps/organicmaps/issues/1565.
|
||||
if (isRealPart && firstFeatureId == kInvalidFeatureId)
|
||||
firstFeatureId = segment.GetFeatureId();
|
||||
|
||||
// Either the segment is fake and it can be converted to real one with |Joint| end (RoadPoint),
|
||||
// or it's the real one and its end (RoadPoint) is |Joint|.
|
||||
if ((isRealPart || IsRealSegment(beforeConvert)) && IsJoint(segment, fromStart))
|
||||
{
|
||||
addFake(segment, beforeConvert);
|
||||
continue;
|
||||
}
|
||||
|
||||
if (beforeConvert == endSegment)
|
||||
{
|
||||
addFake(segment, beforeConvert);
|
||||
continue;
|
||||
}
|
||||
|
||||
typename Graph::EdgeListT edges;
|
||||
m_graph.GetEdgesList(beforeConvert, fromStart, edges);
|
||||
|
||||
for (auto const & edge : edges)
|
||||
{
|
||||
Segment child = edge.GetTarget();
|
||||
auto const & newWeight = weight[beforeConvert] + edge.GetWeight();
|
||||
if (weight.find(child) == weight.end() || weight[child] > newWeight)
|
||||
{
|
||||
parent[child] = beforeConvert;
|
||||
weight[child] = newWeight;
|
||||
queue.push(child);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
template <typename Graph>
|
||||
JointSegment IndexGraphStarterJoints<Graph>::CreateInvisibleJoint(Segment const & segment, bool start)
|
||||
{
|
||||
JointSegment const result = JointSegment::MakeFake(start ? kInvisibleStartId : kInvisibleEndId);
|
||||
m_fakeJointSegments.emplace(result, FakeJointSegment(segment, segment));
|
||||
return result;
|
||||
}
|
||||
|
||||
template <typename Graph>
|
||||
void IndexGraphStarterJoints<Graph>::Reset()
|
||||
{
|
||||
m_startJoint = JointSegment();
|
||||
m_endJoint = JointSegment();
|
||||
m_startSegment = Segment();
|
||||
m_endSegment = Segment();
|
||||
m_savedWeight.clear();
|
||||
m_fakeJointSegments.clear();
|
||||
m_reconstructedFakeJoints.clear();
|
||||
m_startOutEdges.clear();
|
||||
m_endOutEdges.clear();
|
||||
m_fakeId = 0;
|
||||
m_init = false;
|
||||
}
|
||||
} // namespace routing
|
||||
140
libs/routing/index_road_graph.cpp
Normal file
140
libs/routing/index_road_graph.cpp
Normal file
|
|
@ -0,0 +1,140 @@
|
|||
#include "routing/index_road_graph.hpp"
|
||||
|
||||
#include "routing/data_source.hpp"
|
||||
#include "routing/fake_feature_ids.hpp"
|
||||
#include "routing/index_graph_starter.hpp"
|
||||
#include "routing/latlon_with_altitude.hpp"
|
||||
#include "routing/routing_exceptions.hpp"
|
||||
#include "routing/transit_graph.hpp"
|
||||
|
||||
#include <cstdint>
|
||||
#include <utility>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
IndexRoadGraph::IndexRoadGraph(IndexGraphStarter & starter, std::vector<Segment> const & segments,
|
||||
std::vector<geometry::PointWithAltitude> const & junctions, MwmDataSource & dataSource)
|
||||
: m_dataSource(dataSource)
|
||||
, m_starter(starter)
|
||||
, m_segments(segments)
|
||||
{
|
||||
// j0 j1 j2 j3
|
||||
// *--s0--*--s1--*--s2--*
|
||||
CHECK_EQUAL(segments.size() + 1, junctions.size(), ());
|
||||
|
||||
for (size_t i = 0; i < junctions.size(); ++i)
|
||||
{
|
||||
geometry::PointWithAltitude const & junction = junctions[i];
|
||||
if (i > 0)
|
||||
m_endToSegment[junction].push_back(segments[i - 1]);
|
||||
if (i < segments.size())
|
||||
m_beginToSegment[junction].push_back(segments[i]);
|
||||
}
|
||||
}
|
||||
|
||||
void IndexRoadGraph::GetOutgoingEdges(geometry::PointWithAltitude const & junction, EdgeListT & edges) const
|
||||
{
|
||||
GetEdges(junction, true, edges);
|
||||
}
|
||||
|
||||
void IndexRoadGraph::GetIngoingEdges(geometry::PointWithAltitude const & junction, EdgeListT & edges) const
|
||||
{
|
||||
GetEdges(junction, false, edges);
|
||||
}
|
||||
|
||||
void IndexRoadGraph::GetEdgeTypes(Edge const & edge, feature::TypesHolder & types) const
|
||||
{
|
||||
if (edge.IsFake())
|
||||
{
|
||||
types = feature::TypesHolder(feature::GeomType::Line);
|
||||
return;
|
||||
}
|
||||
|
||||
FeatureID const featureId = edge.GetFeatureId();
|
||||
if (FakeFeatureIds::IsGuidesFeature(featureId.m_index))
|
||||
{
|
||||
types = feature::TypesHolder(feature::GeomType::Line);
|
||||
return;
|
||||
}
|
||||
|
||||
auto ft = m_dataSource.GetFeature(featureId);
|
||||
if (!ft)
|
||||
{
|
||||
LOG(LERROR, ("Can't load types for feature", featureId));
|
||||
return;
|
||||
}
|
||||
|
||||
ASSERT_EQUAL(ft->GetGeomType(), feature::GeomType::Line, ());
|
||||
types = feature::TypesHolder(*ft);
|
||||
}
|
||||
|
||||
void IndexRoadGraph::GetJunctionTypes(geometry::PointWithAltitude const & junction, feature::TypesHolder & types) const
|
||||
{
|
||||
types = feature::TypesHolder();
|
||||
}
|
||||
|
||||
void IndexRoadGraph::GetRouteEdges(EdgeVector & edges) const
|
||||
{
|
||||
edges.clear();
|
||||
edges.reserve(m_segments.size());
|
||||
|
||||
for (Segment const & segment : m_segments)
|
||||
{
|
||||
auto const & junctionFrom = m_starter.GetJunction(segment, false /* front */).ToPointWithAltitude();
|
||||
auto const & junctionTo = m_starter.GetJunction(segment, true /* front */).ToPointWithAltitude();
|
||||
|
||||
if (IndexGraphStarter::IsFakeSegment(segment) || TransitGraph::IsTransitSegment(segment))
|
||||
{
|
||||
Segment real = segment;
|
||||
if (m_starter.ConvertToReal(real))
|
||||
{
|
||||
edges.push_back(Edge::MakeFakeWithRealPart({m_dataSource.GetMwmId(real.GetMwmId()), real.GetFeatureId()},
|
||||
segment.GetSegmentIdx(), real.IsForward(), real.GetSegmentIdx(),
|
||||
junctionFrom, junctionTo));
|
||||
}
|
||||
else
|
||||
{
|
||||
edges.push_back(Edge::MakeFake(junctionFrom, junctionTo));
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
edges.push_back(Edge::MakeReal({m_dataSource.GetMwmId(segment.GetMwmId()), segment.GetFeatureId()},
|
||||
segment.IsForward(), segment.GetSegmentIdx(), junctionFrom, junctionTo));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void IndexRoadGraph::GetEdges(geometry::PointWithAltitude const & junction, bool isOutgoing, EdgeListT & edges) const
|
||||
{
|
||||
edges.clear();
|
||||
|
||||
for (Segment const & segment : GetSegments(junction, isOutgoing))
|
||||
{
|
||||
IndexGraphStarter::EdgeListT tmpEdges;
|
||||
m_starter.GetEdgesList(segment, isOutgoing, tmpEdges);
|
||||
|
||||
for (SegmentEdge const & segmentEdge : tmpEdges)
|
||||
{
|
||||
Segment const & segment = segmentEdge.GetTarget();
|
||||
if (IndexGraphStarter::IsFakeSegment(segment))
|
||||
continue;
|
||||
|
||||
edges.push_back(Edge::MakeReal({m_dataSource.GetMwmId(segment.GetMwmId()), segment.GetFeatureId()},
|
||||
segment.IsForward(), segment.GetSegmentIdx(),
|
||||
m_starter.GetJunction(segment, false /* front */).ToPointWithAltitude(),
|
||||
m_starter.GetJunction(segment, true /* front */).ToPointWithAltitude()));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
IndexRoadGraph::SegmentListT const & IndexRoadGraph::GetSegments(geometry::PointWithAltitude const & junction,
|
||||
bool isOutgoing) const
|
||||
{
|
||||
auto const & junctionToSegment = isOutgoing ? m_endToSegment : m_beginToSegment;
|
||||
|
||||
auto const it = junctionToSegment.find(junction);
|
||||
CHECK(it != junctionToSegment.cend(), ("junctionToSegment doesn't contain", junction, ", isOutgoing =", isOutgoing));
|
||||
return it->second;
|
||||
}
|
||||
} // namespace routing
|
||||
47
libs/routing/index_road_graph.hpp
Normal file
47
libs/routing/index_road_graph.hpp
Normal file
|
|
@ -0,0 +1,47 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/road_graph.hpp"
|
||||
#include "routing/segment.hpp"
|
||||
|
||||
#include "routing_common/num_mwm_id.hpp"
|
||||
|
||||
#include "geometry/point_with_altitude.hpp"
|
||||
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
class MwmDataSource;
|
||||
class IndexGraphStarter;
|
||||
|
||||
class IndexRoadGraph : public RoadGraphBase
|
||||
{
|
||||
public:
|
||||
IndexRoadGraph(IndexGraphStarter & starter, std::vector<Segment> const & segments,
|
||||
std::vector<geometry::PointWithAltitude> const & junctions, MwmDataSource & dataSource);
|
||||
|
||||
// IRoadGraphBase overrides:
|
||||
virtual void GetOutgoingEdges(geometry::PointWithAltitude const & junction, EdgeListT & edges) const override;
|
||||
virtual void GetIngoingEdges(geometry::PointWithAltitude const & junction, EdgeListT & edges) const override;
|
||||
virtual void GetEdgeTypes(Edge const & edge, feature::TypesHolder & types) const override;
|
||||
virtual void GetJunctionTypes(geometry::PointWithAltitude const & junction,
|
||||
feature::TypesHolder & types) const override;
|
||||
virtual void GetRouteEdges(EdgeVector & edges) const override;
|
||||
|
||||
std::vector<Segment> const & GetRouteSegments() const { return m_segments; }
|
||||
|
||||
private:
|
||||
void GetEdges(geometry::PointWithAltitude const & junction, bool isOutgoing, EdgeListT & edges) const;
|
||||
|
||||
using SegmentListT = SmallList<Segment>;
|
||||
SegmentListT const & GetSegments(geometry::PointWithAltitude const & junction, bool isOutgoing) const;
|
||||
|
||||
MwmDataSource & m_dataSource;
|
||||
IndexGraphStarter & m_starter;
|
||||
std::vector<Segment> m_segments;
|
||||
std::map<geometry::PointWithAltitude, SegmentListT> m_beginToSegment;
|
||||
std::map<geometry::PointWithAltitude, SegmentListT> m_endToSegment;
|
||||
};
|
||||
} // namespace routing
|
||||
1808
libs/routing/index_router.cpp
Normal file
1808
libs/routing/index_router.cpp
Normal file
File diff suppressed because it is too large
Load diff
286
libs/routing/index_router.hpp
Normal file
286
libs/routing/index_router.hpp
Normal file
|
|
@ -0,0 +1,286 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/base/astar_algorithm.hpp"
|
||||
#include "routing/base/astar_progress.hpp"
|
||||
#include "routing/base/routing_result.hpp"
|
||||
|
||||
#include "routing/data_source.hpp"
|
||||
#include "routing/directions_engine.hpp"
|
||||
#include "routing/edge_estimator.hpp"
|
||||
#include "routing/fake_edges_container.hpp"
|
||||
#include "routing/features_road_graph.hpp"
|
||||
#include "routing/guides_connections.hpp"
|
||||
#include "routing/nearest_edge_finder.hpp"
|
||||
#include "routing/regions_decl.hpp"
|
||||
#include "routing/router.hpp"
|
||||
#include "routing/routing_callbacks.hpp"
|
||||
#include "routing/segment.hpp"
|
||||
#include "routing/segmented_route.hpp"
|
||||
|
||||
#include "routing_common/num_mwm_id.hpp"
|
||||
#include "routing_common/vehicle_model.hpp"
|
||||
|
||||
#include "platform/country_file.hpp"
|
||||
|
||||
#include "geometry/point2d.hpp"
|
||||
#include "geometry/tree4d.hpp"
|
||||
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <set>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace traffic
|
||||
{
|
||||
class TrafficCache;
|
||||
}
|
||||
|
||||
namespace routing
|
||||
{
|
||||
class IndexGraph;
|
||||
class IndexGraphStarter;
|
||||
|
||||
class IndexRouter : public IRouter
|
||||
{
|
||||
public:
|
||||
class BestEdgeComparator final
|
||||
{
|
||||
public:
|
||||
BestEdgeComparator(m2::PointD const & point, m2::PointD const & direction);
|
||||
|
||||
/// \returns -1 if |edge1| is closer to |m_point| and |m_direction| than |edge2|.
|
||||
/// returns 0 if |edge1| and |edge2| have almost the same direction and are equidistant from |m_point|.
|
||||
/// returns 1 if |edge1| is further from |m_point| and |m_direction| than |edge2|.
|
||||
int Compare(Edge const & edge1, Edge const & edge2) const;
|
||||
|
||||
bool IsDirectionValid() const { return !m_direction.IsAlmostZero(); }
|
||||
|
||||
/// \brief According to current implementation vectors |edge| and |m_direction|
|
||||
/// are almost collinear and co-directional if the angle between them is less than 14 degrees.
|
||||
bool IsAlmostCodirectional(Edge const & edge) const;
|
||||
|
||||
/// \returns the square of shortest distance from |m_point| to |edge| in mercator.
|
||||
double GetSquaredDist(Edge const & edge) const;
|
||||
|
||||
private:
|
||||
m2::PointD const m_point;
|
||||
m2::PointD const m_direction;
|
||||
};
|
||||
|
||||
IndexRouter(VehicleType vehicleType, bool loadAltitudes, CountryParentNameGetterFn const & countryParentNameGetterFn,
|
||||
TCountryFileFn const & countryFileFn, CountryRectFn const & countryRectFn,
|
||||
std::shared_ptr<NumMwmIds> numMwmIds, std::unique_ptr<m4::Tree<NumMwmId>> numMwmTree,
|
||||
traffic::TrafficCache const & trafficCache, DataSource & dataSource);
|
||||
|
||||
std::unique_ptr<WorldGraph> MakeSingleMwmWorldGraph();
|
||||
|
||||
// IRouter overrides:
|
||||
std::string GetName() const override { return m_name; }
|
||||
void ClearState() override;
|
||||
|
||||
void SetGuides(GuidesTracks && guides) override;
|
||||
RouterResultCode CalculateRoute(Checkpoints const & checkpoints, m2::PointD const & startDirection,
|
||||
bool adjustToPrevRoute, RouterDelegate const & delegate, Route & route) override;
|
||||
|
||||
bool FindClosestProjectionToRoad(m2::PointD const & point, m2::PointD const & direction, double radius,
|
||||
EdgeProj & proj) override;
|
||||
|
||||
bool GetBestOutgoingEdges(m2::PointD const & checkpoint, WorldGraph & graph, std::vector<Edge> & edges);
|
||||
|
||||
VehicleType GetVehicleType() const { return m_vehicleType; }
|
||||
|
||||
private:
|
||||
RouterResultCode CalculateSubrouteJointsMode(IndexGraphStarter & starter, RouterDelegate const & delegate,
|
||||
std::shared_ptr<AStarProgress> const & progress,
|
||||
std::vector<Segment> & subroute);
|
||||
RouterResultCode CalculateSubrouteNoLeapsMode(IndexGraphStarter & starter, RouterDelegate const & delegate,
|
||||
std::shared_ptr<AStarProgress> const & progress,
|
||||
std::vector<Segment> & subroute);
|
||||
RouterResultCode CalculateSubrouteLeapsOnlyMode(Checkpoints const & checkpoints, size_t subrouteIdx,
|
||||
IndexGraphStarter & starter, RouterDelegate const & delegate,
|
||||
std::shared_ptr<AStarProgress> const & progress,
|
||||
std::vector<Segment> & subroute);
|
||||
|
||||
RouterResultCode DoCalculateRoute(Checkpoints const & checkpoints, m2::PointD const & startDirection,
|
||||
RouterDelegate const & delegate, Route & route);
|
||||
RouterResultCode CalculateSubroute(Checkpoints const & checkpoints, size_t subrouteIdx,
|
||||
RouterDelegate const & delegate, std::shared_ptr<AStarProgress> const & progress,
|
||||
IndexGraphStarter & graph, std::vector<Segment> & subroute,
|
||||
bool guidesActive = false);
|
||||
|
||||
RouterResultCode AdjustRoute(Checkpoints const & checkpoints, m2::PointD const & startDirection,
|
||||
RouterDelegate const & delegate, Route & route);
|
||||
|
||||
std::unique_ptr<WorldGraph> MakeWorldGraph();
|
||||
|
||||
using EdgeProjectionT = IRoadGraph::EdgeProjectionT;
|
||||
class PointsOnEdgesSnapping
|
||||
{
|
||||
IndexRouter & m_router;
|
||||
WorldGraph & m_graph;
|
||||
|
||||
using RoadInfoT = IRoadGraph::FullRoadInfo;
|
||||
using EdgeProjectionT = IndexRouter::EdgeProjectionT;
|
||||
|
||||
// The idea here is to store dead-ends from the successful previous call, not to filter
|
||||
// dead-end candidates if they belong to one graph's cluster (island).
|
||||
std::set<Segment> m_deadEnds[2];
|
||||
std::vector<Segment> m_startSegments;
|
||||
|
||||
static uint32_t constexpr kFirstSearchDistanceM = 40;
|
||||
|
||||
public:
|
||||
PointsOnEdgesSnapping(IndexRouter & router, WorldGraph & graph) : m_router(router), m_graph(graph) {}
|
||||
|
||||
/// @return 0 - ok, 1 - start not found, 2 - finish not found.
|
||||
int Snap(m2::PointD const & start, m2::PointD const & finish, m2::PointD const & direction,
|
||||
FakeEnding & startEnding, FakeEnding & finishEnding, bool & startIsCodirectional);
|
||||
|
||||
void SetNextStartSegment(Segment const & seg) { m_startSegments = {seg}; }
|
||||
|
||||
private:
|
||||
void FillDeadEndsCache(m2::PointD const & point);
|
||||
|
||||
/// \brief Removes all roads from |roads| that go to dead ends and all roads that
|
||||
/// are not good according to |worldGraph|. For car routing there are roads with hwtag nocar as well.
|
||||
/// \param checkpoint which is used to look for the closest segment in a road. The closest segment
|
||||
/// is used then to check if it's a dead end.
|
||||
void EraseIfDeadEnd(m2::PointD const & checkpoint, std::vector<RoadInfoT> & roads,
|
||||
std::set<Segment> & deadEnds) const;
|
||||
|
||||
/// \returns true if a segment (|point|, |edgeProjection.second|) crosses one of segments
|
||||
/// in |fences| except for the one which has the same geometry with |edgeProjection.first|.
|
||||
static bool IsFencedOff(m2::PointD const & point, EdgeProjectionT const & edgeProjection,
|
||||
std::vector<RoadInfoT> const & fences);
|
||||
|
||||
static void RoadsToNearestEdges(m2::PointD const & point, std::vector<RoadInfoT> const & roads,
|
||||
IsEdgeProjGood const & isGood, std::vector<EdgeProjectionT> & edgeProj);
|
||||
|
||||
Segment GetSegmentByEdge(Edge const & edge) const;
|
||||
|
||||
public:
|
||||
/// \brief Fills |closestCodirectionalEdge| with a codirectional edge which is closest to
|
||||
/// |point| and returns true if there's any. If not returns false.
|
||||
static bool FindClosestCodirectionalEdge(m2::PointD const & point, m2::PointD const & direction,
|
||||
std::vector<EdgeProjectionT> const & candidates,
|
||||
Edge & closestCodirectionalEdge);
|
||||
|
||||
/// \brief Finds best segments (edges) that may be considered as starts or finishes
|
||||
/// of the route. According to current implementation the closest to |checkpoint| segment which
|
||||
/// is almost codirectianal to |direction| is the best.
|
||||
/// If there's no an almost codirectional segment in the neighbourhood then all not dead end
|
||||
/// candidates that may be reached without crossing the road graph will be added to |bestSegments|.
|
||||
/// \param isOutgoing == true if |checkpoint| is considered as the start of the route.
|
||||
/// isOutgoing == false if |checkpoint| is considered as the finish of the route.
|
||||
/// \param bestSegmentIsAlmostCodirectional is filled with true if |bestSegment| is chosen
|
||||
/// because |direction| and direction of |bestSegment| are almost equal and with false otherwise.
|
||||
/// \return true if the best segment is found and false otherwise.
|
||||
/// \note Candidates in |bestSegments| are sorted from better to worse.
|
||||
bool FindBestSegments(m2::PointD const & checkpoint, m2::PointD const & direction, bool isOutgoing,
|
||||
std::vector<Segment> & bestSegments, bool & bestSegmentIsAlmostCodirectional);
|
||||
|
||||
bool FindBestEdges(m2::PointD const & checkpoint, m2::PointD const & direction, bool isOutgoing,
|
||||
double closestEdgesRadiusM, std::vector<Edge> & bestEdges,
|
||||
bool & bestSegmentIsAlmostCodirectional);
|
||||
};
|
||||
|
||||
using RoutingResultT = RoutingResult<Segment, RouteWeight>;
|
||||
class RoutesCalculator
|
||||
{
|
||||
std::map<std::pair<Segment, Segment>, RoutingResultT> m_cache;
|
||||
IndexGraphStarter & m_starter;
|
||||
RouterDelegate const & m_delegate;
|
||||
|
||||
public:
|
||||
RoutesCalculator(IndexGraphStarter & starter, RouterDelegate const & delegate)
|
||||
: m_starter(starter)
|
||||
, m_delegate(delegate)
|
||||
{}
|
||||
|
||||
using ProgressPtrT = std::shared_ptr<AStarProgress>;
|
||||
RoutingResultT const * Calc(Segment const & beg, Segment const & end, ProgressPtrT const & progress,
|
||||
double progressCoef);
|
||||
// Makes JointSingleMwm first and Joints then, if first attempt was failed.
|
||||
RoutingResultT const * Calc2Times(Segment const & beg, Segment const & end, ProgressPtrT const & progress,
|
||||
double progressCoef);
|
||||
};
|
||||
|
||||
// Input route may contains 'leaps': shortcut edges from mwm border enter to exit.
|
||||
// ProcessLeaps replaces each leap with calculated route through mwm.
|
||||
RouterResultCode ProcessLeapsJoints(std::vector<Segment> const & input, IndexGraphStarter & starter,
|
||||
std::shared_ptr<AStarProgress> const & progress, RoutesCalculator & calculator,
|
||||
RoutingResultT & result);
|
||||
|
||||
RouterResultCode RedressRoute(std::vector<Segment> const & segments, base::Cancellable const & cancellable,
|
||||
IndexGraphStarter & starter, Route & route);
|
||||
|
||||
bool AreSpeedCamerasProhibited(NumMwmId mwmID) const;
|
||||
bool AreMwmsNear(IndexGraphStarter const & starter) const;
|
||||
bool DoesTransitSectionExist(NumMwmId numMwmId);
|
||||
|
||||
RouterResultCode ConvertTransitResult(std::set<NumMwmId> const & mwmIds, RouterResultCode resultCode);
|
||||
|
||||
/// \brief Fills |speedcamProhibitedMwms| with mwms which are crossed by |segments|
|
||||
/// where speed cameras are prohibited.
|
||||
void FillSpeedCamProhibitedMwms(std::vector<Segment> const & segments,
|
||||
std::vector<platform::CountryFile> & speedCamProhibitedMwms) const;
|
||||
|
||||
template <typename Vertex, typename Edge, typename Weight>
|
||||
RouterResultCode ConvertResult(typename AStarAlgorithm<Vertex, Edge, Weight>::Result result) const
|
||||
{
|
||||
switch (result)
|
||||
{
|
||||
case AStarAlgorithm<Vertex, Edge, Weight>::Result::NoPath: return RouterResultCode::RouteNotFound;
|
||||
case AStarAlgorithm<Vertex, Edge, Weight>::Result::Cancelled: return RouterResultCode::Cancelled;
|
||||
case AStarAlgorithm<Vertex, Edge, Weight>::Result::OK: return RouterResultCode::NoError;
|
||||
}
|
||||
UNREACHABLE();
|
||||
}
|
||||
|
||||
template <typename Vertex, typename Edge, typename Weight, typename AStarParams>
|
||||
RouterResultCode FindPath(AStarParams & params, std::set<NumMwmId> const & mwmIds,
|
||||
RoutingResult<Vertex, Weight> & routingResult)
|
||||
{
|
||||
AStarAlgorithm<Vertex, Edge, Weight> algorithm;
|
||||
return ConvertTransitResult(
|
||||
mwmIds, ConvertResult<Vertex, Edge, Weight>(algorithm.FindPathBidirectional(params, routingResult)));
|
||||
}
|
||||
|
||||
void SetupAlgorithmMode(IndexGraphStarter & starter, bool guidesActive = false) const;
|
||||
uint32_t ConnectTracksOnGuidesToOsm(std::vector<m2::PointD> const & checkpoints, WorldGraph & graph);
|
||||
|
||||
void ConnectCheckpointsOnGuidesToOsm(std::vector<m2::PointD> const & checkpoints, WorldGraph & graph);
|
||||
|
||||
void AddGuidesOsmConnectionsToGraphStarter(size_t checkpointIdxFrom, size_t checkpointIdxTo,
|
||||
IndexGraphStarter & starter);
|
||||
|
||||
void AppendPartsOfReal(LatLonWithAltitude const & point1, LatLonWithAltitude const & point2, uint32_t & startIdx,
|
||||
ConnectionToOsm & link);
|
||||
|
||||
std::vector<Segment> GetBestOutgoingSegments(m2::PointD const & checkpoint, WorldGraph & graph);
|
||||
|
||||
VehicleType m_vehicleType;
|
||||
bool m_loadAltitudes;
|
||||
std::string const m_name;
|
||||
MwmDataSource m_dataSource;
|
||||
std::shared_ptr<VehicleModelFactoryInterface> m_vehicleModelFactory;
|
||||
|
||||
TCountryFileFn const m_countryFileFn;
|
||||
CountryRectFn const m_countryRectFn;
|
||||
std::shared_ptr<NumMwmIds> m_numMwmIds;
|
||||
std::shared_ptr<m4::Tree<NumMwmId>> m_numMwmTree;
|
||||
std::shared_ptr<TrafficStash> m_trafficStash;
|
||||
FeaturesRoadGraphBase m_roadGraph;
|
||||
|
||||
std::shared_ptr<EdgeEstimator> m_estimator;
|
||||
std::unique_ptr<DirectionsEngine> m_directionsEngine;
|
||||
std::unique_ptr<SegmentedRoute> m_lastRoute;
|
||||
std::unique_ptr<FakeEdgesContainer> m_lastFakeEdges;
|
||||
|
||||
// If a ckeckpoint is near to the guide track we need to build route through this track.
|
||||
GuidesConnections m_guides;
|
||||
|
||||
CountryParentNameGetterFn m_countryParentNameGetterFn;
|
||||
};
|
||||
} // namespace routing
|
||||
23
libs/routing/joint.cpp
Normal file
23
libs/routing/joint.cpp
Normal file
|
|
@ -0,0 +1,23 @@
|
|||
#include "routing/joint.hpp"
|
||||
|
||||
#include <sstream>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
// static
|
||||
Joint::Id constexpr Joint::kInvalidId;
|
||||
|
||||
std::string DebugPrint(Joint const & joint)
|
||||
{
|
||||
std::ostringstream oss;
|
||||
oss << "Joint [";
|
||||
for (size_t i = 0; i < joint.GetSize(); ++i)
|
||||
{
|
||||
if (i > 0)
|
||||
oss << ", ";
|
||||
oss << DebugPrint(joint.GetEntry(i));
|
||||
}
|
||||
oss << "]";
|
||||
return oss.str();
|
||||
}
|
||||
} // namespace routing
|
||||
32
libs/routing/joint.hpp
Normal file
32
libs/routing/joint.hpp
Normal file
|
|
@ -0,0 +1,32 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/road_point.hpp"
|
||||
|
||||
#include "base/buffer_vector.hpp"
|
||||
|
||||
#include <cstdint>
|
||||
#include <limits>
|
||||
#include <string>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
// Joint represents roads connection.
|
||||
// It contains RoadPoint for each connected road.
|
||||
class Joint final
|
||||
{
|
||||
public:
|
||||
using Id = uint32_t;
|
||||
static Id constexpr kInvalidId = std::numeric_limits<Id>::max();
|
||||
|
||||
void AddPoint(RoadPoint const & rp) { m_points.emplace_back(rp); }
|
||||
|
||||
size_t GetSize() const { return m_points.size(); }
|
||||
|
||||
RoadPoint const & GetEntry(size_t i) const { return m_points[i]; }
|
||||
|
||||
private:
|
||||
buffer_vector<RoadPoint, 2> m_points;
|
||||
};
|
||||
|
||||
std::string DebugPrint(Joint const & joint);
|
||||
} // namespace routing
|
||||
51
libs/routing/joint_index.cpp
Normal file
51
libs/routing/joint_index.cpp
Normal file
|
|
@ -0,0 +1,51 @@
|
|||
#include "routing/joint_index.hpp"
|
||||
|
||||
#include "routing/routing_exceptions.hpp"
|
||||
|
||||
namespace routing
|
||||
{
|
||||
void JointIndex::Build(RoadIndex const & roadIndex, uint32_t numJoints)
|
||||
{
|
||||
// + 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.assign(numJoints + 1, 0);
|
||||
|
||||
// Calculate sizes.
|
||||
// Example for numJoints = 6:
|
||||
// 2, 5, 3, 4, 2, 3, 0
|
||||
roadIndex.ForEachRoad([this, numJoints](uint32_t /* featureId */, RoadJointIds const & road)
|
||||
{
|
||||
road.ForEachJoint([this, numJoints](uint32_t /* pointId */, Joint::Id jointId)
|
||||
{
|
||||
UNUSED_VALUE(numJoints);
|
||||
ASSERT_LESS(jointId, numJoints, ());
|
||||
++m_offsets[jointId];
|
||||
});
|
||||
});
|
||||
|
||||
// Fill offsets with end bounds.
|
||||
// Example: 2, 7, 10, 14, 16, 19, 19
|
||||
for (size_t i = 1; i < m_offsets.size(); ++i)
|
||||
m_offsets[i] += m_offsets[i - 1];
|
||||
|
||||
m_points.resize(m_offsets.back());
|
||||
|
||||
// Now fill points.
|
||||
// Offsets after this operation are begin bounds:
|
||||
// 0, 2, 7, 10, 14, 16, 19
|
||||
roadIndex.ForEachRoad([this](uint32_t featureId, RoadJointIds const & road)
|
||||
{
|
||||
road.ForEachJoint([this, featureId](uint32_t pointId, Joint::Id jointId)
|
||||
{
|
||||
uint32_t & offset = m_offsets[jointId];
|
||||
--offset;
|
||||
m_points[offset] = {featureId, pointId};
|
||||
});
|
||||
});
|
||||
|
||||
CHECK_EQUAL(m_offsets[0], 0, ());
|
||||
CHECK_EQUAL(m_offsets.back(), m_points.size(), ());
|
||||
}
|
||||
} // namespace routing
|
||||
59
libs/routing/joint_index.hpp
Normal file
59
libs/routing/joint_index.hpp
Normal file
|
|
@ -0,0 +1,59 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/joint.hpp"
|
||||
#include "routing/road_index.hpp"
|
||||
#include "routing/road_point.hpp"
|
||||
|
||||
#include "base/assert.hpp"
|
||||
|
||||
#include <cstdint>
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
// JointIndex contains mapping from Joint::Id to RoadPoints.
|
||||
//
|
||||
// It is vector<Joint> conceptually.
|
||||
// Technically Joint entries are joined into the single vector to reduce allocations overheads.
|
||||
class JointIndex final
|
||||
{
|
||||
public:
|
||||
// Read comments in Build method about -1.
|
||||
uint32_t GetNumJoints() const
|
||||
{
|
||||
CHECK_GREATER(m_offsets.size(), 0, ());
|
||||
return static_cast<uint32_t>(m_offsets.size() - 1);
|
||||
}
|
||||
|
||||
uint32_t GetNumPoints() const { return static_cast<uint32_t>(m_points.size()); }
|
||||
RoadPoint GetPoint(Joint::Id jointId) const { return m_points[Begin(jointId)]; }
|
||||
|
||||
template <typename F>
|
||||
void ForEachPoint(Joint::Id jointId, F && f) const
|
||||
{
|
||||
for (uint32_t i = Begin(jointId); i < End(jointId); ++i)
|
||||
f(m_points[i]);
|
||||
}
|
||||
|
||||
void Build(RoadIndex const & roadIndex, uint32_t numJoints);
|
||||
|
||||
private:
|
||||
// Begin index for jointId entries.
|
||||
uint32_t Begin(Joint::Id jointId) const
|
||||
{
|
||||
ASSERT_LESS(jointId, m_offsets.size(), ());
|
||||
return m_offsets[jointId];
|
||||
}
|
||||
|
||||
// End index (not inclusive) for jointId entries.
|
||||
uint32_t End(Joint::Id jointId) const
|
||||
{
|
||||
Joint::Id const nextId = jointId + 1;
|
||||
ASSERT_LESS(nextId, m_offsets.size(), ());
|
||||
return m_offsets[nextId];
|
||||
}
|
||||
|
||||
std::vector<uint32_t> m_offsets;
|
||||
std::vector<RoadPoint> m_points;
|
||||
};
|
||||
} // namespace routing
|
||||
123
libs/routing/joint_segment.cpp
Normal file
123
libs/routing/joint_segment.cpp
Normal file
|
|
@ -0,0 +1,123 @@
|
|||
#include "routing/joint_segment.hpp"
|
||||
|
||||
#include "base/assert.hpp"
|
||||
|
||||
#include "std/boost_container_hash.hpp"
|
||||
|
||||
#include <sstream>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
JointSegment::JointSegment(Segment const & from, Segment const & to)
|
||||
{
|
||||
static_assert(sizeof(JointSegment) == 16, "");
|
||||
|
||||
CHECK(!from.IsFakeCreated() && !to.IsFakeCreated(),
|
||||
("Segments of joints can not be fake. Only through MakeFake() function."));
|
||||
|
||||
CHECK_EQUAL(from.GetMwmId(), to.GetMwmId(), ());
|
||||
m_numMwmId = from.GetMwmId();
|
||||
|
||||
CHECK_EQUAL(from.IsForward(), to.IsForward(), ());
|
||||
m_forward = from.IsForward();
|
||||
|
||||
CHECK_EQUAL(from.GetFeatureId(), to.GetFeatureId(), ());
|
||||
m_featureId = from.GetFeatureId();
|
||||
|
||||
m_startSegmentId = from.GetSegmentIdx();
|
||||
m_endSegmentId = to.GetSegmentIdx();
|
||||
}
|
||||
|
||||
void JointSegment::AssignID(Segment const & seg)
|
||||
{
|
||||
m_numMwmId = seg.GetMwmId();
|
||||
m_featureId = seg.GetFeatureId();
|
||||
}
|
||||
|
||||
void JointSegment::AssignID(JointSegment const & seg)
|
||||
{
|
||||
m_numMwmId = seg.m_numMwmId;
|
||||
m_featureId = seg.m_featureId;
|
||||
}
|
||||
|
||||
JointSegment JointSegment::MakeFake(uint32_t fakeId, uint32_t featureId /* = kInvalidFeatureId*/)
|
||||
{
|
||||
JointSegment res;
|
||||
res.m_featureId = featureId;
|
||||
res.m_startSegmentId = fakeId;
|
||||
res.m_endSegmentId = fakeId;
|
||||
res.m_numMwmId = kFakeNumMwmId;
|
||||
res.m_forward = false;
|
||||
return res;
|
||||
}
|
||||
|
||||
bool JointSegment::IsFake() const
|
||||
{
|
||||
// This is enough, but let's check in Debug for confidence.
|
||||
bool const result = (m_numMwmId == kFakeNumMwmId && m_startSegmentId != kInvalidSegmentId);
|
||||
if (result)
|
||||
{
|
||||
// Try if m_featureId can be real in a fake JointSegment.
|
||||
// ASSERT_EQUAL(m_featureId, kInvalidFeatureId, ());
|
||||
|
||||
ASSERT_EQUAL(m_startSegmentId, m_endSegmentId, ());
|
||||
ASSERT_EQUAL(m_forward, false, ());
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
Segment JointSegment::GetSegment(bool start) const
|
||||
{
|
||||
return {m_numMwmId, m_featureId, start ? m_startSegmentId : m_endSegmentId, m_forward};
|
||||
}
|
||||
|
||||
bool JointSegment::operator<(JointSegment const & rhs) const
|
||||
{
|
||||
if (m_featureId != rhs.m_featureId)
|
||||
return m_featureId < rhs.m_featureId;
|
||||
|
||||
if (m_forward != rhs.m_forward)
|
||||
return m_forward < rhs.m_forward;
|
||||
|
||||
if (m_startSegmentId != rhs.m_startSegmentId)
|
||||
return m_startSegmentId < rhs.m_startSegmentId;
|
||||
|
||||
if (m_endSegmentId != rhs.m_endSegmentId)
|
||||
return m_endSegmentId < rhs.m_endSegmentId;
|
||||
|
||||
return m_numMwmId < rhs.m_numMwmId;
|
||||
}
|
||||
|
||||
bool JointSegment::operator==(JointSegment const & rhs) const
|
||||
{
|
||||
return m_featureId == rhs.m_featureId && m_forward == rhs.m_forward && m_startSegmentId == rhs.m_startSegmentId &&
|
||||
m_endSegmentId == rhs.m_endSegmentId && m_numMwmId == rhs.m_numMwmId;
|
||||
}
|
||||
|
||||
std::string DebugPrint(JointSegment const & jointSegment)
|
||||
{
|
||||
std::ostringstream out;
|
||||
if (jointSegment.IsFake())
|
||||
out << "[FAKE]";
|
||||
|
||||
out << std::boolalpha << "JointSegment(" << jointSegment.GetMwmId() << ", " << jointSegment.GetFeatureId() << ", "
|
||||
<< "[" << jointSegment.GetStartSegmentId() << " => " << jointSegment.GetEndSegmentId() << "], "
|
||||
<< jointSegment.IsForward() << ")";
|
||||
return out.str();
|
||||
}
|
||||
} // namespace routing
|
||||
|
||||
namespace std
|
||||
{
|
||||
size_t std::hash<routing::JointSegment>::operator()(routing::JointSegment const & jointSegment) const
|
||||
{
|
||||
size_t seed = 0;
|
||||
boost::hash_combine(seed, jointSegment.GetMwmId());
|
||||
boost::hash_combine(seed, jointSegment.GetFeatureId());
|
||||
boost::hash_combine(seed, jointSegment.GetStartSegmentId());
|
||||
boost::hash_combine(seed, jointSegment.GetEndSegmentId());
|
||||
boost::hash_combine(seed, jointSegment.IsForward());
|
||||
return seed;
|
||||
}
|
||||
} // namespace std
|
||||
76
libs/routing/joint_segment.hpp
Normal file
76
libs/routing/joint_segment.hpp
Normal file
|
|
@ -0,0 +1,76 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/route_weight.hpp"
|
||||
#include "routing/segment.hpp"
|
||||
|
||||
#include "indexer/feature_decl.hpp"
|
||||
|
||||
#include <limits>
|
||||
#include <string>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
class JointSegment
|
||||
{
|
||||
public:
|
||||
static auto constexpr kInvalidSegmentId = std::numeric_limits<uint32_t>::max();
|
||||
|
||||
JointSegment() = default;
|
||||
JointSegment(Segment const & from, Segment const & to);
|
||||
|
||||
uint32_t GetFeatureId() const { return m_featureId; }
|
||||
NumMwmId GetMwmId() const { return m_numMwmId; }
|
||||
uint32_t GetStartSegmentId() const { return m_startSegmentId; }
|
||||
uint32_t GetEndSegmentId() const { return m_endSegmentId; }
|
||||
uint32_t GetSegmentId(bool start) const { return start ? m_startSegmentId : m_endSegmentId; }
|
||||
bool IsForward() const { return m_forward; }
|
||||
|
||||
void AssignID(Segment const & seg);
|
||||
void AssignID(JointSegment const & seg);
|
||||
|
||||
static JointSegment MakeFake(uint32_t fakeId, uint32_t featureId = kInvalidFeatureId);
|
||||
bool IsFake() const;
|
||||
|
||||
Segment GetSegment(bool start) const;
|
||||
|
||||
bool operator<(JointSegment const & rhs) const;
|
||||
bool operator==(JointSegment const & rhs) const;
|
||||
bool operator!=(JointSegment const & rhs) const { return !(*this == rhs); }
|
||||
|
||||
private:
|
||||
uint32_t m_featureId = kInvalidFeatureId;
|
||||
|
||||
uint32_t m_startSegmentId = kInvalidSegmentId;
|
||||
uint32_t m_endSegmentId = kInvalidSegmentId;
|
||||
|
||||
NumMwmId m_numMwmId = kFakeNumMwmId;
|
||||
bool m_forward = false;
|
||||
};
|
||||
|
||||
class JointEdge
|
||||
{
|
||||
public:
|
||||
JointEdge() = default; // needed for buffer_vector only
|
||||
JointEdge(JointSegment const & target, RouteWeight const & weight) : m_target(target), m_weight(weight) {}
|
||||
|
||||
JointSegment const & GetTarget() const { return m_target; }
|
||||
JointSegment & GetTarget() { return m_target; }
|
||||
RouteWeight & GetWeight() { return m_weight; }
|
||||
RouteWeight const & GetWeight() const { return m_weight; }
|
||||
|
||||
private:
|
||||
JointSegment m_target;
|
||||
RouteWeight m_weight;
|
||||
};
|
||||
|
||||
std::string DebugPrint(JointSegment const & jointSegment);
|
||||
} // namespace routing
|
||||
|
||||
namespace std
|
||||
{
|
||||
template <>
|
||||
struct hash<routing::JointSegment>
|
||||
{
|
||||
size_t operator()(routing::JointSegment const & jointSegment) const;
|
||||
};
|
||||
} // namespace std
|
||||
56
libs/routing/junction_visitor.cpp
Normal file
56
libs/routing/junction_visitor.cpp
Normal file
|
|
@ -0,0 +1,56 @@
|
|||
#include "junction_visitor.hpp"
|
||||
|
||||
#include "routing/joint_segment.hpp"
|
||||
#include "routing/route_weight.hpp"
|
||||
|
||||
#include "base/logging.hpp"
|
||||
|
||||
namespace routing
|
||||
{
|
||||
|
||||
#ifdef DEBUG
|
||||
// Leaps algorithm.
|
||||
void DebugRoutingState(Segment const & vertex, std::optional<Segment> const & parent, RouteWeight const & heuristic,
|
||||
RouteWeight const & distance)
|
||||
{
|
||||
// 1. Dump current processing vertex.
|
||||
// std::cout << DebugPrint(vertex);
|
||||
// std::cout << std::setprecision(8) << "; H = " << heuristic << "; D = " << distance;
|
||||
|
||||
// 2. Dump parent vertex.
|
||||
// std::cout << "; P = " << (parent ? DebugPrint(*parent) : std::string("NO"));
|
||||
|
||||
// std::cout << std::endl;
|
||||
}
|
||||
|
||||
// Joints algorithm.
|
||||
void DebugRoutingState(JointSegment const & vertex, std::optional<JointSegment> const & parent,
|
||||
RouteWeight const & heuristic, RouteWeight const & distance)
|
||||
{
|
||||
// 1. Dump current processing vertex.
|
||||
// std::cout << DebugPrint(vertex);
|
||||
// std::cout << std::setprecision(8) << "; H = " << heuristic << "; D = " << distance;
|
||||
|
||||
// 2. Dump parent vertex.
|
||||
// std::cout << "; P = " << (parent ? DebugPrint(*parent) : std::string("NO"));
|
||||
|
||||
// std::cout << std::endl;
|
||||
|
||||
// 3. Set breakpoint on a specific vertex.
|
||||
// if (vertex.GetMwmId() == 400 && vertex.GetFeatureId() == 2412 &&
|
||||
// vertex.GetEndSegmentId() == 0)
|
||||
// {
|
||||
// int noop = 0;
|
||||
// }
|
||||
|
||||
// 4. Set breakpoint when MWM was changed.
|
||||
// if (parent)
|
||||
// {
|
||||
// auto const & p = *parent;
|
||||
// if (!p.IsFake() && !vertex.IsFake() && p.GetMwmId() != vertex.GetMwmId())
|
||||
// int noop = 0;
|
||||
// }
|
||||
}
|
||||
#endif
|
||||
|
||||
} // namespace routing
|
||||
90
libs/routing/junction_visitor.hpp
Normal file
90
libs/routing/junction_visitor.hpp
Normal file
|
|
@ -0,0 +1,90 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/base/astar_progress.hpp"
|
||||
|
||||
#include "routing/router_delegate.hpp"
|
||||
|
||||
#include "geometry/point2d.hpp"
|
||||
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
double constexpr kProgressInterval = 0.5;
|
||||
|
||||
#ifdef DEBUG
|
||||
inline void DebugRoutingState(...) {}
|
||||
|
||||
class JointSegment;
|
||||
class RouteWeight;
|
||||
class Segment;
|
||||
void DebugRoutingState(Segment const & vertex, std::optional<Segment> const & parent, RouteWeight const & heuristic,
|
||||
RouteWeight const & distance);
|
||||
void DebugRoutingState(JointSegment const & vertex, std::optional<JointSegment> const & parent,
|
||||
RouteWeight const & heuristic, RouteWeight const & distance);
|
||||
#endif
|
||||
|
||||
template <typename Graph>
|
||||
class JunctionVisitor
|
||||
{
|
||||
public:
|
||||
using Vertex = typename Graph::Vertex;
|
||||
|
||||
JunctionVisitor(Graph & graph, RouterDelegate const & delegate, uint32_t visitPeriod,
|
||||
std::shared_ptr<AStarProgress> const & progress = nullptr)
|
||||
: m_graph(graph)
|
||||
, m_delegate(delegate)
|
||||
, m_visitPeriod(visitPeriod)
|
||||
, m_progress(progress)
|
||||
{
|
||||
if (progress)
|
||||
m_lastProgressPercent = progress->GetLastPercent();
|
||||
}
|
||||
|
||||
/// @param[in] p { Current state, Step context } pair.
|
||||
/// @param[in] to End vertex (final for forward and start for backward waves).
|
||||
template <class StateContextPair>
|
||||
void operator()(StateContextPair const & p, Vertex const & to)
|
||||
{
|
||||
auto const & state = p.first;
|
||||
#ifdef DEBUG
|
||||
// For Debug purpose.
|
||||
DebugRoutingState(state.vertex, p.second->GetParent(state.vertex), state.heuristic, state.distance);
|
||||
#endif
|
||||
this->operator()(state.vertex, to);
|
||||
}
|
||||
|
||||
/// @param[in] from Current processing vertex.
|
||||
/// @param[in] to End vertex (final for forward and start for backward waves).
|
||||
void operator()(Vertex const & from, Vertex const & to)
|
||||
{
|
||||
++m_visitCounter;
|
||||
if (m_visitCounter % m_visitPeriod != 0)
|
||||
return;
|
||||
|
||||
auto const & pointFrom = m_graph.GetPoint(from, true /* front */);
|
||||
m_delegate.OnPointCheck(pointFrom);
|
||||
|
||||
auto progress = m_progress.lock();
|
||||
if (!progress)
|
||||
return;
|
||||
|
||||
auto const & pointTo = m_graph.GetPoint(to, true /* front */);
|
||||
auto const currentPercent = progress->UpdateProgress(pointFrom, pointTo);
|
||||
if (currentPercent - m_lastProgressPercent > kProgressInterval)
|
||||
{
|
||||
m_lastProgressPercent = currentPercent;
|
||||
m_delegate.OnProgress(currentPercent);
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
Graph & m_graph;
|
||||
RouterDelegate const & m_delegate;
|
||||
uint32_t m_visitCounter = 0;
|
||||
uint32_t m_visitPeriod;
|
||||
std::weak_ptr<AStarProgress> m_progress;
|
||||
double m_lastProgressPercent = 0.0;
|
||||
};
|
||||
} // namespace routing
|
||||
24
libs/routing/lanes/lane_info.cpp
Normal file
24
libs/routing/lanes/lane_info.cpp
Normal file
|
|
@ -0,0 +1,24 @@
|
|||
#include "lane_info.hpp"
|
||||
|
||||
#include <sstream>
|
||||
|
||||
namespace routing::turns::lanes
|
||||
{
|
||||
std::string DebugPrint(LaneInfo const & laneInfo)
|
||||
{
|
||||
std::stringstream out;
|
||||
out << "LaneInfo{" << DebugPrint(laneInfo.laneWays) << ", recommendedWay: " << DebugPrint(laneInfo.recommendedWay)
|
||||
<< "}";
|
||||
return out.str();
|
||||
}
|
||||
|
||||
std::string DebugPrint(LanesInfo const & lanesInfo)
|
||||
{
|
||||
std::stringstream out;
|
||||
out << "LanesInfo[";
|
||||
for (auto const & laneInfo : lanesInfo)
|
||||
out << DebugPrint(laneInfo) << ", ";
|
||||
out << "]";
|
||||
return out.str();
|
||||
}
|
||||
} // namespace routing::turns::lanes
|
||||
23
libs/routing/lanes/lane_info.hpp
Normal file
23
libs/routing/lanes/lane_info.hpp
Normal file
|
|
@ -0,0 +1,23 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/lanes/lane_way.hpp"
|
||||
|
||||
#include <vector>
|
||||
|
||||
namespace routing::turns::lanes
|
||||
{
|
||||
struct LaneInfo
|
||||
{
|
||||
LaneWays laneWays;
|
||||
LaneWay recommendedWay = LaneWay::None;
|
||||
|
||||
bool operator==(LaneInfo const & rhs) const
|
||||
{
|
||||
return laneWays == rhs.laneWays && recommendedWay == rhs.recommendedWay;
|
||||
}
|
||||
};
|
||||
using LanesInfo = std::vector<LaneInfo>;
|
||||
|
||||
std::string DebugPrint(LaneInfo const & laneInfo);
|
||||
std::string DebugPrint(LanesInfo const & lanesInfo);
|
||||
} // namespace routing::turns::lanes
|
||||
50
libs/routing/lanes/lane_way.cpp
Normal file
50
libs/routing/lanes/lane_way.cpp
Normal file
|
|
@ -0,0 +1,50 @@
|
|||
#include "lane_way.hpp"
|
||||
|
||||
#include "base/assert.hpp"
|
||||
|
||||
namespace routing::turns::lanes
|
||||
{
|
||||
std::string DebugPrint(LaneWay const laneWay)
|
||||
{
|
||||
using enum LaneWay;
|
||||
switch (laneWay)
|
||||
{
|
||||
case None: return "None";
|
||||
case ReverseLeft: return "ReverseLeft";
|
||||
case SharpLeft: return "SharpLeft";
|
||||
case Left: return "Left";
|
||||
case MergeToLeft: return "MergeToLeft";
|
||||
case SlightLeft: return "SlightLeft";
|
||||
case Through: return "Through";
|
||||
case SlightRight: return "SlightRight";
|
||||
case MergeToRight: return "MergeToRight";
|
||||
case Right: return "Right";
|
||||
case SharpRight: return "SharpRight";
|
||||
case ReverseRight: return "ReverseRight";
|
||||
case Count: return "Count";
|
||||
default:
|
||||
ASSERT_FAIL("Unsupported value: " + std::to_string(static_cast<std::uint8_t>(laneWay)));
|
||||
return "Unsupported";
|
||||
}
|
||||
}
|
||||
|
||||
std::string DebugPrint(LaneWays const & laneWays)
|
||||
{
|
||||
std::stringstream out;
|
||||
out << "LaneWays: [";
|
||||
std::uint8_t const waysCount = laneWays.m_laneWays.count();
|
||||
std::uint8_t waysPrinted = 0;
|
||||
for (std::size_t i = 0; i < laneWays.m_laneWays.size(); ++i)
|
||||
{
|
||||
if (laneWays.m_laneWays.test(i))
|
||||
{
|
||||
out << DebugPrint(static_cast<LaneWay>(i));
|
||||
if (waysPrinted < waysCount - 1)
|
||||
out << ", ";
|
||||
waysPrinted++;
|
||||
}
|
||||
}
|
||||
out << "]";
|
||||
return out.str();
|
||||
}
|
||||
} // namespace routing::turns::lanes
|
||||
84
libs/routing/lanes/lane_way.hpp
Normal file
84
libs/routing/lanes/lane_way.hpp
Normal file
|
|
@ -0,0 +1,84 @@
|
|||
#pragma once
|
||||
|
||||
#include "base/assert.hpp"
|
||||
|
||||
#include <bitset>
|
||||
#include <initializer_list>
|
||||
#include <string>
|
||||
|
||||
namespace routing::turns::lanes
|
||||
{
|
||||
enum class LaneWay : std::uint8_t
|
||||
{
|
||||
None = 0,
|
||||
ReverseLeft,
|
||||
SharpLeft,
|
||||
Left,
|
||||
MergeToLeft,
|
||||
SlightLeft,
|
||||
Through,
|
||||
SlightRight,
|
||||
MergeToRight,
|
||||
Right,
|
||||
SharpRight,
|
||||
ReverseRight,
|
||||
|
||||
Count
|
||||
};
|
||||
|
||||
class LaneWays
|
||||
{
|
||||
using LaneWaysT = std::bitset<static_cast<std::uint8_t>(LaneWay::Count)>;
|
||||
|
||||
friend std::string DebugPrint(LaneWays const & laneWays);
|
||||
|
||||
public:
|
||||
constexpr LaneWays() = default;
|
||||
constexpr LaneWays(std::initializer_list<LaneWay> const laneWays)
|
||||
{
|
||||
for (auto const & laneWay : laneWays)
|
||||
Add(laneWay);
|
||||
}
|
||||
|
||||
constexpr bool operator==(LaneWays const & rhs) const { return m_laneWays == rhs.m_laneWays; }
|
||||
|
||||
constexpr void Add(LaneWay laneWay)
|
||||
{
|
||||
ASSERT_LESS(laneWay, LaneWay::Count, ());
|
||||
m_laneWays.set(static_cast<std::uint8_t>(laneWay));
|
||||
}
|
||||
|
||||
constexpr void Remove(LaneWay laneWay)
|
||||
{
|
||||
ASSERT_LESS(laneWay, LaneWay::Count, ());
|
||||
m_laneWays.reset(static_cast<std::uint8_t>(laneWay));
|
||||
}
|
||||
|
||||
constexpr bool Contains(LaneWay laneWay) const
|
||||
{
|
||||
ASSERT_LESS(laneWay, LaneWay::Count, ());
|
||||
return m_laneWays.test(static_cast<std::uint8_t>(laneWay));
|
||||
}
|
||||
|
||||
/// An unrestricted lane is a lane that has no restrictions, i.e., it contains no lane ways.
|
||||
constexpr bool IsUnrestricted() const
|
||||
{
|
||||
return m_laneWays.none() || (m_laneWays.count() == 1 && Contains(LaneWay::None));
|
||||
}
|
||||
|
||||
[[nodiscard]] std::vector<LaneWay> GetActiveLaneWays() const
|
||||
{
|
||||
std::vector<LaneWay> result;
|
||||
for (std::size_t i = 0; i < m_laneWays.size(); ++i)
|
||||
if (m_laneWays.test(i))
|
||||
result.emplace_back(static_cast<LaneWay>(i));
|
||||
return result;
|
||||
}
|
||||
|
||||
private:
|
||||
LaneWaysT m_laneWays;
|
||||
};
|
||||
|
||||
std::string DebugPrint(LaneWay laneWay);
|
||||
std::string DebugPrint(LaneWays const & laneWays);
|
||||
} // namespace routing::turns::lanes
|
||||
84
libs/routing/lanes/lanes_parser.cpp
Normal file
84
libs/routing/lanes/lanes_parser.cpp
Normal file
|
|
@ -0,0 +1,84 @@
|
|||
#include "lanes_parser.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <ranges>
|
||||
|
||||
namespace routing::turns::lanes
|
||||
{
|
||||
namespace
|
||||
{
|
||||
std::uint8_t constexpr kLaneWayNamesCount = static_cast<std::uint8_t>(LaneWay::Count) + 4;
|
||||
|
||||
/**
|
||||
* The order is important. Starting with the most frequent tokens according to
|
||||
* taginfo.openstreetmap.org we minimize the number of the comparisons in ParseSingleLane().
|
||||
*
|
||||
* A `none` lane can be represented either as "none" or as "". That means both "none" and ""
|
||||
* should be considered names, even though they refer to the same thing. As a result,
|
||||
* `LaneWay::None` appears twice in this array, which is one longer than the number of
|
||||
* enum values.
|
||||
*/
|
||||
std::array<std::pair<LaneWay, std::string_view>, kLaneWayNamesCount> constexpr g_laneWayNames{{
|
||||
{LaneWay::None, ""},
|
||||
{LaneWay::Through, "through"},
|
||||
{LaneWay::Left, "left"},
|
||||
{LaneWay::Right, "right"},
|
||||
{LaneWay::None, "none"},
|
||||
{LaneWay::SharpLeft, "sharp_left"},
|
||||
{LaneWay::SlightLeft, "slight_left"},
|
||||
{LaneWay::MergeToRight, "merge_to_right"},
|
||||
{LaneWay::MergeToLeft, "merge_to_left"},
|
||||
{LaneWay::SlightRight, "slight_right"},
|
||||
{LaneWay::SharpRight, "sharp_right"},
|
||||
{LaneWay::ReverseLeft, "reverse"},
|
||||
{LaneWay::Right,
|
||||
"next_right"}, // "next_right" means "turn right, not in the first intersection but the one after that".
|
||||
{LaneWay::Through, "slide_left"}, // "slide_left" means "move a bit left within the lane".
|
||||
{LaneWay::Through, "slide_right"} // "slide_right" means "move a bit right within the lane".
|
||||
}};
|
||||
|
||||
bool ParseSingleLane(auto && laneWayRange, LaneWay & laneWay)
|
||||
{
|
||||
auto const it = std::ranges::find_if(
|
||||
g_laneWayNames, [&laneWayRange](auto const & pair) { return std::ranges::equal(laneWayRange, pair.second); });
|
||||
if (it != g_laneWayNames.end())
|
||||
{
|
||||
laneWay = it->first;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
LanesInfo ParseLanes(std::string_view lanesString)
|
||||
{
|
||||
if (lanesString.empty())
|
||||
return {};
|
||||
|
||||
LanesInfo lanes;
|
||||
for (auto && laneInfo : lanesString | std::views::split('|'))
|
||||
{
|
||||
LaneInfo lane;
|
||||
if (std::ranges::empty(laneInfo))
|
||||
lane.laneWays.Add(LaneWay::None);
|
||||
else
|
||||
{
|
||||
for (auto && laneWay : laneInfo | std::views::split(';'))
|
||||
{
|
||||
auto way = LaneWay::None;
|
||||
auto && laneWayProcessed = laneWay | std::views::filter([](char const c) { return !std::isspace(c); }) |
|
||||
std::views::transform([](char const c) { return std::tolower(c); });
|
||||
if (!ParseSingleLane(laneWayProcessed, way))
|
||||
return {};
|
||||
lane.laneWays.Add(way);
|
||||
if (way == LaneWay::ReverseLeft)
|
||||
lane.laneWays.Add(LaneWay::ReverseRight);
|
||||
}
|
||||
}
|
||||
|
||||
lanes.push_back(lane);
|
||||
}
|
||||
return lanes;
|
||||
}
|
||||
} // namespace routing::turns::lanes
|
||||
16
libs/routing/lanes/lanes_parser.hpp
Normal file
16
libs/routing/lanes/lanes_parser.hpp
Normal file
|
|
@ -0,0 +1,16 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/lanes/lane_info.hpp"
|
||||
|
||||
#include <vector>
|
||||
|
||||
namespace routing::turns::lanes
|
||||
{
|
||||
/**
|
||||
* Parse lane information which comes from lanesString
|
||||
* @param lanesString lane information. Example through|through|through|through;right
|
||||
* @return LanesInfo. @see LanesInfo
|
||||
* @note if lanesString is empty, returns empty LanesInfo.
|
||||
*/
|
||||
LanesInfo ParseLanes(std::string_view lanesString);
|
||||
} // namespace routing::turns::lanes
|
||||
129
libs/routing/lanes/lanes_recommendation.cpp
Normal file
129
libs/routing/lanes/lanes_recommendation.cpp
Normal file
|
|
@ -0,0 +1,129 @@
|
|||
#include "lanes_recommendation.hpp"
|
||||
|
||||
#include "routing/route.hpp"
|
||||
|
||||
namespace routing::turns::lanes
|
||||
{
|
||||
namespace
|
||||
{
|
||||
void FixRecommendedReverseLane(LaneWays & ways, LaneWay const recommendedWay)
|
||||
{
|
||||
if (recommendedWay == LaneWay::ReverseLeft)
|
||||
ways.Remove(LaneWay::ReverseRight);
|
||||
else if (recommendedWay == LaneWay::ReverseRight)
|
||||
ways.Remove(LaneWay::ReverseLeft);
|
||||
}
|
||||
} // namespace
|
||||
|
||||
void SelectRecommendedLanes(std::vector<RouteSegment> & routeSegments)
|
||||
{
|
||||
for (auto & segment : routeSegments)
|
||||
{
|
||||
auto & t = segment.GetTurn();
|
||||
if (t.IsTurnNone() || t.m_lanes.empty())
|
||||
continue;
|
||||
auto & lanesInfo = segment.GetTurnLanes();
|
||||
// Check if there are elements in lanesInfo that correspond with the turn exactly.
|
||||
// If so, fix up all the elements in lanesInfo that correspond with the turn.
|
||||
if (impl::SetRecommendedLaneWays(t.m_turn, lanesInfo))
|
||||
continue;
|
||||
// If not, check if there are elements in lanesInfo that correspond with the turn
|
||||
// approximately. If so, fix up all those elements.
|
||||
if (impl::SetRecommendedLaneWaysApproximately(t.m_turn, lanesInfo))
|
||||
continue;
|
||||
// If not, check if there is an unrestricted lane that could correspond to the
|
||||
// turn. If so, fix up that lane.
|
||||
if (impl::SetUnrestrictedLaneAsRecommended(t.m_turn, lanesInfo))
|
||||
continue;
|
||||
// Otherwise, we don't have lane recommendations for the user, so we don't
|
||||
// want to send the lane data any further.
|
||||
segment.ClearTurnLanes();
|
||||
}
|
||||
}
|
||||
|
||||
bool impl::SetRecommendedLaneWays(CarDirection const carDirection, LanesInfo & lanesInfo)
|
||||
{
|
||||
LaneWay laneWay;
|
||||
switch (carDirection)
|
||||
{
|
||||
case CarDirection::GoStraight: laneWay = LaneWay::Through; break;
|
||||
case CarDirection::TurnRight: laneWay = LaneWay::Right; break;
|
||||
case CarDirection::TurnSharpRight: laneWay = LaneWay::SharpRight; break;
|
||||
case CarDirection::TurnSlightRight: [[fallthrough]];
|
||||
case CarDirection::ExitHighwayToRight: laneWay = LaneWay::SlightRight; break;
|
||||
case CarDirection::TurnLeft: laneWay = LaneWay::Left; break;
|
||||
case CarDirection::TurnSharpLeft: laneWay = LaneWay::SharpLeft; break;
|
||||
case CarDirection::TurnSlightLeft: [[fallthrough]];
|
||||
case CarDirection::ExitHighwayToLeft: laneWay = LaneWay::SlightLeft; break;
|
||||
case CarDirection::UTurnLeft: laneWay = LaneWay::ReverseLeft; break;
|
||||
case CarDirection::UTurnRight: laneWay = LaneWay::ReverseRight; break;
|
||||
default: return false;
|
||||
}
|
||||
|
||||
bool isLaneConformed = false;
|
||||
for (auto & [laneWays, recommendedWay] : lanesInfo)
|
||||
{
|
||||
if (laneWays.Contains(laneWay))
|
||||
{
|
||||
recommendedWay = laneWay;
|
||||
isLaneConformed = true;
|
||||
}
|
||||
FixRecommendedReverseLane(laneWays, recommendedWay);
|
||||
}
|
||||
return isLaneConformed;
|
||||
}
|
||||
|
||||
bool impl::SetRecommendedLaneWaysApproximately(CarDirection const carDirection, LanesInfo & lanesInfo)
|
||||
{
|
||||
std::vector<LaneWay> approximateLaneWays;
|
||||
switch (carDirection)
|
||||
{
|
||||
case CarDirection::UTurnLeft: approximateLaneWays = {LaneWay::SharpLeft}; break;
|
||||
case CarDirection::TurnSharpLeft: approximateLaneWays = {LaneWay::Left}; break;
|
||||
case CarDirection::TurnLeft: approximateLaneWays = {LaneWay::SlightLeft, LaneWay::SharpLeft}; break;
|
||||
case CarDirection::TurnSlightLeft: [[fallthrough]];
|
||||
case CarDirection::ExitHighwayToLeft: approximateLaneWays = {LaneWay::Left}; break;
|
||||
case CarDirection::GoStraight: approximateLaneWays = {LaneWay::SlightRight, LaneWay::SlightLeft}; break;
|
||||
case CarDirection::ExitHighwayToRight: [[fallthrough]];
|
||||
case CarDirection::TurnSlightRight: approximateLaneWays = {LaneWay::Right}; break;
|
||||
case CarDirection::TurnRight: approximateLaneWays = {LaneWay::SlightRight, LaneWay::SharpRight}; break;
|
||||
case CarDirection::TurnSharpRight: approximateLaneWays = {LaneWay::Right}; break;
|
||||
case CarDirection::UTurnRight: approximateLaneWays = {LaneWay::SharpRight}; break;
|
||||
default: return false;
|
||||
}
|
||||
|
||||
bool isLaneConformed = false;
|
||||
for (auto & [laneWays, recommendedWay] : lanesInfo)
|
||||
{
|
||||
for (auto const & laneWay : approximateLaneWays)
|
||||
{
|
||||
if (laneWays.Contains(laneWay))
|
||||
{
|
||||
recommendedWay = laneWay;
|
||||
isLaneConformed = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return isLaneConformed;
|
||||
}
|
||||
|
||||
bool impl::SetUnrestrictedLaneAsRecommended(CarDirection const carDirection, LanesInfo & lanesInfo)
|
||||
{
|
||||
static auto constexpr setFirstUnrestrictedLane = [](LaneWay const laneWay, auto beginIt, auto endIt)
|
||||
{
|
||||
auto it = std::find_if(beginIt, endIt, [](auto const & laneInfo) { return laneInfo.laneWays.IsUnrestricted(); });
|
||||
if (it == endIt)
|
||||
return false;
|
||||
it->recommendedWay = laneWay;
|
||||
return true;
|
||||
};
|
||||
|
||||
if (IsTurnMadeFromLeft(carDirection))
|
||||
return setFirstUnrestrictedLane(LaneWay::Left, lanesInfo.begin(), lanesInfo.end());
|
||||
if (IsTurnMadeFromRight(carDirection))
|
||||
return setFirstUnrestrictedLane(LaneWay::Right, lanesInfo.rbegin(), lanesInfo.rend());
|
||||
return false;
|
||||
}
|
||||
} // namespace routing::turns::lanes
|
||||
31
libs/routing/lanes/lanes_recommendation.hpp
Normal file
31
libs/routing/lanes/lanes_recommendation.hpp
Normal file
|
|
@ -0,0 +1,31 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/lanes/lane_info.hpp"
|
||||
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
class RouteSegment;
|
||||
|
||||
namespace turns
|
||||
{
|
||||
enum class CarDirection;
|
||||
} // namespace turns
|
||||
} // namespace routing
|
||||
|
||||
namespace routing::turns::lanes
|
||||
{
|
||||
/// Selects lanes which are recommended for an end user.
|
||||
void SelectRecommendedLanes(std::vector<RouteSegment> & routeSegments);
|
||||
|
||||
// Keep signatures in the header for testing purposes
|
||||
namespace impl
|
||||
{
|
||||
bool SetRecommendedLaneWays(CarDirection carDirection, LanesInfo & lanesInfo);
|
||||
|
||||
bool SetRecommendedLaneWaysApproximately(CarDirection carDirection, LanesInfo & lanesInfo);
|
||||
|
||||
bool SetUnrestrictedLaneAsRecommended(CarDirection carDirection, LanesInfo & lanesInfo);
|
||||
} // namespace impl
|
||||
} // namespace routing::turns::lanes
|
||||
32
libs/routing/latlon_with_altitude.cpp
Normal file
32
libs/routing/latlon_with_altitude.cpp
Normal file
|
|
@ -0,0 +1,32 @@
|
|||
#include "routing/latlon_with_altitude.hpp"
|
||||
|
||||
#include "geometry/mercator.hpp"
|
||||
|
||||
#include <sstream>
|
||||
#include <utility>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
std::string DebugPrint(LatLonWithAltitude const & latLonWithAltitude)
|
||||
{
|
||||
std::stringstream ss;
|
||||
ss << "LatLonWithAltitude(" << DebugPrint(latLonWithAltitude.GetLatLon()) << ", " << latLonWithAltitude.GetAltitude()
|
||||
<< ")";
|
||||
return ss.str();
|
||||
}
|
||||
|
||||
bool LatLonWithAltitude::operator<(LatLonWithAltitude const & rhs) const
|
||||
{
|
||||
return m_latlon < rhs.m_latlon;
|
||||
}
|
||||
|
||||
bool LatLonWithAltitude::operator==(LatLonWithAltitude const & rhs) const
|
||||
{
|
||||
return m_latlon == rhs.m_latlon;
|
||||
}
|
||||
|
||||
geometry::PointWithAltitude LatLonWithAltitude::ToPointWithAltitude() const
|
||||
{
|
||||
return geometry::PointWithAltitude(mercator::FromLatLon(m_latlon), m_altitude);
|
||||
}
|
||||
} // namespace routing
|
||||
30
libs/routing/latlon_with_altitude.hpp
Normal file
30
libs/routing/latlon_with_altitude.hpp
Normal file
|
|
@ -0,0 +1,30 @@
|
|||
#pragma once
|
||||
|
||||
#include "geometry/latlon.hpp"
|
||||
#include "geometry/point_with_altitude.hpp"
|
||||
|
||||
#include <string>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
class LatLonWithAltitude
|
||||
{
|
||||
public:
|
||||
LatLonWithAltitude() = default;
|
||||
LatLonWithAltitude(ms::LatLon const & latlon, geometry::Altitude altitude) : m_latlon(latlon), m_altitude(altitude) {}
|
||||
|
||||
bool operator==(LatLonWithAltitude const & rhs) const;
|
||||
bool operator<(LatLonWithAltitude const & rhs) const;
|
||||
|
||||
ms::LatLon const & GetLatLon() const { return m_latlon; }
|
||||
geometry::Altitude GetAltitude() const { return m_altitude; }
|
||||
|
||||
geometry::PointWithAltitude ToPointWithAltitude() const;
|
||||
|
||||
private:
|
||||
ms::LatLon m_latlon;
|
||||
geometry::Altitude m_altitude = geometry::kDefaultAltitudeMeters;
|
||||
};
|
||||
|
||||
std::string DebugPrint(LatLonWithAltitude const & latLonWithAltitude);
|
||||
} // namespace routing
|
||||
145
libs/routing/leaps_graph.cpp
Normal file
145
libs/routing/leaps_graph.cpp
Normal file
|
|
@ -0,0 +1,145 @@
|
|||
#include "routing/leaps_graph.hpp"
|
||||
|
||||
#include "routing/cross_mwm_graph.hpp"
|
||||
#include "routing/index_graph_starter.hpp"
|
||||
|
||||
#include "base/assert.hpp"
|
||||
|
||||
#include <set>
|
||||
#include <utility>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
LeapsGraph::LeapsGraph(IndexGraphStarter & starter, MwmHierarchyHandler && hierarchyHandler)
|
||||
: m_starter(starter)
|
||||
, m_hierarchyHandler(std::move(hierarchyHandler))
|
||||
{
|
||||
m_startPoint = m_starter.GetPoint(m_starter.GetStartSegment(), true /* front */);
|
||||
m_finishPoint = m_starter.GetPoint(m_starter.GetFinishSegment(), true /* front */);
|
||||
m_startSegment = m_starter.GetStartSegment();
|
||||
m_finishSegment = m_starter.GetFinishSegment();
|
||||
}
|
||||
|
||||
void LeapsGraph::GetOutgoingEdgesList(astar::VertexData<Vertex, Weight> const & vertexData, EdgeListT & edges)
|
||||
{
|
||||
GetEdgesList(vertexData.m_vertex, true /* isOutgoing */, edges);
|
||||
}
|
||||
|
||||
void LeapsGraph::GetIngoingEdgesList(astar::VertexData<Vertex, Weight> const & vertexData, EdgeListT & edges)
|
||||
{
|
||||
GetEdgesList(vertexData.m_vertex, false /* isOutgoing */, edges);
|
||||
}
|
||||
|
||||
RouteWeight LeapsGraph::HeuristicCostEstimate(Segment const & from, Segment const & to)
|
||||
{
|
||||
ASSERT(to == m_startSegment || to == m_finishSegment, ());
|
||||
bool const toFinish = to == m_finishSegment;
|
||||
auto const & toPoint = toFinish ? m_finishPoint : m_startPoint;
|
||||
return m_starter.HeuristicCostEstimate(from, toPoint);
|
||||
}
|
||||
|
||||
void LeapsGraph::GetEdgesList(Segment const & segment, bool isOutgoing, EdgeListT & edges)
|
||||
{
|
||||
edges.clear();
|
||||
|
||||
if (segment == m_startSegment)
|
||||
{
|
||||
CHECK(isOutgoing, ("Only forward wave of A* should get edges from start. Backward wave should "
|
||||
"stop when first time visit the |m_startSegment|."));
|
||||
return GetEdgesListFromStart(edges);
|
||||
}
|
||||
|
||||
if (segment == m_finishSegment)
|
||||
{
|
||||
CHECK(!isOutgoing, ("Only backward wave of A* should get edges to finish. Forward wave should "
|
||||
"stop when first time visit the |m_finishSegment|."));
|
||||
return GetEdgesListToFinish(edges);
|
||||
}
|
||||
|
||||
if (!m_starter.IsRoutingOptionsGood(segment))
|
||||
return;
|
||||
|
||||
auto & crossMwmGraph = m_starter.GetGraph().GetCrossMwmGraph();
|
||||
|
||||
if (crossMwmGraph.IsTransition(segment, isOutgoing))
|
||||
{
|
||||
auto const segMwmId = segment.GetMwmId();
|
||||
|
||||
std::vector<Segment> twins;
|
||||
m_starter.GetGraph().GetTwinsInner(segment, isOutgoing, twins);
|
||||
for (auto const & twin : twins)
|
||||
edges.emplace_back(twin, m_hierarchyHandler.GetCrossBorderPenalty(segMwmId, twin.GetMwmId()));
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
if (isOutgoing)
|
||||
crossMwmGraph.GetOutgoingEdgeList(segment, edges);
|
||||
else
|
||||
crossMwmGraph.GetIngoingEdgeList(segment, edges);
|
||||
}
|
||||
|
||||
void LeapsGraph::GetEdgesListFromStart(EdgeListT & edges) const
|
||||
{
|
||||
for (auto const mwmId : m_starter.GetStartMwms())
|
||||
{
|
||||
// Connect start to all exits (|isEnter| == false).
|
||||
m_starter.GetGraph().ForEachTransition(mwmId, false /* isEnter */, [&](Segment const & exit)
|
||||
{
|
||||
auto const & exitFrontPoint = m_starter.GetPoint(exit, true /* front */);
|
||||
auto const weight = m_starter.GetGraph().CalcLeapWeight(m_startPoint, exitFrontPoint, mwmId);
|
||||
|
||||
edges.emplace_back(exit, weight);
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
void LeapsGraph::GetEdgesListToFinish(EdgeListT & edges) const
|
||||
{
|
||||
for (auto const mwmId : m_starter.GetFinishMwms())
|
||||
{
|
||||
// Connect finish to all enters (|isEnter| == true).
|
||||
m_starter.GetGraph().ForEachTransition(mwmId, true /* isEnter */, [&](Segment const & enter)
|
||||
{
|
||||
auto const & enterFrontPoint = m_starter.GetPoint(enter, true /* front */);
|
||||
auto const weight = m_starter.GetGraph().CalcLeapWeight(enterFrontPoint, m_finishPoint, mwmId);
|
||||
|
||||
edges.emplace_back(enter, weight);
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
ms::LatLon const & LeapsGraph::GetPoint(Segment const & segment, bool front) const
|
||||
{
|
||||
return m_starter.GetPoint(segment, front);
|
||||
}
|
||||
|
||||
RouteWeight LeapsGraph::GetAStarWeightEpsilon()
|
||||
{
|
||||
return m_starter.GetAStarWeightEpsilon();
|
||||
}
|
||||
|
||||
RouteWeight LeapsGraph::CalcMiddleCrossMwmWeight(std::vector<Segment> const & path)
|
||||
{
|
||||
ASSERT_GREATER(path.size(), 1, ());
|
||||
auto & crossMwmGraph = m_starter.GetGraph().GetCrossMwmGraph();
|
||||
|
||||
RouteWeight res;
|
||||
for (size_t i = 1; i < path.size() - 2; ++i)
|
||||
{
|
||||
auto const & from = path[i];
|
||||
auto const & to = path[i + 1];
|
||||
NumMwmId const fromMwm = from.GetMwmId();
|
||||
NumMwmId const toMwm = to.GetMwmId();
|
||||
ASSERT(fromMwm != kFakeNumMwmId && toMwm != kFakeNumMwmId, ());
|
||||
|
||||
if (fromMwm != toMwm)
|
||||
res += m_hierarchyHandler.GetCrossBorderPenalty(fromMwm, toMwm);
|
||||
else
|
||||
res += crossMwmGraph.GetWeightSure(from, to);
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
} // namespace routing
|
||||
53
libs/routing/leaps_graph.hpp
Normal file
53
libs/routing/leaps_graph.hpp
Normal file
|
|
@ -0,0 +1,53 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/base/astar_graph.hpp"
|
||||
#include "routing/base/astar_vertex_data.hpp"
|
||||
#include "routing/mwm_hierarchy_handler.hpp"
|
||||
#include "routing/route_weight.hpp"
|
||||
#include "routing/segment.hpp"
|
||||
|
||||
#include "geometry/latlon.hpp"
|
||||
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
class IndexGraphStarter;
|
||||
|
||||
class LeapsGraph : public AStarGraph<Segment, SegmentEdge, RouteWeight>
|
||||
{
|
||||
public:
|
||||
LeapsGraph(IndexGraphStarter & starter, MwmHierarchyHandler && hierarchyHandler);
|
||||
|
||||
// AStarGraph overrides:
|
||||
// @{
|
||||
void GetOutgoingEdgesList(astar::VertexData<Vertex, Weight> const & vertexData, EdgeListT & edges) override;
|
||||
void GetIngoingEdgesList(astar::VertexData<Vertex, Weight> const & vertexData, EdgeListT & edges) override;
|
||||
RouteWeight HeuristicCostEstimate(Segment const & from, Segment const & to) override;
|
||||
RouteWeight GetAStarWeightEpsilon() override;
|
||||
// @}
|
||||
|
||||
Segment const & GetStartSegment() const { return m_startSegment; }
|
||||
Segment const & GetFinishSegment() const { return m_finishSegment; }
|
||||
ms::LatLon const & GetPoint(Segment const & segment, bool front) const;
|
||||
|
||||
RouteWeight CalcMiddleCrossMwmWeight(std::vector<Segment> const & path);
|
||||
|
||||
private:
|
||||
void GetEdgesList(Segment const & segment, bool isOutgoing, EdgeListT & edges);
|
||||
|
||||
void GetEdgesListFromStart(EdgeListT & edges) const;
|
||||
void GetEdgesListToFinish(EdgeListT & edges) const;
|
||||
|
||||
private:
|
||||
ms::LatLon m_startPoint;
|
||||
ms::LatLon m_finishPoint;
|
||||
|
||||
Segment m_startSegment;
|
||||
Segment m_finishSegment;
|
||||
|
||||
IndexGraphStarter & m_starter;
|
||||
|
||||
MwmHierarchyHandler m_hierarchyHandler;
|
||||
};
|
||||
} // namespace routing
|
||||
224
libs/routing/leaps_postprocessor.cpp
Normal file
224
libs/routing/leaps_postprocessor.cpp
Normal file
|
|
@ -0,0 +1,224 @@
|
|||
#include "routing/leaps_postprocessor.hpp"
|
||||
|
||||
#include "base/assert.hpp"
|
||||
#include "base/logging.hpp"
|
||||
#include "base/non_intersecting_intervals.hpp"
|
||||
#include "base/scope_guard.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <iterator>
|
||||
#include <sstream>
|
||||
#include <utility>
|
||||
|
||||
namespace
|
||||
{
|
||||
using namespace routing;
|
||||
|
||||
class NonIntersectingSegmentPaths
|
||||
{
|
||||
public:
|
||||
void AddInterval(LeapsPostProcessor::PathInterval const & pathInterval)
|
||||
{
|
||||
if (!m_nonIntersectingIntervals.AddInterval(pathInterval.m_left, pathInterval.m_right))
|
||||
return;
|
||||
|
||||
m_intervals.emplace_back(pathInterval);
|
||||
}
|
||||
|
||||
std::vector<LeapsPostProcessor::PathInterval> && StealIntervals() { return std::move(m_intervals); }
|
||||
|
||||
private:
|
||||
base::NonIntersectingIntervals<size_t> m_nonIntersectingIntervals;
|
||||
std::vector<LeapsPostProcessor::PathInterval> m_intervals;
|
||||
};
|
||||
} // namespace
|
||||
|
||||
namespace routing
|
||||
{
|
||||
// static
|
||||
size_t const LeapsPostProcessor::kMaxStep = 5;
|
||||
|
||||
/// \brief It is assumed that there in no cycles in path.
|
||||
/// But sometimes it's wrong (because of heuristic searching of intermediate
|
||||
/// points (mwms' exists) in LeapsOnly routing algorithm).
|
||||
/// Look here for screenshots of such problem: https://github.com/mapsme/omim/pull/11091
|
||||
/// This code removes cycles from path before |Init()|.
|
||||
/// The algorithm saves jumps between the same segments, so if we have such loop:
|
||||
/// s1, s2, ... , s10, s1, s11, s12, ... , s100, where "s3" means segment number 3.
|
||||
/// ^------------------^ <--- loop (from s1 to s1).
|
||||
/// Thus if we look at the first s1, we will find out that a jump to second s1 exists
|
||||
/// and we will skip all segments between first and second s1 and the result path will be:
|
||||
/// s1, s11, s12, ... , s100
|
||||
LeapsPostProcessor::LeapsPostProcessor(std::vector<Segment> const & path, IndexGraphStarter & starter)
|
||||
: m_starter(starter)
|
||||
, m_bfs(starter)
|
||||
{
|
||||
std::map<size_t, size_t> jumps;
|
||||
std::map<Segment, size_t> segmentToIndex;
|
||||
|
||||
for (size_t i = 0; i < path.size(); ++i)
|
||||
{
|
||||
auto const it = segmentToIndex.find(path[i]);
|
||||
if (it == segmentToIndex.cend())
|
||||
{
|
||||
segmentToIndex[path[i]] = i;
|
||||
continue;
|
||||
}
|
||||
|
||||
auto const prevIndex = it->second;
|
||||
jumps[prevIndex] = i;
|
||||
it->second = i;
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < path.size(); ++i)
|
||||
{
|
||||
auto it = jumps.find(i);
|
||||
if (it == jumps.end())
|
||||
{
|
||||
m_path.emplace_back(path[i]);
|
||||
continue;
|
||||
}
|
||||
|
||||
while (it != jumps.end())
|
||||
{
|
||||
i = it->second;
|
||||
it = jumps.find(i);
|
||||
}
|
||||
|
||||
CHECK_LESS(i, path.size(), ());
|
||||
m_path.emplace_back(path[i]);
|
||||
}
|
||||
|
||||
Init();
|
||||
}
|
||||
|
||||
void LeapsPostProcessor::Init()
|
||||
{
|
||||
m_prefixSumETA = std::vector<double>(m_path.size(), 0.0);
|
||||
|
||||
for (size_t i = 1; i < m_path.size(); ++i)
|
||||
{
|
||||
auto const & segment = m_path[i];
|
||||
m_prefixSumETA[i] = m_prefixSumETA[i - 1] + m_starter.CalculateETAWithoutPenalty(segment);
|
||||
|
||||
CHECK_EQUAL(m_segmentToIndex.count(segment), 0, ());
|
||||
m_segmentToIndex[segment] = i;
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<Segment> LeapsPostProcessor::GetProcessedPath()
|
||||
{
|
||||
auto const intervalsToRelax = CalculateIntervalsToRelax();
|
||||
|
||||
NonIntersectingSegmentPaths toReplace;
|
||||
for (auto const & interval : intervalsToRelax)
|
||||
toReplace.AddInterval(interval);
|
||||
|
||||
std::vector<PathInterval> intervals = toReplace.StealIntervals();
|
||||
std::sort(intervals.begin(), intervals.end());
|
||||
|
||||
std::vector<Segment> output;
|
||||
size_t prevIndex = 0;
|
||||
for (auto & interval : intervals)
|
||||
{
|
||||
CHECK(m_path.begin() + prevIndex <= m_path.begin() + interval.m_left, ());
|
||||
output.insert(output.end(), m_path.begin() + prevIndex, m_path.begin() + interval.m_left);
|
||||
output.insert(output.end(), interval.m_path.begin(), interval.m_path.end());
|
||||
|
||||
prevIndex = interval.m_right + 1;
|
||||
}
|
||||
|
||||
output.insert(output.end(), m_path.begin() + prevIndex, m_path.end());
|
||||
return output;
|
||||
}
|
||||
|
||||
auto LeapsPostProcessor::CalculateIntervalsToRelax() -> std::set<PathInterval, PathInterval::GreaterByWeight>
|
||||
{
|
||||
std::set<PathInterval, PathInterval::GreaterByWeight> result;
|
||||
for (size_t right = kMaxStep; right < m_path.size(); ++right)
|
||||
{
|
||||
std::map<Segment, SegmentData> segmentsData;
|
||||
auto const & segment = m_path[right];
|
||||
segmentsData.emplace(segment, SegmentData(0, m_starter.CalculateETAWithoutPenalty(segment)));
|
||||
|
||||
FillIngoingPaths(segment, segmentsData);
|
||||
|
||||
for (auto const & item : segmentsData)
|
||||
{
|
||||
Segment const & visited = item.first;
|
||||
SegmentData const & data = item.second;
|
||||
|
||||
auto const it = m_segmentToIndex.find(visited);
|
||||
if (it == m_segmentToIndex.cend())
|
||||
continue;
|
||||
|
||||
size_t const left = it->second;
|
||||
if (left >= right || left == 0)
|
||||
continue;
|
||||
|
||||
auto const prevWeight = m_prefixSumETA[right] - m_prefixSumETA[left - 1];
|
||||
auto const curWeight = data.m_summaryETA;
|
||||
if (prevWeight - PathInterval::kWeightEps <= curWeight)
|
||||
continue;
|
||||
|
||||
double const winWeight = prevWeight - curWeight;
|
||||
result.emplace(winWeight, left, right, m_bfs.ReconstructPath(visited, true /* reverse */));
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
void LeapsPostProcessor::FillIngoingPaths(Segment const & start,
|
||||
std::map<Segment, LeapsPostProcessor::SegmentData> & segmentsData)
|
||||
{
|
||||
m_bfs.Run(start, false /* isOutgoing */, [&segmentsData, this](BFS<IndexGraphStarter>::State const & state)
|
||||
{
|
||||
if (segmentsData.count(state.m_vertex) != 0)
|
||||
return false;
|
||||
|
||||
auto const & parent = segmentsData[state.m_parent];
|
||||
ASSERT_LESS_OR_EQUAL(parent.m_steps, kMaxStep, ());
|
||||
if (parent.m_steps == kMaxStep)
|
||||
return false;
|
||||
|
||||
auto & current = segmentsData[state.m_vertex];
|
||||
current.m_summaryETA = parent.m_summaryETA + m_starter.CalculateETAWithoutPenalty(state.m_vertex);
|
||||
|
||||
current.m_steps = parent.m_steps + 1;
|
||||
|
||||
return true;
|
||||
});
|
||||
}
|
||||
|
||||
LeapsPostProcessor::SegmentData::SegmentData(size_t steps, double eta) : m_steps(steps), m_summaryETA(eta) {}
|
||||
|
||||
LeapsPostProcessor::PathInterval::PathInterval(double weight, size_t left, size_t right, std::vector<Segment> && path)
|
||||
: m_winWeight(weight)
|
||||
, m_left(left)
|
||||
, m_right(right)
|
||||
, m_path(std::move(path))
|
||||
{}
|
||||
|
||||
bool LeapsPostProcessor::PathInterval::GreaterByWeight::operator()(PathInterval const & lhs,
|
||||
PathInterval const & rhs) const
|
||||
{
|
||||
return lhs.m_winWeight > rhs.m_winWeight;
|
||||
}
|
||||
|
||||
bool LeapsPostProcessor::PathInterval::operator<(PathInterval const & rhs) const
|
||||
{
|
||||
CHECK(m_left > rhs.m_right || m_right < rhs.m_left || (m_left == rhs.m_left && m_right == rhs.m_right),
|
||||
("Intervals shouldn't intersect.", *this, rhs));
|
||||
|
||||
return m_right < rhs.m_left;
|
||||
}
|
||||
|
||||
std::string DebugPrint(LeapsPostProcessor::PathInterval const & interval)
|
||||
{
|
||||
std::stringstream ss;
|
||||
ss << "[" << interval.m_left << ", " << interval.m_right << "], weight = " << interval.m_winWeight;
|
||||
|
||||
return ss.str();
|
||||
}
|
||||
} // namespace routing
|
||||
86
libs/routing/leaps_postprocessor.hpp
Normal file
86
libs/routing/leaps_postprocessor.hpp
Normal file
|
|
@ -0,0 +1,86 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/base/bfs.hpp"
|
||||
|
||||
#include "routing/index_graph_starter.hpp"
|
||||
#include "routing/segment.hpp"
|
||||
|
||||
#include "base/math.hpp"
|
||||
|
||||
#include <cstdint>
|
||||
#include <map>
|
||||
#include <set>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
/// \brief After LeapsOnly mode we can have Loops in the path, like this:
|
||||
/// Path through features: {f0, f1, ..., f9}.
|
||||
/// *-f0->*-f1->*-f2->*-f3-> * -f6->*-f7->*-f8->*-f9->
|
||||
/// | ↑
|
||||
/// f4 f5
|
||||
/// ↓ |
|
||||
/// * <------- mwm exit.
|
||||
///
|
||||
/// As you can see, we can go from f3 to f6 but because of heuristic searching of intermediate
|
||||
/// points (mwms' exists) in LeapsOnly routing algorithm, we build route through this point
|
||||
/// and received next path:
|
||||
/// {..., f3, f4, f5, f6, ... } - this is Loop.
|
||||
/// So next class provides local optimization and relax such paths:
|
||||
/// (... f3 -> f4 -> f5 -> f6 ...) to (... f3 -> f6 ...).
|
||||
///
|
||||
/// Works for: |kMaxStep| * N. Where N it is path's length.
|
||||
/// Thus it is O(N), but we should not increase |kMaxStep| much.
|
||||
class LeapsPostProcessor
|
||||
{
|
||||
public:
|
||||
LeapsPostProcessor(std::vector<Segment> const & path, IndexGraphStarter & starter);
|
||||
|
||||
struct PathInterval
|
||||
{
|
||||
static double constexpr kWeightEps = 1.0;
|
||||
|
||||
PathInterval(double weight, size_t left, size_t right, std::vector<Segment> && path);
|
||||
|
||||
struct GreaterByWeight
|
||||
{
|
||||
bool operator()(PathInterval const & lhs, PathInterval const & rhs) const;
|
||||
};
|
||||
|
||||
bool operator<(PathInterval const & rhs) const;
|
||||
|
||||
double m_winWeight = 0.0;
|
||||
size_t m_left = 0;
|
||||
size_t m_right = 0;
|
||||
std::vector<Segment> m_path;
|
||||
};
|
||||
|
||||
std::vector<Segment> GetProcessedPath();
|
||||
|
||||
private:
|
||||
static size_t const kMaxStep;
|
||||
|
||||
struct SegmentData
|
||||
{
|
||||
SegmentData() = default;
|
||||
SegmentData(size_t steps, double eta);
|
||||
size_t m_steps = 0;
|
||||
double m_summaryETA = 0.0;
|
||||
};
|
||||
|
||||
void Init();
|
||||
|
||||
std::set<PathInterval, PathInterval::GreaterByWeight> CalculateIntervalsToRelax();
|
||||
void FillIngoingPaths(Segment const & start, std::map<Segment, SegmentData> & segmentsData);
|
||||
|
||||
std::vector<Segment> m_path;
|
||||
IndexGraphStarter & m_starter;
|
||||
|
||||
BFS<IndexGraphStarter> m_bfs;
|
||||
std::map<Segment, size_t> m_segmentToIndex;
|
||||
std::vector<double> m_prefixSumETA;
|
||||
};
|
||||
|
||||
std::string DebugPrint(LeapsPostProcessor::PathInterval const & interval);
|
||||
} // namespace routing
|
||||
38
libs/routing/loaded_path_segment.hpp
Normal file
38
libs/routing/loaded_path_segment.hpp
Normal file
|
|
@ -0,0 +1,38 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/lanes/lane_info.hpp"
|
||||
#include "routing/route.hpp"
|
||||
#include "routing/segment.hpp"
|
||||
#include "routing/turns.hpp"
|
||||
|
||||
#include "indexer/ftypes_matcher.hpp"
|
||||
|
||||
#include "geometry/point_with_altitude.hpp"
|
||||
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
/*!
|
||||
* \brief The LoadedPathSegment struct is a representation of a single node path.
|
||||
* It unpacks and stores information about path and road type flags.
|
||||
* Postprocessing must read information from the structure and does not initiate disk readings.
|
||||
*/
|
||||
struct LoadedPathSegment
|
||||
{
|
||||
std::vector<geometry::PointWithAltitude> m_path;
|
||||
turns::lanes::LanesInfo m_lanes;
|
||||
RouteSegment::RoadNameInfo m_roadNameInfo;
|
||||
// double m_weight = 0.0; /*!< Time in seconds to pass the segment. */
|
||||
SegmentRange m_segmentRange;
|
||||
std::vector<Segment> m_segments; /*!< Traffic segments for |m_path|. */
|
||||
ftypes::HighwayClass m_highwayClass = ftypes::HighwayClass::Undefined;
|
||||
bool m_onRoundabout = false;
|
||||
bool m_isLink = false;
|
||||
bool m_isOneWay = false;
|
||||
|
||||
bool IsValid() const { return m_path.size() > 1; }
|
||||
};
|
||||
|
||||
using TUnpackedPathSegments = std::vector<LoadedPathSegment>;
|
||||
} // namespace routing
|
||||
82
libs/routing/maxspeeds.cpp
Normal file
82
libs/routing/maxspeeds.cpp
Normal file
|
|
@ -0,0 +1,82 @@
|
|||
#include "routing/maxspeeds.hpp"
|
||||
#include "routing/maxspeeds_serialization.hpp"
|
||||
|
||||
#include "platform/measurement_utils.hpp"
|
||||
|
||||
#include "coding/files_container.hpp"
|
||||
|
||||
#include "base/assert.hpp"
|
||||
#include "base/logging.hpp"
|
||||
|
||||
#include "defines.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
bool Maxspeeds::IsEmpty() const
|
||||
{
|
||||
return m_forwardMaxspeedsTable.size() == 0 && m_bidirectionalMaxspeeds.empty();
|
||||
}
|
||||
|
||||
Maxspeed Maxspeeds::GetMaxspeed(uint32_t fid) const
|
||||
{
|
||||
// Forward only maxspeeds.
|
||||
if (HasForwardMaxspeed(fid))
|
||||
{
|
||||
auto const r = m_forwardMaxspeedsTable.rank(fid);
|
||||
CHECK_LESS(r, m_forwardMaxspeeds.Size(), ());
|
||||
uint8_t const macro = m_forwardMaxspeeds.Get(r);
|
||||
auto const speed = GetMaxspeedConverter().MacroToSpeed(static_cast<SpeedMacro>(macro));
|
||||
CHECK(speed.IsValid(), ());
|
||||
return {speed.GetUnits(), speed.GetSpeed(), kInvalidSpeed};
|
||||
}
|
||||
|
||||
// Bidirectional maxspeeds.
|
||||
auto const range = std::equal_range(m_bidirectionalMaxspeeds.cbegin(), m_bidirectionalMaxspeeds.cend(), fid,
|
||||
FeatureMaxspeed::Less());
|
||||
|
||||
if (range.second == range.first)
|
||||
return Maxspeed(); // No maxspeed for |fid| is set. Returns an invalid Maxspeed instance.
|
||||
|
||||
CHECK_EQUAL(range.second - range.first, 1, ());
|
||||
return range.first->GetMaxspeed();
|
||||
}
|
||||
|
||||
MaxspeedType Maxspeeds::GetDefaultSpeed(bool inCity, HighwayType hwType) const
|
||||
{
|
||||
auto const & theMap = m_defaultSpeeds[inCity ? 1 : 0];
|
||||
auto const it = theMap.find(hwType);
|
||||
return (it != theMap.end()) ? it->second : kInvalidSpeed;
|
||||
}
|
||||
|
||||
bool Maxspeeds::HasForwardMaxspeed(uint32_t fid) const
|
||||
{
|
||||
return fid < m_forwardMaxspeedsTable.size() ? m_forwardMaxspeedsTable[fid] : false;
|
||||
}
|
||||
|
||||
void Maxspeeds::Load(ReaderT const & reader)
|
||||
{
|
||||
ReaderSource<ReaderT> src(reader);
|
||||
MaxspeedsSerializer::Deserialize(src, *this);
|
||||
}
|
||||
|
||||
std::unique_ptr<Maxspeeds> LoadMaxspeeds(MwmSet::MwmHandle const & handle)
|
||||
{
|
||||
auto const * value = handle.GetValue();
|
||||
CHECK(value, ());
|
||||
|
||||
try
|
||||
{
|
||||
auto maxspeeds = std::make_unique<Maxspeeds>();
|
||||
if (value->m_cont.IsExist(MAXSPEEDS_FILE_TAG))
|
||||
maxspeeds->Load(value->m_cont.GetReader(MAXSPEEDS_FILE_TAG));
|
||||
return maxspeeds;
|
||||
}
|
||||
catch (Reader::Exception const & e)
|
||||
{
|
||||
LOG(LERROR, ("File", value->GetCountryFileName(), "Error while reading", MAXSPEEDS_FILE_TAG, "section.", e.Msg()));
|
||||
return std::make_unique<Maxspeeds>();
|
||||
}
|
||||
}
|
||||
} // namespace routing
|
||||
57
libs/routing/maxspeeds.hpp
Normal file
57
libs/routing/maxspeeds.hpp
Normal file
|
|
@ -0,0 +1,57 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing_common/maxspeed_conversion.hpp"
|
||||
#include "routing_common/vehicle_model.hpp"
|
||||
|
||||
#include "indexer/mwm_set.hpp"
|
||||
|
||||
#include "coding/memory_region.hpp"
|
||||
#include "coding/reader.hpp"
|
||||
#include "coding/simple_dense_coding.hpp"
|
||||
|
||||
#include <memory>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include "3party/succinct/elias_fano.hpp"
|
||||
|
||||
namespace routing
|
||||
{
|
||||
class MaxspeedsSerializer;
|
||||
|
||||
class Maxspeeds
|
||||
{
|
||||
friend class MaxspeedsSerializer;
|
||||
|
||||
public:
|
||||
/// \returns false if there's no maxspeeds (forward or bidirectional) and true otherwise.
|
||||
bool IsEmpty() const;
|
||||
|
||||
/// \returns Maxspeed for feature id |fid|. If there's no Maxspeed value for |fid|
|
||||
/// returns an invalid Maxspeed value.
|
||||
Maxspeed GetMaxspeed(uint32_t fid) const;
|
||||
MaxspeedType GetDefaultSpeed(bool inCity, HighwayType hwType) const;
|
||||
|
||||
using ReaderT = ModelReaderPtr;
|
||||
void Load(ReaderT const & reader);
|
||||
|
||||
private:
|
||||
bool HasForwardMaxspeed(uint32_t fid) const;
|
||||
|
||||
// Feature IDs (compressed rank bit-vector).
|
||||
std::unique_ptr<CopiedMemoryRegion> m_forwardMaxspeedTableRegion;
|
||||
succinct::elias_fano m_forwardMaxspeedsTable;
|
||||
|
||||
// Forward speeds (compressed uint8_t vector).
|
||||
std::unique_ptr<CopiedMemoryRegion> m_forwardMaxspeedRegion;
|
||||
coding::SimpleDenseCoding m_forwardMaxspeeds;
|
||||
|
||||
std::vector<FeatureMaxspeed> m_bidirectionalMaxspeeds;
|
||||
|
||||
static int constexpr DEFAULT_SPEEDS_COUNT = 2;
|
||||
// Default speeds (KmPerH) for MWM, 0 - outside a city, 1 - inside a city.
|
||||
std::unordered_map<HighwayType, MaxspeedType> m_defaultSpeeds[DEFAULT_SPEEDS_COUNT];
|
||||
};
|
||||
|
||||
std::unique_ptr<Maxspeeds> LoadMaxspeeds(MwmSet::MwmHandle const & handle);
|
||||
} // namespace routing
|
||||
19
libs/routing/maxspeeds_serialization.cpp
Normal file
19
libs/routing/maxspeeds_serialization.cpp
Normal file
|
|
@ -0,0 +1,19 @@
|
|||
#include "routing/maxspeeds_serialization.hpp"
|
||||
|
||||
namespace routing
|
||||
{
|
||||
std::vector<uint8_t> MaxspeedsSerializer::GetForwardMaxspeeds(std::vector<FeatureSpeedMacro> const & speeds,
|
||||
uint32_t & maxFeatureID)
|
||||
{
|
||||
std::vector<uint8_t> result;
|
||||
for (auto const & s : speeds)
|
||||
{
|
||||
if (!s.IsBidirectional())
|
||||
{
|
||||
result.push_back(static_cast<uint8_t>(s.m_forward));
|
||||
maxFeatureID = s.m_featureID;
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
} // namespace routing
|
||||
272
libs/routing/maxspeeds_serialization.hpp
Normal file
272
libs/routing/maxspeeds_serialization.hpp
Normal file
|
|
@ -0,0 +1,272 @@
|
|||
#pragma once
|
||||
|
||||
#include "routing/maxspeeds.hpp"
|
||||
|
||||
#include "routing_common/vehicle_model.hpp"
|
||||
|
||||
#include "coding/reader.hpp"
|
||||
#include "coding/succinct_mapper.hpp"
|
||||
#include "coding/varint.hpp"
|
||||
#include "coding/write_to_sink.hpp"
|
||||
#include "coding/writer.hpp"
|
||||
|
||||
#include "base/assert.hpp"
|
||||
#include "base/logging.hpp"
|
||||
#include "base/stl_helpers.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cstdint>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
/// \brief
|
||||
/// Section name: MAXSPEEDS_FILE_TAG.
|
||||
/// Description: keeping value of maxspeed tag for roads.
|
||||
/// Section tables:
|
||||
/// * Header
|
||||
/// * HighwayType -> Average speed macro
|
||||
/// * Forward speeds:
|
||||
/// elias_fano with feature ids which have maxspeed tag in forward direction only
|
||||
/// SimpleDenseCoding with speed macros
|
||||
/// * Bidirectional speeds in raw table:
|
||||
/// <varint(delta(Feature id))><Forward speed macro><Backward speed macro>
|
||||
///
|
||||
/// \note elias_fano is better than raw table in case of high density of entries, so we assume that
|
||||
/// Bidirectional speeds count *is much less than* Forward speeds count.
|
||||
class MaxspeedsSerializer
|
||||
{
|
||||
// 0 - start version
|
||||
// 1 - added HighwayType -> SpeedMacro
|
||||
// 2 - HighwayType -> SpeedMacro for inside/outside a city
|
||||
using VersionT = uint16_t;
|
||||
static VersionT constexpr kVersion = 2;
|
||||
|
||||
public:
|
||||
MaxspeedsSerializer() = delete;
|
||||
|
||||
struct FeatureSpeedMacro
|
||||
{
|
||||
FeatureSpeedMacro(uint32_t featureID, SpeedMacro forward, SpeedMacro backward = SpeedMacro::Undefined)
|
||||
: m_featureID(featureID)
|
||||
, m_forward(forward)
|
||||
, m_backward(backward)
|
||||
{
|
||||
// We store bidirectional speeds only if they are not equal,
|
||||
// see Maxspeed::IsBidirectional() comments.
|
||||
if (m_forward == m_backward)
|
||||
m_backward = SpeedMacro::Undefined;
|
||||
}
|
||||
|
||||
bool IsBidirectional() const { return m_backward != SpeedMacro::Undefined; }
|
||||
|
||||
uint32_t m_featureID;
|
||||
SpeedMacro m_forward;
|
||||
SpeedMacro m_backward;
|
||||
};
|
||||
|
||||
using HW2SpeedMap = std::map<HighwayType, SpeedMacro>;
|
||||
|
||||
static int constexpr DEFAULT_SPEEDS_COUNT = Maxspeeds::DEFAULT_SPEEDS_COUNT;
|
||||
|
||||
template <class Sink>
|
||||
static void Serialize(std::vector<FeatureSpeedMacro> const & featureSpeeds, HW2SpeedMap typeSpeeds[], Sink & sink)
|
||||
{
|
||||
CHECK(base::IsSortedAndUnique(featureSpeeds.cbegin(), featureSpeeds.cend(),
|
||||
[](FeatureSpeedMacro const & l, FeatureSpeedMacro const & r)
|
||||
{ return l.m_featureID < r.m_featureID; }),
|
||||
());
|
||||
|
||||
// Version
|
||||
auto const startOffset = sink.Pos();
|
||||
WriteToSink(sink, kVersion);
|
||||
|
||||
// Header
|
||||
auto const headerOffset = sink.Pos();
|
||||
Header header;
|
||||
header.Serialize(sink);
|
||||
|
||||
auto const forwardMaxspeedTableOffset = sink.Pos();
|
||||
|
||||
// Saving forward (one direction) maxspeeds.
|
||||
uint32_t maxFeatureID;
|
||||
std::vector<uint8_t> forwardMaxspeeds = GetForwardMaxspeeds(featureSpeeds, maxFeatureID);
|
||||
|
||||
size_t forwardCount = forwardMaxspeeds.size();
|
||||
if (forwardCount > 0)
|
||||
{
|
||||
succinct::elias_fano::elias_fano_builder builder(maxFeatureID + 1, forwardCount);
|
||||
for (auto const & e : featureSpeeds)
|
||||
if (!e.IsBidirectional())
|
||||
builder.push_back(e.m_featureID);
|
||||
|
||||
coding::FreezeVisitor<Writer> visitor(sink);
|
||||
succinct::elias_fano(&builder).map(visitor);
|
||||
|
||||
header.m_forwardMaxspeedOffset = static_cast<uint32_t>(sink.Pos() - forwardMaxspeedTableOffset);
|
||||
|
||||
coding::SimpleDenseCoding simpleDenseCoding(forwardMaxspeeds);
|
||||
coding::Freeze(simpleDenseCoding, sink, "ForwardMaxspeeds");
|
||||
}
|
||||
else
|
||||
{
|
||||
header.m_forwardMaxspeedOffset = 0;
|
||||
}
|
||||
header.m_bidirectionalMaxspeedOffset = static_cast<uint32_t>(sink.Pos() - forwardMaxspeedTableOffset);
|
||||
|
||||
// Saving bidirectional maxspeeds.
|
||||
uint32_t prevFeatureId = 0;
|
||||
for (auto const & s : featureSpeeds)
|
||||
{
|
||||
if (!s.IsBidirectional())
|
||||
continue;
|
||||
|
||||
++header.m_bidirectionalMaxspeedNumber;
|
||||
|
||||
// Valid, because we did base::IsSortedAndUnique in the beginning.
|
||||
WriteVarUint(sink, s.m_featureID - prevFeatureId);
|
||||
prevFeatureId = s.m_featureID;
|
||||
|
||||
WriteToSink(sink, static_cast<uint8_t>(s.m_forward));
|
||||
WriteToSink(sink, static_cast<uint8_t>(s.m_backward));
|
||||
}
|
||||
|
||||
// Save HighwayType speeds, sorted by type.
|
||||
for (int ind = 0; ind < DEFAULT_SPEEDS_COUNT; ++ind)
|
||||
{
|
||||
WriteVarUint(sink, static_cast<uint32_t>(typeSpeeds[ind].size()));
|
||||
for (auto const & [type, speed] : typeSpeeds[ind])
|
||||
{
|
||||
WriteVarUint(sink, static_cast<uint32_t>(type));
|
||||
WriteToSink(sink, static_cast<uint8_t>(speed));
|
||||
}
|
||||
}
|
||||
|
||||
// Finalize Header data.
|
||||
auto const endOffset = sink.Pos();
|
||||
sink.Seek(headerOffset);
|
||||
header.Serialize(sink);
|
||||
sink.Seek(endOffset);
|
||||
|
||||
// Print statistics.
|
||||
LOG(LINFO, ("Serialized", forwardCount, "forward maxspeeds and", header.m_bidirectionalMaxspeedNumber,
|
||||
"bidirectional maxspeeds. Section size:", endOffset - startOffset, "bytes."));
|
||||
LOG(LINFO,
|
||||
("Succinct EF compression ratio:", forwardCount * (4 + 1) / double(header.m_bidirectionalMaxspeedOffset)));
|
||||
}
|
||||
|
||||
template <class Source>
|
||||
static void Deserialize(Source & src, Maxspeeds & maxspeeds)
|
||||
{
|
||||
CHECK(maxspeeds.IsEmpty(), ());
|
||||
|
||||
// Version
|
||||
auto const version = ReadPrimitiveFromSource<VersionT>(src);
|
||||
|
||||
// Header
|
||||
Header header;
|
||||
header.Deserialize(src);
|
||||
|
||||
// Loading forward only speeds.
|
||||
if (header.m_forwardMaxspeedOffset != header.m_bidirectionalMaxspeedOffset)
|
||||
{
|
||||
CHECK_LESS(header.m_forwardMaxspeedOffset, header.m_bidirectionalMaxspeedOffset, ());
|
||||
|
||||
// Reading maxspeed information for features which have only forward maxspeed.
|
||||
std::vector<uint8_t> forwardTableData(header.m_forwardMaxspeedOffset);
|
||||
src.Read(forwardTableData.data(), forwardTableData.size());
|
||||
maxspeeds.m_forwardMaxspeedTableRegion = std::make_unique<CopiedMemoryRegion>(std::move(forwardTableData));
|
||||
coding::Map(maxspeeds.m_forwardMaxspeedsTable, maxspeeds.m_forwardMaxspeedTableRegion->ImmutableData(),
|
||||
"ForwardMaxspeedsTable");
|
||||
|
||||
std::vector<uint8_t> forwardData(header.m_bidirectionalMaxspeedOffset - header.m_forwardMaxspeedOffset);
|
||||
src.Read(forwardData.data(), forwardData.size());
|
||||
maxspeeds.m_forwardMaxspeedRegion = std::make_unique<CopiedMemoryRegion>(std::move(forwardData));
|
||||
Map(maxspeeds.m_forwardMaxspeeds, maxspeeds.m_forwardMaxspeedRegion->ImmutableData(), "ForwardMaxspeeds");
|
||||
}
|
||||
|
||||
// Loading bidirectional speeds.
|
||||
auto const & converter = GetMaxspeedConverter();
|
||||
auto ReadSpeed = [&](size_t i)
|
||||
{
|
||||
uint8_t const idx = ReadPrimitiveFromSource<uint8_t>(src);
|
||||
auto const speed = converter.MacroToSpeed(static_cast<SpeedMacro>(idx));
|
||||
CHECK(speed.IsValid(), (i));
|
||||
return speed;
|
||||
};
|
||||
|
||||
maxspeeds.m_bidirectionalMaxspeeds.reserve(header.m_bidirectionalMaxspeedNumber);
|
||||
uint32_t featureId = 0;
|
||||
for (size_t i = 0; i < header.m_bidirectionalMaxspeedNumber; ++i)
|
||||
{
|
||||
auto const delta = ReadVarUint<uint32_t>(src);
|
||||
auto const forwardSpeed = ReadSpeed(i);
|
||||
auto const backwardSpeed = ReadSpeed(i);
|
||||
|
||||
CHECK(HaveSameUnits(forwardSpeed, backwardSpeed), (i));
|
||||
|
||||
// Note. If neither |forwardSpeed| nor |backwardSpeed| are numeric, it means
|
||||
// both of them have value "walk" or "none". So the units are not relevant for this case.
|
||||
measurement_utils::Units units = measurement_utils::Units::Metric;
|
||||
if (forwardSpeed.IsNumeric())
|
||||
units = forwardSpeed.GetUnits();
|
||||
else if (backwardSpeed.IsNumeric())
|
||||
units = backwardSpeed.GetUnits();
|
||||
|
||||
featureId += delta;
|
||||
maxspeeds.m_bidirectionalMaxspeeds.emplace_back(featureId, units, forwardSpeed.GetSpeed(),
|
||||
backwardSpeed.GetSpeed());
|
||||
}
|
||||
|
||||
// Load HighwayType speeds.
|
||||
if (version >= 2)
|
||||
{
|
||||
for (int ind = 0; ind < DEFAULT_SPEEDS_COUNT; ++ind)
|
||||
{
|
||||
uint32_t const count = ReadVarUint<uint32_t>(src);
|
||||
for (uint32_t i = 0; i < count; ++i)
|
||||
{
|
||||
auto const type = static_cast<HighwayType>(ReadVarUint<uint32_t>(src));
|
||||
auto const speed = converter.MacroToSpeed(static_cast<SpeedMacro>(ReadPrimitiveFromSource<uint8_t>(src)));
|
||||
// Constraint should be met, but unit tests use different input Units. No problem here.
|
||||
// ASSERT_EQUAL(speed.GetUnits(), measurement_utils::Units::Metric, ());
|
||||
maxspeeds.m_defaultSpeeds[ind][type] = speed.GetSpeed();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
static std::vector<uint8_t> GetForwardMaxspeeds(std::vector<FeatureSpeedMacro> const & speeds,
|
||||
uint32_t & maxFeatureID);
|
||||
|
||||
struct Header
|
||||
{
|
||||
public:
|
||||
template <class Sink>
|
||||
void Serialize(Sink & sink) const
|
||||
{
|
||||
WriteToSink(sink, m_endianness);
|
||||
WriteToSink(sink, m_forwardMaxspeedOffset);
|
||||
WriteToSink(sink, m_bidirectionalMaxspeedOffset);
|
||||
WriteToSink(sink, m_bidirectionalMaxspeedNumber);
|
||||
}
|
||||
|
||||
template <class Source>
|
||||
void Deserialize(Source & src)
|
||||
{
|
||||
m_endianness = ReadPrimitiveFromSource<decltype(m_endianness)>(src);
|
||||
m_forwardMaxspeedOffset = ReadPrimitiveFromSource<decltype(m_forwardMaxspeedOffset)>(src);
|
||||
m_bidirectionalMaxspeedOffset = ReadPrimitiveFromSource<decltype(m_bidirectionalMaxspeedOffset)>(src);
|
||||
m_bidirectionalMaxspeedNumber = ReadPrimitiveFromSource<decltype(m_bidirectionalMaxspeedNumber)>(src);
|
||||
}
|
||||
|
||||
// Field |m_endianness| is reserved for endianness of the section.
|
||||
uint16_t m_endianness = 0;
|
||||
uint32_t m_forwardMaxspeedOffset = 0;
|
||||
uint32_t m_bidirectionalMaxspeedOffset = 0;
|
||||
uint32_t m_bidirectionalMaxspeedNumber = 0;
|
||||
};
|
||||
};
|
||||
} // namespace routing
|
||||
Some files were not shown because too many files have changed in this diff Show more
Loading…
Add table
Add a link
Reference in a new issue