572 lines
22 KiB
C++
572 lines
22 KiB
C++
/*
|
|
open source routing machine
|
|
Copyright (C) Dennis Luxen, others 2010
|
|
|
|
This program is free software; you can redistribute it and/or modify
|
|
it under the terms of the GNU AFFERO General Public License as published by
|
|
the Free Software Foundation; either version 3 of the License, or
|
|
any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU Affero General Public License
|
|
along with this program; if not, write to the Free Software
|
|
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
or see http://www.gnu.org/licenses/agpl.txt.
|
|
*/
|
|
|
|
#include "EdgeBasedGraphFactory.h"
|
|
|
|
EdgeBasedGraphFactory::EdgeBasedGraphFactory(
|
|
int number_of_nodes,
|
|
std::vector<ImportEdge> & input_edge_list,
|
|
std::vector<NodeID> & barrier_node_list,
|
|
std::vector<NodeID> & traffic_light_node_list,
|
|
std::vector<TurnRestriction> & input_restrictions_list,
|
|
std::vector<NodeInfo> & m_node_info_list,
|
|
SpeedProfileProperties speed_profile
|
|
) : speed_profile(speed_profile),
|
|
m_turn_restrictions_count(0),
|
|
m_node_info_list(m_node_info_list)
|
|
{
|
|
BOOST_FOREACH(const TurnRestriction & restriction, input_restrictions_list) {
|
|
std::pair<NodeID, NodeID> restriction_source =
|
|
std::make_pair(restriction.fromNode, restriction.viaNode);
|
|
unsigned index;
|
|
RestrictionMap::iterator restriction_iter = m_restriction_map.find(restriction_source);
|
|
if(restriction_iter == m_restriction_map.end()) {
|
|
index = m_restriction_bucket_list.size();
|
|
m_restriction_bucket_list.resize(index+1);
|
|
m_restriction_map.emplace(restriction_source, index);
|
|
} else {
|
|
index = restriction_iter->second;
|
|
//Map already contains an is_only_*-restriction
|
|
if(m_restriction_bucket_list.at(index).begin()->second) {
|
|
continue;
|
|
} else if(restriction.flags.isOnly) {
|
|
//We are going to insert an is_only_*-restriction. There can be only one.
|
|
m_turn_restrictions_count -= m_restriction_bucket_list.at(index).size();
|
|
m_restriction_bucket_list.at(index).clear();
|
|
}
|
|
}
|
|
++m_turn_restrictions_count;
|
|
m_restriction_bucket_list.at(index).push_back(
|
|
std::make_pair( restriction.toNode, restriction.flags.isOnly)
|
|
);
|
|
}
|
|
|
|
m_barrier_nodes.insert(
|
|
barrier_node_list.begin(),
|
|
barrier_node_list.end()
|
|
);
|
|
|
|
m_traffic_lights.insert(
|
|
traffic_light_node_list.begin(),
|
|
traffic_light_node_list.end()
|
|
);
|
|
|
|
DeallocatingVector< NodeBasedEdge > edges_list;
|
|
NodeBasedEdge edge;
|
|
BOOST_FOREACH(const ImportEdge & import_edge, input_edge_list) {
|
|
if(!import_edge.isForward()) {
|
|
edge.source = import_edge.target();
|
|
edge.target = import_edge.source();
|
|
edge.data.backward = import_edge.isForward();
|
|
edge.data.forward = import_edge.isBackward();
|
|
} else {
|
|
edge.source = import_edge.source();
|
|
edge.target = import_edge.target();
|
|
edge.data.forward = import_edge.isForward();
|
|
edge.data.backward = import_edge.isBackward();
|
|
}
|
|
if(edge.source == edge.target) {
|
|
continue;
|
|
}
|
|
edge.data.distance = (std::max)((int)import_edge.weight(), 1 );
|
|
assert( edge.data.distance > 0 );
|
|
edge.data.shortcut = false;
|
|
edge.data.roundabout = import_edge.isRoundabout();
|
|
edge.data.ignoreInGrid = import_edge.ignoreInGrid();
|
|
edge.data.nameID = import_edge.name();
|
|
edge.data.type = import_edge.type();
|
|
edge.data.isAccessRestricted = import_edge.isAccessRestricted();
|
|
edge.data.edgeBasedNodeID = edges_list.size();
|
|
edge.data.contraFlow = import_edge.isContraFlow();
|
|
edges_list.push_back( edge );
|
|
if( edge.data.backward ) {
|
|
std::swap( edge.source, edge.target );
|
|
edge.data.forward = import_edge.isBackward();
|
|
edge.data.backward = import_edge.isForward();
|
|
edge.data.edgeBasedNodeID = edges_list.size();
|
|
edges_list.push_back( edge );
|
|
}
|
|
}
|
|
std::vector<ImportEdge>().swap(input_edge_list);
|
|
std::sort( edges_list.begin(), edges_list.end() );
|
|
m_node_based_graph = boost::make_shared<NodeBasedDynamicGraph>(
|
|
number_of_nodes, edges_list
|
|
);
|
|
}
|
|
|
|
void EdgeBasedGraphFactory::GetEdgeBasedEdges(
|
|
DeallocatingVector< EdgeBasedEdge >& output_edge_list
|
|
) {
|
|
BOOST_ASSERT_MSG(
|
|
0 == output_edge_list.size(),
|
|
"Vector is not empty"
|
|
);
|
|
m_edge_based_edge_list.swap(output_edge_list);
|
|
}
|
|
|
|
void EdgeBasedGraphFactory::GetEdgeBasedNodes( std::vector<EdgeBasedNode> & nodes) {
|
|
#ifndef NDEBUG
|
|
BOOST_FOREACH(const EdgeBasedNode & node, m_edge_based_node_list){
|
|
assert(node.lat1 != INT_MAX); assert(node.lon1 != INT_MAX);
|
|
assert(node.lat2 != INT_MAX); assert(node.lon2 != INT_MAX);
|
|
}
|
|
#endif
|
|
nodes.swap(m_edge_based_node_list);
|
|
}
|
|
|
|
NodeID EdgeBasedGraphFactory::CheckForEmanatingIsOnlyTurn(
|
|
const NodeID u,
|
|
const NodeID v
|
|
) const {
|
|
const std::pair < NodeID, NodeID > restriction_source = std::make_pair(u, v);
|
|
RestrictionMap::const_iterator restriction_iter = m_restriction_map.find(restriction_source);
|
|
if (restriction_iter != m_restriction_map.end()) {
|
|
const unsigned index = restriction_iter->second;
|
|
BOOST_FOREACH(
|
|
const RestrictionSource & restriction_target,
|
|
m_restriction_bucket_list.at(index)
|
|
) {
|
|
if(restriction_target.second) {
|
|
return restriction_target.first;
|
|
}
|
|
}
|
|
}
|
|
return UINT_MAX;
|
|
}
|
|
|
|
bool EdgeBasedGraphFactory::CheckIfTurnIsRestricted(
|
|
const NodeID u,
|
|
const NodeID v,
|
|
const NodeID w
|
|
) const {
|
|
//only add an edge if turn is not a U-turn except it is the end of dead-end street.
|
|
const std::pair < NodeID, NodeID > restriction_source = std::make_pair(u, v);
|
|
RestrictionMap::const_iterator restriction_iter = m_restriction_map.find(restriction_source);
|
|
if (restriction_iter != m_restriction_map.end()) {
|
|
const unsigned index = restriction_iter->second;
|
|
BOOST_FOREACH(
|
|
const RestrictionTarget & restriction_target,
|
|
m_restriction_bucket_list.at(index)
|
|
) {
|
|
if(w == restriction_target.first) {
|
|
return true;
|
|
}
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
void EdgeBasedGraphFactory::InsertEdgeBasedNode(
|
|
EdgeIterator e1,
|
|
NodeIterator u,
|
|
NodeIterator v,
|
|
bool belongsToTinyComponent) {
|
|
EdgeData & data = m_node_based_graph->GetEdgeData(e1);
|
|
EdgeBasedNode currentNode;
|
|
currentNode.nameID = data.nameID;
|
|
currentNode.lat1 = m_node_info_list[u].lat;
|
|
currentNode.lon1 = m_node_info_list[u].lon;
|
|
currentNode.lat2 = m_node_info_list[v].lat;
|
|
currentNode.lon2 = m_node_info_list[v].lon;
|
|
currentNode.belongsToTinyComponent = belongsToTinyComponent;
|
|
currentNode.id = data.edgeBasedNodeID;
|
|
currentNode.ignoreInGrid = data.ignoreInGrid;
|
|
currentNode.weight = data.distance;
|
|
m_edge_based_node_list.push_back(currentNode);
|
|
}
|
|
|
|
void EdgeBasedGraphFactory::Run(
|
|
const char * original_edge_data_filename,
|
|
lua_State *lua_state
|
|
) {
|
|
SimpleLogger().Write() << "Identifying components of the road network";
|
|
|
|
Percent p(m_node_based_graph->GetNumberOfNodes());
|
|
unsigned skipped_turns_counter = 0;
|
|
unsigned node_based_edge_counter = 0;
|
|
unsigned original_edges_counter = 0;
|
|
|
|
std::ofstream edge_data_file(
|
|
original_edge_data_filename,
|
|
std::ios::binary
|
|
);
|
|
|
|
//writes a dummy value that is updated later
|
|
edge_data_file.write(
|
|
(char*)&original_edges_counter,
|
|
sizeof(unsigned)
|
|
);
|
|
|
|
unsigned current_component = 0, current_component_size = 0;
|
|
//Run a BFS on the undirected graph and identify small components
|
|
std::queue<std::pair<NodeID, NodeID> > bfs_queue;
|
|
std::vector<unsigned> component_index_list(
|
|
m_node_based_graph->GetNumberOfNodes(),
|
|
UINT_MAX
|
|
);
|
|
|
|
std::vector<NodeID> component_size_list;
|
|
//put unexplorered node with parent pointer into queue
|
|
for(
|
|
NodeID node = 0,
|
|
last_node = m_node_based_graph->GetNumberOfNodes();
|
|
node < last_node;
|
|
++node
|
|
) {
|
|
if(UINT_MAX == component_index_list[node]) {
|
|
bfs_queue.push(std::make_pair(node, node));
|
|
//mark node as read
|
|
component_index_list[node] = current_component;
|
|
p.printIncrement();
|
|
while(!bfs_queue.empty()) {
|
|
//fetch element from BFS queue
|
|
std::pair<NodeID, NodeID> current_queue_item = bfs_queue.front();
|
|
bfs_queue.pop();
|
|
// SimpleLogger().Write() << "sizeof queue: " << bfs_queue.size() <<
|
|
// ", current_component_sizes: " << current_component_size <<
|
|
//", settled nodes: " << settledNodes++ << ", max: " << endNodes;
|
|
const NodeID v = current_queue_item.first; //current node
|
|
const NodeID u = current_queue_item.second; //parent
|
|
//increment size counter of current component
|
|
++current_component_size;
|
|
const bool is_barrier_node = (m_barrier_nodes.find(v) != m_barrier_nodes.end());
|
|
if(!is_barrier_node) {
|
|
const NodeID to_node_of_only_restriction = CheckForEmanatingIsOnlyTurn(u, v);
|
|
|
|
//relaxieren edge outgoing edge like below where edge-expanded graph
|
|
for(
|
|
EdgeIterator e2 = m_node_based_graph->BeginEdges(v);
|
|
e2 < m_node_based_graph->EndEdges(v);
|
|
++e2
|
|
) {
|
|
NodeIterator w = m_node_based_graph->GetTarget(e2);
|
|
|
|
if(
|
|
to_node_of_only_restriction != UINT_MAX &&
|
|
w != to_node_of_only_restriction
|
|
) {
|
|
//We are at an only_-restriction but not at the right turn.
|
|
continue;
|
|
}
|
|
if( u != w ) {
|
|
//only add an edge if turn is not a U-turn except
|
|
//when it is at the end of a dead-end street.
|
|
if (!CheckIfTurnIsRestricted(u, v, w) ) {
|
|
//only add an edge if turn is not prohibited
|
|
if(UINT_MAX == component_index_list[w]) {
|
|
//insert next (node, parent) only if w has
|
|
//not yet been explored
|
|
//mark node as read
|
|
component_index_list[w] = current_component;
|
|
bfs_queue.push(std::make_pair(w,v));
|
|
p.printIncrement();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
//push size into vector
|
|
component_size_list.push_back(current_component_size);
|
|
//reset counters;
|
|
current_component_size = 0;
|
|
++current_component;
|
|
}
|
|
}
|
|
SimpleLogger().Write() <<
|
|
"identified: " << component_size_list.size() << " many components";
|
|
SimpleLogger().Write() <<
|
|
"generating edge-expanded nodes";
|
|
|
|
p.reinit(m_node_based_graph->GetNumberOfNodes());
|
|
//loop over all edges and generate new set of nodes.
|
|
for(
|
|
NodeIterator u = 0,
|
|
number_of_nodes = m_node_based_graph->GetNumberOfNodes();
|
|
u < number_of_nodes;
|
|
++u
|
|
) {
|
|
p.printIncrement();
|
|
for(
|
|
EdgeIterator e1 = m_node_based_graph->BeginEdges(u),
|
|
last_edge = m_node_based_graph->EndEdges(u);
|
|
e1 < last_edge;
|
|
++e1
|
|
) {
|
|
NodeIterator v = m_node_based_graph->GetTarget(e1);
|
|
|
|
if(m_node_based_graph->GetEdgeData(e1).type != SHRT_MAX) {
|
|
BOOST_ASSERT_MSG(e1 != UINT_MAX, "edge id invalid");
|
|
BOOST_ASSERT_MSG(u != UINT_MAX, "souce node invalid");
|
|
BOOST_ASSERT_MSG(v != UINT_MAX, "target node invalid");
|
|
//Note: edges that end on barrier nodes or on a turn restriction
|
|
//may actually be in two distinct components. We choose the smallest
|
|
const unsigned size_of_component = std::min(
|
|
component_size_list[component_index_list[u]],
|
|
component_size_list[component_index_list[v]]
|
|
);
|
|
|
|
InsertEdgeBasedNode( e1, u, v, size_of_component < 1000 );
|
|
}
|
|
}
|
|
}
|
|
|
|
SimpleLogger().Write()
|
|
<< "Generated " << m_edge_based_node_list.size() << " nodes in " <<
|
|
"edge-expanded graph";
|
|
SimpleLogger().Write() <<
|
|
"generating edge-expanded edges";
|
|
|
|
std::vector<NodeID>().swap(component_size_list);
|
|
BOOST_ASSERT_MSG(
|
|
0 == component_size_list.capacity(),
|
|
"component size vector not deallocated"
|
|
);
|
|
std::vector<NodeID>().swap(component_index_list);
|
|
BOOST_ASSERT_MSG(
|
|
0 == component_index_list.capacity(),
|
|
"component index vector not deallocated"
|
|
);
|
|
std::vector<OriginalEdgeData> original_edge_data_vector;
|
|
original_edge_data_vector.reserve(10000);
|
|
|
|
//Loop over all turns and generate new set of edges.
|
|
//Three nested loop look super-linear, but we are dealing with a (kind of)
|
|
//linear number of turns only.
|
|
p.reinit(m_node_based_graph->GetNumberOfNodes());
|
|
for(
|
|
NodeIterator u = 0,
|
|
last_node = m_node_based_graph->GetNumberOfNodes();
|
|
u < last_node;
|
|
++u
|
|
) {
|
|
for(
|
|
EdgeIterator e1 = m_node_based_graph->BeginEdges(u),
|
|
last_edge_u = m_node_based_graph->EndEdges(u);
|
|
e1 < last_edge_u;
|
|
++e1
|
|
) {
|
|
++node_based_edge_counter;
|
|
NodeIterator v = m_node_based_graph->GetTarget(e1);
|
|
bool is_barrier_node = (m_barrier_nodes.find(v) != m_barrier_nodes.end());
|
|
NodeID to_node_of_only_restriction = CheckForEmanatingIsOnlyTurn(u, v);
|
|
for(
|
|
EdgeIterator e2 = m_node_based_graph->BeginEdges(v),
|
|
last_edge_v = m_node_based_graph->EndEdges(v);
|
|
e2 < last_edge_v;
|
|
++e2
|
|
) {
|
|
const NodeIterator w = m_node_based_graph->GetTarget(e2);
|
|
if(
|
|
to_node_of_only_restriction != UINT_MAX &&
|
|
w != to_node_of_only_restriction
|
|
) {
|
|
//We are at an only_-restriction but not at the right turn.
|
|
++skipped_turns_counter;
|
|
continue;
|
|
}
|
|
|
|
if(u == w && 1 != m_node_based_graph->GetOutDegree(v) ) {
|
|
continue;
|
|
}
|
|
|
|
if( !is_barrier_node ) {
|
|
//only add an edge if turn is not a U-turn except when it is
|
|
//at the end of a dead-end street
|
|
if (
|
|
!CheckIfTurnIsRestricted(u, v, w) ||
|
|
(to_node_of_only_restriction != UINT_MAX && w == to_node_of_only_restriction)
|
|
) { //only add an edge if turn is not prohibited
|
|
const EdgeData edge_data1 = m_node_based_graph->GetEdgeData(e1);
|
|
const EdgeData edge_data2 = m_node_based_graph->GetEdgeData(e2);
|
|
assert(edge_data1.edgeBasedNodeID < m_node_based_graph->GetNumberOfEdges());
|
|
assert(edge_data2.edgeBasedNodeID < m_node_based_graph->GetNumberOfEdges());
|
|
|
|
if(!edge_data1.forward || !edge_data2.forward) {
|
|
continue;
|
|
}
|
|
|
|
unsigned distance = edge_data1.distance;
|
|
if(m_traffic_lights.find(v) != m_traffic_lights.end()) {
|
|
distance += speed_profile.trafficSignalPenalty;
|
|
}
|
|
const unsigned penalty =
|
|
GetTurnPenalty(u, v, w, lua_state);
|
|
TurnInstruction turnInstruction = AnalyzeTurn(u, v, w);
|
|
if(turnInstruction == TurnInstructions.UTurn){
|
|
distance += speed_profile.uTurnPenalty;
|
|
}
|
|
distance += penalty;
|
|
|
|
assert(edge_data1.edgeBasedNodeID != edge_data2.edgeBasedNodeID);
|
|
original_edge_data_vector.push_back(
|
|
OriginalEdgeData(
|
|
v,
|
|
edge_data2.nameID,
|
|
turnInstruction
|
|
)
|
|
);
|
|
++original_edges_counter;
|
|
|
|
if(original_edge_data_vector.size() > 100000) {
|
|
edge_data_file.write(
|
|
(char*)&(original_edge_data_vector[0]),
|
|
original_edge_data_vector.size()*sizeof(OriginalEdgeData)
|
|
);
|
|
original_edge_data_vector.clear();
|
|
}
|
|
|
|
m_edge_based_edge_list.push_back(
|
|
EdgeBasedEdge(
|
|
edge_data1.edgeBasedNodeID,
|
|
edge_data2.edgeBasedNodeID,
|
|
m_edge_based_edge_list.size(),
|
|
distance,
|
|
true,
|
|
false
|
|
)
|
|
);
|
|
} else {
|
|
++skipped_turns_counter;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
p.printIncrement();
|
|
}
|
|
edge_data_file.write(
|
|
(char*)&(original_edge_data_vector[0]),
|
|
original_edge_data_vector.size()*sizeof(OriginalEdgeData)
|
|
);
|
|
edge_data_file.seekp(std::ios::beg);
|
|
edge_data_file.write(
|
|
(char*)&original_edges_counter,
|
|
sizeof(unsigned)
|
|
);
|
|
edge_data_file.close();
|
|
|
|
SimpleLogger().Write() <<
|
|
"Generated " << m_edge_based_node_list.size() << " edge based nodes";
|
|
SimpleLogger().Write() <<
|
|
"Node-based graph contains " << node_based_edge_counter << " edges";
|
|
SimpleLogger().Write() <<
|
|
"Edge-expanded graph ...";
|
|
SimpleLogger().Write() <<
|
|
" contains " << m_edge_based_edge_list.size() << " edges";
|
|
SimpleLogger().Write() <<
|
|
" skips " << skipped_turns_counter << " turns, "
|
|
"defined by " << m_turn_restrictions_count << " restrictions";
|
|
}
|
|
|
|
int EdgeBasedGraphFactory::GetTurnPenalty(
|
|
const NodeID u,
|
|
const NodeID v,
|
|
const NodeID w,
|
|
lua_State *lua_state
|
|
) const {
|
|
const double angle = GetAngleBetweenThreeFixedPointCoordinates (
|
|
m_node_info_list[u],
|
|
m_node_info_list[v],
|
|
m_node_info_list[w]
|
|
);
|
|
|
|
if( speed_profile.has_turn_penalty_function ) {
|
|
try {
|
|
//call lua profile to compute turn penalty
|
|
return luabind::call_function<int>(
|
|
lua_state,
|
|
"turn_function",
|
|
180.-angle
|
|
);
|
|
} catch (const luabind::error &er) {
|
|
SimpleLogger().Write(logWARNING) << er.what();
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
TurnInstruction EdgeBasedGraphFactory::AnalyzeTurn(
|
|
const NodeID u,
|
|
const NodeID v,
|
|
const NodeID w
|
|
) const {
|
|
if(u == w) {
|
|
return TurnInstructions.UTurn;
|
|
}
|
|
|
|
EdgeIterator edge1 = m_node_based_graph->FindEdge(u, v);
|
|
EdgeIterator edge2 = m_node_based_graph->FindEdge(v, w);
|
|
|
|
EdgeData & data1 = m_node_based_graph->GetEdgeData(edge1);
|
|
EdgeData & data2 = m_node_based_graph->GetEdgeData(edge2);
|
|
|
|
if(!data1.contraFlow && data2.contraFlow) {
|
|
return TurnInstructions.EnterAgainstAllowedDirection;
|
|
}
|
|
if(data1.contraFlow && !data2.contraFlow) {
|
|
return TurnInstructions.LeaveAgainstAllowedDirection;
|
|
}
|
|
|
|
//roundabouts need to be handled explicitely
|
|
if(data1.roundabout && data2.roundabout) {
|
|
//Is a turn possible? If yes, we stay on the roundabout!
|
|
if( 1 == m_node_based_graph->GetOutDegree(v) ) {
|
|
//No turn possible.
|
|
return TurnInstructions.NoTurn;
|
|
}
|
|
return TurnInstructions.StayOnRoundAbout;
|
|
}
|
|
//Does turn start or end on roundabout?
|
|
if(data1.roundabout || data2.roundabout) {
|
|
//We are entering the roundabout
|
|
if( (!data1.roundabout) && data2.roundabout) {
|
|
return TurnInstructions.EnterRoundAbout;
|
|
}
|
|
//We are leaving the roundabout
|
|
if(data1.roundabout && (!data2.roundabout) ) {
|
|
return TurnInstructions.LeaveRoundAbout;
|
|
}
|
|
}
|
|
|
|
//If street names stay the same and if we are certain that it is not a
|
|
//a segment of a roundabout, we skip it.
|
|
if( data1.nameID == data2.nameID ) {
|
|
//TODO: Here we should also do a small graph exploration to check for
|
|
// more complex situations
|
|
if( 0 != data1.nameID ) {
|
|
return TurnInstructions.NoTurn;
|
|
} else if (m_node_based_graph->GetOutDegree(v) <= 2) {
|
|
return TurnInstructions.NoTurn;
|
|
}
|
|
}
|
|
|
|
const double angle = GetAngleBetweenThreeFixedPointCoordinates (
|
|
m_node_info_list[u],
|
|
m_node_info_list[v],
|
|
m_node_info_list[w]
|
|
);
|
|
|
|
return TurnInstructions.GetTurnDirectionOfInstruction(angle);
|
|
}
|
|
|
|
unsigned EdgeBasedGraphFactory::GetNumberOfNodes() const {
|
|
return m_node_based_graph->GetNumberOfEdges();
|
|
}
|