Merge remote-tracking branch 'refs/remotes/origin/main2'

# Conflicts:
#	GHA_triaxial/ES_gha2.py

GHA_triaxial/ES_gha2.py

@@ -8,179 +8,134 @@ from GHA_triaxial.panou_2013_2GHA_num import gha2_num
 from utils import sigma2alpha
 
 ell_ES: EllipsoidTriaxial = None
-P_start: NDArray = None
+P_left: NDArray = None
-P_prev: NDArray = None
+P_right: NDArray = None
-P_next: NDArray = None
-P_end: NDArray = None
-stepLen: float = None
 
-def Bogenlaenge(P1: NDArray, P2: NDArray) -> float:
+
+def Sehne(P1: NDArray, P2: NDArray) -> float:
     """
-    Berechnung der mittleren Bogenlänge zwischen zwei kartesischen Punkten
+    Berechnung der 3D-Distanz 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)
+    R12 = P2-P1
-    R2 = np.linalg.norm(P2)
+    s = np.linalg.norm(R12)
-    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):
+def midpoint_fitness(x: tuple) -> float:
+    """
+    Fitness für einen Mittelpunkt P_middle zwischen P_left und P_right auf dem triaxialen Ellipsoid:
+    - Minimiert d(P_left, P_middle) + d(P_middle, P_right)
+    - Erzwingt d(P_left,P_middle) ≈ d(P_middle,P_right)  (echter Mittelpunkt im Sinne der Polygonkette)
+    :param x: enthält die Startwerte von u und v
+    :return: Fitnesswert (f)
+    """
+    global ell_ES, P_left, P_right
+
+    u, v = x
+    P_middle = ell_ES.para2cart(u, v)
+    d1 = Sehne(P_left, P_middle)
+    d2 = Sehne(P_middle, P_right)
+    base = d1 + d2
+
+    # midpoint penalty (dimensionslos)
+    # relative Differenz, skaliert stabil über verschiedene Segmentlängen
+    denom = max(base, 1e-9)
+    pen_equal = ((d1 - d2) / denom) ** 2
+    w_equal = 10.0
+
+    f = base + denom * w_equal * pen_equal
+    return f
+
+
+def gha2_ES(ell: EllipsoidTriaxial, P0: NDArray, Pk: NDArray, maxSegLen: float = None, stopeval: int = 2000, maxIter: 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
+    Die CMA-ES optimiert sukzessive den Mittelpunkt zwischen Start- und Zielpunkt. Der Abbruch der Berechnung erfolgt, wenn alle Segmentlängen <= maxSegLen sind.
-    Abstand zum Zielpunkt aufweisen. Der Abbruch der Optimierung erfolgt, wenn die Restdistanz zwischen vorherigen und Zielpunkt die
+    Die Distanzen zwischen den einzelnen Punkten werden als direkte 3D-Distanzen berechnet und aufaddiert.
-    '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 maxSegLen: maximale Segmentlänge
-    :param sigmaStep: Sigma Startwert für die CMA-ES (Suchraum um den zu optimierenden Punkt)
+    :param stopeval: maximale Durchläufe der CMA-ES
-    :param stopeval: maximale Iterationen
+    :param maxIter: maximale Durchläufe der Mittelpunktsgenerierung
-    :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
+    :return: Richtungswinkel des Start- und Zielpunktes und Gesamtlänge
     """
-    global ell_ES, P_start, P_prev, P_next, P_end, stepLen
+    global ell_ES
     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
+    if maxSegLen is None:
-    stepLen = stepLenTarget
+        maxSegLen = R0 * 1 / (637.4) # 10km Segment bei mittleren Erdradius
 
-    P_all = [P_start]
+    sigma_uv_nom = 1e-3 * (maxSegLen / R0)  # ~1e-5
-    totalLen = 0
 
-    P_prev = P_start
+    points: list[NDArray] = [P0, Pk]
+    startIter = 0
+    level = 0
 
-    for i in range(1, maxSteps):
+    while True:
-
+        seg_lens = [Sehne(points[i], points[i+1]) for i in range(len(points)-1)]
-        d_remain = Bogenlaenge(P_prev, P_end)
+        max_len = max(seg_lens)
-
+        if max_len <= maxSegLen:
-        # 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)
+        level += 1
-        # P0 = P_prev;
+        new_points: list[NDArray] = [points[0]]
-        # Pk = P_end;
 
