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)