From e11edd2a43704cf4bc6395c68ffe7e76385dd2e1 Mon Sep 17 00:00:00 2001 From: Nico Date: Thu, 5 Feb 2026 21:01:06 +0100 Subject: [PATCH] henreick --- GHA_triaxial/gha1_ES.py | 107 ++++++++++++++++++++++------------------ 1 file changed, 58 insertions(+), 49 deletions(-) diff --git a/GHA_triaxial/gha1_ES.py b/GHA_triaxial/gha1_ES.py index c3ffe11..ac00280 100644 --- a/GHA_triaxial/gha1_ES.py +++ b/GHA_triaxial/gha1_ES.py @@ -1,8 +1,6 @@ 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 @@ -10,8 +8,6 @@ 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): """ @@ -23,51 +19,43 @@ def ellipsoid_formparameter(ell: EllipsoidTriaxial): 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) + 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) - 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) - + D = np.sqrt(ell.ax*ell.ax - ell.b*ell.b) # 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) - + 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) - 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) + Sz = np.sqrt(ell.ax*ell.ax*(so*so) + ell.ay*ell.ay*(co*co) - ell.b*ell.b) - # Karney Eq. (4) + # 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 --- + # --- Ableitungen - Karney Gl. (5a,b,c)--- # E = ∂R/∂ω dX_dw = -ell.ax * so * Sx / D dY_dw = ell.ay * cb * co @@ -80,7 +68,7 @@ def ENU_beta_omega(beta: float, omega: float, ell: EllipsoidTriaxial) \ 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 = 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) @@ -94,10 +82,9 @@ def ENU_beta_omega(beta: float, omega: float, ell: EllipsoidTriaxial) \ 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α + Jacobi-Konstante nach Karney (2025), Gl. (14) :param beta: Beta Koordinate :param omega: Omega Koordinate :param alpha: Azimut alpha @@ -105,28 +92,48 @@ def jacobi_konstante(beta: float, omega: float, alpha: float, ell: EllipsoidTria :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)) + 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): +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 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): +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) -> Tuple[float, float, NDArray, float]: + """ + + :param beta_i: + :param omega_i: + :param alpha_target: + :param ds: + :param gamma0: + :param ell: + :param maxSegLen: + :param sigma0: + :return: + """ # 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 @@ -164,8 +171,7 @@ def optimize_next_point(beta_i: float, omega_i: float, alpha_target: float, ds: return float(w_len * J_len + w_gamma * J_gamma) - # Aufruf CMA-ES - xb = escma(fitness, N=2, xmean=xmean, sigma=sigma0) + xb = escma(fitness, N=2, xmean=xmean, sigma=sigma0) # Aufruf CMA-ES beta_best = float(np.clip(float(xb[0]), -0.499999 * np.pi, 0.499999 * np.pi)) omega_best = wrap_to_pi(float(xb[1])) @@ -176,6 +182,7 @@ def optimize_next_point(beta_i: float, omega_i: float, alpha_target: float, ds: 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) @@ -185,6 +192,7 @@ def gha1_ES(ell: EllipsoidTriaxial, beta0: float, omega0: float, alpha0: float, gamma0 = jacobi_konstante(beta, omega, alpha, ell) # Referenz-γ0 points: List[NDArray] = [ell.ell2cart(beta, omega)] + alpha_end: List[float] = [alpha] s_acc = 0.0 step = 0 @@ -198,31 +206,32 @@ def gha1_ES(ell: EllipsoidTriaxial, beta0: float, omega0: float, alpha0: float, 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) - + alpha_end.append(alpha) if step > nsteps_est + 50: raise RuntimeError("Zu viele Schritte – vermutlich Konvergenzproblem / falsche Azimut-Konvention.") - Pk = points[-1] + alpha1 = alpha_end[-1] + + return Pk, alpha1 - return Pk if __name__ == "__main__": ell = EllipsoidTriaxial.init_name("BursaSima1980round") - s = 1888916.50873 + s = 188891.650873 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) + res, alpha = 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(alpha) # print("alpha1 (am Endpunkt):", res.alpha1) print(res - point1) print(point1app - point1, "approx") \ No newline at end of file