Masterprojekt/GHA_triaxial/utils.py

from typing import Tuple

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

from ellipsoide import EllipsoidTriaxial


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 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-12:
        raise Exception("Alpha Umrechnung fehlgeschlagen")

    return beta, lamb, 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-9:
        raise Exception("Alpha Umrechnung fehlgeschlagen")

    return u, v, 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
    """
    x, y, z = point
    n = ell.func_n(point)

    beta, lamb = ell.cart2ell(point)
    B = ell.Ex ** 2 * cos(beta) ** 2 + ell.Ee ** 2 * sin(beta) ** 2
    L = ell.Ex ** 2 - ell.Ee ** 2 * cos(lamb) ** 2

    c1 = x ** 2 + y ** 2 + z ** 2 - (ell.ax ** 2 + ell.ay ** 2 + ell.b ** 2)
    c0 = (ell.ax ** 2 * ell.ay ** 2 + ell.ax ** 2 * ell.b ** 2 + ell.ay ** 2 * ell.b ** 2 -
          (ell.ay ** 2 + ell.b ** 2) * x ** 2 - (ell.ax ** 2 + ell.b ** 2) * y ** 2 - (
                  ell.ax ** 2 + ell.ay ** 2) * z ** 2)
    t2 = (-c1 + sqrt(c1 ** 2 - 4 * c0)) / 2

    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]])

    t1 = np.dot(n, q)
    t2 = np.dot(n, p)
    t3 = np.dot(p, q)
    if not (t1 < 1e-10 or t1 > 1-1e-10) and not (t2 < 1e-10 or t2 > 1-1e-10) and not (t3 < 1e-10 or t3 > 1-1e-10):
        raise Exception("Fehler in den normierten Vektoren")

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

    return p, q