Use the RKNG

ksp_plugin/flight_plan.cpp

@@ -14,9 +14,11 @@ namespace internal_flight_plan {
 
 using base::make_not_null_unique;
 using geometry::Position;
+using geometry::Vector;
 using geometry::Velocity;
 using integrators::EmbeddedExplicitRungeKuttaNyströmIntegrator;
 using integrators::methods::DormandالمكاوىPrince1986RKN434FM;
+using quantities::Acceleration;
 using quantities::si::Metre;
 using quantities::si::Second;
 
@@ -321,38 +323,35 @@ void FlightPlan::BurnLastSegment(NavigationManœuvre const& manœuvre) {
         anomalous_segments_ = 1;
       }
     } else {
-      // We decompose the manœuvre in smaller unguided manœuvres (movements),
-      // which are unguided.
-      // TODO(egg): eventually we should just compute the direction in the
-      // right-hand-side of the integrator, but for that we need the velocity,
-      // which means we need general Runge-Kutta integrators.
-      constexpr int movements = 100;
-      Mass remaining_mass = manœuvre.initial_mass();
-      serialization::DynamicFrame serialized_manœuvre_frame;
-      manœuvre.frame()->WriteToMessage(&serialized_manœuvre_frame);
-      for (int i = 0; i < movements; ++i) {
-        NavigationManœuvre movement(manœuvre.thrust(),
-                                    remaining_mass,
-                                    manœuvre.specific_impulse(),
-                                    manœuvre.direction(),
-                                    NavigationFrame::ReadFromMessage(
-                                        serialized_manœuvre_frame, ephemeris_),
-                                    /*is_inertially_fixed=*/true);
-        movement.set_duration(manœuvre.duration() / movements);
-        movement.set_initial_time(segments_.back()->last().time());
-        movement.set_coasting_trajectory(segments_.back());
-        remaining_mass = movement.final_mass();
-        bool const reached_desired_final_time =
-            ephemeris_->FlowWithAdaptiveStep(segments_.back(),
-                                             movement.IntrinsicAcceleration(),
-                                             movement.final_time(),
-                                             adaptive_step_parameters_,
-                                             max_ephemeris_steps_per_frame,
-                                             /*last_point_only=*/false).ok();
-        if (!reached_desired_final_time) {
-          anomalous_segments_ = 1;
-          return;
-        }
+      // TODO(egg): move this to the Manœuvre class.
+      auto const intrinsic_acceleration =
+          [&manœuvre](Instant const& time,
+                      Position<Barycentric> const& position,
+                      Velocity<Barycentric> const& velocity)
+          -> Vector<Acceleration, Barycentric> {
+        auto const to_manœuvre_frame =
+            manœuvre.frame()->ToThisFrameAtTime(time);
+        auto const from_manœuvre_frame = to_manœuvre_frame.Inverse();
+        Vector<double, Barycentric> const direction =
+            from_manœuvre_frame.orthogonal_map()(manœuvre.frame()->FrenetFrame(
+                time,
+                to_manœuvre_frame({position, velocity}))(manœuvre.direction()));
+        return direction *
+               (manœuvre.thrust() /
+                (manœuvre.initial_mass() -
+                 (time - manœuvre.initial_time()) * manœuvre.mass_flow()));
+      };
+      bool const reached_desired_final_time =
+          ephemeris_->FlowWithAdaptiveStepGeneralized(
+              segments_.back(),
+              intrinsic_acceleration,
+              manœuvre.final_time(),
+              adaptive_step_parameters_,
+              max_ephemeris_steps_per_frame,
+              /*last_point_only=*/false).ok();
+      if (!reached_desired_final_time) {
+        anomalous_segments_ = 1;
+        return;
       }
     }
   }

ksp_plugin_test/interface_external_test.cpp

@@ -20,6 +20,7 @@ using ksp_plugin::PartId;
 using ksp_plugin::FakePlugin;
 using ksp_plugin::Vessel;
 using physics::SolarSystem;
+using quantities::si::Centi;
 using quantities::si::Hour;
 using quantities::si::Kilo;
 using quantities::si::Kilogram;
@@ -32,8 +33,10 @@ using testing_utilities::Componentwise;
 using testing_utilities::IsOk;
 using testing_utilities::IsNear;
 using testing_utilities::SolarSystemFactory;
-using ::testing::AnyOf;
+using ::testing::AllOf;
 using ::testing::Eq;
