Root/monav/gpsgrid/gpsgridclient.cpp

1/*
2Copyright 2010 Christian Vetter veaac.fdirct@gmail.com
3
4This file is part of MoNav.
5
6MoNav is free software: you can redistribute it and/or modify
7it under the terms of the GNU General Public License as published by
8the Free Software Foundation, either version 3 of the License, or
9(at your option) any later version.
10
11MoNav is distributed in the hope that it will be useful,
12but WITHOUT ANY WARRANTY; without even the implied warranty of
13MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14GNU General Public License for more details.
15
16You should have received a copy of the GNU General Public License
17along with MoNav. If not, see <http://www.gnu.org/licenses/>.
18*/
19
20#include "gpsgridclient.h"
21#include <QtDebug>
22#include <QHash>
23#include <algorithm>
24#include "utils/qthelpers.h"
25#ifndef NOGUI
26    #include <QInputDialog>
27#endif
28#include <QSettings>
29
30GPSGridClient::GPSGridClient()
31{
32    index = NULL;
33    gridFile = NULL;
34    QSettings settings( "MoNavClient" );
35    settings.beginGroup( "GPS Grid" );
36    cacheSize = settings.value( "cacheSize", 1 ).toInt();
37    cache.setMaxCost( 1024 * 1024 * 3 * cacheSize / 4 );
38}
39
40GPSGridClient::~GPSGridClient()
41{
42    QSettings settings( "MoNavClient" );
43    settings.beginGroup( "GPS Grid" );
44    settings.setValue( "cacheSize", cacheSize );
45    unload();
46}
47
48void GPSGridClient::unload()
49{
50    if ( index != NULL )
51        delete index;
52    index = NULL;
53    if ( gridFile != NULL )
54        delete gridFile;
55    gridFile = NULL;
56    cache.clear();
57}
58
59QString GPSGridClient::GetName()
60{
61    return "GPS Grid";
62}
63
64void GPSGridClient::SetInputDirectory( const QString& dir )
65{
66    directory = dir;
67}
68
69void GPSGridClient::ShowSettings()
70{
71#ifndef NOGUI
72    bool ok = false;
73    int result = QInputDialog::getInt( NULL, "Settings", "Enter Cache Size [MB]", cacheSize, 1, 1024, 1, &ok );
74    if ( !ok )
75        return;
76    cacheSize = result;
77    if ( index != NULL )
78        index->SetCacheSize( 1024 * 1024 * cacheSize / 4 );
79    cache.setMaxCost( 1024 * 1024 * 3 * cacheSize / 4 );
80#endif
81}
82
83bool GPSGridClient::LoadData()
84{
85    unload();
86    QString filename = fileInDirectory( directory, "GPSGrid" );
87    QFile configFile( filename + "_config" );
88    if ( !openQFile( &configFile, QIODevice::ReadOnly ) )
89        return false;
90
91    index = new gg::Index( filename + "_index" );
92    index->SetCacheSize( 1024 * 1024 * cacheSize / 4 );
93
94    gridFile = new QFile( filename + "_grid" );
95    if ( !gridFile->open( QIODevice::ReadOnly ) ) {
96        qCritical() << "failed to open file: " << gridFile->fileName();
97        return false;
98    }
99
100    return true;
101}
102
103bool GPSGridClient::GetNearestEdge( Result* result, const UnsignedCoordinate& coordinate, double radius, bool headingPenalty, double heading )
104{
105    const GPSCoordinate gps = coordinate.ToProjectedCoordinate().ToGPSCoordinate();
106    const GPSCoordinate gpsMoved( gps.latitude, gps.longitude + 1 );
107
108    const double meter = gps.ApproximateDistance( gpsMoved );
109    double gridRadius = (( double ) UnsignedCoordinate( ProjectedCoordinate( gpsMoved ) ).x - coordinate.x ) / meter * radius;
110    gridRadius *= gridRadius;
111    heading = fmod( ( heading + 270 ) * 2.0 * M_PI / 360.0, 2 * M_PI );
112
113    static const int width = 32 * 32 * 32;
114
115    ProjectedCoordinate position = coordinate.ToProjectedCoordinate();
116    NodeID yGrid = floor( position.y * width );
117    NodeID xGrid = floor( position.x * width );
118
119    result->distance = gridRadius;
120    QVector< UnsignedCoordinate > path;
121
122    checkCell( result, &path, xGrid - 1, yGrid - 1, coordinate, heading, headingPenalty );
123    checkCell( result, &path, xGrid - 1, yGrid, coordinate, heading, headingPenalty );
124    checkCell( result, &path, xGrid - 1, yGrid + 1, coordinate, heading, headingPenalty );
125
126    checkCell( result, &path, xGrid, yGrid - 1, coordinate, heading, headingPenalty );
127    checkCell( result, &path, xGrid, yGrid, coordinate, heading, headingPenalty );
128    checkCell( result, &path, xGrid, yGrid + 1, coordinate, heading, headingPenalty );
129
130    checkCell( result, &path, xGrid + 1, yGrid - 1, coordinate, heading, headingPenalty );
131    checkCell( result, &path, xGrid + 1, yGrid, coordinate, heading, headingPenalty );
132    checkCell( result, &path, xGrid + 1, yGrid + 1, coordinate, heading, headingPenalty );
133
134    if ( path.empty() )
135        return false;
136
137    double length = 0;
138    double lengthToNearest = 0;
139    for ( int pathID = 1; pathID < path.size(); pathID++ ) {
140        UnsignedCoordinate sourceCoord = path[pathID - 1];
141        UnsignedCoordinate targetCoord = path[pathID];
142        double xDiff = ( double ) sourceCoord.x - targetCoord.x;
143        double yDiff = ( double ) sourceCoord.y - targetCoord.y;
144
145        double distance = sqrt( xDiff * xDiff + yDiff * yDiff );
146        length += distance;
147        if ( pathID < ( int ) result->previousWayCoordinates )
148            lengthToNearest += distance;
149        else if ( pathID == ( int ) result->previousWayCoordinates )
150            lengthToNearest += result->percentage * distance;
151    }
152    if ( length == 0 )
153        result->percentage = 0;
154    else
155        result->percentage = lengthToNearest / length;
156    return true;
157}
158
159bool GPSGridClient::checkCell( Result* result, QVector< UnsignedCoordinate >* path, NodeID gridX, NodeID gridY, const UnsignedCoordinate& coordinate, double heading, double headingPenalty ) {
160    static const int width = 32 * 32 * 32;
161    ProjectedCoordinate minPos( ( double ) gridX / width, ( double ) gridY / width );
162    ProjectedCoordinate maxPos( ( double ) ( gridX + 1 ) / width, ( double ) ( gridY + 1 ) / width );
163    UnsignedCoordinate min( minPos );
164    UnsignedCoordinate max( maxPos );
165    if ( distance( min, max, coordinate ) >= result->distance )
166        return false;
167
168    qint64 cellNumber = ( qint64( gridX ) << 32 ) + gridY;
169    if ( !cache.contains( cellNumber ) ) {
170        qint64 position = index->GetIndex( gridX, gridY );
171        if ( position == -1 )
172            return true;
173        gridFile->seek( position );
174        int size;
175        gridFile->read( (char* ) &size, sizeof( size ) );
176        unsigned char* buffer = new unsigned char[size + 8]; // reading buffer + 4 bytes
177
178        gridFile->read( ( char* ) buffer, size );
179        gg::Cell* cell = new gg::Cell();
180        cell->read( buffer, min, max );
181        cache.insert( cellNumber, cell, cell->edges.size() * sizeof( gg::Cell::Edge ) );
182        delete[] buffer;
183    }
184    gg::Cell* cell = cache.object( cellNumber );
185    if ( cell == NULL )
186        return true;
187
188    UnsignedCoordinate nearestPoint;
189    for ( std::vector< gg::Cell::Edge >::const_iterator i = cell->edges.begin(), e = cell->edges.end(); i != e; ++i ) {
190        bool found = false;
191
192        for ( int pathID = 1; pathID < i->pathLength; pathID++ ) {
193            UnsignedCoordinate sourceCoord = cell->coordinates[pathID + i->pathID - 1];
194            UnsignedCoordinate targetCoord = cell->coordinates[pathID + i->pathID];
195            double percentage = 0;
196
197            double d = distance( &nearestPoint, &percentage, sourceCoord, targetCoord, coordinate );
198            if ( d + headingPenalty > result->distance )
199                continue;
200
201            double xDiff = ( double ) targetCoord.x - sourceCoord.x;
202            double yDiff = ( double ) targetCoord.y - sourceCoord.y;
203            double direction = 0;
204            if ( xDiff != 0 || yDiff != 0 )
205                direction = fmod( atan2( yDiff, xDiff ), 2 * M_PI );
206            else
207                headingPenalty = 0;
208            double penalty = fabs( direction - heading );
209            if ( penalty > M_PI )
210                penalty = 2 * M_PI - penalty;
211            if ( i->bidirectional && penalty > M_PI / 2 )
212                penalty = M_PI - penalty;
213            penalty = penalty / M_PI * headingPenalty;
214            d += penalty;
215
216            if ( d < result->distance ) {
217                result->nearestPoint = nearestPoint;
218                result->distance = d;
219                result->previousWayCoordinates = pathID;
220                result->percentage = percentage;
221                found = true;
222            }
223        }
224
225        if ( found ) {
226            result->source = i->source;
227            result->target = i->target;
228            result->edgeID = i->edgeID;
229            path->clear();
230            for ( int pathID = 0; pathID < i->pathLength; pathID++ )
231                path->push_back( cell->coordinates[pathID + i->pathID] );
232        }
233    }
234
235    return true;
236}
237
238double GPSGridClient::distance( UnsignedCoordinate* nearestPoint, double* percentage, const UnsignedCoordinate source, const UnsignedCoordinate target, const UnsignedCoordinate& coordinate ) {
239    const double vY = ( double ) target.y - source.y;
240    const double vX = ( double ) target.x - source.x;
241    const double wY = ( double ) coordinate.y - source.y;
242    const double wX = ( double ) coordinate.x - source.x;
243    const double vLengthSquared = vY * vY + vX * vX;
244
245    double r = 0;
246    if ( vLengthSquared != 0 )
247        r = ( vX * wX + vY * wY ) / vLengthSquared;
248    *percentage = r;
249
250    if ( r <= 0 ) {
251        *nearestPoint = source;
252        *percentage = 0;
253        return wY * wY + wX * wX;
254    } else if ( r >= 1 ) {
255        *nearestPoint = target;
256        *percentage = 1;
257        const double dY = ( double ) coordinate.y - target.y;
258        const double dX = ( double ) coordinate.x - target.x;
259        return dY * dY + dX * dX;
260    }
261
262    nearestPoint->x = source.x + r * vX;
263    nearestPoint->y = source.y + r * vY;
264
265    const double dX = ( double ) nearestPoint->x - coordinate.x;
266    const double dY = ( double ) nearestPoint->y - coordinate.y;
267
268    return dY * dY + dX * dX;
269}
270
271double GPSGridClient::distance( const UnsignedCoordinate& min, const UnsignedCoordinate& max, const UnsignedCoordinate& coordinate ) {
272    UnsignedCoordinate nearest = coordinate;
273
274    if ( coordinate.x <= min.x )
275        nearest.x = min.x;
276    else if ( coordinate.x >= max.x )
277        nearest.x = max.x;
278
279    if ( coordinate.y <= min.y )
280        nearest.y = min.y;
281    else if ( coordinate.y >= max.y )
282        nearest.y = max.y;
283
284    double xDiff = ( double ) coordinate.x - nearest.x;
285    double yDiff = ( double ) coordinate.y - nearest.y;
286
287    return xDiff * xDiff + yDiff * yDiff;
288}
289
290Q_EXPORT_PLUGIN2(gpsgridclient, GPSGridClient)
291

Archive Download this file

Branches:
master



interactive