828 lines
30 KiB
Python
828 lines
30 KiB
Python
import numpy as np
|
|
from numpy import sin, cos, arctan, arctan2, sqrt, pi, arccos
|
|
import winkelumrechnungen as wu
|
|
import jacobian_Ligas
|
|
import matplotlib.pyplot as plt
|
|
from typing import Tuple
|
|
from numpy.typing import NDArray
|
|
import math
|
|
from utils_angle import wrap_mpi_pi, wrap_0_2pi, wrap_mhalfpi_halfpi
|
|
|
|
|
|
class EllipsoidBiaxial:
|
|
def __init__(self, a: float, b: float):
|
|
self.a = a
|
|
self.b = b
|
|
self.c = a ** 2 / b
|
|
self.e = sqrt(a ** 2 - b ** 2) / a
|
|
self.e_ = sqrt(a ** 2 - b ** 2) / b
|
|
|
|
@classmethod
|
|
def init_name(cls, name: str):
|
|
if name == "Bessel":
|
|
a = 6377397.15508
|
|
b = 6356078.96290
|
|
return cls(a, b)
|
|
elif name == "Hayford":
|
|
a = 6378388
|
|
f = 1/297
|
|
b = a - a * f
|
|
return cls(a, b)
|
|
elif name == "Krassowski":
|
|
a = 6378245
|
|
f = 298.3
|
|
b = a - a * f
|
|
return cls(a, b)
|
|
elif name == "WGS84":
|
|
a = 6378137
|
|
f = 298.257223563
|
|
b = a - a * f
|
|
return cls(a, b)
|
|
|
|
@classmethod
|
|
def init_af(cls, a: float, f: float):
|
|
b = a - a * f
|
|
return cls(a, b)
|
|
|
|
V = lambda self, phi: sqrt(1 + self.e_ ** 2 * cos(phi) ** 2)
|
|
M = lambda self, phi: self.c / self.V(phi) ** 3
|
|
N = lambda self, phi: self.c / self.V(phi)
|
|
|
|
beta2psi = lambda self, beta: np.arctan2(self.a * np.sin(beta), self.b * np.cos(beta))
|
|
beta2phi = lambda self, beta: np.arctan2(self.a ** 2 * np.sin(beta), self.b ** 2 * np.cos(beta))
|
|
|
|
psi2beta = lambda self, psi: np.arctan2(self.b * np.sin(psi), self.a * np.cos(psi))
|
|
psi2phi = lambda self, psi: np.arctan2(self.a * np.sin(psi), self.b * np.cos(psi))
|
|
|
|
phi2beta = lambda self, phi: np.arctan2(self.b**2 * np.sin(phi), self.a**2 * np.cos(phi))
|
|
phi2psi = lambda self, phi: np.arctan2(self.b * np.sin(phi), self.a * np.cos(phi))
|
|
|
|
phi2p = lambda self, phi: self.N(phi) * cos(phi)
|
|
|
|
def bi_cart2ell(self, point: NDArray, Eh: float = 0.001, Ephi: float = wu.gms2rad([0, 0, 0.001])) -> Tuple[float, float, float]:
|
|
"""
|
|
Umrechnung von kartesischen in ellipsoidische Koordinaten auf einem Rotationsellipsoid
|
|
# TODO: Quelle
|
|
:param point: Punkt in kartesischen Koordinaten
|
|
:param Eh: Grenzwert für die Höhe
|
|
:param Ephi: Grenzwert für die Breite
|
|
:return: ellipsoidische Breite, Länge, geodätische Höhe
|
|
"""
|
|
x, y, z = point
|
|
|
|
lamb = arctan2(y, x)
|
|
|
|
p = sqrt(x**2+y**2)
|
|
|
|
phi_null = arctan2(z, p*(1 - self.e**2))
|
|
|
|
hi = [0]
|
|
phii = [phi_null]
|
|
|
|
i = 0
|
|
|
|
while True:
|
|
N = self.a / sqrt(1 - self.e**2 * sin(phii[i])**2)
|
|
h = p / cos(phii[i]) - N
|
|
phi = arctan2(z, p * (1-(self.e**2*N) / (N+h)))
|
|
hi.append(h)
|
|
phii.append(phi)
|
|
dh = abs(hi[i]-h)
|
|
dphi = abs(phii[i]-phi)
|
|
i = i+1
|
|
if dh < Eh:
|
|
if dphi < Ephi:
|
|
break
|
|
return phi, lamb, h
|
|
|
|
def bi_ell2cart(self, phi: float, lamb: float, h: float) -> NDArray:
|
|
"""
|
|
Umrechnung von ellipsoidischen in kartesische Koordinaten auf einem Rotationsellipsoid
|
|
# TODO: Quelle
|
|
:param phi: ellipsoidische Breite
|
|
:param lamb: ellipsoidische Länge
|
|
:param h: geodätische Höhe
|
|
:return: Punkt in kartesischen Koordinaten
|
|
"""
|
|
W = sqrt(1 - self.e**2 * sin(phi)**2)
|
|
N = self.a / W
|
|
x = (N+h) * cos(phi) * cos(lamb)
|
|
y = (N+h) * cos(phi) * sin(lamb)
|
|
z = (N * (1-self.e**2) + h) * sin(phi)
|
|
return np.array([x, y, z])
|
|
|
|
class EllipsoidTriaxial:
|
|
def __init__(self, ax: float, ay: float, b: float):
|
|
self.ax = ax
|
|
self.ay = ay
|
|
self.b = b
|
|
self.ex = sqrt((self.ax**2 - self.b**2) / self.ax**2)
|
|
self.ey = sqrt((self.ay**2 - self.b**2) / self.ay**2)
|
|
self.ee = sqrt((self.ax**2 - self.ay**2) / self.ax**2)
|
|
self.ex_ = sqrt((self.ax**2 - self.b**2) / self.b**2)
|
|
self.ey_ = sqrt((self.ay**2 - self.b**2) / self.b**2)
|
|
self.ee_ = sqrt((self.ax**2 - self.ay**2) / self.ay**2)
|
|
self.Ex = sqrt(self.ax**2 - self.b**2)
|
|
self.Ey = sqrt(self.ay**2 - self.b**2)
|
|
self.Ee = sqrt(self.ax**2 - self.ay**2)
|
|
|
|
@classmethod
|
|
def init_name(cls, name: str):
|
|
"""
|
|
Mögliche Ellipsoide: BursaFialova1993, BursaSima1980, BursaSima1980round, Eitschberger1978, Bursa1972,
|
|
Bursa1970, BesselBiaxial, Fiction, KarneyTest2024
|
|
Panou et al (2020)
|
|
:param name: Name des dreiachsigen Ellipsoids
|
|
"""
|
|
if name == "BursaFialova1993":
|
|
ax = 6378171.36
|
|
ay = 6378101.61
|
|
b = 6356751.84
|
|
return cls(ax, ay, b)
|
|
elif name == "BursaSima1980":
|
|
ax = 6378172
|
|
ay = 6378102.7
|
|
b = 6356752.6
|
|
return cls(ax, ay, b)
|
|
elif name == "BursaSima1980round":
|
|
# Panou 2013
|
|
ax = 6378172
|
|
ay = 6378103
|
|
b = 6356753
|
|
return cls(ax, ay, b)
|
|
elif name == "Eitschberger1978":
|
|
ax = 6378173.435
|
|
ay = 6378103.9
|
|
b = 6356754.4
|
|
return cls(ax, ay, b)
|
|
elif name == "Bursa1972":
|
|
ax = 6378173
|
|
ay = 6378104
|
|
b = 6356754
|
|
return cls(ax, ay, b)
|
|
elif name == "Bursa1970":
|
|
ax = 6378173
|
|
ay = 6378105
|
|
b = 6356754
|
|
return cls(ax, ay, b)
|
|
elif name == "BesselBiaxial":
|
|
ax = 6377397.15509
|
|
ay = 6377397.15508
|
|
b = 6356078.96290
|
|
return cls(ax, ay, b)
|
|
elif name == "Fiction":
|
|
ax = 6000000
|
|
ay = 4000000
|
|
b = 2000000
|
|
return cls(ax, ay, b)
|
|
elif name == "KarneyTest2024":
|
|
ax = sqrt(2)
|
|
ay = 1
|
|
b = 1 / sqrt(2)
|
|
return cls(ax, ay, b)
|
|
|
|
def func_H(self, point: NDArray) -> float:
|
|
"""
|
|
Berechnung H
|
|
Panou, Korakitis 2019 [43]
|
|
:param point: Punkt
|
|
:return: H
|
|
"""
|
|
x, y, z = point
|
|
return x ** 2 + y ** 2 / (1 - self.ee ** 2) ** 2 + z ** 2 / (1 - self.ex ** 2) ** 2
|
|
|
|
def func_n(self, point: NDArray, H: float = None) -> NDArray:
|
|
"""
|
|
Berechnung normalen Vektor
|
|
Panou, Korakitis 2019 [9-12]
|
|
:param point: Punkt
|
|
:param H:
|
|
:return:
|
|
"""
|
|
if H is None:
|
|
H = self.func_H(point)
|
|
sqrtH = sqrt(H)
|
|
x, y, z = point
|
|
return np.array([x / sqrtH,
|
|
y / ((1 - self.ee ** 2) * sqrtH),
|
|
z / ((1 - self.ex ** 2) * sqrtH)])
|
|
|
|
def func_t12(self, point: NDArray) -> Tuple[float, float]:
|
|
"""
|
|
Berechnung Wurzeln
|
|
Panou, Korakitis 2019 [9-12]
|
|
:param point: Punkt
|
|
:return: Wurzeln t1, t2
|
|
"""
|
|
x, y, z = point
|
|
c1 = x ** 2 + y ** 2 + z ** 2 - (self.ax ** 2 + self.ay ** 2 + self.b ** 2)
|
|
c0 = (self.ax ** 2 * self.ay ** 2 + self.ax ** 2 * self.b ** 2 + self.ay ** 2 * self.b ** 2 -
|
|
(self.ay ** 2 + self.b ** 2) * x ** 2 - (self.ax ** 2 + self.b ** 2) * y ** 2 - (
|
|
self.ax ** 2 + self.ay ** 2) * z ** 2)
|
|
if c1 ** 2 - 4 * c0 < -1e-9:
|
|
t2 = np.nan
|
|
raise Exception("t1, t2: Negativer Wurzelterm")
|
|
elif c1 ** 2 - 4 * c0 < 0:
|
|
t2 = 0
|
|
else:
|
|
t2 = (-c1 + sqrt(c1 ** 2 - 4 * c0)) / 2
|
|
if t2 == 0:
|
|
t2 = 1e-18
|
|
t1 = c0 / t2
|
|
return t1, t2
|
|
|
|
def ellu2cart(self, beta: float, lamb: float, u: float) -> NDArray:
|
|
"""
|
|
Panou 2014 12ff.
|
|
:param beta: ellipsoidische Breite
|
|
:param lamb: ellipsoidische Länge
|
|
:param u: radiale Koordinate entlang der kleinen Halbachse
|
|
:return: Punkt in kartesischen Koordinaten
|
|
"""
|
|
x = sqrt(u**2 + self.Ex**2) * sqrt(cos(beta)**2 + self.Ee**2/self.Ex**2 * sin(beta)**2) * cos(lamb)
|
|
y = sqrt(u**2 + self.Ey**2) * cos(beta) * sin(lamb)
|
|
z = u * sin(beta) * sqrt(1 - self.Ee**2/self.Ex**2 * cos(lamb)**2)
|
|
|
|
return np.array([x, y, z])
|
|
|
|
def cart2ellu(self, point: NDArray) -> Tuple[float, float, float]:
|
|
"""
|
|
Panou 2014 15ff.
|
|
:param point: Punkt in kartesischen Koordinaten
|
|
:return: ellipsoidische Breite, ellipsoidische Länge, radiale Koordinate entlang der kleinen Halbachse
|
|
"""
|
|
x, y, z = point
|
|
c2 = self.ax**2 + self.ay**2 + self.b**2 - x**2 - y**2 - z**2
|
|
c1 = (self.ax**2 * self.ay**2 + self.ax**2 * self.b**2 + self.ay**2 * self.b**2 -
|
|
(self.ay**2+self.b**2) * x**2 - (self.ax**2 + self.b**2) * y**2 - (self.ax**2 + self.ay**2) * z**2)
|
|
c0 = (self.ax**2 * self.ay**2 * self.b**2 - self.ay**2 * self.b**2 * x**2 -
|
|
self.ax**2 * self.b**2 * y**2 - self.ax**2 * self.ay**2 * z**2)
|
|
|
|
p = (c2**2 - 3*c1) / 9
|
|
q = (9*c1*c2 - 27*c0 - 2*c2**3) / 54
|
|
omega = arccos(q / sqrt(p**3))
|
|
|
|
s1 = 2 * sqrt(p) * cos(omega/3) - c2/3
|
|
s2 = 2 * sqrt(p) * cos(omega/3 - 2*pi/3) - c2/3
|
|
s3 = 2 * sqrt(p) * cos(omega/3 - 4*pi/3) - c2/3
|
|
# print(s1, s2, s3)
|
|
|
|
beta = arctan(sqrt((-self.b**2 - s2) / (self.ay**2 + s2)))
|
|
if abs((-self.ay**2 - s3) / (self.ax**2 + s3)) > 1e-7:
|
|
lamb = arctan(sqrt((-self.ay**2 - s3) / (self.ax**2 + s3)))
|
|
else:
|
|
lamb = 0
|
|
u = sqrt(self.b**2 + s1)
|
|
|
|
return beta, lamb, u
|
|
|
|
def ell2cart(self, beta: float | NDArray, lamb: float | NDArray) -> NDArray:
|
|
"""
|
|
Panou, Korakitis 2019 2
|
|
:param beta: ellipsoidische Breite
|
|
:param lamb: ellipsoidische Länge
|
|
:return: Punkt in kartesischen Koordinaten
|
|
"""
|
|
beta = np.asarray(beta, dtype=float)
|
|
lamb = np.asarray(lamb, dtype=float)
|
|
|
|
beta, lamb = np.broadcast_arrays(beta, lamb)
|
|
|
|
beta = np.where(
|
|
np.isclose(np.abs(beta), np.pi / 2, atol=1e-15),
|
|
beta * 8999999999999999 / 9000000000000000,
|
|
beta
|
|
)
|
|
B = self.Ex ** 2 * cos(beta) ** 2 + self.Ee ** 2 * sin(beta) ** 2
|
|
L = self.Ex ** 2 - self.Ee ** 2 * cos(lamb) ** 2
|
|
|
|
x = self.ax / self.Ex * sqrt(B) * cos(lamb)
|
|
y = self.ay * cos(beta) * sin(lamb)
|
|
z = self.b / self.Ex * sin(beta) * sqrt(L)
|
|
|
|
xyz = np.stack((x, y, z), axis=-1)
|
|
|
|
# Pole
|
|
mask_south = beta == -pi / 2
|
|
mask_north = beta == pi / 2
|
|
xyz[mask_south] = np.array([0, 0, -self.b])
|
|
xyz[mask_north] = np.array([0, 0, self.b])
|
|
|
|
# Äquator
|
|
mask_eq = beta == 0
|
|
xyz[mask_eq & (lamb == -pi / 2)] = np.array([0, -self.ay, 0])
|
|
xyz[mask_eq & (lamb == pi / 2)] = np.array([0, self.ay, 0])
|
|
xyz[mask_eq & (lamb == 0)] = np.array([self.ax, 0, 0])
|
|
xyz[mask_eq & (lamb == pi)] = np.array([-self.ax, 0, 0])
|
|
|
|
return xyz
|
|
|
|
def ell2cart_bektas(self, beta: float | NDArray, omega: float | NDArray) -> NDArray:
|
|
"""
|
|
Bektas 2015
|
|
:param beta: ellipsoidische Breite
|
|
:param omega: ellipsoidische Länge
|
|
:return: Punkt in kartesischen Koordinaten
|
|
"""
|
|
x = self.ax * cos(omega) * sqrt((self.ax**2 - self.ay**2 * sin(beta)**2 - self.b**2 * cos(beta)**2) / (self.ax**2 - self.b**2))
|
|
y = self.ay * cos(beta) * sin(omega)
|
|
z = self.b * sin(beta) * sqrt((self.ax**2 * sin(omega)**2 + self.ay**2 * cos(omega)**2 - self.b**2) / (self.ax**2 - self.b**2))
|
|
|
|
return np.array([x, y, z])
|
|
|
|
def ell2cart_karney(self, beta: float | NDArray, lamb: float | NDArray) -> NDArray:
|
|
"""
|
|
Karney 2025 Geographic Lib
|
|
:param beta: ellipsoidische Breite
|
|
:param lamb: ellipsoidische Länge
|
|
:return: Punkt in kartesischen Koordinaten
|
|
"""
|
|
k = sqrt(self.ay**2 - self.b**2) / sqrt(self.ax**2 - self.b**2)
|
|
k_ = sqrt(self.ax**2 - self.ay**2) / sqrt(self.ax**2 - self.b**2)
|
|
X = self.ax * cos(lamb) * sqrt(k**2*cos(beta)**2+k_**2)
|
|
Y = self.ay * cos(beta) * sin(lamb)
|
|
Z = self.b * sin(beta) * sqrt(k**2 + k_**2 * sin(lamb)**2)
|
|
return np.array([X, Y, Z])
|
|
|
|
def cart2ell_yFake(self, point: NDArray, delta_y: float = 1e-4) -> Tuple[float, float]:
|
|
"""
|
|
Bei Fehlschlagen von cart2ell
|
|
:param point: Punkt in kartesischen Koordinaten
|
|
:param delta_y: Startwert für Suche nach kleinstmöglichem delta_y
|
|
:return: ellipsoidische Breite und Länge
|
|
"""
|
|
x, y, z = point
|
|
best_delta = np.inf
|
|
while True:
|
|
try:
|
|
y1 = y - delta_y
|
|
beta1, lamb1 = self.cart2ell(np.array([x, y1, z]), noFake=True)
|
|
point1 = self.ell2cart(beta1, lamb1)
|
|
|
|
y2 = y + delta_y
|
|
beta2, lamb2 = self.cart2ell(np.array([x, y2, z]), noFake=True)
|
|
point2 = self.ell2cart(beta2, lamb2)
|
|
|
|
pointM = (point1 + point2) / 2
|
|
|
|
actual_delta = np.linalg.norm(point - pointM)
|
|
except:
|
|
actual_delta = np.inf
|
|
|
|
if actual_delta < best_delta:
|
|
best_delta = actual_delta
|
|
delta_y /= 10
|
|
else:
|
|
delta_y *= 10
|
|
|
|
y1 = y - delta_y
|
|
beta1, lamb1 = self.cart2ell(np.array([x, y1, z]), noFake=True)
|
|
|
|
return beta1, lamb1
|
|
|
|
def cart2ell(self, point: NDArray, eps: float = 1e-12, maxI: int = 100, noFake: bool = False) -> Tuple[float, float]:
|
|
"""
|
|
Panou, Korakitis 2019 3f. (num)
|
|
:param point: Punkt in kartesischen Koordinaten
|
|
:param eps: zu erreichende Genauigkeit
|
|
:param maxI: maximale Anzahl Iterationen
|
|
:param noFake: y numerisch anpassen?
|
|
:return: ellipsoidische Breite und Länge
|
|
"""
|
|
x, y, z = point
|
|
beta, lamb = self.cart2ell_panou(point)
|
|
delta_ell = np.array([np.inf, np.inf]).T
|
|
tiny = 1e-30
|
|
|
|
try:
|
|
i = 0
|
|
while np.linalg.norm(delta_ell) > eps and i < maxI:
|
|
x0, y0, z0 = self.ell2cart(beta, lamb)
|
|
delta_l = np.array([x-x0, y-y0, z-z0]).T
|
|
|
|
B = max(self.Ex ** 2 * cos(beta) ** 2 + self.Ee ** 2 * sin(beta) ** 2, tiny)
|
|
L = max(self.Ex ** 2 - self.Ee ** 2 * cos(lamb) ** 2, tiny)
|
|
|
|
J = np.array([[(-self.ax * self.Ey ** 2) / (2 * self.Ex) * sin(2 * beta) / sqrt(B) * cos(lamb),
|
|
-self.ax / self.Ex * sqrt(B) * sin(lamb)],
|
|
[-self.ay * sin(beta) * sin(lamb),
|
|
self.ay * cos(beta) * cos(lamb)],
|
|
[self.b / self.Ex * cos(beta) * sqrt(L),
|
|
(self.b * self.Ee ** 2) / (2 * self.Ex) * sin(beta) * sin(2 * lamb) / sqrt(L)]])
|
|
|
|
N = J.T @ J
|
|
det = N[0, 0] * N[1, 1] - N[0, 1] * N[1, 0]
|
|
N_inv = 1 / det * np.array([[N[1, 1], -N[0, 1]], [-N[1, 0], N[0, 0]]])
|
|
delta_ell = N_inv @ J.T @ delta_l
|
|
# delta_ell, *_ = np.linalg.lstsq(J, delta_l, rcond=None)
|
|
|
|
beta += delta_ell[0]
|
|
lamb += delta_ell[1]
|
|
i += 1
|
|
|
|
if i == maxI:
|
|
raise Exception("Umrechnung cart2ell: nicht konvergiert")
|
|
|
|
point_n = self.ell2cart(beta, lamb)
|
|
delta_r = np.linalg.norm(point - point_n, axis=-1)
|
|
if delta_r > 1e-6:
|
|
raise Exception("Umrechnung cart2ell: Punktdifferenz")
|
|
|
|
return wrap_mhalfpi_halfpi(beta), wrap_mpi_pi(lamb)
|
|
|
|
except Exception as e:
|
|
# Wenn die Berechnung fehlschlägt auf Grund von sehr kleinem y, solange anpassen, bis Umrechnung ohne Fehler
|
|
delta_y = 10 ** math.floor(math.log10(abs(self.ay/1000)))
|
|
if abs(y) < delta_y and not noFake:
|
|
return self.cart2ell_yFake(point, delta_y)
|
|
else:
|
|
raise e
|
|
|
|
def cart2ell_panou(self, point: NDArray) -> Tuple[float, float]:
|
|
"""
|
|
Panou, Korakitis 2019 2f. (analytisch -> Näherung)
|
|
:param point: Punkt in kartesischen Koordinaten
|
|
:return: ellipsoidische Breite und Länge
|
|
"""
|
|
x, y, z = point
|
|
|
|
eps = 1e-9
|
|
|
|
if abs(x) < eps and abs(y) < eps: # Punkt in der z-Achse
|
|
beta = pi / 2 if z > 0 else -pi / 2
|
|
lamb = 0.0
|
|
return beta, lamb
|
|
|
|
elif abs(x) < eps and abs(z) < eps: # Punkt in der y-Achse
|
|
beta = 0.0
|
|
lamb = pi / 2 if y > 0 else -pi / 2
|
|
return beta, lamb
|
|
|
|
elif abs(y) < eps and abs(z) < eps: # Punkt in der x-Achse
|
|
beta = 0.0
|
|
lamb = 0.0 if x > 0 else pi
|
|
return beta, lamb
|
|
|
|
# ---- Allgemeiner Fall -----
|
|
|
|
t1, t2 = self.func_t12(point)
|
|
|
|
num_beta = max(t1 - self.b ** 2, 0)
|
|
den_beta = max(self.ay ** 2 - t1, 1e-30)
|
|
num_lamb = max(t2 - self.ay ** 2, 0)
|
|
den_lamb = max(self.ax ** 2 - t2, 1e-30)
|
|
|
|
beta = arctan(sqrt(num_beta / den_beta))
|
|
lamb = arctan(sqrt(num_lamb / den_lamb))
|
|
|
|
if z < 0:
|
|
beta = -beta
|
|
|
|
if y < 0:
|
|
lamb = -lamb
|
|
|
|
if x < 0:
|
|
lamb = pi - lamb
|
|
|
|
if abs(x) < eps:
|
|
lamb = -pi/2 if y < 0 else pi/2
|
|
|
|
elif abs(y) < eps:
|
|
lamb = 0 if x > 0 else pi
|
|
|
|
elif abs(z) < eps:
|
|
beta = 0
|
|
|
|
return beta, lamb
|
|
|
|
def cart2ell_bektas(self, point: NDArray, eps: float = 1e-12, maxI: int = 100) -> Tuple[float, float]:
|
|
"""
|
|
Bektas 2015
|
|
:param point: Punkt in kartesischen Koordinaten
|
|
:param eps: zu erreichende Genauigkeit
|
|
:param maxI: maximale Anzahl Iterationen
|
|
:return: ellipsoidische Breite und Länge
|
|
"""
|
|
x, y, z = point
|
|
phi, lamb = self.cart2para(point)
|
|
p = sqrt((self.ax**2 - self.ay**2) / (self.ax**2 - self.b**2))
|
|
d_phi = np.inf
|
|
d_lamb = np.inf
|
|
|
|
i = 0
|
|
while d_phi > eps and d_lamb > eps and i < maxI:
|
|
lamb_new = arctan2(self.ax * y * sqrt((p**2-1) * sin(phi)**2 + 1), self.ay * x * cos(phi))
|
|
phi_new = arctan2(self.ay * z * sin(lamb), self.b * y * sqrt(1 - p**2 * cos(lamb)**2))
|
|
d_phi = abs(phi_new - phi)
|
|
phi = phi_new
|
|
d_lamb = abs(lamb_new - lamb)
|
|
lamb = lamb_new
|
|
i += 1
|
|
|
|
if i == maxI:
|
|
raise Exception("Umrechnung cart2ell: nicht konvergiert")
|
|
|
|
return phi, lamb
|
|
|
|
def geod2cart(self, phi: float | NDArray, lamb: float | NDArray, h: float) -> NDArray:
|
|
"""
|
|
Ligas 2012, 250
|
|
:param phi: geodätische Breite
|
|
:param lamb: geodätische Länge
|
|
:param h: geodätische Höhe
|
|
:return: kartesische Koordinaten
|
|
"""
|
|
v = self.ax / sqrt(1 - self.ex**2*sin(phi)**2-self.ee**2*cos(phi)**2*sin(lamb)**2)
|
|
xG = (v + h) * cos(phi) * cos(lamb)
|
|
yG = (v * (1-self.ee**2) + h) * cos(phi) * sin(lamb)
|
|
zG = (v * (1-self.ex**2) + h) * sin(phi)
|
|
return np.array([xG, yG, zG])
|
|
|
|
def cart2geod(self, point: NDArray, mode: str = "ligas3", maxIter: int = 30, maxLoa: float = 0.005) -> Tuple[float, float, float]:
|
|
"""
|
|
Ligas 2012
|
|
:param mode: ligas1, ligas2, oder ligas3
|
|
:param point: Punkt in kartesischen Koordinaten
|
|
:param maxIter: maximale Anzahl Iterationen
|
|
:param maxLoa: Level of Accuracy, das erreicht werden soll
|
|
:return: geodätische Breite, Länge, Höhe
|
|
"""
|
|
xG, yG, zG = point
|
|
|
|
eps = 1e-9
|
|
|
|
if abs(xG) < eps and abs(yG) < eps: # Punkt in der z-Achse
|
|
phi = pi / 2 if zG > 0 else -pi / 2
|
|
lamb = 0.0
|
|
h = abs(zG) - self.b
|
|
return phi, lamb, h
|
|
|
|
elif abs(xG) < eps and abs(zG) < eps: # Punkt in der y-Achse
|
|
phi = 0.0
|
|
lamb = pi / 2 if yG > 0 else -pi / 2
|
|
h = abs(yG) - self.ay
|
|
return phi, lamb, h
|
|
|
|
elif abs(yG) < eps and abs(zG) < eps: # Punkt in der x-Achse
|
|
phi = 0.0
|
|
lamb = 0.0 if xG > 0 else pi
|
|
h = abs(xG) - self.ax
|
|
return phi, lamb, h
|
|
|
|
rG = sqrt(xG ** 2 + yG ** 2 + zG ** 2)
|
|
pE = np.array([self.ax * xG / rG, self.ax * yG / rG, self.ax * zG / rG], dtype=np.float64)
|
|
|
|
E = 1 / self.ax**2
|
|
F = 1 / self.ay**2
|
|
G = 1 / self.b**2
|
|
|
|
i = 0
|
|
loa = np.inf
|
|
|
|
while i < maxIter and loa > maxLoa:
|
|
if mode == "ligas1":
|
|
invJ, fxE = jacobian_Ligas.case1(E, F, G, np.array([xG, yG, zG]), pE)
|
|
elif mode == "ligas2":
|
|
invJ, fxE = jacobian_Ligas.case2(E, F, G, np.array([xG, yG, zG]), pE)
|
|
elif mode == "ligas3":
|
|
invJ, fxE = jacobian_Ligas.case3(E, F, G, np.array([xG, yG, zG]), pE)
|
|
pEi = pE.reshape(-1, 1) - invJ @ fxE.reshape(-1, 1)
|
|
pEi = pEi.reshape(1, -1).flatten()
|
|
loa = sqrt((pEi[0]-pE[0])**2 + (pEi[1]-pE[1])**2 + (pEi[2]-pE[2])**2)
|
|
pE = pEi
|
|
i += 1
|
|
|
|
if i == maxIter and loa > maxLoa:
|
|
act_mode = int(mode[-1])
|
|
new_mode = 3 if act_mode == 1 else act_mode - 1
|
|
phi, lamb, h = self.cart2geod(point, f"ligas{new_mode}", maxIter, maxLoa)
|
|
|
|
else:
|
|
phi = arctan((1-self.ee**2) / (1-self.ex**2) * pE[2] / sqrt((1-self.ee**2)**2 * pE[0]**2 + pE[1]**2))
|
|
lamb = arctan(1/(1-self.ee**2) * pE[1]/pE[0])
|
|
h = np.sign(zG - pE[2]) * np.sign(pE[2]) * sqrt((pE[0] - xG) ** 2 + (pE[1] - yG) ** 2 + (pE[2] - zG) ** 2)
|
|
if h < -self.ax:
|
|
act_mode = int(mode[-1])
|
|
new_mode = 3 if act_mode == 1 else act_mode - 1
|
|
phi, lamb, h = self.cart2geod(point, f"ligas{new_mode}", maxIter, maxLoa)
|
|
else:
|
|
if xG < 0 and yG < 0:
|
|
lamb = -pi + lamb
|
|
|
|
elif xG < 0:
|
|
lamb = pi + lamb
|
|
|
|
if abs(zG) < eps:
|
|
phi = 0
|
|
wrap_mhalfpi_halfpi(phi), wrap_mpi_pi(lamb)
|
|
return wrap_mhalfpi_halfpi(phi), wrap_mpi_pi(lamb), h
|
|
|
|
def para2cart(self, u: float | NDArray, v: float | NDArray) -> NDArray:
|
|
"""
|
|
Panou, Korakitits 2020, 4
|
|
:param u: parametrische Breite
|
|
:param v: parametrische Länge
|
|
:return: Punkt in kartesischen Koordinaten
|
|
"""
|
|
x = self.ax * cos(u) * cos(v)
|
|
y = self.ay * cos(u) * sin(v)
|
|
z = self.b * sin(u)
|
|
z = np.broadcast_to(z, np.shape(x))
|
|
return np.array([x, y, z])
|
|
|
|
def cart2para(self, point: NDArray) -> Tuple[float, float]:
|
|
"""
|
|
Panou, Korakitits 2020, 4
|
|
:param point: Punkt in kartesischen Koordinaten
|
|
:return: parametrische Breite, Länge
|
|
"""
|
|
x, y, z = point
|
|
|
|
u_check1 = z*sqrt(1 - self.ee**2)
|
|
u_check2 = sqrt(x**2 * (1-self.ee**2) + y**2) * sqrt(1-self.ex**2)
|
|
if u_check1 <= u_check2:
|
|
u = arctan2(u_check1, u_check2)
|
|
else:
|
|
u = pi/2 - arctan2(u_check2, u_check1)
|
|
|
|
v_check1 = y
|
|
v_check2 = x*sqrt(1-self.ee**2)
|
|
v_factor = sqrt(x**2*(1-self.ee**2)+y**2)
|
|
if v_check1 <= v_check2:
|
|
v = 2 * arctan2(v_check1, v_check2 + v_factor)
|
|
else:
|
|
v = pi/2 - 2 * arctan2(v_check2, v_check1 + v_factor)
|
|
wrap_mhalfpi_halfpi(u), wrap_mpi_pi(v)
|
|
return wrap_mhalfpi_halfpi(u), wrap_mpi_pi(v)
|
|
|
|
def ell2para(self, beta: float, lamb: float) -> Tuple[float, float]:
|
|
"""
|
|
Umrechung von ellipsoidischen in parametrische Koordinaten (über kartesische Koordinaten)
|
|
:param beta: ellipsoidische Breite
|
|
:param lamb: ellipsoidische Länge
|
|
:return: parametrische Breite, Länge
|
|
"""
|
|
cart = self.ell2cart(beta, lamb)
|
|
return self.cart2para(cart)
|
|
|
|
def para2ell(self, u: float, v: float) -> Tuple[float, float]:
|
|
"""
|
|
Umrechung von parametrischen in ellipsoidische Koordinaten (über kartesische Koordinaten)
|
|
:param u: parametrische Breite
|
|
:param v: parametrische Länge
|
|
:return: ellipsoidische Breite, Länge
|
|
"""
|
|
cart = self.para2cart(u, v)
|
|
return self.cart2ell(cart)
|
|
|
|
def para2geod(self, u: float, v: float, mode: str = "ligas3", maxIter: int = 30, maxLoa: float = 0.005) -> Tuple[float, float, float]:
|
|
"""
|
|
Umrechung von parametrischen in geodätische Koordinaten (über kartesische Koordinaten)
|
|
:param u: parametrische Breite
|
|
:param v: parametrische Länge
|
|
:param mode: ligas1, ligas2, oder ligas3
|
|
:param maxIter: maximale Anzahl Iterationen
|
|
:param maxLoa: Level of Accuracy, das erreicht werden soll
|
|
:return: geodätische Breite, Länge, Höhe
|
|
"""
|
|
cart = self.para2cart(u, v)
|
|
return self.cart2geod(cart, mode, maxIter, maxLoa)
|
|
|
|
def geod2para(self, phi: float, lamb: float, h: float) -> Tuple[float, float]:
|
|
"""
|
|
Umrechung von geodätischen in parametrische Koordinaten (über kartesische Koordinaten)
|
|
:param phi: geodätische Breite
|
|
:param lamb: geodätische Länge
|
|
:param h: geodätische Höhe
|
|
:return: parametrische Breite, Länge
|
|
"""
|
|
cart = self.geod2cart(phi, lamb, h)
|
|
return self.cart2para(cart)
|
|
|
|
def ell2geod(self, beta: float, lamb: float, mode: str = "ligas3", maxIter: int = 30, maxLoa: float = 0.005) -> Tuple[float, float, float]:
|
|
"""
|
|
Umrechung von ellipsoidischen in geodätische Koordinaten (über kartesische Koordinaten)
|
|
:param beta: ellipsoidische Breite
|
|
:param lamb: ellipsoidische Länge
|
|
:param mode: ligas1, ligas2, oder ligas3
|
|
:param maxIter: maximale Anzahl Iterationen
|
|
:param maxLoa: Level of Accuracy, das erreicht werden soll
|
|
:return: geodätische Breite, Länge, Höhe
|
|
"""
|
|
cart = self.ell2cart(beta, lamb)
|
|
return self.cart2geod(cart, mode, maxIter, maxLoa)
|
|
|
|
def geod2ell(self, phi: float, lamb: float, h: float) -> Tuple[float, float]:
|
|
"""
|
|
Umrechung von geodätischen in ellipsoidische Koordinaten (über kartesische Koordinaten)
|
|
:param phi: geodätische Breite
|
|
:param lamb: geodätische Länge
|
|
:param h: geodätische Höhe
|
|
:return: ellipsoidische Breite, Länge
|
|
"""
|
|
cart = self.geod2cart(phi, lamb, h)
|
|
return self.cart2ell(cart)
|
|
|
|
def point_on(self, point: NDArray) -> bool:
|
|
"""
|
|
Test, ob ein Punkt auf dem Ellipsoid liegt
|
|
:param point: kartesische 3D-Koordinaten
|
|
:return: Punkt auf dem Ellispoid?
|
|
"""
|
|
value = point[0]**2/self.ax**2 + point[1]**2/self.ay**2 + point[2]**2/self.b**2
|
|
if abs(1-value) < 1e-6:
|
|
return True
|
|
else:
|
|
return False
|
|
|
|
def point_onto_ellipsoid(self, point: NDArray) -> NDArray:
|
|
"""
|
|
Berechnung des Lotpunktes entlang der Normalkrümmung auf einem Ellipsoiden
|
|
:param point: Punkt in kartesischen Koordinaten, der gelotet werden soll
|
|
:return: Lotpunkt in kartesischen Koordinaten
|
|
"""
|
|
phi, lamb, h = self.cart2geod(point, "ligas3")
|
|
p = self. geod2cart(phi, lamb, 0)
|
|
return p
|
|
|
|
def cart_ellh(self, point: NDArray, h: float) -> NDArray:
|
|
"""
|
|
Punkt auf Ellipsoid hoch loten
|
|
:param point: Punkt auf dem Ellipsoid
|
|
:param h: Höhe über dem Ellipsoid
|
|
:return: hochgeloteter Punkt
|
|
"""
|
|
phi, lamb, _ = self.cart2geod(point, "ligas3")
|
|
pointH = self. geod2cart(phi, lamb, h)
|
|
return pointH
|
|
|
|
|
|
if __name__ == "__main__":
|
|
ell = EllipsoidTriaxial.init_name("KarneyTest2024")
|
|
# cart = ell.ell2cart(np.pi/2, 0)
|
|
# print(cart)
|
|
# cart = ell.ell2cart(np.pi/2*8999999999999999/9000000000000000, 0)
|
|
# print(cart)
|
|
elli = ell.cart2ell([0, 0.0, 1/np.sqrt(2)])
|
|
print(elli)
|
|
|
|
# ell = EllipsoidTriaxial.init_name("BursaSima1980")
|
|
# diff_list = []
|
|
# diffs_para = []
|
|
# diffs_ell = []
|
|
# diffs_geod = []
|
|
# points = []
|
|
# for v_deg in range(-180, 181, 5):
|
|
# for u_deg in range(-90, 91, 5):
|
|
# v = wu.deg2rad(v_deg)
|
|
# u = wu.deg2rad(u_deg)
|
|
# point = ell.para2cart(u, v)
|
|
# points.append(point)
|
|
#
|
|
# elli = ell.cart2ell(point)
|
|
# cart_elli = ell.ell2cart(elli[0], elli[1])
|
|
# diff_ell = np.linalg.norm(point - cart_elli, axis=-1)
|
|
#
|
|
# para = ell.cart2para(point)
|
|
# cart_para = ell.para2cart(para[0], para[1])
|
|
# diff_para = np.linalg.norm(point - cart_para, axis=-1)
|
|
#
|
|
# geod = ell.cart2geod(point, "ligas3")
|
|
# cart_geod = ell.geod2cart(geod[0], geod[1], geod[2])
|
|
# diff_geod3 = np.linalg.norm(point - cart_geod, axis=-1)
|
|
#
|
|
# diff_list.append([v_deg, u_deg, diff_ell, diff_para, diff_geod3])
|
|
# diffs_ell.append([diff_ell])
|
|
# diffs_para.append([diff_para])
|
|
# diffs_geod.append([diff_geod3])
|
|
#
|
|
# diff_list = np.array(diff_list)
|
|
# diffs_ell = np.array(diffs_ell)
|
|
# diffs_para = np.array(diffs_para)
|
|
# diffs_geod = np.array(diffs_geod)
|
|
#
|
|
# pass
|
|
#
|
|
# points = np.array(points)
|
|
# fig = plt.figure()
|
|
# ax = fig.add_subplot(projection='3d')
|
|
#
|
|
# sc = ax.scatter(
|
|
# points[:, 0],
|
|
# points[:, 1],
|
|
# points[:, 2],
|
|
# c=diffs_ell, # Farbcode = diff
|
|
# cmap='viridis', # Colormap
|
|
# s=10 + 20 * diffs_ell, # optional: Größe abhängig vom diff
|
|
# alpha=0.8
|
|
# )
|
|
#
|
|
# # Farbskala
|
|
# cbar = plt.colorbar(sc)
|
|
# cbar.set_label("diff")
|
|
#
|
|
# ax.set_xlabel("X")
|
|
# ax.set_ylabel("Y")
|
|
# ax.set_zlabel("Z")
|
|
#
|
|
# plt.show() |