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)