+using ::testing::Gt;
+using ::testing::Lt;
 
 namespace {
 
@@ -102,14 +105,13 @@ TEST_F(InterfaceExternalTest, GetNearestPlannedCoastDegreesOfFreedom) {
   // the apoapsis.
   EXPECT_THAT(
       barycentric_result,
-      Componentwise(
-          Componentwise(IsNear(-12'000 * Kilo(Metre)),
-                        IsNear(-120 * Kilo(Metre)),
-                        AnyOf(IsNear(2.2 * Metre), IsNear(-35 * Metre))),
-          Componentwise(IsNear(-6.6 * Metre / Second),
-                        IsNear(-4.9 * Kilo(Metre) / Second),
-                        AnyOf(IsNear(54 * Micro(Metre) / Second),
-                              IsNear(6.6 * Milli(Metre) / Second)))));
+      Componentwise(Componentwise(IsNear(-12'000 * Kilo(Metre)),
+                                  IsNear(-120 * Kilo(Metre)),
+                                  AllOf(Gt(-50 * Metre), Lt(50 * Metre))),
+                    Componentwise(IsNear(-6.6 * Metre / Second),
+                                  IsNear(-4.9 * Kilo(Metre) / Second),
+                                  AllOf(Gt(-1 * Centi(Metre) / Second),
+                                        Lt(1 * Centi(Metre) / Second)))));
 }
 
 }  // namespace interface

physics/ephemeris.hpp

@@ -32,6 +32,7 @@ using base::Status;
 using geometry::Instant;
 using geometry::Position;
 using geometry::Vector;
+using geometry::Velocity;
 using integrators::AdaptiveStepSizeIntegrator;
 using integrators::FixedStepSizeIntegrator;
 using integrators::Integrator;
@@ -54,6 +55,11 @@ class Ephemeris {
   using IntrinsicAcceleration =
       std::function<Vector<Acceleration, Frame>(Instant const& time)>;
   static std::nullptr_t constexpr NoIntrinsicAcceleration = nullptr;
+  using GeneralIntrinsicAcceleration =
+      std::function<Vector<Acceleration, Frame>(
+          Instant const& time,
+          Position<Frame> const& position,
+          Velocity<Frame> const& velocity)>;
   using IntrinsicAccelerations = std::vector<IntrinsicAcceleration>;
   static IntrinsicAccelerations const NoIntrinsicAccelerations;
   static std::int64_t constexpr unlimited_max_ephemeris_steps =
@@ -184,6 +190,23 @@ class Ephemeris {
       std::int64_t max_ephemeris_steps,
       bool last_point_only);
 
+  // Integrates, until exactly |t| (except for timeouts or singularities), the
+  // |trajectory| followed by a massless body in the gravitational potential
+  // described by |*this|.  If |t > t_max()|, calls |Prolong(t)| beforehand.
+  // Prolongs the ephemeris by at most |max_ephemeris_steps|.  If
+  // |last_point_only| is true, only the last point is appended to the
+  // trajectory.  Returns OK if and only if |*trajectory| was integrated until
+  // |t|.
+  // TODO(phl): the |parameters| are used, but |parameters.integrator_| is
+  // ignored and a hard-coded RKNG is used instead.
+  virtual Status FlowWithAdaptiveStepGeneralized(
+      not_null<DiscreteTrajectory<Frame>*> trajectory,
+      GeneralIntrinsicAcceleration intrinsic_acceleration,
+      Instant const& t,
+      AdaptiveStepParameters const& parameters,
+      std::int64_t max_ephemeris_steps,
+      bool last_point_only);
+
   // Integrates, until at most |t|, the trajectories followed by massless
   // bodies in the gravitational potential described by |*this|.  If
   // |t > t_max()|, calls |Prolong(t)| beforehand.  The trajectories and

physics/ephemeris_body.hpp

@@ -43,8 +43,11 @@ using geometry::Position;
 using geometry::R3Element;
 using geometry::Sign;
 using geometry::Velocity;
+using integrators::EmbeddedExplicitGeneralizedRungeKuttaNyströmIntegrator;
+using integrators::ExplicitSecondOrderOrdinaryDifferentialEquation;
 using integrators::Integrator;
 using integrators::IntegrationProblem;
+using integrators::methods::Fine1987RKNG34;
 using numerics::Bisect;
 using numerics::DoublePrecision;
 using numerics::Hermite3;
@@ -552,6 +555,117 @@ Status Ephemeris<Frame>::FlowWithAdaptiveStep(
   }
 }
 
+template<typename Frame>
+Status Ephemeris<Frame>::FlowWithAdaptiveStepGeneralized(
+      not_null<DiscreteTrajectory<Frame>*> trajectory,
+      GeneralIntrinsicAcceleration intrinsic_acceleration,
+      Instant const& t,
+      AdaptiveStepParameters const& parameters,
+      std::int64_t max_ephemeris_steps,
+      bool last_point_only) {
+  // TODO(phl): FlowWithAdaptiveStepGeneralized and FlowWithAdaptiveStep are
+  // very similar.  Factor that which can be factored.
+  using ODE = ExplicitSecondOrderOrdinaryDifferentialEquation<Position<Frame>>;
+  Instant const& trajectory_last_time = trajectory->last().time();
+  if (trajectory_last_time == t) {
+    return Status::OK;
+  }
+
+  std::vector<not_null<DiscreteTrajectory<Frame>*>> const trajectories =
+      {trajectory};
+  // The |min| is here to prevent us from spending too much time computing the
+  // ephemeris.  The |max| is here to ensure that we always try to integrate
+  // forward.  We use |last_state_.time.value| because this is always finite,
+  // contrary to |t_max()|, which is -∞ when |empty()|.
+  Instant const t_final =
+      std::min(std::max(instance_time() +
+                            max_ephemeris_steps * parameters_.step(),
+                        trajectory_last_time + parameters_.step()),
+               t);
+  Prolong(t_final);
+
+  IntegrationProblem<ODE> problem;
+  problem.equation.compute_acceleration =
+      [this, &intrinsic_acceleration](
+          Instant const& t,
+          std::vector<Position<Frame>> const& positions,
+          std::vector<Velocity<Frame>> const& velocities,
+          std::vector<Vector<Acceleration, Frame>>& accelerations) {
+        if (!ComputeMasslessBodiesGravitationalAccelerations(
+                t, positions, accelerations)) {
+          return Status(Error::OUT_OF_RANGE, "Collision detected");
+        }
+        accelerations[0] +=
+            intrinsic_acceleration(t, positions[0], velocities[0]);
+        return Status::OK;
+      };
+
+  auto const trajectory_last = trajectory->last();
+  auto const last_degrees_of_freedom = trajectory_last.degrees_of_freedom();
+  problem.initial_state = {{last_degrees_of_freedom.position()},
+                           {last_degrees_of_freedom.velocity()},
+                           trajectory_last.time()};
+
+  typename AdaptiveStepSizeIntegrator<ODE>::Parameters const
+      integrator_parameters(
+          /*first_time_step=*/t_final - problem.initial_state.time.value,
+          /*safety_factor=*/0.9,
+          parameters.max_steps_,
+          /*last_step_is_exact=*/true);
+  CHECK_GT(integrator_parameters.first_time_step, 0 * Second)
+      << "Flow back to the future: " << t_final
+      << " <= " << problem.initial_state.time.value;
+  auto const tolerance_to_error_ratio =
+      std::bind(&Ephemeris<Frame>::ToleranceToErrorRatio,
+                std::cref(parameters.length_integration_tolerance_),
+                std::cref(parameters.speed_integration_tolerance_),
+                _1, _2);
+
+  typename AdaptiveStepSizeIntegrator<ODE>::AppendState append_state;
+  typename ODE::SystemState last_state;
+  if (last_point_only) {
+    append_state = [&last_state](typename ODE::SystemState const& state) {
+      last_state = state;
+    };
+  } else {
+    append_state = std::bind(
+        &Ephemeris::AppendMasslessBodiesState, _1, std::cref(trajectories));
+  }
+
+  auto const instance =
+      EmbeddedExplicitGeneralizedRungeKuttaNyströmIntegrator<
+          Fine1987RKNG34, Position<Frame>>().NewInstance(
+              problem,
+              append_state,
+              tolerance_to_error_ratio,
+              integrator_parameters);
+  auto status = instance->Solve(t_final);
+
+  // We probably don't care if the vessel gets too close to the singularity, as
+  // we only use this integrator for the future.  So we swallow the error.
+  // TODO(phl): Is this the right thing to do long term?
+  if (status.error() == Error::OUT_OF_RANGE) {
+    status = Status::OK;
+  }
+
+  if (last_point_only) {
+    AppendMasslessBodiesState(last_state, trajectories);
+  }
+
+  // TODO(egg): when we have events in trajectories, we should add a singularity
+  // event at the end if the outcome indicates a singularity
+  // (|VanishingStepSize|).  We should not have an event on the trajectory if
+  // |ReachedMaximalStepCount|, since that is not a physical property, but
+  // rather a self-imposed constraint.
+  if (!status.ok() || t_final == t) {
+    return status;
+  } else {
+    return Status(Error::DEADLINE_EXCEEDED,
+                  "Couldn't reach " + DebugString(t_final) + ", stopping at " +
+                      DebugString(t));
+  }
+}
+
 template<typename Frame>
 Status Ephemeris<Frame>::FlowWithFixedStep(
     Instant const& t,