Merge pull request #4619 from pleroy/4608

Add support for specifying the burn in spherical coordinates to the C++ code

Author: pleroy

geometry/permutation.hpp

@@ -69,6 +69,9 @@ class Permutation : public LinearMap<Permutation<FromFrame, ToFrame>,
 
   Sign Determinant() const;
 
+  // Only use for interchange.
+  CoordinatePermutation coordinate_permutation() const;
+
   Permutation<ToFrame, FromFrame> Inverse() const;
 
   template<typename Scalar>

geometry/permutation_body.hpp

@@ -29,10 +29,16 @@ Permutation<FromFrame, ToFrame>::Permutation(
     : coordinate_permutation_(coordinate_permutation) {}
 
 template<typename FromFrame, typename ToFrame>
-inline Sign Permutation<FromFrame, ToFrame>::Determinant() const {
+Sign Permutation<FromFrame, ToFrame>::Determinant() const {
   return Sign::OfNonZero(static_cast<int>(coordinate_permutation_));
 }
 
+template<typename FromFrame, typename ToFrame>
+typename Permutation<FromFrame, ToFrame>::CoordinatePermutation
+Permutation<FromFrame, ToFrame>::coordinate_permutation() const {
+  return coordinate_permutation_;
+}
+
 template<typename FromFrame, typename ToFrame>
 Permutation<ToFrame, FromFrame>
 Permutation<FromFrame, ToFrame>::Inverse() const {

geometry/r3_element.hpp

@@ -99,7 +99,12 @@ struct SphericalCoordinates final {
 
   // Uses the x-y plane as the equator, the x-axis as the reference direction on
   // the equator, and the z-axis as the north pole.
-  R3Element<Scalar> ToCartesian();
+  R3Element<Scalar> ToCartesian() const;
+
+  void WriteToMessage(
+      not_null<serialization::SphericalCoordinates*> message) const;
+  static SphericalCoordinates ReadFromMessage(
+      serialization::SphericalCoordinates const& message);
 
   Scalar radius;
   Angle latitude;

geometry/r3_element_body.hpp

@@ -210,14 +210,32 @@ R3Element<Scalar> R3Element<Scalar>::ReadFromMessage(
 }
 
 template<typename Scalar>
-R3Element<Scalar> SphericalCoordinates<Scalar>::ToCartesian() {
+R3Element<Scalar> SphericalCoordinates<Scalar>::ToCartesian() const {
   auto const [sin_latitude, cos_latitude] = SinCos(latitude);
   auto const [sin_longitude, cos_longitude] = SinCos(longitude);
   return {radius * cos_longitude * cos_latitude,
           radius * sin_longitude * cos_latitude,
           radius * sin_latitude};
 }
 
+template<typename Scalar>
+void SphericalCoordinates<Scalar>::WriteToMessage(
+    not_null<serialization::SphericalCoordinates*> const message) const {
+  radius.WriteToMessage(message->mutable_radius());
+  latitude.WriteToMessage(message->mutable_latitude());
+  longitude.WriteToMessage(message->mutable_longitude());
+}
+
+template<typename Scalar>
+SphericalCoordinates<Scalar> SphericalCoordinates<Scalar>::ReadFromMessage(
+    serialization::SphericalCoordinates const& message) {
+  SphericalCoordinates<Scalar> result;
+  result.radius = Scalar::ReadFromMessage(message.radius());
+  result.latitude = Angle::ReadFromMessage(message.latitude());
+  result.longitude = Angle::ReadFromMessage(message.longitude());
+  return result;
+}
+
 template<typename Scalar>
 SphericalCoordinates<Scalar> RadiusLatitudeLongitude(Scalar const& radius,
                                                      Angle const& latitude,

journal/profiles.hpp

@@ -33,9 +33,9 @@ using interface::ConfigurationAdaptiveStepParameters;
 using interface::ConfigurationDownsamplingParameters;
 using interface::ConfigurationFixedStepParameters;
 using interface::CoordinateSystem;
-using interface::DeltaV;
 using interface::EquatorialCrossings;
 using interface::FlightPlanAdaptiveStepParameters;
+using interface::Intensity;
 using interface::Interval;
 using interface::Iterator;
 using interface::KeplerianElements;

ksp_plugin/flight_plan_optimizer.cpp

@@ -784,7 +784,7 @@ NavigationManœuvre::Burn FlightPlanOptimizer::UpdatedBurn(
     NavigationManœuvre const& manœuvre) {
   auto const argument = Dehomogeneize(homogeneous_argument);
   NavigationManœuvre::Burn burn = manœuvre.burn();
-  burn.intensity = {.Δv = manœuvre.Δv() + argument.ΔΔv};
+  burn.intensity.set_Δv(burn.intensity.Δv() + argument.ΔΔv);
   burn.timing = {.initial_time =
                      manœuvre.initial_time() + argument.Δinitial_time};
   return burn;

ksp_plugin/interface.hpp

@@ -87,11 +87,11 @@ std::unique_ptr<T[]> TakeOwnershipArray(T** pointer);
 bool operator==(AdaptiveStepParameters const& left,
                 AdaptiveStepParameters const& right);
 bool operator==(Burn const& left, Burn const& right);
-bool operator==(DeltaV const& left, DeltaV const& right);
 bool operator==(EquatorialCrossings const& left,
                 EquatorialCrossings const& right);
 bool operator==(FlightPlanAdaptiveStepParameters const& left,
                 FlightPlanAdaptiveStepParameters const& right);
+bool operator==(Intensity const& left, Intensity const& right);
 bool operator==(Interval const& left, Interval const& right);
 bool operator==(NavigationFrameParameters const& left,
                 NavigationFrameParameters const& right);
@@ -165,6 +165,8 @@ FlightPlanAdaptiveStepParameters ToFlightPlanAdaptiveStepParameters(
         Barycentric>::GeneralizedAdaptiveStepParameters const&
         generalized_adaptive_step_parameters);
 
+Intensity ToIntensity(NavigationManœuvre::Intensity const& intensity);
+
 KeplerianElements ToKeplerianElements(
     physics::_kepler_orbit::KeplerianElements<Barycentric> const&
         keplerian_elements);
@@ -178,11 +180,17 @@ QP ToQP(RelativeDegreesOfFreedom<AliceSun> const& relative_dof);
 // Ownership of the status and its message is transferred to the caller.
 Status* ToNewStatus(absl::Status const& status);
 
+// Ownership of the object is transferred to the caller.
+SphericalCoordinates* ToNewSphericalCoordinates(
+    geometry::_r3_element::SphericalCoordinates<Speed> const&
+        spherical_coordinates);
+
 WXYZ ToWXYZ(Quaternion const& quaternion);
 
 XY ToXY(RP2Point<Length, Camera> const& rp2_point);
 
 XYZ ToXYZ(R3Element<double> const& r3_element);
+XYZ ToXYZ(R3Element<Speed> const& r3_element);
 XYZ ToXYZ(Position<World> const& position);
 XYZ ToXYZ(Vector<double, World> const& direction);
 XYZ ToXYZ(Velocity<Frenet<NavigationFrame>> const& velocity);

ksp_plugin/interface_body.hpp

@@ -13,6 +13,7 @@
 #include "absl/strings/str_split.h"
 #include "base/array.hpp"
 #include "geometry/orthogonal_map.hpp"
+#include "geometry/permutation.hpp"
 #include "geometry/r3x3_matrix.hpp"
 #include "geometry/rotation.hpp"
 #include "geometry/sign.hpp"
@@ -33,6 +34,7 @@ namespace interface {
 
 using namespace principia::base::_array;
 using namespace principia::geometry::_orthogonal_map;
+using namespace principia::geometry::_permutation;
 using namespace principia::geometry::_r3x3_matrix;
 using namespace principia::geometry::_rotation;
 using namespace principia::geometry::_sign;
@@ -168,6 +170,16 @@ struct XYZConverter<R3Element<MomentOfInertia>> {
   }
 };
 
+template<>
+struct XYZConverter<R3Element<Speed>> {
+  static constexpr Speed mts_unit = Metre / Second;
+  static R3Element<Speed> FromXYZ(XYZ const& xyz) {
+    return R3Element<Speed>(xyz.x * (Metre / Second),
+                            xyz.y * (Metre / Second),
+                            xyz.z * (Metre / Second));
+  }
+};
+
 inline bool NaNIndependentEq(double const left, double const right) {
   return (left == right) || (std::isnan(left) && std::isnan(right));
 }
@@ -205,16 +217,7 @@ inline bool operator==(Burn const& left, Burn const& right) {
                           right.specific_impulse_in_seconds_g0) &&
          left.frame == right.frame &&
          NaNIndependentEq(left.initial_time, right.initial_time) &&
-         left.delta_v == right.delta_v;
-}
-
-inline bool operator==(DeltaV const& left, DeltaV const& right) {
-  return left.coordinate_system == right.coordinate_system &&
-         ((left.spherical_coordinates != nullptr &&
-           right.spherical_coordinates != nullptr &&
-           *left.spherical_coordinates == *right.spherical_coordinates) ||
-          (left.xyz != nullptr && right.xyz != nullptr &&
-           *left.xyz == *right.xyz));
+         left.intensity == right.intensity;
 }
 
 inline bool operator==(FlightPlanAdaptiveStepParameters const& left,
@@ -229,6 +232,15 @@ inline bool operator==(FlightPlanAdaptiveStepParameters const& left,
                           right.speed_integration_tolerance);
 }
 
+inline bool operator==(Intensity const& left, Intensity const& right) {
+  return left.coordinate_system == right.coordinate_system &&
+         ((left.spherical_coordinates != nullptr &&
+           right.spherical_coordinates != nullptr &&
+           *left.spherical_coordinates == *right.spherical_coordinates) ||
+          (left.xyz != nullptr && right.xyz != nullptr &&
+           *left.xyz == *right.xyz));
+}
+
 inline bool operator==(Interval const& left, Interval const& right) {
   return NaNIndependentEq(left.min, right.min) &&
          NaNIndependentEq(left.max, right.max);
@@ -522,6 +534,12 @@ inline FromXYZ<R3Element<MomentOfInertia>>(XYZ const& xyz) {
   return XYZConverter<R3Element<MomentOfInertia>>::FromXYZ(xyz);
 }
 
+template<>
+R3Element<Speed>
+inline FromXYZ<R3Element<Speed>>(XYZ const& xyz) {
+  return XYZConverter<R3Element<Speed>>::FromXYZ(xyz);
+}
+
 inline AdaptiveStepParameters ToAdaptiveStepParameters(
     physics::_ephemeris::Ephemeris<Barycentric>::AdaptiveStepParameters const&
         adaptive_step_parameters) {
@@ -557,6 +575,33 @@ inline FlightPlanAdaptiveStepParameters ToFlightPlanAdaptiveStepParameters(
               (Metre / Second)};
 }
 
+inline Intensity ToIntensity(NavigationManœuvre::Intensity const& intensity) {
+  if (intensity.has_spherical_coordinates()) {
+    switch (intensity.permutation().coordinate_permutation()) {
+      case EvenPermutation::XYZ:
+        return {.coordinate_system = CoordinateSystem::SPHERICAL_TNB,
+                .xyz = nullptr,
+                .spherical_coordinates = ToNewSphericalCoordinates(
+                    intensity.Δv_spherical_coordinates())};
+      case EvenPermutation::YZX:
+        return {.coordinate_system = CoordinateSystem::SPHERICAL_NBT,
+                .xyz = nullptr,
+                .spherical_coordinates = ToNewSphericalCoordinates(
+                    intensity.Δv_spherical_coordinates())};
+      case EvenPermutation::ZXY:
+        return {.coordinate_system = CoordinateSystem::SPHERICAL_BTN,
+                .xyz = nullptr,
+                .spherical_coordinates = ToNewSphericalCoordinates(
+                    intensity.Δv_spherical_coordinates())};
+    }
+    LOG(FATAL) << "Unexpected permutation: " << intensity.permutation();
+  } else {
+    return {.coordinate_system = CoordinateSystem::CARTESIAN_TNB,
+            .xyz = new XYZ(ToXYZ(intensity.Δv_cartesian_coordinates())),
+            .spherical_coordinates = nullptr};
+  }
+}
+
 inline KeplerianElements ToKeplerianElements(
     physics::_kepler_orbit::KeplerianElements<Barycentric> const&
         keplerian_elements) {
@@ -636,6 +681,15 @@ inline Status* ToNewStatus(absl::Status const& status) {
   }
 }
 
+inline SphericalCoordinates* ToNewSphericalCoordinates(
+    geometry::_r3_element::SphericalCoordinates<Speed> const&
+        spherical_coordinates) {
+  return new SphericalCoordinates{
+      .radius = spherical_coordinates.radius / (Metre / Second),
+      .latitude_in_degrees = spherical_coordinates.latitude / Degree,
+      .longitude_in_degrees = spherical_coordinates.longitude / Degree};
+}
+
 inline WXYZ ToWXYZ(Quaternion const& quaternion) {
   return {.w = quaternion.real_part(),
           .x = quaternion.imaginary_part().x,
@@ -651,6 +705,12 @@ inline XYZ ToXYZ(R3Element<double> const& r3_element) {
   return {.x = r3_element.x, .y = r3_element.y, .z = r3_element.z};
 }
 
+inline XYZ ToXYZ(R3Element<Speed> const& r3_element) {
+  return {.x = r3_element.x / (Metre / Second),
+          .y = r3_element.y / (Metre / Second),
+          .z = r3_element.z / (Metre / Second)};
+}
+
 inline XYZ ToXYZ(Position<World> const& position) {
   return XYZConverter<Position<World>>::ToXYZ(position);
 }

ksp_plugin/interface_flight_plan.cpp

@@ -9,6 +9,8 @@
 #include "base/not_null.hpp"
 #include "geometry/grassmann.hpp"
 #include "geometry/orthogonal_map.hpp"
+#include "geometry/permutation.hpp"
+#include "geometry/r3_element.hpp"
 #include "journal/method.hpp"
 #include "journal/profiles.hpp"  // 🧙 For generated profiles.
 #include "ksp_plugin/flight_plan.hpp"
@@ -34,6 +36,8 @@ namespace interface {
 using namespace principia::base::_not_null;
 using namespace principia::geometry::_grassmann;
 using namespace principia::geometry::_orthogonal_map;
+using namespace principia::geometry::_permutation;
+using namespace principia::geometry::_r3_element;
 using namespace principia::journal::_method;
 using namespace principia::ksp_plugin::_flight_plan;
 using namespace principia::ksp_plugin::_flight_plan_optimization_driver;
@@ -56,11 +60,43 @@ namespace {
 
 NavigationManœuvre::Burn FromInterfaceBurn(Plugin const& plugin,
                                            Burn const& burn) {
-  NavigationManœuvre::Intensity intensity;
-  intensity.Δv = FromXYZ<Velocity<Frenet<NavigationFrame>>>(burn.delta_v);
+  std::optional<NavigationManœuvre::Intensity> navigation_manœuvre_intensity;
+  auto const& intensity = burn.intensity;
+  switch (intensity.coordinate_system) {
+    case CoordinateSystem::CARTESIAN_TNB: {
+      navigation_manœuvre_intensity = NavigationManœuvre::Intensity(
+          FromXYZ<R3Element<Speed>>(*intensity.xyz));
+      break;
+    }
+    case CoordinateSystem::SPHERICAL_TNB:
+    case CoordinateSystem::SPHERICAL_NBT:
+    case CoordinateSystem::SPHERICAL_BTN: {
+      EvenPermutation coordinate_permutation;
+      switch (intensity.coordinate_system) {
+        case CoordinateSystem::SPHERICAL_TNB:
+          coordinate_permutation = EvenPermutation::XYZ;
+          break;
+        case CoordinateSystem::SPHERICAL_BTN:
+          coordinate_permutation = EvenPermutation::YZX;
+          break;
+        case CoordinateSystem::SPHERICAL_NBT:
+          coordinate_permutation = EvenPermutation::ZXY;
+          break;
+      }
+      auto const& spherical_coordinates = *intensity.spherical_coordinates;
+      navigation_manœuvre_intensity = NavigationManœuvre::Intensity(
+          Permutation<PermutedFrenet<Navigation>, Frenet<Navigation>>(
+              coordinate_permutation),
+          RadiusLatitudeLongitude<Speed>(
+              spherical_coordinates.radius * (Metre / Second),
+              spherical_coordinates.latitude_in_degrees * Degree,
+              spherical_coordinates.longitude_in_degrees * Degree));
+      break;
+    }
+  }
   NavigationManœuvre::Timing timing;
   timing.initial_time = FromGameTime(plugin, burn.initial_time);
-  return {.intensity = intensity,
+  return {.intensity = *navigation_manœuvre_intensity,
           .timing = timing,
           .thrust = burn.thrust_in_kilonewtons * Kilo(Newton),
           .specific_impulse =
@@ -149,7 +185,7 @@ Burn GetBurn(Plugin const& plugin,
               manœuvre.specific_impulse() / (Second * StandardGravity),
           .frame = parameters,
           .initial_time = ToGameTime(plugin, manœuvre.initial_time()),
-          .delta_v = ToXYZ(manœuvre.Δv()),
+          .intensity = ToIntensity(manœuvre.intensity()),
           .is_inertially_fixed = manœuvre.is_inertially_fixed()};
 }