SVN checkout 11/12/2010
[monav:monav.git] / utils / coordinates.h
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 #ifndef COORDINATES_H_INCLUDED
21 #define COORDINATES_H_INCLUDED
22
23 #include <limits>
24 #include <cmath>
25 #include <cassert>
26 #ifndef M_PI
27 #define M_PI 3.14159265358979323846
28 #endif
29
30 class GPSCoordinate {
31 public:
32
33         GPSCoordinate()
34         {
35                 latitude = longitude = std::numeric_limits< double >::max();
36         }
37         GPSCoordinate( double lat, double lon )
38         {
39                 latitude = lat;
40                 longitude = lon;
41         }
42
43         double Distance( const GPSCoordinate &right ) const
44         {
45                 assert( fabs( latitude ) < 90 && fabs( right.latitude ) < 90 );
46                 //assert ( nicht antipodal, nicht an den Polen )
47
48                 // convert inputs in degrees to radians:
49                 static const double DEG_TO_RAD = 0.017453292519943295769236907684886;
50                 double lat1 = latitude * DEG_TO_RAD;
51                 double lon1 = longitude * DEG_TO_RAD;
52                 double lat2 = right.latitude * DEG_TO_RAD;
53                 double lon2 = right.longitude * DEG_TO_RAD;
54
55                 static const double a = 6378137;
56                 static const double b = 6356752.31424518;
57                 static const double f = ( a - b ) / a;
58
59                 const double U1 = atan(( 1 - f ) * tan( lat1 ) );
60                 const double U2 = atan(( 1 - f ) * tan( lat2 ) );
61                 const double cosU1 = cos( U1 );
62                 const double cosU2 = cos( U2 );
63                 const double sinU1 = sin( U1 );
64                 const double sinU2 = sin( U2 );
65                 const double L = fabs( lon2 - lon1 );
66
67                 double lambda = L;
68                 double lambdaOld;
69                 unsigned iterLimit = 50;
70                 while ( true ) {
71                         const double cosLambda = cos( lambda );
72                         const double sinLambda = sin( lambda );
73                         const double leftSinSigma = cosU2 * sinLambda;
74                         const double rightSinSigma = cosU1 * sinU2 - sinU1 * cosU2 * cosLambda;
75                         const double sinSigma = sqrt( leftSinSigma * leftSinSigma + rightSinSigma * rightSinSigma );
76                         if ( sinSigma == 0 ) // Fast identisch
77                                 return 0;
78                         const double cosSigma = sinU1 * sinU2 + cosU1 * cosU2 * cosLambda;
79                         const double sigma = atan2( sinSigma, cosSigma );
80                         const double sinAlpha = cosU1 * cosU2 * sinLambda / sinSigma;
81                         const double cosSquareAlpha = 1 - sinAlpha * sinAlpha;
82                         double cos2sigmam = cosSigma - 2 * sinU1 * sinU2 / cosSquareAlpha;
83                         if ( cos2sigmam != cos2sigmam ) // NAN: Parellel zum Ă„quator
84                                 cos2sigmam = 0;
85                         const double C = f / 16 * cosSquareAlpha * ( 4 + f * ( 4 - 3 * cosSquareAlpha ) );
86                         lambdaOld = lambda;
87                         lambda = L + ( 1 - C ) * f * sinAlpha * ( sigma + C * sinSigma * ( cos2sigmam + C * cosSigma * ( -1 + 2 * cos2sigmam * cos2sigmam ) ) );
88                         if ( fabs( lambda - lambdaOld ) < 1e-12 || --iterLimit == 0 ) {
89                                 const double u2 = cosSquareAlpha * ( a * a - b * b ) / ( b * b );
90                                 const double A = 1 + u2 / 16384 * ( 4096 + u2 * ( -768 + u2 * ( 320 - 175 * u2 ) ) );
91                                 const double B = u2 / 1024 * ( 256 + u2 * ( -128 + u2 * ( 74 - 47 * u2 ) ) );
92                                 const double deltasigma = B * sinSigma * ( cos2sigmam + B / 4 * ( cosSigma * ( -1 + 2 * cos2sigmam * cos2sigmam ) - B / 6 * cos2sigmam * ( -3 + 4 * sinSigma * sinSigma ) * ( -3 + 4 * cos2sigmam * cos2sigmam ) ) );
93                                 return b * A * ( sigma - deltasigma );
94                         }
95                 }
96
97                 //should never be reached
98                 return 0;
99         }
100
101         double ApproximateDistance( const GPSCoordinate &right ) const
102         {
103                 static const double DEG_TO_RAD = 0.017453292519943295769236907684886;
104                 ///Earth's quatratic mean radius for WGS-84
105                 static const double EARTH_RADIUS_IN_METERS = 6372797.560856;
106                 double latitudeArc  = ( latitude - right.latitude ) * DEG_TO_RAD;
107                 double longitudeArc = ( longitude - right.longitude ) * DEG_TO_RAD;
108                 double latitudeH = sin( latitudeArc * 0.5 );
109                 latitudeH *= latitudeH;
110                 double lontitudeH = sin( longitudeArc * 0.5 );
111                 lontitudeH *= lontitudeH;
112                 double tmp = cos( latitude * DEG_TO_RAD ) * cos( right.latitude * DEG_TO_RAD );
113                 double distanceArc =  2.0 * asin( sqrt( latitudeH + tmp * lontitudeH ) );
114                 return EARTH_RADIUS_IN_METERS * distanceArc;
115         }
116
117         bool operator==( const GPSCoordinate& right ) const
118         {
119                 return latitude == right.latitude && longitude == right.longitude;
120         }
121
122         bool operator!=( const GPSCoordinate& right ) const
123         {
124                 return !( *this == right );
125         }
126
127         bool operator<( const GPSCoordinate& right ) const
128         {
129                 if ( latitude != right.latitude )
130                         return latitude < right.latitude;
131                 return longitude < right.longitude;
132         }
133
134         bool IsValid() const
135         {
136                 return latitude != std::numeric_limits< double >::max() && longitude != std::numeric_limits< double >::max();
137         }
138
139         double latitude, longitude;
140 };
141
142 class ProjectedCoordinate {
143 public:
144
145         ProjectedCoordinate()
146         {
147                 x = y = std::numeric_limits< double >::max();
148         }
149
150         ProjectedCoordinate( double xVal, double yVal )
151         {
152                 x = xVal;
153                 y = yVal;
154         }
155
156         ProjectedCoordinate( double xVal, double yVal, int zoom ) {
157                 x = xVal / ( 1u << zoom );
158                 y = yVal / ( 1u << zoom );
159         }
160
161         explicit ProjectedCoordinate( const GPSCoordinate& gps )
162         {
163                 x = ( gps.longitude + 180.0 ) / 360.0;
164                 y = ( 1.0 - log( tan( gps.latitude * M_PI / 180.0 ) + 1.0 / cos( gps.latitude * M_PI / 180.0 ) ) / M_PI ) / 2.0;
165         }
166
167         GPSCoordinate ToGPSCoordinate() const
168         {
169                 GPSCoordinate gps;
170                 gps.longitude = x * 360.0 - 180;
171                 const double n = M_PI - 2.0 * M_PI * y;
172                 gps.latitude = 180.0 / M_PI * atan( 0.5 * ( exp( n ) - exp( -n ) ) );
173                 return gps;
174         }
175
176         bool operator==( const ProjectedCoordinate& right ) const
177         {
178                 return x == right.x && y == right.y;
179         }
180
181         bool operator!=( const ProjectedCoordinate& right ) const
182         {
183                 return !( *this == right );
184         }
185
186         bool operator<( const ProjectedCoordinate& right ) const
187         {
188                 if ( x != right.x )
189                         return x < right.x;
190                 return y < right.y;
191         }
192
193         bool IsValid() const
194         {
195                 return x != std::numeric_limits< double >::max() && y != std::numeric_limits< double >::max();
196         }
197
198         double x, y;
199 };
200
201 class UnsignedCoordinate {
202 public:
203         
204         UnsignedCoordinate()
205         {
206                 x = y = std::numeric_limits< unsigned >::max();
207         }
208
209         UnsignedCoordinate( unsigned xVal, unsigned yVal )
210         {
211                 x = xVal;
212                 y = yVal;
213         }
214
215         explicit UnsignedCoordinate( ProjectedCoordinate tile )
216         {
217                 x = floor( tile.x * ( 1u << 30 ) );
218                 y = floor( tile.y * ( 1u << 30 ) );
219         }
220
221         explicit UnsignedCoordinate( GPSCoordinate gps )
222         {
223                 *this = UnsignedCoordinate( ProjectedCoordinate( gps ) );
224         }
225
226         GPSCoordinate ToGPSCoordinate() const
227         {
228                 return ToProjectedCoordinate().ToGPSCoordinate();
229         }
230
231         ProjectedCoordinate ToProjectedCoordinate() const
232         {
233                 ProjectedCoordinate tile;
234                 tile.x = x;
235                 tile.y = y;
236                 tile.x /= ( 1u << 30 );
237                 tile.y /= ( 1u << 30 );
238                 return tile;
239         }
240
241         unsigned GetTileX( int zoom ) const {
242                 if ( zoom == 0 )
243                         return 0;
244                 return x >> ( 30 - zoom );
245         }
246         unsigned GetTileY( int zoom ) const {
247                 if ( zoom == 0 )
248                         return 0;
249                 return y >> ( 30 - zoom );
250         }
251         unsigned GetTileSubX( int zoom, int precision ) const {
252                 assert( zoom + precision < 31 );
253                 assert( zoom + precision > 0 );
254                 const unsigned subX = ( x << zoom ) >> zoom;
255                 return subX >> ( 30 - precision - zoom );
256         }
257         unsigned GetTileSubY( int zoom, int precision ) const {
258                 assert( zoom + precision < 31 );
259                 assert( zoom + precision > 0 );
260                 const unsigned subY = ( y << zoom ) >> zoom;
261                 return subY >> ( 30 - precision - zoom );
262         }
263
264         bool operator==( const UnsignedCoordinate& right ) const
265         {
266                 return x == right.x && y == right.y;
267         }
268
269         bool operator!=( const UnsignedCoordinate& right ) const
270         {
271                 return !( *this == right );
272         }
273
274         bool operator<( const UnsignedCoordinate& right ) const
275         {
276                 if ( x != right.x )
277                         return x < right.x;
278                 return y < right.y;
279         }
280
281         bool IsValid() const
282         {
283                 return x != std::numeric_limits< unsigned >::max() && y != std::numeric_limits< unsigned >::max();
284         }
285
286         unsigned x, y;
287 };
288
289 #endif // COORDINATES_H_INCLUDED