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