<commit_before>#!/usr/bin/env python3
# <NAME> ported and adaptation from
# GNU Octave Mapping Toolbox by
# Copyright (c) 2013, <NAME>
# Copyright (c) 2013, <NAME>
from numpy import sin,cos,tan,sqrt,radians,arctan2,hypot,degrees,any,mod


class EarthEllipsoid:
    def __init__(self):
        self.a = 6378137.0  # semi-major axis [m]
        self.f = 1.0 / 298.2572235630  # flattening
        self.b = self.a * (1 - self.f)  # semi-minor axis
        self.e = sqrt ( (self.a**2 - self.b**2) / (self.a**2)) # first eccentricity

# Parameters needed to evaluate the geopotential.
# I got them from the US National Geospatial-Inteligence Agency's
# website, "NGA/NASA EGM96, N=M=360 Earth Gravity Model"
# (NGA > Products and Services >Geospatial Sciences Division
# > Physical Geodesy Home)
# <http://earth-info.nga.mil/GandG/wgsegm/egm96.html>
#
# Earth's Gravitational Constant w/ atmosphere:
        self.GM = 0.3986004418e15  # m^3/s^2

        # Earth's angular speed:
        self.omega = 7292115.0e-11  # rad/s

# Dynamical form factor of the Earth:
# The four defining paramenters of WGS-84 are
# GM, omega, a, and 1/f. We need J2 instead of f:
       # self.J2 = get_J2 (self.a, self.b, self.omega, self.GM)

def aer2ecef(az,el,srange,lat0,lon0,alt0,ell=EarthEllipsoid()):
    # Origin of the local system in geocentric coordinates.
    x0,y0,z0 = geodetic2ecef(lat0,lon0,alt0,ell)
    # Convert Local Spherical AER to ENU
    e1, n1, u1 = aer2enu(az, el, srange)
    # Rotating ENU to ECEF
    dx, dy, dz = enu2ecef_int(e1, n1, u1, lat0, lon0)
    # Origin + offset from origin equals position in ECEF
    x1 = x0 + dx
    y1 = y0 + dy
    z1 = z0 + dz

    return x1,y1,z1

def aer2enu(az,el,srange):
    #Calculation of AER2ENU
    u1 = srange * sin(radians(el))
    r   = srange * cos(radians(el))
    e1  = r * sin(radians(az))
    n1 = r * cos(radians(az))

    return e1,n1,u1

def aer2geodetic(az,el,srange,lat0,lon0,alt0):
#angles in DEGREES
#range in METERS
    x,y,z = aer2ecef(az,el,srange,lat0,lon0,alt0)

    lat1,lon1,alt1 = ecef2geodetic(x,y,z)
    return degrees(lat1),degrees(lon1),alt1

def aer2ned(az,elev,slantRange):
    xNorth,yEast,zUp = aer2enu(az,elev,slantRange)
    zDown = -zUp
    return xNorth,yEast,zDown

def ecef2aer(x, y, z, lat0, lon0, h0, ell=EarthEllipsoid()):
    xEast, yNorth, zUp = ecef2enu(x, y, z, lat0, lon0, h0, ell)
    az,elev,slantRange = enu2aer(xEast, yNorth, zUp)
    return az,elev,slantRange

def ecef2enu(x, y, z, lat0, lon0, h0, ell=EarthEllipsoid()):
    x0,y0,z0 = geodetic2ecef(lat0, lon0, h0,ell)
    xEast, yNorth, zUp = ecef2enu_int(x - x0, y - y0, z - z0, lat0, lon0)
    return xEast,yNorth,zUp

def ecef2enu_int(u, v, w, lat0, lon0):
    t     =  cos(radians(lon0)) * u + sin(radians(lon0)) * v
    East  = -sin(radians(lon0)) * u + cos(radians(lon0)) * v
    Up    =  cos(radians(lat0)) * t + sin(radians(lat0)) * w
    North = -sin(radians(lat0)) * t + cos(radians(lat0)) * w
    return East, North, Up

