273 lines
9.8 KiB
Python
273 lines
9.8 KiB
Python
from __future__ import annotations
|
||
|
||
from codeop import PyCF_ALLOW_INCOMPLETE_INPUT
|
||
from typing import List, Optional, Tuple
|
||
import numpy as np
|
||
from ellipsoide import EllipsoidTriaxial
|
||
from GHA_triaxial.gha1_ana import gha1_ana
|
||
from GHA_triaxial.gha1_approx import gha1_approx
|
||
from Hansen_ES_CMA import escma
|
||
from utils_angle import wrap_to_pi
|
||
from numpy.typing import NDArray
|
||
import winkelumrechnungen as wu
|
||
|
||
|
||
def ellipsoid_formparameter(ell: EllipsoidTriaxial):
|
||
"""
|
||
Berechnet die Formparameter des dreiachsigen Ellipsoiden nach Karney (2025), Gl. (2)
|
||
:param ell: Ellipsoid
|
||
:return: e, k und k'
|
||
"""
|
||
nenner = np.sqrt(max(ell.ax * ell.ax - ell.b * ell.b, 0.0))
|
||
k = np.sqrt(max(ell.ay * ell.ay - ell.b * ell.b, 0.0)) / nenner
|
||
k_ = np.sqrt(max(ell.ax * ell.ax - ell.ay * ell.ay, 0.0)) / nenner
|
||
e = np.sqrt(max(ell.ax * ell.ax - ell.b * ell.b, 0.0)) / ell.ay
|
||
|
||
return e, k, k_
|
||
|
||
|
||
def ENU_beta_omega(beta: float, omega: float, ell: EllipsoidTriaxial) \
|
||
-> Tuple[NDArray, NDArray, NDArray, float, float, NDArray]:
|
||
"""
|
||
Analytische ENU-Basis in ellipsoidische Koordinaten (β, ω) nach Karney (2025), S. 2
|
||
:param beta: Beta Koordinate
|
||
:param omega: Omega Koordinate
|
||
:param ell: Ellipsoid
|
||
:return: E_hat = Einheitsrichtung entlang wachsendem ω (East)
|
||
N_hat = Einheitsrichtung entlang wachsendem β (North)
|
||
U_hat = Einheitsnormale (Up)
|
||
En & Nn = Längen der unnormierten Ableitungen
|
||
R (XYZ) = Punkt in XYZ
|
||
"""
|
||
# Berechnungshilfen
|
||
omega = wrap_to_pi(omega)
|
||
cb = np.cos(beta)
|
||
sb = np.sin(beta)
|
||
co = np.cos(omega)
|
||
so = np.sin(omega)
|
||
# D = sqrt(a^2 - c^2)
|
||
D = np.sqrt(ell.ax*ell.ax - ell.b*ell.b)
|
||
# Sx = sqrt(a^2 - b^2 sin^2β - c^2 cos^2β)
|
||
Sx = np.sqrt(ell.ax*ell.ax - ell.ay*ell.ay*(sb*sb) - ell.b*ell.b*(cb*cb))
|
||
# Sz = sqrt(a^2 sin^2ω + b^2 cos^2ω - c^2)
|
||
Sz = np.sqrt(ell.ax*ell.ax*(so*so) + ell.ay*ell.ay*(co*co) - ell.b*ell.b)
|
||
|
||
# Karney Gl. (4)
|
||
X = ell.ax * co * Sx / D
|
||
Y = ell.ay * cb * so
|
||
Z = ell.b * sb * Sz / D
|
||
R = np.array([X, Y, Z], dtype=float)
|
||
|
||
# --- Ableitungen - Karney Gl. (5a,b,c)---
|
||
# E = ∂R/∂ω
|
||
dX_dw = -ell.ax * so * Sx / D
|
||
dY_dw = ell.ay * cb * co
|
||
dZ_dw = ell.b * sb * (so * co * (ell.ax*ell.ax - ell.ay*ell.ay) / Sz) / D
|
||
E = np.array([dX_dw, dY_dw, dZ_dw], dtype=float)
|
||
|
||
# N = ∂R/∂β
|
||
dX_db = ell.ax * co * (sb * cb * (ell.b*ell.b - ell.ay*ell.ay) / Sx) / D
|
||
dY_db = -ell.ay * sb * so
|
||
dZ_db = ell.b * cb * Sz / D
|
||
N = np.array([dX_db, dY_db, dZ_db], dtype=float)
|
||
|
||
# U = Grad(x^2/a^2 + y^2/b^2 + z^2/c^2 - 1)
|
||
U = np.array([X/(ell.ax*ell.ax), Y/(ell.ay*ell.ay), Z/(ell.b*ell.b)], dtype=float)
|
||
|
||
En = np.linalg.norm(E)
|
||
Nn = np.linalg.norm(N)
|
||
Un = np.linalg.norm(U)
|
||
|
||
N_hat = N / Nn
|
||
E_hat = E / En
|
||
U_hat = U / Un
|
||
E_hat = E_hat - float(np.dot(E_hat, N_hat)) * N_hat
|
||
E_hat = E_hat / max(np.linalg.norm(E_hat), 1e-18)
|
||
|
||
return E_hat, N_hat, U_hat, En, Nn, R
|
||
|
||
|
||
def jacobi_konstante(beta: float, omega: float, alpha: float, ell: EllipsoidTriaxial) -> float:
|
||
"""
|
||
Jacobi-Konstante nach Karney (2025), Gl. (14)
|
||
:param beta: Beta Koordinate
|
||
:param omega: Omega Koordinate
|
||
:param alpha: Azimut alpha
|
||
:param ell: Ellipsoid
|
||
:return: Jacobi-Konstante
|
||
"""
|
||
e, k, k_ = ellipsoid_formparameter(ell)
|
||
gamma_jacobi = float((k ** 2) * (np.cos(beta) ** 2) * (np.sin(alpha) ** 2) - (k_ ** 2) * (np.sin(omega) ** 2) * (np.cos(alpha) ** 2))
|
||
|
||
return gamma_jacobi
|
||
|
||
|
||
def azimuth_at_ESpoint(P_prev: NDArray, P_curr: NDArray, E_hat_curr: NDArray, N_hat_curr: NDArray, U_hat_curr: NDArray) -> float:
|
||
"""
|
||
Berechnet das Azimut in der lokalen Tangentialebene am aktuellen Punkt P_curr, gemessen
|
||
an der Bewegungsrichtung vom vorherigen Punkt P_prev nach P_curr.
|
||
:param P_prev: vorheriger Punkt
|
||
:param P_curr: aktueller Punkt
|
||
:param E_hat_curr: Einheitsvektor der lokalen Tangentialrichtung am Punkt P_curr
|
||
:param N_hat_curr: Einheitsvektor der lokalen Tangentialrichtung am Punkt P_curr
|
||
:param U_hat_curr: Einheitsnormalenvektor am Punkt P_curr
|
||
:return: Azimut in Radiant
|
||
"""
|
||
v = (P_curr - P_prev).astype(float)
|
||
vT = v - float(np.dot(v, U_hat_curr)) * U_hat_curr
|
||
vTn = max(np.linalg.norm(vT), 1e-18)
|
||
vT_hat = vT / vTn
|
||
#vT_hat = vT / np.linalg.norm(vT)
|
||
sE = float(np.dot(vT_hat, E_hat_curr))
|
||
sN = float(np.dot(vT_hat, N_hat_curr))
|
||
|
||
return wrap_to_pi(float(np.arctan2(sE, sN)))
|
||
|
||
|
||
def optimize_next_point(beta_i: float, omega_i: float, alpha_i: float, ds: float, gamma0: float,
|
||
ell: EllipsoidTriaxial, maxSegLen: float = 1000.0, sigma0: float = None) -> Tuple[float, float, NDArray, float]:
|
||
"""
|
||
Berechnung der 1. GHA mithilfe der CMA-ES.
|
||
Die CMA-ES optimiert sukzessive einen Punkt, der maxSegLen vom vorherigen Punkt entfernt und zusätzlich auf der
|
||
geodätischen Linien liegt. Somit entsteht ein Geodäten ähnlicher Polygonzug auf der Oberfläche des dreiachsigen Ellipsoids.
|
||
:param beta_i: Beta Koordinate am Punkt i
|
||
:param omega_i: Omega Koordinate am Punkt i
|
||
:param alpha_i: Azimut am Punkt i
|
||
:param ds: Gesamtlänge
|
||
:param gamma0: Jacobi-Konstante am Startpunkt
|
||
:param ell: Ellipsoid
|
||
:param maxSegLen: maximale Segmentlänge
|
||
:param sigma0:
|
||
:return:
|
||
"""
|
||
# Startbasis
|
||
E_i, N_i, U_i, En_i, Nn_i, P_i = ENU_beta_omega(beta_i, omega_i, ell)
|
||
# Prediktor: dβ ≈ ds cosα / |N|, dω ≈ ds sinα / |E|
|
||
|
||
En_eff = max(En_i, 1e-9)
|
||
Nn_eff = max(Nn_i, 1e-9)
|
||
|
||
d_beta = ds * np.cos(alpha_i) / Nn_eff
|
||
d_omega = ds * np.sin(alpha_i) / En_eff
|
||
|
||
# optional: harte Schritt-Clamps (verhindert wrap-chaos)
|
||
d_beta = float(np.clip(d_beta, -0.2, 0.2)) # rad
|
||
d_omega = float(np.clip(d_omega, -0.2, 0.2)) # rad
|
||
|
||
|
||
#d_beta = ds * float(np.cos(alpha_i)) / Nn_i
|
||
#d_omega = ds * float(np.sin(alpha_i)) / En_i
|
||
beta_pred = beta_i + d_beta
|
||
omega_pred = wrap_to_pi(omega_i + d_omega)
|
||
|
||
xmean = np.array([beta_pred, omega_pred], dtype=float)
|
||
|
||
if sigma0 is None:
|
||
R0 = (ell.ax + ell.ay + ell.b) / 3
|
||
sigma0 = 1e-5 * (ds / R0)
|
||
|
||
def fitness(x: NDArray) -> float:
|
||
"""
|
||
Fitnessfunktion: Fitnesscheck erfolgt anhand der Segmentlänge und der Jacobi-Konstante.
|
||
Die Segmentlänge muss möglichst gut zum Sollwert passen. Die Jacobi-Konstante am Punkt x muss zur
|
||
Jacobi-Konstanten am Startpunkt passen, damit der Polygonzug auf derselben geodätischen Linie bleibt.
|
||
:param x: Koordinate in beta, lambda aus der CMA-ES
|
||
:return: Fitnesswert (f)
|
||
"""
|
||
beta = x[0]
|
||
omega = wrap_to_pi(x[1])
|
||
|
||
P = ell.ell2cart_karney(beta, omega) # in kartesischer Koordinaten
|
||
d = float(np.linalg.norm(P - P_i)) # Distanz zwischen
|
||
|
||
# maxSegLen einhalten
|
||
J_len = ((d - ds) / ds) ** 2
|
||
w_len = 1.0
|
||
|
||
# Azimut für Jacobi-Konstante
|
||
E_j, N_j, U_j, _, _, _ = ENU_beta_omega(beta, omega, ell)
|
||
alpha_end = azimuth_at_ESpoint(P_i, P, E_j, N_j, U_j)
|
||
|
||
# Jacobi-Konstante
|
||
g_end = jacobi_konstante(beta, omega, alpha_end, ell)
|
||
J_gamma = (g_end - gamma0) ** 2
|
||
w_gamma = 10
|
||
|
||
f = float(w_len * J_len + w_gamma * J_gamma)
|
||
|
||
return f
|
||
|
||
|
||
xb = escma(fitness, N=2, xmean=xmean, sigma=sigma0) # Aufruf CMA-ES
|
||
|
||
beta_best = xb[0]
|
||
omega_best = wrap_to_pi(xb[1])
|
||
P_best = ell.ell2cart_karney(beta_best, omega_best)
|
||
E_j, N_j, U_j, _, _, _ = ENU_beta_omega(beta_best, omega_best, ell)
|
||
alpha_end = azimuth_at_ESpoint(P_i, P_best, E_j, N_j, U_j)
|
||
|
||
return beta_best, omega_best, P_best, alpha_end
|
||
|
||
|
||
def gha1_ES(ell: EllipsoidTriaxial, beta0: float, omega0: float, alpha0: float, s_total: float, maxSegLen: float = 1000)\
|
||
-> Tuple[NDArray, float, NDArray]:
|
||
"""
|
||
Aufruf der 1. GHA mittels CMA-ES
|
||
:param ell: Ellipsoid
|
||
:param beta0: Beta Startkoordinate
|
||
:param omega0: Omega Startkoordinate
|
||
:param alpha0: Azimut Startkoordinate
|
||
:param s_total: Gesamtstrecke
|
||
:param maxSegLen: maximale Segmentlänge
|
||
:return: Zielpunkt Pk, Azimut am Zielpunkt und Punktliste
|
||
"""
|
||
beta = float(beta0)
|
||
omega = wrap_to_pi(float(omega0))
|
||
alpha = wrap_to_pi(float(alpha0))
|
||
|
||
gamma0 = jacobi_konstante(beta, omega, alpha, ell) # Referenz-γ0
|
||
|
||
P_all: NDArray[NDArray] = np.array([ell.ell2cart_karney(beta, omega)])
|
||
alpha_end: List[float] = [alpha]
|
||
|
||
s_acc = 0.0
|
||
step = 0
|
||
nsteps_est = int(np.ceil(s_total / maxSegLen))
|
||
while s_acc < s_total - 1e-9:
|
||
step += 1
|
||
ds = min(maxSegLen, s_total - s_acc)
|
||
print(f"[GHA1-ES] Step {step}/{nsteps_est} ds={ds:.3f} m s_acc={s_acc:.3f} m beta={beta:.6f} omega={omega:.6f} alpha={alpha:.6f}")
|
||
|
||
beta, omega, P, alpha = optimize_next_point(beta_i=beta, omega_i=omega, alpha_i=alpha, ds=ds, gamma0=gamma0,
|
||
ell=ell, maxSegLen=maxSegLen)
|
||
s_acc += ds
|
||
P_all.append(P)
|
||
alpha_end.append(alpha)
|
||
if step > nsteps_est + 50:
|
||
raise RuntimeError("Zu viele Schritte – vermutlich Konvergenzproblem / falsche Azimut-Konvention.")
|
||
Pk = P_all[-1]
|
||
alpha1 = float(alpha_end[-1])
|
||
|
||
return Pk, alpha1, P_all
|
||
|
||
|
||
if __name__ == "__main__":
|
||
ell = EllipsoidTriaxial.init_name("BursaSima1980round")
|
||
s = 18000
|
||
#alpha0 = 3
|
||
alpha0 = wu.gms2rad([5 ,0 ,0])
|
||
beta = 0
|
||
omega = 0
|
||
P0 = ell.ell2cart(beta, omega)
|
||
point1, alpha1 = gha1_ana(ell, P0, alpha0=alpha0, s=s, maxM=100, maxPartCircum=32)
|
||
point1app, alpha1app = gha1_approx(ell, P0, alpha0=alpha0, s=s, ds=1000)
|
||
|
||
res, alpha, points = gha1_ES(ell, beta0=beta, omega0=-omega, alpha0=alpha0, s_total=s, maxSegLen=1000)
|
||
|
||
print(point1)
|
||
print(res)
|
||
print(alpha)
|
||
#print(points)
|
||
#print("alpha1 (am Endpunkt):", res.alpha1)
|
||
print(res - point1)
|
||
print(point1app - point1, "approx") |