Skip to content
Permalink

Comparing changes

Choose two branches to see what’s changed or to start a new pull request. If you need to, you can also or learn more about diff comparisons.

Open a pull request

Create a new pull request by comparing changes across two branches. If you need to, you can also . Learn more about diff comparisons here.
base repository: mockingbirdnest/Principia
Failed to load repositories. Confirm that selected base ref is valid, then try again.
Loading
base: e3244497983b
Choose a base ref
...
head repository: mockingbirdnest/Principia
Failed to load repositories. Confirm that selected head ref is valid, then try again.
Loading
compare: a291654b45d3
Choose a head ref
  • 5 commits
  • 5 files changed
  • 1 contributor

Commits on Dec 3, 2019

  1. Copy the full SHA
    40bb54b View commit details

Commits on Dec 4, 2019

  1. Clean up the new test.

    pleroy committed Dec 4, 2019
    Copy the full SHA
    12b06b0 View commit details

Commits on Dec 7, 2019

  1. Quaternion normalization.

    pleroy committed Dec 7, 2019
    Copy the full SHA
    6f24f7d View commit details
  2. Copy the full SHA
    e074b3a View commit details
  3. Merge pull request #2392 from pleroy/2389

    Normalize the quaternions that we get from Unity
    pleroy authored Dec 7, 2019
    Copy the full SHA
    a291654 View commit details
Showing with 157 additions and 7 deletions.
  1. +8 −0 geometry/orthogonal_map.hpp
  2. +6 −0 geometry/quaternion.hpp
  3. +21 −4 geometry/quaternion_body.hpp
  4. +6 −1 ksp_plugin/interface_body.hpp
  5. +116 −2 physics/rigid_motion_test.cpp
8 changes: 8 additions & 0 deletions geometry/orthogonal_map.hpp
Original file line number Diff line number Diff line change
@@ -10,6 +10,11 @@
#include "serialization/geometry.pb.h"

