Masterprojekt/GHA_triaxial/gha2_num.py

import numpy as np
from ellipsoide import EllipsoidTriaxial
import runge_kutta as rk
from typing import Tuple
from numpy.typing import NDArray

from utils_angle import arccot, cot, wrap_to_pi


def sph_azimuth(beta1, lam1, beta2, lam2):
    # sphärischer Anfangsazimut (von Norden/meridian, im Bogenmaß)
    dlam = wrap_to_pi(lam2 - lam1)
    y = np.sin(dlam) * np.cos(beta2)
    x = np.cos(beta1) * np.sin(beta2) - np.sin(beta1) * np.cos(beta2) * np.cos(dlam)
    a = np.arctan2(y, x)  # (-pi, pi]
    if a < 0:
        a += 2 * np.pi
    return a

def BETA_LAMBDA(ell, beta, lamb):

    BETA = (ell.ay**2 * np.sin(beta)**2 + ell.b**2 * np.cos(beta)**2) / (ell.Ex**2 - ell.Ey**2 * np.sin(beta)**2)
    LAMBDA = (ell.ax**2 * np.sin(lamb)**2 + ell.ay**2 * np.cos(lamb)**2) / (ell.Ex**2 - ell.Ee**2 * np.cos(lamb)**2)

    # Erste Ableitungen von ΒETA und LAMBDA
    BETA_ = (ell.ax**2 * ell.Ey**2 * np.sin(2*beta)) / (ell.Ex**2 - ell.Ey**2 * np.sin(beta)**2)**2
    LAMBDA_ = - (ell.b**2 * ell.Ee**2 * np.sin(2*lamb)) / (ell.Ex**2 - ell.Ee**2 * np.cos(lamb)**2)**2

    # Zweite Ableitungen von ΒETA und LAMBDA
    BETA__ = ((2 * ell.ax**2 * ell.Ey**4 * np.sin(2*beta)**2) / (ell.Ex**2 - ell.Ey**2 * np.sin(beta)**2)**3) + ((2 * ell.ax**2 * ell.Ey**2 * np.cos(2*beta)) / (ell.Ex**2 - ell.Ey**2 * np.sin(beta)**2)**2)
    LAMBDA__ = (((2 * ell.b**2 * ell.Ee**4 * np.sin(2*lamb)**2) / (ell.Ex**2 - ell.Ee**2 * np.cos(lamb)**2)**3) -
                ((2 * ell.b**2 * ell.Ee**2 * np.sin(2*lamb)) / (ell.Ex**2 - ell.Ee**2 * np.cos(lamb)**2)**2))

    E = BETA * (ell.Ey ** 2 * np.cos(beta) ** 2 + ell.Ee ** 2 * np.sin(lamb) ** 2)
    F = 0
    G = LAMBDA * (ell.Ey ** 2 * np.cos(beta) ** 2 + ell.Ee ** 2 * np.sin(lamb) ** 2)

    # Erste Ableitungen von E und G
    E_beta = BETA_ * (ell.Ey**2 * np.cos(beta)**2 + ell.Ee**2 * np.sin(lamb)**2) - BETA * ell.Ey**2 * np.sin(2*beta)
    E_lamb = BETA * ell.Ee**2 * np.sin(2*lamb)

    G_beta = - LAMBDA * ell.Ey**2 * np.sin(2*beta)
    G_lamb = LAMBDA_ * (ell.Ey**2 * np.cos(beta)**2 + ell.Ee**2 * np.sin(lamb)**2) + LAMBDA * ell.Ee**2 * np.sin(2*lamb)

    # Zweite Ableitungen von E und G
    E_beta_beta = BETA__ * (ell.Ey**2 * np.cos(beta)**2 + ell.Ee**2 * np.sin(lamb)**2) - 2 * BETA_ * ell.Ey**2 * np.sin(2*beta) - 2 * BETA * ell.Ey**2 * np.cos(2*beta)
    E_beta_lamb = BETA_ * ell.Ee**2 * np.sin(2*lamb)
    E_lamb_lamb = 2 * BETA * ell.Ee**2 * np.cos(2*lamb)

    G_beta_beta = - 2 * LAMBDA * ell.Ey**2 * np.cos(2*beta)
    G_beta_lamb = - LAMBDA_ * ell.Ey**2 * np.sin(2*beta)
    G_lamb_lamb = LAMBDA__ * (ell.Ey**2 * np.cos(beta)**2 + ell.Ee**2 * np.sin(lamb)**2) + 2 * LAMBDA_ * ell.Ee**2 * np.sin(2*lamb) + 2 * LAMBDA * ell.Ee**2 * np.cos(2*lamb)

    return (BETA, LAMBDA, E, G,
            BETA_, LAMBDA_, BETA__, LAMBDA__,
            E_beta, E_lamb, G_beta, G_lamb,
            E_beta_beta, E_beta_lamb, E_lamb_lamb,
            G_beta_beta, G_beta_lamb, G_lamb_lamb)

