Masterprojekt/GHA_triaxial/utils.py

from __future__ import annotations

from typing import Tuple

import numpy as np
from numpy import arctan2, cos, sin, sqrt
from numpy.typing import NDArray

from ellipsoid_triaxial import EllipsoidTriaxial
from utils_angle import wrap_0_2pi


def sigma2alpha(ell: EllipsoidTriaxial, sigma: NDArray, point: NDArray) -> float:
    """
    Berechnung des Azimuts an einem Punkt anhand der Ableitung zu den kartesischen Koordinaten
    :param ell: Ellipsoid
    :param sigma: Ableitungsvektor ver kartesischen Koordinaten
    :param point: Punkt
    :return: Azimuts
    """
    p, q = pq_ell(ell, point)
    P = float(p @ sigma)
    Q = float(q @ sigma)

    alpha = arctan2(P, Q)
    return wrap_0_2pi(alpha)


def alpha_para2ell(ell: EllipsoidTriaxial, u: float, v: float, alpha_para: float) -> Tuple[float, float, float]:
    """
    Umrechnung des Azimuts bezogen auf parametrische Koordinaten zu ellipsoidischen
    :param ell: Ellipsoid
    :param u: parametrische Breite
    :param v: parametrische Länge
    :param alpha_para: Azimut bezogen auf parametrische Koordinaten
    :return: Azimut bezogen auf ellipsoidische Koordinaten
    """
    point = ell.para2cart(u, v)
    beta, lamb = ell.para2ell(u, v)

    p_para, q_para = pq_para(ell, point)
    sigma_para = p_para * sin(alpha_para) + q_para * cos(alpha_para)

    p_ell, q_ell = pq_ell(ell, point)
    alpha_ell = arctan2(p_ell @ sigma_para, q_ell @ sigma_para)
    sigma_ell = p_ell * sin(alpha_ell) + q_ell * cos(alpha_ell)

    if np.linalg.norm(sigma_para - sigma_ell) > 1e-7:
        raise Exception("alpha_para2ell: Differenz in den Richtungsableitungen")

    return beta, lamb, wrap_0_2pi(alpha_ell)


def alpha_ell2para(ell: EllipsoidTriaxial, beta: float, lamb: float, alpha_ell: float) -> Tuple[float, float, float]:
    """
    Umrechnung des Azimuts bezogen auf ellipsoidische Koordinaten zu parametrischen
    :param ell: Ellipsoid
    :param beta: ellipsoidische Breite
    :param lamb: ellipsoidische Länge
    :param alpha_ell: Azimut bezogen auf ellipsoidische Koordinaten
    :return: Azimut bezogen auf parametrische Koordinaten
    """
    point = ell.ell2cart(beta, lamb)
    u, v = ell.ell2para(beta, lamb)

    p_ell, q_ell = pq_ell(ell, point)
    sigma_ell = p_ell * sin(alpha_ell) + q_ell * cos(alpha_ell)

    p_para, q_para = pq_para(ell, point)
    alpha_para = arctan2(p_para @ sigma_ell, q_para @ sigma_ell)
    sigma_para = p_para * sin(alpha_para) + q_para * cos(alpha_para)

    if np.linalg.norm(sigma_para - sigma_ell) > 1e-7:
        raise Exception("alpha_ell2para: Differenz in den Richtungsableitungen")

    return u, v, wrap_0_2pi(alpha_para)


def func_sigma_ell(ell: EllipsoidTriaxial, point: NDArray, alpha_ell: float) -> NDArray:
    """
    Berechnung der Richtungsableitungen in kartesischen Koordinaten aus ellipsoidischem Azimut
    Panou (2019) [6]
    :param ell: Ellipsoid
    :param point: Punkt in kartesischen Koordinaten
    :param alpha_ell: ellipsoidischer Azimut
    :return: Richtungsableitungen in kartesischen Koordinaten
    """
    p, q = pq_ell(ell, point)
    sigma = p * sin(alpha_ell) + q * cos(alpha_ell)
    return sigma


def func_sigma_para(ell: EllipsoidTriaxial, point: NDArray, alpha_para: float) -> NDArray:
    """
    Berechnung der Richtungsableitungen in kartesischen Koordinaten aus parametischem Azimut
    Panou, Korakitis (2019) [6]
    :param ell: Ellipsoid
    :param point: Punkt in kartesischen Koordinaten
    :param alpha_para: parametrischer Azimut
    :return: Richtungsableitungen in kartesischen Koordinaten
    """
    p, q = pq_para(ell, point)
    sigma = p * sin(alpha_para) + q * cos(alpha_para)
    return sigma


