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);
}
}