From 82444208d71c4ca2bf841081ecf0dc9d0d2852df Mon Sep 17 00:00:00 2001 From: Nico Date: Thu, 5 Feb 2026 19:20:09 +0100 Subject: [PATCH] gha1_ES --- GHA_triaxial/gha1_ES.py | 228 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 228 insertions(+) create mode 100644 GHA_triaxial/gha1_ES.py diff --git a/GHA_triaxial/gha1_ES.py b/GHA_triaxial/gha1_ES.py new file mode 100644 index 0000000..c3ffe11 --- /dev/null +++ b/GHA_triaxial/gha1_ES.py @@ -0,0 +1,228 @@ +from __future__ import annotations +from typing import List, Optional, Tuple +import numpy as np + +from GHA_triaxial.utils import pq_ell +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 + +ell: EllipsoidTriaxial = None + + +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 ellipsoidalen Koordinaten (β, ω) nach Karney. + R(β, ω) gemäß Karney (2025), Eq. (4). :contentReference[oaicite:2]{index=2} + Die Ableitungen E=∂R/∂ω und N=∂R/∂β ergeben sich durch direktes Ableiten. + + Rückgabe: + E_hat, N_hat, U_hat, En=||E||, Nn=||N||, R (XYZ) + """ + + 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) + D2 = ell.ax*ell.ax - ell.b*ell.b + if D2 <= 0: + raise ValueError("Degenerierter Fall a^2 - c^2 <= 0 (nahe Kugel).") + D = np.sqrt(D2) + + # Sx = sqrt(a^2 - b^2 sin^2β - c^2 cos^2β) + Sx2 = ell.ax*ell.ax - ell.ay*ell.ay*(sb*sb) - ell.b*ell.b*(cb*cb) + if Sx2 < 0: # numerische Schutzklemme + Sx2 = 0.0 + Sx = np.sqrt(Sx2) + + # Sz = sqrt(a^2 sin^2ω + b^2 cos^2ω - c^2) + Sz2 = ell.ax*ell.ax*(so*so) + ell.ay*ell.ay*(co*co) - ell.b*ell.b + if Sz2 < 0: + Sz2 = 0.0 + Sz = np.sqrt(Sz2) + + # Karney Eq. (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 --- + # 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 + + 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): γ = k^2 cos^2β sin^2α − k'^2 sin^2ω cos^2α + :param beta: Beta Koordinate + :param omega: Omega Koordinate + :param alpha: Azimut alpha + :param ell: Ellipsoid + :return: Jacobi-Konstante + """ + e, k, k_ = ellipsoid_formparameter(ell) + cb = np.cos(beta) + so = np.sin(omega) + sa = np.sin(alpha) + ca = np.cos(alpha) + return float((k ** 2) * (cb ** 2) * (sa ** 2) - (k_ ** 2) * (so ** 2) * (ca ** 2)) + + +def azimuth_at_ESpoint(P_prev: NDArray, P_curr: NDArray, E_hat_curr: NDArray, N_hat_curr: NDArray, U_hat_curr: NDArray): + v = (P_curr - P_prev).astype(float) + vT = v - float(np.dot(v, U_hat_curr)) * U_hat_curr + 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_target: float, ds: float, gamma0: float, + ell: EllipsoidTriaxial, maxSegLen: float = 10000.0, sigma0: float = None): + + # Startbasis (für Predictor + optionales alpha_start) + E_i, N_i, U_i, En_i, Nn_i, P_i = ENU_beta_omega(beta_i, omega_i, ell) + + # Predictor: dβ ≈ ds cosα / |N|, dω ≈ ds sinα / |E| + d_beta = ds * float(np.cos(alpha_target)) / Nn_i + d_omega = ds * float(np.sin(alpha_target)) / 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-3 * (ds / R0) + + def fitness(x: NDArray) -> float: + beta = x[0] + omega = wrap_to_pi(x[1]) + + P = ell.ell2cart(beta, omega) + d = float(np.linalg.norm(P - P_i)) + + # length penalty + J_len = ((d - ds) / ds) ** 2 + if d > maxSegLen * 1.02: + J_len += 1e3 * ((d / maxSegLen) - 1.02) ** 2 + w_len = 1.0 + + # alpha at end, computed using previous point (for Jacobi gamma) + 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 gamma at candidate/end + g_end = jacobi_konstante(beta, omega, alpha_end, ell) + J_gamma = (g_end - gamma0) ** 2 + w_gamma = 10 + + return float(w_len * J_len + w_gamma * J_gamma) + + # Aufruf CMA-ES + xb = escma(fitness, N=2, xmean=xmean, sigma=sigma0) + + beta_best = float(np.clip(float(xb[0]), -0.499999 * np.pi, 0.499999 * np.pi)) + omega_best = wrap_to_pi(float(xb[1])) + P_best = ell.ell2cart(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): + + beta = float(beta0) + omega = wrap_to_pi(float(omega0)) + alpha = wrap_to_pi(float(alpha0)) + + gamma0 = jacobi_konstante(beta, omega, alpha, ell) # Referenz-γ0 + + points: List[NDArray] = [ell.ell2cart(beta, omega)] + + 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_target=alpha, ds=ds, gamma0=gamma0, + ell=ell, maxSegLen=maxSegLen) + + s_acc += ds + points.append(P) + + if step > nsteps_est + 50: + raise RuntimeError("Zu viele Schritte – vermutlich Konvergenzproblem / falsche Azimut-Konvention.") + + Pk = points[-1] + + return Pk + + +if __name__ == "__main__": + ell = EllipsoidTriaxial.init_name("BursaSima1980round") + s = 1888916.50873 + alpha0 = 70/(180/np.pi) + + P0 = ell.ell2cart(5/(180/np.pi), -90/(180/np.pi)) + 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 = gha1_ES(ell, beta0=5/(180/np.pi), omega0=-90/(180/np.pi), alpha0=alpha0, s_total=s, maxSegLen=1000.0) + + print(point1) + print(res) + # print("alpha1 (am Endpunkt):", res.alpha1) + print(res - point1) + print(point1app - point1, "approx") \ No newline at end of file