def _lambert(k, r0, r, tof, short, numiter, rtol): if short: t_m = +1 else: t_m = -1 norm_r0 = dot(r0, r0)**.5 norm_r = dot(r, r)**.5 cos_dnu = dot(r0, r) / (norm_r0 * norm_r) A = t_m * (norm_r * norm_r0 * (1 + cos_dnu))**.5 if A == 0.0: raise RuntimeError("Cannot compute orbit, phase angle is 180 degrees") psi = 0.0 psi_low = -4 * np.pi psi_up = 4 * np.pi count = 0 while count < numiter: y = norm_r0 + norm_r + A * (psi * c3(psi) - 1) / c2(psi)**.5 if A > 0.0 and y < 0.0: # Readjust xi_low until y > 0.0 # Translated directly from Vallado while y < 0.0: psi_low = psi psi = (0.8 * (1.0 / c3(psi)) * (1.0 - (norm_r0 + norm_r) * np.sqrt(c2(psi)) / A)) y = norm_r0 + norm_r + A * (psi * c3(psi) - 1) / c2(psi)**.5 xi = np.sqrt(y / c2(psi)) tof_new = (xi**3 * c3(psi) + A * np.sqrt(y)) / np.sqrt(k) # Convergence check if np.abs((tof_new - tof) / tof) < rtol: break else: count += 1 # Bisection check if tof_new <= tof: psi_low = psi else: psi_up = psi psi = (psi_up + psi_low) / 2 else: raise RuntimeError("Maximum number of iterations reached") f = 1 - y / norm_r0 g = A * np.sqrt(y / k) gdot = 1 - y / norm_r return f, g, (f * gdot - 1) / g, gdot
def _kepler(k, r0, v0, tof, numiter, rtol): # Cache some results dot_r0v0 = dot(r0, v0) norm_r0 = dot(r0, r0) ** .5 sqrt_mu = k**.5 alpha = -dot(v0, v0) / k + 2 / norm_r0 # First guess if alpha > 0: # Elliptic orbit xi_new = sqrt_mu * tof * alpha elif alpha < 0: # Hyperbolic orbit xi_new = (np.sign(tof) * (-1 / alpha)**.5 * np.log((-2 * k * alpha * tof) / (dot_r0v0 + np.sign(tof) * np.sqrt(-k / alpha) * (1 - norm_r0 * alpha)))) else: # Parabolic orbit # (Conservative initial guess) xi_new = sqrt_mu * tof / norm_r0 # Newton-Raphson iteration on the Kepler equation count = 0 while count < numiter: xi = xi_new psi = xi * xi * alpha c2_psi = c2(psi) c3_psi = c3(psi) norm_r = (xi * xi * c2_psi + dot_r0v0 / sqrt_mu * xi * (1 - psi * c3_psi) + norm_r0 * (1 - psi * c2_psi)) xi_new = xi + (sqrt_mu * tof - xi * xi * xi * c3_psi - dot_r0v0 / sqrt_mu * xi * xi * c2_psi - norm_r0 * xi * (1 - psi * c3_psi)) / norm_r if abs(np.divide(xi_new - xi, xi_new)) < rtol or abs(xi_new - xi) < rtol: break else: count += 1 else: raise RuntimeError("Maximum number of iterations reached") # Compute Lagrange coefficients f = 1 - xi**2 / norm_r0 * c2_psi g = tof - xi**3 / sqrt_mu * c3_psi gdot = 1 - xi**2 / norm_r * c2_psi fdot = sqrt_mu / (norm_r * norm_r0) * xi * (psi * c3_psi - 1) return f, g, fdot, gdot