def p_coef(beta, lamb):

    (BETA, LAMBDA, E, G,
     BETA_, LAMBDA_, BETA__, LAMBDA__,
     E_beta, E_lamb, G_beta, G_lamb,
     E_beta_beta, E_beta_lamb, E_lamb_lamb,
     G_beta_beta, G_beta_lamb, G_lamb_lamb) = BETA_LAMBDA(ell, beta, lamb)

    p_3 = - 0.5 * (E_lamb / G)
    p_2 = (G_beta / G) - 0.5 * (E_beta / E)
    p_1 = 0.5 * (G_lamb / G) - (E_lamb / E)
    p_0 = 0.5 * (G_beta / E)

    p_33 = - 0.5 * ((E_beta_lamb * G - E_lamb * G_beta) / (G ** 2))
    p_22 = ((G * G_beta_beta - G_beta * G_beta) / (G ** 2)) - 0.5 * ((E * E_beta_beta - E_beta * E_beta) / (E ** 2))
    p_11 = 0.5 * ((G * G_beta_lamb - G_beta * G_lamb) / (G ** 2)) - ((E * E_beta_lamb - E_beta * E_lamb) / (E ** 2))
    p_00 = 0.5 * ((E * G_beta_beta - E_beta * G_beta) / (E ** 2))

    return (BETA, LAMBDA, E, G,
            p_3, p_2, p_1, p_0,
            p_33, p_22, p_11, p_00)

def buildODElamb():
    def ODE(lamb, v):
        beta, beta_p, X3, X4 = v

        (BETA, LAMBDA, E, G,
         p_3, p_2, p_1, p_0,
         p_33, p_22, p_11, p_00) = p_coef(beta, lamb)

        dbeta = beta_p
        dbeta_p = p_3 * beta_p ** 3 + p_2 * beta_p ** 2 + p_1 * beta_p + p_0
        dX3 = X4
        dX4 = (p_33 * beta_p ** 3 + p_22 * beta_p ** 2 + p_11 * beta_p + p_00) * X3 + \
              (3 * p_3 * beta_p ** 2 + 2 * p_2 * beta_p + p_1) * X4
        return np.array([dbeta, dbeta_p, dX3, dX4])

    return ODE

def q_coef(beta, lamb):

    (BETA, LAMBDA, E, G,
     BETA_, LAMBDA_, BETA__, LAMBDA__,
     E_beta, E_lamb, G_beta, G_lamb,
     E_beta_beta, E_beta_lamb, E_lamb_lamb,
     G_beta_beta, G_beta_lamb, G_lamb_lamb) = BETA_LAMBDA(ell, beta, lamb)

    q_3 = - 0.5 * (G_beta / E)
    q_2 = (E_lamb / E) - 0.5 * (G_lamb / G)
    q_1 = 0.5 * (E_beta / E) - (G_beta / G)
    q_0 = 0.5 * (E_lamb / G)

    q_33 = - 0.5 * ((E * G_beta_lamb - E_lamb * G_lamb) / (E ** 2))
    q_22 = ((E * E_lamb_lamb - E_lamb * E_lamb) / (E ** 2)) - 0.5 * ((G * G_lamb_lamb - G_lamb * G_lamb) / (G ** 2))
    q_11 = 0.5 * ((E * E_beta_lamb - E_beta * E_lamb) / (E ** 2)) - ((G * G_beta_lamb - G_beta * G_lamb) / (G ** 2))
    q_00 = 0.5 * ((E_lamb_lamb * G - E_lamb * G_lamb) / (G ** 2))

    return (BETA, LAMBDA, E, G,
            q_3, q_2, q_1, q_0,
            q_33, q_22, q_11, q_00)

