Merge remote-tracking branch 'origin/main'

GHA_triaxial/gha1_ES.py

@@ -1,4 +1,6 @@
 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
@@ -7,6 +9,7 @@ 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):
@@ -78,6 +81,8 @@ def ENU_beta_omega(beta: float, omega: float, ell: EllipsoidTriaxial) \
     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
 
@@ -110,7 +115,9 @@ def azimuth_at_ESpoint(P_prev: NDArray, P_curr: NDArray, E_hat_curr: NDArray, N_
     """
     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)
+    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))
 
@@ -136,8 +143,20 @@ def optimize_next_point(beta_i: float, omega_i: float, alpha_i: float, ds: float
     # 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
+    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)
 
@@ -158,7 +177,7 @@ def optimize_next_point(beta_i: float, omega_i: float, alpha_i: float, ds: float
         beta = x[0]
         omega = wrap_to_pi(x[1])
 
-        P = ell.ell2cart(beta, omega) # in kartesischer Koordinaten
+        P = ell.ell2cart_karney(beta, omega) # in kartesischer Koordinaten
         d = float(np.linalg.norm(P - P_i)) # Distanz zwischen
 
         # maxSegLen einhalten
@@ -183,14 +202,15 @@ def optimize_next_point(beta_i: float, omega_i: float, alpha_i: float, ds: float
 
     beta_best = xb[0]
     omega_best = wrap_to_pi(xb[1])
-    P_best = ell.ell2cart(beta_best, omega_best)
+    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):
+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
@@ -199,7 +219,7 @@ def gha1_ES(ell: EllipsoidTriaxial, beta0: float, omega0: float, alpha0: float,
     :param alpha0: Azimut Startkoordinate
     :param s_total: Gesamtstrecke
     :param maxSegLen: maximale Segmentlänge
-    :return: Zielpunkt Pk und Azimut am Zielpunkt
+    :return: Zielpunkt Pk, Azimut am Zielpunkt und Punktliste
     """
     beta = float(beta0)
     omega = wrap_to_pi(float(omega0))
@@ -207,7 +227,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)]
+    P_all: NDArray[NDArray] = np.array([ell.ell2cart_karney(beta, omega)])
     alpha_end: List[float] = [alpha]
 
     s_acc = 0.0
@@ -221,30 +241,33 @@ 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_i=alpha, ds=ds, gamma0=gamma0,
                                                     ell=ell, maxSegLen=maxSegLen)
         s_acc += ds
-        points.append(P)
+        P_all.append(P)
         alpha_end.append(alpha)
         if step > nsteps_est + 50:
             raise RuntimeError("Zu viele Schritte – vermutlich Konvergenzproblem / falsche Azimut-Konvention.")
-    Pk = points[-1]
+    Pk = P_all[-1]
-    alpha1 = alpha_end[-1]
+    alpha1 = float(alpha_end[-1])
 
-    return Pk, alpha1
+    return Pk, alpha1, P_all
 
 
 if __name__ == "__main__":
     ell = EllipsoidTriaxial.init_name("BursaSima1980round")
-    s = 188891.650873
+    s = 18000
-    alpha0 = 70/(180/np.pi)
+    #alpha0 = 3
-
+    alpha0 = wu.gms2rad([5 ,0 ,0])
-    P0 = ell.ell2cart(5/(180/np.pi), -90/(180/np.pi))
+    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 = gha1_ES(ell, beta0=5/(180/np.pi), omega0=-90/(180/np.pi), alpha0=alpha0, s_total=s, maxSegLen=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("alpha1 (am Endpunkt):", res.alpha1)
+    #print(points)
+    #print("alpha1 (am Endpunkt):", res.alpha1)
     print(res - point1)
     print(point1app - point1, "approx")