coordinates.py 7.85 KB
 Thomas Albrecht committed Jun 08, 2014 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 ``````# -*- coding: utf-8 -*- """ Transform global (aka geodetic) coordinates to a local cartesian, in meters. A flat earth approximation (http://williams.best.vwh.net/avform.htm) seems good enough if distances are up to a few km. Alternatively, use UTM, but that fails if the origin is near an UTM zone boundary. Also, this requires the utm python package (pip install utm). The correct approach, though, is probably to do exactly what FG does, which I think is - transform geodetic to geocentric coordinates (find a python lib for that) - from there, compute the (geocentric) Cartesian coordinates as described here: http://www.flightgear.org/Docs/Scenery/CoordinateSystem/CoordinateSystem.html - project them onto local Cartesian (including correct up vector etc) Created on Sat Jun 7 22:38:59 2014 @author: albrecht """ #http://williams.best.vwh.net/avform.htm #Local, flat earth approximation `````` Thomas Albrecht committed Sep 16, 2014 21 22 23 24 25 ``````# If you stay in the vicinity of a given fixed point (lat0,lon0), it may be a # good enough approximation to consider the earth as "flat", and use a North, # East, Down rectangular coordinate system with origin at the fixed point. If # we call the changes in latitude and longitude dlat=lat-lat0, dlon=lon-lon0 # (Here treating North and East as positive!), then `````` Thomas Albrecht committed Jun 08, 2014 26 27 28 29 ``````# # distance_North=R1*dlat # distance_East=R2*cos(lat0)*dlon # `````` Thomas Albrecht committed Sep 16, 2014 30 31 ``````# R1 and R2 are called the meridional radius of curvature and the radius of # curvature in the prime vertical, respectively. `````` Thomas Albrecht committed Jun 08, 2014 32 33 34 ``````# # R1=a(1-e^2)/(1-e^2*(sin(lat0))^2)^(3/2) # R2=a/sqrt(1-e^2*(sin(lat0))^2) `````` Thomas Albrecht committed Apr 21, 2013 35 ``````# `````` Thomas Albrecht committed Sep 16, 2014 36 37 ``````# a is the equatorial radius of the earth (=6378.137000km for WGS84), and # e^2=f*(2-f) with the flattening f=1/298.257223563 for WGS84. `````` Thomas Albrecht committed Apr 21, 2013 38 ``````# `````` Thomas Albrecht committed Sep 16, 2014 39 40 ``````# In the spherical model used elsewhere in the Formulary, R1=R2=R, the earth's # radius. (using R=1 we get distances in radians, using R=60*180/pi distances are in nm.) `````` Thomas Albrecht committed Apr 21, 2013 41 ``````# `````` Thomas Albrecht committed Sep 16, 2014 42 43 ``````# In the flat earth approximation, distances and bearings are given by the # usual plane trigonometry formulae, i.e: `````` Thomas Albrecht committed Jun 08, 2014 44 45 46 47 48 ``````# # distance = sqrt(distance_North^2 + distance_East^2) # bearing to (lat,lon) = mod(atan2(distance_East, distance_North), 2*pi) # (= mod(atan2(cos(lat0)*dlon, dlat), 2*pi) in the spherical case) # `````` Thomas Albrecht committed Sep 16, 2014 49 50 ``````# These approximations fail in the vicinity of either pole and at large # distances. The fractional errors are of order (distance/R)^2. `````` Thomas Albrecht committed Apr 21, 2013 51 52 `````` `````` Rick Gruber-Riemer committed Jan 01, 2016 53 ``````from math import acos, asin, atan2, sin, cos, sqrt, radians, degrees, pi `````` Thomas Albrecht committed Apr 21, 2013 54 ``````import logging `````` Rick Gruber-Riemer committed Jan 01, 2016 55 56 ``````import unittest `````` Thomas Albrecht committed Apr 21, 2013 57 `````` `````` Rick Gruber-Riemer committed Jan 01, 2016 58 59 60 ``````NAUTICAL_MILES_METERS = 1852 `````` Thomas Albrecht committed Jun 08, 2014 61 62 63 ``````class Transformation(object): """global <-> local coordinate system transformation, using flat earth approximation http://williams.best.vwh.net/avform.htm#flat `````` Thomas Albrecht committed Apr 21, 2013 64 `````` """ `````` Rick Gruber-Riemer committed Sep 04, 2016 65 66 `````` def __init__(self, xxx_todo_changeme=(0, 0), hdg=0): (lon, lat) = xxx_todo_changeme `````` Thomas Albrecht committed Jun 08, 2014 67 68 69 `````` if hdg != 0.: logging.error("heading != 0 not yet implemented.") raise NotImplemented `````` Thomas Albrecht committed Apr 21, 2013 70 71 `````` self._lon = lon self._lat = lat `````` Thomas Albrecht committed Jun 08, 2014 72 `````` self._update() `````` Thomas Albrecht committed Apr 21, 2013 73 74 `````` def _update(self): `````` Thomas Albrecht committed Jun 08, 2014 75 76 77 78 `````` """compute radii for local origin""" a = 6378137.000 # m for WGS84 f=1./298.257223563 e2 = f*(2.-f) `````` Thomas Albrecht committed Apr 21, 2013 79 `````` `````` Thomas Albrecht committed Jun 08, 2014 80 81 82 `````` self._coslat = cos(radians(self._lat)) sinlat = sin(radians(self._lat)) self._R1 = a*(1.-e2)/(1.-e2*(sinlat**2))**(3./2.) `````` Rick Gruber-Riemer committed Jan 01, 2016 83 `````` self._R2 = a/sqrt(1-e2*sinlat**2) `````` Thomas Albrecht committed Apr 21, 2013 84 `````` `````` Rick Gruber-Riemer committed Sep 04, 2016 85 `````` def setOrigin(self, xxx_todo_changeme1): `````` Thomas Albrecht committed Apr 21, 2013 86 `````` """set origin to given global coordinates (lon, lat)""" `````` Rick Gruber-Riemer committed Sep 04, 2016 87 `````` (lon, lat) = xxx_todo_changeme1 `````` Thomas Albrecht committed Apr 21, 2013 88 89 90 91 92 93 94 95 96 `````` self._lon, self._lat = lon, lat self._update() def getOrigin(self): """return origin in global coordinates""" return self._lat, self._lon origin = property(getOrigin, setOrigin) `````` Rick Gruber-Riemer committed Sep 04, 2016 97 `````` def toLocal(self, xxx_todo_changeme2): `````` Thomas Albrecht committed Jun 08, 2014 98 `````` """transform global -> local coordinates""" `````` Rick Gruber-Riemer committed Sep 04, 2016 99 `````` (lon, lat) = xxx_todo_changeme2 `````` Thomas Albrecht committed Jun 08, 2014 100 101 102 `````` y = self._R1 * radians(lat - self._lat) x = self._R2 * radians(lon - self._lon) * self._coslat return x, y `````` Thomas Albrecht committed Apr 21, 2013 103 `````` `````` Rick Gruber-Riemer committed Sep 04, 2016 104 `````` def toGlobal(self, xxx_todo_changeme3): `````` Thomas Albrecht committed Jun 08, 2014 105 `````` """transform local -> global coordinates""" `````` Rick Gruber-Riemer committed Sep 04, 2016 106 `````` (x, y) = xxx_todo_changeme3 `````` Thomas Albrecht committed Jun 08, 2014 107 108 109 `````` lat = degrees(y / self._R1) + self._lat lon = degrees(x / (self._R2 * self._coslat)) + self._lon return lon, lat `````` Thomas Albrecht committed Apr 21, 2013 110 111 `````` def __str__(self): `````` Thomas Albrecht committed Jun 08, 2014 112 `````` return "(%f %f)" % (self._lon, self._lat) `````` Thomas Albrecht committed Apr 21, 2013 113 114 `````` `````` Rick Gruber-Riemer committed Jan 01, 2016 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 ``````def calc_angle_of_line_local(x1, y1, x2, y2): """Returns the angle in degrees of a line relative to North. Based on local coordinates (x,y) of two points. """ angle = atan2(x2 - x1, y2 - y1) degree = degrees(angle) if degree < 0: degree += 360 return degree def calc_distance_local(x1, y1, x2, y2): """Returns the distance between two points based on local coordindates (x,y).""" return sqrt(pow(x1 - x2, 2) + pow(y1 - y2, 2)) `````` Rick Gruber-Riemer committed Jan 01, 2016 131 132 133 134 135 136 137 ``````def calc_distance_global(lon1, lat1, lon2, lat2): lon1_r = radians(lon1) lat1_r = radians(lat1) lon2_r = radians(lon2) lat2_r = radians(lat2) distance_radians = calc_distance_global_radians(lon1_r, lat1_r, lon2_r, lat2_r) return distance_radians*((180*60)/pi) * NAUTICAL_MILES_METERS `````` Thomas Albrecht committed Apr 21, 2013 138 139 `````` `````` Rick Gruber-Riemer committed Jan 01, 2016 140 141 ``````def calc_distance_global_radians(lon1_r, lat1_r, lon2_r, lat2_r): return 2*asin(sqrt(pow(sin((lat1_r-lat2_r)/2), 2) + cos(lat1_r)*cos(lat2_r)*pow(sin((lon1_r-lon2_r)/2), 2))) `````` Thomas Albrecht committed Apr 21, 2013 142 143 `````` `````` Rick Gruber-Riemer committed Jan 01, 2016 144 145 146 147 148 149 150 151 152 153 154 155 ``````def calc_angle_of_line_global(lon1, lat1, lon2, lat2): lon1_r = radians(lon1) lat1_r = radians(lat1) lon2_r = radians(lon2) lat2_r = radians(lat2) d = calc_distance_global_radians(lon1_r, lat1_r, lon2_r, lat2_r) if sin(lon2_r-lon1_r) > 0: angle = acos((sin(lat2_r)-sin(lat1_r)*cos(d))/(sin(d)*cos(lat1_r))) else: angle = 2*pi-acos((sin(lat2_r)-sin(lat1_r)*cos(d))/(sin(d)*cos(lat1_r))) angle = degrees(angle) return angle `````` Thomas Albrecht committed Apr 21, 2013 156 157 `````` `````` Thomas Albrecht committed Jun 08, 2014 158 159 ``````if __name__ == "__main__": t = Transformation((0, 0)) `````` Rick Gruber-Riemer committed Sep 04, 2016 160 161 162 163 164 165 166 167 `````` print(t.toLocal((0., 0))) print(t.toLocal((1., 0))) print(t.toLocal((0, 1.))) print() print(t.toGlobal((100., 0))) print(t.toGlobal((1000., 0))) print(t.toGlobal((10000., 0))) print(t.toGlobal((100000., 0))) `````` Rick Gruber-Riemer committed Jan 01, 2016 168 169 170 171 `````` # ================ UNITTESTS ======================= `````` Rick Gruber-Riemer committed Jan 01, 2016 172 173 ``````class TestCoordinates(unittest.TestCase): def test_calc_angle_of_line_local(self): `````` Rick Gruber-Riemer committed Jan 01, 2016 174 175 176 177 178 179 180 181 `````` self.assertEqual(0, calc_angle_of_line_local(0, 0, 0, 1), "North") self.assertEqual(90, calc_angle_of_line_local(0, 0, 1, 0), "East") self.assertEqual(180, calc_angle_of_line_local(0, 1, 0, 0), "South") self.assertEqual(270, calc_angle_of_line_local(1, 0, 0, 0), "West") self.assertEqual(45, calc_angle_of_line_local(0, 0, 1, 1), "North East") self.assertEqual(315, calc_angle_of_line_local(1, 0, 0, 1), "North West") self.assertEqual(225, calc_angle_of_line_local(1, 1, 0, 0), "South West") `````` Rick Gruber-Riemer committed Jan 01, 2016 182 183 184 185 186 187 188 189 190 191 `````` def test_calc_angle_of_line_global(self): self.assertAlmostEqual(360, calc_angle_of_line_global(1, 46, 1, 47), delta=0.5) # "North" self.assertAlmostEqual(90, calc_angle_of_line_global(1, -46, 2, -46), delta=0.5) # "East" self.assertAlmostEqual(180, calc_angle_of_line_global(-1, -33, -1, -34), delta=0.5) # "South" self.assertAlmostEqual(270, calc_angle_of_line_global(-29, 20, -30, 20), delta=0.5) # "West" self.assertAlmostEqual(45, calc_angle_of_line_global(0, 0, 1, 1), delta=0.5) # "North East" self.assertAlmostEqual(315, calc_angle_of_line_global(1, 0, 0, 1), delta=0.5) # "North West" self.assertAlmostEqual(225, calc_angle_of_line_global(1, 1, 0, 0), delta=0.5) # "South West" def test_calc_distance_local(self): `````` Rick Gruber-Riemer committed Jan 01, 2016 192 193 `````` self.assertEqual(5, calc_distance_local(0, -1, -4, 2)) `````` Rick Gruber-Riemer committed Jan 01, 2016 194 195 196 197 198 `````` def test_calc_distance_global(self): self.assertAlmostEqual(NAUTICAL_MILES_METERS * 60, calc_distance_global(1, 46, 1, 47), delta=10) self.assertAlmostEqual(NAUTICAL_MILES_METERS * 60, calc_distance_global(1, -33, 1, -34), delta=10) self.assertAlmostEqual(NAUTICAL_MILES_METERS * 60, calc_distance_global(1, 0, 2, 0), delta=10) self.assertAlmostEqual(NAUTICAL_MILES_METERS * 60 * sqrt(2), calc_distance_global(1, 0, 2, 1), delta=10)``````