From a836d2d53493445e1d894444d8917bdfa39bf89f Mon Sep 17 00:00:00 2001 From: Nico Date: Fri, 6 Feb 2026 14:30:11 +0100 Subject: [PATCH] Punktliste --- GHA_triaxial/gha1_ES.py | 59 ++++++++++++++++++++++++++++------------- 1 file changed, 41 insertions(+), 18 deletions(-) diff --git a/GHA_triaxial/gha1_ES.py b/GHA_triaxial/gha1_ES.py index fbdb159..adeda08 100644 --- a/GHA_triaxial/gha1_ES.py +++ b/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] - alpha1 = alpha_end[-1] + Pk = P_all[-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 - alpha0 = 70/(180/np.pi) - - P0 = ell.ell2cart(5/(180/np.pi), -90/(180/np.pi)) + 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 = 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") \ No newline at end of file