-        # Näherung für die ES ermitteln
+        for i in range(len(points) - 1):
-        # % d = P_end - P_prev;
+            A = points[i]
-        # % L = Bogenlaenge(P_prev, P_end);
+            B = points[i+1]
-        # % t = min(stepLen / L, 1.0);
+            dAB = Sehne(A, B)
-        # % Q = P_prev + t * d;
+            print(dAB)
-        # % q = [Q(1) / a;
-        # Q(2) / b;
-        # Q(3) / c];
-        # % nq = norm(q);
-        # % q = q / nq;
 
-        # %q
+            if dAB > maxSegLen:
-        # entspricht[cos(u)
+                global P_left, P_right
-        # cos(v);
+                P_left, P_right = A, B
-        # cos(u)
+                Au, Av = ell_ES.cart2para(A)
-        # sin(v);
+                Bu, Bv = ell_ES.cart2para(B)
-        # sin(u)] auf
+                u0 = (Au + Bu) / 2
-        # Einheitskugel
+                v0 = Av + 0.5 * np.arctan2(np.sin(Bv - Av), np.cos(Bv - Av))
-        #arg_u = max(-1, min(1, q(3)));
+                xmean = [u0, v0]
-        #
-        # %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);
+                sigmaStep = sigma_uv_nom * (Sehne(A, B) / maxSegLen)
-        # 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')
+                u, v = escma(midpoint_fitness, N=2, xmean=xmean, sigma=sigmaStep, stopfitness=-np.inf,
-        xmean_init = np.array(ell_ES.cart2para(xmean_init))
+                             stopeval=stopeval)
-
-        u, v = escma(geoLength, N=2, xmean=xmean_init, sigma=sigmaStep, stopfitness=-np.inf, stopeval=stopeval)
 
                 P_next = ell.para2cart(u, v)
+                new_points.append(P_next)
+                startIter += 1
+                if startIter > maxIter:
+                    raise RuntimeError("Abbruch: maximale Iterationen überschritten.")
 
-        d_step = Bogenlaenge(P_prev, P_next)
+            new_points.append(B)
-        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')
+        points = new_points
+        print(f"[Level {level}] Punkte: {len(points)} | max Segment: {max_len:.3f} m")
 
-        # Sicherheitscheck: wenn wir nicht näher kommen, abbrechen
+    P_all = np.vstack(points)
-        if d_new >= d_old:
+    totalLen = float(np.sum(np.linalg.norm(P_all[1:] - P_all[:-1], axis=1)))
-            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)
+    if len(points) >= 3:
-        totalLen += d_step
+        p0i = ell_ES.point_onto_ellipsoid(P0 + 10.0 * (points[1] - P0) / np.linalg.norm(points[1] - P0))
-        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 + stepLenTarget/1000 * (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 - stepLenTarget/1000 * (Pk - P_all[-2]) / np.linalg.norm(Pk - P_all[-2]))
+        p1i = ell_ES.point_onto_ellipsoid(Pk - 10.0 * (Pk - points[-2]) / np.linalg.norm(Pk - points[-2]))
         sigma1 = (Pk - p1i) / np.linalg.norm(Pk - p1i)
         alpha1 = sigma2alpha(ell_ES, sigma1, Pk)
+    else:
+        alpha0 = None
+        alpha1 = None
 
     if all_points:
-        return alpha0, alpha1, totalLen, np.array(P_all)
+        return alpha0, alpha1, totalLen, 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
@@ -210,7 +165,7 @@ def show_points(points: NDArray, pointsES: NDArray, p0: NDArray, p1: NDArray):
 
 
 if __name__ == '__main__':
-    ell = EllipsoidTriaxial.init_name("Fiction")
+    ell = EllipsoidTriaxial.init_name("Bursa1970")
 
     beta0, lamb0 = (0.2, 0.1)
     P0 = ell.ell2cart(beta0, lamb0)
@@ -223,6 +178,8 @@ if __name__ == '__main__':
         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)
+    alpha0, alpha1, s, points = gha2_ES(ell, P0, P1, all_points=True)
+    print(s_num)
+    print(s)
     print(s - s_num)
     show_points(points, points_num, P0, P1)