Source code for morse.helpers.coordinates

import logging; logger = logging.getLogger("morse." + __name__)
from math import sqrt, cos, sin, tan, atan, atan2, radians, degrees, pi
from morse.core import blenderapi
from morse.helpers.morse_math import normalise_angle
import numpy

[docs]class CoordinateConverter: """ Allow to convert coordinates from Geodetic to LTP to ECEF-r ... """ A = 6378137.0 # WGS-84 Earth semi-major axis F = 1 / 298.257223563 # WGS-84 flattening ECC = 8.181919191e-2 # first excentricity R = 6378137.0 # Radius of Earth at equator A2 = A**2 ECC2 = ECC**2 ECC4 = ECC**4 _instance = None def __init__(self, latitude, longitude, altitude, angle_east_blender_x): P = [radians(longitude), radians(latitude), altitude] self.origin_ecef = self.geodetic_to_ecef(numpy.matrix(P)) _rot = \ [[-sin(P[0]), cos(P[0]), 0], [-cos(P[0]) * sin(P[1]), -sin(P[1]) * sin(P[0]), cos(P[1])], [cos(P[1]) * cos(P[0]), cos(P[1]) * sin(P[0]), sin(P[1])]] self._rot_ltp_ecef = numpy.matrix(_rot) self._rot_ecef_ltp = self._rot_ltp_ecef.T _rot_east_x = \ [[cos(angle_east_blender_x), -sin(angle_east_blender_x), 0], [sin(angle_east_blender_x), cos(angle_east_blender_x), 0], [0, 0, 1]] self._angle_east = angle_east_blender_x self._rot_blender_ltp = numpy.matrix(_rot_east_x ) self._rot_ltp_blender = self._rot_blender_ltp.T
[docs] @staticmethod def instance(): if not CoordinateConverter._instance: try: ssr = blenderapi.getssr() latitude = ssr["latitude"] longitude = ssr["longitude"] altitude = ssr["altitude"] try: angle_against_east = radians(ssr["angle_against_north"]) - pi / 2 except KeyError as e: angle_against_east = 0.0 CoordinateConverter._instance = \ CoordinateConverter(latitude, longitude, altitude, angle_against_east) except KeyError as e: logger.error("Missing environment parameter %s\n", e) return CoordinateConverter._instance
[docs] def geodetic_to_ecef(self, P): """ converts gps-data(radians) to ECEF-r coordinates """ lg = P[0, 0] la = P[0, 1] h = P[0, 2] N = self.A/sqrt(1-(self.ECC2*(sin(la)**2))) return numpy.matrix([ (h + N)*cos(la)*cos(lg), (h + N)*cos(la)*sin(lg), (h + (1 - self.ECC2) * N)*sin(la)])
[docs] def ltp_to_ecef(self, xt): """ converts point in LTP(Blender) to ECEF-r coordinates """ return self.origin_ecef + xt * self._rot_ltp_ecef #transformed xt -> xe
[docs] def ecef_to_ltp(self, xt): """ converts point in ECEF-r coordinates to LTP(Blender) """ return (xt - self.origin_ecef) * self._rot_ecef_ltp
[docs] def ecef_to_geodetic(self, xe): """ converts point in ECEF-r coordinates into Geodetic (GPS) via Vermeille's method """ x = xe[0, 0] y = xe[0, 1] z = xe[0, 2] #"just intermediary parameters" see FoIz p = (x**2+y**2)/self.A2 q = (1-self.ECC2)/self.A2*z**2 r = (p+q-self.ECC4)/6 s = self.ECC4 * (p*q)/(4*r**3) t = (1+s+sqrt(s*(2+s)))**(1/3.0) u = r*(1+t+1/t) v = sqrt(u**2+(self.ECC4*q)) w = self.ECC2*((u+v-q)/(2*v)) k = sqrt(u+v+w**2)-w D = (k*(sqrt(x**2+y**2)))/(k+self.ECC2) gps_coords = numpy.matrix([ 2*atan(y/(x+(sqrt(x**2+y**2)))), 2*atan(z/(D+sqrt(D**2+z**2))), ((k+self.ECC2-1)/k)*sqrt(D**2+z**2)]) return gps_coords
[docs] def ltp_to_geodetic(self, xe): return self.ecef_to_geodetic(self.ltp_to_ecef(xe))
[docs] def geodetic_to_ltp(self, xe): return self.ecef_to_ltp(self.geodetic_to_ecef(xe))
[docs] def geodetic_to_geocentric(self, lat, h): """ Convert geodetic latitude to geocentric latitude :param: latitude geodetic latitude in degree :param: h height against sea level in meter :return: geocentric latitude in degree """ lat_rad = radians(lat) lat_surface = atan((1 - self.F)**2 *tan(lat_rad)) sin_lat = sin(lat_rad) cos_lat = cos(lat_rad) s1 = h * sin_lat + self.R * sin(lat_surface) cc = h * cos_lat + self.R * cos(lat_surface) lat_geoc = atan(s1 / cc) return degrees(lat_geoc)
[docs] def geocentric_to_ecef(self, xt): longitude = xt[0, 0] latitude = xt[0, 1] radius = xt[0, 2] clon = cos(longitude) slon = sin(longitude) clat = cos(latitude) slat = sin(latitude) return radius * numpy.matrix([ clat * clon, clat * slon, slat])
[docs] def ecef_to_geocentric(self, xt): x = xt[0, 0] y = xt[0, 1] z = xt[0, 2] longitude = atan2(y, x) nxy = sqrt(x * x + y * y) latitude = atan2(z, nxy); radius = sqrt(x * x + y * y + z * z) return numpy.matrix([longitude, latitude, radius])
[docs] def blender_to_ltp(self, xt): return xt * self._rot_blender_ltp
[docs] def ltp_to_blender(self, xt): return xt * self._rot_ltp_blender
[docs] def angle_against_geographic_north(self, orientation): """ Return the angle against geographic_north, as returned by a compass, i.e. between [0, 2 pi], clockwise """ res = pi / 2 - (orientation[2] - self._angle_east) while res < 0: res += 2 * pi return res