wraps

GHA_triaxial/gha1_approx.py

@@ -6,6 +6,7 @@ from GHA_triaxial.gha1_ana import gha1_ana
 from GHA_triaxial.utils import func_sigma_ell, louville_constant, pq_ell
 import plotly.graph_objects as go
 import winkelumrechnungen as wu
+from utils_angle import wrap_0_2pi, wrap_mhalfpi_halfpi, wrap_mpi_pi
 
 def gha1_approx(ell: EllipsoidTriaxial, p0: np.ndarray, alpha0: float, s: float, ds: float, all_points: bool = False) -> Tuple[NDArray, float] | Tuple[NDArray, float, NDArray]:
     """
@@ -37,19 +38,24 @@ def gha1_approx(ell: EllipsoidTriaxial, p0: np.ndarray, alpha0: float, s: float,
         if last_p is not None and np.dot(p, last_p) < 0:
             p = -p
             q = -q
+        last_p = p
         sigma = p * sin(alpha1) + q * cos(alpha1)
         if last_sigma is not None and np.dot(sigma, last_sigma) < 0:
             sigma = -sigma
+            alpha1 += np.pi
+            alpha1 = wrap_0_2pi(alpha1)
         p2 = p1 + ds_step * sigma
         p2 = ell.point_onto_ellipsoid(p2)
 
-        dalpha = 1e-6
+        dalpha = 1e-9
         l2 = louville_constant(ell, p2, alpha1)
         dl_dalpha = (louville_constant(ell, p2, alpha1+dalpha) - l2) / dalpha
-        alpha2 = alpha1 + (l0 - l2) / dl_dalpha
-
+        if abs(dl_dalpha) < 1e-20:
+            alpha2 = alpha1 + 0
+        else:
+            alpha2 = alpha1 + (l0 - l2) / dl_dalpha
         points.append(p2)
-        alphas.append(alpha2)
+        alphas.append(wrap_0_2pi(alpha2))
 
         ds_step = np.linalg.norm(p2 - p1)
         s_curr += ds_step
@@ -88,11 +94,11 @@ def show_points(points: NDArray, p0: NDArray, p1: NDArray):
 
 
 if __name__ == '__main__':
-    ell = EllipsoidTriaxial.init_name("BursaSima1980round")
-    P0 = ell.ell2cart(wu.deg2rad(89), wu.deg2rad(1))
-    alpha0 = wu.deg2rad(2)
-    s = 200000
-    P1_app, alpha1_app, points, alphas = gha1_approx(ell, P0, alpha0, s, ds=100, all_points=True)
-    P1_ana, alpha1_ana = gha1_ana(ell, P0, alpha0, s, maxM=20, maxPartCircum=2)
-    print(np.linalg.norm(P1_app - P1_ana))
-    show_points(points, P0, P1_ana)
+    ell = EllipsoidTriaxial.init_name("KarneyTest2024")
+    P0 = ell.ell2cart(wu.deg2rad(15), wu.deg2rad(15))
+    alpha0 = wu.deg2rad(270)
+    s = 1
+    P1_app, alpha1_app, points, alphas = gha1_approx(ell, P0, alpha0, s, ds=0.1, all_points=True)
+    # P1_ana, alpha1_ana = gha1_ana(ell, P0, alpha0, s, maxM=40, maxPartCircum=32)
+    # print(np.linalg.norm(P1_app - P1_ana))
+    # show_points(points, P0, P0)