mirror of
git://projects.qi-hardware.com/nanomap.git
synced 2025-04-21 12:27:27 +03:00
add monav plugins from http://code.google.com/p/monav/ and use them to calculate routes
This commit is contained in:
203
monav/utils/bithelpers.h
Normal file
203
monav/utils/bithelpers.h
Normal file
@@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef BITHELPERS_H
|
||||
#define BITHELPERS_H
|
||||
|
||||
#include <cstring>
|
||||
#include <algorithm>
|
||||
|
||||
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
|
||||
25
monav/utils/config.h
Normal file
25
monav/utils/config.h
Normal file
@@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef CONFIG_H_INCLUDED
|
||||
#define CONFIG_H_INCLUDED
|
||||
|
||||
typedef unsigned NodeID;
|
||||
|
||||
#endif // CONFIG_H_INCLUDED
|
||||
289
monav/utils/coordinates.h
Normal file
289
monav/utils/coordinates.h
Normal file
@@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef COORDINATES_H_INCLUDED
|
||||
#define COORDINATES_H_INCLUDED
|
||||
|
||||
#include <limits>
|
||||
#include <cmath>
|
||||
#include <cassert>
|
||||
#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
|
||||
135
monav/utils/edgeconnector.h
Normal file
135
monav/utils/edgeconnector.h
Normal file
@@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef EDGECONNECTOR_H
|
||||
#define EDGECONNECTOR_H
|
||||
|
||||
#include "utils/coordinates.h"
|
||||
#include <vector>
|
||||
#include <QMultiHash>
|
||||
#include <algorithm>
|
||||
|
||||
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
|
||||
75
monav/utils/qthelpers.h
Normal file
75
monav/utils/qthelpers.h
Normal file
@@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef QTHELPERS_H
|
||||
#define QTHELPERS_H
|
||||
|
||||
#include <QFile>
|
||||
#include <QtDebug>
|
||||
#include <QDataStream>
|
||||
#include <QTime>
|
||||
#include <QDir>
|
||||
|
||||
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
|
||||
Reference in New Issue
Block a user