Masterprojekt/GHA_triaxial/panou_2013_2GHA_num.py

import numpy as np
from ellipsoide import EllipsoidTriaxial
import runge_kutta as rk
import GHA_triaxial.numeric_examples_karney as ne_karney
import GHA_triaxial.numeric_examples_panou as ne_panou
import winkelumrechnungen as wu

# Panou 2013
def gha2_num(ell: EllipsoidTriaxial, beta_1, lamb_1, beta_2, lamb_2, n=16000, epsilon=10**-12, iter_max=30):
    """

    :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
    :return:
    """

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

    def arccot(x):
        return np.arctan2(1.0, x)


    def BETA_LAMBDA(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(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 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(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)

    if lamb_1 != lamb_2:
        # def functions():
        #     def f_beta(lamb, beta, beta_p, X3, X4):
        #         return beta_p
        #
        #     def f_beta_p(lamb, beta, beta_p, X3, X4):
        #         (BETA, LAMBDA, E, G,
        #          p_3, p_2, p_1, p_0,
        #          p_33, p_22, p_11, p_00) = p_coef(beta, lamb)
        #         return p_3 * beta_p ** 3 + p_2 * beta_p ** 2 + p_1 * beta_p + p_0
        #
        #     def f_X3(lamb, beta, beta_p, X3, X4):
        #         return X4
        #
        #     def f_X4(lamb, beta, beta_p, X3, X4):
        #         (BETA, LAMBDA, E, G,
        #          p_3, p_2, p_1, p_0,
        #          p_33, p_22, p_11, p_00) = p_coef(beta, lamb)
        #         return (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 [f_beta, f_beta_p, f_X3, f_X4]

        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

        N = n

        dlamb = lamb_2 - lamb_1

        if abs(dlamb) < 1e-15:
            beta_0 = 0.0
        else:
            beta_0 = (beta_2 - beta_1) / (lamb_2 - lamb_1)

        converged = False
        iterations = 0

        # funcs = functions()
        ode_lamb = buildODElamb()

        for i in range(iter_max):
            iterations = i + 1

            # startwerte = [lamb_1, beta_1, beta_0, 0.0, 1.0]
            startwerte = np.array([beta_1, beta_0, 0.0, 1.0])

            # werte = rk.verfahren(funcs, startwerte, dlamb, N)
            lamb_list, werte = rk.rk4(ode_lamb, lamb_1, startwerte, dlamb, N, False)
            # lamb_end, beta_end, beta_p_end, X3_end, X4_end = werte[-1]
            lamb_end = lamb_list[-1]
            beta_end, beta_p_end, X3_end, X4_end = werte[-1]

            d_beta_end_d_beta0 = X3_end
            delta = beta_end - beta_2

            if abs(delta) < epsilon:
                converged = True
                break

            if abs(d_beta_end_d_beta0) < 1e-20:
                raise RuntimeError("Abbruch.")

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

        if not converged:
            raise RuntimeError("konvergiert nicht.")

        # Z
        # werte = rk.verfahren(funcs, [lamb_1, beta_1, beta_0, 0.0, 1.0], dlamb, N, False)
        lamb_list, werte = rk.rk4(ode_lamb, lamb_1, np.array([beta_1, beta_0, 0.0, 1.0]), dlamb, N, False)

        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(beta_arr[0], lamb_arr[0])
        (_, _, E2, G2,
         *_) = BETA_LAMBDA(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(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
        )

        return alpha_1, alpha_2, s

    if lamb_1 == lamb_2:

        N = n
        dbeta = beta_2 - beta_1

        if abs(dbeta) < 10**-15:
            return 0, 0, 0

        lamb_0 = 0

        converged = False
        iterations = 0

        # def functions_beta():
        #     def g_lamb(beta, lamb, lamb_p, Y3, Y4):
        #         return lamb_p
        #
        #     def g_lamb_p(beta, lamb, lamb_p, Y3, Y4):
        #         (BETA, LAMBDA, E, G,
        #          q_3, q_2, q_1, q_0,
        #          q_33, q_22, q_11, q_00) = q_coef(beta, lamb)
        #         return q_3 * lamb_p ** 3 + q_2 * lamb_p ** 2 + q_1 * lamb_p + q_0
        #
        #     def g_Y3(beta, lamb, lamb_p, Y3, Y4):
        #         return Y4
        #
        #     def g_Y4(beta, lamb, lamb_p, Y3, Y4):
        #         (BETA, LAMBDA, E, G,
        #          q_3, q_2, q_1, q_0,
        #          q_33, q_22, q_11, q_00) = q_coef(beta, lamb)
        #         return (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 [g_lamb, g_lamb_p, g_Y3, g_Y4]

        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

        # funcs_beta = functions_beta()
        ode_beta = buildODEbeta()

        for i in range(iter_max):
            iterations = i + 1

            startwerte = [lamb_1, lamb_0, 0.0, 1.0]

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

            beta_end = beta_list[-1]
            # beta_end, lamb_end, lamb_p_end, Y3_end, Y4_end = werte[-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:
                converged = True
                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

        # werte = rk.verfahren(funcs_beta, [beta_1, lamb_1, lamb_0, 0.0, 1.0], dbeta, N, False)
        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(beta_arr[0], lamb_arr[0])
        (BETA2, LAMBDA2, E2, G2,
         *_) = BETA_LAMBDA(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(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)

        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