wgs84.py 2.89 KB
Newer Older
oscar's avatar
oscar committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130
import math
import numpy as np
from rotations import degree2rad, rad2degree
from numba import jit

a = 6378137.0
f = 0.003352810664
b = 6356752.3142499477
c = 6399593.6257536924
e2 = 0.006694379988651241
ep2 = 0.0067394967407662064

m0 = 6335439.3273023246
m2 = 63617.757378010137
m4 = 532.35180239277622
m6 = 4.1577261283373916
m8 = 0.031312573415813519

a0 = 6367449.1457686741
a2 = 32077.017223574985
a4 = 67.330398573595
a6 = 0.13188597734903185
a8 = 0.00024462947981104312

#class Wgs84:

def GetN(radB):
    cosB = math.cos(radB)
    V = math.sqrt(1 + ep2 * cosB * cosB)
    N = c / V
    return N


def GetX(radB):
    sinB = math.sin(radB)
    cosB = math.cos(radB)
    al = a0 * radB
    sc = sinB * cosB
    ss = sinB * sinB
    X = al - sc *((a2 - a4 + a6) + (2 * a4 - a6 * 16 / 3) * ss + a6 * 16 * ss * ss /3)
    return X

@jit
def GetLatByX(X):
    Bfi0 = X / a0
    Bfi1 = 0
    num = 1
    minAngle = math.pi * 1e-9
    menus_minAngle = 0 - minAngle

    while (num == 1):
        num = 0
        sinB = math.sin(Bfi0)
        sinB2 = sinB * sinB
        cosB = math.cos(Bfi0)
        FBfi = 0 - sinB * cosB *((a2 - a4 + a6) + sinB2 * (2 * a4 - 16 * a6 / 3) + sinB2 * sinB2 * a6 * 16 / 3)
        Bfi1 = (X - FBfi) / a0

        deltaB = Bfi1 - Bfi0
        if deltaB < menus_minAngle or deltaB > minAngle:
            num = 1
            Bfi0 = Bfi1

    Bf = Bfi1
    return Bf

@jit
def GetV(lat):
    cosB = math.cos(lat)
    V = math.sqrt(1 + ep2 * cosB * cosB)
    return V


def BLH2ECEF(ptBLH):
    cosB = math.cos(degree2rad(ptBLH[0]))
    sinB = math.sin(degree2rad(ptBLH[0]))
    cosL = math.cos(degree2rad(ptBLH[1]))
    sinL = math.sin(degree2rad(ptBLH[1]))

    N = GetN(degree2rad(ptBLH[0]))

    X = (N + ptBLH[2]) * cosB * cosL
    Y = (N + ptBLH[2]) * cosB * sinL
    Z = (N * (1 - e2) + ptBLH[2]) * sinB
    ptXYZ = np.array([X, Y, Z])
    return ptXYZ



def ECEF2BLH(ptXYZ):
    L_radian = math.atan(ptXYZ[1] / ptXYZ[0])
    if L_radian < 0:
        L_radian += math.pi
    L = rad2degree(L_radian)
    sqrtXY = math.sqrt(ptXYZ[0] * ptXYZ[0] + ptXYZ[1] * ptXYZ[1])
    t0 = ptXYZ[2] / sqrtXY
    p = c * e2 / sqrtXY
    k = 1+ep2
    ti = t0
    ti1 = 0
    loop = 0
    while loop < 10000:
        loop += 1
        ti1 = t0 + p * ti / math.sqrt( k + ti * ti)
        if math.fabs(ti1 - ti) < 1e-12:
            break
        ti = ti1
    B_radian = math.atan(ti1)
    N = GetN(B_radian)
    H = sqrtXY / math.cos(B_radian) - N
    B = rad2degree(B_radian)
    geopoint = np.array([B, L, H])
    return geopoint


def GetR_ENU2ECEF(radianL, radianB):
    sinB = math.sin(radianB)
    cosB = math.cos(radianB)
    sinL = math.sin(radianL)
    cosL = math.cos(radianL)
    R4 = np.array([[-sinL, -sinB * cosL, cosB * cosL],
                   [cosL, -sinB * sinL, cosB * sinL],
                   [0, cosB, sinB]])
    return R4


def GetAngleDif(distance):
    angleRafian = distance / a
    return rad2degree(angleRafian)