Skip to content

Commit

Permalink
Merge pull request #2475 from eggrobin/euler
Browse files Browse the repository at this point in the history
Euler
pleroy authored Feb 22, 2020
2 parents b5dd83a + 3228cd2 commit 8dca142
Showing 13 changed files with 150 additions and 30 deletions.
4 changes: 3 additions & 1 deletion geometry/r3x3_matrix.hpp
Original file line number Diff line number Diff line change
@@ -37,7 +37,9 @@ class R3x3Matrix final {
R3Element<Scalar> const& row_y,
R3Element<Scalar> const& row_z);

static R3x3Matrix Diagonal(R3Element<Scalar> const& diagonal);
static R3x3Matrix DiagonalMatrix(R3Element<Scalar> const& diagonal);

R3Element<Scalar> Diagonal() const;

Scalar Trace() const;
Cube<Scalar> Determinant() const;
7 changes: 6 additions & 1 deletion geometry/r3x3_matrix_body.hpp
Original file line number Diff line number Diff line change
@@ -30,7 +30,7 @@ R3x3Matrix<Scalar>::R3x3Matrix(R3Element<Scalar> const& row_x,
: rows_({row_x, row_y, row_z}) {}

template<typename Scalar>
R3x3Matrix<Scalar> R3x3Matrix<Scalar>::Diagonal(
R3x3Matrix<Scalar> R3x3Matrix<Scalar>::DiagonalMatrix(
R3Element<Scalar> const& diagonal) {
return {
{diagonal.x, {}, {}},
@@ -39,6 +39,11 @@ R3x3Matrix<Scalar> R3x3Matrix<Scalar>::Diagonal(
};
}

template<typename Scalar>
R3Element<Scalar> R3x3Matrix<Scalar>::Diagonal() const {
return {rows_[X].x, rows_[Y].y, rows_[Z].z};
}

template<typename Scalar>
Scalar R3x3Matrix<Scalar>::Trace() const {
return rows_[X].x + rows_[Y].y + rows_[Z].z;
2 changes: 2 additions & 0 deletions ksp_plugin/ksp_plugin.vcxproj
Original file line number Diff line number Diff line change
@@ -57,6 +57,8 @@
<ClCompile Include="..\journal\profiles.cpp" />
<ClCompile Include="..\journal\recorder.cpp" />
<ClCompile Include="..\numerics\cbrt.cpp" />
<ClCompile Include="..\numerics\elliptic_functions.cpp" />
<ClCompile Include="..\numerics\elliptic_integrals.cpp" />
<ClCompile Include="..\physics\protector.cpp" />
<ClCompile Include="celestial.cpp" />
<ClCompile Include="equator_relevance_threshold.cpp" />
6 changes: 6 additions & 0 deletions ksp_plugin/ksp_plugin.vcxproj.filters
Original file line number Diff line number Diff line change
@@ -169,6 +169,12 @@
<ClCompile Include="interface_part.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="..\numerics\elliptic_functions.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="..\numerics\elliptic_integrals.cpp">
<Filter>Source Files</Filter>
</ClCompile>
</ItemGroup>
<ItemGroup>
<CustomBuild Include="..\serialization\journal.proto" />
67 changes: 62 additions & 5 deletions ksp_plugin/pile_up.cpp
Original file line number Diff line number Diff line change
@@ -27,6 +27,7 @@ using geometry::NonRotating;
using geometry::OrthogonalMap;
using geometry::Position;
using geometry::RigidTransformation;
using geometry::Rotation;
using geometry::Velocity;
using geometry::Wedge;
using physics::DegreesOfFreedom;
@@ -69,6 +70,8 @@ PileUp::PileUp(
actual_part_rigid_motion_.emplace(
part, barycentric_to_pile_up * part->rigid_motion());
}
MakeEulerSolver(mechanical_system.InertiaTensor(), t);

psychohistory_ = history_->NewForkAtLast();

RecomputeFromParts();
@@ -98,7 +101,9 @@ Status PileUp::DeformAndAdvanceTime(Instant const& t) {
absl::MutexLock l(lock_.get());
Status status;
if (psychohistory_->back().time < t) {
DeformPileUpIfNeeded();
angular_momentum_ += intrinsic_torque_ * (t - psychohistory_->back().time) +
angular_momentum_change_;
DeformPileUpIfNeeded(t);
status = AdvanceTime(t);
NudgeParts();
}
@@ -274,6 +279,15 @@ not_null<std::unique_ptr<PileUp>> PileUp::ReadFromMessage(
}
}
pile_up->RecomputeFromParts();
// TODO(phl): the Euler solver should be serialized; when we don't have one we
// should compute it from an actual inertia tensor.
pile_up->MakeEulerSolver(
InertiaTensor<NonRotatingPileUp>(
geometry::R3x3Matrix<quantities::MomentOfInertia>::DiagonalMatrix(
{1000 * quantities::SIUnit<quantities::MomentOfInertia>(),
1729 * quantities::SIUnit<quantities::MomentOfInertia>(),
355 * quantities::SIUnit<quantities::MomentOfInertia>()})),
pile_up->psychohistory_->back().time);
return check_not_null(std::move(pile_up));
}

@@ -295,8 +309,51 @@ PileUp::PileUp(
psychohistory_(psychohistory),
deletion_callback_(std::move(deletion_callback)) {}

void PileUp::DeformPileUpIfNeeded() {
void PileUp::MakeEulerSolver(
InertiaTensor<NonRotatingPileUp> const& inertia_tensor,
Instant const& t) {
auto const eigensystem = inertia_tensor.Diagonalize<PileUpPrincipalAxes>();
euler_solver_.emplace(eigensystem.form.coordinates().Diagonal(),
angular_momentum_,
eigensystem.rotation,
t);
RigidTransformation<NonRotatingPileUp, PileUpPrincipalAxes> const
to_pile_up_principal_axes(
NonRotatingPileUp::origin,
PileUpPrincipalAxes::origin,
eigensystem.rotation.Inverse().Forget<OrthogonalMap>());
rigid_pile_up_.clear();
for (auto const& [part, actual_rigid_motion] : actual_part_rigid_motion_) {
rigid_pile_up_.emplace(
part,
to_pile_up_principal_axes * actual_rigid_motion.rigid_transformation());
}
}

void PileUp::DeformPileUpIfNeeded(Instant const& t) {
if (apparent_part_rigid_motion_.empty()) {
Bivector<AngularMomentum, PileUpPrincipalAxes> const angular_momentum =
euler_solver_->AngularMomentumAt(t);
Rotation<PileUpPrincipalAxes, NonRotatingPileUp> const attitude =
euler_solver_->AttitudeAt(angular_momentum, t);
AngularVelocity<NonRotatingPileUp> const angular_velocity_of_rigid_pile_up =
attitude(euler_solver_->AngularVelocityFor(angular_momentum));

RigidMotion<PileUpPrincipalAxes, NonRotatingPileUp> const pile_up_motion(
RigidTransformation<PileUpPrincipalAxes, NonRotatingPileUp>(
PileUpPrincipalAxes::origin,
NonRotatingPileUp::origin,
attitude.Forget<OrthogonalMap>()),
angular_velocity_of_rigid_pile_up,
NonRotatingPileUp::unmoving);

for (auto& [part, actual_rigid_motion] : actual_part_rigid_motion_) {
actual_rigid_motion =
pile_up_motion * RigidMotion<RigidPart, PileUpPrincipalAxes>(
rigid_pile_up_.at(part),
PileUpPrincipalAxes::nonrotating,
PileUpPrincipalAxes::unmoving);
}
return;
}
// A consistency check that |SetPartApparentDegreesOfFreedom| was called for
@@ -365,7 +422,6 @@ void PileUp::DeformPileUpIfNeeded() {
apparent_pile_up_equivalent_rotation *
apparent_system.LinearMotion().Inverse();

using PileUpPrincipalAxes = Frame<enum class PileUpPrincipalAxesTag>;
trace << "rotational correction:\n"
<< (actual_pile_up_equivalent_rotation.Inverse() *
apparent_pile_up_equivalent_rotation)
@@ -398,15 +454,16 @@ void PileUp::DeformPileUpIfNeeded() {
part, apparent_bubble_to_pile_up_motion * apparent_part_rigid_motion);
}
apparent_part_rigid_motion_.clear();

MakeEulerSolver(Identity<ApparentPileUp, NonRotatingPileUp>()(inertia_tensor),
t);
}

Status PileUp::AdvanceTime(Instant const& t) {
CHECK_NOTNULL(psychohistory_);

Status status;
auto const history_last = --history_->end();
angular_momentum_ += intrinsic_torque_ * (t - psychohistory_->back().time) +
angular_momentum_change_;
if (intrinsic_force_ == Vector<Force, Barycentric>{}) {
// Remove the fork.
history_->DeleteFork(psychohistory_);
26 changes: 20 additions & 6 deletions ksp_plugin/pile_up.hpp
Original file line number Diff line number Diff line change
@@ -6,6 +6,7 @@
#include <list>
#include <map>
#include <memory>
#include <optional>
#include <string>

#include "absl/synchronization/mutex.h"
@@ -17,6 +18,7 @@
#include "integrators/integrators.hpp"
#include "physics/discrete_trajectory.hpp"
#include "physics/ephemeris.hpp"
#include "physics/euler_solver.hpp"
#include "physics/massless_body.hpp"
#include "physics/mechanical_system.hpp"
#include "physics/rigid_motion.hpp"
@@ -41,11 +43,13 @@ using geometry::Handedness;
using geometry::InertiaTensor;
using geometry::Instant;
using geometry::NonRotating;
using geometry::RigidTransformation;
using geometry::Vector;
using integrators::Integrator;
using physics::DiscreteTrajectory;
using physics::DegreesOfFreedom;
using physics::Ephemeris;
using physics::EulerSolver;
using physics::MasslessBody;
using physics::MechanicalSystem;
using physics::RelativeDegreesOfFreedom;
@@ -68,6 +72,13 @@ using NonRotatingPileUp = Frame<serialization::Frame::PluginTag,
Handedness::Right,
serialization::Frame::NON_ROTATING_PILE_UP>;

// The origin of |PileUpPrincipalAxes| is the centre of mass of the pile up. Its
// axes are instantaneous principal axes of the pile up.
using PileUpPrincipalAxes = Frame<serialization::Frame::PluginTag,
Arbitrary,
Handedness::Right,
serialization::Frame::PILE_UP_PRINCIPAL_AXES>;

// A |PileUp| handles a connected component of the graph of |Parts| under
// physical contact. It advances the history and psychohistory of its component
// |Parts|, modeling them as a massless body at their centre of mass.
@@ -145,16 +156,17 @@ class PileUp {
not_null<Ephemeris<Barycentric>*> ephemeris,
std::function<void()> deletion_callback);

// Sets |euler_solver_| and updates |rigid_pile_up_|.
void MakeEulerSolver(InertiaTensor<NonRotatingPileUp> const& inertia_tensor,
Instant const& t);

// Update the degrees of freedom (in |NonRotatingPileUp|) of all the parts by
// translating the *apparent* degrees of freedom so that their centre of mass
// matches that computed by integration.
// |SetPartApparentDegreesOfFreedom| must have been called for each part in
// the pile-up, or for none.
// The degrees of freedom set by this method are used by |NudgeParts|.
// NOTE(egg): Eventually, this will also change their velocities and angular
// velocities so that the angular momentum matches that which has been
// computed for |this| |PileUp|.
void DeformPileUpIfNeeded();
void DeformPileUpIfNeeded(Instant const& t);

// Flows the history authoritatively as far as possible up to |t|, advances
// the histories of the parts and updates the degrees of freedom of the parts
@@ -219,11 +231,13 @@ class PileUp {
Ephemeris<Barycentric>::NewtonianMotionEquation>::Instance>
fixed_instance_;

// TODO(phl): Use |RigidPileUp|, a reference frame whose axes are the
// principal axes of the pile up.
PartTo<RigidMotion<RigidPart, NonRotatingPileUp>> actual_part_rigid_motion_;
PartTo<RigidMotion<RigidPart, ApparentBubble>> apparent_part_rigid_motion_;

PartTo<RigidTransformation<RigidPart, PileUpPrincipalAxes>> rigid_pile_up_;
std::optional<EulerSolver<NonRotatingPileUp, PileUpPrincipalAxes>>
euler_solver_;

// Called in the destructor.
std::function<void()> deletion_callback_;

1 change: 0 additions & 1 deletion ksp_plugin/plugin.cpp
Original file line number Diff line number Diff line change
@@ -691,7 +691,6 @@ RigidMotion<Barycentric, World> Plugin::BarycentricToWorld(

auto const barycentric_to_main_body_motion =
main_body_frame.ToThisFrameAtTime(current_time_);
// In coordinates, this rotation is the identity.
auto const barycentric_to_main_body_rotation =
barycentric_to_main_body_motion.rigid_transformation().linear_map();
auto const reference_part_degrees_of_freedom =
44 changes: 35 additions & 9 deletions ksp_plugin_adapter/ksp_plugin_adapter.cs
Original file line number Diff line number Diff line change
@@ -364,15 +364,41 @@ private void UpdatePredictions() {
}

private void UpdateVessel(Vessel vessel, double universal_time) {
if (plugin_.HasVessel(vessel.id.ToString())) {
QP from_parent = plugin_.VesselFromParent(
vessel.mainBody.flightGlobalsIndex,
vessel.id.ToString());
vessel.orbit.UpdateFromStateVectors(pos : (Vector3d)from_parent.q,
vel : (Vector3d)from_parent.p,
refBody : vessel.orbit.referenceBody,
UT : universal_time);
}
if (plugin_.HasVessel(vessel.id.ToString())) {
QP from_parent = plugin_.VesselFromParent(
vessel.mainBody.flightGlobalsIndex,
vessel.id.ToString());
vessel.orbit.UpdateFromStateVectors(pos : (Vector3d)from_parent.q,
vel : (Vector3d)from_parent.p,
refBody : vessel.orbit.referenceBody,
UT : universal_time);
if (vessel.loaded) {
foreach (Part part in vessel.parts.Where(part => part.rb != null)) {
// TODO(egg): What if the plugin doesn't have the part? this seems brittle.
// NOTE(egg): I am not sure what the origin is here, as we are
// running before the floating origin and krakensbane.
// Do everything with respect to the root part, since the overall linear motion
// of the vessel is handled with by the orbit anyway.
// TODO(egg): check that the vessel is moved *after* this. Shouldn't we be calling
// vessel.orbitDriver.updateFromParameters() after setting the orbit anyway?
QPRW part_actual_motion = plugin_.PartGetActualDegreesOfFreedom(
part.flightID,
new Origin{
reference_part_is_at_origin = true,
reference_part_is_unmoving = true,
main_body_centre_in_world =
(XYZ)FlightGlobals.ActiveVessel.mainBody.position,
reference_part_id =
vessel.rootPart.flightID});
part.rb.position = vessel.rootPart.rb.position + (Vector3d)part_actual_motion.qp.q;
part.rb.transform.position = vessel.rootPart.rb.position + (Vector3d)part_actual_motion.qp.q;
part.rb.rotation = (UnityEngine.QuaternionD)part_actual_motion.r;
part.rb.transform.rotation = (UnityEngine.QuaternionD)part_actual_motion.r;
part.rb.velocity = vessel.rootPart.rb.velocity + (Vector3d)part_actual_motion.qp.p;
part.rb.angularVelocity = (Vector3d)part_actual_motion.w;
}
}
}
}

private bool time_is_advancing(double universal_time) {
2 changes: 2 additions & 0 deletions ksp_plugin_test/ksp_plugin_test.vcxproj
Original file line number Diff line number Diff line change
@@ -35,6 +35,8 @@
<ClCompile Include="..\ksp_plugin\renderer.cpp" />
<ClCompile Include="..\ksp_plugin\vessel.cpp" />
<ClCompile Include="..\numerics\cbrt.cpp" />
<ClCompile Include="..\numerics\elliptic_functions.cpp" />
<ClCompile Include="..\numerics\elliptic_integrals.cpp" />
<ClCompile Include="..\physics\protector.cpp" />
<ClCompile Include="benchmark.cpp" />
<ClCompile Include="celestial_test.cpp" />
6 changes: 6 additions & 0 deletions ksp_plugin_test/ksp_plugin_test.vcxproj.filters
Original file line number Diff line number Diff line change
@@ -188,6 +188,12 @@
<ClCompile Include="..\ksp_plugin\interface_part.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="..\numerics\elliptic_functions.cpp">
<Filter>Source Files</Filter>
</ClCompile>
<ClCompile Include="..\numerics\elliptic_integrals.cpp">
<Filter>Source Files</Filter>
</ClCompile>
</ItemGroup>
<ItemGroup>
<ClInclude Include="mock_plugin.hpp">
4 changes: 2 additions & 2 deletions ksp_plugin_test/pile_up_test.cpp
Original file line number Diff line number Diff line change
@@ -288,7 +288,7 @@ TEST_F(PileUpTest, LifecycleWithIntrinsicForce) {

CheckPreDeformPileUpInvariants(pile_up);

pile_up.DeformPileUpIfNeeded();
pile_up.DeformPileUpIfNeeded(astronomy::J2000 + 1 * Second);

CheckPreAdvanceTimeInvariants(pile_up);

@@ -385,7 +385,7 @@ TEST_F(PileUpTest, LifecycleWithoutIntrinsicForce) {

CheckPreDeformPileUpInvariants(pile_up);

pile_up.DeformPileUpIfNeeded();
pile_up.DeformPileUpIfNeeded(astronomy::J2000 + 1 * Second);

CheckPreAdvanceTimeInvariants(pile_up);

10 changes: 5 additions & 5 deletions physics/mechanical_system_test.cpp
Original file line number Diff line number Diff line change
@@ -96,7 +96,7 @@ TEST_F(MechanicalSystemTest, RigidTwoPointMasses) {
system_.AngularMomentum(),
Componentwise(AngularMomentum{}, AngularMomentum{}, r * μ * v * Radian));
EXPECT_THAT(system_.InertiaTensor().coordinates(),
Eq(R3x3Matrix<MomentOfInertia>::Diagonal(
Eq(R3x3Matrix<MomentOfInertia>::DiagonalMatrix(
{{}, μ * Pow<2>(r), μ * Pow<2>(r)})));

Mass const m = system_.mass();
@@ -122,7 +122,7 @@ TEST_F(MechanicalSystemTest, RigidTwoCubes) {
constexpr Length cube_side = 3 * Metre;
using Cube = Frame<enum class CubeTag>;
SymmetricBilinearForm<MomentOfInertia, Cube, Bivector> const cube_inertia(
R3x3Matrix<MomentOfInertia>::Diagonal(
R3x3Matrix<MomentOfInertia>::DiagonalMatrix(
{cube_mass * Pow<2>(cube_side) / 6,
cube_mass * Pow<2>(cube_side) / 6,
cube_mass * Pow<2>(cube_side) / 6}));
@@ -159,7 +159,7 @@ TEST_F(MechanicalSystemTest, RigidTwoCubes) {
EXPECT_THAT(system_.mass(), Eq(2 * cube_mass));
EXPECT_THAT(
system_.InertiaTensor().coordinates(),
Eq(R3x3Matrix<MomentOfInertia>::Diagonal(
Eq(R3x3Matrix<MomentOfInertia>::DiagonalMatrix(
{2 * cube_mass * (2 * Pow<2>(cube_side)) / 12,
2 * cube_mass * (Pow<2>(2 * cube_side) + Pow<2>(cube_side)) / 12,
2 * cube_mass * (Pow<2>(2 * cube_side) + Pow<2>(cube_side)) / 12})));
@@ -174,7 +174,7 @@ TEST_F(MechanicalSystemTest, NonRigidTwoCubes) {
constexpr Length cube_side = 3 * Metre;
using Cube = Frame<enum class CubeTag>;
SymmetricBilinearForm<MomentOfInertia, Cube, Bivector> const cube_inertia(
R3x3Matrix<MomentOfInertia>::Diagonal(
R3x3Matrix<MomentOfInertia>::DiagonalMatrix(
{cube_mass * Pow<2>(cube_side) / 6,
cube_mass * Pow<2>(cube_side) / 6,
cube_mass * Pow<2>(cube_side) / 6}));
@@ -207,7 +207,7 @@ TEST_F(MechanicalSystemTest, NonRigidTwoCubes) {
EXPECT_THAT(system_.mass(), Eq(2 * cube_mass));
EXPECT_THAT(
system_.InertiaTensor().coordinates(),
Eq(R3x3Matrix<MomentOfInertia>::Diagonal(
Eq(R3x3Matrix<MomentOfInertia>::DiagonalMatrix(
{2 * cube_mass * (2 * Pow<2>(cube_side)) / 12,
2 * cube_mass * (Pow<2>(2 * cube_side) + Pow<2>(cube_side)) / 12,
2 * cube_mass * (Pow<2>(2 * cube_side) + Pow<2>(cube_side)) / 12})));
1 change: 1 addition & 0 deletions serialization/geometry.proto
Original file line number Diff line number Diff line change
@@ -164,6 +164,7 @@ message Frame {
NAVBALL = 10;
NAVIGATION = 4;
NON_ROTATING_PILE_UP = 11;
PILE_UP_PRINCIPAL_AXES = 18;
// PRE_BOREL_BARYCENTRIC = 3;
RIGID_PART = 16;
WORLD = 5;

0 comments on commit 8dca142

Please sign in to comment.