1 GHA ES
This commit is contained in:
Tammo.Weber
364
1,lambda-ES-CMA.py
Normal file
364
1,lambda-ES-CMA.py
Normal file
@@ -0,0 +1,364 @@
|
||||
import numpy as np
|
||||
import plotly.graph_objects as go
|
||||
from ellipsoide import EllipsoidTriaxial
|
||||
|
||||
|
||||
def ell2cart(ell: EllipsoidTriaxial, beta, lamb):
|
||||
x = ell.ax * np.cos(beta) * np.cos(lamb)
|
||||
y = ell.ay * np.cos(beta) * np.sin(lamb)
|
||||
z = ell.b * np.sin(beta)
|
||||
return np.array([x, y, z], dtype=float)
|
||||
|
||||
|
||||
def liouville(ell: EllipsoidTriaxial, beta, lamb, alpha):
|
||||
k = (ell.Ee**2) / (ell.Ex**2)
|
||||
c_sq = (np.cos(beta)**2 + k * np.sin(beta)**2) * np.sin(alpha)**2 \
|
||||
+ k * np.cos(lamb)**2 * np.cos(alpha)**2
|
||||
return c_sq
|
||||
|
||||
|
||||
def normalize_angles(beta, lamb):
|
||||
beta = np.clip(beta, -np.pi / 2.0, np.pi / 2.0)
|
||||
lamb = (lamb + np.pi) % (2 * np.pi) - np.pi
|
||||
return beta, lamb
|
||||
|
||||
|
||||
def compute_azimuth(beta0: float, lamb0: float,
|
||||
beta1: float, lamb1: float) -> float:
|
||||
dlam = lamb1 - lamb0
|
||||
y = np.cos(beta1) * np.sin(dlam)
|
||||
x = np.cos(beta0) * np.sin(beta1) - np.sin(beta0) * np.cos(beta1) * np.cos(dlam)
|
||||
alpha = np.arctan2(y, x)
|
||||
return alpha
|
||||
|
||||
|
||||
def sphere_forward_step(beta0: float,
|
||||
lamb0: float,
|
||||
alpha0: float,
|
||||
s: float,
|
||||
R: float) -> tuple[float, float]:
|
||||
|
||||
delta = s / R
|
||||
sin_beta0 = np.sin(beta0)
|
||||
cos_beta0 = np.cos(beta0)
|
||||
|
||||
sin_beta2 = sin_beta0 * np.cos(delta) + cos_beta0 * np.sin(delta) * np.cos(alpha0)
|
||||
beta2 = np.arcsin(sin_beta2)
|
||||
|
||||
y = np.sin(alpha0) * np.sin(delta) * cos_beta0
|
||||
x = np.cos(delta) - sin_beta0 * sin_beta2
|
||||
dlam = np.arctan2(y, x)
|
||||
lamb2 = lamb0 + dlam
|
||||
|
||||
beta2, lamb2 = normalize_angles(beta2, lamb2)
|
||||
return beta2, lamb2
|
||||
|
||||
|
||||
def local_step_objective(candidate: np.ndarray,
|
||||
beta_start: float,
|
||||
lamb_start: float,
|
||||
alpha_prev: float,
|
||||
c_target: float,
|
||||
step_length: float,
|
||||
ell: EllipsoidTriaxial,
|
||||
beta_pred: float,
|
||||
lamb_pred: float,
|
||||
w_L: float = 5.0,
|
||||
w_d: float = 5.0,
|
||||
w_p: float = 2.0,
|
||||
w_a: float = 2.0) -> float:
|
||||
|
||||
beta1, lamb1 = candidate
|
||||
beta1, lamb1 = normalize_angles(beta1, lamb1)
|
||||
|
||||
P0 = ell2cart(ell, beta_start, lamb_start)
|
||||
P1 = ell2cart(ell, beta1, lamb1)
|
||||
d = np.linalg.norm(P1 - P0)
|
||||
|
||||
alpha1 = compute_azimuth(beta_start, lamb_start, beta1, lamb1)
|
||||
|
||||
c1 = liouville(ell, beta1, lamb1, alpha1)
|
||||
J_L = (c1 - c_target) ** 2
|
||||
|
||||
J_d = (d - step_length) ** 2
|
||||
|
||||
d_beta = beta1 - beta_pred
|
||||
d_lamb = lamb1 - lamb_pred
|
||||
d_ang2 = d_beta**2 + (np.cos(beta_pred) * d_lamb)**2
|
||||
J_p = d_ang2
|
||||
|
||||
d_alpha = np.arctan2(np.sin(alpha1 - alpha_prev),
|
||||
np.cos(alpha1 - alpha_prev))
|
||||
J_a = d_alpha**2
|
||||
|
||||
return w_L * J_L + w_d * J_d + w_p * J_p + w_a * J_a
|
||||
|
||||
|
||||
|
||||
|
||||
def ES_CMA_step(beta_start: float,
|
||||
lamb_start: float,
|
||||
alpha_prev: float,
|
||||
c_target: float,
|
||||
step_length: float,
|
||||
ell: EllipsoidTriaxial,
|
||||
beta_pred: float,
|
||||
lamb_pred: float,
|
||||
sigma0: float,
|
||||
stopfitness: float = 1e-18) -> tuple[float, float]:
|
||||
|
||||
N = 2
|
||||
xmean = np.array([beta_pred, lamb_pred], dtype=float)
|
||||
sigma = sigma0
|
||||
stopeval = int(400 * N**2)
|
||||
|
||||
lamb = 30
|
||||
mu = 1
|
||||
weights = np.array([1.0])
|
||||
mueff = 1.0
|
||||
|
||||
cc = (4 + mueff / N) / (N + 4 + 2 * mueff / N)
|
||||
cs = (mueff + 2) / (N + mueff + 5)
|
||||
c1 = 2 / ((N + 1.3)**2 + mueff)
|
||||
cmu = min(1 - c1,
|
||||
2 * (mueff - 2 + 1/mueff) / ((N + 2)**2 + mueff))
|
||||
damps = 1 + 2 * max(0, np.sqrt((mueff - 1) / (N + 1)) - 1) + cs
|
||||
|
||||
pc = np.zeros(N)
|
||||
ps = np.zeros(N)
|
||||
B = np.eye(N)
|
||||
D = np.eye(N)
|
||||
C = B @ D @ (B @ D).T
|
||||
eigeneval = 0
|
||||
chiN = np.sqrt(N) * (1 - 1 / (4 * N) + 1 / (21 * N**2))
|
||||
|
||||
counteval = 0
|
||||
arx = np.zeros((N, lamb))
|
||||
arz = np.zeros((N, lamb))
|
||||
arfitness = np.zeros(lamb)
|
||||
|
||||
while counteval < stopeval:
|
||||
for k in range(lamb):
|
||||
arz[:, k] = np.random.randn(N)
|
||||
arx[:, k] = xmean + sigma * (B @ D @ arz[:, k])
|
||||
|
||||
arfitness[k] = local_step_objective(
|
||||
arx[:, k],
|
||||
beta_start, lamb_start,
|
||||
alpha_prev,
|
||||
c_target,
|
||||
step_length,
|
||||
ell,
|
||||
beta_pred, lamb_pred
|
||||
)
|
||||
counteval += 1
|
||||
|
||||
idx = np.argsort(arfitness)
|
||||
arfitness = arfitness[idx]
|
||||
arindex = idx
|
||||
|
||||
xold = xmean.copy()
|
||||
xmean = arx[:, arindex[:mu]] @ weights
|
||||
zmean = arz[:, arindex[:mu]] @ weights
|
||||
|
||||
ps = (1 - cs) * ps + np.sqrt(cs * (2 - cs) * mueff) * (B @ zmean)
|
||||
norm_ps = np.linalg.norm(ps)
|
||||
hsig = norm_ps / np.sqrt(1 - (1 - cs)**(2 * counteval / lamb)) / chiN < 1.4 + 2 / (N + 1)
|
||||
hsig = 1.0 if hsig else 0.0
|
||||
pc = (1 - cc) * pc + hsig * np.sqrt(cc * (2 - cc) * mueff) * (B @ D @ zmean)
|
||||
|
||||
BDz = B @ D @ arz[:, arindex[:mu]]
|
||||
C = (1 - c1 - cmu) * C \
|
||||
+ c1 * (np.outer(pc, pc) + (1 - hsig) * cc * (2 - cc) * C) \
|
||||
+ cmu * BDz @ np.diag(weights) @ BDz.T
|
||||
|
||||
sigma = sigma * np.exp((cs / damps) * (norm_ps / chiN - 1))
|
||||
|
||||
if counteval - eigeneval > lamb / ((c1 + cmu) * N * 10):
|
||||
eigeneval = counteval
|
||||
C = (C + C.T) / 2.0
|
||||
eigvals, B = np.linalg.eigh(C)
|
||||
D = np.diag(np.sqrt(np.maximum(eigvals, 1e-20)))
|
||||
|
||||
if arfitness[0] <= stopfitness:
|
||||
break
|
||||
|
||||
xmin = arx[:, arindex[0]]
|
||||
beta1, lamb1 = normalize_angles(xmin[0], xmin[1])
|
||||
return beta1, lamb1
|
||||
|
||||
|
||||
def march_geodesic(beta1: float,
|
||||
lamb1: float,
|
||||
alpha1: float,
|
||||
S_total: float,
|
||||
step_length: float,
|
||||
ell: EllipsoidTriaxial):
|
||||
|
||||
beta_curr = beta1
|
||||
lamb_curr = lamb1
|
||||
alpha_curr = alpha1
|
||||
|
||||
betas = [beta_curr]
|
||||
lambs = [lamb_curr]
|
||||
alphas = [alpha_curr]
|
||||
|
||||
c_target = liouville(ell, beta_curr, lamb_curr, alpha_curr)
|
||||
|
||||
total_distance = 0.0
|
||||
R_sphere = ell.ax
|
||||
sigma0 = 1e-10
|
||||
|
||||
while total_distance < S_total - 1e-6:
|
||||
remaining = S_total - total_distance
|
||||
this_step = min(step_length, remaining)
|
||||
|
||||
beta_pred, lamb_pred = sphere_forward_step(
|
||||
beta_curr, lamb_curr, alpha_curr,
|
||||
this_step, R_sphere
|
||||
)
|
||||
|
||||
beta_next, lamb_next = ES_CMA_step(
|
||||
beta_curr, lamb_curr, alpha_curr,
|
||||
c_target,
|
||||
this_step,
|
||||
ell,
|
||||
beta_pred,
|
||||
lamb_pred,
|
||||
sigma0=sigma0
|
||||
)
|
||||
|
||||
P_curr = ell2cart(ell, beta_curr, lamb_curr)
|
||||
P_next = ell2cart(ell, beta_next, lamb_next)
|
||||
d_step = np.linalg.norm(P_next - P_curr)
|
||||
total_distance += d_step
|
||||
|
||||
alpha_next = compute_azimuth(beta_curr, lamb_curr, beta_next, lamb_next)
|
||||
|
||||
beta_curr, lamb_curr, alpha_curr = beta_next, lamb_next, alpha_next
|
||||
betas.append(beta_curr)
|
||||
lambs.append(lamb_curr)
|
||||
alphas.append(alpha_curr)
|
||||
|
||||
return np.array(betas), np.array(lambs), np.array(alphas), total_distance
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
ell = EllipsoidTriaxial.init_name("BursaSima1980round")
|
||||
|
||||
beta1 = np.deg2rad(10.0)
|
||||
lamb1 = np.deg2rad(10.0)
|
||||
alpha1 = 0.7494562507041596 # Ergebnis 20°, 20°
|
||||
|
||||
STEP_LENGTH = 500.0
|
||||
S_total = 1542703.1458877102
|
||||
|
||||
betas, lambs, alphas, S_real = march_geodesic(
|
||||
beta1, lamb1, alpha1,
|
||||
S_total,
|
||||
STEP_LENGTH,
|
||||
ell
|
||||
)
|
||||
|
||||
print("Anzahl Schritte:", len(betas) - 1)
|
||||
print("Resultierende Gesamtstrecke (Chord, Ellipsoid):", S_real, "m")
|
||||
print("Letzter Punkt (beta, lambda) in Grad:",
|
||||
np.rad2deg(betas[-1]), np.rad2deg(lambs[-1]))
|
||||
|
||||
def plot_geodesic_3d(ell: EllipsoidTriaxial,
|
||||
betas: np.ndarray,
|
||||
lambs: np.ndarray,
|
||||
n_beta: int = 60,
|
||||
n_lamb: int = 120):
|
||||
|
||||
beta_grid = np.linspace(-np.pi/2, np.pi/2, n_beta)
|
||||
lamb_grid = np.linspace(-np.pi, np.pi, n_lamb)
|
||||
B, L = np.meshgrid(beta_grid, lamb_grid, indexing="ij")
|
||||
|
||||
Xs = ell.ax * np.cos(B) * np.cos(L)
|
||||
Ys = ell.ay * np.cos(B) * np.sin(L)
|
||||
Zs = ell.b * np.sin(B)
|
||||
|
||||
Xp = ell.ax * np.cos(betas) * np.cos(lambs)
|
||||
Yp = ell.ay * np.cos(betas) * np.sin(lambs)
|
||||
Zp = ell.b * np.sin(betas)
|
||||
|
||||
fig = go.Figure()
|
||||
|
||||
fig.add_trace(
|
||||
go.Surface(
|
||||
x=Xs,
|
||||
y=Ys,
|
||||
z=Zs,
|
||||
opacity=0.6,
|
||||
showscale=False,
|
||||
colorscale="Viridis",
|
||||
name="Ellipsoid"
|
||||
)
|
||||
)
|
||||
|
||||
fig.add_trace(
|
||||
go.Scatter3d(
|
||||
x=Xp,
|
||||
y=Yp,
|
||||
z=Zp,
|
||||
mode="lines+markers",
|
||||
line=dict(width=5, color="red"),
|
||||
marker=dict(size=4, color="black"),
|
||||
name="ES-CMA Schritte"
|
||||
)
|
||||
)
|
||||
|
||||
fig.add_trace(
|
||||
go.Scatter3d(
|
||||
x=[Xp[0]], y=[Yp[0]], z=[Zp[0]],
|
||||
mode="markers",
|
||||
marker=dict(size=6, color="green"),
|
||||
name="Start"
|
||||
)
|
||||
)
|
||||
fig.add_trace(
|
||||
go.Scatter3d(
|
||||
x=[Xp[-1]], y=[Yp[-1]], z=[Zp[-1]],
|
||||
mode="markers",
|
||||
marker=dict(size=6, color="blue"),
|
||||
name="Ende"
|
||||
)
|
||||
)
|
||||
|
||||
fig.update_layout(
|
||||
title="ES-CMA",
|
||||
scene=dict(
|
||||
xaxis_title="X [m]",
|
||||
yaxis_title="Y [m]",
|
||||
zaxis_title="Z [m]",
|
||||
aspectmode="data"
|
||||
)
|
||||
)
|
||||
|
||||
fig.show()
|
||||
|
||||
plot_geodesic_3d(ell, betas, lambs)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user