Algorithmen Test

GHA_triaxial/approx_gha1_2.py

@@ -0,0 +1,120 @@
+import numpy as np
+from ellipsoide import EllipsoidTriaxial
+from panou import louville_constant, func_sigma_ell, gha1_ana
+import plotly.graph_objects as go
+import winkelumrechnungen as wu
+from numpy import sin, cos, arccos
+
+def Bogenlaenge(P1: NDArray, P2: NDArray) -> float:
+    """
+    Berechnung der mittleren Bogenlänge zwischen zwei kartesischen Punkten
+    :param P1: kartesische Koordinate Punkt 1
+    :param P2: kartesische Koordinate Punkt 2
+    :return: Bogenlänge s
+    """
+    R1 = np.linalg.norm(P1)
+    R2 = np.linalg.norm(P2)
+    R = 0.5 * (R1 + R2)
+    if P1 @ P2 / (R1 * R2) > 1:
+        s = np.linalg.norm(P1 - P2)
+    else:
+        theta = arccos(P1 @ P2 / (R1 * R2))
+        s = float(R * theta)
+    return s
+
+def gha1_approx2(ell: EllipsoidTriaxial, p0: np.ndarray, alpha0: float, s: float, ds: float, all_points: bool = False) -> Tuple[NDArray, float] | Tuple[NDArray, float, NDArray]:
+    """
+    Berechung einer Näherungslösung der ersten Hauptaufgabe
+    :param ell: Ellipsoid
+    :param p0: Anfangspunkt
+    :param alpha0: Azimut im Anfangspunkt
+    :param s: Strecke bis zum Endpunkt
+    :param ds: Länge einzelner Streckenelemente
+    :param all_points: Ausgabe aller Punkte als Array?
+    :return: Endpunkt, Azimut im Endpunkt, optional alle Punkte
+    """
+    l0 = louville_constant(ell, p0, alpha0)
+    points = [p0]
+    alphas = [alpha0]
+    s_curr = 0.0
+
+    while s_curr < s:
+        ds_target = min(ds, s - s_curr)
+        if ds_target < 1e-8:
+            break
+
+        p1 = points[-1]
+        alpha1 = alphas[-1]
+        alpha1_mid = alphas[-1]
+        p2 = points[-1]
+        alpha2 = alphas[-1]
+
+        i = 0
+        while i < 2:
+            i += 1
+
+            sigma = func_sigma_ell(ell, p1, alpha1_mid)
+            p2_new = p1 + ds_target * sigma
+            p2_new = ell.point_onto_ellipsoid(p2_new)
+            p2 = p2_new
+
+            j = 0
+            while j < 2:
+                j += 1
+
+                dalpha = 1e-6
+                l2 = louville_constant(ell, p2, alpha2)
+                dl_dalpha = (louville_constant(ell, p2, alpha2 + dalpha) - l2) / dalpha
+                alpha2_new = alpha2 + (l0 - l2) / dl_dalpha
+                alpha2 = alpha2_new
+
+            alpha1_mid = (alpha1 + alpha2) / 2
+
+        points.append(p2)
+        alphas.append(alpha2)
+
+        ds_actual = np.linalg.norm(p2 - p1)
+        s_curr += ds_actual
+        if s_curr > 10000000:
+            pass
+
+    if all_points:
+        return points[-1], alphas[-1], np.array(points)
+    else:
+        return points[-1], alphas[-1]
+
+def show_points(points: NDArray, p0: NDArray, p1: NDArray):
+    """
+    Anzeigen der Punkte
+    :param points: Array aller approximierten Punkte
+    :param p0: Startpunkt
+    :param p1: wahrer Endpunkt
+    """
+    fig = go.Figure()
+
+    fig.add_scatter3d(x=points[:, 0], y=points[:, 1], z=points[:, 2],
+                      mode='lines', line=dict(color="red", width=3), name="Approx")
+    fig.add_scatter3d(x=[p0[0]], y=[p0[1]], z=[p0[2]],
+                      mode='markers', marker=dict(color="green"), name="P0")
+    fig.add_scatter3d(x=[p1[0]], y=[p1[1]], z=[p1[2]],
+                      mode='markers', marker=dict(color="green"), name="P1")
+
+    fig.update_layout(
+        scene=dict(xaxis_title='X [km]',
+                   yaxis_title='Y [km]',
+                   zaxis_title='Z [km]',
+                   aspectmode='data'),
+        title="CHAMP")
+
+    fig.show()
+
+
+if __name__ == '__main__':
+    ell = EllipsoidTriaxial.init_name("BursaSima1980round")
+    P0 = ell.para2cart(0.2, 0.3)
+    alpha0 = wu.deg2rad(35)
+    s = 13000000
+    P1_app, alpha1_app, points = gha1_approx2(ell, P0, alpha0, s, ds=10000, all_points=True)
+    P1_ana, alpha1_ana = gha1_ana(ell, P0, alpha0, s, maxM=60, maxPartCircum=16)
+    show_points(points, P0, P1_ana)
+    print(np.linalg.norm(P1_app - P1_ana))