Merge pull request #3848 from pleroy/Collision3

A function to compute the first collision between a vessel and a celestial

numerics/чебышёв_series_body.hpp

@@ -310,7 +310,11 @@ absl::btree_set<Argument> ЧебышёвSeries<Value, Argument>::RealRoots(
                                    std::pair{1, 1});
   auto const half_width = 0.5 * (upper_bound_ - lower_bound_);
   for (auto const& scaled_real_root : scaled_real_roots) {
-    real_roots.insert(scaled_real_root * half_width + midpoint);
+    // Чебышёв polynomials don't make sense outside of [-1, 1] but they may
+    // stil have roots there.
+    if (-1 <= scaled_real_root && scaled_real_root <= 1) {
+      real_roots.insert(scaled_real_root * half_width + midpoint);
+    }
   }
   return real_roots;
 }

physics/apsides.hpp

@@ -49,6 +49,22 @@ std::vector<Interval<Instant>> ComputeCollisionIntervals(
     DiscreteTrajectory<Frame> const& apoapsides,
     DiscreteTrajectory<Frame> const& periapsides);
 
+// Computes the first collision between a vessel and a rotating body over the
+// given time |interval|.  Returns |nullopt| if there is no collision over the
+// |interval|.  The |interval| should have been obtained by
+// |ComputeCollisionIntervals|.  |radius| must give the radius of the celestial
+// at a particular position given by its latitude and longitude.  It must never
+// exceed the |max_radius| of the body.
+template<typename Frame>
+std::optional<typename DiscreteTrajectory<Frame>::value_type>
+ComputeFirstCollision(
+    RotatingBody<Frame> const& reference_body,
+    Trajectory<Frame> const& reference,
+    Trajectory<Frame> const& trajectory,
+    Interval<Instant> const& interval,
+    std::function<Length(Angle const& latitude, Angle const& longitude)> const&
+        radius);
+
 // Computes a collision between a vessel and a rotating body.  |first_time| and
 // |last_time| must be on opposite sides of the surface of the body (in
 // particular, a collision must exist).  |radius| gives the radius of the
@@ -97,6 +113,7 @@ void ComputeApsides(Trajectory<Frame> const& trajectory1,
 using internal::ComputeApsides;
 using internal::ComputeCollision;
 using internal::ComputeCollisionIntervals;
+using internal::ComputeFirstCollision;
 using internal::ComputeNodes;
 
 }  // namespace _apsides

physics/apsides_body.hpp

@@ -11,6 +11,7 @@
 #include "geometry/r3_element.hpp"
 #include "geometry/sign.hpp"
 #include "geometry/space.hpp"
+#include "numerics/approximation.hpp"
 #include "numerics/hermite3.hpp"
 #include "numerics/root_finders.hpp"
 #include "physics/degrees_of_freedom.hpp"
@@ -27,12 +28,15 @@ using namespace principia::geometry::_barycentre_calculator;
 using namespace principia::geometry::_r3_element;
 using namespace principia::geometry::_sign;
 using namespace principia::geometry::_space;
+using namespace principia::numerics::_approximation;
 using namespace principia::numerics::_hermite3;
 using namespace principia::numerics::_root_finders;
 using namespace principia::physics::_degrees_of_freedom;
 using namespace principia::quantities::_elementary_functions;
 using namespace principia::quantities::_named_quantities;
 
