Status EmbeddedExplicitRungeKuttaNyströmIntegrator<Position, higher_order, lower_order, stages, first_same_as_last>:: Instance::Solve(Instant const& t_final) { using Displacement = typename ODE::Displacement; using Velocity = typename ODE::Velocity; using Acceleration = typename ODE::Acceleration; auto const& a = integrator_.a_; auto const& b_hat = integrator_.b_hat_; auto const& b_prime_hat = integrator_.b_prime_hat_; auto const& b = integrator_.b_; auto const& b_prime = integrator_.b_prime_; auto const& c = integrator_.c_; auto& current_state = this->current_state_; auto& append_state = this->append_state_; auto& adaptive_step_size = this->adaptive_step_size_; auto const& equation = this->equation_; // |current_state| gets updated as the integration progresses to allow // restartability. // State before the last, truncated step. std::experimental::optional<typename ODE::SystemState> final_state; // Argument checks. int const dimension = current_state.positions.size(); Sign const integration_direction = Sign(adaptive_step_size.first_time_step); if (integration_direction.Positive()) { // Integrating forward. CHECK_LT(current_state.time.value, t_final); } else { // Integrating backward. CHECK_GT(current_state.time.value, t_final); } // Time step. Time h = adaptive_step_size.first_time_step; // Current time. This is a non-const reference whose purpose is to make the // equations more readable. DoublePrecision<Instant>& t = current_state.time; // Position increment (high-order). std::vector<Displacement> Δq_hat(dimension); // Velocity increment (high-order). std::vector<Velocity> Δv_hat(dimension); // Current position. This is a non-const reference whose purpose is to make // the equations more readable. std::vector<DoublePrecision<Position>>& q_hat = current_state.positions; // Current velocity. This is a non-const reference whose purpose is to make // the equations more readable. std::vector<DoublePrecision<Velocity>>& v_hat = current_state.velocities; // Difference between the low- and high-order approximations. typename ODE::SystemStateError error_estimate; error_estimate.position_error.resize(dimension); error_estimate.velocity_error.resize(dimension); // Current Runge-Kutta-Nyström stage. std::vector<Position> q_stage(dimension); // Accelerations at each stage. // TODO(egg): this is a rectangular container, use something more appropriate. std::vector<std::vector<Acceleration>> g(stages); for (auto& g_stage : g) { g_stage.resize(dimension); } bool at_end = false; double tolerance_to_error_ratio; // The first stage of the Runge-Kutta-Nyström iteration. In the FSAL case, // |first_stage == 1| after the first step, since the first RHS evaluation has // already occured in the previous step. In the non-FSAL case and in the // first step of the FSAL case, |first_stage == 0|. int first_stage = 0; // The number of steps already performed. std::int64_t step_count = 0; // No step size control on the first step. goto runge_kutta_nyström_step; while (!at_end) { // Compute the next step with decreasing step sizes until the error is // tolerable. do { // Adapt step size. // TODO(egg): find out whether there's a smarter way to compute that root, // especially since we make the order compile-time. h *= adaptive_step_size.safety_factor * std::pow(tolerance_to_error_ratio, 1.0 / (lower_order + 1)); // TODO(egg): should we check whether it vanishes in double precision // instead? if (t.value + (t.error + h) == t.value) { return Status(termination_condition::VanishingStepSize, "At time " + DebugString(t.value) + ", step size is effectively zero. Singularity or " "stiff system suspected."); } runge_kutta_nyström_step: // Termination condition. Time const time_to_end = (t_final - t.value) - t.error; at_end = integration_direction * h >= integration_direction * time_to_end; if (at_end) { // The chosen step size will overshoot. Clip it to just reach the end, // and terminate if the step is accepted. h = time_to_end; final_state = current_state; } // Runge-Kutta-Nyström iteration; fills |g|. for (int i = first_stage; i < stages; ++i) { Instant const t_stage = t.value + c[i] * h; for (int k = 0; k < dimension; ++k) { Acceleration Σj_a_ij_g_jk{}; for (int j = 0; j < i; ++j) { Σj_a_ij_g_jk += a[i][j] * g[j][k]; } q_stage[k] = q_hat[k].value + h * (c[i] * v_hat[k].value + h * Σj_a_ij_g_jk); } equation.compute_acceleration(t_stage, q_stage, g[i]); } // Increment computation and step size control. for (int k = 0; k < dimension; ++k) { Acceleration Σi_b_hat_i_g_ik{}; Acceleration Σi_b_i_g_ik{}; Acceleration Σi_b_prime_hat_i_g_ik{}; Acceleration Σi_b_prime_i_g_ik{}; // Please keep the eight assigments below aligned, they become illegible // otherwise. for (int i = 0; i < stages; ++i) { Σi_b_hat_i_g_ik += b_hat[i] * g[i][k]; Σi_b_i_g_ik += b[i] * g[i][k]; Σi_b_prime_hat_i_g_ik += b_prime_hat[i] * g[i][k]; Σi_b_prime_i_g_ik += b_prime[i] * g[i][k]; } // The hat-less Δq and Δv are the low-order increments. Δq_hat[k] = h * (h * (Σi_b_hat_i_g_ik) + v_hat[k].value); Displacement const Δq_k = h * (h * (Σi_b_i_g_ik) + v_hat[k].value); Δv_hat[k] = h * Σi_b_prime_hat_i_g_ik; Velocity const Δv_k = h * Σi_b_prime_i_g_ik; error_estimate.position_error[k] = Δq_k - Δq_hat[k]; error_estimate.velocity_error[k] = Δv_k - Δv_hat[k]; } tolerance_to_error_ratio = adaptive_step_size.tolerance_to_error_ratio(h, error_estimate); } while (tolerance_to_error_ratio < 1.0); if (first_same_as_last) { using std::swap; swap(g.front(), g.back()); first_stage = 1; } // Increment the solution with the high-order approximation. t.Increment(h); for (int k = 0; k < dimension; ++k) { q_hat[k].Increment(Δq_hat[k]); v_hat[k].Increment(Δv_hat[k]); } append_state(current_state); ++step_count; if (step_count == adaptive_step_size.max_steps && !at_end) { return Status(termination_condition::ReachedMaximalStepCount, "Reached maximum step count " + std::to_string(adaptive_step_size.max_steps) + " at time " + DebugString(t.value) + "; requested t_final is " + DebugString(t_final) + "."); } } // The resolution is restartable from the last non-truncated state. CHECK(final_state); current_state = *final_state; return Status(termination_condition::Done, ""); }
void SymplecticRungeKuttaNyströmIntegrator<Position, order, time_reversible, evaluations, composition>::Solve( IntegrationProblem<ODE> const& problem, Time const& step) const { using Displacement = typename ODE::Displacement; using Velocity = typename ODE::Velocity; using Acceleration = typename ODE::Acceleration; // Argument checks. CHECK_NOTNULL(problem.initial_state); int const dimension = problem.initial_state->positions.size(); CHECK_EQ(dimension, problem.initial_state->velocities.size()); CHECK_NE(Time(), step); Sign const integration_direction = Sign(step); if (integration_direction.Positive()) { // Integrating forward. CHECK_LT(problem.initial_state->time.value, problem.t_final); } else { // Integrating backward. CHECK_GT(problem.initial_state->time.value, problem.t_final); } typename ODE::SystemState current_state = *problem.initial_state; // Time step. Time const& h = step; Time const abs_h = integration_direction * h; // Current time. This is a non-const reference whose purpose is to make the // equations more readable. DoublePrecision<Instant>& t = current_state.time; // Position increment. std::vector<Displacement> Δq(dimension); // Velocity increment. std::vector<Velocity> Δv(dimension); // Current position. This is a non-const reference whose purpose is to make // the equations more readable. std::vector<DoublePrecision<Position>>& q = current_state.positions; // Current velocity. This is a non-const reference whose purpose is to make // the equations more readable. std::vector<DoublePrecision<Velocity>>& v = current_state.velocities; // Current Runge-Kutta-Nyström stage. std::vector<Position> q_stage(dimension); // Accelerations at the current stage. std::vector<Acceleration> g(dimension); // The first full stage of the step, i.e. the first stage where // exp(bᵢ h B) exp(aᵢ h A) must be entirely computed. // Always 0 in the non-FSAL kBA case, always 1 in the kABA case since b₀ = 0, // means the first stage is only exp(a₀ h A), and 1 after the first step // in the kBAB case, since the last right-hand-side evaluation can be used for // exp(bᵢ h B). int first_stage = composition == kABA ? 1 : 0; while (abs_h <= Abs((problem.t_final - t.value) - t.error)) { std::fill(Δq.begin(), Δq.end(), Displacement{}); std::fill(Δv.begin(), Δv.end(), Velocity{}); if (first_stage == 1) { for (int k = 0; k < dimension; ++k) { if (composition == kBAB) { // exp(b₀ h B) Δv[k] += h * b_[0] * g[k]; } // exp(a₀ h A) Δq[k] += h * a_[0] * (v[k].value + Δv[k]); } } for (int i = first_stage; i < stages_; ++i) { for (int k = 0; k < dimension; ++k) { q_stage[k] = q[k].value + Δq[k]; } problem.equation.compute_acceleration(t.value + c_[i] * h, q_stage, &g); for (int k = 0; k < dimension; ++k) { // exp(bᵢ h B) Δv[k] += h * b_[i] * g[k]; // exp(aᵢ h A) Δq[k] += h * a_[i] * (v[k].value + Δv[k]); } } if (composition == kBAB) { first_stage = 1; } // Increment the solution. t.Increment(h); for (int k = 0; k < dimension; ++k) { q[k].Increment(Δq[k]); v[k].Increment(Δv[k]); } problem.append_state(current_state); } }