diff --git a/1,lambda-ES-CMA.py b/1,lambda-ES-CMA.py new file mode 100644 index 0000000..7887b35 --- /dev/null +++ b/1,lambda-ES-CMA.py @@ -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) + + + + + + + + + + + + + + + + + + + + + +