Merge pull request #2533 from eggrobin/bring-back-the-toggle

A more stable angular momentum correction.

ksp_plugin/interface_part.cpp

@@ -112,20 +112,17 @@ void __cdecl principia__PartSetApparentRigidMotion(
     PartId const part_id,
     QP const degrees_of_freedom,
     WXYZ const rotation,
-    XYZ const angular_velocity,
-    QP const main_body_degrees_of_freedom) {
+    XYZ const angular_velocity) {
   journal::Method<journal::PartSetApparentRigidMotion> m(
       {plugin,
        part_id,
        degrees_of_freedom,
        rotation,
-       angular_velocity,
-       main_body_degrees_of_freedom});
+       angular_velocity});
   CHECK_NOTNULL(plugin);
   plugin->SetPartApparentRigidMotion(
       part_id,
-      MakePartRigidMotion(degrees_of_freedom, rotation, angular_velocity),
-      FromQP<DegreesOfFreedom<World>>(main_body_degrees_of_freedom));
+      MakePartRigidMotion(degrees_of_freedom, rotation, angular_velocity));
   return m.Return();
 }
 

ksp_plugin/interface_vessel.cpp

@@ -195,5 +195,29 @@ XYZ __cdecl principia__VesselVelocity(Plugin const* const plugin,
   return m.Return(ToXYZ(plugin->VesselVelocity(vessel_guid)));
 }
 
+void __cdecl principia__SetAngularMomentumConservation(
+    bool const correct_orientation,
+    bool const correct_angular_velocity,
+    bool const thresholding) {
+  journal::Method<journal::SetAngularMomentumConservation> m(
+      {correct_orientation, correct_angular_velocity, thresholding});
+  ksp_plugin::PileUp::correct_orientation = correct_orientation;
+  ksp_plugin::PileUp::correct_angular_velocity = correct_angular_velocity;
+  ksp_plugin::PileUp::thresholding = thresholding;
+  return m.Return();
+}
+
+char const* __cdecl principia__VesselGetPileUpTrace(
+    Plugin const* const plugin,
+    char const* const vessel_guid) {
+  journal::Method<journal::VesselGetPileUpTrace> m({plugin, vessel_guid});
+  CHECK_NOTNULL(plugin);
+  char const* trace;
+  plugin->GetVessel(vessel_guid)->ForSomePart([&trace](ksp_plugin::Part& part) {
+    trace = part.containing_pile_up()->trace.data();
+  });
+  return m.Return(trace);
+}
+
 }  // namespace interface
 }  // namespace principia

ksp_plugin/pile_up.cpp

@@ -1,6 +1,7 @@
 
 #include "ksp_plugin/pile_up.hpp"
 
+#include <algorithm>
 #include <functional>
 #include <list>
 #include <map>
@@ -22,18 +23,25 @@ using base::make_not_null_unique;
 using geometry::AngularVelocity;
 using geometry::BarycentreCalculator;
 using geometry::Bivector;
+using geometry::Commutator;
 using geometry::Frame;
 using geometry::Identity;
 using geometry::NonRotating;
+using geometry::Normalize;
+using geometry::NormalizeOrZero;
 using geometry::OrthogonalMap;
 using geometry::Position;
+using geometry::Quaternion;
 using geometry::RigidTransformation;
 using geometry::Rotation;
 using geometry::Velocity;
 using geometry::Wedge;
 using physics::DegreesOfFreedom;
 using physics::RigidMotion;
+using quantities::Angle;
+using quantities::AngularFrequency;
 using quantities::AngularMomentum;
+using quantities::Time;
 using quantities::si::Radian;
 using ::std::placeholders::_1;
 using ::std::placeholders::_2;
