Root/monav/utils/coordinates.h

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#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
30class GPSCoordinate {
31public:
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
142class ProjectedCoordinate {
143public:
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
201class UnsignedCoordinate {
202public:
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
290

Archive Download this file

Branches:
master



interactive