mirror of
git://projects.qi-hardware.com/nanomap.git
synced 2024-11-22 13:46:15 +02:00
291 lines
9.1 KiB
C++
291 lines
9.1 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/>.
|
|
*/
|
|
|
|
#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)
|