Source code for tracklib.core.ObsCoords

"""
This Module contains the classes to manage point coordinates:

    - :class:`GeoCoords` : For representation og geographic coordinates (lon, lat, alti)
    - :class:`ENUCoords` : For local projection (East, North, Up)
    - :class:`ECEFCoords` : For Earth-Centered-Earth-Fixed coordinates (X, Y, Z)

"""

# For type annotation
from __future__ import annotations   
from typing import Union

import math
import copy
import matplotlib.pyplot as plt

# The current constants are used in this module : 
Re: float = 6378137.0            #: Earth semi-major axis
Be: float = 6356752.314          #: Earth semi-minor axis
Fe: float = 1.0 / 298.257223563  #: Earth flattening
Ee: float = 0.0818191910428      #: Earth eccentricity

STANDARD_PROJ = 1   # 1: flat, 2: stereographic 

[docs]class GeoCoords: """Class to represent geographics coordinates"""
[docs] def __init__(self, lon: float, lat: float, hgt: float = 0.0): """Constructor of :class:`GeoCoords` class :param lon: longitude in decimal degrees :param lat: latitude in decimal degrees :param hgt: height in meters above geoid or ellipsoid, defaults to 0 """ self.lon = lon self.lat = lat self.hgt = hgt
[docs] def __str__(self) -> str: """Transform the object in string :return: String representation of coordinates """ output = "[lon=" + "{:12.9f}".format(self.lon) + ", " output += "lat=" + "{:11.9f}".format(self.lat) + ", " output += "hgt=" + "{:7.3f}".format(self.hgt) + "]" return output
[docs] def copy(self) -> GeoCoords: """TODO""" return copy.deepcopy(self)
[docs] def toECEFCoords(self) -> ECEFCoords: """Convert geodetic coordinates to absolute ECEF :return: absolute ECEF coordinates """ xyz = ECEFCoords(0.0, 0.0, 0.0) e = math.sqrt(Fe * (2 - Fe)) lon = self.lon * math.pi / 180.0 lat = self.lat * math.pi / 180.0 hgt = self.hgt n = Re / math.sqrt(1 - (e * math.sin(lat)) ** 2) xyz.X = (n + hgt) * math.cos(lat) * math.cos(lon) xyz.Y = (n + hgt) * math.cos(lat) * math.sin(lon) xyz.Z = ((1 - e * e) * n + hgt) * math.sin(lat) return xyz
[docs] def toENUCoords(self, base: Union[ECEFCoords, GeoCoords]) -> ENUCoords: """Convert geodetic coordinates to local ENU coords :param base: Base coordinates for conversion :return: Converted coordinates """ # Special SRID projection if isinstance(base, int): return self.toProjCoords(base) # --------------------------------------- # Sterographic projection test # --------------------------------------- if (STANDARD_PROJ == 2): phi = self.lat*math.pi/180 psi = math.atan(Be/Re*math.tan(phi)) w = Be/math.sqrt((Re*math.sin(psi))**2 + (Be*math.cos(psi))**2) N = Re/w rho = Re*(1-Ee**2)/w**3 R = math.sqrt(N*rho) lon = (self.lon-base.lon)*math.pi/180 lat = (self.lat-base.lat)*math.pi/180 x = +R*math.tan(math.pi/4-lat/2)*math.sin(lon)*math.cos(self.lat*math.pi/180) y = -R*math.tan(math.pi/4-lat/2)*math.cos(lon)+6380968.157 output = ENUCoords(x, y, self.hgt) return output # --------------------------------------- # Standard flat projection # --------------------------------------- else: base_ecef = base.toECEFCoords() point_ecef = self.toECEFCoords() return point_ecef.toENUCoords(base_ecef)
[docs] def toGeoCoords(self) -> GeoCoords: """Artificial function to ensure point is GeoCoords :return: Copy of current object """ return self.copy()
[docs] def distanceTo(self, point: GeoCoords) -> float: """Distance between two geodetic coordinates :param point: Geographic coordinate :return: Distance """ return self.toECEFCoords().distanceTo(point.toECEFCoords())
[docs] def distance2DTo(self, point: GeoCoords) -> float: """2D Distance between two geodetic coordinates :param point: Geographic coordinate :return: 2D Distance """ return self.toENUCoords(point).norm2D()
[docs] def elevationTo(self, point: GeoCoords) -> float: """Elevation between two geodetic coordinates :param point: Geographic coordinate :return: Elevation (in rad) """ objectif = point.toENUCoords(self) return math.atan2(objectif.U, objectif.norm2D())
[docs] def azimuthTo(self, point: GeoCoords) -> float: """Azimut between two geodetic coordinates :param point: Geographic coordinate :return: Azimut (in rad) """ objectif = point.toENUCoords(self) return math.atan2(objectif.E, objectif.N)
[docs] def toProjCoords(self, srid_number: int) -> ENUCoords: """Special function to convert to specific ENU srid :param srid_number: A SRID number describing projection coords (e.g. 2154 for Lambert 93) :return: an ENUCoords object """ return _proj(self, srid_number)
# -------------------------------------------------- # Coords Alias X, Y, Z # --------------------------------------------------
[docs] def getX(self) -> float: """Return the X coordinate""" return self.lon
[docs] def getY(self) -> float: """Return the Y coordinate""" return self.lat
[docs] def getZ(self) -> float: """Return the Z coordinate""" return self.hgt
[docs] def setX(self, X: float): """Set the X coordinate :param X: X coordinate """ self.lon = X
[docs] def setY(self, Y: float): """Set the Y coordinate :param Y: Y coordinate """ self.lat = Y
[docs] def setZ(self, Z: float): """Set the Z coordinate :param Z: Z coordinate """ self.hgt = Z
[docs] def plot(self, sym="ro"): """TODO""" plt.plot(self.lon, self.lat, sym)
[docs]class ENUCoords: """Class for representation of local projection (East, North, Up)"""
[docs] def __init__(self, E: float, N: float, U: float = 0): """Constructor of class:`ENUCoords` class :param E: East coordinate (in meters) :param N: North coordinate (in meters) :param U: Elevation (in meter), defaults to 0 """ self.E = E self.N = N self.U = U
[docs] def __str__(self): """Transform the object in string :return: String representation of coordinates """ output = "[E=" + "{:12.3f}".format(self.E) + ", " output += "N=" + "{:12.3f}".format(self.N) + ", " output += "U=" + "{:12.3f}".format(self.U) + "]" return output
[docs] def copy(self) -> ENUCoords: """Copy the current object :return: A copy of current object """ return copy.deepcopy(self)
[docs] def toECEFCoords(self, base: Union[ECEFCoords, GeoCoords]) -> ECEFCoords: """Convert local planimetric to absolute geocentric :param base: Base coordinates :return: Transformet coordinates """ base = base.toECEFCoords() xyz = ECEFCoords(0.0, 0.0, 0.0) e = self.E n = self.N u = self.U base_geo = base.toGeoCoords() blon = base_geo.lon * math.pi / 180.0 blat = base_geo.lat * math.pi / 180.0 slon = math.sin(blon) slat = math.sin(blat) clon = math.cos(blon) clat = math.cos(blat) xyz.X = -e * slon - n * clon * slat + u * clon * clat + base.X xyz.Y = e * clon - n * slon * slat + u * slon * clat + base.Y xyz.Z = n * clat + u * slat + base.Z return xyz
[docs] def toGeoCoords(self, base: Union[ECEFCoords, GeoCoords]) -> GeoCoords: """Convert local ENU coordinates to geo coords :param base: Base coordinates :return: Transformed coordinates """ # Special SRID projection if isinstance(base, int): return _unproj(self, base) base_ecef = base.toECEFCoords() point_ecef = self.toECEFCoords(base_ecef) return point_ecef.toGeoCoords()
[docs] def toENUCoords(self, base1: Union[ECEFCoords, GeoCoords], base2: Union[ECEFCoords, GeoCoords]) -> ENUCoords: """Convert local ENU coordinates relative to base1 to local ENU coordinates relative to base2. :param base1: Base 1 coordinates :param base2: Base 2 coordinates :return: Transformed coordinates """ base_ecef1 = base1.toECEFCoords() base_ecef2 = base2.toECEFCoords() point_ecef = self.toECEFCoords(base_ecef1) return point_ecef.toENUCoords(base_ecef2)
[docs] def norm2D(self) -> float: """Planimetric euclidian norm of point :return: Euclidian norm """ return math.sqrt(self.E ** 2 + self.N ** 2)
[docs] def norm(self) -> float: """R^3 space euclidian norm of point :return: R^3 space euclidian norm of point """ return math.sqrt(self.E ** 2 + self.N ** 2 + self.U ** 2)
[docs] def dot(self, point): """Dot product between two vectors :param point: [description] :return: [description] """ return self.E * point.E + self.N * point.N + self.U * point.U
[docs] def elevationTo(self, point: ENUCoords) -> float: """Elevation between two ENU coordinates :param point: A ENUCoordinate :return: Elevation (in rad) """ visee = point - self return math.atan2(visee.U, visee.norm2D())
[docs] def azimuthTo(self, point: ENUCoords) -> float: """Azimut between two ENU coordinates :param point: A ENUCoordinate :return: Azimut (in rad) """ visee = point - self return math.atan2(visee.E, visee.N)
def __sub__(self, p: ENUCoords) -> ENUCoords: """ Vector difference between two ENU coordinates (AB: B-A). :param p: An ENU coordinate :return: An ENU coordinate """ # return ENUCoords(p.E - self.E, p.N - self.N, p.U - self.U) return ENUCoords(self.E - p.E, self.N - p.N, self.U - p.U) def __add__(self, p: ENUCoords) -> ENUCoords: """Vector addition between two ENU coordinates :param p: An ENU coordinate :return: An ENU coordinate """ return ENUCoords(p.E + self.E, p.N + self.N, p.U + self.U)
[docs] def distance2DTo(self, point: ENUCoords) -> float: """Distance 2D between two ENU coordinates :param point: A ENU coordinate :return: 2D distance """ return (point - self).norm2D()
[docs] def distanceTo(self, point: ENUCoords) -> float: """Distance 3D between two ENU coordinates :param point: A ENU coordinate :return: 2D distance """ return (point - self).norm()
[docs] def rotate(self, theta: float): """Rotation (2D) of point :param theta: Angle of rotation (in rad) """ cr = math.cos(theta) sr = math.sin(theta) xr = +cr * self.E - sr * self.N yr = +sr * self.E + cr * self.N self.E = xr self.N = yr
[docs] def scale(self, h: float): """Homotehtic transformation (2D) of point :param h: factor """ self.E *= h self.N *= h
[docs] def translate(self, tx: float, ty: float, tz: float = 0): """Translation (3D) of point :param tx: X translation :param ty: Y translation :param tz: Z translation, defaults to 0 """ self.E += tx self.N += ty self.U += tz
# -------------------------------------------------- # Coords Alias X, Y, Z # --------------------------------------------------
[docs] def getX(self) -> float: """Return the X coordinate""" return self.E
[docs] def getY(self) -> float: """Return the Y coordinate""" return self.N
[docs] def getZ(self) -> float: """Return the Z coordinate""" return self.U
[docs] def setX(self, X: float): """Set the X coordinate :param X: X coordinate """ self.E = X
[docs] def setY(self, Y: float): """Set the Y coordinate :param Y: Y coordinate """ self.N = Y
[docs] def setZ(self, Z: float): """Set the Z coordinate :param Z: Z coordinate """ self.U = Z
[docs] def plot(self, sym="ro"): """TODO""" plt.plot(self.E, self.N, sym)
[docs]class ECEFCoords: """Class to represent Earth-Centered-Earth-Fixed coordinates""" # -------------------------------------------------- # X, Y, Z in meters # --------------------------------------------------
[docs] def __init__(self, X: float, Y: float, Z: float): """Constructor of :class:`ECEFCoords` class :param X: X corrdinate (meters) :param Y: Y corrdinate (meters) :param Z: Z corrdinate (meters) """ self.X = X self.Y = Y self.Z = Z
[docs] def __str__(self) -> str: """Transform the object in string :return: String representation of coordinates """ output = "[X=" + "{:12.3f}".format(self.X) + ", " output += "Y=" + "{:12.3f}".format(self.Y) + ", " output += "Z=" + "{:12.3f}".format(self.Z) + "]" return output
[docs] def copy(self) -> ECEFCoords: """Copy the current object :return: A copy of current object """ return copy.deepcopy(self)
[docs] def toGeoCoords(self) -> GeoCoords: """Convert absolute geocentric coords to geodetic longitude, latitude and height :return: GeoCoords representation of current coordinates """ geo = GeoCoords(0.0, 0.0, 0.0) b = Re * (1 - Fe) e = math.sqrt(Fe * (2 - Fe)) X = self.X Y = self.Y Z = self.Z h = Re * Re - b * b p = math.sqrt(X * X + Y * Y) t = math.atan2(Z * Re, p * b) geo.lon = math.atan2(Y, X) geo.lat = math.atan2( Z + h / b * pow(math.sin(t), 3), p - h / Re * (math.cos(t)) ** 3 ) n = Re / math.sqrt(1 - (e * math.sin(geo.lat)) ** 2) geo.hgt = (p / math.cos(geo.lat)) - n geo.lon *= 180.0 / math.pi geo.lat *= 180.0 / math.pi return geo
[docs] def toENUCoords(self, base: Union[ECEFCoords, GeoCoords]) -> ENUCoords: """Convert local coordinates to absolute geocentric :param base: Base coordinates :return: Transformed coordinates """ base = base.toECEFCoords() enu = ENUCoords(0.0, 0.0, 0.0) base_geo = base.toGeoCoords() blon = base_geo.lon * math.pi / 180.0 blat = base_geo.lat * math.pi / 180.0 x = self.X - base.X y = self.Y - base.Y z = self.Z - base.Z slon = math.sin(blon) slat = math.sin(blat) clon = math.cos(blon) clat = math.cos(blat) enu.E = -x * slon + y * clon enu.N = -x * clon * slat - y * slon * slat + z * clat enu.U = x * clon * clat + y * slon * clat + z * slat return enu
[docs] def toECEFCoords(self) -> ECEFCoords: """Artificial function to ensure point is ECEFCoords :return: ECEFCoords """ return self.copy()
[docs] def elevationTo(self, point: ECEFCoords) -> float: """Elevation between two ECEF coordinates :param point: Coordinate 2 :return: Elevation (rad) """ objectif = point.toENUCoords(self) return math.atan2(objectif.U, objectif.norm2D())
[docs] def azimuthTo(self, point: ECEFCoords) -> float: """Azimut between two ECEF coordinates :param point: Coordinate 2 :return: Azimuth (rad) """ objectif = point.toENUCoords(self) return math.atan2(objectif.E, objectif.N)
[docs] def dot(self, point): """Dot product between two vectors""" return self.X * point.X + self.Y * point.Y + self.Z * point.Z
[docs] def norm(self) -> float: """R^3 space euclidian norm of point :return: R^3 space euclidian norm """ return math.sqrt(self.dot(self))
[docs] def scalar(self, factor: float): """Scalar multiplication of a vector :param factor: Multiplication factor """ self.X *= factor self.Y *= factor self.Z *= factor
def __sub__(self, p: ECEFCoords) -> ECEFCoords: """Vector difference between two ECEF coordinates :param p: Corrdinate 2 :return: Result of substration """ return ECEFCoords(p.X - self.X, p.Y - self.Y, p.Z - self.Z) def __add__(self, p: ECEFCoords) -> ECEFCoords: """Vector sum between two ECEF coordinates :param p: Corrdinate 2 :return: Result of sum """ return ECEFCoords(p.X + self.X, p.Y + self.Y, p.Z + self.Z)
[docs] def distanceTo(self, point: ECEFCoords) -> float: """Distance between two ECEF coordinates :param point: Corrdinate 2 :return: Distance (meters) """ return (point - self).norm()
[docs] def getX(self) -> float: """Return the X coordinate""" return self.X
[docs] def getY(self) -> float: """Return the X coordinate""" return self.Y
[docs] def getZ(self) -> float: """Return the X coordinate""" return self.Z
[docs] def setX(self, X: float): """Set the X coordinate :param X: X coordinate """ self.X = X
[docs] def setY(self, Y: float): """Set the Y coordinate :param Y: Y coordinate """ self.Y = Y
[docs] def setZ(self, Z: float): """Set the Z coordinate :param Z: Z coordinate """ self.Z = Z
# -------------------------------------------------- # Static projection methods # --------------------------------------------------
[docs]def _proj(coords, srid: int): """TODO""" if srid == 2154: return _projToLambert93(coords) print("Error: SRID code " + str(srid) + " is not implmented in Tracklib") exit()
[docs]def _unproj(coords, srid: int): """TODO""" if srid == 2154: return __projFromLambert93(coords) if (srid >= 32600) and (srid <= 32799): zone = (srid - 32600) % 100 north = srid < 32700 return _projFromUTM(coords, zone, north) print("Error: SRID code " + str(srid) + " is not implmented in Tracklib") exit()
[docs]def __projFromLambert93(coords) -> GeoCoords: """TODO""" E = 0.08181919106 #: TODO Xp = 700000.000 #: TODO Yp = 12655612.050 #: TODO n = 0.725607765053267 #: TODO C = 11754255.4260960 #: TODO lambda0 = 0.0523598775598299 #: TODO X = coords.getX() Y = coords.getY() lon = math.atan(-(X - Xp) / (Y - Yp)) / n + lambda0 latiso = -math.log(math.sqrt((X - Xp) ** 2 + (Y - Yp) ** 2) / C) / n phi = 2 * math.atan(math.exp(latiso)) - math.pi / 2 for i in range(10): phi = 2 * math.atan( ((1 + E * math.sin(phi)) / (1 - E * math.sin(phi))) ** (E / 2) * math.exp(latiso) ) phi -= math.pi / 2 return GeoCoords(lon * 180 / math.pi, phi * 180 / math.pi, coords.getZ())
[docs]def _projToLambert93(coords) -> ENUCoords: """TODO""" E = 0.08181919106 #: TODO Xp = 700000.000 #: TODO Yp = 12655612.050 #: TODO n = 0.725607765053267 #: TODO C = 11754255.4260960 #: TODO lambda0 = 0.0523598775598299 #: TODO lon = coords.getX() * math.pi / 180.0 phi = coords.getY() * math.pi / 180.0 latiso = ((1 - E * math.sin(phi)) / (1 + E * math.sin(phi))) ** (E / 2) latiso = math.tan(math.pi / 4 + phi / 2) * latiso latiso = math.log(latiso) X = Xp + C * math.exp(-n * latiso) * math.sin(n * (lon - lambda0)) Y = Yp - C * math.exp(-n * latiso) * math.cos(n * (lon - lambda0)) return ENUCoords(X, Y, coords.getZ())
# -------------------------------------------------------------------------- # Copyright (C) 2012 Tobias Bieniek <Tobias.Bieniek@gmx.de> # Permission is hereby granted, free of charge, to any person obtaining a # copy of this software and associated documentation files (the "Software"), # to deal in the Software without restriction, including without limitation # the rights to use, copy, modify, merge, publish, distribute, sublicense, # and/or sell copies of the Software, and to permit persons to whom the # Software is furnished to do so, subject to the following conditions: # The above copyright notice and this permission notice shall be included in # all copies or substantial portions of the Software. # -------------------------------------------------------------------------- # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER # DEALINGS IN THE SOFTWARE. # --------------------------------------------------------------------------
[docs]def _projFromUTM(coords, zone, northern=True): """TODO""" x = coords.getX() - 500000 y = coords.getY() zone_number_to_central_longitude = (zone - 1) * 6 - 180 + 3 if not northern: y -= 10000000 K0 = 0.9996 E = 0.00669438 E2 = E * E E3 = E2 * E E_P2 = E / (1.0 - E) SQRT_E = math.sqrt(1 - E) _E = (1 - SQRT_E) / (1 + SQRT_E) _E2 = _E * _E _E3 = _E2 * _E _E4 = _E3 * _E _E5 = _E4 * _E M1 = 1 - E / 4 - 3 * E2 / 64 - 5 * E3 / 256 M2 = 3 * E / 8 + 3 * E2 / 32 + 45 * E3 / 1024 M3 = 15 * E2 / 256 + 45 * E3 / 1024 M4 = 35 * E3 / 3072 P2 = 3.0 / 2 * _E - 27.0 / 32 * _E3 + 269.0 / 512 * _E5 P3 = 21.0 / 16 * _E2 - 55.0 / 32 * _E4 P4 = 151.0 / 96 * _E3 - 417.0 / 128 * _E5 P5 = 1097.0 / 512 * _E4 R = 6378137 m = y / K0 mu = m / (R * M1) p_rad = ( mu + P2 * math.sin(2 * mu) + P3 * math.sin(4 * mu) + P4 * math.sin(6 * mu) + P5 * math.sin(8 * mu) ) p_sin = math.sin(p_rad) p_sin2 = p_sin * p_sin p_cos = math.cos(p_rad) p_tan = p_sin / p_cos p_tan2 = p_tan * p_tan p_tan4 = p_tan2 * p_tan2 ep_sin = 1 - E * p_sin2 ep_sin_sqrt = math.sqrt(1 - E * p_sin2) n = R / ep_sin_sqrt r = (1 - E) / ep_sin c = E_P2 * p_cos ** 2 c2 = c * c d = x / (n * K0) d2 = d * d d3 = d2 * d d4 = d3 * d d5 = d4 * d d6 = d5 * d latitude = ( p_rad - (p_tan / r) * (d2 / 2 - d4 / 24 * (5 + 3 * p_tan2 + 10 * c - 4 * c2 - 9 * E_P2)) + d6 / 720 * (61 + 90 * p_tan2 + 298 * c + 45 * p_tan4 - 252 * E_P2 - 3 * c2) ) longitude = ( d - d3 / 6 * (1 + 2 * p_tan2 + c) + d5 / 120 * (5 - 2 * c + 28 * p_tan2 - 3 * c2 + 8 * E_P2 + 24 * p_tan4) ) / p_cos longitude = longitude + math.radians( zone_number_to_central_longitude ) # !!!! mod angle return GeoCoords(longitude * 180 / math.pi, latitude * 180 / math.pi, coords.getZ())