Merge remote-tracking branch 'origin/main'

# Conflicts:
#	GHA_triaxial/gha2_num.py

GHA_triaxial/gha1_ES.py

@@ -0,0 +1,250 @@
+from __future__ import annotations
+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
+
+
+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
+
+    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
+    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|
+    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(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(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):
+    """
+    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 und Azimut am Zielpunkt
+    """
+    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)]
+    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
+        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
+
+
+if __name__ == "__main__":
+    ell = EllipsoidTriaxial.init_name("BursaSima1980round")
+    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, alpha = gha1_ES(ell, beta0=5/(180/np.pi), omega0=-90/(180/np.pi), alpha0=alpha0, s_total=s, maxSegLen=1000)
+
+    print(point1)
+    print(res)
+    print(alpha)
+    # print("alpha1 (am Endpunkt):", res.alpha1)
+    print(res - point1)
+    print(point1app - point1, "approx")

GHA_triaxial/gha2_ES.py

@@ -49,7 +49,7 @@ def midpoint_fitness(x: tuple) -> float:
     return f
 
 
-def gha2_ES(ell: EllipsoidTriaxial, P0: NDArray, Pk: NDArray, maxSegLen: float = None, stopeval: int = 2000, maxIter: int = 10000, all_points: bool = False):
+def gha2_ES(ell: EllipsoidTriaxial, P0: NDArray, Pk: NDArray, maxSegLen: float = None, maxIter: int = 10000, all_points: bool = False):
     """
     Berechnen der 2. GHA mithilfe der CMA-ES.
     Die CMA-ES optimiert sukzessive den Mittelpunkt zwischen Start- und Zielpunkt. Der Abbruch der Berechnung erfolgt, wenn alle Segmentlängen <= maxSegLen sind.
@@ -58,7 +58,6 @@ def gha2_ES(ell: EllipsoidTriaxial, P0: NDArray, Pk: NDArray, maxSegLen: float =
     :param P0: Startpunkt
     :param Pk: Zielpunkt
     :param maxSegLen: maximale Segmentlänge
-    :param stopeval: maximale Durchläufe der CMA-ES
     :param maxIter: maximale Durchläufe der Mittelpunktsgenerierung
     :param all_points: Ergebnisliste mit allen Punkte, die wahlweise mit ausgegeben werden kann
     :return: Richtungswinkel des Start- und Zielpunktes und Gesamtlänge
@@ -67,7 +66,7 @@ def gha2_ES(ell: EllipsoidTriaxial, P0: NDArray, Pk: NDArray, maxSegLen: float =
     ell_ES = ell
     R0 = (ell.ax + ell.ay + ell.b) / 3
     if maxSegLen is None:
-        maxSegLen = R0 * 1 / (637.4) # 10km Segment bei mittleren Erdradius
+        maxSegLen = R0 * 1 / (637.4*2) # 10km Segment bei mittleren Erdradius
 
     sigma_uv_nom = 1e-3 * (maxSegLen / R0)  # ~1e-5
 
@@ -101,8 +100,7 @@ def gha2_ES(ell: EllipsoidTriaxial, P0: NDArray, Pk: NDArray, maxSegLen: float =
 
                 sigmaStep = sigma_uv_nom * (Sehne(A, B) / maxSegLen)
 
-                u, v = escma(midpoint_fitness, N=2, xmean=xmean, sigma=sigmaStep, stopfitness=-np.inf,
+                u, v = escma(midpoint_fitness, N=2, xmean=xmean, sigma=sigmaStep)
-                             stopeval=stopeval)
 
                 P_next = ell.para2cart(u, v)
                 new_points.append(P_next)
@@ -171,7 +169,7 @@ if __name__ == '__main__':
     beta1, lamb1 = (0.7, 0.3)
     P1 = ell.ell2cart(beta1, lamb1)
 
-    alpha0, alpha1, s_num, betas, lambs = gha2_num(ell, beta0, lamb0, beta1, lamb1, n=10000, all_points=True)
+    alpha0, alpha1, s_num, betas, lambs = gha2_num(ell, beta0, lamb0, beta1, lamb1, n=1000, all_points=True)
     points_num = []
     for beta, lamb in zip(betas, lambs):
         points_num.append(ell.ell2cart(beta, lamb))

Hansen_ES_CMA.py

@@ -9,8 +9,8 @@ def felli(x):
     return float(np.sum((1e6 ** exponents) * (x ** 2)))
 
 
-def escma(func, *, N=10, xmean=None, sigma=0.5, stopfitness=1e-10, stopeval=None,
+def escma(func, *, N=10, xmean=None, sigma=0.5, stopfitness=1e-14, stopeval=2000,
-          func_args=(), func_kwargs=None, seed=None,
+          func_args=(), func_kwargs=None, seed=0,
           bestEver = np.inf, noImproveGen = 0, absTolImprove = 1e-12, maxNoImproveGen = 100, sigmaImprove = 1e-12):
 
     if func_kwargs is None:
@@ -142,6 +142,7 @@ def escma(func, *, N=10, xmean=None, sigma=0.5, stopfitness=1e-10, stopeval=None
             sigma = sigma * np.exp(0.2 + cs / damps)
             print('    [CMA-ES] stopfitness erreicht.')
             #print("warning: flat fitness, consider reformulating the objective")
+            break
 
         #print(f"{counteval}: {arfitness[0]}")