@@ -419,44 +427,154 @@ void PileUp::DeformPileUpIfNeeded(Instant const& t) {
   // is unaffected by the apparent-bubble-to-pile-up correction, which is rigid
   // and involves no change in axes.
   auto const inertia_tensor = apparent_system.InertiaTensor();
+
+  // This is the axis around which we may perform an orientation correction.
+  // Identifying the uncorrected (apparent) and corrected coordinates, it is
+  // orthogonal to both.
+  // Note that the computations are identical in coordinates:
+  // |apparent_correction_axis.coordinates() ==
+  //  actual_correction_axis.coordinates()|.
+  auto const apparent_correction_axis = NormalizeOrZero(Commutator(
+      apparent_angular_momentum,
+      Identity<NonRotatingPileUp, ApparentPileUp>()(angular_momentum_)));
+  auto const actual_correction_axis = NormalizeOrZero(Commutator(
+      Identity<ApparentPileUp, NonRotatingPileUp>()(apparent_angular_momentum),
+      angular_momentum_));
+
+  // We apply a rigid rotational correction to the motions of the parts coming
+  // from the game (the apparent motions) so as to enforce the conservation of
+  // the angular momentum (|angular_momentum_| is authoritative).
+  // Mapping L_apparent to L_actual by a rigid motion can be done in many ways.
+  // We prefer doing some of that correction by a change of attitude, rather
+  // than solely by a change in angular velocity, since, under isotropic
+  // conditions, a change in attitude does not alter the physical system (and in
+  // particular it does not mess with symplectic integration).  We thus try to
+  // map L̂_apparent to L̂_actual, by rotation around the correction axis defined
+  // above, which is perpendicular to both, and to impart the appropriate
+  // angular velocity correction to correct the norm of L.
+  // This amounts to trusting the direction of the angular momentum with respect
+  // to the vessel as given to us by the game.
+  // However, mapping L̂_apparent to L̂_actual is essentially singular when either
+  // is 0.  We remedy to that by only performing an attitude correction if its
+  // angle would be less than ω Δt, where ω is the smaller of the two equivalent
+  // angular frequencies |L_actual / I| and |L_apparent / I|.
+  // As a result, as either L tends towards 0, so does the largest attitude
+  // correction that we allow.
+  // If the attitude correction would exceed this threshold, we leave the
+  // attitude unchanged, and the correction in angular momentum is effected
+  // solely by a change in angular velocity.
+
+  // The correction is computed via an intermediate frame.
+  // In the |EquivalentRigidPileUp| reference frame, a rigid body with the same
+  // inertia and angular momentum as the pile up would be immobile.
+  // If no attitude correction is performed, the axes of
+  // |EquivalentRigidPileUp|, |ApparentPileUp|, and |NonRotatingPileUp| are
+  // identical.
+  // If an attitude correction is performed, the y axis of
+  // |EquivalentRigidPileUp| is the direction of the angular momentum, and the x
+  // axis is the correction axis.
+  // NOTE(egg): while the correction axis is essentially singular at
+  // L_apparent = L_actual, the correction itself is only removably singular (as
+  // the angle goes to 0).  Since the computation ensured that
+  // |apparent_correction_axis.coordinates() ==
+  //  actual_correction_axis.coordinates()| exactly, we need not worry about the
+  // intermediate essential singularity, and only need to remove it.
+  using EquivalentRigidPileUp = Frame<enum class EquivalentRigidPileUpTag>;
+
+  auto const L̂_apparent = NormalizeOrZero(apparent_angular_momentum);
+  auto const L̂_actual = NormalizeOrZero(angular_momentum_);
+  // NOTE(egg):
+  // — The two sides of this disjunction are always equal;
+  // — technically this will also be true on the essential singularity.
+  bool const on_removable_singularity =
+      apparent_correction_axis == Bivector<double, ApparentPileUp>{} ||
+      actual_correction_axis == Bivector<double, NonRotatingPileUp>{};
+  bool const on_essential_singularity =
+      L̂_apparent == Bivector<double, ApparentPileUp>{} ||
+      L̂_actual == Bivector<double, NonRotatingPileUp>{};
+
+  bool const trivial_rotations = !correct_orientation ||
+                                 on_removable_singularity ||
+                                 on_essential_singularity;
+
+  Rotation<ApparentPileUp, EquivalentRigidPileUp>
+      apparent_pile_up_equivalent_rotation =
+          trivial_rotations
+              ? Rotation<ApparentPileUp, EquivalentRigidPileUp>::Identity()
+              : Rotation<ApparentPileUp, EquivalentRigidPileUp>(
+                    apparent_correction_axis,
+                    L̂_apparent,
+                    Commutator(apparent_correction_axis, L̂_apparent));
+  Rotation<NonRotatingPileUp, EquivalentRigidPileUp>
+      actual_pile_up_equivalent_rotation =
+          trivial_rotations
+              ? Rotation<NonRotatingPileUp, EquivalentRigidPileUp>::Identity()
+              : Rotation<NonRotatingPileUp, EquivalentRigidPileUp>(
+                    actual_correction_axis,
+                    L̂_actual,
+                    Commutator(actual_correction_axis, L̂_actual));
+  Rotation<ApparentPileUp, NonRotatingPileUp> const
+      tentative_attitude_correction =
+          actual_pile_up_equivalent_rotation.Inverse() *
+          apparent_pile_up_equivalent_rotation;
+
   // The angular velocity of a rigid body with the inertia and angular momentum
   // of the apparent parts.
   auto const apparent_equivalent_angular_velocity =
       apparent_angular_momentum / inertia_tensor;
   // The angular velocity of a rigid body with the inertia of the apparent
   // parts, and the angular momentum of the pile up.
-  auto const actual_equivalent_angular_velocity =
-      angular_momentum_ /
-      Identity<ApparentPileUp, NonRotatingPileUp>()(inertia_tensor);
+  auto actual_equivalent_angular_velocity =
+      angular_momentum_ / tentative_attitude_correction(inertia_tensor);
+
+  // So far we have only dealt with the essential singularity by removing it.
+  // We now need to deal with its neighbourhood, by ensuring that the tentative
+  // correction is not too big compared to the angular velocities involved.
+  Quaternion const q = tentative_attitude_correction.quaternion();
+  // α is the angle of the tentative attitude correction.
+  Angle const α =
+      2 * quantities::ArcTan(q.imaginary_part().Norm(), q.real_part());
+  AngularFrequency const ω =
+      std::min(apparent_equivalent_angular_velocity.Norm(),
+               actual_equivalent_angular_velocity.Norm());
+  Time const Δt = t - psychohistory_->back().time;
+  if (thresholding && α > ω * Δt) {
+    // The attitude correction is too large.  Preserve attitude.
+    apparent_pile_up_equivalent_rotation =
+        Rotation<ApparentPileUp, EquivalentRigidPileUp>::Identity();
+    actual_pile_up_equivalent_rotation =
+        Rotation<NonRotatingPileUp, EquivalentRigidPileUp>::Identity();
+    actual_equivalent_angular_velocity =
+        angular_momentum_ /
+        Identity<ApparentPileUp, NonRotatingPileUp>()(inertia_tensor);
+  }
+
+  bool const correcting_orientation =
+      apparent_pile_up_equivalent_rotation.quaternion() != Quaternion(1) ||
+      actual_pile_up_equivalent_rotation.quaternion() != Quaternion(1);
 
-  // In the |EquivalentRigidPileUp| reference frame, a rigid body with the same
-  // inertia and angular momentum as the pile up would be immobile.  We use this
-  // intermediate frame to apply a rigid rotational correction to the motions of
-  // the part coming from the game (the apparent motions) so as to enforce the
-  // conservation of the angular momentum (|angular_momentum_| is
-  // authoritative).
-  using EquivalentRigidPileUp = Frame<enum class EquivalentRigidPileUpTag>;
   RigidMotion<ApparentPileUp, EquivalentRigidPileUp> const
-      apparent_pile_up_equivalent_rotation(
+      apparent_pile_up_equivalent_motion(
           RigidTransformation<ApparentPileUp, EquivalentRigidPileUp>(
               ApparentPileUp::origin,
               EquivalentRigidPileUp::origin,
-              OrthogonalMap<ApparentPileUp, EquivalentRigidPileUp>::Identity()),
-          apparent_equivalent_angular_velocity,
+              apparent_pile_up_equivalent_rotation.Forget<OrthogonalMap>()),
+          correct_angular_velocity ? apparent_equivalent_angular_velocity
+                                   : ApparentPileUp::nonrotating,
           ApparentPileUp::unmoving);
   RigidMotion<NonRotatingPileUp, EquivalentRigidPileUp> const
-      actual_pile_up_equivalent_rotation(
+      actual_pile_up_equivalent_motion(
           RigidTransformation<NonRotatingPileUp, EquivalentRigidPileUp>(
               NonRotatingPileUp::origin,
               EquivalentRigidPileUp::origin,
-              OrthogonalMap<NonRotatingPileUp,
-                            EquivalentRigidPileUp>::Identity()),
-          actual_equivalent_angular_velocity,
+              actual_pile_up_equivalent_rotation.Forget<OrthogonalMap>()),
+          correct_angular_velocity ? actual_equivalent_angular_velocity
+                                   : NonRotatingPileUp::nonrotating,
           NonRotatingPileUp::unmoving);
   RigidMotion<ApparentBubble, NonRotatingPileUp> const
       apparent_bubble_to_pile_up_motion =
-          actual_pile_up_equivalent_rotation.Inverse() *
-          apparent_pile_up_equivalent_rotation *
+          actual_pile_up_equivalent_motion.Inverse() *
+          apparent_pile_up_equivalent_motion *
           apparent_system.LinearMotion().Inverse();
 
   // Now update the motions of the parts in the pile-up frame.
@@ -469,6 +587,35 @@ void PileUp::DeformPileUpIfNeeded(Instant const& t) {
   }
   apparent_part_rigid_motion_.clear();
 
+  std::stringstream s;
+  s << "Apparent: " << apparent_angular_momentum << "\n"
+    << "norm: " << apparent_angular_momentum.Norm() << "\n"
+    << "Actual: " << angular_momentum_ << "\n"
+    << "norm: " << angular_momentum_.Norm() << "\n"
+    << "|Lap-Lac|: "
+    << (angular_momentum_ - Identity<ApparentPileUp, NonRotatingPileUp>()(
+                                apparent_angular_momentum))
+           .Norm() << "\n"
+    << "|Lap|-|Lac|: "
+    << angular_momentum_.Norm() - apparent_angular_momentum.Norm() << "\n"
+    << u8"∡Lap, Lac: "
+    << geometry::AngleBetween(angular_momentum_,
+                              Identity<ApparentPileUp, NonRotatingPileUp>()(
+                                  apparent_angular_momentum)) /
+           quantities::si::Degree
+    << u8"°\n"
+    << u8"α: " << α / quantities::si::Degree << u8"°\n"
+    << u8"|ωap|: "
+    << apparent_equivalent_angular_velocity.Norm() /
+           (2 * π * Radian / quantities::si::Minute)
+    << " rpm\n"
+    << u8"|ωac|: "
+    << actual_equivalent_angular_velocity.Norm() /
+           (2 * π * Radian / quantities::si::Minute)
+    << " rpm\n"
+    << (correcting_orientation ? "CORRECTING ORIENTATION" : u8"—");
+  trace = s.str();
+
   MakeEulerSolver(Identity<ApparentPileUp, NonRotatingPileUp>()(inertia_tensor),
                   t);
 }
