c2c/co-maps
c2c/co-maps
2
0
Fork 0
Code Issues Pull requests Projects Releases 1 Packages Wiki Activity Actions

Repo created

Browse source
This commit is contained in:
Fr4nz D13trich 2025-11-22 13:58:55 +01:00
parent 4af19165ec
commit 68073add76
12458 changed files with 12350765 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

820
libs/routing/base/astar_algorithm.hpp Normal file
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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
View 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