def ecef2geodetic(x,y,z,ell=EarthEllipsoid()):
    #Algorithm is based on
    #http://www.astro.uni.torun.pl/~kb/Papers/geod/Geod-BG.htm
    #This algorithm provides a converging solution to the latitude equation
    #in terms of the parametric or reduced latitude form (v)
    #This algorithm provides a uniform solution over all latitudes as it does
    #not involve division by cos(phi) or sin(phi)
    a = ell.a
    b = ell.b
    r = hypot(x,y)
# Constant required for Latitude equation
    rho = arctan2(b*z,a*r)
#Constant required for latitude equation
    c = (a**2 - b**2) / sqrt((a*r)**2 + (b*z)**2)
# Starter for the Newtons Iteration Method
    vnew = arctan2(a*z,b*r)
# Initializing the parametric latitude
    v = 0.
    count = 0
    while any(v != vnew) and count<5:
        v = vnew
# Derivative of latitude equation
        w = 2*(cos(v - rho) - c*cos(2.*v))
# Newtons Method for computing iterations
        vnew = v - ((2*sin(v-rho)-c*sin(2.*v))/w)
 #       print(count)
        count+=1

    #Computing latitude from the root of the latitude equation
    lat = arctan2((a*tan(vnew)),b);
    #by inspection
    lon = arctan2(y,x)

    alt = ((r-(a*cos(vnew)))*cos(lat)) + ((z-(b*sin(vnew)))*sin(lat))

    return lat,lon,alt

def ecef2ned(x, y, z, lat0, lon0, h0, ell=EarthEllipsoid()):
    yEast, xNorth, zUp = ecef2enu(x, y, z, lat0, lon0, h0, ell)
    zDown = -zUp
    return North, yEast, zDown
<commit_msg>fixing geodetic2ecef<commit_after>#!/usr/bin/env python3
# <NAME> ported and adaptation from
# GNU Octave Mapping Toolbox by
# Copyright (c) 2013, <NAME>
# Copyright (c) 2013, <NAME>
from numpy import sin,cos,tan,sqrt,radians,arctan2,hypot,degrees,any,mod,all


class EarthEllipsoid:
    def __init__(self):
        self.a = 6378137.0  # semi-major axis [m]
        self.f = 1.0 / 298.2572235630  # flattening
        self.b = self.a * (1 - self.f)  # semi-minor axis
        self.e = sqrt ( (self.a**2 - self.b**2) / (self.a**2)) # first eccentricity

# Parameters needed to evaluate the geopotential.
# I got them from the US National Geospatial-Inteligence Agency's
# website, "NGA/NASA EGM96, N=M=360 Earth Gravity Model"
# (NGA > Products and Services >Geospatial Sciences Division
# > Physical Geodesy Home)
# <http://earth-info.nga.mil/GandG/wgsegm/egm96.html>
#
# Earth's Gravitational Constant w/ atmosphere:
        self.GM = 0.3986004418e15  # m^3/s^2

        # Earth's angular speed:
        self.omega = 7292115.0e-11  # rad/s

# Dynamical form factor of the Earth:
# The four defining paramenters of WGS-84 are
# GM, omega, a, and 1/f. We need J2 instead of f:
       # self.J2 = get_J2 (self.a, self.b, self.omega, self.GM)

def aer2ecef(az,el,srange,lat0,lon0,alt0,ell=EarthEllipsoid()):
    # Origin of the local system in geocentric coordinates.
    x0,y0,z0 = geodetic2ecef(lat0,lon0,alt0,ell)
    # Convert Local Spherical AER to ENU
    e1, n1, u1 = aer2enu(az, el, srange)
    # Rotating ENU to ECEF
    dx, dy, dz = enu2ecef_int(e1, n1, u1, lat0, lon0)
    # Origin + offset from origin equals position in ECEF
    x1 = x0 + dx
    y1 = y0 + dy
    z1 = z0 + dz

    return x1,y1,z1