@@ -595,6 +742,10 @@ PileUpFuture::PileUpFuture(not_null<PileUp const*> const pile_up,
     : pile_up(pile_up),
       future(std::move(future)) {}
 
+bool PileUp::correct_orientation = true;
+bool PileUp::correct_angular_velocity = true;
+bool PileUp::thresholding = true;
+
 }  // namespace internal_pile_up
 }  // namespace ksp_plugin
 }  // namespace principia

ksp_plugin/pile_up.hpp

@@ -94,6 +94,11 @@ class PileUp {
 
   virtual ~PileUp();
 
+  std::string trace;
+  static bool correct_orientation;
+  static bool correct_angular_velocity;
+  static bool thresholding;
+
   // This class is moveable.
   PileUp(PileUp&& pile_up) = default;
   PileUp& operator=(PileUp&& pile_up) = default;

ksp_plugin/plugin.cpp

@@ -656,19 +656,25 @@ void Plugin::FreeVesselsAndPartsAndCollectPileUps(Time const& Δt) {
 
 void Plugin::SetPartApparentRigidMotion(
     PartId const part_id,
-    RigidMotion<RigidPart, World> const& rigid_motion,
-    DegreesOfFreedom<World> const& main_body_degrees_of_freedom) {
-  // Define |ApparentBubble| as the reference frame with the axes of
-  // |Barycentric| centred on the current main body.
+    RigidMotion<RigidPart, World> const& rigid_motion) {
+  // As a reference frame, |ApparentBubble| differs from |World| only by having
+  // the same axes as |Barycentric| and being nonrotating.  However, there is
+  // another semantic distinction: |Apparent...| coordinates are uncorrected
+  // data from the game, given immediately after its physics step; before using
+  // them, we must correct them in accordance with the data computed by the pile
+  // up.  This correction overrides the origin of position and velocity, so we
+  // need not worry about the current definition of
+  // |{World::origin, World::unmoving}| as we do when getting the actual degrees
+  // of freedom (via |Plugin::BarycentricToWorld|).
   RigidMotion<World, ApparentBubble> world_to_apparent_bubble{
       RigidTransformation<World, ApparentBubble>{
-          main_body_degrees_of_freedom.position(),
+          World::origin,
           ApparentBubble::origin,
           OrthogonalMap<Barycentric, ApparentBubble>::Identity() *
               renderer_->WorldToBarycentric(PlanetariumRotation())},
       renderer_->BarycentricToWorld(PlanetariumRotation())(
           -angular_velocity_of_world_),
-      main_body_degrees_of_freedom.velocity()};
+      World::unmoving};
 
   not_null<Vessel*> vessel = FindOrDie(part_id_to_vessel_, part_id);
   CHECK(is_loaded(vessel));

ksp_plugin/plugin.hpp

@@ -254,8 +254,7 @@ class Plugin {
   // part.  This part must be in a loaded vessel.
   virtual void SetPartApparentRigidMotion(
       PartId part_id,
-      RigidMotion<RigidPart, World> const& rigid_motion,
-      DegreesOfFreedom<World> const& main_body_degrees_of_freedom);
+      RigidMotion<RigidPart, World> const& rigid_motion);
 
   // Returns the motion of the given part in |World|, assuming that
   // the origin of |World| is fixed at the centre of mass of the

ksp_plugin_adapter/ksp_plugin_adapter.cs

@@ -1251,8 +1251,7 @@ private System.Collections.IEnumerator WaitedForFixedUpdate() {
                 new QP{q = (XYZ)(Vector3d)part.rb.position,
                        p = (XYZ)(Vector3d)part.rb.velocity},
                 (WXYZ)(UnityEngine.QuaternionD)part.rb.rotation,
-                (XYZ)(Vector3d)part.rb.angularVelocity,
-                main_body_degrees_of_freedom);
+                (XYZ)(Vector3d)part.rb.angularVelocity);
         }
       }
     }