coordinates.py 7.85 KB
Newer Older
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
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
26 27 28 29
#
#       distance_North=R1*dlat
#       distance_East=R2*cos(lat0)*dlon
#
30 31
# R1 and R2 are called the meridional radius of curvature and the radius of 
# curvature in the prime vertical, respectively.
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)
35
#
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.
38
#
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.)
41
#
42 43
# In the flat earth approximation, distances and bearings are given by the
# usual plane trigonometry formulae, i.e:
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)
#
49 50
# These approximations fail in the vicinity of either pole and at large 
# distances. The fractional errors are of order (distance/R)^2.
51 52


53
from math import acos, asin, atan2, sin, cos, sqrt, radians, degrees, pi
54
import logging
55 56
import unittest

57

58 59 60
NAUTICAL_MILES_METERS = 1852


61 62 63
class Transformation(object):
    """global <-> local coordinate system transformation, using flat earth approximation
       http://williams.best.vwh.net/avform.htm#flat
64
    """
65 66
    def __init__(self, xxx_todo_changeme=(0, 0), hdg=0):
        (lon, lat) = xxx_todo_changeme
67 68 69
        if hdg != 0.:
            logging.error("heading != 0 not yet implemented.")
            raise NotImplemented
70 71
        self._lon = lon
        self._lat = lat
72
        self._update()
73 74

    def _update(self):
75 76 77 78
        """compute radii for local origin"""
        a = 6378137.000 # m for WGS84
        f=1./298.257223563
        e2 = f*(2.-f)
79

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.)
83
        self._R2 = a/sqrt(1-e2*sinlat**2)
84

85
    def setOrigin(self, xxx_todo_changeme1):
86
        """set origin to given global coordinates (lon, lat)"""
87
        (lon, lat) = xxx_todo_changeme1
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)

97
    def toLocal(self, xxx_todo_changeme2):
98
        """transform global -> local coordinates"""
99
        (lon, lat) = xxx_todo_changeme2
100 101 102
        y = self._R1 * radians(lat - self._lat)
        x = self._R2 * radians(lon - self._lon) * self._coslat
        return x, y
103

104
    def toGlobal(self, xxx_todo_changeme3):
105
        """transform local -> global coordinates"""
106
        (x, y) = xxx_todo_changeme3
107 108 109
        lat = degrees(y / self._R1) + self._lat
        lon = degrees(x / (self._R2 * self._coslat)) + self._lon
        return lon, lat
110 111

    def __str__(self):
112
        return "(%f %f)" % (self._lon, self._lat)
113 114


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))


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
138 139


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)))
142 143


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
156 157


158 159
if __name__ == "__main__":
    t = Transformation((0, 0))
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)))
168 169 170 171

# ================ UNITTESTS =======================


172 173
class TestCoordinates(unittest.TestCase):
    def test_calc_angle_of_line_local(self):
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")

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):
192 193
        self.assertEqual(5, calc_distance_local(0, -1, -4, 2))

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)