// Check that the small body has the right degrees of freedom in the frame of
// the big body.
TEST_F(BodyCentredNonRotatingDynamicFrameTest, SmallBodyInBigFrame) {
int const kSteps = 100;
Bivector<double, ICRFJ2000Equator> const axis({0, 0, 1});
RelativeDegreesOfFreedom<ICRFJ2000Equator> const initial_big_to_small =
small_initial_state_ - big_initial_state_;
ContinuousTrajectory<ICRFJ2000Equator>::Hint hint;
for (Instant t = t0_; t < t0_ + 1 * period_; t += period_ / kSteps) {
DegreesOfFreedom<ICRFJ2000Equator> const small_in_inertial_frame_at_t =
solar_system_.trajectory(*ephemeris_, kSmall).
EvaluateDegreesOfFreedom(t, &hint);
auto const rotation_in_big_frame_at_t =
Rotation<ICRFJ2000Equator, Big>(-2 * π * (t - t0_) * Radian / period_,
axis);
DegreesOfFreedom<Big> const small_in_big_frame_at_t(
rotation_in_big_frame_at_t(initial_big_to_small.displacement()) +
Big::origin,
rotation_in_big_frame_at_t(initial_big_to_small.velocity()));
auto const to_big_frame_at_t = big_frame_->ToThisFrameAtTime(t);
EXPECT_THAT(AbsoluteError(
to_big_frame_at_t(small_in_inertial_frame_at_t).position(),
small_in_big_frame_at_t.position()),
Lt(0.3 * Milli(Metre)));
EXPECT_THAT(AbsoluteError(
to_big_frame_at_t(small_in_inertial_frame_at_t).velocity(),
small_in_big_frame_at_t.velocity()),
Lt(4 * Milli(Metre) / Second));
}
}
DegreesOfFreedom<ToFrame> RigidMotion<FromFrame, ToFrame>::operator()(
DegreesOfFreedom<FromFrame> const& degrees_of_freedom) const {
return {rigid_transformation_(degrees_of_freedom.position()),
orthogonal_map()(
degrees_of_freedom.velocity() - velocity_of_to_frame_origin_ -
angular_velocity_of_to_frame_ *
(degrees_of_freedom.position() -
rigid_transformation_.Inverse()(ToFrame::origin)) /
Radian)};
}
RigidMotion<InertialFrame, ThisFrame>
BodyCentredNonRotatingDynamicFrame<InertialFrame, ThisFrame>::ToThisFrameAtTime(
Instant const& t) const {
DegreesOfFreedom<InertialFrame> const centre_degrees_of_freedom =
centre_trajectory_->EvaluateDegreesOfFreedom(t, &hint_);
RigidTransformation<InertialFrame, ThisFrame> const
rigid_transformation(centre_degrees_of_freedom.position(),
ThisFrame::origin,
Identity<InertialFrame, ThisFrame>().Forget());
return RigidMotion<InertialFrame, ThisFrame>(
rigid_transformation,
AngularVelocity<InertialFrame>(),
centre_degrees_of_freedom.velocity());
}
QP principia__IteratorGetQP(Iterator const* const iterator) {
journal::Method<journal::IteratorGetQP> m({iterator});
CHECK_NOTNULL(iterator);
auto const typed_iterator = check_not_null(
dynamic_cast<TypedIterator<DiscreteTrajectory<World>> const*>(iterator));
return m.Return(typed_iterator->Get<QP>(
[](DiscreteTrajectory<World>::Iterator const& iterator) -> QP {
DegreesOfFreedom<World> const degrees_of_freedom =
iterator.degrees_of_freedom();
return {
ToXYZ(
(degrees_of_freedom.position() - World::origin).coordinates() /
Metre),
ToXYZ(degrees_of_freedom.velocity().coordinates() /
(Metre / Second))};
}));
}
RigidMotion<InertialFrame, ThisFrame>
BodySurfaceDynamicFrame<InertialFrame, ThisFrame>::ToThisFrameAtTime(
Instant const& t) const {
DegreesOfFreedom<InertialFrame> const centre_degrees_of_freedom =
centre_trajectory_->EvaluateDegreesOfFreedom(t, &hint_);
Rotation<InertialFrame, ThisFrame> rotation =
centre_->template ToSurfaceFrame<ThisFrame>(t);
AngularVelocity<InertialFrame> angular_velocity = centre_->angular_velocity();
RigidTransformation<InertialFrame, ThisFrame> const
rigid_transformation(centre_degrees_of_freedom.position(),
ThisFrame::origin,
rotation.Forget());
return RigidMotion<InertialFrame, ThisFrame>(
rigid_transformation,
angular_velocity,
centre_degrees_of_freedom.velocity());
}
void ContinuousTrajectory<Frame>::Append(
Instant const& time,
DegreesOfFreedom<Frame> const& degrees_of_freedom) {
// Consistency checks.
if (first_time_) {
Instant const t0;
CHECK_GE(1,
ULPDistance((last_points_.back().first + step_ - t0) /
SIUnit<Time>(),
(time - t0) / SIUnit<Time>()))
<< "Append at times that are not equally spaced";
} else {
first_time_ = time;
}
if (last_points_.size() == divisions) {
// These vectors are static to avoid deallocation/reallocation each time we
// go through this code path.
static std::vector<Displacement<Frame>> q(divisions + 1);
static std::vector<Velocity<Frame>> v(divisions + 1);
q.clear();
v.clear();
for (auto const& pair : last_points_) {
DegreesOfFreedom<Frame> const& degrees_of_freedom = pair.second;
q.push_back(degrees_of_freedom.position() - Frame::origin);
v.push_back(degrees_of_freedom.velocity());
}
q.push_back(degrees_of_freedom.position() - Frame::origin);
v.push_back(degrees_of_freedom.velocity());
ComputeBestNewhallApproximation(
time, q, v, &ЧебышёвSeries<Displacement<Frame>>::NewhallApproximation);
// Wipe-out the points that have just been incorporated in a series.
last_points_.clear();
}
// Note that we only insert the new point in the map *after* computing the
// approximation, because clearing the map is much more efficient than erasing
// every element but one.
last_points_.emplace_back(time, degrees_of_freedom);
}
Rotation<Frenet<ThisFrame>, ThisFrame>
DynamicFrame<InertialFrame, ThisFrame>::FrenetFrame(
Instant const& t,
DegreesOfFreedom<ThisFrame> const& degrees_of_freedom) const {
Velocity<ThisFrame> const& velocity = degrees_of_freedom.velocity();
Vector<Acceleration, ThisFrame> const acceleration =
GeometricAcceleration(t, degrees_of_freedom);
Vector<Acceleration, ThisFrame> normal_acceleration = acceleration;
velocity.template Orthogonalize<Acceleration>(&normal_acceleration);
Vector<double, ThisFrame> tangent = Normalize(velocity);
Vector<double, ThisFrame> normal = Normalize(normal_acceleration);
Bivector<double, ThisFrame> binormal = Wedge(tangent, normal);
// Maps |tangent| to {1, 0, 0}, |normal| to {0, 1, 0}, and |binormal| to
// {0, 0, 1}.
return Rotation<Frenet<ThisFrame>, ThisFrame>(
R3x3Matrix(tangent.coordinates(),
normal.coordinates(),
binormal.coordinates()).Transpose());
}
std::string DebugString(DegreesOfFreedom<Frame> const& degrees_of_freedom) {
return "{" + DebugString(degrees_of_freedom.position()) + ", " +
DebugString(degrees_of_freedom.velocity()) + "}";
}
// Interpreting the elements as Jacobi coordinates in the Jool system.
std::vector<DegreesOfFreedom<KSP>> JacobiInitialStates() {
// Jool-centric coordinates: a nonrotating inertial frame in which Jool is
// centred and immobile, for building the Jool system in Jacobi coordinates.
// We only use this frame at |game_epoch_|.
using JoolCentric = Frame<serialization::Frame::TestTag,
serialization::Frame::TEST1,
/*is_inertial=*/false>;
// These coordinate systems have the same axes.
auto const id = OrthogonalMap<KSP, JoolCentric>::Identity();
std::map<not_null<MassiveBody const*>, KeplerOrbit<KSP>> orbits;
orbits.emplace(jool_, KeplerOrbit<KSP>(*sun_,
*jool_,
elements_[jool_],
game_epoch_));
// The barycentre of the bodies of the Jool system considered so far.
BarycentreCalculator<DegreesOfFreedom<JoolCentric>, GravitationalParameter>
inner_system_barycentre;
// TODO(egg): BarycentreCalculator should just have a method that returns
// the weight of the whole thing, so we're not accumulating it twice.
GravitationalParameter inner_system_parameter =
jool_->gravitational_parameter();
std::map<not_null<MassiveBody const*>, DegreesOfFreedom<JoolCentric>>
jool_centric_initial_state;
// Jool.
DegreesOfFreedom<JoolCentric> const jool_dof = {JoolCentric::origin,
Velocity<JoolCentric>()};
inner_system_barycentre.Add(jool_dof, jool_->gravitational_parameter());
// The elements of each moon are interpreted as the osculating elements of
// an orbit around a point mass at the barycentre of Jool and the
// previously-added moons, so that the state vectors are Jacobi coordinates
// for the system.
for (auto const moon : joolian_moons_) {
jool_centric_initial_state.emplace(
moon,
inner_system_barycentre.Get() +
id(KeplerOrbit<KSP>(MassiveBody(inner_system_parameter),
*moon,
elements_[moon],
game_epoch_).StateVectors(game_epoch_)));
inner_system_parameter += moon->gravitational_parameter();
inner_system_barycentre.Add(jool_centric_initial_state.at(moon),
moon->gravitational_parameter());
}
// |inner_system_barycentre| is now the barycentre of the whole Jool system.
// We want that to be placed where dictated by Jool's orbit.
DegreesOfFreedom<KSP> const jool_barycentre_initial_state =
origin_ + orbits.at(jool_).StateVectors(game_epoch_);
// TODO(egg): this is very messy, a constructor from a pair of
// |DegreesOfFreedom|s like the constructor for |RigidTransformation| from a
// pair of |Position|s would be nice...
RigidMotion<JoolCentric, KSP> const to_heliocentric(
RigidTransformation<JoolCentric, KSP>(
inner_system_barycentre.Get().position(),
jool_barycentre_initial_state.position(),
id.Inverse()),
AngularVelocity<JoolCentric>(),
/*velocity_of_to_frame_origin=*/
inner_system_barycentre.Get().velocity() -
id(jool_barycentre_initial_state.velocity()));
// The Sun and Jool.
std::vector<DegreesOfFreedom<KSP>> initial_states = {
origin_, to_heliocentric(jool_dof)};
for (auto const moon : joolian_moons_) {
initial_states.emplace_back(
to_heliocentric(jool_centric_initial_state.at(moon)));
}
return initial_states;
}
void Ephemeris<Frame>::ComputeApsides(
not_null<MassiveBody const*> const body,
typename DiscreteTrajectory<Frame>::Iterator const begin,
typename DiscreteTrajectory<Frame>::Iterator const end,
DiscreteTrajectory<Frame>& apoapsides,
DiscreteTrajectory<Frame>& periapsides) {
not_null<ContinuousTrajectory<Frame> const*> const body_trajectory =
trajectory(body);
typename ContinuousTrajectory<Frame>::Hint hint;
std::experimental::optional<Instant> previous_time;
std::experimental::optional<DegreesOfFreedom<Frame>>
previous_degrees_of_freedom;
std::experimental::optional<Square<Length>> previous_squared_distance;
std::experimental::optional<Variation<Square<Length>>>
previous_squared_distance_derivative;
for (auto it = begin; it != end; ++it) {
Instant const time = it.time();
DegreesOfFreedom<Frame> const degrees_of_freedom = it.degrees_of_freedom();
DegreesOfFreedom<Frame> const body_degrees_of_freedom =
body_trajectory->EvaluateDegreesOfFreedom(time, &hint);
RelativeDegreesOfFreedom<Frame> const relative =
degrees_of_freedom - body_degrees_of_freedom;
Square<Length> const squared_distance =
InnerProduct(relative.displacement(), relative.displacement());
// This is the derivative of |squared_distance|.
Variation<Square<Length>> const squared_distance_derivative =
2.0 * InnerProduct(relative.displacement(), relative.velocity());
if (previous_squared_distance_derivative &&
Sign(squared_distance_derivative) !=
Sign(*previous_squared_distance_derivative)) {
CHECK(previous_time &&
previous_degrees_of_freedom &&
previous_squared_distance);
// The derivative of |squared_distance| changed sign. Construct a Hermite
// approximation of |squared_distance| and find its extrema.
Hermite3<Instant, Square<Length>> const
squared_distance_approximation(
{*previous_time, time},
{*previous_squared_distance, squared_distance},
{*previous_squared_distance_derivative,
squared_distance_derivative});
std::set<Instant> const extrema =
squared_distance_approximation.FindExtrema();
// Now look at the extrema and check that exactly one is in the required
// time interval. This is normally the case, but it can fail due to
// ill-conditioning.
Instant apsis_time;
int valid_extrema = 0;
for (auto const& extremum : extrema) {
if (extremum >= *previous_time && extremum <= time) {
apsis_time = extremum;
++valid_extrema;
}
}
if (valid_extrema != 1) {
// Something went wrong when finding the extrema of
// |squared_distance_approximation|. Use a linear interpolation of
// |squared_distance_derivative| instead.
apsis_time = Barycentre<Instant, Variation<Square<Length>>>(
{time, *previous_time},
{*previous_squared_distance_derivative,
-squared_distance_derivative});
}
// Now that we know the time of the apsis, construct a Hermite
// approximation of the position of the body, and use it to derive its
// degrees of freedom. Note that an extremum of
// |squared_distance_approximation| is in general not an extremum for
// |position_approximation|: the distance computed using the latter is a
// 6th-degree polynomial. However, approximating this polynomial using a
// 3rd-degree polynomial would yield |squared_distance_approximation|, so
// we shouldn't be far from the truth.
Hermite3<Instant, Position<Frame>> position_approximation(
{*previous_time, time},
{previous_degrees_of_freedom->position(),
degrees_of_freedom.position()},
{previous_degrees_of_freedom->velocity(),
degrees_of_freedom.velocity()});
DegreesOfFreedom<Frame> const apsis_degrees_of_freedom(
position_approximation.Evaluate(apsis_time),
position_approximation.EvaluateDerivative(apsis_time));
if (Sign(squared_distance_derivative).Negative()) {
apoapsides.Append(apsis_time, apsis_degrees_of_freedom);
} else {
periapsides.Append(apsis_time, apsis_degrees_of_freedom);
}
}
previous_time = time;
previous_degrees_of_freedom = degrees_of_freedom;
previous_squared_distance = squared_distance;
previous_squared_distance_derivative = squared_distance_derivative;
}
}
