Merge branch 'develop' of https://github.com/DennisOSRM/Project-OSRM into develop

This commit is contained in:
DennisOSRM 2013-06-25 20:49:51 +02:00
commit b43a51f912
24 changed files with 638 additions and 486 deletions

View File

@ -12,7 +12,7 @@ file(GLOB PrepareGlob Contractor/*.cpp)
set(PrepareSources createHierarchy.cpp ${PrepareGlob})
add_executable(osrm-prepare ${PrepareSources})
file(GLOB RoutedGlob Server/DataStructures/*.cpp Descriptors/*.cpp)
file(GLOB RoutedGlob Server/DataStructures/*.cpp Descriptors/*.cpp DataStructures/SearchEngine*.cpp)
set(RoutedSources routed.cpp ${RoutedGlob})
add_executable(osrm-routed ${RoutedSources})
set_target_properties(osrm-routed PROPERTIES COMPILE_FLAGS -DROUTED)

View File

@ -20,17 +20,6 @@ or see http://www.gnu.org/licenses/agpl.txt.
#ifndef CONTRACTOR_H_INCLUDED
#define CONTRACTOR_H_INCLUDED
#include <algorithm>
#include <limits>
#include <vector>
#include <cfloat>
#include <ctime>
#include <boost/foreach.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#include "TemporaryStorage.h"
#include "../DataStructures/BinaryHeap.h"
@ -42,6 +31,19 @@ or see http://www.gnu.org/licenses/agpl.txt.
#include "../Util/OpenMPWrapper.h"
#include "../Util/StringUtil.h"
#include <boost/assert.hpp>
#include <boost/foreach.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
#include <cfloat>
#include <ctime>
#include <algorithm>
#include <limits>
#include <vector>
class Contractor {
private:
@ -119,7 +121,7 @@ public:
newEdge.target = diter->target();
newEdge.data = _ContractorEdgeData( (std::max)((int)diter->weight(), 1 ), 1, diter->id(), false, diter->isForward(), diter->isBackward());
assert( newEdge.data.distance > 0 );
BOOST_ASSERT_MSG( newEdge.data.distance > 0, "edge distance < 1" );
#ifndef NDEBUG
if ( newEdge.data.distance > 24 * 60 * 60 * 10 ) {
WARN("Edge weight large -> " << newEdge.data.distance);
@ -232,8 +234,9 @@ public:
//initialize the variables
#pragma omp parallel for schedule ( guided )
for ( int x = 0; x < ( int ) numberOfNodes; ++x )
for ( int x = 0; x < ( int ) numberOfNodes; ++x ) {
remainingNodes[x].id = x;
}
std::cout << "initializing elimination PQ ..." << std::flush;
#pragma omp parallel
@ -247,7 +250,7 @@ public:
std::cout << "ok" << std::endl << "preprocessing " << numberOfNodes << " nodes ..." << std::flush;
bool flushedContractor = false;
while ( numberOfContractedNodes < numberOfNodes ) {
while ( numberOfNodes > 2 && numberOfContractedNodes < numberOfNodes ) {
if(!flushedContractor && (numberOfContractedNodes > (numberOfNodes*0.65) ) ){
DeallocatingVector<_ContractorEdge> newSetOfEdges; //this one is not explicitely cleared since it goes out of scope anywa
std::cout << " [flush " << numberOfContractedNodes << " nodes] " << std::flush;
@ -282,7 +285,6 @@ public:
//walk over all nodes
for(unsigned i = 0; i < _graph->GetNumberOfNodes(); ++i) {
//INFO("Restructuring node " << i << "|" << _graph->GetNumberOfNodes());
const NodeID start = i;
for(_DynamicGraph::EdgeIterator currentEdge = _graph->BeginEdges(start); currentEdge < _graph->EndEdges(start); ++currentEdge) {
_DynamicGraph::EdgeData & data = _graph->GetEdgeData(currentEdge);
@ -301,8 +303,14 @@ public:
newEdge.target = newNodeIDFromOldNodeIDMap[target];
newEdge.data = data;
newEdge.data.originalViaNodeID = true;
assert(UINT_MAX != newNodeIDFromOldNodeIDMap[start] );
assert(UINT_MAX != newNodeIDFromOldNodeIDMap[target]);
BOOST_ASSERT_MSG(
UINT_MAX != newNodeIDFromOldNodeIDMap[start],
"new start id not resolveable"
);
BOOST_ASSERT_MSG(
UINT_MAX != newNodeIDFromOldNodeIDMap[target],
"new target id not resolveable"
);
newSetOfEdges.push_back(newEdge);
}
}
@ -311,8 +319,6 @@ public:
tempStorage.seek(temporaryStorageSlotID, initialFilePosition);
tempStorage.writeToSlot(temporaryStorageSlotID, (char*)&numberOfTemporaryEdges, sizeof(unsigned));
// INFO("Flushed " << numberOfTemporaryEdges << " edges to disk");
//Delete map from old NodeIDs to new ones.
std::vector<NodeID>().swap(newNodeIDFromOldNodeIDMap);
@ -438,38 +444,48 @@ public:
Percent p (_graph->GetNumberOfNodes());
INFO("Getting edges of minimized graph");
NodeID numberOfNodes = _graph->GetNumberOfNodes();
if(oldNodeIDFromNewNodeIDMap.size()) {
if(_graph->GetNumberOfNodes()) {
for ( NodeID node = 0; node < numberOfNodes; ++node ) {
p.printStatus(node);
for ( _DynamicGraph::EdgeIterator edge = _graph->BeginEdges( node ), endEdges = _graph->EndEdges( node ); edge < endEdges; ++edge ) {
const NodeID target = _graph->GetTarget( edge );
const _DynamicGraph::EdgeData& data = _graph->GetEdgeData( edge );
Edge newEdge;
if(0 != oldNodeIDFromNewNodeIDMap.size()) {
newEdge.source = oldNodeIDFromNewNodeIDMap[node];
newEdge.target = oldNodeIDFromNewNodeIDMap[target];
assert(UINT_MAX != newEdge.source);
assert(UINT_MAX != newEdge.target);
} else {
newEdge.source = node;
newEdge.target = target;
}
BOOST_ASSERT_MSG(
UINT_MAX != newEdge.source,
"Source id invalid"
);
BOOST_ASSERT_MSG(
UINT_MAX != newEdge.target,
"Target id invalid"
);
newEdge.data.distance = data.distance;
newEdge.data.shortcut = data.shortcut;
if(!data.originalViaNodeID)
if(!data.originalViaNodeID && oldNodeIDFromNewNodeIDMap.size()) {
newEdge.data.id = oldNodeIDFromNewNodeIDMap[data.id];
else
} else {
newEdge.data.id = data.id;
assert(newEdge.data.id != UINT_MAX);
}
BOOST_ASSERT_MSG(
newEdge.data.id <= INT_MAX, //2^31
"edge id invalid"
);
newEdge.data.forward = data.forward;
newEdge.data.backward = data.backward;
edges.push_back( newEdge );
}
}
}
INFO("Renumbered edges of minimized graph, freeing space");
_graph.reset();
std::vector<NodeID>().swap(oldNodeIDFromNewNodeIDMap);
INFO("Loading temporary edges");
// std::ifstream temporaryEdgeStorage(temporaryEdgeStorageFilename.c_str(), std::ios::binary);
TemporaryStorage & tempStorage = TemporaryStorage::GetInstance();
//Also get the edges from temporary storage
unsigned numberOfTemporaryEdges = 0;
@ -494,7 +510,6 @@ public:
edges.push_back( newEdge );
}
tempStorage.deallocateSlot(temporaryStorageSlotID);
INFO("Hierarchy has " << edges.size() << " edges");
}
private:
@ -517,24 +532,27 @@ private:
if ( heap.GetData( node ).target ) {
++targetsFound;
if ( targetsFound >= numTargets )
if ( targetsFound >= numTargets ) {
return;
}
}
//iterate over all edges of node
for ( _DynamicGraph::EdgeIterator edge = _graph->BeginEdges( node ), endEdges = _graph->EndEdges( node ); edge != endEdges; ++edge ) {
const _ContractorEdgeData& data = _graph->GetEdgeData( edge );
if ( !data.forward )
if ( !data.forward ){
continue;
}
const NodeID to = _graph->GetTarget( edge );
if(middleNode == to)
if(middleNode == to) {
continue;
}
const int toDistance = distance + data.distance;
//New Node discovered -> Add to Heap + Node Info Storage
if ( !heap.WasInserted( to ) )
if ( !heap.WasInserted( to ) ) {
heap.Insert( to, toDistance, _HeapData(currentHop, false) );
}
//Found a shorter Path -> Update distance
else if ( toDistance < heap.GetKey( to ) ) {
heap.DecreaseKey( to, toDistance );
@ -584,8 +602,9 @@ private:
for ( _DynamicGraph::EdgeIterator outEdge = _graph->BeginEdges( node ), endOutEdges = _graph->EndEdges( node ); outEdge != endOutEdges; ++outEdge ) {
const _ContractorEdgeData& outData = _graph->GetEdgeData( outEdge );
if ( !outData.forward )
if ( !outData.forward ) {
continue;
}
const NodeID target = _graph->GetTarget( outEdge );
const int pathDistance = inData.distance + outData.distance;
maxDistance = std::max( maxDistance, pathDistance );
@ -595,15 +614,16 @@ private:
}
}
if( Simulate )
if( Simulate ) {
_Dijkstra( maxDistance, numTargets, 1000, data, node );
else
} else {
_Dijkstra( maxDistance, numTargets, 2000, data, node );
}
for ( _DynamicGraph::EdgeIterator outEdge = _graph->BeginEdges( node ), endOutEdges = _graph->EndEdges( node ); outEdge != endOutEdges; ++outEdge ) {
const _ContractorEdgeData& outData = _graph->GetEdgeData( outEdge );
if ( !outData.forward )
if ( !outData.forward ) {
continue;
}
const NodeID target = _graph->GetTarget( outEdge );
const int pathDistance = inData.distance + outData.distance;
const int distance = heap.GetKey( target );
@ -643,9 +663,10 @@ private:
found = true;
break;
}
if ( !found )
if ( !found ) {
insertedEdges[insertedEdgesSize++] = insertedEdges[i];
}
}
insertedEdges.resize( insertedEdgesSize );
}
return true;

View File

@ -89,7 +89,10 @@ EdgeBasedGraphFactory::EdgeBasedGraphFactory(int nodes, std::vector<NodeBasedEdg
}
void EdgeBasedGraphFactory::GetEdgeBasedEdges(DeallocatingVector< EdgeBasedEdge >& outputEdgeList ) {
GUARANTEE(0 == outputEdgeList.size(), "Vector passed to EdgeBasedGraphFactory::GetEdgeBasedEdges(..) is not empty");
BOOST_ASSERT_MSG(
0 == outputEdgeList.size(),
"Vector is not empty"
);
edgeBasedEdges.swap(outputEdgeList);
}

View File

@ -21,7 +21,10 @@ or see http://www.gnu.org/licenses/agpl.txt.
#ifndef COORDINATE_H_
#define COORDINATE_H_
#include <cassert>
#include <cmath>
#include <climits>
#include <iostream>
struct _Coordinate {
@ -102,5 +105,4 @@ inline double ApproximateDistanceByEuclid(const _Coordinate &c1, const _Coordina
return d;
}
#endif /* COORDINATE_H_ */

View File

@ -21,18 +21,20 @@ or see http://www.gnu.org/licenses/agpl.txt.
#ifndef DYNAMICGRAPH_H_INCLUDED
#define DYNAMICGRAPH_H_INCLUDED
#include <vector>
#include "../DataStructures/DeallocatingVector.h"
#include <boost/integer.hpp>
#include <algorithm>
#include <limits>
#include "../DataStructures/DeallocatingVector.h"
#include <vector>
template< typename EdgeDataT>
class DynamicGraph {
public:
typedef EdgeDataT EdgeData;
typedef unsigned NodeIterator;
typedef unsigned EdgeIterator;
typedef uint32_t NodeIterator;
typedef uint32_t EdgeIterator;
class InputEdge {
public:
@ -47,16 +49,16 @@ class DynamicGraph {
};
//Constructs an empty graph with a given number of nodes.
DynamicGraph( int nodes ) : m_numNodes(nodes), m_numEdges(0) {
DynamicGraph( int32_t nodes ) : m_numNodes(nodes), m_numEdges(0) {
m_nodes.reserve( m_numNodes );
m_nodes.resize( m_numNodes );
m_edges.reserve( m_numNodes * 1.1 );
m_edges.resize( m_numNodes );
}
template<class ContainerT>
DynamicGraph( const int nodes, const ContainerT &graph )
{
DynamicGraph( const int32_t nodes, const ContainerT &graph ) {
m_numNodes = nodes;
m_numEdges = ( EdgeIterator ) graph.size();
m_nodes.reserve( m_numNodes +1);
@ -80,7 +82,10 @@ class DynamicGraph {
for ( EdgeIterator i = m_nodes[node].firstEdge, e = m_nodes[node].firstEdge + m_nodes[node].edges; i != e; ++i ) {
m_edges[i].target = graph[edge].target;
m_edges[i].data = graph[edge].data;
GUARANTEE(graph[edge].data.distance > 0, "edge: " << edge << "(" << graph[edge].source << "," << graph[edge].target << ")=" << graph[edge].data.distance);
BOOST_ASSERT_MSG(
graph[edge].data.distance > 0,
"edge distance invalid"
);
++edge;
}
}
@ -88,17 +93,15 @@ class DynamicGraph {
~DynamicGraph(){ }
unsigned GetNumberOfNodes() const
{
uint32_t GetNumberOfNodes() const {
return m_numNodes;
}
unsigned GetNumberOfEdges() const
{
uint32_t GetNumberOfEdges() const {
return m_numEdges;
}
unsigned GetOutDegree( const NodeIterator n ) const {
uint32_t GetOutDegree( const NodeIterator n ) const {
return m_nodes[n].edges;
}
@ -133,7 +136,7 @@ class DynamicGraph {
m_edges[node.firstEdge] = m_edges[node.firstEdge + node.edges];
} else {
EdgeIterator newFirstEdge = ( EdgeIterator ) m_edges.size();
unsigned newSize = node.edges * 1.1 + 2;
uint32_t newSize = node.edges * 1.1 + 2;
EdgeIterator requiredCapacity = newSize + m_edges.size();
EdgeIterator oldCapacity = m_edges.capacity();
if ( requiredCapacity >= oldCapacity ) {
@ -162,15 +165,15 @@ class DynamicGraph {
Node &node = m_nodes[source];
--m_numEdges;
--node.edges;
const unsigned last = node.firstEdge + node.edges;
const uint32_t last = node.firstEdge + node.edges;
//swap with last edge
m_edges[e] = m_edges[last];
makeDummy( last );
}
//removes all edges (source,target)
int DeleteEdgesTo( const NodeIterator source, const NodeIterator target ) {
int deleted = 0;
int32_t DeleteEdgesTo( const NodeIterator source, const NodeIterator target ) {
int32_t deleted = 0;
for ( EdgeIterator i = BeginEdges( source ), iend = EndEdges( source ); i < iend - deleted; ++i ) {
if ( m_edges[i].target == target ) {
do {
@ -212,7 +215,7 @@ class DynamicGraph {
//index of the first edge
EdgeIterator firstEdge;
//amount of edges
unsigned edges;
uint32_t edges;
};
struct Edge {

View File

@ -0,0 +1,89 @@
/*
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 "SearchEngine.h"
SearchEngine::SearchEngine(
QueryGraph * g,
NodeInformationHelpDesk * nh,
std::vector<std::string> & n
) :
_queryData(g, nh, n),
shortestPath(_queryData),
alternativePaths(_queryData)
{}
SearchEngine::~SearchEngine() {}
void SearchEngine::GetCoordinatesForNodeID(
NodeID id,
_Coordinate& result
) const {
result.lat = _queryData.nodeHelpDesk->getLatitudeOfNode(id);
result.lon = _queryData.nodeHelpDesk->getLongitudeOfNode(id);
}
void SearchEngine::FindPhantomNodeForCoordinate(
const _Coordinate & location,
PhantomNode & result,
const unsigned zoomLevel
) const {
_queryData.nodeHelpDesk->FindPhantomNodeForCoordinate(
location,
result, zoomLevel
);
}
NodeID SearchEngine::GetNameIDForOriginDestinationNodeID(
const NodeID s,
const NodeID t
) const {
if(s == t){
return 0;
}
EdgeID e = _queryData.graph->FindEdge(s, t);
if(e == UINT_MAX) {
e = _queryData.graph->FindEdge( t, s );
}
if(UINT_MAX == e) {
return 0;
}
assert(e != UINT_MAX);
const QueryEdge::EdgeData ed = _queryData.graph->GetEdgeData(e);
return ed.id;
}
std::string SearchEngine::GetEscapedNameForNameID(const unsigned nameID) const {
bool is_name_invalid = (nameID >= _queryData.names.size() || nameID == 0);
if (is_name_invalid) {
return std::string("");
}
return HTMLEntitize(_queryData.names.at(nameID));
}
SearchEngineHeapPtr SearchEngineData::forwardHeap;
SearchEngineHeapPtr SearchEngineData::backwardHeap;
SearchEngineHeapPtr SearchEngineData::forwardHeap2;
SearchEngineHeapPtr SearchEngineData::backwardHeap2;
SearchEngineHeapPtr SearchEngineData::forwardHeap3;
SearchEngineHeapPtr SearchEngineData::backwardHeap3;

View File

@ -21,152 +21,48 @@ or see http://www.gnu.org/licenses/agpl.txt.
#ifndef SEARCHENGINE_H_
#define SEARCHENGINE_H_
#include <climits>
#include <deque>
#include "SimpleStack.h"
#include <boost/thread.hpp>
#include "BinaryHeap.h"
#include "Coordinate.h"
#include "NodeInformationHelpDesk.h"
#include "PhantomNodes.h"
#include "QueryEdge.h"
#include "SearchEngineData.h"
#include "../RoutingAlgorithms/AlternativePathRouting.h"
#include "../RoutingAlgorithms/BasicRoutingInterface.h"
#include "../RoutingAlgorithms/ShortestPathRouting.h"
#include "../Util/StringUtil.h"
#include "../typedefs.h"
struct _HeapData {
NodeID parent;
_HeapData( NodeID p ) : parent(p) { }
};
#include <climits>
#include <string>
#include <vector>
typedef BinaryHeap< NodeID, NodeID, int, _HeapData, UnorderedMapStorage<NodeID, int> > QueryHeapType;
typedef boost::thread_specific_ptr<QueryHeapType> SearchEngineHeapPtr;
template<class EdgeData, class GraphT>
struct SearchEngineData {
typedef GraphT Graph;
typedef QueryHeapType QueryHeap;
SearchEngineData(GraphT * g, NodeInformationHelpDesk * nh, std::vector<std::string> & n) :graph(g), nodeHelpDesk(nh), names(n) {}
const GraphT * graph;
NodeInformationHelpDesk * nodeHelpDesk;
std::vector<std::string> & names;
static SearchEngineHeapPtr forwardHeap;
static SearchEngineHeapPtr backwardHeap;
static SearchEngineHeapPtr forwardHeap2;
static SearchEngineHeapPtr backwardHeap2;
static SearchEngineHeapPtr forwardHeap3;
static SearchEngineHeapPtr backwardHeap3;
inline void InitializeOrClearFirstThreadLocalStorage() {
if(!forwardHeap.get()) {
forwardHeap.reset(new QueryHeap(nodeHelpDesk->getNumberOfNodes()));
}
else
forwardHeap->Clear();
if(!backwardHeap.get()) {
backwardHeap.reset(new QueryHeap(nodeHelpDesk->getNumberOfNodes()));
}
else
backwardHeap->Clear();
}
inline void InitializeOrClearSecondThreadLocalStorage() {
if(!forwardHeap2.get()) {
forwardHeap2.reset(new QueryHeap(nodeHelpDesk->getNumberOfNodes()));
}
else
forwardHeap2->Clear();
if(!backwardHeap2.get()) {
backwardHeap2.reset(new QueryHeap(nodeHelpDesk->getNumberOfNodes()));
}
else
backwardHeap2->Clear();
}
inline void InitializeOrClearThirdThreadLocalStorage() {
if(!forwardHeap3.get()) {
forwardHeap3.reset(new QueryHeap(nodeHelpDesk->getNumberOfNodes()));
}
else
forwardHeap3->Clear();
if(!backwardHeap3.get()) {
backwardHeap3.reset(new QueryHeap(nodeHelpDesk->getNumberOfNodes()));
}
else
backwardHeap3->Clear();
}
};
template<class EdgeData, class GraphT>
class SearchEngine {
private:
typedef SearchEngineData<EdgeData, GraphT> SearchEngineDataT;
SearchEngineDataT _queryData;
SearchEngineData _queryData;
inline double absDouble(double input) { if(input < 0) return input*(-1); else return input;}
public:
ShortestPathRouting<SearchEngineDataT> shortestPath;
AlternativeRouting<SearchEngineDataT> alternativePaths;
ShortestPathRouting<SearchEngineData> shortestPath;
AlternativeRouting<SearchEngineData> alternativePaths;
SearchEngine(GraphT * g, NodeInformationHelpDesk * nh, std::vector<std::string> & n) :
_queryData(g, nh, n),
shortestPath(_queryData),
alternativePaths(_queryData)
{}
~SearchEngine() {}
SearchEngine(
QueryGraph * g,
NodeInformationHelpDesk * nh,
std::vector<std::string> & n
);
~SearchEngine();
inline void GetCoordinatesForNodeID(NodeID id, _Coordinate& result) const {
result.lat = _queryData.nodeHelpDesk->getLatitudeOfNode(id);
result.lon = _queryData.nodeHelpDesk->getLongitudeOfNode(id);
}
void GetCoordinatesForNodeID(NodeID id, _Coordinate& result) const;
inline void FindRoutingStarts(const _Coordinate & start, const _Coordinate & target, PhantomNodes & routingStarts) const {
_queryData.nodeHelpDesk->FindRoutingStarts(start, target, routingStarts);
}
void FindPhantomNodeForCoordinate(
const _Coordinate & location,
PhantomNode & result,
unsigned zoomLevel
) const;
inline void FindPhantomNodeForCoordinate(const _Coordinate & location, PhantomNode & result, unsigned zoomLevel) const {
_queryData.nodeHelpDesk->FindPhantomNodeForCoordinate(location, result, zoomLevel);
}
inline NodeID GetNameIDForOriginDestinationNodeID(const NodeID s, const NodeID t) const {
if(s == t)
return 0;
EdgeID e = _queryData.graph->FindEdge(s, t);
if(e == UINT_MAX)
e = _queryData.graph->FindEdge( t, s );
if(UINT_MAX == e) {
return 0;
}
assert(e != UINT_MAX);
const EdgeData ed = _queryData.graph->GetEdgeData(e);
return ed.via;
}
inline std::string GetEscapedNameForNameID(const unsigned nameID) const {
return ((nameID >= _queryData.names.size() || nameID == 0) ? std::string("") : HTMLEntitize(_queryData.names.at(nameID)));
}
inline std::string GetEscapedNameForEdgeBasedEdgeID(const unsigned edgeID) const {
const unsigned nameID = _queryData.graph->GetEdgeData(edgeID).nameID1;
return GetEscapedNameForNameID(nameID);
}
NodeID GetNameIDForOriginDestinationNodeID(
const NodeID s, const NodeID t) const;
std::string GetEscapedNameForNameID(const unsigned nameID) const;
};
template<class EdgeData, class GraphT> SearchEngineHeapPtr SearchEngineData<EdgeData, GraphT>::forwardHeap;
template<class EdgeData, class GraphT> SearchEngineHeapPtr SearchEngineData<EdgeData, GraphT>::backwardHeap;
template<class EdgeData, class GraphT> SearchEngineHeapPtr SearchEngineData<EdgeData, GraphT>::forwardHeap2;
template<class EdgeData, class GraphT> SearchEngineHeapPtr SearchEngineData<EdgeData, GraphT>::backwardHeap2;
template<class EdgeData, class GraphT> SearchEngineHeapPtr SearchEngineData<EdgeData, GraphT>::forwardHeap3;
template<class EdgeData, class GraphT> SearchEngineHeapPtr SearchEngineData<EdgeData, GraphT>::backwardHeap3;
#endif /* SEARCHENGINE_H_ */

View File

@ -0,0 +1,60 @@
/*
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 "SearchEngineData.h"
void SearchEngineData::InitializeOrClearFirstThreadLocalStorage() {
if(!forwardHeap.get()) {
forwardHeap.reset(new QueryHeap(nodeHelpDesk->getNumberOfNodes()));
} else {
forwardHeap->Clear();
}
if(!backwardHeap.get()) {
backwardHeap.reset(new QueryHeap(nodeHelpDesk->getNumberOfNodes()));
} else {
backwardHeap->Clear();
}
}
void SearchEngineData::InitializeOrClearSecondThreadLocalStorage() {
if(!forwardHeap2.get()) {
forwardHeap2.reset(new QueryHeap(nodeHelpDesk->getNumberOfNodes()));
} else {
forwardHeap2->Clear();
}
if(!backwardHeap2.get()) {
backwardHeap2.reset(new QueryHeap(nodeHelpDesk->getNumberOfNodes()));
} else {
backwardHeap2->Clear();
}
}
void SearchEngineData::InitializeOrClearThirdThreadLocalStorage() {
if(!forwardHeap3.get()) {
forwardHeap3.reset(new QueryHeap(nodeHelpDesk->getNumberOfNodes()));
} else {
forwardHeap3->Clear();
}
if(!backwardHeap3.get()) {
backwardHeap3.reset(new QueryHeap(nodeHelpDesk->getNumberOfNodes()));
} else {
backwardHeap3->Clear();
}
}

View File

@ -0,0 +1,60 @@
/*
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 "BinaryHeap.h"
#include "QueryEdge.h"
#include "NodeInformationHelpDesk.h"
#include "StaticGraph.h"
#include "../typedefs.h"
#include <boost/thread.hpp>
#include <string>
#include <vector>
struct _HeapData {
NodeID parent;
_HeapData( NodeID p ) : parent(p) { }
};
typedef StaticGraph<QueryEdge::EdgeData> QueryGraph;
typedef BinaryHeap< NodeID, NodeID, int, _HeapData, UnorderedMapStorage<NodeID, int> > QueryHeapType;
typedef boost::thread_specific_ptr<QueryHeapType> SearchEngineHeapPtr;
struct SearchEngineData {
typedef QueryGraph Graph;
typedef QueryHeapType QueryHeap;
SearchEngineData(QueryGraph * g, NodeInformationHelpDesk * nh, std::vector<std::string> & n) :graph(g), nodeHelpDesk(nh), names(n) {}
const QueryGraph * graph;
NodeInformationHelpDesk * nodeHelpDesk;
std::vector<std::string> & names;
static SearchEngineHeapPtr forwardHeap;
static SearchEngineHeapPtr backwardHeap;
static SearchEngineHeapPtr forwardHeap2;
static SearchEngineHeapPtr backwardHeap2;
static SearchEngineHeapPtr forwardHeap3;
static SearchEngineHeapPtr backwardHeap3;
void InitializeOrClearFirstThreadLocalStorage();
void InitializeOrClearSecondThreadLocalStorage();
void InitializeOrClearThirdThreadLocalStorage();
};

View File

@ -28,8 +28,9 @@ or see http://www.gnu.org/licenses/agpl.txt.
#include <vector>
#include "../typedefs.h"
#include "../DataStructures/PhantomNodes.h"
#include "../DataStructures/HashTable.h"
#include "../DataStructures/PhantomNodes.h"
#include "../DataStructures/SearchEngine.h"
#include "../Util/StringUtil.h"
#include "../Plugins/RawRouteData.h"
@ -42,13 +43,12 @@ struct _DescriptorConfig {
unsigned short z;
};
template<class SearchEngineT>
class BaseDescriptor {
public:
BaseDescriptor() { }
//Maybe someone can explain the pure virtual destructor thing to me (dennis)
virtual ~BaseDescriptor() { }
virtual void Run(http::Reply & reply, const RawRouteData &rawRoute, PhantomNodes &phantomNodes, SearchEngineT &sEngine) = 0;
virtual void Run(http::Reply & reply, const RawRouteData &rawRoute, PhantomNodes &phantomNodes, SearchEngine &sEngine) = 0;
virtual void SetConfig(const _DescriptorConfig & config) = 0;
};

View File

@ -82,7 +82,7 @@ void DescriptionFactory::AppendUnencodedPolylineString(std::string &output) {
pc.printUnencodedString(pathDescription, output);
}
void DescriptionFactory::Run(const SearchEngineT &sEngine, const unsigned zoomLevel) {
void DescriptionFactory::Run(const SearchEngine &sEngine, const unsigned zoomLevel) {
if(0 == pathDescription.size())
return;

View File

@ -27,7 +27,6 @@
#include "../Algorithms/DouglasPeucker.h"
#include "../Algorithms/PolylineCompressor.h"
#include "../DataStructures/Coordinate.h"
#include "../DataStructures/QueryEdge.h"
#include "../DataStructures/SearchEngine.h"
#include "../DataStructures/SegmentInformation.h"
#include "../DataStructures/TurnInstructions.h"
@ -40,8 +39,6 @@ class DescriptionFactory {
PolylineCompressor pc;
PhantomNode startPhantom, targetPhantom;
typedef SearchEngine<QueryEdge::EdgeData, StaticGraph<QueryEdge::EdgeData> > SearchEngineT;
double DegreeToRadian(const double degree) const;
double RadianToDegree(const double degree) const;
public:
@ -73,7 +70,7 @@ public:
void SetStartSegment(const PhantomNode & startPhantom);
void SetEndSegment(const PhantomNode & startPhantom);
void AppendEncodedPolylineString(std::string & output, bool isEncoded);
void Run(const SearchEngineT &sEngine, const unsigned zoomLevel);
void Run(const SearchEngine &sEngine, const unsigned zoomLevel);
};
#endif /* DESCRIPTIONFACTORY_H_ */

View File

@ -24,8 +24,7 @@ or see http://www.gnu.org/licenses/agpl.txt.
#include <boost/foreach.hpp>
#include "BaseDescriptor.h"
template<class SearchEngineT>
class GPXDescriptor : public BaseDescriptor<SearchEngineT>{
class GPXDescriptor : public BaseDescriptor{
private:
_DescriptorConfig config;
_Coordinate current;
@ -33,7 +32,7 @@ private:
std::string tmp;
public:
void SetConfig(const _DescriptorConfig& c) { config = c; }
void Run(http::Reply & reply, const RawRouteData &rawRoute, PhantomNodes &phantomNodes, SearchEngineT &sEngine) {
void Run(http::Reply & reply, const RawRouteData &rawRoute, PhantomNodes &phantomNodes, SearchEngine &sEngine) {
reply.content += ("<?xml version=\"1.0\" encoding=\"UTF-8\"?>");
reply.content += "<gpx creator=\"OSRM Routing Engine\" version=\"1.1\" xmlns=\"http://www.topografix.com/GPX/1/1\" "
"xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\" "

View File

@ -34,8 +34,7 @@ or see http://www.gnu.org/licenses/agpl.txt.
#include "../Util/Azimuth.h"
#include "../Util/StringUtil.h"
template<class SearchEngineT>
class JSONDescriptor : public BaseDescriptor<SearchEngineT>{
class JSONDescriptor : public BaseDescriptor{
private:
_DescriptorConfig config;
DescriptionFactory descriptionFactory;
@ -68,7 +67,7 @@ public:
JSONDescriptor() : numberOfEnteredRestrictedAreas(0) {}
void SetConfig(const _DescriptorConfig & c) { config = c; }
void Run(http::Reply & reply, const RawRouteData &rawRoute, PhantomNodes &phantomNodes, SearchEngineT &sEngine) {
void Run(http::Reply & reply, const RawRouteData &rawRoute, PhantomNodes &phantomNodes, SearchEngine &sEngine) {
WriteHeaderToOutput(reply.content);
@ -246,7 +245,7 @@ public:
reply.content += "}";
}
void GetRouteNames(std::vector<Segment> & shortestSegments, std::vector<Segment> & alternativeSegments, const SearchEngineT &sEngine, RouteNames & routeNames) {
void GetRouteNames(std::vector<Segment> & shortestSegments, std::vector<Segment> & alternativeSegments, const SearchEngine &sEngine, RouteNames & routeNames) {
/*** extract names for both alternatives ***/
Segment shortestSegment1, shortestSegment2;
@ -304,7 +303,7 @@ public:
"\"status\":";
}
inline void BuildTextualDescription(DescriptionFactory & descriptionFactory, http::Reply & reply, const int lengthOfRoute, const SearchEngineT &sEngine, std::vector<Segment> & segmentVector) {
inline void BuildTextualDescription(DescriptionFactory & descriptionFactory, http::Reply & reply, const int lengthOfRoute, const SearchEngine &sEngine, std::vector<Segment> & segmentVector) {
//Segment information has following format:
//["instruction","streetname",length,position,time,"length","earth_direction",azimuth]
//Example: ["Turn left","High Street",200,4,10,"200m","NE",22.5]

View File

@ -257,8 +257,8 @@ void ExtractionContainers::PrepareData(const std::string & outputFileName, const
fout.write((char*)&edgeIT->ignoreInGrid, sizeof(bool));
fout.write((char*)&edgeIT->isAccessRestricted, sizeof(bool));
fout.write((char*)&edgeIT->isContraFlow, sizeof(bool));
}
++usedEdgeCounter;
}
++edgeIT;
}
}

View File

@ -52,14 +52,14 @@ private:
StaticGraph<QueryEdge::EdgeData> * graph;
HashTable<std::string, unsigned> descriptorTable;
std::string pluginDescriptorString;
SearchEngine<QueryEdge::EdgeData, StaticGraph<QueryEdge::EdgeData> > * searchEngine;
SearchEngine * searchEnginePtr;
public:
ViaRoutePlugin(QueryObjectsStorage * objects, std::string psd = "viaroute") : names(objects->names), pluginDescriptorString(psd) {
nodeHelpDesk = objects->nodeHelpDesk;
graph = objects->graph;
searchEngine = new SearchEngine<QueryEdge::EdgeData, StaticGraph<QueryEdge::EdgeData> >(graph, nodeHelpDesk, names);
searchEnginePtr = new SearchEngine(graph, nodeHelpDesk, names);
descriptorTable.Set("", 0); //default descriptor
descriptorTable.Set("json", 0);
@ -67,7 +67,7 @@ public:
}
virtual ~ViaRoutePlugin() {
delete searchEngine;
delete searchEnginePtr;
}
std::string GetDescriptor() const { return pluginDescriptorString; }
@ -101,7 +101,7 @@ public:
}
}
// INFO("Brute force lookup of coordinate " << i);
searchEngine->FindPhantomNodeForCoordinate( rawRoute.rawViaNodeCoordinates[i], phantomNodeVector[i], routeParameters.zoomLevel);
searchEnginePtr->FindPhantomNodeForCoordinate( rawRoute.rawViaNodeCoordinates[i], phantomNodeVector[i], routeParameters.zoomLevel);
}
for(unsigned i = 0; i < phantomNodeVector.size()-1; ++i) {
@ -112,10 +112,10 @@ public:
}
if( ( routeParameters.alternateRoute ) && (1 == rawRoute.segmentEndCoordinates.size()) ) {
// INFO("Checking for alternative paths");
searchEngine->alternativePaths(rawRoute.segmentEndCoordinates[0], rawRoute);
searchEnginePtr->alternativePaths(rawRoute.segmentEndCoordinates[0], rawRoute);
} else {
searchEngine->shortestPath(rawRoute.segmentEndCoordinates, rawRoute);
searchEnginePtr->shortestPath(rawRoute.segmentEndCoordinates, rawRoute);
}
@ -125,7 +125,7 @@ public:
reply.status = http::Reply::ok;
//TODO: Move to member as smart pointer
BaseDescriptor<SearchEngine<QueryEdge::EdgeData, StaticGraph<QueryEdge::EdgeData> > > * desc;
BaseDescriptor * desc;
if("" != routeParameters.jsonpParameter) {
reply.content += routeParameters.jsonpParameter;
reply.content += "(";
@ -140,15 +140,15 @@ public:
switch(descriptorType){
case 0:
desc = new JSONDescriptor<SearchEngine<QueryEdge::EdgeData, StaticGraph<QueryEdge::EdgeData> > >();
desc = new JSONDescriptor();
break;
case 1:
desc = new GPXDescriptor<SearchEngine<QueryEdge::EdgeData, StaticGraph<QueryEdge::EdgeData> > >();
desc = new GPXDescriptor();
break;
default:
desc = new JSONDescriptor<SearchEngine<QueryEdge::EdgeData, StaticGraph<QueryEdge::EdgeData> > >();
desc = new JSONDescriptor();
break;
}
@ -161,7 +161,7 @@ public:
// INFO("Number of segments: " << rawRoute.segmentEndCoordinates.size());
desc->SetConfig(descriptorConfig);
desc->Run(reply, rawRoute, phantomNodes, *searchEngine);
desc->Run(reply, rawRoute, phantomNodes, *searchEnginePtr);
if("" != routeParameters.jsonpParameter) {
reply.content += ")\n";
}

View File

@ -23,13 +23,15 @@ or see http://www.gnu.org/licenses/agpl.txt.
#ifndef BASICROUTINGINTERFACE_H_
#define BASICROUTINGINTERFACE_H_
#include "../Plugins/RawRouteData.h"
#include "../Util/ContainerUtils.h"
#include <boost/noncopyable.hpp>
#include <cassert>
#include <climits>
#include "../Plugins/RawRouteData.h"
#include "../Util/ContainerUtils.h"
#include <stack>
template<class QueryDataT>
class BasicRoutingInterface : boost::noncopyable{

View File

@ -22,14 +22,27 @@ or see http://www.gnu.org/licenses/agpl.txt.
#include "QueryObjectsStorage.h"
#include "../../Util/GraphLoader.h"
QueryObjectsStorage::QueryObjectsStorage(std::string hsgrPath, std::string ramIndexPath, std::string fileIndexPath, std::string nodesPath, std::string edgesPath, std::string namesPath, std::string timestampPath) {
QueryObjectsStorage::QueryObjectsStorage(
std::string hsgrPath,
std::string ramIndexPath,
std::string fileIndexPath,
std::string nodesPath,
std::string edgesPath,
std::string namesPath,
std::string timestampPath
) {
INFO("loading graph data");
std::ifstream hsgrInStream(hsgrPath.c_str(), std::ios::binary);
if(!hsgrInStream) { ERR(hsgrPath << " not found"); }
//Deserialize road network graph
std::vector< QueryGraph::_StrNode> nodeList;
std::vector< QueryGraph::_StrEdge> edgeList;
const int n = readHSGRFromStream(hsgrInStream, nodeList, edgeList, &checkSum);
const int n = readHSGRFromStream(
hsgrInStream,
nodeList,
edgeList,
&checkSum
);
INFO("Data checksum is " << checkSum);
graph = new QueryGraph(nodeList, edgeList);
@ -39,7 +52,7 @@ QueryObjectsStorage::QueryObjectsStorage(std::string hsgrPath, std::string ramIn
if(timestampPath.length()) {
INFO("Loading Timestamp");
std::ifstream timestampInStream(timestampPath.c_str());
if(!timestampInStream) { ERR(timestampPath << " not found"); }
if(!timestampInStream) { WARN(timestampPath << " not found"); }
getline(timestampInStream, timestamp);
timestampInStream.close();

View File

@ -21,6 +21,15 @@ or see http://www.gnu.org/licenses/agpl.txt.
#ifndef GRAPHLOADER_H
#define GRAPHLOADER_H
#include "../DataStructures/ImportNode.h"
#include "../DataStructures/ImportEdge.h"
#include "../DataStructures/NodeCoords.h"
#include "../DataStructures/Restriction.h"
#include "../typedefs.h"
#include <boost/assert.hpp>
#include <boost/unordered_map.hpp>
#include <cassert>
#include <cmath>
@ -30,19 +39,11 @@ or see http://www.gnu.org/licenses/agpl.txt.
#include <iomanip>
#include <vector>
#include <boost/unordered_map.hpp>
#include "../DataStructures/ImportNode.h"
#include "../DataStructures/ImportEdge.h"
#include "../DataStructures/NodeCoords.h"
#include "../DataStructures/Restriction.h"
#include "../typedefs.h"
typedef boost::unordered_map<NodeID, NodeID> ExternalNodeMap;
template<class EdgeT>
struct _ExcessRemover {
inline bool operator()( EdgeT & edge ) const {
inline bool operator()( const EdgeT & edge ) const {
return edge.source() == UINT_MAX;
}
};
@ -116,9 +117,9 @@ NodeID readBinaryOSRMGraphFromStream(std::istream &in, std::vector<EdgeT>& edgeL
in.read((char*)&isAccessRestricted, sizeof(bool));
in.read((char*)&isContraFlow, sizeof(bool));
GUARANTEE(length > 0, "loaded null length edge" );
GUARANTEE(weight > 0, "loaded null weight");
GUARANTEE(0<=dir && dir<=2, "loaded bogus direction");
BOOST_ASSERT_MSG(length > 0, "loaded null length edge" );
BOOST_ASSERT_MSG(weight > 0, "loaded null weight");
BOOST_ASSERT_MSG(0<=dir && dir<=2, "loaded bogus direction");
bool forward = true;
bool backward = true;
@ -144,7 +145,9 @@ NodeID readBinaryOSRMGraphFromStream(std::istream &in, std::vector<EdgeT>& edgeL
continue;
}
target = intNodeID->second;
GUARANTEE(source != UINT_MAX && target != UINT_MAX, "nonexisting source or target");
BOOST_ASSERT_MSG(source != UINT_MAX && target != UINT_MAX,
"nonexisting source or target"
);
if(source > target) {
std::swap(source, target);

View File

@ -18,14 +18,19 @@ Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
or see http://www.gnu.org/licenses/agpl.txt.
*/
#ifndef LUAUTIL_H_
#define LUAUTIL_H_
extern "C" {
#include <lua.h>
#include <lauxlib.h>
#include <lualib.h>
}
#include <boost/filesystem/convenience.hpp>
#include <iostream>
#include <string>
#include <boost/filesystem/convenience.hpp>
template<typename T>
void LUA_print(T number) {

View File

@ -18,25 +18,7 @@ Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
or see http://www.gnu.org/licenses/agpl.txt.
*/
extern "C" {
#include <lua.h>
#include <lauxlib.h>
#include <lualib.h>
}
#include <luabind/luabind.hpp>
#include <boost/foreach.hpp>
#include <fstream>
#include <istream>
#include <iostream>
#include <cstring>
#include <string>
#include <vector>
#include "Algorithms/IteratorBasedCRC32.h"
#include "Util/OpenMPWrapper.h"
#include "typedefs.h"
#include "Contractor/Contractor.h"
#include "Contractor/EdgeBasedGraphFactory.h"
#include "DataStructures/BinaryHeap.h"
@ -47,7 +29,20 @@ extern "C" {
#include "Util/GraphLoader.h"
#include "Util/InputFileUtil.h"
#include "Util/LuaUtil.h"
#include "Util/OpenMPWrapper.h"
#include "Util/StringUtil.h"
#include "typedefs.h"
#include <boost/foreach.hpp>
#include <luabind/luabind.hpp>
#include <fstream>
#include <istream>
#include <iostream>
#include <cstring>
#include <string>
#include <vector>
typedef QueryEdge::EdgeData EdgeData;
typedef DynamicGraph<EdgeData>::InputEdge InputEdge;
@ -61,6 +56,7 @@ std::vector<NodeID> trafficLightNodes;
std::vector<ImportEdge> edgeList;
int main (int argc, char *argv[]) {
try {
if(argc < 3) {
ERR("usage: " << std::endl << argv[0] << " <osrm-data> <osrm-restrictions> [<profile>]");
}
@ -209,7 +205,7 @@ int main (int argc, char *argv[]) {
std::sort(contractedEdgeList.begin(), contractedEdgeList.end());
unsigned numberOfNodes = 0;
unsigned numberOfEdges = contractedEdgeList.size();
INFO("Serializing compacted graph");
INFO("Serializing compacted graph of " << numberOfEdges << " edges");
std::ofstream edgeOutFile(graphOut.c_str(), std::ios::binary);
BOOST_FOREACH(const QueryEdge & edge, contractedEdgeList) {
@ -268,5 +264,8 @@ int main (int argc, char *argv[]) {
//cleanedEdgeList.clear();
_nodes.clear();
INFO("finished preprocessing");
} catch (std::exception &e) {
ERR("Exception occured: " << e.what());
}
return 0;
}

View File

@ -18,12 +18,6 @@ Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
or see http://www.gnu.org/licenses/agpl.txt.
*/
#include <cstdlib>
#include <iostream>
#include <fstream>
#include <string>
#include "typedefs.h"
#include "Extractor/ExtractorCallbacks.h"
#include "Extractor/ExtractionContainers.h"
#include "Extractor/ScriptingEnvironment.h"
@ -34,6 +28,13 @@ or see http://www.gnu.org/licenses/agpl.txt.
#include "Util/MachineInfo.h"
#include "Util/OpenMPWrapper.h"
#include "Util/StringUtil.h"
#include "typedefs.h"
#include <cstdlib>
#include <iostream>
#include <fstream>
#include <string>
typedef BaseConfiguration ExtractorConfiguration;

View File

@ -6,9 +6,9 @@ Feature: Car - Handle ferryshuttle train routes
Scenario: Car - Use a ferry route
Given the node map
| a | b | c | | |
| | | d | | |
| | | e | f | g |
| a | b | c | | | |
| | | d | | | |
| | | e | f | g | h |
And the ways
| nodes | highway | route | bicycle |
@ -16,10 +16,11 @@ Feature: Car - Handle ferryshuttle train routes
| cde | | shuttle_train | yes |
| ef | primary | | |
| fg | | ferry_man | |
| gh | primary | | no |
When I route I should get
| from | to | route |
| a | g | abc,cde,ef |
| a | f | abc,cde,ef |
| b | f | abc,cde,ef |
| e | c | cde |
| e | b | cde,abc |
@ -27,5 +28,6 @@ Feature: Car - Handle ferryshuttle train routes
| c | e | cde |
| c | f | cde,ef |
| f | g | |
| g | h | gh |

View File

@ -35,16 +35,14 @@ or see http://www.gnu.org/licenses/agpl.txt.
#include <iostream>
#define INFO(x) do {std::cout << "[info " << __FILE__ << ":" << __LINE__ << "] " << x << std::endl;} while(0);
#define ERR(x) do {std::cerr << "[error " << __FILE__ << ":" << __LINE__ << "] " << x << std::endl; std::exit(-1);} while(0);
#define WARN(x) do {std::cerr << "[warn " << __FILE__ << ":" << __LINE__ << "] " << x << std::endl;} while(0);
#define INFO(x) do {std::cout << "[i " << __FILE__ << ":" << __LINE__ << "] " << x << std::endl;} while(0);
#define ERR(x) do {std::cerr << "[! " << __FILE__ << ":" << __LINE__ << "] " << x << std::endl; std::exit(-1);} while(0);
#define WARN(x) do {std::cerr << "[? " << __FILE__ << ":" << __LINE__ << "] " << x << std::endl;} while(0);
#ifdef NDEBUG
#define DEBUG(x)
#define GUARANTEE(x,y)
#else
#define DEBUG(x) do {std::cout << "[debug " << __FILE__ << ":" << __LINE__ << "] " << x << std::endl;} while(0);
#define GUARANTEE(x,y) do { {do{ if(false == (x)) { ERR(y) } } while(0);} } while(0);
#define DEBUG(x) do {std::cout << "[d " << __FILE__ << ":" << __LINE__ << "] " << x << std::endl;} while(0);
#endif
#ifndef M_PI