# source:subversion/applications/utils/where_are_they/geo_helper.py@34613

Last change on this file since 34613 was 2189, checked in by nickburch, 13 years ago

Add "where are they" stuff (http://wiki.openstreetmap.org/index.php/Where_Are_They) to svn

• Property svn:eol-style set to `native`
File size: 17.3 KB
Line
1# Geographical helper functions for nmea_info.py and friends
2#
3# Helps with geographic functions, including:
4#  Lat+Long+Height -> XYZ
5#  XYZ -> Lat+Long+Height
6#  Lat+Long -> other Lat+Long (Helmert Transform)
7#  Lat+Long -> easting/northing (OS Only)
8#  easting/northing -> Lat+Long (OS Only)
9#  OS easting/northing -> OS 6 figure ref
10#
11# See http://gagravarr.org/code/ for updates and information
12#
13# GPL
14#
15# Nick Burch - v0.04 (11/12/2006)
16
17import math
18
19# The earth's radius, in meters, at the equator (should be close enough)
20earths_radius = 6335.437 * 1000
21
22# For each co-ordinate system we do, what are the A, B and E2 values?
23# List is A, B, E^2 (E^2 calculated after)
24abe_values = {
25        'wgs84': [ 6378137.0, 6356752.3141, -1 ],
26        'osgb' : [ 6377563.396, 6356256.91, -1 ],
27        'osie' : [ 6377340.189, 6356034.447, -1 ]
28}
29
30# Calculate the E2 values
31for system in abe_values.keys():
32        a = abe_values[system][0]
33        b = abe_values[system][1]
34        e2 = (a*a - b*b) / (a*a)
35        abe_values[system][2] = e2
36
37# For each co-ordinate system we can translate between, what are
38#  the tx, ty, tz, s, rx, ry and rz values?
39# List is tx, ty, tz, s, rx, ry, rz
40transform_values = {
41        'wgs84_to_osgb' : [ -446.448, 125.157, -542.060,
42                                                        20.4894 / 1000.0 / 1000.0, # given as ppm
43                                                        -0.1502 / 206265.0, # given as seconds of arc
44                                                        -0.2470 / 206265.0, # given as seconds of arc
45                                                        -0.8421 / 206265.0  # given as seconds of arc
46                                        ],
47        'wgs84_to_osie' : [ -482.530, 130.596, -564.557,
48                                                        -8.1500 / 1000.0 / 1000.0, # given as ppm
49                                                        -1.0420 / 206265.0, # given as seconds of arc
50                                                        -0.2140 / 206265.0, # given as seconds of arc
51                                                        -0.6310 / 206265.0  # given as seconds of arc
52                                        ],
53        'itrs2000_to_etrs89' : [ 0.054, 0.051, -0.048, 0,
54                                                         0.000081 / 206265.0, # given as seconds of arc
55                                                         0.00049 / 206265.0, # given as seconds of arc
56                                                         0.000792 / 206265.0  # given as seconds of arc
57                                        ]
58}
59
60# Calculate reverse transforms
61for systems in [('wgs84','osgb'), ('wgs84','osie'), ('itrs2000','etrs89')]:
62        fs = systems[0] + "_to_" + systems[1]
63        rs = systems[1] + "_to_" + systems[0]
64        ra = []
65        for val in transform_values[fs]:
66                ra.append(-1.0 * val)
67        transform_values[rs] = ra
68
69# Easting and Northin system values, for the systems we work with.
70# List is n0, e0, F0, theta0 and landa0
71en_values = {
72        'osgb' : [ -100000.0, 400000.0, 0.9996012717,
73                                        49.0 /360.0 *2.0*math.pi,
74                                        -2.0 /360.0 *2.0*math.pi
75                         ],
76        'osie' : [ 250000.0, 200000.0, 1.000035,
77                                        53.5 /360.0 *2.0*math.pi,
78                                        -8.0 /360.0 *2.0*math.pi
79                         ]
80}
81
82# Cassini Projection Origins
83# List is lat (rad), long (rad), false easting, false northing
84cassini_values = {
85        'osgb' : [ (53.0 + (13.0 / 60.0) + (17.274 / 3600.0)) /360.0 *2.0*math.pi,
86                   -(2.0 + (41.0 / 60.0) +  (3.562 / 3600.0)) /360.0 *2.0*math.pi,
87                   0, 0 ]
88}
89
90# How many feet to the meter
91feet_per_meter = 1.0 / 0.3048007491   # 3.28083
92
93##############################################################
94#           OS Specific Helpers for Generic Methods          #
95##############################################################
96
97def turn_wgs84_into_osgb36(lat_dec,long_dec,height):
98        """See http://www.gps.gov.uk/guide6.asp#6.2 and http://www.gps.gov.uk/guide6.asp#6.6 for the calculations, and http://www.posc.org/Epicentre.2_2/DataModel/ExamplesofUsage/eu_cs34h.html for some background."""
99
100        wgs84_xyz = turn_llh_into_xyz(lat_dec,long_dec,height,'wgs84')
101
102        osgb_xyz = turn_xyz_into_other_xyz(
103                                        wgs84_xyz[0],wgs84_xyz[1],wgs84_xyz[2],'wgs84','osgb')
104
105        osgb_latlong = turn_xyz_into_llh(
106                                        osgb_xyz[0],osgb_xyz[1],osgb_xyz[2],'osgb')
107
108        return osgb_latlong
109def turn_osgb36_into_wgs84(lat_dec,long_dec,height):
110        """See http://www.gps.gov.uk/guide6.asp#6.2 and http://www.gps.gov.uk/guide6.asp#6.6 for the calculations, and http://www.posc.org/Epicentre.2_2/DataModel/ExamplesofUsage/eu_cs34h.html for some background."""
111
112        osgb_xyz = turn_llh_into_xyz(lat_dec,long_dec,height,'osgb')
113
114        wgs84_xyz = turn_xyz_into_other_xyz(
115                                        osgb_xyz[0],osgb_xyz[1],osgb_xyz[2],'osgb','wgs84')
116
117        wgs84_latlong = turn_xyz_into_llh(
118                                        wgs84_xyz[0],wgs84_xyz[1],wgs84_xyz[2],'wgs84')
119
120        return wgs84_latlong
121
122def turn_osgb36_into_eastingnorthing(lat_dec,long_dec):
123        """Turn OSGB36 (decimal) lat/long values into OS easting and northing values."""
124        return turn_latlong_into_eastingnorthing(lat_dec,long_dec,'osgb')
125
126def turn_eastingnorthing_into_osgb36(easting,northing):
127        """Turn OSGB36 easting and northing values into (decimal) lat/long values inOSGB36."""
128        return turn_eastingnorthing_into_latlong(easting,northing,'osgb')
129
130##############################################################
131#             Generic Transform Functions                    #
132##############################################################
133
134def turn_llh_into_xyz(lat_dec,long_dec,height,system):
135        """Convert Lat, Long and Height into 3D Cartesian x,y,z
136       See http://www.ordnancesurvey.co.uk/gps/docs/convertingcoordinates3D.pdf"""
137
138        a = abe_values[system][0]
139        b = abe_values[system][1]
140        e2 = abe_values[system][2]
141
142        theta = float(lat_dec)  / 360.0 * 2.0 * math.pi
143        landa = float(long_dec) / 360.0 * 2.0 * math.pi
144        height = float(height)
145
146        v = a / math.sqrt( 1.0 - e2 * (math.sin(theta) * math.sin(theta)) )
147        x = (v + height) * math.cos(theta) * math.cos(landa)
148        y = (v + height) * math.cos(theta) * math.sin(landa)
149        z = ( (1.0 - e2) * v + height ) * math.sin(theta)
150
151        return [x,y,z]
152
153def turn_xyz_into_llh(x,y,z,system):
154        """Convert 3D Cartesian x,y,z into Lat, Long and Height
155       See http://www.ordnancesurvey.co.uk/gps/docs/convertingcoordinates3D.pdf"""
156
157        a = abe_values[system][0]
158        b = abe_values[system][1]
159        e2 = abe_values[system][2]
160
161        p = math.sqrt(x*x + y*y)
162
163        long = math.atan(y/x)
164        lat_init = math.atan( z / (p * (1.0 - e2)) )
165        v = a / math.sqrt( 1.0 - e2 * (math.sin(lat_init) * math.sin(lat_init)) )
166        lat = math.atan( (z + e2*v*math.sin(lat_init)) / p )
167
168        height = (p / math.cos(lat)) - v # Ignore if a bit out
169
170        # Turn from radians back into degrees
171        long = long / 2 / math.pi * 360
172        lat = lat / 2 / math.pi * 360
173
174        return [lat,long,height]
175
176def turn_xyz_into_other_xyz(old_x,old_y,old_z,from_scheme,to_scheme):
177        """Helmert Transformation between one lat+long system and another
178See http://www.ordnancesurvey.co.uk/oswebsite/gps/information/coordinatesystemsinfo/guidecontents/guide6.html for the calculations, and http://www.movable-type.co.uk/scripts/LatLongConvertCoords.html for a friendlier version with examples"""
179
180        transform = from_scheme + "_to_" + to_scheme
181        tx = transform_values[transform][0]
182        ty = transform_values[transform][1]
183        tz = transform_values[transform][2]
184        s =  transform_values[transform][3]
185        rx = transform_values[transform][4]
186        ry = transform_values[transform][5]
187        rz = transform_values[transform][6]
188
189        # Do the transform
190        new_x = tx + ((1.0+s) * old_x) + (-rz * old_y) + (ry * old_z)
191        new_y = ty + (rz * old_x) + ((1.0+s) * old_y) + (-rx * old_z)
192        new_z = tz + (-ry * old_x) + (rx * old_y) + ((1.0+s) * old_z)
193
194        return [new_x,new_y,new_z]
195
196def calculate_distance_and_bearing(from_lat_dec,from_long_dec,to_lat_dec,to_long_dec):
197        """Uses the spherical law of cosines to calculate the distance and bearing between two positions"""
198
199        # Turn them all into radians
200        from_theta = float(from_lat_dec)  / 360.0 * 2.0 * math.pi
201        from_landa = float(from_long_dec) / 360.0 * 2.0 * math.pi
202        to_theta = float(to_lat_dec)  / 360.0 * 2.0 * math.pi
203        to_landa = float(to_long_dec) / 360.0 * 2.0 * math.pi
204
205        d = math.acos(
206                        math.sin(from_theta) * math.sin(to_theta) +
207                        math.cos(from_theta) * math.cos(to_theta) * math.cos(to_landa-from_landa)
208                ) * earths_radius
209
210        bearing = math.atan2(
211                        math.sin(to_landa-from_landa) * math.cos(to_theta),
212                        math.cos(from_theta) * math.sin(to_theta) -
213                        math.sin(from_theta) * math.cos(to_theta) * math.cos(to_landa-from_landa)
214                )
215        bearing = bearing / 2.0 / math.pi * 360.0
216
217        return [d,bearing]
218
219##############################################################
220#            Easting/Northing Transform Methods              #
221##############################################################
222
223def turn_latlong_into_eastingnorthing(lat_dec,long_dec,scheme):
224        """Turn OSGB36 or OSIE36 (decimal) lat/long values into OS easting and northing values. See http://www.ordnancesurvey.co.uk/oswebsite/gps/information/coordinatesystemsinfo/guidecontents/guide7.html for the calculations, and http://www.posc.org/Epicentre.2_2/DataModel/ExamplesofUsage/eu_cs34h.html for some background."""
225
226        n0 = en_values[scheme][0]
227        e0 = en_values[scheme][1]
228        f0 = en_values[scheme][2]
229
230        theta0 = en_values[scheme][3]
231        landa0 = en_values[scheme][4]
232
233        a = abe_values[scheme][0]
234        b = abe_values[scheme][1]
235        e2 = abe_values[scheme][2]
236
237        theta = float(lat_dec)  /360.0 *2.0*math.pi
238        landa = float(long_dec) /360.0 *2.0*math.pi
239
240        n = (a-b) / (a+b)
241        v = a * f0 * math.pow( (1 - e2 * math.sin(theta)*math.sin(theta)), -0.5 )
242        ro = a * f0 * (1 - e2) * math.pow( (1 - e2 * math.sin(theta)*math.sin(theta)), -1.5 )
243        nu2 = v/ro - 1
244
245        M = b * f0 * ( \
246                (1.0 + n + 5.0/4.0 *n*n + 5.0/4.0 *n*n*n) * (theta-theta0) - \
247                (3.0*n + 3.0*n*n + 21.0/8.0 *n*n*n) *math.sin(theta-theta0) *math.cos(theta+theta0) + \
248                (15.0/8.0*n*n + 15.0/8.0*n*n*n) *math.sin(2.0*(theta-theta0)) *math.cos(2.0*(theta+theta0)) - \
249                35.0/24.0*n*n*n *math.sin(3.0*(theta-theta0)) *math.cos(3.0*(theta+theta0)) \
250        )
251
252        I = M + n0
253        II = v/2.0 * math.sin(theta) * math.cos(theta)
254        III = v/24.0 * math.sin(theta) * math.pow( math.cos(theta),3 ) * \
255                (5.0 - math.pow(math.tan(theta),2) + 9.0*nu2)
256        IIIa = v/720.0 * math.sin(theta) * math.pow( math.cos(theta),5 ) * \
257                ( 61.0 - 58.0 *math.pow(math.tan(theta),2) + math.pow(math.tan(theta),4) )
258        IV = v * math.cos(theta)
259        V = v/6.0 * math.pow( math.cos(theta),3 ) * \
260                ( v/ro - math.pow(math.tan(theta),2) )
261        VI = v/120.0 * math.pow(math.cos(theta),5) * \
262                ( 5.0 - 18.0 *math.pow(math.tan(theta),2) + \
263                math.pow(math.tan(theta),4) + 14.0*nu2 - \
264                58.0 * math.pow(math.tan(theta),2)*nu2 )
265
266        northing = I + II*math.pow(landa-landa0,2) + \
267                III*math.pow(landa-landa0,4) + \
268                IIIa*math.pow(landa-landa0,6)
269        easting = e0 + IV*(landa-landa0) + V*math.pow(landa-landa0,3) + \
270                VI*math.pow(landa-landa0,5)
271
272        return (easting,northing)
273
274def turn_eastingnorthing_into_latlong(easting,northing,scheme):
275        """Turn OSGB36 or OSIE36 easting and northing values into (decimal) lat/long values in OSGB36 / OSIE36. See http://www.ordnancesurvey.co.uk/oswebsite/gps/information/coordinatesystemsinfo/guidecontents/guide7.html for the calculations, and http://www.posc.org/Epicentre.2_2/DataModel/ExamplesofUsage/eu_cs34h.html for some background."""
276
277        n0 = en_values[scheme][0]
278        e0 = en_values[scheme][1]
279        f0 = en_values[scheme][2]
280
281        theta0 = en_values[scheme][3]
282        landa0 = en_values[scheme][4]
283
284        a = abe_values[scheme][0]
285        b = abe_values[scheme][1]
286        e2 = abe_values[scheme][2]
287
288        n = (a-b) / (a+b)
289
290        # Prepare to iterate
291        M = 0
292        theta = theta0
293        # Iterate, 4 times should be enough
294        for i in range(4):
295                theta = ((northing - n0 - M) / (a * f0)) + theta
296                M = b * f0 * ( \
297                        (1.0 + n + 5.0/4.0 *n*n + 5.0/4.0 *n*n*n) * (theta-theta0) - \
298                        (3.0*n + 3.0*n*n + 21.0/8.0 *n*n*n) *math.sin(theta-theta0) *math.cos(theta+theta0) + \
299                        (15.0/8.0*n*n + 15.0/8.0*n*n*n) *math.sin(2.0*(theta-theta0)) *math.cos(2.0*(theta+theta0)) - \
300                        35.0/24.0*n*n*n *math.sin(3.0*(theta-theta0)) *math.cos(3.0*(theta+theta0)) \
301                )
302
303        # Compute intermediate values
304        v = a * f0 * math.pow( (1 - e2 * math.sin(theta)*math.sin(theta)), -0.5 )
305        ro = a * f0 * (1 - e2) * math.pow( (1 - e2 * math.sin(theta)*math.sin(theta)), -1.5 )
306        nu2 = v/ro - 1
307        tantheta2 = math.pow(math.tan(theta),2)
308
309        VII = math.tan(theta) / (2 * ro * v)
310        VIII = math.tan(theta) / (24 * ro * math.pow(v,3)) \
311                        * (5 + 3 * tantheta2 + nu2 - \
312                           9 * tantheta2 *  nu2 )
313        IX = math.tan(theta) / (720 * ro * math.pow(v,5)) \
314                        * (61 + 90 * tantheta2 + 45 * tantheta2 * tantheta2)
315        X = 1 / (math.cos(theta) * v)
316        XI = 1 / (math.cos(theta) * 6 * math.pow(v,3)) \
317                        * (v/ro + 2*tantheta2)
318        XII = 1 / (math.cos(theta) * 120 * math.pow(v,5)) \
319                        * (5 + 28 * tantheta2 + 24 * tantheta2 * tantheta2)
320        XIIa = 1 / (math.cos(theta) * 5040 * math.pow(v,7)) \
321                        * (61 + 662 * tantheta2 + 1320 * tantheta2 * tantheta2 \
322                                + 720 * tantheta2 * tantheta2 * tantheta2)
323
324        lat_rad = theta - VII * math.pow((easting-e0),2) \
325                                + VIII * math.pow((easting-e0),4) \
326                                - IX * math.pow((easting-e0),6)
327        long_rad = landa0 + X * (easting-e0) \
328                                - XI * math.pow((easting-e0),3) \
329                                + XII * math.pow((easting-e0),5) \
330                                - XIIa * math.pow((easting-e0),7)
331
332        lat = lat_rad / 2.0 / math.pi * 360.0
333        long = long_rad / 2.0 / math.pi * 360.0
334
335        return (lat,long)
336
337##############################################################
338#         Cassini Easting/Northing Transform Methods         #
339##############################################################
340
341def turn_latlong_into_cassini_en(lat_dec,long_dec,scheme):
342        """Latitude and Longitude, into Cassini-Soldner easting and northing co-ordinates, in the given scheme. See http://www.posc.org/Epicentre.2_2/DataModel/ExamplesofUsage/eu_cs34g.html for details of the calculation used"""
343
344        a = abe_values[scheme][0]
345        b = abe_values[scheme][1]
346        e2 = abe_values[scheme][2]
347
348        e4 = e2 * e2
349        e6 = e2 * e2 * e2
350
351        theta = float(lat_dec)  /360.0 *2.0*math.pi
352        landa = float(long_dec) /360.0 *2.0*math.pi
353
354        theta0 = cassini_values[scheme][0]
355        landa0 = cassini_values[scheme][1]
356        false_easting = cassini_values[scheme][2]
357        false_northing = cassini_values[scheme][3]
358
359        # Compute intermediate values
360        A = (landa - landa0) * math.cos(theta)
361        T = math.tan(theta) * math.tan(theta)
362        C = e2 / (1.0 - e2) * math.cos(theta) * math.cos(theta)
363        v = a / math.sqrt( 1 - (e2 * math.sin(theta) * math.sin(theta)) )
364
365        A2 = A ** 2
366        A3 = A ** 3
367        A4 = A ** 4
368        A5 = A ** 5
369
370        # And M, which is how far along the meridian our latitude is from the origin
371        def makeM(picked_theta):
372                return a * (
373                        (1.0 - e2/4.0 - 3.0*e4/64.0 - 5.0*e6/256.0) * picked_theta
374                  - (3.0*e2/8.0 + 3.0*e4/32.0 + 45.0*e6/1024.0) * math.sin(2.0*picked_theta)
375                  + (15.0*e4/256.0 + 45.0*e6/1024.0) * math.sin(4.0*picked_theta)
376                  - (35.0*e6/3072.0) * math.sin(6.0*picked_theta)
377            )
378        M = makeM(theta)
379        M0 = makeM(theta0)
380
381        # Now calculate
382        easting = false_easting + v * (
383                                A - T * A3 / 6.0 - (8.0 - T + 8.0*C) * T * A5 / 120.0 )
384        northing = false_northing + M - M0 + v * math.tan(theta) * (
385                                A2 / 2.0 + (5.0 - T + 6.0*C) * A4 / 24.0 )
386
387        return (easting,northing)
388
389def turn_cassini_en_into_latlong(easting,northing,scheme):
390        """Cassini-Soldner easting and northing, into Latitude and Longitude, in the given scheme. See http://www.posc.org/Epicentre.2_2/DataModel/ExamplesofUsage/eu_cs34g.html for details of the calculation used"""
391
392        a = abe_values[scheme][0]
393        b = abe_values[scheme][1]
394        e2 = abe_values[scheme][2]
395
396        e4 = e2 * e2
397        e6 = e2 * e2 * e2
398
399        theta0 = cassini_values[scheme][0]
400        landa0 = cassini_values[scheme][1]
401        false_easting = cassini_values[scheme][2]
402        false_northing = cassini_values[scheme][3]
403
404        def makeM(picked_theta):
405                return a * (
406                        (1.0 - e2/4.0 - 3.0*e4/64.0 - 5.0*e6/256.0) * picked_theta
407                  - (3.0*e2/8.0 + 3.0*e4/32.0 + 45.0*e6/1024.0) * math.sin(2.0*picked_theta)
408                  + (15.0*e4/256.0 + 45.0*e6/1024.0) * math.sin(4.0*picked_theta)
409                  - (35.0*e6/3072.0) * math.sin(6.0*picked_theta)
410            )
411
412        # Compute first batch of intermediate values
413        M1 = makeM(theta0) + (northing - false_northing)
414        mu1 = M1 / (a * (1.0 - e2/4.0 - 3.0*e4/64.0 - 5.0*e6/256.0) )
415        e1 = (1 - ((1-e2) ** 0.5)) / (1 + ((1-e2) ** 0.5))
416
417        e1_2 = e1 ** 2
418        e1_3 = e1 ** 3
419        e1_4 = e1 ** 4
420
421        # Now compute theta1 at T1
422        theta1 = mu1 + (
423        + (3.0*e1 / 2.0 - 27.0*e1_3 / 32.0) * math.sin(2.0*mu1)
424        + (21.0*e1_2 / 16.0 - 55.0*e1_4 / 32.0) * math.sin(4.0*mu1)
425        + (151.0*e1_3 / 96.0) * math.sin(6.0*mu1)
426        + (1097.0*e1_4 / 512.0) * math.sin(8.0*mu1)
427        )
428        T1 = (math.tan(theta1)) ** 2
429
430        # Now we can find v1, ro1 and D
431        v1 = a / math.sqrt( 1.0 - (e2 * math.sin(theta1) * math.sin(theta1)) )
432        ro1 = a * (1 - e2) / ((1 - e2 * math.sin(theta1) * math.sin(theta1)) ** 1.5)
433        D = (easting - false_easting) / v1
434
435        # And finally the lat and long
436        lat = theta1 - (v1 * math.tan(theta1)) / ro1 * (
437                        D*D/2.0 - (1.0 + 3.0 * T1) * ( (D**4) / 24.0 ) )
438        long = landa0 + (
439                                D - T1 * (D**3) / 3.0 + (1 + 3.0 * T1) * T1 * (D**5) / 15.0
440                        ) / math.cos(theta1)
441
442        # Now make decimal versions
443        lat_dec = lat * 360.0 / 2.0 / math.pi
444        long_dec = long * 360.0 / 2.0 / math.pi
445
446        return (lat_dec,long_dec)
447
448##############################################################
449#             OS Specific Methods Follow                     #
450##############################################################
451
452def turn_easting_northing_into_six_fig(easting,northing):
453        """Turn OS easting and northing values into the six figure OS grid refecence. See http://www.jstott.me.uk/jscoord/"""
454        first_letter = ""
455        second_letter = ""
456
457        easting = int(easting)
458        northing = int(northing)
459
460        # Get the 100 km part
461        hkm_east = int( math.floor(easting / 100000.0) )
462        hkm_north = int( math.floor(northing / 100000.0) )
463        if hkm_north < 5:
464                if hkm_east < 5:
465                        first_letter = "S"
466                else:
467                        first_letter = "T"
468        elif hkm_north < 10:
469                if hkm_east < 5:
470                        first_letter = "N"
471                else:
472                        first_letter = "O"
473        else:
474                first_letter = "H"
475
476        # Get the 10km part
477        index = 65 + ((4 - (hkm_north % 5)) * 5) + (hkm_east % 5)
478        ti = index
479        if index >= 73:
480                index += 1
481        second_letter = chr(index)
482
483        # Get digits 2-4 on easting and northing
484        e = math.floor( (easting  - (100000.0 * hkm_east))  / 100.0)
485        n = math.floor( (northing - (100000.0 * hkm_north)) / 100.0)
486        e = "%03d" % e
487        n = "%03d" % n
488
489        return first_letter + second_letter + e + n
Note: See TracBrowser for help on using the repository browser.