namespace principia {

namespace physics {
class RigidMotionTest;
} // namespace physics

namespace geometry {

FORWARD_DECLARE_FROM(identity,
@@ -91,6 +96,9 @@ class OrthogonalMap : public LinearMap<FromFrame, ToFrame> {
OrthogonalMap<From, Through> const& right);

friend class OrthogonalMapTest;

// TODO(phl): This friendship could be avoided if we had symmetries.
friend class physics::RigidMotionTest;
};

template<typename FromFrame, typename ThroughFrame, typename ToFrame>
6 changes: 6 additions & 0 deletions geometry/quaternion.hpp
Original file line number Diff line number Diff line change
@@ -21,6 +21,9 @@ class Quaternion final {
double real_part() const;
R3Element<double> const& imaginary_part() const;

double Norm() const;
double Norm²() const;

Quaternion Conjugate() const;
Quaternion Inverse() const;

@@ -55,10 +58,13 @@ Quaternion operator*(double left, Quaternion const& right);
Quaternion operator*(Quaternion const& left, double right);
Quaternion operator/(Quaternion const& left, double right);

Quaternion Normalize(Quaternion const& quaternion);

std::ostream& operator<<(std::ostream& out, Quaternion const& quaternion);

} // namespace internal_quaternion

using internal_quaternion::Normalize;
using internal_quaternion::Quaternion;

} // namespace geometry
25 changes: 21 additions & 4 deletions geometry/quaternion_body.hpp
Original file line number Diff line number Diff line change
@@ -4,11 +4,16 @@
#include "geometry/quaternion.hpp"

#include "geometry/r3_element.hpp"
#include "quantities/elementary_functions.hpp"
#include "quantities/quantities.hpp"

namespace principia {
namespace geometry {
namespace internal_quaternion {

using quantities::DebugString;
using quantities::Sqrt;

inline Quaternion::Quaternion() : real_part_(0) {}

inline Quaternion::Quaternion(double const real_part)
@@ -28,6 +33,14 @@ Quaternion::imaginary_part() const {
return imaginary_part_;
}

inline double Quaternion::Norm() const {
return Sqrt(Norm²());
}

inline double Quaternion::Norm²() const {
return real_part_ * real_part_ + imaginary_part_.Norm²();
}

inline Quaternion Quaternion::Conjugate() const {
return Quaternion(real_part_, -imaginary_part_);
}
@@ -139,12 +152,16 @@ inline Quaternion operator/(Quaternion const& left, double const right) {
left.imaginary_part() / right);
}

inline Quaternion Normalize(Quaternion const& quaternion) {
return quaternion / quaternion.Norm();
}

inline std::ostream& operator<<(std::ostream& out,
Quaternion const& quaternion) {
return out << quaternion.real_part() << " + "
<< quaternion.imaginary_part().x << " i + "
<< quaternion.imaginary_part().y << " j + "
<< quaternion.imaginary_part().z << " k";
return out << DebugString(quaternion.real_part()) << " + "
<< DebugString(quaternion.imaginary_part().x) << " i + "
<< DebugString(quaternion.imaginary_part().y) << " j + "
<< DebugString(quaternion.imaginary_part().z) << " k";
}

} // namespace internal_quaternion
7 changes: 6 additions & 1 deletion ksp_plugin/interface_body.hpp
Original file line number Diff line number Diff line change
@@ -345,7 +345,12 @@ inline RelativeDegreesOfFreedom<World> FromQP(QP const& qp) {
}

inline Quaternion FromWXYZ(WXYZ const& wxyz) {
return Quaternion{wxyz.w, {wxyz.x, wxyz.y, wxyz.z}};
// It is critical to normalize the quaternion that we receive from Unity: it
// is normalized in *single* precision, which is fine for KSP where the moving
// origin of World ensures that coordinates are never very large. But in the
// C++ code we do some computations in Barycentric, which typically results in
// large coordinates for which we need normalization in *double* precision.
return Normalize(Quaternion{wxyz.w, {wxyz.x, wxyz.y, wxyz.z}});
}

inline R3Element<double> FromXYZ(XYZ const& xyz) {
118 changes: 116 additions & 2 deletions physics/rigid_motion_test.cpp
Original file line number Diff line number Diff line change
@@ -3,6 +3,9 @@

#include "geometry/frame.hpp"
#include "geometry/permutation.hpp"
#include "geometry/quaternion.hpp"
#include "geometry/rotation.hpp"
#include "geometry/sign.hpp"
#include "gmock/gmock.h"
#include "gtest/gtest.h"
#include "quantities/named_quantities.hpp"
@@ -11,30 +14,52 @@
#include "serialization/physics.pb.h"
#include "testing_utilities/almost_equals.hpp"
#include "testing_utilities/componentwise.hpp"
#include "testing_utilities/is_near.hpp"
#include "testing_utilities/numerics_matchers.hpp"
#include "testing_utilities/vanishes_before.hpp"

namespace principia {
namespace physics {
namespace internal_rigid_motion {

using geometry::AngularVelocity;
using geometry::Displacement;
using geometry::Frame;
using geometry::InnerProduct;
using geometry::Normalize;
using geometry::OrthogonalMap;
using geometry::Permutation;
using geometry::Point;
using geometry::Position;
using geometry::Rotation;
using geometry::Quaternion;
using geometry::Sign;
using geometry::Vector;
using geometry::Velocity;
using geometry::Wedge;
using quantities::AngularFrequency;
using quantities::Length;
using quantities::Speed;
using quantities::si::Day;
using quantities::si::Kilo;
using quantities::si::Metre;
using quantities::si::Radian;
using quantities::si::Second;
using testing_utilities::AlmostEquals;
using testing_utilities::Componentwise;
using testing_utilities::IsNear;
using testing_utilities::RelativeErrorFrom;
using testing_utilities::VanishesBefore;
using testing_utilities::operator""_⑴;

class RigidMotionTest : public testing::Test {
protected:
template<typename FromFrame, typename ToFrame>
OrthogonalMap<FromFrame, ToFrame> MakeOrthogonalMap(
Sign const& determinant,
Rotation<FromFrame, ToFrame> const& rotation) {
return OrthogonalMap(determinant, rotation);
}

// Our Earth and Moon both run a bit slow.
AngularFrequency const earth_rotation_speed_ = 2 * π * Radian / Day;
AngularFrequency const moon_rotation_speed_ = 2 * π * Radian / (30 * Day);
@@ -227,6 +252,95 @@ TEST_F(RigidMotionTest, Serialization) {
EXPECT_THAT(d1.velocity(), AlmostEquals(d2.velocity(), 0));
}

} // namespace internal_rigid_motion
TEST_F(RigidMotionTest, QuaternionNormalization) {
using Barycentric =
Frame<serialization::Frame::TestTag, serialization::Frame::TEST1, false>;
using RigidPart =
Frame<serialization::Frame::TestTag, serialization::Frame::TEST2, false>;
using World =
Frame<serialization::Frame::TestTag, serialization::Frame::TEST3, false>;

AngularVelocity<RigidPart> const ω1(
{-4.31524874936563274e-04 * Radian / Second,
+3.58760764995860824e-03 * Radian / Second,
-5.60639012817497860e-04 * Radian / Second});
Velocity<RigidPart> const v1({+9.45391439618678553e-06 * Metre / Second,
-3.92345736956409459e-03 * Metre / Second,
-2.74220213842711266e-06 * Metre / Second});
Position<RigidPart> const from1 = RigidPart::origin;
Position<World> const to1 =
World::origin + Displacement<World>({+3.37126515805721283e-02 * Metre,
+1.21778284665197134e-03 * Metre,
-2.06734146922826767e-02 * Metre});

// Unity gives us quaternions that are normalized in single precision. If we
// don't normalize them in double precision, the tests below fail with huge
// errors (orders of magnitude).
Rotation<RigidPart, World> const rotation1(
Normalize(Quaternion(-3.23732018470764160e-01,
{3.28581303358078003e-01,
-6.28009378910064697e-01,
-6.26766622066497803e-01})));

RigidMotion<RigidPart, World> const rigid_motion1(
RigidTransformation<RigidPart, World>(from1, to1, rotation1.Forget()),
ω1,
v1);

AngularVelocity<World> const ω2({-1.09321577613522688e-36 * Radian / Second,
+2.91570900559802080e-04 * Radian / Second,
+5.42101086242752217e-20 * Radian / Second});
Velocity<World> const v2({+3.10203149761506589e+06 * Metre / Second,
+2.41841229267265589e-02 * Metre / Second,
+2.45903920001263265e+06 * Metre / Second});
Position<World> const from2 =
World::origin + Displacement<World>({+4.91525322355270386e+05 * Metre,
+1.01811877291461769e+03 * Metre,
-3.44263258773803711e+05 * Metre});
Position<Barycentric> const to2 =
Barycentric::origin +
Displacement<Barycentric>({-1.36081879406308479e+10 * Metre,
+7.17490648244777508e+06 * Metre,
-4.33826960235058723e+04 * Metre});
Rotation<World, Barycentric> const rotation2(
Quaternion(6.36714825392272976e-01,
{-6.36714825392272976e-01,
-3.07561751727498445e-01,
+3.07561751727498445e-01}));
OrthogonalMap<World, Barycentric> const orthogonal2 =
MakeOrthogonalMap(Sign::Negative(), rotation2);

RigidMotion<World, Barycentric> const rigid_motion2(
RigidTransformation<World, Barycentric>(from2, to2, orthogonal2), ω2, v2);

DegreesOfFreedom<World> const d1 =
rigid_motion1({RigidPart::origin, Velocity<RigidPart>{}});
DegreesOfFreedom<Barycentric> const d2 =
(rigid_motion2 * rigid_motion1)(
{RigidPart::origin, Velocity<RigidPart>{}});
DegreesOfFreedom<Barycentric> const d3 =
rigid_motion2(rigid_motion1({RigidPart::origin, Velocity<RigidPart>{}}));
DegreesOfFreedom<World> const d4 =
rigid_motion2.Inverse()((rigid_motion2 * rigid_motion1)(
{RigidPart::origin, Velocity<RigidPart>{}}));
DegreesOfFreedom<World> const d5 = rigid_motion2.Inverse()(
rigid_motion2(rigid_motion1({RigidPart::origin, Velocity<RigidPart>{}})));

EXPECT_THAT(d2.position() - Barycentric::origin,
AlmostEquals(d3.position() - Barycentric::origin, 0));
EXPECT_THAT(d2.velocity(),
RelativeErrorFrom(d3.velocity(), IsNear(1.1e-12_⑴)));
EXPECT_THAT(d4.position() - World::origin,
RelativeErrorFrom(d1.position() - World::origin,
IsNear(5.8e-6_⑴)));
EXPECT_THAT(d4.velocity(),
RelativeErrorFrom(d1.velocity(), IsNear(2.7e-6_⑴)));
EXPECT_THAT(d5.position() - World::origin,
RelativeErrorFrom(d1.position() - World::origin,
IsNear(5.8e-6_⑴)));
EXPECT_THAT(d5.velocity(),
RelativeErrorFrom(d1.velocity(), IsNear(5.7e-8_⑴)));
}

} // namespace physics
} // namespace principia