def buildODEbeta():
    def ODE(beta, v):
        lamb, lamb_p, Y3, Y4 = v

        (BETA, LAMBDA, E, G,
         q_3, q_2, q_1, q_0,
         q_33, q_22, q_11, q_00) = q_coef(beta, lamb)

        dlamb = lamb_p
        dlamb_p = q_3 * lamb_p ** 3 + q_2 * lamb_p ** 2 + q_1 * lamb_p + q_0
        dY3 = Y4
        dY4 = (q_33 * lamb_p ** 3 + q_22 * lamb_p ** 2 + q_11 * lamb_p + q_00) * Y3 + \
              (3 * q_3 * lamb_p ** 2 + 2 * q_2 * lamb_p + q_1) * Y4

        return np.array([dlamb, dlamb_p, dY3, dY4])

    return ODE

# Panou 2013
def gha2_num(ell: EllipsoidTriaxial, beta_1: float, lamb_1: float, beta_2: float, lamb_2: float,
             n: int = 16000, epsilon: float = 10**-12, iter_max: int = 30, all_points: bool = False
             ) -> Tuple[float, float, float] | Tuple[float, float, float, NDArray, NDArray]:
    """

    :param ell: triaxiales Ellipsoid
    :param beta_1: reduzierte ellipsoidische Breite Punkt 1
    :param lamb_1: elllipsoidische Länge Punkt 1
    :param beta_2: reduzierte ellipsoidische Breite Punkt 2
    :param lamb_2: elllipsoidische Länge Punkt 2
    :param n: Anzahl Schritte
    :param epsilon:
    :param iter_max: Maximale Anzhal Iterationen
    :param all_points:
    :return:
    """

    # h_x, h_y, h_e entsprechen E_x, E_y, E_e

    if lamb_1 != lamb_2:
        N = n
        dlamb = lamb_2 - lamb_1
        alpha0_sph = sph_azimuth(beta_1, lamb_1, beta_2, lamb_2)

        if abs(dlamb) < 1e-15:
            beta_0 = 0.0
        else:
            (_, _, E1, G1, *_) = BETA_LAMBDA(ell, beta_1, lamb_1)
            beta_0 = np.sqrt(G1 / E1) * cot(alpha0_sph)

        ode_lamb = buildODElamb()

        def solve_newton(beta_p0_init: float):
            beta_p0 = float(beta_p0_init)

            for _ in range(iter_max):
                startwerte = np.array([beta_1, beta_p0, 0.0, 1.0], dtype=float)
                lamb_list, states = rk.rk4(ode_lamb, lamb_1, startwerte, dlamb, N, False)

                beta_end, beta_p_end, X3_end, X4_end = states[-1]
                delta = beta_end - beta_2

                if abs(delta) < epsilon:
                    return True, beta_p0, lamb_list, states

                d_beta_end_d_beta0 = X3_end
                if abs(d_beta_end_d_beta0) < 1e-20:
                    return False, None, None, None

                step = delta / d_beta_end_d_beta0
                max_step = 0.5
                if abs(step) > max_step:
                    step = np.sign(step) * max_step

                beta_p0 = beta_p0 - step

            return False, None, None, None

        alpha0_sph = sph_azimuth(beta_1, lamb_1, beta_2, lamb_2)
        (_, _, E1, G1, *_) = BETA_LAMBDA(ell, beta_1, lamb_1)
        beta_p0_sph = np.sqrt(G1 / E1) * cot(alpha0_sph)

        guesses = [
            beta_p0_sph,
            0.5 * beta_p0_sph,
            2.0 * beta_p0_sph,
            -beta_p0_sph,
            -0.5 * beta_p0_sph,
        ]

        best = None

        for g in guesses:
            ok, beta_p0_sol, lamb_list_cand, states_cand = solve_newton(g)
            if not ok:
                continue

            beta_arr_c = np.array([st[0] for st in states_cand], dtype=float)
            beta_p_arr_c = np.array([st[1] for st in states_cand], dtype=float)
            lamb_arr_c = np.array(lamb_list_cand, dtype=float)

            integrand = np.zeros(N + 1)
            for i in range(N + 1):
                (_, _, Ei, Gi, *_) = BETA_LAMBDA(ell, beta_arr_c[i], lamb_arr_c[i])
                integrand[i] = np.sqrt(Ei * beta_p_arr_c[i] ** 2 + Gi)

            h = abs(dlamb) / N
            if N % 2 == 0:
                S = integrand[0] + integrand[-1] \
                    + 4.0 * np.sum(integrand[1:-1:2]) \
                    + 2.0 * np.sum(integrand[2:-1:2])
                s_cand = h / 3.0 * S
            else:
                s_cand = np.trapz(integrand, dx=h)

            if (best is None) or (s_cand < best[0]):
                best = (s_cand, beta_p0_sol, lamb_list_cand, states_cand)

        if best is None:
            raise RuntimeError("Keine Multi-Start-Variante konvergiert.")

        s_best, beta_0, lamb_list, werte = best

        beta_arr = np.zeros(N + 1)
        # lamb_arr = np.zeros(N + 1)
        lamb_arr = np.array(lamb_list)
        beta_p_arr = np.zeros(N + 1)

        for i, state in enumerate(werte):
            # lamb_arr[i] = state[0]
            # beta_arr[i] = state[1]
            # beta_p_arr[i] = state[2]
            beta_arr[i] = state[0]
            beta_p_arr[i] = state[1]

        (_, _, E1, G1,
         *_) = BETA_LAMBDA(ell, beta_arr[0], lamb_arr[0])
        (_, _, E2, G2,
         *_) = BETA_LAMBDA(ell, beta_arr[-1], lamb_arr[-1])

        alpha_1 = arccot(np.sqrt(E1 / G1) * beta_p_arr[0])
        alpha_2 = arccot(np.sqrt(E2 / G2) * beta_p_arr[-1])

        integrand = np.zeros(N + 1)
        for i in range(N + 1):
            (_, _, Ei, Gi,
             *_) = BETA_LAMBDA(ell, beta_arr[i], lamb_arr[i])
            integrand[i] = np.sqrt(Ei * beta_p_arr[i] ** 2 + Gi)

        h = abs(dlamb) / N
        if N % 2 == 0:
            S = integrand[0] + integrand[-1] \
                + 4.0 * np.sum(integrand[1:-1:2]) \
                + 2.0 * np.sum(integrand[2:-1:2])
            s = h / 3.0 * S
        else:
            s = np.trapz(integrand, dx=h)

        beta0 = beta_arr[0]
        lamb0 = lamb_arr[0]
        c = np.sqrt(
            (np.cos(beta0) ** 2 + (ell.Ee**2 / ell.Ex**2) * np.sin(beta0) ** 2) * np.sin(alpha_1) ** 2
            + (ell.Ee**2 / ell.Ex**2) * np.cos(lamb0) ** 2 * np.cos(alpha_1) ** 2
        )

        if all_points:
            return alpha_1, alpha_2, s, beta_arr, lamb_arr
        else:
            return alpha_1, alpha_2, s

    else:  # lamb_1 == lamb_2
        N = n
        dbeta = beta_2 - beta_1

        if abs(dbeta) < 1e-15:
            if all_points:
                return 0, 0, 0, np.array([]), np.array([])
            else:
                return 0, 0, 0

        lamb_0 = 0

        ode_beta = buildODEbeta()

        for i in range(iter_max):
            startwerte = [lamb_1, lamb_0, 0.0, 1.0]

            beta_list, werte = rk.rk4(ode_beta, beta_1, startwerte, dbeta, N, False)

            beta_end = beta_list[-1]
            lamb_end, lamb_p_end, Y3_end, Y4_end = werte[-1]

            d_lamb_end_d_lambda0 = Y3_end
            delta = lamb_end - lamb_2

            if abs(delta) < epsilon:
                break

            if abs(d_lamb_end_d_lambda0) < 1e-20:
                raise RuntimeError("Abbruch (Ableitung ~ 0).")

            max_step = 1.0
            step = delta / d_lamb_end_d_lambda0
            if abs(step) > max_step:
                step = np.sign(step) * max_step

            lamb_0 = lamb_0 - step

        beta_list, werte = rk.rk4(ode_beta, beta_1, np.array([lamb_1, lamb_0, 0.0, 1.0]), dbeta, N, False)

        # beta_arr = np.zeros(N + 1)
        beta_arr = np.array(beta_list)
        lamb_arr = np.zeros(N + 1)
        lambda_p_arr = np.zeros(N + 1)

        for i, state in enumerate(werte):
            # beta_arr[i] = state[0]
            # lamb_arr[i] = state[1]
            # lambda_p_arr[i] = state[2]
            lamb_arr[i] = state[0]
            lambda_p_arr[i] = state[1]

        # Azimute
        (BETA1, LAMBDA1, E1, G1,
         *_) = BETA_LAMBDA(ell, beta_arr[0], lamb_arr[0])
        (BETA2, LAMBDA2, E2, G2,
         *_) = BETA_LAMBDA(ell, beta_arr[-1], lamb_arr[-1])

        alpha_1 = (np.pi / 2.0) - arccot(np.sqrt(LAMBDA1 / BETA1) * lambda_p_arr[0])
        alpha_2 = (np.pi / 2.0) - arccot(np.sqrt(LAMBDA2 / BETA2) * lambda_p_arr[-1])

        integrand = np.zeros(N + 1)
        for i in range(N + 1):
            (_, _, Ei, Gi,
             *_) = BETA_LAMBDA(ell, beta_arr[i], lamb_arr[i])
            integrand[i] = np.sqrt(Ei + Gi * lambda_p_arr[i] ** 2)

        h = abs(dbeta) / N
        if N % 2 == 0:
            S = integrand[0] + integrand[-1] \
                + 4.0 * np.sum(integrand[1:-1:2]) \
                + 2.0 * np.sum(integrand[2:-1:2])
            s = h / 3.0 * S
        else:
            s = np.trapz(integrand, dx=h)

        if all_points:
            return alpha_1, alpha_2, s, beta_arr, lamb_arr
        else:
            return alpha_1, alpha_2, s


