mirror of
git://projects.qi-hardware.com/nanomap.git
synced 2024-11-22 13:08:26 +02:00
f1a2430393
This reverts commit da88ec21cd
.
This was not the right solution
601 lines
17 KiB
C++
601 lines
17 KiB
C++
/*
|
|
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 <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#ifndef COMPRESSEDGRAPH_H
|
|
#define COMPRESSEDGRAPH_H
|
|
|
|
#include "interfaces/irouter.h"
|
|
#include "utils/coordinates.h"
|
|
#include "utils/bithelpers.h"
|
|
#include "blockcache.h"
|
|
#include <QString>
|
|
#include <QFile>
|
|
#include <algorithm>
|
|
#include <vector>
|
|
|
|
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
|