+constexpr double max_error_relative_to_radius = 1e-4;
+
 template<typename Frame>
 void ComputeApsides(Trajectory<Frame> const& reference,
                     Trajectory<Frame> const& trajectory,
@@ -283,6 +287,60 @@ std::vector<Interval<Instant>> ComputeCollisionIntervals(
   return intervals;
 }
 
+template<typename Frame>
+std::optional<typename DiscreteTrajectory<Frame>::value_type>
+ComputeFirstCollision(
+    RotatingBody<Frame> const& reference_body,
+    Trajectory<Frame> const& reference,
+    Trajectory<Frame> const& trajectory,
+    Interval<Instant> const& interval,
+    std::function<Length(Angle const& latitude, Angle const& longitude)> const&
+        radius) {
+  // The frame of the surface of the celestial.
+  using SurfaceFrame = geometry::_frame::Frame<struct SurfaceFrameTag>;
+
+  auto height_above_terrain_at_time = [&radius,
+                                       &reference,
+                                       &reference_body,
+                                       &trajectory](Instant const& t) {
+    auto const reference_position = reference.EvaluatePosition(t);
+    auto const trajectory_position = trajectory.EvaluatePosition(t);
+    Displacement<Frame> const displacement_in_frame =
+        trajectory_position - reference_position;
+
+    auto const to_surface_frame =
+        reference_body.template ToSurfaceFrame<SurfaceFrame>(t);
+    Displacement<SurfaceFrame> const displacement_in_surface =
+        to_surface_frame(displacement_in_frame);
+
+    SphericalCoordinates<Length> const spherical_coordinates =
+        displacement_in_surface.coordinates().ToSpherical();
+
+    return spherical_coordinates.radius -
+           radius(spherical_coordinates.latitude,
+                  spherical_coordinates.longitude);
+  };
+
+  // Interpolate the height above the terrain using a Чебышёв polynomial.
+  auto const чебышёв_interpolant = ЧебышёвPolynomialInterpolant(
+      height_above_terrain_at_time,
+      interval.min,
+      interval.max,
+      reference_body.mean_radius() * max_error_relative_to_radius);
+  auto const& real_roots = чебышёв_interpolant.RealRoots(
+      max_error_relative_to_radius);
+  if (real_roots.empty()) {
+    // No root, no collision.
+    return std::nullopt;
+  } else {
+    // The smallest root is the first collision.
+    Instant const first_collision_time = *real_roots.begin();
+    return typename DiscreteTrajectory<Frame>::value_type(
+        first_collision_time,
+        trajectory.EvaluateDegreesOfFreedom(first_collision_time));
+  }
+}
+
 template<typename Frame>
 typename DiscreteTrajectory<Frame>::value_type ComputeCollision(
     RotatingBody<Frame> const& reference_body,

physics/apsides_test.cpp

@@ -179,6 +179,107 @@ TEST_F(ApsidesTest, ComputeApsidesDiscreteTrajectory) {
 
 #endif
 
+TEST_F(ApsidesTest, ComputeFirstCollision) {
+  Instant const t0;
+  DiscreteTrajectory<World> reference_trajectory;
+  DiscreteTrajectory<World> vessel_trajectory;
+
+  // At |t0| the vessel is inside the celestial, so we expect the collision at a
+  // negative time.
+  AppendTrajectoryTimeline(
+      NewLinearTrajectoryTimeline(
+          DegreesOfFreedom<World>(
+              World::origin +
+                  Displacement<World>({0 * Metre, -1 * Metre, 0 * Metre}),
+              Velocity<World>({1 * Metre / Second,
+                               0 * Metre / Second,
+                               0 * Metre / Second})),
+          /*Δt=*/1 * Second,
+          t0,
+          /*t1=*/t0 - 10 * Second,
+          /*t2=*/t0 + 10 * Second),
+      reference_trajectory);
+
+  // Note that the trajectory is short enough that we only have to go to degree
+  // 64 for the interpolant.
+  AppendTrajectoryTimeline(
+      NewLinearTrajectoryTimeline(
+          DegreesOfFreedom<World>(
+              World::origin +
+                  Displacement<World>({1 * Metre, -1 * Metre, 0 * Metre}),
+              Velocity<World>({0 * Metre / Second,
+                               -1 * Metre / Second,
+                               0 * Metre / Second})),
+          /*Δt=*/1 * Second,
+          t0,
+          /*t1=*/t0 - 10 * Second,
+          /*t2=*/t0 + 1.9 * Second),
+      vessel_trajectory);
+
+  RotatingBody<World> const body(
+      1 * Kilogram,
+      RotatingBody<World>::Parameters(
+          /*min_radius=*/1 * Metre,
+          /*mean_radius=*/2 * Metre,
+          /*max_radius=*/3 * Metre,
+          /*reference_angle=*/0 * Radian,
+          /*reference_instant=*/t0,
+          /*angular_frequency=*/2 * π * Radian / Second,
+          /*right_ascension_of_pole=*/0 * Radian,
+          /*declination_of_pole=*/π / 2 * Radian));
+
+  // The celestial is infinite in the z direction and has four lobes in the x-y
+  // plane.  Think of a LEGO® axle.
+  auto radius = [](Angle const& latitude, Angle const& longitude) {
+    return (Cos(4 * longitude) + 2) * Metre;
+  };
+
+  // The computations below were verified with Mathematica to the given
+  // accuracy.
+  DiscreteTrajectory<World> apoapsides;
+  DiscreteTrajectory<World> periapsides;
+  ComputeApsides(reference_trajectory,
+                 vessel_trajectory,
+                 vessel_trajectory.begin(),
+                 vessel_trajectory.end(),
+                 /*max_point=*/10,
+                 apoapsides,
+                 periapsides);
+  EXPECT_THAT(apoapsides, IsEmpty());
+  EXPECT_THAT(periapsides, SizeIs(1));
+  EXPECT_THAT(periapsides.begin()->time, AlmostEquals(t0 + 0.5 * Second, 0));
+
+  const auto intervals = ComputeCollisionIntervals(body,
+                                                   reference_trajectory,
+                                                   vessel_trajectory,
+                                                   apoapsides,
+                                                   periapsides);
+  EXPECT_THAT(intervals,
+              ElementsAre(IntervalMatches(
+                  AlmostEquals(t0 + (1.0 - Sqrt(17.0)) / 2.0 * Second, 1),
+                  AlmostEquals(t0 + 1 * Second, 0))));
+
+  auto const maybe_collision =
+      ComputeFirstCollision(body,
+                            reference_trajectory,
+                            vessel_trajectory,
+                            intervals[0],
+                            radius);
+  auto const& collision = maybe_collision.value();
+
+  EXPECT_THAT(collision.time - t0,
+              IsNear(-1.43862_(1) * Second));
+  EXPECT_THAT(collision.degrees_of_freedom.position() - World::origin,
+              Componentwise(1 * Metre,
+                            IsNear(0.43862_(1) * Metre),
+                            0 * Metre));
+  EXPECT_THAT(
+      collision.degrees_of_freedom.velocity(),
+      AlmostEquals(Velocity<World>({0 * Metre / Second,
+                                    -1 * Metre / Second,
+                                    0 * Metre / Second}), 0));
+}
+
 TEST_F(ApsidesTest, ComputeCollision) {
   Instant const t0;
   DiscreteTrajectory<World> reference_trajectory;