365 lines
9.5 KiB
Python
365 lines
9.5 KiB
Python
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)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|