if __name__ == "__main__":
    # ell = EllipsoidTriaxial.init_name("Fiction")
    # # beta1 = np.deg2rad(75)
    # # lamb1 = np.deg2rad(-90)
    # # beta2 = np.deg2rad(75)
    # # lamb2 = np.deg2rad(66)
    # # a1, a2, s = gha2_num(ell, beta1, lamb1, beta2, lamb2)
    # # print(aus.gms("a1", a1, 4))
    # # print(aus.gms("a2", a2, 4))
    # # print(s)
    # cart1 = ell.para2cart(0, 0)
    # cart2 = ell.para2cart(0.4, 1.4)
    # beta1, lamb1 = ell.cart2ell(cart1)
    # beta2, lamb2 = ell.cart2ell(cart2)
    #
    # a1, a2, s = gha2_num(ell, beta1, lamb1, beta2, lamb2, n=5000)
    # print(s)

    # ell = EllipsoidTriaxial.init_name("BursaSima1980round")
    # diffs_panou = []
    # examples_panou = ne_panou.get_random_examples(4)
    # for example in examples_panou:
    #     beta0, lamb0, beta1, lamb1, _, alpha0, alpha1, s = example
    #     P0 = ell.ell2cart(beta0, lamb0)
    #     try:
    #         alpha0_num, alpha1_num, s_num = gha2_num(ell, beta0, lamb0, beta1, lamb1, n=4000, iter_max=10)
    #         diffs_panou.append(
    #             (wu.rad2deg(abs(alpha0 - alpha0_num)), wu.rad2deg(abs(alpha1 - alpha1_num)), abs(s - s_num)))
    #     except:
    #         print(f"Fehler für {beta0}, {lamb0}, {beta1}, {lamb1}")
    # diffs_panou = np.array(diffs_panou)
    # print(diffs_panou)
    #
    # ell = EllipsoidTriaxial.init_name("KarneyTest2024")
    # diffs_karney = []
    # # examples_karney = ne_karney.get_examples((30500, 40500))
    # examples_karney = ne_karney.get_random_examples(2)
    # for example in examples_karney:
    #     beta0, lamb0, alpha0, beta1, lamb1, alpha1, s = example
    #
    #     try:
    #         alpha0_num, alpha1_num, s_num = gha2_num(ell, beta0, lamb0, beta1, lamb1, n=4000, iter_max=10)
    #         diffs_karney.append((wu.rad2deg(abs(alpha0-alpha0_num)), wu.rad2deg(abs(alpha1-alpha1_num)), abs(s-s_num)))
    #     except:
    #         print(f"Fehler für {beta0}, {lamb0}, {beta1}, {lamb1}")
    # diffs_karney = np.array(diffs_karney)
    # print(diffs_karney)

    pass