def louville_constant(ell: EllipsoidTriaxial, p0: NDArray, alpha_ell: float) -> float:
    """
    Berechnung der Louville Konstanten
    Panou, Korakitis (2019) [6]
    :param ell: Ellipsoid
    :param p0: Punkt in kartesischen Koordinaten
    :param alpha_ell: ellipsoidischer Azimut
    :return:
    """
    beta, lamb = ell.cart2ell(p0)
    l = ell.Ey**2 * cos(beta)**2 * sin(alpha_ell)**2 - ell.Ee**2 * sin(lamb)**2 * cos(alpha_ell)**2
    return l


def pq_ell(ell: EllipsoidTriaxial, point: NDArray) -> Tuple[NDArray, NDArray]:
    """
    Berechnung von p (Tangente entlang konstantem beta) und q (Tangente entlang konstantem lambda)
    Panou, Korakitits (2019) [5f.]
    :param ell: Ellipsoid
    :param point: Punkt
    :return: p und q
    """
    n = ell.func_n(point)

    beta, lamb = ell.cart2ell(point)
    if abs(cos(beta)) < 1e-15 and abs(np.sin(lamb)) < 1e-15:
        if beta > 0:
            p = np.array([0, -1, 0])
        else:
            p = np.array([0, 1, 0])
    else:
        B = ell.Ex ** 2 * cos(beta) ** 2 + ell.Ee ** 2 * sin(beta) ** 2
        L = ell.Ex ** 2 - ell.Ee ** 2 * cos(lamb) ** 2

        _, t2 = ell.func_t12(point)

        F = ell.Ey ** 2 * cos(beta) ** 2 + ell.Ee ** 2 * sin(lamb) ** 2
        p1 = -sqrt(L / (F * t2)) * ell.ax / ell.Ex * sqrt(B) * sin(lamb)
        p2 = sqrt(L / (F * t2)) * ell.ay * cos(beta) * cos(lamb)
        p3 = 1 / sqrt(F * t2) * (ell.b * ell.Ee ** 2) / (2 * ell.Ex) * sin(beta) * sin(2 * lamb)
        p = np.array([p1, p2, p3])
        p = p / np.linalg.norm(p)
    q = np.array([n[1] * p[2] - n[2] * p[1],
                  n[2] * p[0] - n[0] * p[2],
                  n[0] * p[1] - n[1] * p[0]])
    q = q / np.linalg.norm(q)

    return p, q


def pq_para(ell: EllipsoidTriaxial, point: NDArray) -> Tuple[NDArray, NDArray]:
    """
    Berechnung von p (Tangente entlang konstantem u) und q (Tangente entlang konstantem v)
    Panou, Korakitits (2020)
    :param ell: Ellipsoid
    :param point: Punkt
    :return: p und q
    """
    n = ell.func_n(point)
    u, v = ell.cart2para(point)

    # 41-47
    G = sqrt(1 - ell.ex ** 2 * cos(u) ** 2 - ell.ee ** 2 * sin(u) ** 2 * sin(v) ** 2)
    q = np.array([-1 / G * sin(u) * cos(v),
                  -1 / G * sqrt(1 - ell.ee ** 2) * sin(u) * sin(v),
                  1 / G * sqrt(1 - ell.ex ** 2) * cos(u)])
    p = np.array([q[1] * n[2] - q[2] * n[1],
                  q[2] * n[0] - q[0] * n[2],
                  q[0] * n[1] - q[1] * n[0]])

    p = p / np.linalg.norm(p)
    q = q / np.linalg.norm(q)

    return p, q


def jacobi_konstante(beta: float, omega: float, alpha: float, ell: EllipsoidTriaxial) -> float:
    """
    Jacobi-Konstante nach Karney (2025), Gl. (14)
    :param beta: Beta Koordinate
    :param omega: Omega Koordinate
    :param alpha: Azimut alpha
    :param ell: Ellipsoid
    :return: Jacobi-Konstante
    """
    gamma_jacobi = float((ell.k ** 2) * (np.cos(beta) ** 2) * (np.sin(alpha) ** 2) - (ell.k_ ** 2) * (np.sin(omega) ** 2) * (np.cos(alpha) ** 2))
    return gamma_jacobi


if __name__ == "__main__":
    ell = EllipsoidTriaxial.init_name("KarneyTest2024")
    alpha_para = 0
    u, v = ell.ell2para(np.pi/2, 0)
    alpha_ell = alpha_para2ell(ell, u, v, alpha_para)
    pass