Masterprojekt/GHA_triaxial/ES_gha2.py

import numpy as np
from numpy import arccos
from Hansen_ES_CMA import escma
from ellipsoide import EllipsoidTriaxial
from numpy.typing import NDArray
import plotly.graph_objects as go
from GHA_triaxial.panou_2013_2GHA_num import gha2_num
from utils import sigma2alpha

ell_ES: EllipsoidTriaxial = None
P_start: NDArray = None
P_prev: NDArray = None
P_next: NDArray = None
P_end: NDArray = None
stepLen: float = None

def Bogenlaenge(P1: NDArray, P2: NDArray) -> float:
    """
    Berechnung der mittleren Bogenlänge zwischen zwei kartesischen Punkten
    :param P1: kartesische Koordinate Punkt 1
    :param P2: kartesische Koordinate Punkt 2
    :return: Bogenlänge s
    """
    R1 = np.linalg.norm(P1)
    R2 = np.linalg.norm(P2)
    R = 0.5 * (R1 + R2)
    theta = arccos(P1 @ P2 / (R1 * R2))
    s = float(R * theta)
    return s


def gha2_ES(ell: EllipsoidTriaxial, P0: NDArray, Pk: NDArray, stepLenTarget: float = None, sigmaStep: float = 1e-5, stopeval: int = 1000, maxSteps: int = 10000, all_points: bool = False):
    """
    Berechnen der 2. GHA mithilfe der CMA-ES.
    Die CMA-ES optimiert sukzessive einzelne Punkte, die einen definierten Abstand (stepLenTarget) zum vorherigen und den kürzesten
    Abstand zum Zielpunkt aufweisen. Der Abbruch der Optimierung erfolgt, wenn die Restdistanz zwischen vorherigen und Zielpunkt die
    'stepLenTarget' unterschreitet. Die Distanzen zwischen den einzelnen Punkten werden als mittlere Bogenlänge berechnet und aufaddiert.
    :param ell: Parameter des triaxialen Ellipsoids
    :param P0: Startpunkt
    :param Pk: Zielpunkt
    :param stepLenTarget: Abstand zwischen vorherigen und optimierten Punkt
    :param sigmaStep: Sigma Startwert für die CMA-ES (Suchraum um den zu optimierenden Punkt)
    :param stopeval: maximale Iterationen
    :param maxSteps: maximale Anzahl der zu optimierenden Punkte
    :param all_points: Ergebnisliste mit allen Punkte, die wahlweise mit ausgegeben werden kann
    :return: Richtungswinkel des Start- und Zielpunktes, totalLen
    """
    global ell_ES, P_start, P_prev, P_next, P_end, stepLen
    ell_ES = ell
    P_start = P0
    P_end = Pk
    if stepLenTarget is None:
        R0 = (ell.ax + ell.ay + ell.b) / 3
        stepLenTarget = R0 * 1 / 600
    stepLen = stepLenTarget

    P_all = [P_start]
    totalLen = 0

    P_prev = P_start

    for i in range(1, maxSteps):

        d_remain = Bogenlaenge(P_prev, P_end)

        # Abbruch: letzter "Rest-Schritt" ist < 10 km -> Ziel anhängen
        if d_remain <= stepLen:
            P_all.append(P_end)
            totalLen += d_remain
            print(f'[Punkt {i}] Stop: Restdistanz {round(d_remain, 3)} m <= {round(stepLen, 3)} m, Ziel angehängt.')
            break

        # Globals für Fitness: aktueller Start(P0) und Ziel(Pk)
        # P0 = P_prev;
        # Pk = P_end;

        # Näherung für die ES ermitteln
        # % d = P_end - P_prev;
        # % L = Bogenlaenge(P_prev, P_end);
        # % t = min(stepLen / L, 1.0);
        # % Q = P_prev + t * d;
        # % q = [Q(1) / a;
        # Q(2) / b;
        # Q(3) / c];
        # % nq = norm(q);
        # % q = q / nq;

        # %q
        # entspricht[cos(u)
        # cos(v);
        # cos(u)
        # sin(v);
        # sin(u)] auf
        # Einheitskugel
        #arg_u = max(-1, min(1, q(3)));
        #
        # %Quadrantenabfrage
        #u0 = mod(asin(arg_u) + pi / 2, pi) - pi / 2;
        #v0 = atan2(q(2), q(1));
        #xmean_init = [u0;v0];
        xmean_init = ell.point_onto_ellipsoid(P_prev + stepLen * (P_end - P_prev) / np.linalg.norm(P_end - P_prev))

        # [~, ~, aux] = geoLength(xmean_init);
        # print('Startguess: d_step=%.3f (soll %.3f), d_to_target=%.3f\n', aux(1), stepLen, aux(2));

        print(f'[Punkt {i}] Optimiere nächsten Punkt: Restdistanz = {round(d_remain, 3)} m')
        xmean_init = np.array(ell_ES.cart2para(xmean_init))

        u, v = escma(geoLength, N=2, xmean=xmean_init, sigma=sigmaStep, stopfitness=-np.inf, stopeval=stopeval)

        P_next = ell.para2cart(u, v)

        d_step = Bogenlaenge(P_prev, P_next)
        d_new = Bogenlaenge(P_next, P_end)
        d_old = Bogenlaenge(P_prev, P_end)

        print(f'[Punkt {i}] Ergebnis: Schritt = {round(d_step, 3)} m (soll {round(stepLen, 3)}), Rest neu = {round(d_new, 3)} m')

        # Sicherheitscheck: wenn wir nicht näher kommen, abbrechen
        if d_new >= d_old:
            print(f'Punkt {i}: Neuer Punkt ist nicht naeher am Ziel ({d_old} -> {d_new}). Abbruch.')
            P_all.append(P_end)
            totalLen += Bogenlaenge(P_prev, P_end)
            break

        P_all.append(P_next)
        totalLen += d_step
        P_prev = P_next

    print('Maximale Schrittanzahl erreicht.')
    P_all.append(P_end)
    totalLen += Bogenlaenge(P_prev, P_end)

    p0i = ell.point_onto_ellipsoid(P0 + 10 * (P_all[1] - P0) / np.linalg.norm(P_all[1] - P0))
    sigma0 = (p0i - P0) / np.linalg.norm(p0i - P0)
    alpha0 = sigma2alpha(ell_ES, sigma0, P0)

    p1i = ell.point_onto_ellipsoid(Pk - 10 * (Pk - P_all[-2]) / np.linalg.norm(Pk - P_all[-2]))
    sigma1 = (Pk - p1i) / np.linalg.norm(Pk - p1i)
    alpha1 = sigma2alpha(ell_ES, sigma1, Pk)

    if all_points:
        return alpha0, alpha1, totalLen, np.array(P_all)
    else:
        return alpha0, alpha1, totalLen