def aer2enu(az,el,srange):
    #Calculation of AER2ENU
    u1 = srange * sin(radians(el))
    r   = srange * cos(radians(el))
    e1  = r * sin(radians(az))
    n1 = r * cos(radians(az))

    return e1,n1,u1

def aer2geodetic(az,el,srange,lat0,lon0,alt0):
#angles in DEGREES
#range in METERS
    x,y,z = aer2ecef(az,el,srange,lat0,lon0,alt0)

    lat1,lon1,alt1 = ecef2geodetic(x,y,z)
    return degrees(lat1),degrees(lon1),alt1

def aer2ned(az,elev,slantRange):
    xNorth,yEast,zUp = aer2enu(az,elev,slantRange)
    zDown = -zUp
    return xNorth,yEast,zDown

def ecef2aer(x, y, z, lat0, lon0, h0, ell=EarthEllipsoid()):
    xEast, yNorth, zUp = ecef2enu(x, y, z, lat0, lon0, h0, ell)
    az,elev,slantRange = enu2aer(xEast, yNorth, zUp)
    return az,elev,slantRange

def ecef2enu(x, y, z, lat0, lon0, h0, ell=EarthEllipsoid()):
    x0,y0,z0 = geodetic2ecef(lat0, lon0, h0,ell)
    xEast, yNorth, zUp = ecef2enu_int(x - x0, y - y0, z - z0, lat0, lon0)
    return xEast,yNorth,zUp

def ecef2enu_int(u, v, w, lat0, lon0):
    t     =  cos(radians(lon0)) * u + sin(radians(lon0)) * v
    East  = -sin(radians(lon0)) * u + cos(radians(lon0)) * v
    Up    =  cos(radians(lat0)) * t + sin(radians(lat0)) * w
    North = -sin(radians(lat0)) * t + cos(radians(lat0)) * w
    return East, North, Up

def ecef2geodetic(x,y,z,ell=EarthEllipsoid()):
    #Algorithm is based on
    #http://www.astro.uni.torun.pl/~kb/Papers/geod/Geod-BG.htm
    #This algorithm provides a converging solution to the latitude equation
    #in terms of the parametric or reduced latitude form (v)
    #This algorithm provides a uniform solution over all latitudes as it does
    #not involve division by cos(phi) or sin(phi)
    a = ell.a
    b = ell.b
    r = hypot(x,y)
# Constant required for Latitude equation
    rho = arctan2(b*z,a*r)
#Constant required for latitude equation
    c = (a**2 - b**2) / sqrt((a*r)**2 + (b*z)**2)
# Starter for the Newtons Iteration Method
    vnew = arctan2(a*z,b*r)
# Initializing the parametric latitude
    v = 0.
    count = 0
    while any(v != vnew) and count<5:
        v = vnew
# Derivative of latitude equation
        w = 2*(cos(v - rho) - c*cos(2.*v))
# Newtons Method for computing iterations
        vnew = v - ((2*sin(v-rho)-c*sin(2.*v))/w)
 #       print(count)
        count+=1

    #Computing latitude from the root of the latitude equation
    lat = degrees(arctan2((a*tan(vnew)),b));
    #by inspection
    lon = degrees(arctan2(y,x))

    alt = ((r-(a*cos(vnew)))*cos(lat)) + ((z-(b*sin(vnew)))*sin(lat))

    #assert all(-90<=lat) and all(lat<=90)
    #assert all(-180<=lon) and all(lon<=180)

    return lat,lon,alt

def ecef2ned(x, y, z, lat0, lon0, h0, ell=EarthEllipsoid()):
    yEast, xNorth, zUp = ecef2enu(x, y, z, lat0, lon0, h0, ell)
    zDown = -zUp
    return North, yEast, zDown

{'fraction_non_alphanumeric': 0.14018796151264265, 'fraction_numerical': 0.0441933318415753, 'mean_word_length': 3.283181600383325, 'pattern_counts': {'":': 0, '<': 17, '<?xml version=': 0, '>': 17, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '2075358', 'n_tokens_mistral': 3898, 'n_tokens_neox': 3637, 'n_words': 921}