Merge remote-tracking branch 'refs/remotes/origin/main2'
# Conflicts: # GHA_triaxial/ES_gha2.py
This commit is contained in:
@@ -8,179 +8,134 @@ from GHA_triaxial.panou_2013_2GHA_num import gha2_num
|
|||||||
from utils import sigma2alpha
|
from utils import sigma2alpha
|
||||||
|
|
||||||
ell_ES: EllipsoidTriaxial = None
|
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 P1: kartesische Koordinate Punkt 1
|
||||||
:param P2: kartesische Koordinate Punkt 2
|
:param P2: kartesische Koordinate Punkt 2
|
||||||
:return: Bogenlänge s
|
: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
|
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.
|
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 ell: Parameter des triaxialen Ellipsoids
|
||||||
:param P0: Startpunkt
|
:param P0: Startpunkt
|
||||||
:param Pk: Zielpunkt
|
: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
|
: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
|
ell_ES = ell
|
||||||
P_start = P0
|
R0 = (ell.ax + ell.ay + ell.b) / 3
|
||||||
P_end = Pk
|
if maxSegLen is None:
|
||||||
if stepLenTarget is None:
|
maxSegLen = R0 * 1 / (637.4) # 10km Segment bei mittleren Erdradius
|
||||||
R0 = (ell.ax + ell.ay + ell.b) / 3
|
|
||||||
stepLenTarget = R0 * 1 / 600
|
|
||||||
stepLen = stepLenTarget
|
|
||||||
|
|
||||||
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
|
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.")
|
||||||
|
|
||||||
P_next = ell.para2cart(u, v)
|
new_points.append(B)
|
||||||
|
|
||||||
d_step = Bogenlaenge(P_prev, P_next)
|
points = new_points
|
||||||
d_new = Bogenlaenge(P_next, P_end)
|
print(f"[Level {level}] Punkte: {len(points)} | max Segment: {max_len:.3f} m")
|
||||||
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')
|
P_all = np.vstack(points)
|
||||||
|
totalLen = float(np.sum(np.linalg.norm(P_all[1:] - P_all[:-1], axis=1)))
|
||||||
|
|
||||||
# Sicherheitscheck: wenn wir nicht näher kommen, abbrechen
|
if len(points) >= 3:
|
||||||
if d_new >= d_old:
|
p0i = ell_ES.point_onto_ellipsoid(P0 + 10.0 * (points[1] - P0) / np.linalg.norm(points[1] - P0))
|
||||||
print(f'Punkt {i}: Neuer Punkt ist nicht naeher am Ziel ({d_old} -> {d_new}). Abbruch.')
|
sigma0 = (p0i - P0) / np.linalg.norm(p0i - P0)
|
||||||
P_all.append(P_end)
|
alpha0 = sigma2alpha(ell_ES, sigma0, P0)
|
||||||
totalLen += Bogenlaenge(P_prev, P_end)
|
|
||||||
break
|
|
||||||
|
|
||||||
P_all.append(P_next)
|
p1i = ell_ES.point_onto_ellipsoid(Pk - 10.0 * (Pk - points[-2]) / np.linalg.norm(Pk - points[-2]))
|
||||||
totalLen += d_step
|
sigma1 = (Pk - p1i) / np.linalg.norm(Pk - p1i)
|
||||||
P_prev = P_next
|
alpha1 = sigma2alpha(ell_ES, sigma1, Pk)
|
||||||
|
else:
|
||||||
print('Maximale Schrittanzahl erreicht.')
|
alpha0 = None
|
||||||
# P_all.append(P_end)
|
alpha1 = None
|
||||||
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]))
|
|
||||||
sigma1 = (Pk - p1i) / np.linalg.norm(Pk - p1i)
|
|
||||||
alpha1 = sigma2alpha(ell_ES, sigma1, Pk)
|
|
||||||
|
|
||||||
if all_points:
|
if all_points:
|
||||||
return alpha0, alpha1, totalLen, np.array(P_all)
|
return alpha0, alpha1, totalLen, P_all
|
||||||
else:
|
return alpha0, alpha1, totalLen
|
||||||
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):
|
def show_points(points: NDArray, pointsES: NDArray, p0: NDArray, p1: NDArray):
|
||||||
"""
|
"""
|
||||||
Anzeigen der Punkte
|
Anzeigen der Punkte
|
||||||
@@ -210,7 +165,7 @@ def show_points(points: NDArray, pointsES: NDArray, p0: NDArray, p1: NDArray):
|
|||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
if __name__ == '__main__':
|
||||||
ell = EllipsoidTriaxial.init_name("Fiction")
|
ell = EllipsoidTriaxial.init_name("Bursa1970")
|
||||||
|
|
||||||
beta0, lamb0 = (0.2, 0.1)
|
beta0, lamb0 = (0.2, 0.1)
|
||||||
P0 = ell.ell2cart(beta0, lamb0)
|
P0 = ell.ell2cart(beta0, lamb0)
|
||||||
@@ -223,6 +178,8 @@ if __name__ == '__main__':
|
|||||||
points_num.append(ell.ell2cart(beta, lamb))
|
points_num.append(ell.ell2cart(beta, lamb))
|
||||||
points_num = np.array(points_num)
|
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)
|
print(s - s_num)
|
||||||
show_points(points, points_num, P0, P1)
|
show_points(points, points_num, P0, P1)
|
||||||
|
|||||||
Reference in New Issue
Block a user