add monav plugins from http://code.google.com/p/monav/ and use them to calculate routes

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 <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef CELL_H
+#define CELL_H
+
+#include "utils/config.h"
+#include "utils/coordinates.h"
+#include "utils/bithelpers.h"
+#include "utils/edgeconnector.h"
+#include <vector>
+#include <algorithm>
+
+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

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 <http://www.gnu.org/licenses/>.
+*/
+
+#include "gpsgridclient.h"
+#include <QtDebug>
+#include <QHash>
+#include <algorithm>
+#include "utils/qthelpers.h"
+#ifndef NOGUI
+	#include <QInputDialog>
+#endif
+#include <QSettings>
+
+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)

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 <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef GPSGRIDCLIENT_H
+#define GPSGRIDCLIENT_H
+
+#include <QObject>
+#include <QFile>
+#include "interfaces/igpslookup.h"
+#include "cell.h"
+#include "table.h"
+#include <QCache>
+
+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

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

monav/gpsgrid/interfaces

@@ -0,0 +1 @@
+../interfaces/

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 <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef TABLE_H
+#define TABLE_H
+
+#include <QtGlobal>
+#include <QCache>
+#include <QFile>
+#include <string.h>
+#include <vector>
+#include <QtDebug>
+
+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

monav/gpsgrid/utils

@@ -0,0 +1 @@
+../utils/