146 lines
4.4 KiB
Python
146 lines
4.4 KiB
Python
import math
|
|
from math import comb
|
|
from typing import Tuple
|
|
|
|
import numpy as np
|
|
from numpy import arctan2, cos, sin
|
|
from numpy.typing import NDArray
|
|
|
|
import winkelumrechnungen as wu
|
|
from GHA_triaxial.utils import pq_para
|
|
from ellipsoid_triaxial import EllipsoidTriaxial
|
|
from utils_angle import wrap_0_2pi
|
|
|
|
|
|
def gha1_ana_step(ell: EllipsoidTriaxial, point: NDArray, alpha0: float, s: float, maxM: int) -> Tuple[NDArray, float]:
|
|
"""
|
|
Panou, Korakitits 2020, 5ff.
|
|
:param ell: Ellipsoid
|
|
:param point: Punkt in kartesischen Koordinaten
|
|
:param alpha0: Azimut im Startpunkt
|
|
:param s: Strecke
|
|
:param maxM: maximale Ordnung
|
|
:return: Zwischenpunkt, Azimut im Zwischenpunkt
|
|
"""
|
|
x, y, z = point
|
|
|
|
# S. 6
|
|
x_m = [x]
|
|
y_m = [y]
|
|
z_m = [z]
|
|
|
|
p, q = pq_para(ell, point)
|
|
|
|
# 48-50
|
|
x_m.append(p[0] * sin(alpha0) + q[0] * cos(alpha0))
|
|
y_m.append(p[1] * sin(alpha0) + q[1] * cos(alpha0))
|
|
z_m.append(p[2] * sin(alpha0) + q[2] * cos(alpha0))
|
|
|
|
# 34
|
|
H_ = lambda p: np.sum([comb(p, p - i) * (x_m[p - i] * x_m[i] +
|
|
1 / (1 - ell.ee ** 2) ** 2 * y_m[p - i] * y_m[i] +
|
|
1 / (1 - ell.ex ** 2) ** 2 * z_m[p - i] * z_m[i])
|
|
for i in range(0, p + 1)])
|
|
|
|
# 35
|
|
h_ = lambda q: np.sum([comb(q, q-j) * (x_m[q-j+1] * x_m[j+1] +
|
|
1 / (1 - ell.ee ** 2) * y_m[q-j+1] * y_m[j+1] +
|
|
1 / (1 - ell.ex ** 2) * z_m[q-j+1] * z_m[j+1])
|
|
for j in range(0, q+1)])
|
|
|
|
# 31
|
|
hH_ = lambda t: 1/H_(0) * (h_(t) - np.sum([comb(t, l-1) * H_(t+1-l) * hH_t[l-1] for l in range(1, t+1)]))
|
|
|
|
# 28-30
|
|
x_ = lambda m: - np.sum([comb(m-2, k) * hH_t[m-2-k] * x_m[k] for k in range(0, m-2+1)])
|
|
y_ = lambda m: -1 / (1-ell.ee**2) * np.sum([comb(m-2, k) * hH_t[m-2-k] * y_m[k] for k in range(0, m-2+1)])
|
|
z_ = lambda m: -1 / (1-ell.ex**2) * np.sum([comb(m-2, k) * hH_t[m-2-k] * z_m[k] for k in range(0, m-2+1)])
|
|
|
|
hH_t = []
|
|
a_m = []
|
|
b_m = []
|
|
c_m = []
|
|
for m in range(0, maxM+1):
|
|
if m >= 2:
|
|
hH_t.append(hH_(m-2))
|
|
x_m.append(x_(m))
|
|
y_m.append(y_(m))
|
|
z_m.append(z_(m))
|
|
fact_m = math.factorial(m)
|
|
|
|
# 22-24
|
|
a_m.append(x_m[m] / fact_m)
|
|
b_m.append(y_m[m] / fact_m)
|
|
c_m.append(z_m[m] / fact_m)
|
|
|
|
# 19-21
|
|
x_s = 0
|
|
for a in reversed(a_m):
|
|
x_s = x_s * s + a
|
|
y_s = 0
|
|
for b in reversed(b_m):
|
|
y_s = y_s * s + b
|
|
z_s = 0
|
|
for c in reversed(c_m):
|
|
z_s = z_s * s + c
|
|
|
|
p1 = np.array([x_s, y_s, z_s])
|
|
p_s, q_s = pq_para(ell, p1)
|
|
|
|
# 57-59
|
|
dx_s = 0
|
|
for i, a in reversed(list(enumerate(a_m[1:], start=1))):
|
|
dx_s = dx_s * s + i * a
|
|
|
|
dy_s = 0
|
|
for i, b in reversed(list(enumerate(b_m[1:], start=1))):
|
|
dy_s = dy_s * s + i * b
|
|
|
|
dz_s = 0
|
|
for i, c in reversed(list(enumerate(c_m[1:], start=1))):
|
|
dz_s = dz_s * s + i * c
|
|
|
|
# 52-53
|
|
sigma = np.array([dx_s, dy_s, dz_s])
|
|
P = float(p_s @ sigma)
|
|
Q = float(q_s @ sigma)
|
|
|
|
# 51
|
|
alpha1 = arctan2(P, Q)
|
|
|
|
if alpha1 < 0:
|
|
alpha1 += 2 * np.pi
|
|
|
|
return p1, wrap_0_2pi(alpha1)
|
|
|
|
|
|
def gha1_ana(ell: EllipsoidTriaxial, point: NDArray, alpha0: float, s: float, maxM: int, maxPartCircum: int = 4) -> Tuple[NDArray, float]:
|
|
"""
|
|
:param ell: Ellipsoid
|
|
:param point: Punkt in kartesischen Koordinaten
|
|
:param alpha0: Azimut im Startpunkt
|
|
:param s: Strecke
|
|
:param maxM: maximale Ordnung
|
|
:param maxPartCircum: maximale Aufteilung (1/x halber Ellipsoidumfang)
|
|
:return: Zielpunkt, Azimut im Zielpunkt
|
|
"""
|
|
if s > np.pi / maxPartCircum * ell.ax:
|
|
s /= 2
|
|
point_step, alpha_step = gha1_ana(ell, point, alpha0, s, maxM, maxPartCircum)
|
|
point_end, alpha_end = gha1_ana(ell, point_step, alpha_step, s, maxM, maxPartCircum)
|
|
else:
|
|
point_end, alpha_end = gha1_ana_step(ell, point, alpha0, s, maxM)
|
|
|
|
_, _, h = ell.cart2geod(point_end, "ligas3")
|
|
if h > 1e-5:
|
|
raise Exception("GHA1_ana: explodiert, Punkt liegt nicht mehr auf dem Ellipsoid")
|
|
|
|
return point_end, wrap_0_2pi(alpha_end)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
ell = EllipsoidTriaxial.init_name("BursaSima1980round")
|
|
p0 = ell.ell2cart(wu.deg2rad(10), wu.deg2rad(20))
|
|
p1, alpha1 = gha1_ana(ell, p0, wu.deg2rad(36), 200000, 70)
|
|
print(p1, wu.rad2gms(alpha1))
|