Repo created
This commit is contained in:
parent
4af19165ec
commit
68073add76
12458 changed files with 12350765 additions and 2 deletions
310
libs/routing/road_graph.cpp
Normal file
310
libs/routing/road_graph.cpp
Normal file
|
|
@ -0,0 +1,310 @@
|
|||
#include "routing/road_graph.hpp"
|
||||
|
||||
#include "geometry/mercator.hpp"
|
||||
#include "geometry/segment2d.hpp"
|
||||
|
||||
#include "base/assert.hpp"
|
||||
#include "base/checked_cast.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
#include <limits>
|
||||
#include <sstream>
|
||||
|
||||
namespace routing
|
||||
{
|
||||
using namespace std;
|
||||
|
||||
namespace
|
||||
{
|
||||
bool OnEdge(geometry::PointWithAltitude const & p, Edge const & ab)
|
||||
{
|
||||
auto const & a = ab.GetStartJunction();
|
||||
auto const & b = ab.GetEndJunction();
|
||||
return m2::IsPointOnSegmentEps(p.GetPoint(), a.GetPoint(), b.GetPoint(), 1e-9);
|
||||
}
|
||||
|
||||
void SplitEdge(Edge const & ab, geometry::PointWithAltitude const & p, vector<Edge> & edges)
|
||||
{
|
||||
auto const & a = ab.GetStartJunction();
|
||||
auto const & b = ab.GetEndJunction();
|
||||
|
||||
// No need to split the edge by its endpoints.
|
||||
if (a.GetPoint() == p.GetPoint() || b.GetPoint() == p.GetPoint())
|
||||
return;
|
||||
|
||||
edges.push_back(Edge::MakeFake(a, p, ab));
|
||||
edges.push_back(Edge::MakeFake(p, b, ab));
|
||||
}
|
||||
} // namespace
|
||||
|
||||
// Edge ------------------------------------------------------------------------
|
||||
// static
|
||||
Edge Edge::MakeReal(FeatureID featureId, bool forward, uint32_t segId, JunctionPointT const & startJunction,
|
||||
JunctionPointT const & endJunction)
|
||||
{
|
||||
return {Type::Real, std::move(featureId), kInvalidFakeSegmentId, forward, segId, startJunction, endJunction};
|
||||
}
|
||||
|
||||
// static
|
||||
Edge Edge::MakeFakeWithRealPart(FeatureID featureId, uint32_t fakeSegmentId, bool forward, uint32_t segId,
|
||||
JunctionPointT const & startJunction, JunctionPointT const & endJunction)
|
||||
{
|
||||
return {Type::FakeWithRealPart, std::move(featureId), fakeSegmentId, forward, segId, startJunction, endJunction};
|
||||
}
|
||||
|
||||
// static
|
||||
Edge Edge::MakeFake(JunctionPointT const & startJunction, JunctionPointT const & endJunction)
|
||||
{
|
||||
return {Type::FakeWithoutRealPart,
|
||||
FeatureID(),
|
||||
kInvalidFakeSegmentId,
|
||||
true /* forward */,
|
||||
0 /* segmentId */,
|
||||
startJunction,
|
||||
endJunction};
|
||||
}
|
||||
|
||||
// static
|
||||
Edge Edge::MakeFake(JunctionPointT const & startJunction, JunctionPointT const & endJunction, Edge const & prototype)
|
||||
{
|
||||
auto e = prototype;
|
||||
e.m_startJunction = startJunction;
|
||||
e.m_endJunction = endJunction;
|
||||
e.m_type = e.HasRealPart() ? Type::FakeWithRealPart : Type::FakeWithoutRealPart;
|
||||
return e;
|
||||
}
|
||||
|
||||
Edge::Edge(Type type, FeatureID featureId, uint32_t fakeSegmentId, bool forward, uint32_t segId,
|
||||
JunctionPointT const & startJunction, JunctionPointT const & endJunction)
|
||||
: m_type(type)
|
||||
, m_featureId(std::move(featureId))
|
||||
, m_forward(forward)
|
||||
, m_segId(segId)
|
||||
, m_startJunction(startJunction)
|
||||
, m_endJunction(endJunction)
|
||||
, m_fakeSegmentId(fakeSegmentId)
|
||||
{
|
||||
ASSERT_LESS(segId, numeric_limits<uint32_t>::max(), ());
|
||||
ASSERT_EQUAL(m_featureId.IsValid(), HasRealPart(), ());
|
||||
}
|
||||
|
||||
Edge Edge::GetReverseEdge() const
|
||||
{
|
||||
Edge edge = *this;
|
||||
edge.m_forward = !edge.m_forward;
|
||||
swap(edge.m_startJunction, edge.m_endJunction);
|
||||
return edge;
|
||||
}
|
||||
|
||||
bool Edge::SameRoadSegmentAndDirection(Edge const & r) const
|
||||
{
|
||||
return m_featureId == r.m_featureId && m_forward == r.m_forward && m_segId == r.m_segId;
|
||||
}
|
||||
|
||||
bool Edge::operator==(Edge const & r) const
|
||||
{
|
||||
return m_type == r.m_type && m_featureId == r.m_featureId && m_forward == r.m_forward && m_segId == r.m_segId &&
|
||||
m_startJunction == r.m_startJunction && m_endJunction == r.m_endJunction;
|
||||
}
|
||||
|
||||
bool Edge::operator<(Edge const & r) const
|
||||
{
|
||||
if (m_type != r.m_type)
|
||||
return m_type < r.m_type;
|
||||
if (m_featureId != r.m_featureId)
|
||||
return m_featureId < r.m_featureId;
|
||||
if (m_forward != r.m_forward)
|
||||
return (m_forward == false);
|
||||
if (m_segId != r.m_segId)
|
||||
return m_segId < r.m_segId;
|
||||
if (!(m_startJunction == r.m_startJunction))
|
||||
return m_startJunction < r.m_startJunction;
|
||||
if (!(m_endJunction == r.m_endJunction))
|
||||
return m_endJunction < r.m_endJunction;
|
||||
return false;
|
||||
}
|
||||
|
||||
string DebugPrint(Edge const & r)
|
||||
{
|
||||
ostringstream ss;
|
||||
ss << boolalpha << "Edge "
|
||||
<< "{ featureId: " << DebugPrint(r.GetFeatureId()) << ", isForward: " << r.IsForward()
|
||||
<< ", partOfReal: " << r.HasRealPart() << ", segId: " << r.m_segId
|
||||
<< ", startJunction: " << DebugPrint(r.m_startJunction) << ", endJunction: " << DebugPrint(r.m_endJunction)
|
||||
<< " }";
|
||||
return ss.str();
|
||||
}
|
||||
|
||||
std::string Edge::PrintLatLon() const
|
||||
{
|
||||
return "{ " + DebugPrint(mercator::ToLatLon(GetStartPoint())) + ", " + DebugPrint(mercator::ToLatLon(GetEndPoint())) +
|
||||
" }";
|
||||
}
|
||||
// IRoadGraph::RoadInfo --------------------------------------------------------
|
||||
|
||||
IRoadGraph::RoadInfo::RoadInfo() : m_speedKMPH(0.0), m_bidirectional(false) {}
|
||||
IRoadGraph::RoadInfo::RoadInfo(RoadInfo && ri)
|
||||
: m_junctions(std::move(ri.m_junctions))
|
||||
, m_speedKMPH(ri.m_speedKMPH)
|
||||
, m_bidirectional(ri.m_bidirectional)
|
||||
{}
|
||||
|
||||
IRoadGraph::RoadInfo::RoadInfo(bool bidirectional, double speedKMPH, initializer_list<JunctionPointT> const & points)
|
||||
: m_junctions(points)
|
||||
, m_speedKMPH(speedKMPH)
|
||||
, m_bidirectional(bidirectional)
|
||||
{}
|
||||
|
||||
// IRoadGraph::CrossOutgoingLoader ---------------------------------------------
|
||||
void IRoadGraph::CrossOutgoingLoader::LoadEdges(FeatureID const & featureId, PointWithAltitudeVec const & junctions,
|
||||
bool bidirectional)
|
||||
{
|
||||
ForEachEdge(junctions, [&](size_t segId, JunctionPointT const & endJunction, bool forward)
|
||||
{
|
||||
if (forward || bidirectional || m_mode == IRoadGraph::Mode::IgnoreOnewayTag)
|
||||
m_edges.push_back(Edge::MakeReal(featureId, forward, base::asserted_cast<uint32_t>(segId), m_cross, endJunction));
|
||||
});
|
||||
}
|
||||
|
||||
// IRoadGraph::CrossIngoingLoader ----------------------------------------------
|
||||
void IRoadGraph::CrossIngoingLoader::LoadEdges(FeatureID const & featureId, PointWithAltitudeVec const & junctions,
|
||||
bool bidirectional)
|
||||
{
|
||||
ForEachEdge(junctions, [&](size_t segId, JunctionPointT const & endJunction, bool forward)
|
||||
{
|
||||
if (!forward || bidirectional || m_mode == IRoadGraph::Mode::IgnoreOnewayTag)
|
||||
{
|
||||
m_edges.push_back(
|
||||
Edge::MakeReal(featureId, !forward, base::asserted_cast<uint32_t>(segId), endJunction, m_cross));
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
// IRoadGraph ------------------------------------------------------------------
|
||||
void IRoadGraph::GetOutgoingEdges(JunctionPointT const & junction, EdgeListT & edges) const
|
||||
{
|
||||
GetFakeOutgoingEdges(junction, edges);
|
||||
GetRegularOutgoingEdges(junction, edges);
|
||||
}
|
||||
|
||||
void IRoadGraph::GetIngoingEdges(JunctionPointT const & junction, EdgeListT & edges) const
|
||||
{
|
||||
GetFakeIngoingEdges(junction, edges);
|
||||
GetRegularIngoingEdges(junction, edges);
|
||||
}
|
||||
|
||||
void IRoadGraph::GetRegularOutgoingEdges(JunctionPointT const & junction, EdgeListT & edges) const
|
||||
{
|
||||
CrossOutgoingLoader loader(junction, GetMode(), edges);
|
||||
ForEachFeatureClosestToCross(junction.GetPoint(), loader);
|
||||
}
|
||||
|
||||
void IRoadGraph::GetRegularIngoingEdges(JunctionPointT const & junction, EdgeListT & edges) const
|
||||
{
|
||||
CrossIngoingLoader loader(junction, GetMode(), edges);
|
||||
ForEachFeatureClosestToCross(junction.GetPoint(), loader);
|
||||
}
|
||||
|
||||
void IRoadGraph::GetFakeOutgoingEdges(JunctionPointT const & junction, EdgeListT & edges) const
|
||||
{
|
||||
auto const it = m_fakeOutgoingEdges.find(junction);
|
||||
if (it != m_fakeOutgoingEdges.cend())
|
||||
edges.append(it->second.cbegin(), it->second.cend());
|
||||
}
|
||||
|
||||
void IRoadGraph::GetFakeIngoingEdges(JunctionPointT const & junction, EdgeListT & edges) const
|
||||
{
|
||||
auto const it = m_fakeIngoingEdges.find(junction);
|
||||
if (it != m_fakeIngoingEdges.cend())
|
||||
edges.append(it->second.cbegin(), it->second.cend());
|
||||
}
|
||||
|
||||
void IRoadGraph::ResetFakes()
|
||||
{
|
||||
m_fakeOutgoingEdges.clear();
|
||||
m_fakeIngoingEdges.clear();
|
||||
}
|
||||
|
||||
void IRoadGraph::AddEdge(JunctionPointT const & j, Edge const & e, EdgeCacheT & edges)
|
||||
{
|
||||
auto & cont = edges[j];
|
||||
ASSERT(is_sorted(cont.cbegin(), cont.cend()), ());
|
||||
auto const range = equal_range(cont.cbegin(), cont.cend(), e);
|
||||
// Note. The "if" condition below is necessary to prevent duplicates which may be added when
|
||||
// edges from |j| to "projection of |j|" and an edge in the opposite direction are added.
|
||||
if (range.first == range.second)
|
||||
cont.insert(range.second, e);
|
||||
}
|
||||
|
||||
void IRoadGraph::AddFakeEdges(JunctionPointT const & junction, vector<pair<Edge, JunctionPointT>> const & vicinity)
|
||||
{
|
||||
for (auto const & v : vicinity)
|
||||
{
|
||||
Edge const & ab = v.first;
|
||||
JunctionPointT const & p = v.second;
|
||||
|
||||
vector<Edge> edges;
|
||||
SplitEdge(ab, p, edges);
|
||||
|
||||
edges.push_back(Edge::MakeFake(junction, p));
|
||||
edges.push_back(Edge::MakeFake(p, junction));
|
||||
|
||||
ForEachFakeEdge([&](Edge const & uv)
|
||||
{
|
||||
if (OnEdge(p, uv))
|
||||
SplitEdge(uv, p, edges);
|
||||
});
|
||||
|
||||
for (auto const & uv : edges)
|
||||
{
|
||||
AddOutgoingFakeEdge(uv);
|
||||
AddIngoingFakeEdge(uv);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void IRoadGraph::AddOutgoingFakeEdge(Edge const & e)
|
||||
{
|
||||
AddEdge(e.GetStartJunction(), e, m_fakeOutgoingEdges);
|
||||
}
|
||||
|
||||
void IRoadGraph::AddIngoingFakeEdge(Edge const & e)
|
||||
{
|
||||
AddEdge(e.GetEndJunction(), e, m_fakeIngoingEdges);
|
||||
}
|
||||
|
||||
void IRoadGraph::GetEdgeTypes(Edge const & edge, feature::TypesHolder & types) const
|
||||
{
|
||||
if (edge.IsFake())
|
||||
types = feature::TypesHolder(feature::GeomType::Line);
|
||||
else
|
||||
GetFeatureTypes(edge.GetFeatureId(), types);
|
||||
}
|
||||
|
||||
string DebugPrint(IRoadGraph::Mode mode)
|
||||
{
|
||||
switch (mode)
|
||||
{
|
||||
case IRoadGraph::Mode::ObeyOnewayTag: return "ObeyOnewayTag";
|
||||
case IRoadGraph::Mode::IgnoreOnewayTag: return "IgnoreOnewayTag";
|
||||
}
|
||||
UNREACHABLE();
|
||||
}
|
||||
|
||||
IRoadGraph::RoadInfo MakeRoadInfoForTesting(bool bidirectional, double speedKMPH,
|
||||
initializer_list<m2::PointD> const & points)
|
||||
{
|
||||
IRoadGraph::RoadInfo ri(bidirectional, speedKMPH, {});
|
||||
for (auto const & p : points)
|
||||
ri.m_junctions.emplace_back(geometry::MakePointWithAltitudeForTesting(p));
|
||||
|
||||
return ri;
|
||||
}
|
||||
|
||||
// RoadGraphBase ------------------------------------------------------------------
|
||||
void RoadGraphBase::GetRouteEdges(EdgeVector & routeEdges) const
|
||||
{
|
||||
NOTIMPLEMENTED();
|
||||
}
|
||||
|
||||
} // namespace routing
|
||||
Loading…
Add table
Add a link
Reference in a new issue