def geoLength(P_candidate: Tuple) -> float:
    """
    Berechung der Fitness eines Kandidaten anhand der Strecken
    :param P_candidate: Kandidat in parametrischen Koordinaten
    :return: Fitness-Wert
    """
    # P_candidate = [u;v] des naechsten Punktes.
    # Ziel: Distanz zum Ziel  minimieren, aber Schrittlaenge ~ stepLenTarget erzwingen.
    u, v = P_candidate

    global ell_ES, P_start, P_prev, P_next, P_end, stepLen
    # Punkt auf Ellipsoid
    P_next = ell_ES.para2cart(u, v)

    # Distanzen
    d_step = Bogenlaenge(P_prev, P_next)
    d_to_target = Bogenlaenge(P_end, P_next)
    d_prev_to_target = Bogenlaenge(P_end, P_prev)

    # Penalties(dimensionslos)
    pen_step = ((d_step - stepLen) / stepLen)**2

    # falls Punkt "weg" vom Ziel geht, extra bestrafen
    pen_away = max(0.0, (d_to_target - d_prev_to_target) / stepLen)**2

    # Gewichtungen
    alpha = 1e2
    # Schrittlaenge sehr hart
    gamma = 1e2  # "nicht weg vom Ziel" hart

    f = d_to_target * (1 + alpha * pen_step + gamma * pen_away)

    # Für Debug / Extraktion
    # aux = [d_step, d_to_target]
    return f  # , P_candidate, aux

def show_points(points: NDArray, pointsES: NDArray, p0: NDArray, p1: NDArray):
    """
    Anzeigen der Punkte
    :param points: wahre Punkte der Linie
    :param pointsES: Punkte der Linie aus ES
    :param p0: wahrer Startpunkt
    :param p1: wahrer Endpunkt
    """
    fig = go.Figure()

    fig.add_scatter3d(x=pointsES[:, 0], y=pointsES[:, 1], z=pointsES[:, 2],
                      mode='lines', line=dict(color="green", width=3), name="Numerisch")
    fig.add_scatter3d(x=points[:, 0], y=points[:, 1], z=points[:, 2],
                      mode='lines', line=dict(color="red", width=3), name="ES")
    fig.add_scatter3d(x=[p0[0]], y=[p0[1]], z=[p0[2]],
                      mode='markers', marker=dict(color="black"), name="P0")
    fig.add_scatter3d(x=[p1[0]], y=[p1[1]], z=[p1[2]],
                      mode='markers', marker=dict(color="black"), name="P1")

    fig.update_layout(
        scene=dict(xaxis_title='X [km]',
                   yaxis_title='Y [km]',
                   zaxis_title='Z [km]',
                   aspectmode='data'))

    fig.show()


if __name__ == '__main__':
    ell = EllipsoidTriaxial.init_name("Fiction")

    beta0, lamb0 = (0.2, 0.1)
    P0 = ell.ell2cart(beta0, lamb0)
    beta1, lamb1 = (0.7, 0.3)
    P1 = ell.ell2cart(beta1, lamb1)

    alpha0, alpha1, s_num, betas, lambs = gha2_num(ell, beta0, lamb0, beta1, lamb1, n=10000, all_points=True)
    points_num = []
    for beta, lamb in zip(betas, lambs):
        points_num.append(ell.ell2cart(beta, lamb))
    points_num = np.array(points_num)

    alpha0, alpha1, s, points = gha2_ES(ell, P0, P1, all_points=True, sigmaStep=1e-5)
    print(s - s_num)
    show_points(points, points_num, P0, P1)