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, P2):
    """
    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 = R * theta
    return s


def gha2_ES(ell: EllipsoidTriaxial, P0: NDArray, Pk: NDArray, stepLenTarget: float = None, sigmaStep: float = 1e-7, 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 == 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.cartonell(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,2, xmean_init, sigmaStep, -np.inf, 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.cartonell(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.cartonell(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):
    # 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, (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, pointsNum:NDArray, p0: NDArray, p1: NDArray):
    fig = go.Figure()

    fig.add_scatter3d(x=pointsNum[:, 0], y=pointsNum[:, 1], z=pointsNum[:, 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, betas, lambs = gha2_num(ell, beta0, lamb0, beta1, lamb1, n=5000, 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)
    show_points(points, points_num, P0, P1)