diff --git a/app.pro b/app.pro new file mode 100644 index 0000000..7ee5afd --- /dev/null +++ b/app.pro @@ -0,0 +1,40 @@ +TARGET = NanoMap +TEMPLATE = app + +QT += network + +#LIBS += -Lmonav -lgpsgridclient -lcontractionhierarchiesclient +INCLUDEPATH += monav + +SOURCES += main.cpp \ + mainwidget.cpp \ + projection.cpp \ + abstractlayer.cpp \ + gpslayer.cpp \ + markerlayer.cpp \ + gpxlayer.cpp \ + poilayer.cpp \ + monavlayer.cpp \ + timelayer.cpp \ + batterylayer.cpp \ + mapwidget.cpp \ + markerlist.cpp \ + downloadwidget.cpp \ + routingwidget.cpp \ + gpsclient.cpp + +HEADERS += mainwidget.h \ + projection.h \ + abstractlayer.h \ + gpslayer.h \ + markerlayer.h \ + gpxlayer.h \ + poilayer.h \ + monavlayer.h \ + timelayer.h \ + batterylayer.h \ + mapwidget.h \ + markerlist.h \ + downloadwidget.h \ + routingwidget.h \ + gpsclient.h diff --git a/mainwidget.cpp b/mainwidget.cpp index 0cd1458..347f061 100644 --- a/mainwidget.cpp +++ b/mainwidget.cpp @@ -28,6 +28,7 @@ #include "gpslayer.h" #include "gpxlayer.h" #include "markerlayer.h" +#include "monavlayer.h" #include "poilayer.h" #include "timelayer.h" @@ -63,6 +64,9 @@ MainWidget::MainWidget(QWidget *parent) l->load(QDir::homePath()+"/Maps/marker.list"); m_map->addLayer(l, 3, "Marker"); + l = new MonavLayer(m_map); + m_map->addLayer(l, 2, "MoNav Routing"); + l = new GpsLayer(m_map); m_map->addLayer(l, 1, "GPS-Position"); diff --git a/mapwidget.cpp b/mapwidget.cpp index 4f8778f..577a357 100644 --- a/mapwidget.cpp +++ b/mapwidget.cpp @@ -40,8 +40,6 @@ MapWidget::MapWidget(QWidget *parent) : QWidget(parent), - m_routeStart(), - m_routeEnd(), m_usage(false), m_ui(true), m_zoomable(false), @@ -302,7 +300,7 @@ void MapWidget::keyPressEvent(QKeyEvent *event) case Qt::Key_R: { if (event->modifiers() == Qt::NoModifier) { - emit route(m_routeStart, m_routeEnd); + // emit route(m_routeStart, m_routeEnd); } break; } @@ -310,15 +308,6 @@ void MapWidget::keyPressEvent(QKeyEvent *event) { if (event->modifiers() == Qt::AltModifier) { m_takeScreenshot = true; - } else if (event->modifiers() == Qt::NoModifier) { - m_routeStart = geoPos(); - } - break; - } - case Qt::Key_E: - { - if (event->modifiers() == Qt::NoModifier) { - m_routeEnd = geoPos(); } break; } @@ -374,19 +363,6 @@ void MapWidget::paintEvent(QPaintEvent *event) } } - painter.save(); - QPoint p = geo2screen(m_routeStart.x(), m_routeStart.y()); - QPolygon tri; - tri << p << p+QPoint(-5, -9) << p+QPoint(5, -9) << p; - painter.setBrush(Qt::red); - painter.drawPolygon(tri); - p = geo2screen(m_routeEnd.x(), m_routeEnd.y()); - tri.clear(); - tri << p << p+QPoint(-5, -9) << p+QPoint(5, -9) << p; - painter.setBrush(Qt::blue); - painter.drawPolygon(tri); - painter.restore(); - QMapIterator i(m_layer); while (i.hasNext()) { i.next(); diff --git a/mapwidget.h b/mapwidget.h index 8f3507a..a15cea9 100644 --- a/mapwidget.h +++ b/mapwidget.h @@ -74,7 +74,6 @@ private: void downloadTile(int x, int y, int level); void changeZoomLevel(int diff); - QPointF m_routeStart, m_routeEnd; bool m_usage, m_ui, m_zoomable; bool m_takeScreenshot; int m_screenshotNumber; diff --git a/monav/contractionhierarchies/binaryheap.h b/monav/contractionhierarchies/binaryheap.h new file mode 100644 index 0000000..571d3ce --- /dev/null +++ b/monav/contractionhierarchies/binaryheap.h @@ -0,0 +1,242 @@ +/* +Copyright 2010 Christian Vetter veaac.fdirct@gmail.com + +This file is part of MoNav. + +MoNav is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +MoNav 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 General Public License +along with MoNav. If not, see . +*/ + +#ifndef BINARYHEAP_H_INCLUDED +#define BINARYHEAP_H_INCLUDED + +//Not compatible with non contiguous node ids + +#include +#include +#include + +template< typename NodeID, typename Key > +class ArrayStorage { + public: + + ArrayStorage( size_t size ) : + positions( new Key[size] ) + { + } + + ~ArrayStorage() + { + delete[] positions; + } + + Key &operator[]( NodeID node ) + { + return positions[node]; + } + + void clear() {} + + private: + Key* positions; +}; + +template< typename NodeID, typename Key > +class MapStorage { + public: + + MapStorage( size_t ) + { + } + + Key &operator[]( NodeID node ) + { + return nodes[node]; + } + + void clear() + { + nodes.clear(); + } + + private: + QHash< NodeID, Key > nodes; + +}; + +template < typename NodeID, typename Key, typename Weight, typename Data, typename IndexStorage = ArrayStorage< NodeID, Key > > +class BinaryHeap { + private: + BinaryHeap( const BinaryHeap& right ); + void operator=( const BinaryHeap& right ); + public: + typedef Weight WeightType; + typedef Data DataType; + + BinaryHeap( size_t maxID ) + : nodeIndex( maxID ) { + Clear(); + } + + void Clear() { + heap.resize( 1 ); + insertedNodes.clear(); + nodeIndex.clear(); + heap[0].weight = 0; + } + + Key Size() const { + return ( Key )( heap.size() - 1 ); + } + + void Insert( NodeID node, Weight weight, const Data &data ) { + HeapElement element; + element.index = ( NodeID ) insertedNodes.size(); + element.weight = weight; + const Key key = ( Key ) heap.size(); + heap.push_back( element ); + insertedNodes.push_back( HeapNode( node, key, weight, data ) ); + nodeIndex[node] = element.index; + Upheap( key ); + CheckHeap(); + } + + Data& GetData( NodeID node ) { + const Key index = nodeIndex[node]; + return insertedNodes[index].data; + } + + Weight& GetKey( NodeID node ) { + const Key index = nodeIndex[node]; + return insertedNodes[index].weight; + } + + bool WasRemoved( NodeID node ) { + assert( WasInserted( node ) ); + const Key index = nodeIndex[node]; + return insertedNodes[index].key == 0; + } + + bool WasInserted( NodeID node ) { + const Key index = nodeIndex[node]; + if ( index >= ( Key ) insertedNodes.size() ) + return false; + return insertedNodes[index].node == node; + } + + NodeID Min() const { + assert( heap.size() > 1 ); + return insertedNodes[heap[1].index].node; + } + + NodeID DeleteMin() { + assert( heap.size() > 1 ); + const Key removedIndex = heap[1].index; + heap[1] = heap[heap.size()-1]; + heap.pop_back(); + if ( heap.size() > 1 ) + Downheap( 1 ); + insertedNodes[removedIndex].key = 0; + CheckHeap(); + return insertedNodes[removedIndex].node; + } + + void DeleteAll() { + for ( typename std::vector< HeapElement >::iterator i = heap.begin() + 1, iend = heap.end(); i != iend; ++i ) + insertedNodes[i->index].key = 0; + heap.resize( 1 ); + heap[0].weight = 0; + } + + + void DecreaseKey( NodeID node, Weight weight ) { + const Key index = nodeIndex[node]; + Key key = insertedNodes[index].key; + assert ( key != 0 ); + + insertedNodes[index].weight = weight; + heap[key].weight = weight; + Upheap( key ); + CheckHeap(); + } + + private: + class HeapNode { + public: + HeapNode() { + } + HeapNode( NodeID n, Key k, Weight w, Data d ) + : node( n ), key( k ), weight( w ), data( d ) { + } + + NodeID node; + Key key; + Weight weight; + Data data; + }; + struct HeapElement { + Key index; + Weight weight; + }; + + std::vector< HeapNode > insertedNodes; + std::vector< HeapElement > heap; + IndexStorage nodeIndex; + + void Downheap( Key key ) { + const Key droppingIndex = heap[key].index; + const Weight weight = heap[key].weight; + Key nextKey = key << 1; + while ( nextKey < ( Key ) heap.size() ) { + const Key nextKeyOther = nextKey + 1; + if ( ( nextKeyOther < ( Key ) heap.size() ) ) + if ( heap[nextKey].weight > heap[nextKeyOther].weight ) + nextKey = nextKeyOther; + + if ( weight <= heap[nextKey].weight ) + break; + + heap[key] = heap[nextKey]; + insertedNodes[heap[key].index].key = key; + key = nextKey; + nextKey <<= 1; + } + heap[key].index = droppingIndex; + heap[key].weight = weight; + insertedNodes[droppingIndex].key = key; + } + + void Upheap( Key key ) { + const Key risingIndex = heap[key].index; + const Weight weight = heap[key].weight; + Key nextKey = key >> 1; + while ( heap[nextKey].weight > weight ) { + assert( nextKey != 0 ); + heap[key] = heap[nextKey]; + insertedNodes[heap[key].index].key = key; + key = nextKey; + nextKey >>= 1; + } + heap[key].index = risingIndex; + heap[key].weight = weight; + insertedNodes[risingIndex].key = key; + } + + void CheckHeap() { + /*for ( Key i = 2; i < heap.size(); ++i ) { + assert( heap[i].weight >= heap[i >> 1].weight ); + }*/ + } +}; + +#endif //#ifndef BINARYHEAP_H_INCLUDED diff --git a/monav/contractionhierarchies/blockcache.h b/monav/contractionhierarchies/blockcache.h new file mode 100644 index 0000000..2b94fc9 --- /dev/null +++ b/monav/contractionhierarchies/blockcache.h @@ -0,0 +1,162 @@ +/* +Copyright 2010 Christian Vetter veaac.fdirct@gmail.com + +This file is part of MoNav. + +MoNav is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +MoNav 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 General Public License +along with MoNav. If not, see . +*/ + +#ifndef BLOCKCACHE_H_INCLUDED +#define BLOCKCACHE_H_INCLUDED + +#include +#include +#include +#include + +// Block must have member function / variables: +// variable id => block id +// function void load( const unsigned char* buffer ) +template< class Block > +class BlockCache{ + +public: + + BlockCache() + { + m_cache = NULL; + m_LRU = NULL; + m_blocks = NULL; + } + + bool load( const QString& filename, int cacheBlocks, unsigned blockSize ) + { + m_cacheBlocks = cacheBlocks; + m_blockSize = blockSize; + m_inputFile.setFileName( filename ); + if ( !m_inputFile.open( QIODevice::ReadOnly | QIODevice::Unbuffered ) ) { + qCritical() << "failed to open file:" << m_inputFile.fileName(); + return false; + } + + m_cache = new unsigned char[( m_cacheBlocks + 1 ) * m_blockSize]; + m_LRU = new LRUEntry[m_cacheBlocks]; + m_blocks = new Block[m_cacheBlocks]; + + m_firstLoaded = -1; + m_lastLoaded = -1; + m_loadedCount = 0; + + return true; + } + + void unload ( ) + { + m_inputFile.close(); + if ( m_cache != NULL ) + delete[] m_cache; + if ( m_LRU != NULL ) + delete[] m_LRU; + if ( m_blocks != NULL ) + delete[] m_blocks; + m_cache = NULL; + m_LRU = NULL; + m_blocks = NULL; + m_index.clear(); + } + + const Block* getBlock( unsigned block ) + { + int cacheID = m_index.value( block, -1 ); + if ( cacheID == -1 ) + return loadBlock( block ); + + useBlock( cacheID ); + return m_blocks + cacheID; + } + +private: + + const Block* loadBlock( unsigned block ) + { + int freeBlock = m_loadedCount; + // cache is full => select least recently used block + if ( m_loadedCount == m_cacheBlocks ) { + assert ( m_lastLoaded != -1 ); + freeBlock = m_lastLoaded; + m_index.remove( m_blocks[freeBlock].id ); + useBlock( freeBlock ); + } else { + //insert into the front of the list + m_LRU[freeBlock].previousLoaded = -1; + m_LRU[freeBlock].nextLoaded = m_firstLoaded; + if ( m_firstLoaded != -1 ) + m_LRU[m_firstLoaded].previousLoaded = freeBlock; + if ( m_lastLoaded == -1 ) + m_lastLoaded = freeBlock; + m_firstLoaded = freeBlock; + m_loadedCount++; + } + + //load block + m_inputFile.seek( ( long long ) block * m_blockSize ); + m_inputFile.read( ( char* ) m_cache + freeBlock * m_blockSize, m_blockSize ); + m_blocks[freeBlock].load( block, m_cache + freeBlock * m_blockSize ); + m_index[block] = freeBlock; + + return m_blocks + freeBlock; + } + + void useBlock( int cacheID ) + { + assert( m_firstLoaded != -1 ); + if ( m_firstLoaded == cacheID ) + return; + + LRUEntry& block = m_LRU[cacheID]; + + //remove block from the list to put it into the front + if ( block.nextLoaded != -1 ) + m_LRU[block.nextLoaded].previousLoaded = block.previousLoaded; + else + m_lastLoaded = block.previousLoaded; + + m_LRU[block.previousLoaded].nextLoaded = block.nextLoaded; + + // insert block into the front + m_LRU[m_firstLoaded].previousLoaded = cacheID; + block.nextLoaded = m_firstLoaded; + block.previousLoaded = -1; + m_firstLoaded = cacheID; + } + + struct LRUEntry{ + int nextLoaded; + int previousLoaded; + }; + + Block* m_blocks; + LRUEntry* m_LRU; + unsigned char* m_cache; + int m_firstLoaded; + int m_lastLoaded; + int m_loadedCount; + int m_cacheBlocks; + unsigned m_blockSize; + QFile m_inputFile; + QHash< unsigned, int > m_index; + +}; + +#endif // BLOCKCACHE_H_INCLUDED diff --git a/monav/contractionhierarchies/compressedgraph.h b/monav/contractionhierarchies/compressedgraph.h new file mode 100644 index 0000000..404acf9 --- /dev/null +++ b/monav/contractionhierarchies/compressedgraph.h @@ -0,0 +1,600 @@ +/* +Copyright 2010 Christian Vetter veaac.fdirct@gmail.com + +This file is part of MoNav. + +MoNav is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +MoNav 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 General Public License +along with MoNav. If not, see . +*/ + +#ifndef COMPRESSEDGRAPH_H +#define COMPRESSEDGRAPH_H + +#include "interfaces/irouter.h" +#include "utils/coordinates.h" +#include "utils/bithelpers.h" +#include "blockcache.h" +#include +#include +#include +#include + +class CompressedGraph { + +public : + + typedef unsigned NodeIterator; + +protected: + + //TYPES + + struct Block { + struct Settings { + // adress blocks from the adjacent blocks array + unsigned char blockBits; + // address an entry in the adjacent blocks array + //unsigned char adjacentBlockBits; ==> can be computed from adjacentBlockcount bitsNeeded( count - 1 ) + // address an internal node with a shortcut's middle + //unsigned char internalBits; ==> can be computed from nodeCount bitsNeeded( count - 1 ); + // address an external node in another block + unsigned char externalBits; + // address the first edge of a node + unsigned char firstEdgeBits; + // bits used for the short weight class + unsigned char shortWeightBits; + // bits uses for the long weight class + unsigned char longWeightBits; + // bits used for the difference ( x - minX ) + unsigned char xBits; + // bits used for the difference ( y - minY ) + unsigned char yBits; + // minimal x value + unsigned minX; + // minimal y value + unsigned minY; + // #nodes => used for the size of firstEdges + unsigned nodeCount; + // #adjacent blocks => used for the size of adjacentBlocks + unsigned adjacentBlockCount; + } settings; + + unsigned char adjacentBlockBits; + unsigned char internalBits; + + unsigned edges; + unsigned adjacentBlocks; + unsigned firstEdges; + unsigned nodeCoordinates; + + unsigned id; + const unsigned char* buffer; + + void load( unsigned id, const unsigned char* buffer ) + { + CompressedGraph::loadBlock( this, id, buffer ); + } + }; + + struct PathBlock { + + struct DataItem { + unsigned a; + unsigned b; + + DataItem() + { + a = b = 0; + } + + DataItem( const IRouter::Node& node ) + { + assert( bits_needed( node.coordinate.x ) < 32 ); + a = node.coordinate.x << 1; + a |= 1; + b = node.coordinate.y; + } + + DataItem( const IRouter::Edge& description ) + { + a = description.name; + a <<= 1; + a |= description.branchingPossible ? 1 : 0; + a <<= 1; + b = description.type; + b <<= 16; + b |= description.length; + b <<= 8; + b |= encode_integer< 4, 4 >( description.seconds ); + } + + bool isNode() const + { + return ( a & 1 ) == 1; + } + + bool isEdge() const + { + return ( a & 1 ) == 0; + } + + IRouter::Node toNode() + { + IRouter::Node node; + node.coordinate = UnsignedCoordinate( a >> 1, b ); + return node; + } + + IRouter::Edge toEdge() + { + IRouter::Edge edge; + edge.name = a >> 2; + edge.branchingPossible = ( a & 2 ) == 2; + edge.type = b >> 24; + edge.length = ( b >> 8 ) & ( ( 1u << 16 ) -1 ); + edge.seconds = decode_integer< 4, 4 >( b & 255 ); + return edge; + } + }; + + unsigned id; + const unsigned char* buffer; + + void load( unsigned id, const unsigned char* buffer ) + { + CompressedGraph::loadPathBlock( this, id, buffer ); + } + }; + +public: + + // TYPES + + struct Edge { + NodeIterator source; + NodeIterator target; + struct Data { + unsigned distance; + bool shortcut : 1; + bool forward : 1; + bool backward : 1; + bool unpacked : 1; + bool reversed : 1; + union { + NodeIterator middle; + unsigned id; + }; + unsigned path; + } data; + + bool operator<( const Edge& right ) const { + if ( source != right.source ) + return source < right.source; + int l = ( data.forward ? -1 : 0 ) + ( data.backward ? 1 : 0 ); + int r = ( right.data.forward ? -1 : 0 ) + ( right.data.backward ? 1 : 0 ); + if ( l != r ) + return l < r; + if ( target != right.target ) + return target < right.target; + return data.distance < right.data.distance; + } + + }; + + class EdgeIterator { + + friend class CompressedGraph; + + public: + + EdgeIterator() + { + } + + bool hasEdgesLeft() + { + return m_position < m_end; + } + + NodeIterator target() const { return m_target; } + bool forward() const { return m_data.forward; } + bool backward() const { return m_data.backward; } + bool shortcut() const { return m_data.shortcut; } + bool unpacked() const { return m_data.unpacked; } + NodeIterator middle() const { return m_data.middle; } + unsigned distance() const { return m_data.distance; } + IRouter::Edge description() const { return IRouter::Edge( m_data.description.nameID, m_data.description.branchingPossible, m_data.description.type, 1, ( m_data.distance + 5 ) / 10 ); } +#ifdef NDEBUG + private: +#endif + + EdgeIterator( unsigned source, const Block& block, unsigned position, unsigned end ) : + m_block( &block ), m_source( source ), m_position( position ), m_end( end ) + { + } + + const Block* m_block; + NodeIterator m_target; + NodeIterator m_source; + unsigned m_position; + unsigned m_end; + + struct EdgeData { + unsigned distance; + bool shortcut : 1; + bool forward : 1; + bool backward : 1; + bool unpacked : 1; + bool reversed : 1; + union { + NodeIterator middle; + struct { + unsigned nameID : 30; + bool branchingPossible : 1; + unsigned type; + } description; + }; + unsigned path; + } m_data; + }; + + // FUNCTIONS + + CompressedGraph() + { + m_loaded = false; + } + + ~CompressedGraph() + { + if ( m_loaded ) + unloadGraph(); + } + + bool loadGraph( QString filename, unsigned cacheSize ) + { + if ( m_loaded ) + unloadGraph(); + QFile settingsFile( filename + "_config" ); + if ( !settingsFile.open( QIODevice::ReadOnly ) ) { + qCritical() << "failed to open file:" << settingsFile.fileName(); + return false; + } + m_settings.read( settingsFile ); + if ( !m_blockCache.load( filename + "_edges", cacheSize / m_settings.blockSize / 2 + 1, m_settings.blockSize ) ) + return false; + if ( !m_pathCache.load( filename + "_paths", cacheSize / m_settings.blockSize / 2 + 1, m_settings.blockSize ) ) + return false; + m_loaded = true; + return true; + } + + EdgeIterator edges( NodeIterator node ) + { + unsigned blockID = nodeToBlock( node ); + unsigned internal = nodeToInternal( node ); + const Block* block = getBlock( blockID ); + return unpackFirstEdges( *block, internal ); + } + + EdgeIterator findEdge( NodeIterator source, NodeIterator target, unsigned id ) + { + if ( source < target ) + std::swap( source, target ); + EdgeIterator e = edges( source ); + while ( e.hasEdgesLeft() ) { + unpackNextEdge( &e ); + if ( e.target() != target ) + continue; + if ( e.shortcut() ) + continue; + if ( id != 0 ) { + id--; + continue; + } + + return e; + } + assert( false ); + return e; + } + + void unpackNextEdge( EdgeIterator* edge ) + { + const Block& block = *edge->m_block; + EdgeIterator::EdgeData& edgeData = edge->m_data; + const unsigned char* buffer = block.buffer + ( edge->m_position >> 3 ); + int offset = edge->m_position & 7; + + // forward + backward flag + bool forwardAndBackward = read_unaligned_unsigned( &buffer, 1, &offset ) != 0; + if ( forwardAndBackward ) { + edgeData.forward = true; + edgeData.backward = true; + } else { + edgeData.forward = read_unaligned_unsigned( &buffer, 1, &offset ) != 0; + edgeData.backward = !edgeData.forward; + } + + // target + bool internalTarget = read_unaligned_unsigned( &buffer, 1, &offset ) != 0; + if ( internalTarget ) { + unsigned target = read_unaligned_unsigned( &buffer, bits_needed( edge->m_source ), &offset ); + edge->m_target = nodeFromDescriptor( block.id, target ); + } else { + unsigned adjacentBlock = read_unaligned_unsigned( &buffer, block.adjacentBlockBits, &offset ); + unsigned target = read_unaligned_unsigned( &buffer, block.settings.externalBits, &offset ); + unsigned adjacentBlockPosition = block.adjacentBlocks + adjacentBlock * block.settings.blockBits; + unsigned targetBlock = read_unaligned_unsigned( block.buffer + ( adjacentBlockPosition >> 3 ), block.settings.blockBits, adjacentBlockPosition & 7 ); + edge->m_target = nodeFromDescriptor( targetBlock, target ); + } + + // weight + bool longWeight = block.settings.shortWeightBits == block.settings.longWeightBits; + if ( !longWeight ) + longWeight = read_unaligned_unsigned( &buffer, 1, &offset ) != 0; + edgeData.distance = read_unaligned_unsigned( &buffer, longWeight ? block.settings.longWeightBits : block.settings.shortWeightBits, &offset ); + + // unpacked + edgeData.unpacked = read_unaligned_unsigned( &buffer, 1, &offset ) != 0; + if ( edgeData.unpacked ) { + if ( forwardAndBackward ) + edgeData.reversed = read_unaligned_unsigned( &buffer, 1, &offset ) != 0; + else + edgeData.reversed = edgeData.backward; + edgeData.path = read_unaligned_unsigned( &buffer, m_settings.pathBits, &offset ); + } + + // shortcut + edgeData.shortcut = read_unaligned_unsigned( &buffer, 1, &offset ) != 0; + if ( edgeData.shortcut ) { + if ( !edgeData.unpacked ) { + unsigned middle = read_unaligned_unsigned( &buffer, block.internalBits, &offset ); + edgeData.middle = nodeFromDescriptor( block.id, middle ); + } + } + + // edge description + if ( !edgeData.shortcut && !edgeData.unpacked ) { + edgeData.description.type = read_unaligned_unsigned( &buffer, m_settings.typeBits, &offset ); + edgeData.description.nameID = read_unaligned_unsigned( &buffer, m_settings.nameBits, &offset ); + edgeData.description.branchingPossible = read_unaligned_unsigned( &buffer, 1, &offset ); + } + + edge->m_position = ( buffer - block.buffer ) * 8 + offset; + } + + IRouter::Node node( NodeIterator node ) + { + unsigned blockID = nodeToBlock( node ); + unsigned internal = nodeToInternal( node ); + const Block* block = getBlock( blockID ); + IRouter::Node result; + unpackCoordinates( *block, internal, &result.coordinate ); + return result; + } + + unsigned numberOfNodes() const + { + return m_settings.numberOfNodes; + } + + unsigned numberOfEdges() const + { + return m_settings.numberOfEdges; + } + + template< class T, class S > + void path( const EdgeIterator& edge, T path, S edges, bool forward ) + { + assert( edge.unpacked() ); + unsigned pathBegin = path->size(); + unsigned edgesBegin = edges->size(); + int increase = edge.m_data.reversed ? -1 : 1; + + IRouter::Node targetNode = node( edge.target() ); + unsigned pathID = edge.m_data.path; + + if ( !forward ) { + PathBlock::DataItem data = unpackPath( pathID ); + assert( data.isNode() ); + path->push_back( data.toNode().coordinate ); + } + + pathID += increase; + + while( true ) { + PathBlock::DataItem data = unpackPath( pathID ); + if ( data.isEdge() ) { + edges->push_back( data.toEdge() ); + pathID += increase; + continue; + } + assert( data.isNode() ); + IRouter::Node node = data.toNode(); + if ( node.coordinate.x == targetNode.coordinate.x && node.coordinate.y == targetNode.coordinate.y ) + break; + path->push_back( node.coordinate ); + pathID += increase; + } + + if ( forward ) { + path->push_back( targetNode.coordinate ); + } else { + std::reverse( path->begin() + pathBegin, path->end() ); + std::reverse( edges->begin() + edgesBegin, edges->end() ); + } + + assert( edges->size() != ( int ) edgesBegin ); // at least one edge description has to be present + } + +protected: + + // TYPES + + struct GlobalSettings { + unsigned blockSize; + unsigned char internalBits; + unsigned char pathBits; + unsigned char typeBits; + unsigned char nameBits; + unsigned numberOfNodes; + unsigned numberOfEdges; + + void read( QFile& in ) + { + in.read( ( char* ) &blockSize, sizeof( blockSize ) ); + in.read( ( char* ) &internalBits, sizeof( internalBits ) ); + in.read( ( char* ) &pathBits, sizeof( pathBits ) ); + in.read( ( char* ) &typeBits, sizeof( typeBits ) ); + in.read( ( char* ) &nameBits, sizeof( nameBits ) ); + in.read( ( char* ) &numberOfNodes, sizeof( numberOfNodes ) ); + in.read( ( char* ) &numberOfEdges, sizeof( numberOfEdges ) ); + } + + void write( QFile& out ) + { + out.write( ( const char* ) &blockSize, sizeof( blockSize ) ); + out.write( ( const char* ) &internalBits, sizeof( internalBits ) ); + out.write( ( const char* ) &pathBits, sizeof( pathBits ) ); + out.write( ( const char* ) &typeBits, sizeof( typeBits ) ); + out.write( ( const char* ) &nameBits, sizeof( nameBits ) ); + out.write( ( const char* ) &numberOfNodes, sizeof( numberOfNodes ) ); + out.write( ( const char* ) &numberOfEdges, sizeof( numberOfEdges ) ); + } + }; + + struct nodeDescriptor { + unsigned block; + unsigned node; + }; + + // FUNCTIONS + + PathBlock::DataItem unpackPath( unsigned position ) { + unsigned blockID = position / ( m_settings.blockSize / 8 ); + unsigned internal = ( position % ( m_settings.blockSize / 8 ) ) * 8; + const PathBlock* block = getPathBlock( blockID ); + PathBlock::DataItem data; + data.a = *( ( unsigned* ) ( block->buffer + internal ) ); + data.b = *( ( unsigned* ) ( block->buffer + internal + 4 ) ); + return data; + } + + void unpackCoordinates( const Block& block, unsigned node, UnsignedCoordinate* result ) + { + unsigned position = block.nodeCoordinates + ( block.settings.xBits + block.settings.yBits ) * node; + const unsigned char* buffer = block.buffer + ( position >> 3 ); + int offset = position & 7; + result->x = read_unaligned_unsigned( &buffer, block.settings.xBits, &offset ) + block.settings.minX; + result->y = read_unaligned_unsigned( buffer, block.settings.yBits, offset ) + block.settings.minY; + } + + EdgeIterator unpackFirstEdges( const Block& block, unsigned node ) + { + unsigned position = block.firstEdges + block.settings.firstEdgeBits * node; + const unsigned char* buffer = block.buffer + ( position >> 3 ); + int offset = position & 7; + unsigned begin = read_unaligned_unsigned( &buffer, block.settings.firstEdgeBits, &offset ); + unsigned end = read_unaligned_unsigned( buffer, block.settings.firstEdgeBits, offset ); + return EdgeIterator( node, block, begin + block.edges, end + block.edges ); + } + + const Block* getBlock( unsigned block ) + { + return m_blockCache.getBlock( block ); + } + + const PathBlock* getPathBlock( unsigned block ) + { + return m_pathCache.getBlock( block ); + } + + unsigned nodeToBlock( NodeIterator node ) + { + return node >> m_settings.internalBits; + } + + unsigned nodeToInternal( NodeIterator node ) + { + return read_bits( node, m_settings.internalBits ); + } + + NodeIterator nodeFromDescriptor( nodeDescriptor node ) + { + NodeIterator result = ( node.block << m_settings.internalBits ) | node.node; + assert( nodeToBlock( result ) == node.block ); + assert( nodeToInternal( result ) == node.node ); + return result; + } + + NodeIterator nodeFromDescriptor( unsigned block, unsigned node ) + { + NodeIterator result = ( block << m_settings.internalBits ) | node; + assert( nodeToBlock( result ) == block ); + assert( nodeToInternal( result ) == node ); + return result; + } + + static void loadBlock( Block* block, unsigned blockID, const unsigned char* blockBuffer ) + { + const unsigned char* buffer = blockBuffer; + int offset = 0; + + // read settings + block->settings.blockBits = read_unaligned_unsigned( &buffer, 8, &offset ); + block->settings.externalBits = read_unaligned_unsigned( &buffer, 8, &offset ); + block->settings.firstEdgeBits = read_unaligned_unsigned( &buffer, 8, &offset ); + block->settings.shortWeightBits = read_unaligned_unsigned( &buffer, 8, &offset ); + block->settings.longWeightBits = read_unaligned_unsigned( &buffer, 8, &offset ); + block->settings.xBits = read_unaligned_unsigned( &buffer, 8, &offset ); + block->settings.yBits = read_unaligned_unsigned( &buffer, 8, &offset ); + block->settings.minX = read_unaligned_unsigned( &buffer, 32, &offset ); + block->settings.minY = read_unaligned_unsigned( &buffer, 32, &offset ); + block->settings.nodeCount = read_unaligned_unsigned( &buffer, 32, &offset ); + block->settings.adjacentBlockCount = read_unaligned_unsigned( &buffer, 32, &offset ); + + // set other values + block->internalBits = bits_needed( block->settings.nodeCount - 1 ); + block->adjacentBlockBits = bits_needed( block->settings.adjacentBlockCount - 1 ); + block->id = blockID; + block->buffer = blockBuffer; + + // compute offsets + block->nodeCoordinates = ( buffer - blockBuffer ) * 8 + offset; + block->adjacentBlocks = block->nodeCoordinates + ( block->settings.xBits + block->settings.yBits ) * block->settings.nodeCount; + block->firstEdges = block->adjacentBlocks + block->settings.blockBits * block->settings.adjacentBlockCount; + block->edges = block->firstEdges + block->settings.firstEdgeBits * ( block->settings.nodeCount + 1 ); + } + + static void loadPathBlock( PathBlock* block, unsigned blockID, const unsigned char* blockBuffer ) + { + block->id = blockID; + block->buffer = blockBuffer; + } + + void unloadGraph() + { + m_blockCache.unload(); + m_pathCache.unload(); + } + + // VARIABLES + + GlobalSettings m_settings; + BlockCache< Block > m_blockCache; + BlockCache< PathBlock > m_pathCache; + bool m_loaded; +}; + +#endif // COMPRESSEDGRAPH_H diff --git a/monav/contractionhierarchies/contractionhierarchiesclient.cpp b/monav/contractionhierarchies/contractionhierarchiesclient.cpp new file mode 100644 index 0000000..1ea16da --- /dev/null +++ b/monav/contractionhierarchies/contractionhierarchiesclient.cpp @@ -0,0 +1,407 @@ +/* +Copyright 2010 Christian Vetter veaac.fdirct@gmail.com + +This file is part of MoNav. + +MoNav is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +MoNav 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 General Public License +along with MoNav. If not, see . +*/ + +#include "contractionhierarchiesclient.h" +#include "utils/qthelpers.h" +#include +#include +#ifndef NOGUI + #include +#endif + +ContractionHierarchiesClient::ContractionHierarchiesClient() +{ + m_heapForward = NULL; + m_heapBackward = NULL; +} + +ContractionHierarchiesClient::~ContractionHierarchiesClient() +{ + unload(); +} + + +QString ContractionHierarchiesClient::GetName() +{ + return "Contraction Hierarchies"; +} + +void ContractionHierarchiesClient::SetInputDirectory( const QString& dir ) +{ + m_directory = dir; +} + +void ContractionHierarchiesClient::ShowSettings() +{ +#ifndef NOGUI + QMessageBox::information( NULL, "Settings", "No settings available" ); +#endif +} + +void ContractionHierarchiesClient::unload() +{ + if ( m_heapForward != NULL ) + delete m_heapForward; + m_heapForward = NULL; + if ( m_heapBackward != NULL ) + delete m_heapBackward; + m_heapBackward = NULL; + m_types.clear(); +} + +bool ContractionHierarchiesClient::LoadData() +{ + QString filename = fileInDirectory( m_directory,"Contraction Hierarchies" ); + unload(); + + if ( !m_graph.loadGraph( filename, 1024 * 1024 * 4 ) ) + return false; + + m_namesFile.setFileName( filename + "_names" ); + if ( !openQFile( &m_namesFile, QIODevice::ReadOnly ) ) + return false; + m_names = ( const char* ) m_namesFile.map( 0, m_namesFile.size() ); + if ( m_names == NULL ) + return false; + m_namesFile.close(); + + m_heapForward = new Heap( m_graph.numberOfNodes() ); + m_heapBackward = new Heap( m_graph.numberOfNodes() ); + + QFile typeFile( filename + "_types" ); + if ( !openQFile( &typeFile, QIODevice::ReadOnly ) ) + return false; + + QByteArray buffer = typeFile.readAll(); + QString types = QString::fromUtf8( buffer.constData() ); + m_types = types.split( ';' ); + + return true; +} + +bool ContractionHierarchiesClient::GetRoute( double* distance, QVector< Node>* pathNodes, QVector< Edge >* pathEdges, const IGPSLookup::Result& source, const IGPSLookup::Result& target ) +{ + m_heapForward->Clear(); + m_heapBackward->Clear(); + + *distance = computeRoute( source, target, pathNodes, pathEdges ); + if ( *distance == std::numeric_limits< int >::max() ) + return false; + + // is it shorter to drive along the edge? + if ( target.source == source.source && target.target == source.target && source.edgeID == target.edgeID ) { + EdgeIterator targetEdge = m_graph.findEdge( target.source, target.target, target.edgeID ); + double onEdgeDistance = fabs( target.percentage - source.percentage ) * targetEdge.distance(); + if ( onEdgeDistance < *distance ) { + if ( ( targetEdge.forward() && targetEdge.backward() ) || source.percentage < target.percentage ) { + pathNodes->clear(); + pathEdges->clear(); + pathNodes->push_back( source.nearestPoint ); + + QVector< Node > tempNodes; + if ( targetEdge.unpacked() ) + m_graph.path( targetEdge, &tempNodes, pathEdges, target.target == targetEdge.target() ); + else + pathEdges->push_back( targetEdge.description() ); + + if ( target.previousWayCoordinates < source.previousWayCoordinates ) { + for ( unsigned pathID = target.previousWayCoordinates; pathID < source.previousWayCoordinates; pathID++ ) + pathNodes->push_back( tempNodes[pathID - 1] ); + std::reverse( pathNodes->begin() + 1, pathNodes->end() ); + } else { + for ( unsigned pathID = source.previousWayCoordinates; pathID < target.previousWayCoordinates; pathID++ ) + pathNodes->push_back( tempNodes[pathID - 1] ); + } + + pathNodes->push_back( target.nearestPoint ); + pathEdges->front().length = pathNodes->size() - 1; + *distance = onEdgeDistance; + } + } + } + + *distance /= 10; + return true; +} + +bool ContractionHierarchiesClient::GetName( QString* result, unsigned name ) +{ + *result = QString::fromUtf8( m_names + name ); + return true; +} + +bool ContractionHierarchiesClient::GetNames( QVector< QString >* result, QVector< unsigned > names ) +{ + result->resize( names.size() ); + for ( int i = 0; i < names.size(); i++ ) + ( *result )[i] = QString::fromUtf8( m_names + names[i] ); + return true; +} + +bool ContractionHierarchiesClient::GetType( QString* result, unsigned type ) +{ + *result = m_types[type]; + return true; +} + +bool ContractionHierarchiesClient::GetTypes( QVector< QString >* result, QVector< unsigned > types ) +{ + result->resize( types.size() ); + for ( int i = 0; i < types.size(); i++ ) + ( *result )[i] = m_types[types[i]]; + return true; +} + +template< class EdgeAllowed, class StallEdgeAllowed > +void ContractionHierarchiesClient::computeStep( Heap* heapForward, Heap* heapBackward, const EdgeAllowed& edgeAllowed, const StallEdgeAllowed& stallEdgeAllowed, NodeIterator* middle, int* targetDistance ) { + + const NodeIterator node = heapForward->DeleteMin(); + const int distance = heapForward->GetKey( node ); + + if ( heapForward->GetData( node ).stalled ) + return; + + if ( heapBackward->WasInserted( node ) && !heapBackward->GetData( node ).stalled ) { + const int newDistance = heapBackward->GetKey( node ) + distance; + if ( newDistance < *targetDistance ) { + *middle = node; + *targetDistance = newDistance; + } + } + + if ( distance > *targetDistance ) { + heapForward->DeleteAll(); + return; + } + for ( EdgeIterator edge = m_graph.edges( node ); edge.hasEdgesLeft(); ) { + m_graph.unpackNextEdge( &edge ); + const NodeIterator to = edge.target(); + const int edgeWeight = edge.distance(); + assert( edgeWeight > 0 ); + const int toDistance = distance + edgeWeight; + + if ( stallEdgeAllowed( edge.forward(), edge.backward() ) && heapForward->WasInserted( to ) ) { + const int shorterDistance = heapForward->GetKey( to ) + edgeWeight; + if ( shorterDistance < distance ) { + //perform a bfs starting at node + //only insert nodes when a sub-optimal path can be proven + //insert node into the stall queue + heapForward->GetKey( node ) = shorterDistance; + heapForward->GetData( node ).stalled = true; + m_stallQueue.push( node ); + + while ( !m_stallQueue.empty() ) { + //get node from the queue + const NodeIterator stallNode = m_stallQueue.front(); + m_stallQueue.pop(); + const int stallDistance = heapForward->GetKey( stallNode ); + + //iterate over outgoing edges + for ( EdgeIterator stallEdge = m_graph.edges( stallNode ); stallEdge.hasEdgesLeft(); ) { + m_graph.unpackNextEdge( &stallEdge ); + //is edge outgoing/reached/stalled? + if ( !edgeAllowed( stallEdge.forward(), stallEdge.backward() ) ) + continue; + const NodeIterator stallTo = stallEdge.target(); + if ( !heapForward->WasInserted( stallTo ) ) + continue; + if ( heapForward->GetData( stallTo ).stalled == true ) + continue; + + const int stallToDistance = stallDistance + stallEdge.distance(); + //sub-optimal path found -> insert stallTo + if ( stallToDistance < heapForward->GetKey( stallTo ) ) { + if ( heapForward->WasRemoved( stallTo ) ) + heapForward->GetKey( stallTo ) = stallToDistance; + else + heapForward->DecreaseKey( stallTo, stallToDistance ); + + m_stallQueue.push( stallTo ); + heapForward->GetData( stallTo ).stalled = true; + } + } + } + break; + } + } + + if ( edgeAllowed( edge.forward(), edge.backward() ) ) { + //New Node discovered -> Add to Heap + Node Info Storage + if ( !heapForward->WasInserted( to ) ) + heapForward->Insert( to, toDistance, node ); + + //Found a shorter Path -> Update distance + else if ( toDistance <= heapForward->GetKey( to ) ) { + heapForward->DecreaseKey( to, toDistance ); + //new parent + unstall + heapForward->GetData( to ).parent = node; + heapForward->GetData( to ).stalled = false; + } + } + } +} + +int ContractionHierarchiesClient::computeRoute( const IGPSLookup::Result& source, const IGPSLookup::Result& target, QVector< Node>* pathNodes, QVector< Edge >* pathEdges ) { + EdgeIterator sourceEdge = m_graph.findEdge( source.source, source.target, source.edgeID ); + unsigned sourceWeight = sourceEdge.distance(); + EdgeIterator targetEdge = m_graph.findEdge( target.source, target.target, target.edgeID ); + unsigned targetWeight = targetEdge.distance(); + + //insert source into heap + m_heapForward->Insert( source.target, sourceWeight - sourceWeight * source.percentage, source.target ); + if ( sourceEdge.backward() && sourceEdge.forward() && source.target != source.source ) + m_heapForward->Insert( source.source, sourceWeight * source.percentage, source.source ); + + //insert target into heap + m_heapBackward->Insert( target.source, targetWeight * target.percentage, target.source ); + if ( targetEdge.backward() && targetEdge.forward() && target.target != target.source ) + m_heapBackward->Insert( target.target, targetWeight - targetWeight * target.percentage, target.target ); + + int targetDistance = std::numeric_limits< int >::max(); + NodeIterator middle = ( NodeIterator ) 0; + AllowForwardEdge forward; + AllowBackwardEdge backward; + + while ( m_heapForward->Size() + m_heapBackward->Size() > 0 ) { + + if ( m_heapForward->Size() > 0 ) + computeStep( m_heapForward, m_heapBackward, forward, backward, &middle, &targetDistance ); + + if ( m_heapBackward->Size() > 0 ) + computeStep( m_heapBackward, m_heapForward, backward, forward, &middle, &targetDistance ); + + } + + if ( targetDistance == std::numeric_limits< int >::max() ) + return std::numeric_limits< int >::max(); + + std::stack< NodeIterator > stack; + NodeIterator pathNode = middle; + while ( true ) { + NodeIterator parent = m_heapForward->GetData( pathNode ).parent; + stack.push( pathNode ); + if ( parent == pathNode ) + break; + pathNode = parent; + } + + pathNodes->push_back( source.nearestPoint ); + bool reverseSourceDescription = pathNode != source.target; + if ( source.source == source.target && sourceEdge.backward() && sourceEdge.forward() && source.percentage < 0.5 ) + reverseSourceDescription = !reverseSourceDescription; + if ( sourceEdge.unpacked() ) { + bool unpackSourceForward = source.target != sourceEdge.target() ? reverseSourceDescription : !reverseSourceDescription; + m_graph.path( sourceEdge, pathNodes, pathEdges, unpackSourceForward ); + if ( reverseSourceDescription ) { + pathNodes->remove( 1, pathNodes->size() - 1 - source.previousWayCoordinates ); + } else { + pathNodes->remove( 1, source.previousWayCoordinates - 1 ); + } + } else { + pathNodes->push_back( m_graph.node( pathNode ) ); + pathEdges->push_back( sourceEdge.description() ); + } + pathEdges->front().length = pathNodes->size() - 1; + + while ( stack.size() > 1 ) { + const NodeIterator node = stack.top(); + stack.pop(); + unpackEdge( node, stack.top(), true, pathNodes, pathEdges ); + } + + pathNode = middle; + while ( true ) { + NodeIterator parent = m_heapBackward->GetData( pathNode ).parent; + if ( parent == pathNode ) + break; + unpackEdge( parent, pathNode, false, pathNodes, pathEdges ); + pathNode = parent; + } + + int begin = pathNodes->size(); + bool reverseTargetDescription = pathNode != target.source; + if ( target.source == target.target && targetEdge.backward() && targetEdge.forward() && target.percentage > 0.5 ) + reverseSourceDescription = !reverseSourceDescription; + if ( targetEdge.unpacked() ) { + bool unpackTargetForward = target.target != targetEdge.target() ? reverseTargetDescription : !reverseTargetDescription; + m_graph.path( targetEdge, pathNodes, pathEdges, unpackTargetForward ); + if ( reverseTargetDescription ) { + pathNodes->resize( pathNodes->size() - target.previousWayCoordinates ); + } else { + pathNodes->resize( begin + target.previousWayCoordinates - 1 ); + } + } else { + pathEdges->push_back( targetEdge.description() ); + } + pathNodes->push_back( target.nearestPoint ); + pathEdges->back().length = pathNodes->size() - begin; + + return targetDistance; +} + +bool ContractionHierarchiesClient::unpackEdge( const NodeIterator source, const NodeIterator target, bool forward, QVector< Node >* pathNodes, QVector< Edge >* pathEdges ) { + EdgeIterator shortestEdge; + + unsigned distance = std::numeric_limits< unsigned >::max(); + for ( EdgeIterator edge = m_graph.edges( source ); edge.hasEdgesLeft(); ) { + m_graph.unpackNextEdge( &edge ); + if ( edge.target() != target ) + continue; + if ( forward && !edge.forward() ) + continue; + if ( !forward && !edge.backward() ) + continue; + if ( edge.distance() > distance ) + continue; + distance = edge.distance(); + shortestEdge = edge; + } + + if ( shortestEdge.unpacked() ) { + m_graph.path( shortestEdge, pathNodes, pathEdges, forward ); + return true; + } + + if ( !shortestEdge.shortcut() ) { + pathEdges->push_back( shortestEdge.description() ); + if ( forward ) + pathNodes->push_back( m_graph.node( target ).coordinate ); + else + pathNodes->push_back( m_graph.node( source ).coordinate ); + return true; + } + + const NodeIterator middle = shortestEdge.middle(); + + if ( forward ) { + unpackEdge( middle, source, false, pathNodes, pathEdges ); + unpackEdge( middle, target, true, pathNodes, pathEdges ); + return true; + } else { + unpackEdge( middle, target, false, pathNodes, pathEdges ); + unpackEdge( middle, source, true, pathNodes, pathEdges ); + return true; + } +} + +Q_EXPORT_PLUGIN2( contractionhierarchiesclient, ContractionHierarchiesClient ) + diff --git a/monav/contractionhierarchies/contractionhierarchiesclient.h b/monav/contractionhierarchies/contractionhierarchiesclient.h new file mode 100644 index 0000000..d2c3ff1 --- /dev/null +++ b/monav/contractionhierarchies/contractionhierarchiesclient.h @@ -0,0 +1,93 @@ +/* +Copyright 2010 Christian Vetter veaac.fdirct@gmail.com + +This file is part of MoNav. + +MoNav is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +MoNav 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 General Public License +along with MoNav. If not, see . +*/ + +#ifndef CONTRACTIONHIERARCHIESCLIENT_H +#define CONTRACTIONHIERARCHIESCLIENT_H + +#include +#include +#include "interfaces/irouter.h" +#include "binaryheap.h" +#include "compressedgraph.h" +#include + +class ContractionHierarchiesClient : public QObject, public IRouter +{ + Q_OBJECT + Q_INTERFACES( IRouter ) +public: + ContractionHierarchiesClient(); + virtual ~ContractionHierarchiesClient(); + + virtual QString GetName(); + virtual void SetInputDirectory( const QString& dir ); + virtual void ShowSettings(); + virtual bool LoadData(); + virtual bool GetRoute( double* distance, QVector< Node>* pathNodes, QVector< Edge >* pathEdges, const IGPSLookup::Result& source, const IGPSLookup::Result& target ); + virtual bool GetName( QString* result, unsigned name ); + virtual bool GetNames( QVector< QString >* result, QVector< unsigned > names ); + virtual bool GetType( QString* result, unsigned type ); + virtual bool GetTypes( QVector< QString >* result, QVector< unsigned > types ); + +protected: + struct HeapData { + CompressedGraph::NodeIterator parent; + bool stalled: 1; + HeapData( CompressedGraph::NodeIterator p ) { + parent = p; + stalled = false; + } + }; + + class AllowForwardEdge { + public: + bool operator()( bool forward, bool /*backward*/ ) const { + return forward; + } + }; + + class AllowBackwardEdge { + public: + bool operator()( bool /*forward*/, bool backward ) const { + return backward; + } + }; + + typedef CompressedGraph::NodeIterator NodeIterator; + typedef CompressedGraph::EdgeIterator EdgeIterator; + typedef BinaryHeap< NodeIterator, int, int, HeapData, MapStorage< NodeIterator, unsigned > > Heap; + + CompressedGraph m_graph; + const char* m_names; + QFile m_namesFile; + Heap* m_heapForward; + Heap* m_heapBackward; + std::queue< NodeIterator > m_stallQueue; + QString m_directory; + QStringList m_types; + + void unload(); + template< class EdgeAllowed, class StallEdgeAllowed > + void computeStep( Heap* heapForward, Heap* heapBackward, const EdgeAllowed& edgeAllowed, const StallEdgeAllowed& stallEdgeAllowed, NodeIterator* middle, int* targetDistance ); + int computeRoute( const IGPSLookup::Result& source, const IGPSLookup::Result& target, QVector< Node>* pathNodes, QVector< Edge >* pathEdges ); + bool unpackEdge( const NodeIterator source, const NodeIterator target, bool forward, QVector< Node>* pathNodes, QVector< Edge >* pathEdges ); + +}; + +#endif // CONTRACTIONHIERARCHIESCLIENT_H diff --git a/monav/contractionhierarchies/contractionhierarchiesclient.pro b/monav/contractionhierarchies/contractionhierarchiesclient.pro new file mode 100644 index 0000000..81b4284 --- /dev/null +++ b/monav/contractionhierarchies/contractionhierarchiesclient.pro @@ -0,0 +1,25 @@ +TEMPLATE = lib +CONFIG += plugin +#CONFIG += debug +DESTDIR = .. +unix { + QMAKE_CXXFLAGS_RELEASE -= -O2 + QMAKE_CXXFLAGS_RELEASE += -O3 \ + -Wno-unused-function + QMAKE_CXXFLAGS_DEBUG += -Wno-unused-function +} + +HEADERS += \ + utils/coordinates.h \ + utils/config.h \ + blockcache.h \ + binaryheap.h \ + interfaces/irouter.h \ + contractionhierarchiesclient.h \ + compressedgraph.h \ + interfaces/igpslookup.h \ + utils/bithelpers.h \ + utils/qthelpers.h + +SOURCES += \ + contractionhierarchiesclient.cpp diff --git a/monav/contractionhierarchies/contractor.h b/monav/contractionhierarchies/contractor.h new file mode 100644 index 0000000..a770aed --- /dev/null +++ b/monav/contractionhierarchies/contractor.h @@ -0,0 +1,659 @@ +/* +Copyright 2010 Christian Vetter veaac.fdirct@gmail.com + +This file is part of MoNav. + +MoNav is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +MoNav 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 General Public License +along with MoNav. If not, see . +*/ + +#ifndef CONTRACTOR_H_INCLUDED +#define CONTRACTOR_H_INCLUDED +#include +#include +#include +#include "utils/qthelpers.h" +#include "dynamicgraph.h" +#include "binaryheap.h" +#include "utils/config.h" + +class Contractor { + + public: + + struct Witness { + NodeID source; + NodeID target; + NodeID middle; + }; + + private: + + struct _EdgeData { + unsigned distance; + unsigned originalEdges : 29; + bool shortcut : 1; + bool forward : 1; + bool backward : 1; + union { + NodeID middle; // shortcut + unsigned id; // original edge + }; + } data; + + struct _HeapData { + }; + + typedef DynamicGraph< _EdgeData > _DynamicGraph; + typedef BinaryHeap< NodeID, NodeID, unsigned, _HeapData > _Heap; + typedef _DynamicGraph::InputEdge _ImportEdge; + + struct _ThreadData { + _Heap heap; + std::vector< _ImportEdge > insertedEdges; + std::vector< Witness > witnessList; + std::vector< NodeID > neighbours; + _ThreadData( NodeID nodes ): heap( nodes ) { + } + }; + + struct _PriorityData { + int depth; + NodeID bias; + _PriorityData() { + depth = 0; + } + }; + + struct _ContractionInformation { + int edgesDeleted; + int edgesAdded; + int originalEdgesDeleted; + int originalEdgesAdded; + _ContractionInformation() { + edgesAdded = edgesDeleted = originalEdgesAdded = originalEdgesDeleted = 0; + } + }; + + struct _NodePartitionor { + bool operator()( std::pair< NodeID, bool > nodeData ) { + return !nodeData.second; + } + }; + + struct _LogItem { + unsigned iteration; + NodeID nodes; + double contraction; + double independent; + double inserting; + double removing; + double updating; + + _LogItem() { + iteration = nodes = contraction = independent = inserting = removing = updating = 0; + } + + double GetTotalTime() const { + return contraction + independent + inserting + removing + updating; + } + + void PrintStatistics() const { + qDebug( "%d\t%d\t%lf\t%lf\t%lf\t%lf\t%lf", iteration, nodes, independent, contraction, inserting, removing, updating ); + } + }; + + class _LogData { + public: + + std::vector < _LogItem > iterations; + + unsigned GetNIterations() { + return ( unsigned ) iterations.size(); + } + + _LogItem GetSum() const { + _LogItem sum; + sum.iteration = ( unsigned ) iterations.size(); + + for ( int i = 0, e = ( int ) iterations.size(); i < e; ++i ) { + sum.nodes += iterations[i].nodes; + sum.contraction += iterations[i].contraction; + sum.independent += iterations[i].independent; + sum.inserting += iterations[i].inserting; + sum.removing += iterations[i].removing; + sum.updating += iterations[i].updating; + } + + return sum; + } + + void PrintHeader() const { + qDebug( "Iteration\tNodes\tIndependent\tContraction\tInserting\tRemoving\tUpdating" ); + } + + void PrintSummary() const { + PrintHeader(); + GetSum().PrintStatistics(); + } + + void Print() const { + PrintHeader(); + for ( int i = 0, e = ( int ) iterations.size(); i < e; ++i ) + iterations[i].PrintStatistics(); + } + + void Insert( const _LogItem& data ) { + iterations.push_back( data ); + } + + }; + + public: + + template< class InputEdge > + Contractor( int nodes, const std::vector< InputEdge >& inputEdges ) { + std::vector< _ImportEdge > edges; + edges.reserve( 2 * inputEdges.size() ); + int skippedLargeEdges = 0; + for ( typename std::vector< InputEdge >::const_iterator i = inputEdges.begin(), e = inputEdges.end(); i != e; ++i ) { + _ImportEdge edge; + edge.source = i->source; + edge.target = i->target; + edge.data.distance = std::max( i->distance * 10.0 + 0.5, 1.0 ); + if ( edge.data.distance > 24 * 60 * 60 * 10 ) { + skippedLargeEdges++; + continue; + } + edge.data.shortcut = false; + edge.data.id = i - inputEdges.begin(); + edge.data.forward = true; + edge.data.backward = i->bidirectional; + edge.data.originalEdges = 1; + + if ( edge.data.distance < 1 ) { + qDebug() << edge.source << edge.target << edge.data.forward << edge.data.backward << edge.data.distance << edge.data.id << i->distance; + } + + if ( edge.source == edge.target ) { + _loops.push_back( edge ); + continue; + } + + edges.push_back( edge ); + std::swap( edge.source, edge.target ); + edge.data.forward = i->bidirectional; + edge.data.backward = true; + edges.push_back( edge ); + } + if ( skippedLargeEdges != 0 ) + qDebug( "Skipped %d edges with too large edge weight", skippedLargeEdges ); + std::sort( edges.begin(), edges.end() ); + + _graph = new _DynamicGraph( nodes, edges ); + + std::vector< _ImportEdge >().swap( edges ); + } + + ~Contractor() { + delete _graph; + } + + void Run() { + const NodeID numberOfNodes = _graph->GetNumberOfNodes(); + _LogData log; + + int maxThreads = omp_get_max_threads(); + std::vector < _ThreadData* > threadData; + for ( int threadNum = 0; threadNum < maxThreads; ++threadNum ) { + threadData.push_back( new _ThreadData( numberOfNodes ) ); + } + qDebug( "%d nodes, %d edges", numberOfNodes, _graph->GetNumberOfEdges() ); + qDebug( "using %d threads", maxThreads ); + + NodeID levelID = 0; + NodeID iteration = 0; + std::vector< std::pair< NodeID, bool > > remainingNodes( numberOfNodes ); + std::vector< double > nodePriority( numberOfNodes ); + std::vector< _PriorityData > nodeData( numberOfNodes ); + + //initialize the variables + #pragma omp parallel for schedule ( guided ) + for ( int x = 0; x < ( int ) numberOfNodes; ++x ) + remainingNodes[x].first = x; + std::random_shuffle( remainingNodes.begin(), remainingNodes.end() ); + for ( int x = 0; x < ( int ) numberOfNodes; ++x ) + nodeData[remainingNodes[x].first].bias = x; + + qDebug( "Initialise Elimination PQ... " ); + _LogItem statistics0; + statistics0.updating = _Timestamp(); + statistics0.iteration = 0; + #pragma omp parallel + { + _ThreadData* data = threadData[omp_get_thread_num()]; + #pragma omp for schedule ( guided ) + for ( int x = 0; x < ( int ) numberOfNodes; ++x ) { + nodePriority[x] = _Evaluate( data, &nodeData[x], x ); + } + } + qDebug( "done" ); + + statistics0.updating = _Timestamp() - statistics0.updating; + log.Insert( statistics0 ); + + log.PrintHeader(); + statistics0.PrintStatistics(); + + while ( levelID < numberOfNodes ) { + _LogItem statistics; + statistics.iteration = iteration++; + const int last = ( int ) remainingNodes.size(); + + //determine independent node set + double timeLast = _Timestamp(); + #pragma omp parallel + { + _ThreadData* const data = threadData[omp_get_thread_num()]; + #pragma omp for schedule ( guided ) + for ( int i = 0; i < last; ++i ) { + const NodeID node = remainingNodes[i].first; + remainingNodes[i].second = _IsIndependent( nodePriority, nodeData, data, node ); + } + } + _NodePartitionor functor; + const std::vector < std::pair < NodeID, bool > >::const_iterator first = stable_partition( remainingNodes.begin(), remainingNodes.end(), functor ); + const int firstIndependent = first - remainingNodes.begin(); + statistics.nodes = last - firstIndependent; + statistics.independent += _Timestamp() - timeLast; + timeLast = _Timestamp(); + + //contract independent nodes + #pragma omp parallel + { + _ThreadData* const data = threadData[omp_get_thread_num()]; + #pragma omp for schedule ( guided ) nowait + for ( int position = firstIndependent ; position < last; ++position ) { + NodeID x = remainingNodes[position].first; + _Contract< false > ( data, x ); + nodePriority[x] = -1; + } + std::sort( data->insertedEdges.begin(), data->insertedEdges.end() ); + } + statistics.contraction += _Timestamp() - timeLast; + timeLast = _Timestamp(); + + #pragma omp parallel + { + _ThreadData* const data = threadData[omp_get_thread_num()]; + #pragma omp for schedule ( guided ) nowait + for ( int position = firstIndependent ; position < last; ++position ) { + NodeID x = remainingNodes[position].first; + _DeleteIncommingEdges( data, x ); + } + } + statistics.removing += _Timestamp() - timeLast; + timeLast = _Timestamp(); + + //insert new edges + for ( int threadNum = 0; threadNum < maxThreads; ++threadNum ) { + _ThreadData& data = *threadData[threadNum]; + for ( int i = 0; i < ( int ) data.insertedEdges.size(); ++i ) { + const _ImportEdge& edge = data.insertedEdges[i]; + _graph->InsertEdge( edge.source, edge.target, edge.data ); + } + std::vector< _ImportEdge >().swap( data.insertedEdges ); + } + statistics.inserting += _Timestamp() - timeLast; + timeLast = _Timestamp(); + + //update priorities + #pragma omp parallel + { + _ThreadData* const data = threadData[omp_get_thread_num()]; + #pragma omp for schedule ( guided ) nowait + for ( int position = firstIndependent ; position < last; ++position ) { + NodeID x = remainingNodes[position].first; + _UpdateNeighbours( &nodePriority, &nodeData, data, x ); + } + } + statistics.updating += _Timestamp() - timeLast; + timeLast = _Timestamp(); + + //output some statistics + statistics.PrintStatistics(); + //qDebug( wxT( "Printed" ) ); + + //remove contracted nodes from the pool + levelID += last - firstIndependent; + remainingNodes.resize( firstIndependent ); + std::vector< std::pair< NodeID, bool > >( remainingNodes ).swap( remainingNodes ); + log.Insert( statistics ); + } + + for ( int threadNum = 0; threadNum < maxThreads; threadNum++ ) { + _witnessList.insert( _witnessList.end(), threadData[threadNum]->witnessList.begin(), threadData[threadNum]->witnessList.end() ); + delete threadData[threadNum]; + } + + log.PrintSummary(); + qDebug( "Total Time: %lf s", log.GetSum().GetTotalTime() ); + + } + + template< class Edge > + void GetEdges( std::vector< Edge >* edges ) { + NodeID numberOfNodes = _graph->GetNumberOfNodes(); + for ( NodeID node = 0; node < numberOfNodes; ++node ) { + for ( _DynamicGraph::EdgeIterator edge = _graph->BeginEdges( node ), endEdges = _graph->EndEdges( node ); edge != endEdges; ++edge ) { + const NodeID target = _graph->GetTarget( edge ); + const _EdgeData& data = _graph->GetEdgeData( edge ); + Edge newEdge; + newEdge.source = node; + newEdge.target = target; + newEdge.data.distance = data.distance; + newEdge.data.shortcut = data.shortcut; + if ( data.shortcut ) + newEdge.data.middle = data.middle; + else + newEdge.data.id = data.id; + newEdge.data.forward = data.forward; + newEdge.data.backward = data.backward; + edges->push_back( newEdge ); + } + } + } + + template< class Edge > + void GetLoops( std::vector< Edge >* edges ) { + for ( unsigned i = 0; i < _loops.size(); i++ ) { + Edge newEdge; + newEdge.source = _loops[i].source; + newEdge.target = _loops[i].target; + newEdge.data.distance = _loops[i].data.distance; + newEdge.data.shortcut = _loops[i].data.shortcut; + newEdge.data.id = _loops[i].data.id; + newEdge.data.forward = _loops[i].data.forward; + newEdge.data.backward = _loops[i].data.backward; + edges->push_back( newEdge ); + } + } + + void GetWitnessList( std::vector< Witness >& list ) { + list = _witnessList; + } + + private: + + double _Timestamp() { + static Timer timer; + return ( double ) timer.elapsed() / 1000; + } + + bool _ConstructCH( _DynamicGraph* _graph ); + + void _Dijkstra( const unsigned maxDistance, const int maxNodes, _ThreadData* const data ){ + + _Heap& heap = data->heap; + + int nodes = 0; + while ( heap.Size() > 0 ) { + const NodeID node = heap.DeleteMin(); + const unsigned distance = heap.GetKey( node ); + if ( nodes++ > maxNodes ) + return; + //Destination settled? + if ( distance > maxDistance ) + return; + + //iterate over all edges of node + for ( _DynamicGraph::EdgeIterator edge = _graph->BeginEdges( node ), endEdges = _graph->EndEdges( node ); edge != endEdges; ++edge ) { + const _EdgeData& data = _graph->GetEdgeData( edge ); + if ( !data.forward ) + continue; + const NodeID to = _graph->GetTarget( edge ); + const unsigned toDistance = distance + data.distance; + + //New Node discovered -> Add to Heap + Node Info Storage + if ( !heap.WasInserted( to ) ) + heap.Insert( to, toDistance, _HeapData() ); + + //Found a shorter Path -> Update distance + else if ( toDistance < heap.GetKey( to ) ) { + heap.DecreaseKey( to, toDistance ); + } + } + } + } + + double _Evaluate( _ThreadData* const data, _PriorityData* const nodeData, NodeID node ){ + _ContractionInformation stats; + + //perform simulated contraction + _Contract< true > ( data, node, &stats ); + + // Result will contain the priority + double result; + if ( stats.edgesDeleted == 0 || stats.originalEdgesDeleted == 0 ) + result = 1 * nodeData->depth; + else + result = 2 * ((( double ) stats.edgesAdded ) / stats.edgesDeleted ) + 1 * ((( double ) stats.originalEdgesAdded ) / stats.originalEdgesDeleted ) + 1 * nodeData->depth; + assert( result >= 0 ); + return result; + } + + + template< bool Simulate > bool _Contract( _ThreadData* const data, NodeID node, _ContractionInformation* const stats = NULL ) { + _Heap& heap = data->heap; + //std::vector< Witness >& witnessList = data->witnessList; + int insertedEdgesSize = data->insertedEdges.size(); + std::vector< _ImportEdge >& insertedEdges = data->insertedEdges; + + for ( _DynamicGraph::EdgeIterator inEdge = _graph->BeginEdges( node ), endInEdges = _graph->EndEdges( node ); inEdge != endInEdges; ++inEdge ) { + const _EdgeData& inData = _graph->GetEdgeData( inEdge ); + const NodeID source = _graph->GetTarget( inEdge ); + if ( Simulate ) { + assert( stats != NULL ); + stats->edgesDeleted++; + stats->originalEdgesDeleted += inData.originalEdges; + } + if ( !inData.backward ) + continue; + + heap.Clear(); + heap.Insert( source, 0, _HeapData() ); + if ( node != source ) + heap.Insert( node, inData.distance, _HeapData() ); + unsigned maxDistance = 0; + + for ( _DynamicGraph::EdgeIterator outEdge = _graph->BeginEdges( node ), endOutEdges = _graph->EndEdges( node ); outEdge != endOutEdges; ++outEdge ) { + const _EdgeData& outData = _graph->GetEdgeData( outEdge ); + if ( !outData.forward ) + continue; + const NodeID target = _graph->GetTarget( outEdge ); + const unsigned pathDistance = inData.distance + outData.distance; + maxDistance = std::max( maxDistance, pathDistance ); + if ( !heap.WasInserted( target ) ) + heap.Insert( target, pathDistance, _HeapData() ); + else if ( pathDistance < heap.GetKey( target ) ) + heap.DecreaseKey( target, pathDistance ); + } + + if ( Simulate ) + _Dijkstra( maxDistance, 500, data ); + else + _Dijkstra( maxDistance, 1000, data ); + + for ( _DynamicGraph::EdgeIterator outEdge = _graph->BeginEdges( node ), endOutEdges = _graph->EndEdges( node ); outEdge != endOutEdges; ++outEdge ) { + const _EdgeData& outData = _graph->GetEdgeData( outEdge ); + if ( !outData.forward ) + continue; + const NodeID target = _graph->GetTarget( outEdge ); + const int pathDistance = inData.distance + outData.distance; + const int distance = heap.GetKey( target ); + + if ( pathDistance <= distance ) { + if ( Simulate ) { + assert( stats != NULL ); + stats->edgesAdded += 2; + stats->originalEdgesAdded += 2 * ( outData.originalEdges + inData.originalEdges ); + } else { + _ImportEdge newEdge; + newEdge.source = source; + newEdge.target = target; + newEdge.data.distance = pathDistance; + newEdge.data.forward = true; + newEdge.data.backward = false; + newEdge.data.middle = node; + newEdge.data.shortcut = true; + newEdge.data.originalEdges = outData.originalEdges + inData.originalEdges; + insertedEdges.push_back( newEdge ); + std::swap( newEdge.source, newEdge.target ); + newEdge.data.forward = false; + newEdge.data.backward = true; + insertedEdges.push_back( newEdge ); + } + } + /*else if ( !Simulate ) { + Witness witness; + witness.source = source; + witness.target = target; + witness.middle = node; + witnessList.push_back( witness ); + }*/ + } + } + + if ( !Simulate ) { + for ( int i = insertedEdgesSize, iend = insertedEdges.size(); i < iend; i++ ) { + bool found = false; + for ( int other = i + 1 ; other < iend ; ++other ) { + if ( insertedEdges[other].source != insertedEdges[i].source ) + continue; + if ( insertedEdges[other].target != insertedEdges[i].target ) + continue; + if ( insertedEdges[other].data.distance != insertedEdges[i].data.distance ) + continue; + if ( insertedEdges[other].data.shortcut != insertedEdges[i].data.shortcut ) + continue; + insertedEdges[other].data.forward |= insertedEdges[i].data.forward; + insertedEdges[other].data.backward |= insertedEdges[i].data.backward; + found = true; + break; + } + if ( !found ) + insertedEdges[insertedEdgesSize++] = insertedEdges[i]; + } + insertedEdges.resize( insertedEdgesSize ); + } + + return true; + } + + bool _DeleteIncommingEdges( _ThreadData* const data, NodeID node ) { + std::vector< NodeID >& neighbours = data->neighbours; + neighbours.clear(); + + //find all neighbours + for ( _DynamicGraph::EdgeIterator e = _graph->BeginEdges( node ) ; e < _graph->EndEdges( node ) ; ++e ) { + const NodeID u = _graph->GetTarget( e ); + if ( u == node ) + continue; + neighbours.push_back( u ); + } + //eliminate duplicate entries ( forward + backward edges ) + std::sort( neighbours.begin(), neighbours.end() ); + neighbours.resize( std::unique( neighbours.begin(), neighbours.end() ) - neighbours.begin() ); + + for ( int i = 0, e = ( int ) neighbours.size(); i < e; ++i ) { + const NodeID u = neighbours[i]; + _graph->DeleteEdgesTo( u, node ); + } + + return true; + } + + bool _UpdateNeighbours( std::vector< double >* priorities, std::vector< _PriorityData >* const nodeData, _ThreadData* const data, NodeID node ) { + std::vector< NodeID >& neighbours = data->neighbours; + neighbours.clear(); + + //find all neighbours + for ( _DynamicGraph::EdgeIterator e = _graph->BeginEdges( node ) ; e < _graph->EndEdges( node ) ; ++e ) { + const NodeID u = _graph->GetTarget( e ); + if ( u == node ) + continue; + neighbours.push_back( u ); + ( *nodeData )[u].depth = std::max(( *nodeData )[node].depth + 1, ( *nodeData )[u].depth ); + } + //eliminate duplicate entries ( forward + backward edges ) + std::sort( neighbours.begin(), neighbours.end() ); + neighbours.resize( std::unique( neighbours.begin(), neighbours.end() ) - neighbours.begin() ); + + for ( int i = 0, e = ( int ) neighbours.size(); i < e; ++i ) { + const NodeID u = neighbours[i]; + ( *priorities )[u] = _Evaluate( data, &( *nodeData )[u], u ); + } + + return true; + } + + bool _IsIndependent( const std::vector< double >& priorities, const std::vector< _PriorityData >& nodeData, _ThreadData* const data, NodeID node ) { + const double priority = priorities[node]; + + std::vector< NodeID >& neighbours = data->neighbours; + neighbours.clear(); + + for ( _DynamicGraph::EdgeIterator e = _graph->BeginEdges( node ) ; e < _graph->EndEdges( node ) ; ++e ) { + const NodeID target = _graph->GetTarget( e ); + const double targetPriority = priorities[target]; + assert( targetPriority >= 0 ); + //found a neighbour with lower priority? + if ( priority > targetPriority ) + return false; + //tie breaking + if ( priority == targetPriority && nodeData[node].bias < nodeData[target].bias ) + return false; + neighbours.push_back( target ); + } + + std::sort( neighbours.begin(), neighbours.end() ); + neighbours.resize( std::unique( neighbours.begin(), neighbours.end() ) - neighbours.begin() ); + + //examine all neighbours that are at most 2 hops away + for ( std::vector< NodeID >::const_iterator i = neighbours.begin(), lastNode = neighbours.end(); i != lastNode; ++i ) { + const NodeID u = *i; + + for ( _DynamicGraph::EdgeIterator e = _graph->BeginEdges( u ) ; e < _graph->EndEdges( u ) ; ++e ) { + const NodeID target = _graph->GetTarget( e ); + + const double targetPriority = priorities[target]; + assert( targetPriority >= 0 ); + //found a neighbour with lower priority? + if ( priority > targetPriority ) + return false; + //tie breaking + if ( priority == targetPriority && nodeData[node].bias < nodeData[target].bias ) + return false; + } + } + + return true; + } + + + _DynamicGraph* _graph; + std::vector< Witness > _witnessList; + std::vector< _ImportEdge > _loops; +}; + +#endif // CONTRACTOR_H_INCLUDED diff --git a/monav/contractionhierarchies/interfaces b/monav/contractionhierarchies/interfaces new file mode 120000 index 0000000..5e17f8d --- /dev/null +++ b/monav/contractionhierarchies/interfaces @@ -0,0 +1 @@ +../interfaces/ \ No newline at end of file diff --git a/monav/contractionhierarchies/utils b/monav/contractionhierarchies/utils new file mode 120000 index 0000000..19985ba --- /dev/null +++ b/monav/contractionhierarchies/utils @@ -0,0 +1 @@ +../utils/ \ No newline at end of file diff --git a/monav/gpsgrid/cell.h b/monav/gpsgrid/cell.h new file mode 100644 index 0000000..52877f7 --- /dev/null +++ b/monav/gpsgrid/cell.h @@ -0,0 +1,286 @@ +/* +Copyright 2010 Christian Vetter veaac.fdirct@gmail.com + +This file is part of MoNav. + +MoNav is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +MoNav 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 General Public License +along with MoNav. If not, see . +*/ + +#ifndef CELL_H +#define CELL_H + +#include "utils/config.h" +#include "utils/coordinates.h" +#include "utils/bithelpers.h" +#include "utils/edgeconnector.h" +#include +#include + +namespace gg +{ + + class Cell { + + public: + + struct Edge { + unsigned pathID; + unsigned short pathLength; + unsigned short edgeID; + NodeID source; + NodeID target; + bool bidirectional; + bool operator==( const Edge& right ) const { + if ( source != right.source ) + return false; + if ( target != right.target ) + return false; + if ( bidirectional != right.bidirectional ) + return false; + if ( edgeID != right.edgeID ) + return false; + if ( pathLength != right.pathLength ) + return false; + return true; + } + }; + + std::vector< Edge > edges; + std::vector< UnsignedCoordinate > coordinates; + + bool operator==( const Cell& right ) const + { + if ( edges.size() != right.edges.size() ) + return false; + for ( int i = 0; i < ( int ) edges.size(); i++ ) { + if ( !( edges[i] == right.edges[i] ) ) + return false; + for ( unsigned path = 0; path < edges[i].pathLength; path++ ) { + if ( coordinates[path + edges[i].pathID].x != right.coordinates[path + right.edges[i].pathID].x ) + return false; + if ( coordinates[path + edges[i].pathID].y != right.coordinates[path + right.edges[i].pathID].y ) + return false; + } + } + + return true; + } + + size_t write( unsigned char* buffer, UnsignedCoordinate min, UnsignedCoordinate max ) + { + unsigned char* const oldBuffer = buffer; + + NodeID minID = std::numeric_limits< NodeID >::max(); + NodeID maxID = 0; + unsigned short maxEdgeID = 0; + unsigned short maxPathLength = 0; + + { + // build edge connector data structures + std::vector< EdgeConnector< unsigned >::Edge > connectorEdges; + std::vector< unsigned > resultSegments; + std::vector< unsigned > resultSegmentDescriptions; + std::vector< bool > resultReversed; + + for ( unsigned i = 0; i < ( unsigned ) edges.size(); i++ ) { + EdgeConnector< unsigned >::Edge newEdge; + newEdge.source = edges[i].source; + newEdge.target = edges[i].target; + newEdge.reverseable = edges[i].bidirectional; + connectorEdges.push_back( newEdge ); + } + + EdgeConnector< unsigned >::run( &resultSegments, &resultSegmentDescriptions, &resultReversed, connectorEdges ); + + for ( unsigned i = 0; i < ( unsigned ) edges.size(); i++ ) { + if ( resultReversed[i] ) { + std::swap( edges[i].source, edges[i].target ); + std::reverse( coordinates.begin() + edges[i].pathID, coordinates.begin() + edges[i].pathID + edges[i].pathLength ); + } + } + + std::vector< Edge > reorderedEdges; + for ( unsigned i = 0; i < ( unsigned ) resultSegmentDescriptions.size(); i++ ) + reorderedEdges.push_back( edges[resultSegmentDescriptions[i]] ); + + edges.swap( reorderedEdges ); + } + + std::vector< NodeID > nodes; + for ( std::vector< Edge >::iterator i = edges.begin(), e = edges.end(); i != e; ++i ) { + minID = std::min( minID, i->source ); + minID = std::min( minID, i->target ); + maxID = std::max( maxID, i->source ); + maxID = std::max( maxID, i->target ); + maxID = std::max( maxID, i->target ); + maxEdgeID = std::max( maxEdgeID, i->edgeID ); + assert( i->pathLength > 1 ); + maxPathLength = std::max( maxPathLength, ( unsigned short ) ( i->pathLength - 2 ) ); + + nodes.push_back( i->source ); + nodes.push_back( i->target ); + } + + std::sort( nodes.begin(), nodes.end() ); + nodes.resize( std::unique( nodes.begin(), nodes.end() ) - nodes.begin() ); + + const char xBits = bits_needed( max.x - min.x ); + const char yBits = bits_needed( max.y - min.y ); + const char idBits = bits_needed( maxID - minID ); + const char edgeIDBits = bits_needed( maxEdgeID ); + const char pathLengthBits = bits_needed( maxPathLength ); + + int offset = 0; + write_unaligned_unsigned( &buffer, edges.size(), 32, &offset ); + write_unaligned_unsigned( &buffer, minID, 32, &offset ); + write_unaligned_unsigned( &buffer, idBits, 6, &offset ); + write_unaligned_unsigned( &buffer, edgeIDBits, 6, &offset ); + write_unaligned_unsigned( &buffer, pathLengthBits, 6, &offset ); + + NodeID lastTarget = std::numeric_limits< NodeID >::max(); + std::vector< UnsignedCoordinate > wayBuffer; + for ( std::vector< Edge >::const_iterator i = edges.begin(), e = edges.end(); i != e; i++ ) { + bool reuse = lastTarget == i->source; + + write_unaligned_unsigned( &buffer, reuse ? 1 : 0, 1, &offset ); + if ( !reuse ) + write_unaligned_unsigned( &buffer, i->source - minID, idBits, &offset ); + write_unaligned_unsigned( &buffer, i->target - minID, idBits, &offset ); + write_unaligned_unsigned( &buffer, i->edgeID, edgeIDBits, &offset ); + write_unaligned_unsigned( &buffer, i->bidirectional ? 1 : 0, 1, &offset ); + write_unaligned_unsigned( &buffer, i->pathLength - 2, pathLengthBits, &offset ); + + assert( i->pathLength >= 2 ); + for ( int pathID = 1; pathID < i->pathLength - 1; pathID++ ) + wayBuffer.push_back( coordinates[pathID + i->pathID] ); + + lastTarget = i->target; + } + + std::vector< UnsignedCoordinate > nodeCoordinates( nodes.size() ); + for ( std::vector< Edge >::iterator i = edges.begin(), e = edges.end(); i != e; ++i ) { + unsigned sourcePos = lower_bound( nodes.begin(), nodes.end(), i->source ) - nodes.begin(); + unsigned targetPos = lower_bound( nodes.begin(), nodes.end(), i->target ) - nodes.begin(); + nodeCoordinates[sourcePos] = coordinates[i->pathID]; + nodeCoordinates[targetPos] = coordinates[i->pathID + i->pathLength - 1]; + } + + for ( std::vector< UnsignedCoordinate >::const_iterator i = nodeCoordinates.begin(), iend = nodeCoordinates.end(); i != iend; i++ ) { + writeCoordinate( &buffer, &offset, i->x, min.x, max.x, xBits); + writeCoordinate( &buffer, &offset, i->y, min.y, max.y, yBits); + } + + for ( std::vector< UnsignedCoordinate >::const_iterator i = wayBuffer.begin(), iend = wayBuffer.end(); i != iend; i++ ) { + writeCoordinate( &buffer, &offset, i->x, min.x, max.x, xBits); + writeCoordinate( &buffer, &offset, i->y, min.y, max.y, yBits); + } + + buffer += ( offset + 7 ) / 8; + + return buffer - oldBuffer; + } + + size_t read( const unsigned char* buffer, UnsignedCoordinate min, UnsignedCoordinate max ) { + const unsigned char* oldBuffer = buffer; + + int offset = 0; + + const char xBits = bits_needed( max.x - min.x ); + const char yBits = bits_needed( max.y - min.y ); + + unsigned numEdges = read_unaligned_unsigned( &buffer, 32, &offset ); + unsigned minID = read_unaligned_unsigned( &buffer, 32, &offset ); + unsigned idBits = read_unaligned_unsigned( &buffer, 6, &offset ); + unsigned edgeIDBits = read_unaligned_unsigned( &buffer, 6, &offset ); + unsigned pathLengthBits = read_unaligned_unsigned( &buffer, 6, &offset ); + + std::vector< NodeID > nodes; + NodeID lastTarget = std::numeric_limits< NodeID >::max(); + for ( unsigned i = 0; i < numEdges; i++ ) { + Edge edge; + bool reuse = read_unaligned_unsigned( &buffer, 1, &offset ) == 1; + if ( reuse ) + edge.source = lastTarget; + else + edge.source = read_unaligned_unsigned( &buffer, idBits, &offset ) + minID; + edge.target = read_unaligned_unsigned( &buffer, idBits, &offset ) + minID; + edge.edgeID = read_unaligned_unsigned( &buffer, edgeIDBits, &offset ); + edge.bidirectional = read_unaligned_unsigned( &buffer, 1, &offset ) == 1; + edge.pathLength = read_unaligned_unsigned( &buffer, pathLengthBits, &offset ); + + edges.push_back( edge ); + + nodes.push_back( edge.source ); + nodes.push_back( edge.target ); + + lastTarget = edge.target; + } + assert( edges.size() != 0 ); + + std::sort( nodes.begin(), nodes.end() ); + nodes.resize( std::unique( nodes.begin(), nodes.end() ) - nodes.begin() ); + + std::vector< UnsignedCoordinate > nodeCoordinates( nodes.size() ); + for ( std::vector< UnsignedCoordinate >::iterator i = nodeCoordinates.begin(), iend = nodeCoordinates.end(); i != iend; i++ ) { + readCoordinate( &buffer, &offset, &i->x, min.x, max.x, xBits); + readCoordinate( &buffer, &offset, &i->y, min.y, max.y, yBits); + } + + for ( std::vector< Edge >::iterator i = edges.begin(), iend = edges.end(); i != iend; i++ ) { + unsigned sourcePos = lower_bound( nodes.begin(), nodes.end(), i->source ) - nodes.begin(); + unsigned targetPos = lower_bound( nodes.begin(), nodes.end(), i->target ) - nodes.begin(); + if ( coordinates.size() == 0 || coordinates.back().x != nodeCoordinates[sourcePos].x || coordinates.back().y != nodeCoordinates[sourcePos].y ) + coordinates.push_back( nodeCoordinates[sourcePos] ); + i->pathID = coordinates.size() - 1; + for ( int path = 0; path < i->pathLength; path++ ) { + UnsignedCoordinate coordinate; + readCoordinate( &buffer, &offset, &coordinate.x, min.x, max.x, xBits); + readCoordinate( &buffer, &offset, &coordinate.y, min.y, max.y, yBits); + coordinates.push_back( coordinate ); + } + coordinates.push_back( nodeCoordinates[targetPos] ); + + i->pathLength += 2; + } + + buffer += ( offset + 7 ) / 8; + + return buffer - oldBuffer; + } + + protected: + + void writeCoordinate( unsigned char** buffer, int* offset, unsigned value, unsigned min, unsigned max, char bits ) + { + bool inside = !( value < min || value > max ); + write_unaligned_unsigned( buffer, inside ? 1 : 0, 1, offset ); + if ( inside ) + write_unaligned_unsigned( buffer, value - min, bits, offset ); + else + write_unaligned_unsigned( buffer, value, 32, offset ); + } + + void readCoordinate( const unsigned char** buffer, int* offset, unsigned* value, unsigned min, unsigned /*max*/, char bits ) + { + bool inside = read_unaligned_unsigned( buffer, 1, offset ) == 1; + if ( inside ) + *value = read_unaligned_unsigned( buffer, bits, offset ) + min; + else + *value = read_unaligned_unsigned( buffer, 32, offset ); + } + + }; +} + +#endif // CELL_H diff --git a/monav/gpsgrid/gpsgridclient.cpp b/monav/gpsgrid/gpsgridclient.cpp new file mode 100644 index 0000000..fcbc02c --- /dev/null +++ b/monav/gpsgrid/gpsgridclient.cpp @@ -0,0 +1,290 @@ +/* +Copyright 2010 Christian Vetter veaac.fdirct@gmail.com + +This file is part of MoNav. + +MoNav is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +MoNav 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 General Public License +along with MoNav. If not, see . +*/ + +#include "gpsgridclient.h" +#include +#include +#include +#include "utils/qthelpers.h" +#ifndef NOGUI + #include +#endif +#include + +GPSGridClient::GPSGridClient() +{ + index = NULL; + gridFile = NULL; + QSettings settings( "MoNavClient" ); + settings.beginGroup( "GPS Grid" ); + cacheSize = settings.value( "cacheSize", 1 ).toInt(); + cache.setMaxCost( 1024 * 1024 * 3 * cacheSize / 4 ); +} + +GPSGridClient::~GPSGridClient() +{ + QSettings settings( "MoNavClient" ); + settings.beginGroup( "GPS Grid" ); + settings.setValue( "cacheSize", cacheSize ); + unload(); +} + +void GPSGridClient::unload() +{ + if ( index != NULL ) + delete index; + index = NULL; + if ( gridFile != NULL ) + delete gridFile; + gridFile = NULL; + cache.clear(); +} + +QString GPSGridClient::GetName() +{ + return "GPS Grid"; +} + +void GPSGridClient::SetInputDirectory( const QString& dir ) +{ + directory = dir; +} + +void GPSGridClient::ShowSettings() +{ +#ifndef NOGUI + bool ok = false; + int result = QInputDialog::getInt( NULL, "Settings", "Enter Cache Size [MB]", cacheSize, 1, 1024, 1, &ok ); + if ( !ok ) + return; + cacheSize = result; + if ( index != NULL ) + index->SetCacheSize( 1024 * 1024 * cacheSize / 4 ); + cache.setMaxCost( 1024 * 1024 * 3 * cacheSize / 4 ); +#endif +} + +bool GPSGridClient::LoadData() +{ + unload(); + QString filename = fileInDirectory( directory, "GPSGrid" ); + QFile configFile( filename + "_config" ); + if ( !openQFile( &configFile, QIODevice::ReadOnly ) ) + return false; + + index = new gg::Index( filename + "_index" ); + index->SetCacheSize( 1024 * 1024 * cacheSize / 4 ); + + gridFile = new QFile( filename + "_grid" ); + if ( !gridFile->open( QIODevice::ReadOnly ) ) { + qCritical() << "failed to open file: " << gridFile->fileName(); + return false; + } + + return true; +} + +bool GPSGridClient::GetNearestEdge( Result* result, const UnsignedCoordinate& coordinate, double radius, bool headingPenalty, double heading ) +{ + const GPSCoordinate gps = coordinate.ToProjectedCoordinate().ToGPSCoordinate(); + const GPSCoordinate gpsMoved( gps.latitude, gps.longitude + 1 ); + + const double meter = gps.ApproximateDistance( gpsMoved ); + double gridRadius = (( double ) UnsignedCoordinate( ProjectedCoordinate( gpsMoved ) ).x - coordinate.x ) / meter * radius; + gridRadius *= gridRadius; + heading = fmod( ( heading + 270 ) * 2.0 * M_PI / 360.0, 2 * M_PI ); + + static const int width = 32 * 32 * 32; + + ProjectedCoordinate position = coordinate.ToProjectedCoordinate(); + NodeID yGrid = floor( position.y * width ); + NodeID xGrid = floor( position.x * width ); + + result->distance = gridRadius; + QVector< UnsignedCoordinate > path; + + checkCell( result, &path, xGrid - 1, yGrid - 1, coordinate, heading, headingPenalty ); + checkCell( result, &path, xGrid - 1, yGrid, coordinate, heading, headingPenalty ); + checkCell( result, &path, xGrid - 1, yGrid + 1, coordinate, heading, headingPenalty ); + + checkCell( result, &path, xGrid, yGrid - 1, coordinate, heading, headingPenalty ); + checkCell( result, &path, xGrid, yGrid, coordinate, heading, headingPenalty ); + checkCell( result, &path, xGrid, yGrid + 1, coordinate, heading, headingPenalty ); + + checkCell( result, &path, xGrid + 1, yGrid - 1, coordinate, heading, headingPenalty ); + checkCell( result, &path, xGrid + 1, yGrid, coordinate, heading, headingPenalty ); + checkCell( result, &path, xGrid + 1, yGrid + 1, coordinate, heading, headingPenalty ); + + if ( path.empty() ) + return false; + + double length = 0; + double lengthToNearest = 0; + for ( int pathID = 1; pathID < path.size(); pathID++ ) { + UnsignedCoordinate sourceCoord = path[pathID - 1]; + UnsignedCoordinate targetCoord = path[pathID]; + double xDiff = ( double ) sourceCoord.x - targetCoord.x; + double yDiff = ( double ) sourceCoord.y - targetCoord.y; + + double distance = sqrt( xDiff * xDiff + yDiff * yDiff ); + length += distance; + if ( pathID < ( int ) result->previousWayCoordinates ) + lengthToNearest += distance; + else if ( pathID == ( int ) result->previousWayCoordinates ) + lengthToNearest += result->percentage * distance; + } + if ( length == 0 ) + result->percentage = 0; + else + result->percentage = lengthToNearest / length; + return true; +} + +bool GPSGridClient::checkCell( Result* result, QVector< UnsignedCoordinate >* path, NodeID gridX, NodeID gridY, const UnsignedCoordinate& coordinate, double heading, double headingPenalty ) { + static const int width = 32 * 32 * 32; + ProjectedCoordinate minPos( ( double ) gridX / width, ( double ) gridY / width ); + ProjectedCoordinate maxPos( ( double ) ( gridX + 1 ) / width, ( double ) ( gridY + 1 ) / width ); + UnsignedCoordinate min( minPos ); + UnsignedCoordinate max( maxPos ); + if ( distance( min, max, coordinate ) >= result->distance ) + return false; + + qint64 cellNumber = ( qint64( gridX ) << 32 ) + gridY; + if ( !cache.contains( cellNumber ) ) { + qint64 position = index->GetIndex( gridX, gridY ); + if ( position == -1 ) + return true; + gridFile->seek( position ); + int size; + gridFile->read( (char* ) &size, sizeof( size ) ); + unsigned char* buffer = new unsigned char[size + 8]; // reading buffer + 4 bytes + + gridFile->read( ( char* ) buffer, size ); + gg::Cell* cell = new gg::Cell(); + cell->read( buffer, min, max ); + cache.insert( cellNumber, cell, cell->edges.size() * sizeof( gg::Cell::Edge ) ); + delete[] buffer; + } + gg::Cell* cell = cache.object( cellNumber ); + if ( cell == NULL ) + return true; + + UnsignedCoordinate nearestPoint; + for ( std::vector< gg::Cell::Edge >::const_iterator i = cell->edges.begin(), e = cell->edges.end(); i != e; ++i ) { + bool found = false; + + for ( int pathID = 1; pathID < i->pathLength; pathID++ ) { + UnsignedCoordinate sourceCoord = cell->coordinates[pathID + i->pathID - 1]; + UnsignedCoordinate targetCoord = cell->coordinates[pathID + i->pathID]; + double percentage = 0; + + double d = distance( &nearestPoint, &percentage, sourceCoord, targetCoord, coordinate ); + if ( d + headingPenalty > result->distance ) + continue; + + double xDiff = ( double ) targetCoord.x - sourceCoord.x; + double yDiff = ( double ) targetCoord.y - sourceCoord.y; + double direction = 0; + if ( xDiff != 0 || yDiff != 0 ) + direction = fmod( atan2( yDiff, xDiff ), 2 * M_PI ); + else + headingPenalty = 0; + double penalty = fabs( direction - heading ); + if ( penalty > M_PI ) + penalty = 2 * M_PI - penalty; + if ( i->bidirectional && penalty > M_PI / 2 ) + penalty = M_PI - penalty; + penalty = penalty / M_PI * headingPenalty; + d += penalty; + + if ( d < result->distance ) { + result->nearestPoint = nearestPoint; + result->distance = d; + result->previousWayCoordinates = pathID; + result->percentage = percentage; + found = true; + } + } + + if ( found ) { + result->source = i->source; + result->target = i->target; + result->edgeID = i->edgeID; + path->clear(); + for ( int pathID = 0; pathID < i->pathLength; pathID++ ) + path->push_back( cell->coordinates[pathID + i->pathID] ); + } + } + + return true; +} + +double GPSGridClient::distance( UnsignedCoordinate* nearestPoint, double* percentage, const UnsignedCoordinate source, const UnsignedCoordinate target, const UnsignedCoordinate& coordinate ) { + const double vY = ( double ) target.y - source.y; + const double vX = ( double ) target.x - source.x; + const double wY = ( double ) coordinate.y - source.y; + const double wX = ( double ) coordinate.x - source.x; + const double vLengthSquared = vY * vY + vX * vX; + + double r = 0; + if ( vLengthSquared != 0 ) + r = ( vX * wX + vY * wY ) / vLengthSquared; + *percentage = r; + + if ( r <= 0 ) { + *nearestPoint = source; + *percentage = 0; + return wY * wY + wX * wX; + } else if ( r >= 1 ) { + *nearestPoint = target; + *percentage = 1; + const double dY = ( double ) coordinate.y - target.y; + const double dX = ( double ) coordinate.x - target.x; + return dY * dY + dX * dX; + } + + nearestPoint->x = source.x + r * vX; + nearestPoint->y = source.y + r * vY; + + const double dX = ( double ) nearestPoint->x - coordinate.x; + const double dY = ( double ) nearestPoint->y - coordinate.y; + + return dY * dY + dX * dX; +} + +double GPSGridClient::distance( const UnsignedCoordinate& min, const UnsignedCoordinate& max, const UnsignedCoordinate& coordinate ) { + UnsignedCoordinate nearest = coordinate; + + if ( coordinate.x <= min.x ) + nearest.x = min.x; + else if ( coordinate.x >= max.x ) + nearest.x = max.x; + + if ( coordinate.y <= min.y ) + nearest.y = min.y; + else if ( coordinate.y >= max.y ) + nearest.y = max.y; + + double xDiff = ( double ) coordinate.x - nearest.x; + double yDiff = ( double ) coordinate.y - nearest.y; + + return xDiff * xDiff + yDiff * yDiff; +} + +Q_EXPORT_PLUGIN2(gpsgridclient, GPSGridClient) diff --git a/monav/gpsgrid/gpsgridclient.h b/monav/gpsgrid/gpsgridclient.h new file mode 100644 index 0000000..0f80cc0 --- /dev/null +++ b/monav/gpsgrid/gpsgridclient.h @@ -0,0 +1,62 @@ +/* +Copyright 2010 Christian Vetter veaac.fdirct@gmail.com + +This file is part of MoNav. + +MoNav is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +MoNav 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 General Public License +along with MoNav. If not, see . +*/ + +#ifndef GPSGRIDCLIENT_H +#define GPSGRIDCLIENT_H + +#include +#include +#include "interfaces/igpslookup.h" +#include "cell.h" +#include "table.h" +#include + +class GPSGridClient : public QObject, public IGPSLookup +{ + Q_OBJECT + Q_INTERFACES( IGPSLookup ) +public: + GPSGridClient(); + virtual ~GPSGridClient(); + + virtual QString GetName(); + virtual void SetInputDirectory( const QString& dir ); + virtual void ShowSettings(); + virtual bool LoadData(); + virtual bool GetNearestEdge( Result* result, const UnsignedCoordinate& coordinate, double radius, bool headingPenalty, double heading ); + +signals: + +public slots: + +protected: + + void unload(); + double distance( UnsignedCoordinate* nearestPoint, double* percentage, const UnsignedCoordinate source, const UnsignedCoordinate target, const UnsignedCoordinate& coordinate ); + double distance( const UnsignedCoordinate& min, const UnsignedCoordinate& max, const UnsignedCoordinate& coordinate ); + bool checkCell( Result* result, QVector< UnsignedCoordinate >* path, NodeID gridX, NodeID gridY, const UnsignedCoordinate& coordinate, double heading, double headingPenalty ); + + long long cacheSize; + QString directory; + QFile* gridFile; + QCache< qint64, gg::Cell > cache; + gg::Index* index; +}; + +#endif // GPSGRIDCLIENT_H diff --git a/monav/gpsgrid/gpsgridclient.pro b/monav/gpsgrid/gpsgridclient.pro new file mode 100644 index 0000000..4d26c13 --- /dev/null +++ b/monav/gpsgrid/gpsgridclient.pro @@ -0,0 +1,31 @@ +#------------------------------------------------- +# +# Project created by QtCreator 2010-06-25T08:48:06 +# +#------------------------------------------------- + +TEMPLATE = lib +CONFIG += plugin +#CONFIG += debug + +DESTDIR = .. + +HEADERS += \ + utils/coordinates.h \ + utils/config.h \ + cell.h \ + interfaces/igpslookup.h \ + gpsgridclient.h \ + table.h \ + utils/bithelpers.h \ + utils/qthelpers.h + +unix { + QMAKE_CXXFLAGS_RELEASE -= -O2 + QMAKE_CXXFLAGS_RELEASE += -O3 \ + -Wno-unused-function + QMAKE_CXXFLAGS_DEBUG += -Wno-unused-function +} + +SOURCES += \ + gpsgridclient.cpp diff --git a/monav/gpsgrid/interfaces b/monav/gpsgrid/interfaces new file mode 120000 index 0000000..5e17f8d --- /dev/null +++ b/monav/gpsgrid/interfaces @@ -0,0 +1 @@ +../interfaces/ \ No newline at end of file diff --git a/monav/gpsgrid/table.h b/monav/gpsgrid/table.h new file mode 100644 index 0000000..7850264 --- /dev/null +++ b/monav/gpsgrid/table.h @@ -0,0 +1,224 @@ +/* +Copyright 2010 Christian Vetter veaac.fdirct@gmail.com + +This file is part of MoNav. + +MoNav is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +MoNav 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 General Public License +along with MoNav. If not, see . +*/ + +#ifndef TABLE_H +#define TABLE_H + +#include +#include +#include +#include +#include +#include + +namespace gg { + + template< class T, int size > + struct IndexTable { + T index[size * size]; + IndexTable() + { + for ( int i = 0; i < size * size; i++ ) + index[i] = -1; + } + IndexTable( const char* buffer ) + { + Read( buffer ); + } + + T GetIndex( int x, int y ) const + { + if ( x < 0 || x >= 32 ) + return -1; + if ( y < 0 || y >= 32 ) + return -1; + return index[x + y * size]; + } + + void SetIndex( int x, int y, T data ) + { + assert( x >= 0 ); + assert( x < 32 ); + assert( y >= 0 ); + assert( y < 32 ); + assert( index[x + y *size] == -1 ); + index[x + y * size] = data; + } + + static size_t Size() + { + return size * size * sizeof( T ); + } + + void Write( char* buffer ) const + { + memcpy( buffer, index, size * size * sizeof( T ) ); + } + + void Read( const char* buffer ) + { + memcpy( index, buffer, size * size * sizeof( T ) ); + } + + void Debug() + { + for ( int i = 0; i < size; i++ ) { + QString row; + for ( int j = 0; j < size; j++) + row += GetIndex( i, j ) == -1 ? "." : "#"; + qDebug() << row; + } + } + }; + + struct GridIndex { + qint64 position; + int x; + int y; + }; + + class Index { + + public: + Index( QString filename ) : + file2( filename + "_2" ), file3( filename + "_3" ) + { + QFile file1( filename + "_1" ); + file1.open( QIODevice::ReadOnly ); + top.Read( file1.read( top.Size() ).constData() ); + file2.open( QIODevice::ReadOnly ); + file3.open( QIODevice::ReadOnly ); + } + + qint64 GetIndex( int x, int y ) { + int topx = x / 32 / 32; + int topy = y / 32 / 32; + int middle = top.GetIndex( topx, topy ); + if ( middle == -1 ) + return -1; + + int middlex = ( x / 32 ) % 32; + int middley = ( y / 32 ) % 32; + if ( !cache2.contains( middle ) ) { + IndexTable< int, 32 >* newEntry; + file2.seek( middle * newEntry->Size() ); + newEntry = new IndexTable< int, 32 >( file2.read( newEntry->Size() ) ); + cache2.insert( middle, newEntry, newEntry->Size() ); + } + if ( !cache2.contains( middle ) ) + return -1; + IndexTable< int, 32 >* middleTable = cache2.object( middle ); + int bottom = middleTable->GetIndex( middlex, middley ); + if ( bottom == -1 ) + return -1; + + int bottomx = x % 32; + int bottomy = y % 32; + if ( !cache3.contains( bottom ) ) { + IndexTable< qint64, 32 >* newEntry; + file3.seek( bottom * newEntry->Size() ); + newEntry = new IndexTable< qint64, 32 >( file3.read( newEntry->Size() ) ); + cache3.insert( bottom, newEntry, newEntry->Size() ); + } + if ( !cache3.contains( bottom ) ) + return -1; + IndexTable< qint64, 32 >* bottomTable = cache3.object( bottom ); + qint64 position = bottomTable->GetIndex( bottomx, bottomy ); + return position; + } + + void SetCacheSize( long long size ) + { + assert( size > 0 ); + cache2.setMaxCost( size / 4 ); + cache3.setMaxCost( 3 * size / 4 ); + } + + static void Create( QString filename, const std::vector< GridIndex >& data ) + { + gg::IndexTable< int, 32 > top; + std::vector< gg::IndexTable< int, 32 > > middle; + std::vector< gg::IndexTable< qint64, 32 > > bottom; + + for ( std::vector< GridIndex >::const_iterator i = data.begin(), iend = data.end(); i != iend; i++ ) { + int topx = i->x / 32 / 32; + int topy = i->y / 32 / 32; + int middleIndex = top.GetIndex( topx, topy ); + if ( middleIndex == -1 ) { + middleIndex = middle.size(); + top.SetIndex( topx, topy, middleIndex ); + middle.push_back( gg::IndexTable< int, 32 >() ); + } + + int middlex = ( i->x / 32 ) % 32; + int middley = ( i->y / 32 ) % 32; + int bottomIndex = middle[middleIndex].GetIndex( middlex, middley ); + if ( bottomIndex == -1 ) { + bottomIndex = bottom.size(); + middle[middleIndex].SetIndex( middlex, middley, bottomIndex ); + bottom.push_back( IndexTable< qint64, 32 >() ); + } + + int bottomx = i->x % 32; + int bottomy = i->y % 32; + bottom[bottomIndex].SetIndex( bottomx, bottomy, i->position ); + } + + qDebug() << "GPS Grid: top index filled: " << ( double ) middle.size() * 100 / 32 / 32 << "%"; + qDebug() << "GPS Grid: middle tables: " << middle.size(); + qDebug() << "GPS Grid: middle index filled: " << ( double ) bottom.size() * 100 / middle.size() / 32 / 32 << "%"; + qDebug() << "GPS Grid: bottom tables: " << bottom.size(); + qDebug() << "GPS Grid: bottom index filled: " << ( double ) data.size() * 100 / bottom.size() / 32 / 32 << "%"; + qDebug() << "GPS Grid: grid cells: " << data.size(); + + QFile file1( filename + "_1" ); + QFile file2( filename + "_2" ); + QFile file3( filename + "_3" ); + file1.open( QIODevice::WriteOnly ); + file2.open( QIODevice::WriteOnly ); + file3.open( QIODevice::WriteOnly ); + + char* buffer = new char[IndexTable< qint64, 32 >::Size()]; + + top.Write( buffer ); + file1.write( buffer, IndexTable< int, 32 >::Size() ); + + for ( int i = 0; i < ( int ) middle.size(); i++ ) { + middle[i].Write( buffer ); + file2.write( buffer, IndexTable< int, 32 >::Size() ); + } + + for ( int i = 0; i < ( int ) bottom.size(); i++ ) { + bottom[i].Write( buffer ); + file3.write( buffer, IndexTable< qint64, 32 >::Size() ); + } + + delete[] buffer; + } + + private: + QFile file2; + QFile file3; + IndexTable< int, 32 > top; + QCache< int, IndexTable< int, 32 > > cache2; + QCache< int, IndexTable< qint64, 32 > > cache3; + }; +} + +#endif // TABLE_H diff --git a/monav/gpsgrid/utils b/monav/gpsgrid/utils new file mode 120000 index 0000000..19985ba --- /dev/null +++ b/monav/gpsgrid/utils @@ -0,0 +1 @@ +../utils/ \ No newline at end of file diff --git a/monav/interfaces/iaddresslookup.h b/monav/interfaces/iaddresslookup.h new file mode 100644 index 0000000..f2c3b91 --- /dev/null +++ b/monav/interfaces/iaddresslookup.h @@ -0,0 +1,50 @@ +/* +Copyright 2010 Christian Vetter veaac.fdirct@gmail.com + +This file is part of MoNav. + +MoNav is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +MoNav 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 General Public License +along with MoNav. If not, see . +*/ + +#ifndef IADDRESSLOOKUP_H +#define IADDRESSLOOKUP_H + +#include "utils/coordinates.h" +#include +#include + +class IAddressLookup +{ +public: + virtual ~IAddressLookup() {} + + virtual QString GetName() = 0; + virtual void SetInputDirectory( const QString& dir ) = 0; + virtual void ShowSettings() = 0; + virtual bool LoadData() = 0; + // for a given user input's prefix get a list of place name suggestions as well as partial input suggestions + virtual bool GetPlaceSuggestions( const QString& input, int amount, QStringList* suggestions, QStringList* inputSuggestions ) = 0; + // for a given user input's prefix get a list of street name suggestions as well as partial input suggestions + virtual bool GetStreetSuggestions( const QString& input, int amount, QStringList* suggestions, QStringList* inputSuggestions ) = 0; + // for a given place name get a list of places and their coordinates + virtual bool GetPlaceData( QString input, QVector< int >* placeIDs, QVector< UnsignedCoordinate >* placeCoordinates ) = 0; + // selects a place by it's id + virtual bool SelectPlace( int placeID ) = 0; + // uses the selected place to provide street name suggestions and partial input suggestions + virtual bool GetStreetData( QString input, QVector< int >* segmentLength, QVector< UnsignedCoordinate >* coordinates ) = 0; +}; + +Q_DECLARE_INTERFACE( IAddressLookup, "monav.IAddressLookup/1.1" ) + +#endif // IADDRESSLOOKUP_H diff --git a/monav/interfaces/igpslookup.h b/monav/interfaces/igpslookup.h new file mode 100644 index 0000000..ad30761 --- /dev/null +++ b/monav/interfaces/igpslookup.h @@ -0,0 +1,60 @@ +/* +Copyright 2010 Christian Vetter veaac.fdirct@gmail.com + +This file is part of MoNav. + +MoNav is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +MoNav 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 General Public License +along with MoNav. If not, see . +*/ + +#ifndef IGPSLOOKUP_H +#define IGPSLOOKUP_H + +#include "utils/config.h" +#include "utils/coordinates.h" +#include +#include + +class IGPSLookup +{ +public: + + struct Result { + // source + target + edgeID uniquely identify an edge + NodeID source; + NodeID target; + unsigned edgeID; + // the nearest point on the edge + UnsignedCoordinate nearestPoint; + // the amount of way coordinates on the way before the nearest point + unsigned previousWayCoordinates; + // the position on the way + double percentage; + // the distance to the nearest point + double distance; + + }; + + virtual ~IGPSLookup() {} + + virtual QString GetName() = 0; + virtual void SetInputDirectory( const QString& dir ) = 0; + virtual void ShowSettings() = 0; + virtual bool LoadData() = 0; + // gets the nearest routing edge; a heading penalty can be applied if the way's orientation differs greatly from the current heading. + virtual bool GetNearestEdge( Result* result, const UnsignedCoordinate& coordinate, double radius, bool headingPenalty = 0, double heading = 0 ) = 0; +}; + +Q_DECLARE_INTERFACE( IGPSLookup, "monav.IGPSLookup/1.1" ) + +#endif // IGPSLOOKUP_H diff --git a/monav/interfaces/irouter.h b/monav/interfaces/irouter.h new file mode 100644 index 0000000..ba266a3 --- /dev/null +++ b/monav/interfaces/irouter.h @@ -0,0 +1,80 @@ +/* +Copyright 2010 Christian Vetter veaac.fdirct@gmail.com + +This file is part of MoNav. + +MoNav is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +MoNav 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 General Public License +along with MoNav. If not, see . +*/ + +#ifndef IROUTER_H +#define IROUTER_H + +#include "utils/config.h" +#include "utils/coordinates.h" +#include "interfaces/igpslookup.h" +#include +#include + +class IRouter +{ +public: + + struct Node { + Node(){} + Node( UnsignedCoordinate coord ) + { + coordinate = coord; + } + + UnsignedCoordinate coordinate; + }; + + struct Edge { + Edge(){} + Edge( unsigned name_, bool branchingPossible_, unsigned char type_, unsigned short length_, unsigned seconds_ ) + { + name = name_; + branchingPossible = branchingPossible_; + type = type_; + length = length_; + seconds = seconds_; + } + unsigned name : 30; // name ID of the edge + bool branchingPossible : 1; // is there more than one subsequent edge to traverse ( turing around and traversing this edge in the opposite direction does not count ) + unsigned char type; // type ID of the edge + unsigned short length; // the amount of path nodes - 1 == amount of edges + unsigned seconds; + }; + + virtual ~IRouter() {} + + virtual QString GetName() = 0; + virtual void SetInputDirectory( const QString& dir ) = 0; + virtual void ShowSettings() = 0; + virtual bool LoadData() = 0; + // computes the route between source and target and returns the distance in second + virtual bool GetRoute( double* distance, QVector< Node>* pathNodes, QVector< Edge >* pathEdges, const IGPSLookup::Result& source, const IGPSLookup::Result& target ) = 0; + // translate a name ID into the corresponding string + virtual bool GetName( QString* result, unsigned name ) = 0; + // translate a list of name IDs into the corresponding strings + virtual bool GetNames( QVector< QString >* result, QVector< unsigned > names ) = 0; + // translate a type ID into the corresponding description + virtual bool GetType( QString* result, unsigned type ) = 0; + // translate a list of type IDs into the corresponding descriptions + virtual bool GetTypes( QVector< QString >* result, QVector< unsigned > types ) = 0; +}; + +Q_DECLARE_INTERFACE( IRouter, "monav.IRouter/1.1" ) + +#endif // IROUTER_H diff --git a/monav/monav.pro b/monav/monav.pro new file mode 100644 index 0000000..48151ab --- /dev/null +++ b/monav/monav.pro @@ -0,0 +1,7 @@ +TEMPLATE = subdirs + +#CONFIG += debug + +SUBDIRS = contractionhierarchies/contractionhierarchiesclient.pro \ + gpsgrid/gpsgridclient.pro \ + unicodetournamenttrie/unicodetournamenttrieclient.pro diff --git a/monav/unicodetournamenttrie/interfaces b/monav/unicodetournamenttrie/interfaces new file mode 120000 index 0000000..5e17f8d --- /dev/null +++ b/monav/unicodetournamenttrie/interfaces @@ -0,0 +1 @@ +../interfaces/ \ No newline at end of file diff --git a/monav/unicodetournamenttrie/trie.h b/monav/unicodetournamenttrie/trie.h new file mode 100644 index 0000000..d3567f7 --- /dev/null +++ b/monav/unicodetournamenttrie/trie.h @@ -0,0 +1,188 @@ +/* +Copyright 2010 Christian Vetter veaac.fdirct@gmail.com + +This file is part of MoNav. + +MoNav is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +MoNav 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 General Public License +along with MoNav. If not, see . +*/ + +#ifndef TRIE_H +#define TRIE_H + +#include +#include +#include "utils/coordinates.h" +#include "utils/bithelpers.h" + +namespace utt +{ + +struct CityData { + UnsignedCoordinate coordinate; + + size_t GetSize() const { + return 2 * sizeof( unsigned ); + } + + void Write( char* buffer ) const { + *( ( unsigned* ) buffer ) = coordinate.x; + buffer += sizeof( unsigned ); + *( ( unsigned* ) buffer ) = coordinate.y; + } + + void Read( const char* buffer ) { + coordinate.x = readUnaligned< unsigned >( buffer ); + buffer += sizeof( unsigned ); + coordinate.y = readUnaligned< unsigned >( buffer ); + } +}; + +struct Data { + unsigned start; + unsigned short length; + + size_t GetSize() const { + return sizeof( unsigned ) + sizeof( unsigned short ); + } + + void Write( char* buffer ) const { + *( ( unsigned* ) buffer ) = start; + buffer += sizeof( unsigned ); + *( ( unsigned short* ) buffer ) = length; + } + + void Read( const char* buffer ) { + start = readUnaligned< unsigned >( buffer ); + buffer += sizeof( unsigned ); + length = readUnaligned< unsigned short >( buffer ); + } + + bool operator==( const Data& right ) const { + return start == right.start && length == right.length; + } +}; + +struct Label { + QString string; + unsigned index; + unsigned importance; + + bool operator<( const Label& right ) const { + if ( importance != right.importance ) + return importance > right.importance; + return string < right.string; + } + + bool operator==( const Label& right ) const { + return string == right.string && importance == right.importance; + } + + size_t GetSize() const { + size_t result = 0; + result += sizeof( unsigned ); + result += sizeof( unsigned ); + result += strlen( string.toUtf8().constData() ) + 1; + return result; + } + + void Write( char* buffer ) const { + *( ( unsigned* ) buffer ) = index; + buffer += sizeof( unsigned ); + *( ( unsigned* ) buffer ) = importance; + buffer += sizeof( unsigned ); + strcpy( buffer, string.toUtf8().constData() ); + } + + void Read( const char* buffer ) { + index = readUnaligned< unsigned >( buffer ); + buffer += sizeof( unsigned ); + importance = readUnaligned< unsigned >( buffer ); + buffer += sizeof( unsigned ); + string = QString::fromUtf8( buffer ); + } +}; + +struct Node { + std::vector< Data > dataList; + std::vector< Label > labelList; + + size_t GetSize() const { + size_t result = 0; + result += sizeof( short ); + if ( dataList.size() != 0 ) + result += sizeof( unsigned short ); + for ( int i = 0; i < ( int ) labelList.size(); i++ ) + result += labelList[i].GetSize(); + for ( int i = 0; i < ( int ) dataList.size(); i++ ) + result += dataList[i].GetSize(); + return result; + } + + void Write( char* buffer ) const { + assert( ( int ) labelList.size() <= std::numeric_limits< short >::max() ); + assert( dataList.size() <= std::numeric_limits< unsigned short >::max() ); + *( ( short* ) buffer ) = labelList.size() * ( dataList.size() > 0 ? -1 : 1 ); + buffer += sizeof( short ); + if ( dataList.size() > 0 ) { + *( ( unsigned short* ) buffer ) = dataList.size(); + buffer += sizeof( unsigned short ); + } + for ( int i = 0; i < ( int ) labelList.size(); i++ ) { + labelList[i].Write( buffer ); + buffer += labelList[i].GetSize(); + } + for ( int i = 0; i < ( int ) dataList.size(); i++ ) { + dataList[i].Write( buffer ); + buffer += dataList[i].GetSize(); + } + } + + void Read( const char* buffer ) { + short labelSize = readUnaligned< short >( buffer ); + labelList.resize( labelSize >= 0 ? labelSize : -labelSize ); + buffer += sizeof( unsigned short ); + if ( labelSize <= 0 ) { + dataList.resize( readUnaligned< unsigned short >( buffer ) ); + buffer += sizeof( unsigned short ); + } + for( int i = 0; i < ( int ) labelList.size(); i++ ) { + labelList[i].Read( buffer ); + buffer += labelList[i].GetSize(); + } + for( int i = 0; i < ( int ) dataList.size(); i++ ) { + dataList[i].Read( buffer ); + buffer += dataList[i].GetSize(); + } + } + + bool operator== ( const Node& right ) const { + if ( dataList.size() != right.dataList.size() ) + return false; + if ( labelList.size() != right.labelList.size() ) + return false; + for ( int i = 0; i < ( int ) dataList.size(); ++i ) { + if ( !( dataList[i] == right.dataList[i] ) ) + return false; + } + for ( int i = 0; i < ( int ) labelList.size(); ++i ) { + if ( !( labelList[i] == right.labelList[i] ) ) + return false; + } + return true; + } +}; + +} + +#endif // TRIE_H diff --git a/monav/unicodetournamenttrie/unicodetournamenttrieclient.cpp b/monav/unicodetournamenttrie/unicodetournamenttrieclient.cpp new file mode 100644 index 0000000..9a5822a --- /dev/null +++ b/monav/unicodetournamenttrie/unicodetournamenttrieclient.cpp @@ -0,0 +1,296 @@ +/* +Copyright 2010 Christian Vetter veaac.fdirct@gmail.com + +This file is part of MoNav. + +MoNav is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +MoNav 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 General Public License +along with MoNav. If not, see . +*/ + +#include "unicodetournamenttrieclient.h" +#include "utils/qthelpers.h" +#include +#include +#ifndef NOGUI + #include +#endif + +UnicodeTournamentTrieClient::UnicodeTournamentTrieClient() +{ + trieFile = NULL; + subTrieFile = NULL; + dataFile = NULL; + trieData = NULL; + subTrieData = NULL; + placeID = -1; +} + +UnicodeTournamentTrieClient::~UnicodeTournamentTrieClient() +{ + +} + +void UnicodeTournamentTrieClient::unload() +{ + if ( trieFile != NULL ) + delete trieFile; + trieFile = NULL; + if ( subTrieFile != NULL ) + delete subTrieFile; + subTrieFile = NULL; + if ( dataFile != NULL ) + delete dataFile; + dataFile = NULL; +} + +QString UnicodeTournamentTrieClient::GetName() +{ + return "Unicode Tournament Trie"; +} + +void UnicodeTournamentTrieClient::SetInputDirectory( const QString& dir ) +{ + directory = dir; +} + +void UnicodeTournamentTrieClient::ShowSettings() +{ +#ifndef NOGUI + QMessageBox::information( NULL, "Settings", "No settings available" ); +#endif +} + +bool UnicodeTournamentTrieClient::LoadData() +{ + unload(); + QString filename = fileInDirectory( directory, "Unicode Tournament Trie" ); + trieFile = new QFile( filename + "_main" ); + subTrieFile = new QFile( filename + "_sub" ); + dataFile = new QFile( filename + "_ways" ); + + if ( !openQFile( trieFile, QIODevice::ReadOnly ) ) + return false; + if ( !openQFile( subTrieFile, QIODevice::ReadOnly ) ) + return false; + if ( !openQFile( dataFile, QIODevice::ReadOnly ) ) + return false; + + trieData = ( char* ) trieFile->map( 0, trieFile->size() ); + subTrieData = ( char* ) subTrieFile->map( 0, subTrieFile->size() ); + + if ( trieData == NULL ) { + qDebug( "Failed to Memory Map trie data" ); + return false; + } + if ( subTrieData == NULL ) { + qDebug( "Failed to Memory Map sub trie data" ); + return false; + } + + return true; +} + +bool UnicodeTournamentTrieClient::find( const char* trie, unsigned* resultNode, QString* missingPrefix, QString prefix ) +{ + unsigned node = *resultNode; + for ( int i = 0; i < ( int ) prefix.length(); ) { + utt::Node element; + element.Read( trie + node ); + bool found = false; + for ( int c = 0; c < ( int ) element.labelList.size(); c++ ) { + const utt::Label& label = element.labelList[c]; + bool equal = true; + for ( int subIndex = 0; subIndex < ( int ) label.string.length(); ++subIndex ) { + if ( i + subIndex >= ( int ) prefix.length() ) { + *missingPrefix = label.string.mid( subIndex ); + break; + } + if ( label.string[subIndex] != prefix[i + subIndex] ) { + equal = false; + break; + } + } + if ( !equal ) + continue; + + i += label.string.length(); + node = label.index; + found = true; + break; + + } + if ( !found ) + return false; + } + + *resultNode = node; + return true; +} + +int UnicodeTournamentTrieClient::getSuggestion( const char* trie, QStringList* resultNames, unsigned node, int count, const QString prefix ) +{ + std::vector< Suggestion > candidates( 1 ); + candidates[0].index = node; + candidates[0].prefix = prefix; + candidates[0].importance = std::numeric_limits< unsigned >::max(); + + while( count > 0 && candidates.size() > 0 ) { + const Suggestion next = candidates[0]; + candidates[0] = candidates.back(); + candidates.pop_back(); + + utt::Node element; + element.Read( trie + next.index ); + bool isThis = true; + for ( std::vector< utt::Label >::const_iterator c = element.labelList.begin(), e = element.labelList.end(); c != e; ++c ) { + assert( c->importance <= next.importance ); + if ( c->importance == next.importance ) + isThis = false; + } + if ( isThis && element.dataList.size() > 0 ) { + assert( next.prefix.length() > 0 ); + QString suggestion = next.prefix[0].toUpper(); + for ( int i = 1; i < ( int ) next.prefix.length(); ++i ) { + if ( suggestion[i - 1] == ' ' || suggestion[i - 1] == '-' ) + suggestion += next.prefix[i].toUpper(); + else + suggestion += next.prefix[i]; + } + resultNames->push_back( suggestion ); + count--; + } + for ( std::vector< utt::Label >::const_iterator c = element.labelList.begin(), e = element.labelList.end(); c != e; ++c ) { + Suggestion nextEntry; + nextEntry.prefix = next.prefix + c->string; + nextEntry.index = c->index; + nextEntry.importance = c->importance; + candidates.push_back( nextEntry ); + } + std::sort( candidates.begin(), candidates.end() ); + if ( ( int ) candidates.size() > count ) + candidates.resize( count ); + } + + return count; +} + +bool UnicodeTournamentTrieClient::GetPlaceSuggestions( const QString& input, int amount, QStringList* suggestions, QStringList* inputSuggestions ) +{ + unsigned node = 0; + QString prefix; + QString name = input.toLower(); + + if ( !find( trieData, &node, &prefix, name ) ) + return false; + + if ( prefix.length() == 0 ) { + utt::Node element; + element.Read( trieData + node ); + for ( std::vector< utt::Label >::const_iterator c = element.labelList.begin(), e = element.labelList.end(); c != e; ++c ) + inputSuggestions->push_back( input + c->string ); + } + else { + inputSuggestions->push_back( input + prefix ); + } + getSuggestion( trieData, suggestions, node, amount, name + prefix ); + std::sort( inputSuggestions->begin(), inputSuggestions->end() ); + return true; +} + +bool UnicodeTournamentTrieClient::SelectPlace( int id ) +{ + placeID = id; + return true; +} + +bool UnicodeTournamentTrieClient::GetStreetSuggestions( const QString& input, int amount, QStringList* suggestions, QStringList* inputSuggestions ) +{ + if ( placeID < 0 ) + return false; + unsigned node = 0; + QString prefix; + QString name = input.toLower(); + + if ( !find( subTrieData + placeID, &node, &prefix, name ) ) + return false; + + if ( prefix.length() == 0 ) { + utt::Node element; + element.Read( subTrieData + placeID + node ); + for ( std::vector< utt::Label >::const_iterator c = element.labelList.begin(), e = element.labelList.end(); c != e; ++c ) + inputSuggestions->push_back( input + c->string ); + } + else { + inputSuggestions->push_back( input + prefix ); + } + getSuggestion( subTrieData + placeID, suggestions, node, amount, name + prefix ); + std::sort( inputSuggestions->begin(), inputSuggestions->end() ); + return true; +} + +bool UnicodeTournamentTrieClient::GetPlaceData( QString input, QVector< int >* placeIDs, QVector< UnsignedCoordinate >* placeCoordinates ) +{ + unsigned node = 0; + QString prefix; + QString name = input.toLower(); + if ( !find( trieData, &node, &prefix, name ) ) + return false; + + utt::Node element; + element.Read( trieData + node ); + + for ( std::vector< utt::Data >::const_iterator i = element.dataList.begin(), e = element.dataList.end(); i != e; ++i ) { + utt::CityData data; + data.Read( subTrieData + i->start ); + placeCoordinates->push_back( data.coordinate ); + placeIDs->push_back( i->start + data.GetSize() ); + } + + return placeIDs->size() != 0; +} + +bool UnicodeTournamentTrieClient::GetStreetData( QString input, QVector< int >* segmentLength, QVector< UnsignedCoordinate >* coordinates ) +{ + if ( placeID < 0 ) + return false; + unsigned node = 0; + QString prefix; + QString name = input.toLower(); + if ( !find( subTrieData + placeID, &node, &prefix, name ) ) + return false; + + utt::Node element; + element.Read( subTrieData + placeID + node ); + + for ( std::vector< utt::Data >::const_iterator i = element.dataList.begin(), e = element.dataList.end(); i != e; ++i ) { + unsigned* buffer = new unsigned[i->length * 2]; + dataFile->seek( i->start * sizeof( unsigned ) * 2 ); + dataFile->read( ( char* ) buffer, i->length * 2 * sizeof( unsigned ) ); + for ( unsigned start = 0; start < i->length; ++start ) { + UnsignedCoordinate temp; + temp.x = buffer[start * 2]; + temp.y = buffer[start * 2 + 1]; + coordinates->push_back( temp ); + } + delete[] buffer; + segmentLength->push_back( coordinates->size() ); + } + + return segmentLength->size() != 0; +} + +Q_EXPORT_PLUGIN2(unicodetournamenttrieclient, UnicodeTournamentTrieClient) diff --git a/monav/unicodetournamenttrie/unicodetournamenttrieclient.h b/monav/unicodetournamenttrie/unicodetournamenttrieclient.h new file mode 100644 index 0000000..e597044 --- /dev/null +++ b/monav/unicodetournamenttrie/unicodetournamenttrieclient.h @@ -0,0 +1,79 @@ +/* +Copyright 2010 Christian Vetter veaac.fdirct@gmail.com + +This file is part of MoNav. + +MoNav is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +MoNav 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 General Public License +along with MoNav. If not, see . +*/ + +#ifndef UNICODETOURNAMENTTRIECLIENT_H +#define UNICODETOURNAMENTTRIECLIENT_H + +#include +#include +#include +#include "interfaces/iaddresslookup.h" +#include "trie.h" + +class UnicodeTournamentTrieClient : public QObject, public IAddressLookup +{ + Q_OBJECT + Q_INTERFACES( IAddressLookup ) +public: + + explicit UnicodeTournamentTrieClient(); + ~UnicodeTournamentTrieClient(); + + virtual QString GetName(); + virtual void SetInputDirectory( const QString& dir ); + virtual void ShowSettings(); + virtual bool LoadData(); + virtual bool GetPlaceSuggestions( const QString& input, int amount, QStringList* suggestions, QStringList* inputSuggestions ); + virtual bool GetStreetSuggestions( const QString& input, int amount, QStringList* suggestions, QStringList* inputSuggestions ); + virtual bool SelectPlace( int placeID ); + virtual bool GetPlaceData( QString input, QVector< int >* placeIDs, QVector< UnsignedCoordinate >* placeCoordinates ); + virtual bool GetStreetData( QString input, QVector< int >* segmentLength, QVector< UnsignedCoordinate >* coordinates ); + +signals: + +public slots: + +protected: + + struct Suggestion { + unsigned importance; + unsigned index; + QString prefix; + + bool operator<( const Suggestion& right ) const { + return importance > right.importance; + } + }; + + void unload(); + bool find( const char* trie, unsigned* resultNode, QString* missingPrefix, QString prefix ); + int getSuggestion( const char* trie, QStringList* resultNames, unsigned node, int count, const QString prefix ); + + QString directory; + QFile* trieFile; + QFile* subTrieFile; + QFile* dataFile; + const char* trieData; + const char* subTrieData; + + int placeID; + +}; + +#endif // UNICODETOURNAMENTTRIECLIENT_H diff --git a/monav/unicodetournamenttrie/unicodetournamenttrieclient.pro b/monav/unicodetournamenttrie/unicodetournamenttrieclient.pro new file mode 100644 index 0000000..e60d838 --- /dev/null +++ b/monav/unicodetournamenttrie/unicodetournamenttrieclient.pro @@ -0,0 +1,25 @@ +#------------------------------------------------- +# +# Project created by QtCreator 2010-06-22T13:51:45 +# +#------------------------------------------------- + +DESTDIR = .. +TEMPLATE = lib +CONFIG += plugin + +HEADERS += utils/coordinates.h \ + utils/config.h \ + interfaces/iaddresslookup.h \ + trie.h \ + unicodetournamenttrieclient.h \ + utils/qthelpers.h + +unix { + QMAKE_CXXFLAGS_RELEASE -= -O2 + QMAKE_CXXFLAGS_RELEASE += -O3 -Wno-unused-function + QMAKE_CXXFLAGS_DEBUG += -Wno-unused-function +} + +SOURCES += \ + unicodetournamenttrieclient.cpp diff --git a/monav/unicodetournamenttrie/utils b/monav/unicodetournamenttrie/utils new file mode 120000 index 0000000..19985ba --- /dev/null +++ b/monav/unicodetournamenttrie/utils @@ -0,0 +1 @@ +../utils/ \ No newline at end of file diff --git a/monav/utils/bithelpers.h b/monav/utils/bithelpers.h new file mode 100644 index 0000000..3663931 --- /dev/null +++ b/monav/utils/bithelpers.h @@ -0,0 +1,203 @@ +/* +Copyright 2010 Christian Vetter veaac.fdirct@gmail.com + +This file is part of MoNav. + +MoNav is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +MoNav 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 General Public License +along with MoNav. If not, see . +*/ + +#ifndef BITHELPERS_H +#define BITHELPERS_H + +#include +#include + +template< class T > +static inline T readUnaligned( const char* buffer ) { + T temp; + memcpy( &temp, buffer, sizeof( T ) ); + return temp; +} + +// reads first bits to a max of 31 bits ( 31 because 1u << 32 is undefined ) +// offset has to be <8 +// safe with unaligned memory access +static inline unsigned read_unaligned_unsigned( const unsigned char* buffer, int offset ){ + assert ( offset <= 7 ); + + const int diff = ( ( size_t ) buffer ) & 3; + buffer -= diff; + offset += 8 * diff; + + unsigned temp = * ( unsigned * ) buffer; + if ( offset == 0 ) + return temp; + unsigned temp2 = * ( ( ( unsigned * ) buffer ) + 1); + return ( temp >> offset ) | ( temp2 << ( 32 - offset ) ); +} + +static inline unsigned read_unaligned_unsigned( const unsigned char** buffer, int bits, int* offset ){ + assert ( *offset <= 7 ); + + const int diff = ( ( size_t ) *buffer ) & 3; + const unsigned char* alignedBuffer = *buffer - diff; + int alignedOffset = *offset + 8 * diff; + unsigned temp = * ( unsigned * ) alignedBuffer; + unsigned temp2 = * ( ( ( unsigned * ) alignedBuffer ) + 1); + unsigned result; + if ( alignedOffset == 0 ) + result = temp; + else + result = ( temp >> alignedOffset ) | ( temp2 << ( 32 - alignedOffset ) ); + + *offset += bits; + *buffer += ( *offset ) >> 3; + *offset &= 7; + + if ( bits == 32 ) + return result; + + return result & ( ( 1u << bits ) - 1 ); +} + +static inline unsigned read_unaligned_unsigned( const unsigned char* buffer, int bits, int offset ){ + assert ( offset <= 7 ); + + const int diff = ( ( size_t ) buffer ) & 3; + const unsigned char* alignedBuffer = buffer - diff; + int alignedOffset = offset + 8 * diff; + unsigned temp = * ( unsigned * ) alignedBuffer; + unsigned temp2 = * ( ( ( unsigned * ) alignedBuffer ) + 1); + unsigned result; + if ( alignedOffset == 0 ) + result = temp; + else + result = ( temp >> alignedOffset ) | ( temp2 << ( 32 - alignedOffset ) ); + + if ( bits == 32 ) + return result; + + return result & ( ( 1u << bits ) - 1 ); +} + +// writes #bits bits of data into the buffer at the offset +// offset has to be <8, **buffer has to be zeroed +// modifies buffer and offset to point after the inserted data +static inline void write_unaligned_unsigned( unsigned char** buffer, unsigned data, int bits, int* offset ) { + ( ( unsigned* ) *buffer )[0] |= ( data << ( *offset ) ); + ( ( unsigned* ) ( *buffer + 1 ) )[0] |= ( data >> ( 8 - *offset ) ); + +#ifndef NDEBUG + const unsigned char* tempBuffer = *buffer; + int tempOffset = *offset; + unsigned tempData = read_unaligned_unsigned( &tempBuffer, bits, &tempOffset ); + assert( tempData == data ); +#endif + + *offset += bits; + *buffer += ( *offset ) >> 3; + *offset &= 7; +} + +static inline unsigned read_bits ( unsigned data, char bits ) { + if ( bits == 32 ) + return data; + + return data & ( ( 1u << bits ) - 1 ); +} + +static inline unsigned log2_rounded ( unsigned x ) { + static const unsigned bit_position[32] = { + 0, 1, 28, 2, 29, 14, 24, 3, 30, 22, 20, 15, 25, 17, 4, 8, + 31, 27, 13, 23, 21, 19, 16, 7, 26, 12, 18, 6, 11, 5, 10, 9 + }; + + //round up + --x; + x |= x >> 1; + x |= x >> 2; + x |= x >> 4; + x |= x >> 8; + x |= x >> 16; + ++x; + + return bit_position[ ( x * 0x077CB531u ) >> 27]; +} + +// computes log2_rounded( x + 1 ), works even up to x = 2^32 - 1 +static inline unsigned bits_needed( unsigned x ) +{ + static const unsigned bit_position[32] = { + 32, 1, 28, 2, 29, 14, 24, 3, 30, 22, 20, 15, 25, 17, 4, 8, + 31, 27, 13, 23, 21, 19, 16, 7, 26, 12, 18, 6, 11, 5, 10, 9 + }; + //slower, maybe think of a better workaround + if ( x == 0 ) + return 0; + + //+1 and round up + x |= x >> 1; + x |= x >> 2; + x |= x >> 4; + x |= x >> 8; + x |= x >> 16; + ++x; + + return bit_position[ ( x * 0x077CB531u ) >> 27]; +} + +template< int exponentBits, int significantBits > +static unsigned decode_integer( unsigned x ) +{ + if ( x == ( ( 1u << ( exponentBits + significantBits ) ) - 1 ) ) + return 0; + unsigned exponent = x >> significantBits; + unsigned significant = x & ( ( 1u << significantBits ) - 1 ); + significant = ( significant << 1 ) | 1; // implicit 1 + return significant << exponent; +} + +template< int exponentBits, int significantBits > +static unsigned encode_integer( unsigned x ) +{ + assert ( exponentBits > 0 ); + assert( significantBits > 0 ); + + static bool initialized = false; + static const unsigned numEncoded = 1u << ( exponentBits + significantBits ); + typedef std::pair< unsigned, unsigned > Lookup; + static Lookup lookup[numEncoded]; + + if ( !initialized ) { + for ( unsigned value = 0; value < numEncoded; value++ ) + lookup[value] = Lookup( decode_integer< exponentBits, significantBits >( value ), value ); + std::sort( lookup, lookup + numEncoded ); + initialized = true; + } + + Lookup* value = std::lower_bound( lookup, lookup + numEncoded, Lookup( x, 0 ) ); + + if ( value >= lookup + numEncoded - 1 ) + return lookup[numEncoded - 1].second; + + unsigned diffFirst = x - value->first; + unsigned diffSecond = ( value + 1 )->first - x; + + if ( diffFirst < diffSecond ) + return value->second; + else + return ( value + 1 )->second; +} + +#endif // BITHELPERS_H diff --git a/monav/utils/config.h b/monav/utils/config.h new file mode 100644 index 0000000..9261ce5 --- /dev/null +++ b/monav/utils/config.h @@ -0,0 +1,25 @@ +/* +Copyright 2010 Christian Vetter veaac.fdirct@gmail.com + +This file is part of MoNav. + +MoNav is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +MoNav 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 General Public License +along with MoNav. If not, see . +*/ + +#ifndef CONFIG_H_INCLUDED +#define CONFIG_H_INCLUDED + +typedef unsigned NodeID; + +#endif // CONFIG_H_INCLUDED diff --git a/monav/utils/coordinates.h b/monav/utils/coordinates.h new file mode 100644 index 0000000..59f8cd5 --- /dev/null +++ b/monav/utils/coordinates.h @@ -0,0 +1,289 @@ +/* +Copyright 2010 Christian Vetter veaac.fdirct@gmail.com + +This file is part of MoNav. + +MoNav is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +MoNav 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 General Public License +along with MoNav. If not, see . +*/ + +#ifndef COORDINATES_H_INCLUDED +#define COORDINATES_H_INCLUDED + +#include +#include +#include +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +class GPSCoordinate { +public: + + GPSCoordinate() + { + latitude = longitude = std::numeric_limits< double >::max(); + } + GPSCoordinate( double lat, double lon ) + { + latitude = lat; + longitude = lon; + } + + double Distance( const GPSCoordinate &right ) const + { + assert( fabs( latitude ) < 90 && fabs( right.latitude ) < 90 ); + //assert ( nicht antipodal, nicht an den Polen ) + + // convert inputs in degrees to radians: + static const double DEG_TO_RAD = 0.017453292519943295769236907684886; + double lat1 = latitude * DEG_TO_RAD; + double lon1 = longitude * DEG_TO_RAD; + double lat2 = right.latitude * DEG_TO_RAD; + double lon2 = right.longitude * DEG_TO_RAD; + + static const double a = 6378137; + static const double b = 6356752.31424518; + static const double f = ( a - b ) / a; + + const double U1 = atan(( 1 - f ) * tan( lat1 ) ); + const double U2 = atan(( 1 - f ) * tan( lat2 ) ); + const double cosU1 = cos( U1 ); + const double cosU2 = cos( U2 ); + const double sinU1 = sin( U1 ); + const double sinU2 = sin( U2 ); + const double L = fabs( lon2 - lon1 ); + + double lambda = L; + double lambdaOld; + unsigned iterLimit = 50; + while ( true ) { + const double cosLambda = cos( lambda ); + const double sinLambda = sin( lambda ); + const double leftSinSigma = cosU2 * sinLambda; + const double rightSinSigma = cosU1 * sinU2 - sinU1 * cosU2 * cosLambda; + const double sinSigma = sqrt( leftSinSigma * leftSinSigma + rightSinSigma * rightSinSigma ); + if ( sinSigma == 0 ) // Fast identisch + return 0; + const double cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda; + const double sigma = atan2( sinSigma, cosSigma ); + const double sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma; + const double cosSquareAlpha = 1 - sinAlpha * sinAlpha; + double cos2sigmam = cosSigma - 2 * sinU1 * sinU2 / cosSquareAlpha; + if ( cos2sigmam != cos2sigmam ) // NAN: Parellel zum Äquator + cos2sigmam = 0; + const double C = f / 16 * cosSquareAlpha * ( 4 + f * ( 4 - 3 * cosSquareAlpha ) ); + lambdaOld = lambda; + lambda = L + ( 1 - C ) * f * sinAlpha * ( sigma + C * sinSigma * ( cos2sigmam + C * cosSigma * ( -1 + 2 * cos2sigmam * cos2sigmam ) ) ); + if ( fabs( lambda - lambdaOld ) < 1e-12 || --iterLimit == 0 ) { + const double u2 = cosSquareAlpha * ( a * a - b * b ) / ( b * b ); + const double A = 1 + u2 / 16384 * ( 4096 + u2 * ( -768 + u2 * ( 320 - 175 * u2 ) ) ); + const double B = u2 / 1024 * ( 256 + u2 * ( -128 + u2 * ( 74 - 47 * u2 ) ) ); + const double deltasigma = B * sinSigma * ( cos2sigmam + B / 4 * ( cosSigma * ( -1 + 2 * cos2sigmam * cos2sigmam ) - B / 6 * cos2sigmam * ( -3 + 4 * sinSigma * sinSigma ) * ( -3 + 4 * cos2sigmam * cos2sigmam ) ) ); + return b * A * ( sigma - deltasigma ); + } + } + + //should never be reached + return 0; + } + + double ApproximateDistance( const GPSCoordinate &right ) const + { + static const double DEG_TO_RAD = 0.017453292519943295769236907684886; + ///Earth's quatratic mean radius for WGS-84 + static const double EARTH_RADIUS_IN_METERS = 6372797.560856; + double latitudeArc = ( latitude - right.latitude ) * DEG_TO_RAD; + double longitudeArc = ( longitude - right.longitude ) * DEG_TO_RAD; + double latitudeH = sin( latitudeArc * 0.5 ); + latitudeH *= latitudeH; + double lontitudeH = sin( longitudeArc * 0.5 ); + lontitudeH *= lontitudeH; + double tmp = cos( latitude * DEG_TO_RAD ) * cos( right.latitude * DEG_TO_RAD ); + double distanceArc = 2.0 * asin( sqrt( latitudeH + tmp * lontitudeH ) ); + return EARTH_RADIUS_IN_METERS * distanceArc; + } + + bool operator==( const GPSCoordinate& right ) const + { + return latitude == right.latitude && longitude == right.longitude; + } + + bool operator!=( const GPSCoordinate& right ) const + { + return !( *this == right ); + } + + bool operator<( const GPSCoordinate& right ) const + { + if ( latitude != right.latitude ) + return latitude < right.latitude; + return longitude < right.longitude; + } + + bool IsValid() const + { + return latitude != std::numeric_limits< double >::max() && longitude != std::numeric_limits< double >::max(); + } + + double latitude, longitude; +}; + +class ProjectedCoordinate { +public: + + ProjectedCoordinate() + { + x = y = std::numeric_limits< double >::max(); + } + + ProjectedCoordinate( double xVal, double yVal ) + { + x = xVal; + y = yVal; + } + + ProjectedCoordinate( double xVal, double yVal, int zoom ) { + x = xVal / ( 1u << zoom ); + y = yVal / ( 1u << zoom ); + } + + explicit ProjectedCoordinate( const GPSCoordinate& gps ) + { + x = ( gps.longitude + 180.0 ) / 360.0; + y = ( 1.0 - log( tan( gps.latitude * M_PI / 180.0 ) + 1.0 / cos( gps.latitude * M_PI / 180.0 ) ) / M_PI ) / 2.0; + } + + GPSCoordinate ToGPSCoordinate() const + { + GPSCoordinate gps; + gps.longitude = x * 360.0 - 180; + const double n = M_PI - 2.0 * M_PI * y; + gps.latitude = 180.0 / M_PI * atan( 0.5 * ( exp( n ) - exp( -n ) ) ); + return gps; + } + + bool operator==( const ProjectedCoordinate& right ) const + { + return x == right.x && y == right.y; + } + + bool operator!=( const ProjectedCoordinate& right ) const + { + return !( *this == right ); + } + + bool operator<( const ProjectedCoordinate& right ) const + { + if ( x != right.x ) + return x < right.x; + return y < right.y; + } + + bool IsValid() const + { + return x != std::numeric_limits< double >::max() && y != std::numeric_limits< double >::max(); + } + + double x, y; +}; + +class UnsignedCoordinate { +public: + + UnsignedCoordinate() + { + x = y = std::numeric_limits< unsigned >::max(); + } + + UnsignedCoordinate( unsigned xVal, unsigned yVal ) + { + x = xVal; + y = yVal; + } + + explicit UnsignedCoordinate( ProjectedCoordinate tile ) + { + x = floor( tile.x * ( 1u << 30 ) ); + y = floor( tile.y * ( 1u << 30 ) ); + } + + explicit UnsignedCoordinate( GPSCoordinate gps ) + { + *this = UnsignedCoordinate( ProjectedCoordinate( gps ) ); + } + + GPSCoordinate ToGPSCoordinate() const + { + return ToProjectedCoordinate().ToGPSCoordinate(); + } + + ProjectedCoordinate ToProjectedCoordinate() const + { + ProjectedCoordinate tile; + tile.x = x; + tile.y = y; + tile.x /= ( 1u << 30 ); + tile.y /= ( 1u << 30 ); + return tile; + } + + unsigned GetTileX( int zoom ) const { + if ( zoom == 0 ) + return 0; + return x >> ( 30 - zoom ); + } + unsigned GetTileY( int zoom ) const { + if ( zoom == 0 ) + return 0; + return y >> ( 30 - zoom ); + } + unsigned GetTileSubX( int zoom, int precision ) const { + assert( zoom + precision < 31 ); + assert( zoom + precision > 0 ); + const unsigned subX = ( x << zoom ) >> zoom; + return subX >> ( 30 - precision - zoom ); + } + unsigned GetTileSubY( int zoom, int precision ) const { + assert( zoom + precision < 31 ); + assert( zoom + precision > 0 ); + const unsigned subY = ( y << zoom ) >> zoom; + return subY >> ( 30 - precision - zoom ); + } + + bool operator==( const UnsignedCoordinate& right ) const + { + return x == right.x && y == right.y; + } + + bool operator!=( const UnsignedCoordinate& right ) const + { + return !( *this == right ); + } + + bool operator<( const UnsignedCoordinate& right ) const + { + if ( x != right.x ) + return x < right.x; + return y < right.y; + } + + bool IsValid() const + { + return x != std::numeric_limits< unsigned >::max() && y != std::numeric_limits< unsigned >::max(); + } + + unsigned x, y; +}; + +#endif // COORDINATES_H_INCLUDED diff --git a/monav/utils/edgeconnector.h b/monav/utils/edgeconnector.h new file mode 100644 index 0000000..61aa4f2 --- /dev/null +++ b/monav/utils/edgeconnector.h @@ -0,0 +1,135 @@ +/* +Copyright 2010 Christian Vetter veaac.fdirct@gmail.com + +This file is part of MoNav. + +MoNav is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +MoNav 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 General Public License +along with MoNav. If not, see . +*/ + +#ifndef EDGECONNECTOR_H +#define EDGECONNECTOR_H + +#include "utils/coordinates.h" +#include +#include +#include + +static uint qHash( const UnsignedCoordinate& key ) +{ + return qHash( key.x ) ^ qHash( key.y ); +} + +template< class Node > +class EdgeConnector +{ + +public: + + struct Edge { + Node source; + Node target; + bool reverseable; + }; + + static void run( std::vector< unsigned >* segments, std::vector< unsigned >* segmentDescriptions, std::vector< bool >* reversed, const std::vector< Edge >& edges ) + { + // build node index + QMultiHash< Node, unsigned > nodes; + QMultiHash< Node, unsigned > backwardNodes; + for ( unsigned i = 0; i < edges.size(); i++ ) { + nodes.insert( edges[i].source, i ); + backwardNodes.insert( edges[i].target, i ); + if ( edges[i].reverseable ) { + nodes.insert( edges[i].target, i ); + backwardNodes.insert( edges[i].source, i ); + } + } + + std::vector< bool > used( edges.size(), false ); + reversed->resize( edges.size(), false ); + for ( unsigned i = 0; i < edges.size(); i++ ) { + if ( used[i] ) + continue; + + unsigned lastSize = segmentDescriptions->size(); + + segmentDescriptions->push_back( i ); + + used[i] = true; + removeEdge( &nodes, &backwardNodes, edges[i], i ); + + // chain edges forward + Node lastNode = edges[i].target; + while ( nodes.contains( lastNode ) ) { + unsigned nextEdge = nodes.value( lastNode ); + assert( !used[nextEdge] ); + + if ( lastNode != edges[nextEdge].source ) { + assert( lastNode == edges[nextEdge].target ); + assert( edges[nextEdge].reverseable ); + ( *reversed )[nextEdge] = true; + lastNode = edges[nextEdge].source; + } else { + lastNode = edges[nextEdge].target; + } + + segmentDescriptions->push_back( nextEdge ); + + used[nextEdge] = true; + removeEdge( &nodes, &backwardNodes, edges[nextEdge], nextEdge ); + } + + // chain edges backward + std::vector< unsigned > backwardsPath; + lastNode = edges[i].source; + while ( backwardNodes.contains( lastNode ) ) { + unsigned nextEdge = backwardNodes.value( lastNode ); + assert( !used[nextEdge] ); + + if ( lastNode != edges[nextEdge].target ) { + assert( lastNode == edges[nextEdge].source ); + assert( edges[nextEdge].reverseable ); + ( *reversed )[nextEdge] = true; + lastNode = edges[nextEdge].target; + } else { + lastNode = edges[nextEdge].source; + } + + backwardsPath.push_back( nextEdge ); + + used[nextEdge] = true; + removeEdge( &nodes, &backwardNodes, edges[nextEdge], nextEdge ); + } + + segmentDescriptions->insert( segmentDescriptions->begin() + lastSize, backwardsPath.rbegin(), backwardsPath.rend() ); + + segments->push_back( segmentDescriptions->size() - lastSize ); + } + + assert( segmentDescriptions->size() == edges.size() ); + } + +protected: + + static void removeEdge( QMultiHash< Node, unsigned >* nodes, QMultiHash< Node, unsigned >* backwardsNodes, const Edge& edge, unsigned value ) + { + nodes->remove( edge.source, value ); + nodes->remove( edge.target, value ); + backwardsNodes->remove( edge.target, value ); + backwardsNodes->remove( edge.source, value ); + } + +}; + +#endif // EDGECONNECTOR_H diff --git a/monav/utils/qthelpers.h b/monav/utils/qthelpers.h new file mode 100644 index 0000000..871b4c8 --- /dev/null +++ b/monav/utils/qthelpers.h @@ -0,0 +1,75 @@ +/* +Copyright 2010 Christian Vetter veaac.fdirct@gmail.com + +This file is part of MoNav. + +MoNav is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +MoNav 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 General Public License +along with MoNav. If not, see . +*/ + +#ifndef QTHELPERS_H +#define QTHELPERS_H + +#include +#include +#include +#include +#include + +static inline QString fileInDirectory( QString directory, QString filename ) +{ + QDir dir( directory ); + return dir.filePath( filename ); +} + +static inline bool openQFile( QFile* file, QIODevice::OpenMode mode ) +{ + if ( !file->open( mode ) ) { + qCritical() << "could not open file:" << file->fileName() << "," << mode; + return false; + } + return true; +} + +class FileStream : public QDataStream { + +public: + + FileStream( QString filename ) : m_file( filename ) + { + } + + bool open( QIODevice::OpenMode mode ) + { + if ( !openQFile( &m_file, mode ) ) + return false; + setDevice( &m_file ); + return true; + } + +protected: + + QFile m_file; +}; + +class Timer : public QTime { + +public: + + Timer() + { + start(); + } +}; + +#endif // QTHELPERS_H diff --git a/monavlayer.cpp b/monavlayer.cpp new file mode 100644 index 0000000..0d9bce7 --- /dev/null +++ b/monavlayer.cpp @@ -0,0 +1,186 @@ +/* + * Copyright 2010 Niels Kummerfeldt + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) 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 General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, + * Boston, MA 02110-1301 USA + */ + +#include "monavlayer.h" + +#include "mapwidget.h" +#include "projection.h" + +#include +#include +#include +#include +#include + +MonavLayer::MonavLayer(MapWidget *map) : + AbstractLayer(map), + m_gpsLookup(0), + m_router(0), + m_loaded(true), + m_routeStart(), + m_routeEnd(), + m_track(), + m_trackOnScreen(), + m_trackOffset(), + m_zoomLevel(0) +{ + QSettings set(QDir::homePath()+"/Maps/nanomap.conf", QSettings::NativeFormat); + set.beginGroup("monav"); + QString dataDir = set.value("datadir").toString(); + QString routerLib = set.value("router", + "/usr/lib/monav/libcontractionhierarchiesclient.so").toString(); + QString gpsLookupLib = set.value("gpslookup", + "/usr/lib/monav/libgpsgridclient.so").toString(); + set.endGroup(); + + QSettings pluginSettings(dataDir+"/plugins.ini", QSettings::IniFormat); + QString routerName = pluginSettings.value("router").toString(); + QString gpsLookupName = pluginSettings.value("gpsLookup").toString(); + + QPluginLoader rLoader(routerLib); + QObject *plugin = rLoader.instance(); + if (plugin) { + m_router = qobject_cast(plugin); + if (m_router) { + m_router->SetInputDirectory(dataDir); + m_loaded = m_loaded && m_router->LoadData(); + } else { + m_loaded = false; + } + } + QPluginLoader gLoader(gpsLookupLib); + plugin = gLoader.instance(); + if (plugin) { + m_gpsLookup = qobject_cast(plugin); + if (m_gpsLookup) { + m_gpsLookup->SetInputDirectory(dataDir); + m_loaded = m_loaded && m_gpsLookup->LoadData(); + } else { + m_loaded = false; + } + } +} + +void MonavLayer::zoom(int level) +{ + m_zoomLevel = level; + if (m_track.count() > 1) { + int scale = 1 << level; + m_trackOnScreen.clear(); + m_trackOffset = map()->raw2screen(m_track.first().x(), m_track.first().y(), scale); + m_trackOnScreen << QPoint(0, 0); + for (int i = 1; i < m_track.count(); ++i) { + QPointF p = m_track.at(i); + m_trackOnScreen << map()->raw2screen(p.x(), p.y(), scale) - m_trackOffset; + } + } +} + +void MonavLayer::pan(const QPoint &move) +{ + m_trackOffset += move; +} + +void MonavLayer::paint(QPainter *painter) +{ + if (!m_loaded) { + return; + } + if (m_trackOnScreen.count() > 1) { + QPoint p1, p2 = m_trackOnScreen.first(); + for (int i = 1; i < m_trackOnScreen.count(); ++i) { + p1 = m_trackOnScreen.at(i); + painter->drawLine(p1 + m_trackOffset, p2 + m_trackOffset); + p2 = p1; + } + } + + QPoint p = map()->geo2screen(m_routeStart.x(), m_routeStart.y()); + QPolygon tri; + tri << p << p+QPoint(-5, -9) << p+QPoint(5, -9) << p; + painter->setBrush(Qt::red); + painter->drawPolygon(tri); + p = map()->geo2screen(m_routeEnd.x(), m_routeEnd.y()); + tri.clear(); + tri << p << p+QPoint(-5, -9) << p+QPoint(5, -9) << p; + painter->setBrush(Qt::blue); + painter->drawPolygon(tri); +} + +void MonavLayer::keyPressed(QKeyEvent *event) +{ + switch (event->key()) { + case Qt::Key_R: + { + if (event->modifiers() == Qt::NoModifier) { + findRoute(); + } + break; + } + case Qt::Key_S: + { + if (event->modifiers() == Qt::NoModifier) { + m_routeStart = map()->geoPos(); + } + break; + } + case Qt::Key_E: + { + if (event->modifiers() == Qt::NoModifier) { + m_routeEnd = map()->geoPos(); + } + break; + } + } +} + +void MonavLayer::findRoute() +{ + if (!m_loaded) { + return; + } + + QVector nodes; + QVector edges; + double lookupRadius = 1000.0; + double dist; + + UnsignedCoordinate startCoord(GPSCoordinate(m_routeStart.y(), m_routeStart.x())); + IGPSLookup::Result startPos; + if (!m_gpsLookup->GetNearestEdge(&startPos, startCoord, lookupRadius)) { + qDebug() << "source not found"; + return; + } + UnsignedCoordinate endCoord(GPSCoordinate(m_routeEnd.y(), m_routeEnd.x())); + IGPSLookup::Result endPos; + if (!m_gpsLookup->GetNearestEdge(&endPos, endCoord, lookupRadius)) { + qDebug() << "target not found"; + return; + } + if (m_router->GetRoute(&dist, &nodes, &edges, startPos, endPos)) { + qDebug() << "route found"; + m_track.clear(); + for (int j = 0; j < nodes.size(); ++j) { + GPSCoordinate c = nodes[j].coordinate.ToGPSCoordinate(); + m_track << QPointF(Projection::lon2rawx(c.longitude), Projection::lat2rawy(c.latitude)); + } + zoom(m_zoomLevel); + } +} + diff --git a/monavlayer.h b/monavlayer.h new file mode 100644 index 0000000..4359dc7 --- /dev/null +++ b/monavlayer.h @@ -0,0 +1,57 @@ +/* + * Copyright 2010 Niels Kummerfeldt + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) 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 General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, + * Boston, MA 02110-1301 USA + */ + +#ifndef MONAV_LAYER_H +#define MONAV_LAYER_H + +#include "abstractlayer.h" + +#include + +#include "interfaces/irouter.h" +#include "interfaces/igpslookup.h" + +class MonavLayer : public AbstractLayer +{ + Q_OBJECT +public: + MonavLayer(MapWidget *map); + + virtual void zoom(int level); + virtual void pan(const QPoint &move); + virtual void keyPressed(QKeyEvent *event); + +protected: + virtual void paint(QPainter *painter); + +private: + void findRoute(); + + IGPSLookup *m_gpsLookup; + IRouter *m_router; + bool m_loaded; + QPointF m_routeStart, m_routeEnd; + QPolygonF m_track; + QList m_trackOnScreen; + QPoint m_trackOffset; + int m_zoomLevel; + +}; + +#endif // MONAV_LAYER_H diff --git a/nanomap.pro b/nanomap.pro index c1df52c..d6382f8 100644 --- a/nanomap.pro +++ b/nanomap.pro @@ -1,33 +1,4 @@ -TARGET = NanoMap -TEMPLATE = app +TEMPLATE = subdirs -QT += network +SUBDIRS = monav/monav.pro app.pro -SOURCES += main.cpp \ - mainwidget.cpp \ - projection.cpp \ - abstractlayer.cpp \ - gpslayer.cpp \ - markerlayer.cpp \ - gpxlayer.cpp \ - timelayer.cpp \ - batterylayer.cpp \ - mapwidget.cpp \ - markerlist.cpp \ - downloadwidget.cpp \ - routingwidget.cpp \ - gpsclient.cpp - -HEADERS += mainwidget.h \ - projection.h \ - abstractlayer.h \ - gpslayer.h \ - markerlayer.h \ - gpxlayer.h \ - timelayer.h \ - batterylayer.h \ - mapwidget.h \ - markerlist.h \ - downloadwidget.h \ - routingwidget.h \ - gpsclient.h