<commit_before>#include "physics/transforms.hpp"

#include <limits>

#include "geometry/frame.hpp"
#include "base/not_null.hpp"
#include "gmock/gmock.h"
#include "gtest/gtest.h"
#include "physics/degrees_of_freedom.hpp"
#include "physics/massive_body.hpp"
#include "physics/massless_body.hpp"
#include "quantities/named_quantities.hpp"
#include "quantities/quantities.hpp"
#include "serialization/geometry.pb.h"
#include "testing_utilities/almost_equals.hpp"
#include "testing_utilities/componentwise.hpp"
#include "testing_utilities/vanishes_before.hpp"

namespace principia {

using base::make_not_null_unique;
using geometry::Frame;
using quantities::Length;
using quantities::Mass;
using quantities::SIUnit;
using quantities::Speed;
using quantities::Time;
using testing_utilities::AlmostEquals;
using testing_utilities::Componentwise;
using testing_utilities::VanishesBefore;
using ::testing::Eq;
using ::testing::Lt;

namespace physics {

namespace {
const int kNumberOfPoints = 20;
}  // namespace

class TransformsTest : public testing::Test {
 protected:
  using From = Frame<serialization::Frame::TestTag,
                     serialization::Frame::FROM, true>;
  using Through = Frame<serialization::Frame::TestTag,
                        serialization::Frame::THROUGH, false>;
  using To = Frame<serialization::Frame::TestTag,
                   serialization::Frame::TO, true>;

  struct Functors {
    Trajectory<From> const& from_trajectory() const;
    Trajectory<Through> const& through_trajectory() const;
    Trajectory<To> const& to_trajectory() const;

    Trajectory<From>* from;
    Trajectory<Through>* through;
    Trajectory<To>* to;
  };

  TransformsTest()
      : body1_(MassiveBody(1 * SIUnit<Mass>())),
        body2_(MassiveBody(3 * SIUnit<Mass>())),
        body1_from_(make_not_null_unique<Trajectory<From>>(&body1_)),
        body2_from_(make_not_null_unique<Trajectory<From>>(&body2_)),
        body1_to_(make_not_null_unique<Trajectory<To>>(&body1_)),
        body2_to_(make_not_null_unique<Trajectory<To>>(&body2_)),
        satellite_from_(make_not_null_unique<Trajectory<From>>(&satellite_)),
        body1_fn_({body1_from_.get(), nullptr, body1_to_.get()}),
        body2_fn_({body2_from_.get(), nullptr, body2_to_.get()}),
        satellite_fn_({satellite_from_.get(), nullptr, nullptr}) {

    // The various bodies move have both a position and a velocity that
    // increases linearly with time.  This is not a situation that's physically
    // possible, but we don't care, all we want is to make sure that the
    // transforms are properly performed: |Transforms| doesn't know anything
    // about physics.  Also, the trajectories were chosen so that we are not in
    // any "special case" with respect to the positions or the velocities.
    for (int i = 1; i <= kNumberOfPoints; ++i) {
      body1_from_->Append(
          Instant(i * SIUnit<Time>()),
                  DegreesOfFreedom<From>(
                      Position<From>(
                          Displacement<From>({1 * i * SIUnit<Length>(),
                                              2 * i * SIUnit<Length>(),
                                              3 * i * SIUnit<Length>()})),
                      Velocity<From>({4 * i * SIUnit<Speed>(),
                                      8 * i * SIUnit<Speed>(),
                                      16 * i * SIUnit<Speed>()})));
      body2_from_->Append(
          Instant(i * SIUnit<Time>()),
                  DegreesOfFreedom<From>(
                      Position<From>(
                          Displacement<From>({-1 * i * SIUnit<Length>(),
                                              -2 * i * SIUnit<Length>(),
                                              3 * i * SIUnit<Length>()})),
                      Velocity<From>({-4 * i * SIUnit<Speed>(),
                                      8 * i * SIUnit<Speed>(),
                                      -16 * i * SIUnit<Speed>()})));
      satellite_from_->Append(
          Instant(i * SIUnit<Time>()),
                  DegreesOfFreedom<From>(
                      Position<From>(
                          Displacement<From>({10 * i * SIUnit<Length>(),
                                              -20 * i * SIUnit<Length>(),
                                              30 * i * SIUnit<Length>()})),
                      Velocity<From>({40 * i * SIUnit<Speed>(),
                                      -80 * i * SIUnit<Speed>(),
                                      160 * i * SIUnit<Speed>()})));
    }
  }

  MassiveBody body1_;
  MassiveBody body2_;
  MasslessBody satellite_;
  not_null<std::unique_ptr<Trajectory<From>>> body1_from_;
  not_null<std::unique_ptr<Trajectory<From>>> body2_from_;
  not_null<std::unique_ptr<Trajectory<To>>> body1_to_;
  not_null<std::unique_ptr<Trajectory<To>>> body2_to_;
  not_null<std::unique_ptr<Trajectory<From>>> satellite_from_;
  Functors body1_fn_;
  Functors body2_fn_;
  Functors satellite_fn_;
};

// This transform is simple enough that we can compute its effect by hand.  This
// test verifies that we get the expected result both in |Through| and in |To|.
TEST_F(TransformsTest, BodyCentredNonRotating) {
  auto const transforms =
      Transforms<Functors, From, Through, To>::BodyCentredNonRotating(
          body1_fn_, &Functors::to_trajectory);
  Trajectory<Through> body1_through(&body1_);

  int i = 1;
  for (auto it = transforms->first(satellite_fn_, &Functors::from_trajectory);
       !it.at_end();
       ++it, ++i) {
    DegreesOfFreedom<Through> const degrees_of_freedom =
        it.degrees_of_freedom();
    EXPECT_THAT(degrees_of_freedom,
                Componentwise(
                    Eq(Through::origin +
                       Displacement<Through>({9 * i * SIUnit<Length>(),
                                              -22 * i * SIUnit<Length>(),
                                              27 * i * SIUnit<Length>()})),
                    Eq(Velocity<Through>({36 * i * SIUnit<Speed>(),
                                          -88 * i * SIUnit<Speed>(),
                                          144 * i * SIUnit<Speed>()})))) << i;
    body1_through.Append(Instant(i * SIUnit<Time>()), degrees_of_freedom);
  }
  body1_fn_.through = &body1_through;

  i = 1;
  for (auto it = transforms->second(body1_fn_, &Functors::through_trajectory);
       !it.at_end();
       ++it, ++i) {
    body1_to_->Append(
        Instant(i * SIUnit<Time>()),
                DegreesOfFreedom<To>(
                    Position<To>(
                        Displacement<To>({3 * i * SIUnit<Length>(),
                                          1 * i * SIUnit<Length>(),
                                          2 * i * SIUnit<Length>()})),
                    Velocity<To>({16 * i * SIUnit<Speed>(),
                                  4 * i * SIUnit<Speed>(),
                                  8 * i * SIUnit<Speed>()})));

    DegreesOfFreedom<To> const degrees_of_freedom =
        it.degrees_of_freedom();
    EXPECT_THAT(degrees_of_freedom,
                Componentwise(
                    Eq(To::origin +
                        Displacement<To>({12 * i * SIUnit<Length>(),
                                          -21 * i * SIUnit<Length>(),
                                          29 * i * SIUnit<Length>()})),
                    Eq(Velocity<To>({36 * i * SIUnit<Speed>(),
                                      -88 * i * SIUnit<Speed>(),
                                      144 * i * SIUnit<Speed>()})))) << i;
  }
}

// Check that the computations we do match those done using Mathematica.
TEST_F(TransformsTest, SatelliteBarycentricRotating) {
  auto const transforms =
      Transforms<Functors, From, Through, To>::BarycentricRotating(
          body1_fn_, body2_fn_, &Functors::to_trajectory);
  Trajectory<Through> satellite_through(&satellite_);

  int i = 1;
  for (auto it = transforms->first(satellite_fn_, &Functors::from_trajectory);
       !it.at_end();
       ++it, ++i) {
    DegreesOfFreedom<Through> const degrees_of_freedom =
        it.degrees_of_freedom();
    EXPECT_THAT(degrees_of_freedom.position() - Position<Through>(),
                AlmostEquals(Displacement<Through>(
                    {-5.5 * sqrt(5.0) * i * SIUnit<Length>(),
                     62.0 * sqrt(5.0 / 21.0) * i * SIUnit<Length>(),
                     53.0 / sqrt(21.0) * i * SIUnit<Length>()}),
                    1, 8)) << i;
    EXPECT_THAT(degrees_of_freedom.velocity(),
                AlmostEquals(Velocity<Through>(
                    {(362.0 / sqrt(5.0)) * i * SIUnit<Speed>(),
                     (2776.0 / sqrt(105.0)) * i * SIUnit<Speed>(),
                     176.0 / sqrt(21.0) * i * SIUnit<Speed>()}),
                    1, 10)) << i;
    satellite_through.Append(Instant(i * SIUnit<Time>()), degrees_of_freedom);
  }
  satellite_fn_.through = &satellite_through;

  i = 1;
  for (auto it = transforms->second(satellite_fn_,
                                    &Functors::through_trajectory);
       !it.at_end();
       ++it, ++i) {
      body1_to_->Append(
          Instant(i * SIUnit<Time>()),
                  DegreesOfFreedom<To>(
                      Position<To>(
                          Displacement<To>({3 * i * SIUnit<Length>(),
                                            1 * i * SIUnit<Length>(),
                                            2 * i * SIUnit<Length>()})),
                      Velocity<To>({16 * i * SIUnit<Speed>(),
                                    4 * i * SIUnit<Speed>(),
                                    8 * i * SIUnit<Speed>()})));
      body2_to_->Append(
          Instant(i * SIUnit<Time>()),
                  DegreesOfFreedom<To>(
                      Position<To>(
                          Displacement<To>({3 * i * SIUnit<Length>(),
                                           -1 * i * SIUnit<Length>(),
                                           -2 * i * SIUnit<Length>()})),
                      Velocity<To>({-16 * i * SIUnit<Speed>(),
                                    4 * i * SIUnit<Speed>(),
                                    8 * i * SIUnit<Speed>()})));

    DegreesOfFreedom<To> const degrees_of_freedom =
        it.degrees_of_freedom();
    EXPECT_THAT(degrees_of_freedom.position() - To::origin,
                AlmostEquals(Displacement<To>(
                    {(3.0 + 62.0 * sqrt(5.0 / 21.0)) * i * SIUnit<Length>(),
                     (-6.0 + 106.0 / sqrt(105.0)) * i * SIUnit<Length>(),
                     (-12.0 - 53.0 / sqrt(105.0)) * i * SIUnit<Length>()}),
                    3, 21))
        << i;
    EXPECT_THAT(degrees_of_freedom.velocity(),
                AlmostEquals(Velocity<To>(
                    {2776.0 / sqrt(105.0) * i * SIUnit<Speed>(),
                     (72.4 + 352.0 / sqrt(105.0)) * i * SIUnit<Speed>(),
                     (144.8 - 176.0 / sqrt(105.0)) * i * SIUnit<Speed>()}),
                    1, 8)) << i;
  }
}

// Check that the bodies remain on the invariant line and at the right distance
// from each other and from the barycentre, and that the barycentre is the
// centre of the coordinates.
TEST_F(TransformsTest, BodiesBarycentricRotating) {
  auto const transforms =
      Transforms<Functors, From, Through, To>::BarycentricRotating(
          body1_fn_, body2_fn_, &Functors::to_trajectory);
  Trajectory<Through> body1_through(&body1_);
  Trajectory<Through> body2_through(&body2_);

  int i = 1;
  for (auto it1 = transforms->first(body1_fn_, &Functors::from_trajectory),
            it2 = transforms->first(body2_fn_, &Functors::from_trajectory);
       !it1.at_end() && !it2.at_end();
       ++it1, ++it2, ++i) {
    Length const l = i * SIUnit<Length>();
    Speed const s = i * SIUnit<Speed>();
    DegreesOfFreedom<Through> const degrees_of_freedom1 =
        it1.degrees_of_freedom();
    DegreesOfFreedom<Through> const degrees_of_freedom2 =
        it2.degrees_of_freedom();

    EXPECT_THAT(degrees_of_freedom1.position() - Through::origin,
                Componentwise(AlmostEquals(1.5 * sqrt(5.0) * l, 0, 1),
                              VanishesBefore(l, 0, 4),
                              VanishesBefore(l, 0, 2)));
    EXPECT_THAT(degrees_of_freedom2.position() - Through::origin,
                Componentwise(AlmostEquals(-0.5 * sqrt(5.0) * l, 0, 2),
                              VanishesBefore(l, 0, 2),
                              VanishesBefore(l, 0, 1)));
    EXPECT_THAT(degrees_of_freedom1.velocity(),
                Componentwise(AlmostEquals(6.0 / sqrt(5.0) * s, 0, 7),
                              VanishesBefore(s, 0, 34),
                              VanishesBefore(s, 0, 2)));
    EXPECT_THAT(degrees_of_freedom2.velocity(),
                Componentwise(AlmostEquals(-2.0 / sqrt(5.0) * s, 0, 14),
                              VanishesBefore(s, 0, 16),
                              VanishesBefore(s, 0, 1)));

    DegreesOfFreedom<Through> const barycentre_degrees_of_freedom =
        Barycentre<Through, Mass>({degrees_of_freedom1, degrees_of_freedom2},
                                  {body1_.mass(), body2_.mass()});
    EXPECT_THAT(barycentre_degrees_of_freedom.position() - Through::origin,
                Componentwise(VanishesBefore(l, 0, 1),
                              VanishesBefore(l, 0, 2),
                              VanishesBefore(l, 0, 1)));
    EXPECT_THAT(barycentre_degrees_of_freedom.velocity(),
                Componentwise(VanishesBefore(s, 0, 14),
                              VanishesBefore(s, 0, 7),
                              VanishesBefore(s, 0, 1)));

    Length const length = (degrees_of_freedom1.position() -
                           degrees_of_freedom2.position()).Norm();
    EXPECT_THAT(length,
                AlmostEquals(2.0 * sqrt(5.0) * i * SIUnit<Length>(), 0, 2));
  }
}

}  // namespace physics
}  // namespace principia
<commit_msg>Test passing.<commit_after>#include "physics/transforms.hpp"

#include <limits>

#include "geometry/frame.hpp"
#include "base/not_null.hpp"
#include "gmock/gmock.h"
#include "gtest/gtest.h"
#include "physics/degrees_of_freedom.hpp"
#include "physics/massive_body.hpp"
#include "physics/massless_body.hpp"
#include "quantities/named_quantities.hpp"
#include "quantities/quantities.hpp"
#include "serialization/geometry.pb.h"
#include "testing_utilities/almost_equals.hpp"
#include "testing_utilities/componentwise.hpp"
#include "testing_utilities/vanishes_before.hpp"

namespace principia {

using base::make_not_null_unique;
using geometry::Frame;
using quantities::Length;
using quantities::Mass;
using quantities::SIUnit;
using quantities::Speed;
using quantities::Time;
using testing_utilities::AlmostEquals;
using testing_utilities::Componentwise;
using testing_utilities::VanishesBefore;
using ::testing::Eq;
using ::testing::Lt;

namespace physics {

namespace {
const int kNumberOfPoints = 20;
}  // namespace

class TransformsTest : public testing::Test {
 protected:
  using From = Frame<serialization::Frame::TestTag,
                     serialization::Frame::FROM, true>;
  using Through = Frame<serialization::Frame::TestTag,
                        serialization::Frame::THROUGH, false>;
  using To = Frame<serialization::Frame::TestTag,
                   serialization::Frame::TO, true>;

  struct Functors {
    Trajectory<From> const& from_trajectory() const { return *from; }
    Trajectory<Through> const& through_trajectory() const { return *through; }
    Trajectory<To> const& to_trajectory() const { return *to; }

    Trajectory<From>* from;
    Trajectory<Through>* through;
    Trajectory<To>* to;
  };

  TransformsTest()
      : body1_(MassiveBody(1 * SIUnit<Mass>())),
        body2_(MassiveBody(3 * SIUnit<Mass>())),
        body1_from_(make_not_null_unique<Trajectory<From>>(&body1_)),
        body2_from_(make_not_null_unique<Trajectory<From>>(&body2_)),
        body1_to_(make_not_null_unique<Trajectory<To>>(&body1_)),
        body2_to_(make_not_null_unique<Trajectory<To>>(&body2_)),
        satellite_from_(make_not_null_unique<Trajectory<From>>(&satellite_)),
        body1_fn_({body1_from_.get(), nullptr, body1_to_.get()}),
        body2_fn_({body2_from_.get(), nullptr, body2_to_.get()}),
        satellite_fn_({satellite_from_.get(), nullptr, nullptr}) {

    // The various bodies move have both a position and a velocity that
    // increases linearly with time.  This is not a situation that's physically
    // possible, but we don't care, all we want is to make sure that the
    // transforms are properly performed: |Transforms| doesn't know anything
    // about physics.  Also, the trajectories were chosen so that we are not in
    // any "special case" with respect to the positions or the velocities.
    for (int i = 1; i <= kNumberOfPoints; ++i) {
      body1_from_->Append(
          Instant(i * SIUnit<Time>()),
                  DegreesOfFreedom<From>(
                      Position<From>(
                          Displacement<From>({1 * i * SIUnit<Length>(),
                                              2 * i * SIUnit<Length>(),
                                              3 * i * SIUnit<Length>()})),
                      Velocity<From>({4 * i * SIUnit<Speed>(),
                                      8 * i * SIUnit<Speed>(),
                                      16 * i * SIUnit<Speed>()})));
      body2_from_->Append(
          Instant(i * SIUnit<Time>()),
                  DegreesOfFreedom<From>(
                      Position<From>(
                          Displacement<From>({-1 * i * SIUnit<Length>(),
                                              -2 * i * SIUnit<Length>(),
                                              3 * i * SIUnit<Length>()})),
                      Velocity<From>({-4 * i * SIUnit<Speed>(),
                                      8 * i * SIUnit<Speed>(),
                                      -16 * i * SIUnit<Speed>()})));
      satellite_from_->Append(
          Instant(i * SIUnit<Time>()),
                  DegreesOfFreedom<From>(
                      Position<From>(
                          Displacement<From>({10 * i * SIUnit<Length>(),
                                              -20 * i * SIUnit<Length>(),
                                              30 * i * SIUnit<Length>()})),
                      Velocity<From>({40 * i * SIUnit<Speed>(),
                                      -80 * i * SIUnit<Speed>(),
                                      160 * i * SIUnit<Speed>()})));
    }
  }

  MassiveBody body1_;
  MassiveBody body2_;
  MasslessBody satellite_;
  not_null<std::unique_ptr<Trajectory<From>>> body1_from_;
  not_null<std::unique_ptr<Trajectory<From>>> body2_from_;
  not_null<std::unique_ptr<Trajectory<To>>> body1_to_;
  not_null<std::unique_ptr<Trajectory<To>>> body2_to_;
  not_null<std::unique_ptr<Trajectory<From>>> satellite_from_;
  Functors body1_fn_;
  Functors body2_fn_;
  Functors satellite_fn_;
};

// This transform is simple enough that we can compute its effect by hand.  This
// test verifies that we get the expected result both in |Through| and in |To|.
TEST_F(TransformsTest, BodyCentredNonRotating) {
  auto const transforms =
      Transforms<Functors, From, Through, To>::BodyCentredNonRotating(
          body1_fn_, &Functors::to_trajectory);
  Trajectory<Through> body1_through(&body1_);

  int i = 1;
  for (auto it = transforms->first(satellite_fn_, &Functors::from_trajectory);
       !it.at_end();
       ++it, ++i) {
    DegreesOfFreedom<Through> const degrees_of_freedom =
        it.degrees_of_freedom();
    EXPECT_THAT(degrees_of_freedom,
                Componentwise(
                    Eq(Through::origin +
                       Displacement<Through>({9 * i * SIUnit<Length>(),
                                              -22 * i * SIUnit<Length>(),
                                              27 * i * SIUnit<Length>()})),
                    Eq(Velocity<Through>({36 * i * SIUnit<Speed>(),
                                          -88 * i * SIUnit<Speed>(),
                                          144 * i * SIUnit<Speed>()})))) << i;
    body1_through.Append(Instant(i * SIUnit<Time>()), degrees_of_freedom);
  }
  body1_fn_.through = &body1_through;

  i = 1;
  for (auto it = transforms->second(body1_fn_, &Functors::through_trajectory);
       !it.at_end();
       ++it, ++i) {
    body1_to_->Append(
        Instant(i * SIUnit<Time>()),
                DegreesOfFreedom<To>(
                    Position<To>(
                        Displacement<To>({3 * i * SIUnit<Length>(),
                                          1 * i * SIUnit<Length>(),
                                          2 * i * SIUnit<Length>()})),
                    Velocity<To>({16 * i * SIUnit<Speed>(),
                                  4 * i * SIUnit<Speed>(),
                                  8 * i * SIUnit<Speed>()})));

    DegreesOfFreedom<To> const degrees_of_freedom =
        it.degrees_of_freedom();
    EXPECT_THAT(degrees_of_freedom,
                Componentwise(
                    Eq(To::origin +
                        Displacement<To>({12 * i * SIUnit<Length>(),
                                          -21 * i * SIUnit<Length>(),
                                          29 * i * SIUnit<Length>()})),
                    Eq(Velocity<To>({36 * i * SIUnit<Speed>(),
                                      -88 * i * SIUnit<Speed>(),
                                      144 * i * SIUnit<Speed>()})))) << i;
  }
}

// Check that the computations we do match those done using Mathematica.
TEST_F(TransformsTest, SatelliteBarycentricRotating) {
  auto const transforms =
      Transforms<Functors, From, Through, To>::BarycentricRotating(
          body1_fn_, body2_fn_, &Functors::to_trajectory);
  Trajectory<Through> satellite_through(&satellite_);

  int i = 1;
  for (auto it = transforms->first(satellite_fn_, &Functors::from_trajectory);
       !it.at_end();
       ++it, ++i) {
    DegreesOfFreedom<Through> const degrees_of_freedom =
        it.degrees_of_freedom();
    EXPECT_THAT(degrees_of_freedom.position() - Position<Through>(),
                AlmostEquals(Displacement<Through>(
                    {-5.5 * sqrt(5.0) * i * SIUnit<Length>(),
                     62.0 * sqrt(5.0 / 21.0) * i * SIUnit<Length>(),
                     53.0 / sqrt(21.0) * i * SIUnit<Length>()}),
                    1, 8)) << i;
    EXPECT_THAT(degrees_of_freedom.velocity(),
                AlmostEquals(Velocity<Through>(
                    {(362.0 / sqrt(5.0)) * i * SIUnit<Speed>(),
                     (2776.0 / sqrt(105.0)) * i * SIUnit<Speed>(),
                     176.0 / sqrt(21.0) * i * SIUnit<Speed>()}),
                    1, 10)) << i;
    satellite_through.Append(Instant(i * SIUnit<Time>()), degrees_of_freedom);
  }
  satellite_fn_.through = &satellite_through;

  i = 1;
  for (auto it = transforms->second(satellite_fn_,
                                    &Functors::through_trajectory);
       !it.at_end();
       ++it, ++i) {
      body1_to_->Append(
          Instant(i * SIUnit<Time>()),
                  DegreesOfFreedom<To>(
                      Position<To>(
                          Displacement<To>({3 * i * SIUnit<Length>(),
                                            1 * i * SIUnit<Length>(),
                                            2 * i * SIUnit<Length>()})),
                      Velocity<To>({16 * i * SIUnit<Speed>(),
                                    4 * i * SIUnit<Speed>(),
                                    8 * i * SIUnit<Speed>()})));
      body2_to_->Append(
          Instant(i * SIUnit<Time>()),
                  DegreesOfFreedom<To>(
                      Position<To>(
                          Displacement<To>({3 * i * SIUnit<Length>(),
                                           -1 * i * SIUnit<Length>(),
                                           -2 * i * SIUnit<Length>()})),
                      Velocity<To>({-16 * i * SIUnit<Speed>(),
                                    4 * i * SIUnit<Speed>(),
                                    8 * i * SIUnit<Speed>()})));

    DegreesOfFreedom<To> const degrees_of_freedom =
        it.degrees_of_freedom();
    EXPECT_THAT(degrees_of_freedom.position() - To::origin,
                AlmostEquals(Displacement<To>(
                    {(3.0 + 62.0 * sqrt(5.0 / 21.0)) * i * SIUnit<Length>(),
                     (-6.0 + 106.0 / sqrt(105.0)) * i * SIUnit<Length>(),
                     (-12.0 - 53.0 / sqrt(105.0)) * i * SIUnit<Length>()}),
                    3, 21))
        << i;
    EXPECT_THAT(degrees_of_freedom.velocity(),
                AlmostEquals(Velocity<To>(
                    {2776.0 / sqrt(105.0) * i * SIUnit<Speed>(),
                     (72.4 + 352.0 / sqrt(105.0)) * i * SIUnit<Speed>(),
                     (144.8 - 176.0 / sqrt(105.0)) * i * SIUnit<Speed>()}),
                    1, 8)) << i;
  }
}

// Check that the bodies remain on the invariant line and at the right distance
// from each other and from the barycentre, and that the barycentre is the
// centre of the coordinates.
TEST_F(TransformsTest, BodiesBarycentricRotating) {
  auto const transforms =
      Transforms<Functors, From, Through, To>::BarycentricRotating(
          body1_fn_, body2_fn_, &Functors::to_trajectory);
  Trajectory<Through> body1_through(&body1_);
  Trajectory<Through> body2_through(&body2_);

  int i = 1;
  for (auto it1 = transforms->first(body1_fn_, &Functors::from_trajectory),
            it2 = transforms->first(body2_fn_, &Functors::from_trajectory);
       !it1.at_end() && !it2.at_end();
       ++it1, ++it2, ++i) {
    Length const l = i * SIUnit<Length>();
    Speed const s = i * SIUnit<Speed>();
    DegreesOfFreedom<Through> const degrees_of_freedom1 =
        it1.degrees_of_freedom();
    DegreesOfFreedom<Through> const degrees_of_freedom2 =
        it2.degrees_of_freedom();

    EXPECT_THAT(degrees_of_freedom1.position() - Through::origin,
                Componentwise(AlmostEquals(1.5 * sqrt(5.0) * l, 0, 1),
                              VanishesBefore(l, 0, 4),
                              VanishesBefore(l, 0, 2)));
    EXPECT_THAT(degrees_of_freedom2.position() - Through::origin,
                Componentwise(AlmostEquals(-0.5 * sqrt(5.0) * l, 0, 2),
                              VanishesBefore(l, 0, 2),
                              VanishesBefore(l, 0, 1)));
    EXPECT_THAT(degrees_of_freedom1.velocity(),
                Componentwise(AlmostEquals(6.0 / sqrt(5.0) * s, 0, 7),
                              VanishesBefore(s, 0, 34),
                              VanishesBefore(s, 0, 2)));
    EXPECT_THAT(degrees_of_freedom2.velocity(),
                Componentwise(AlmostEquals(-2.0 / sqrt(5.0) * s, 0, 14),
                              VanishesBefore(s, 0, 16),
                              VanishesBefore(s, 0, 1)));

    DegreesOfFreedom<Through> const barycentre_degrees_of_freedom =
        Barycentre<Through, Mass>({degrees_of_freedom1, degrees_of_freedom2},
                                  {body1_.mass(), body2_.mass()});
    EXPECT_THAT(barycentre_degrees_of_freedom.position() - Through::origin,
                Componentwise(VanishesBefore(l, 0, 1),
                              VanishesBefore(l, 0, 2),
                              VanishesBefore(l, 0, 1)));
    EXPECT_THAT(barycentre_degrees_of_freedom.velocity(),
                Componentwise(VanishesBefore(s, 0, 14),
                              VanishesBefore(s, 0, 7),
                              VanishesBefore(s, 0, 1)));

    Length const length = (degrees_of_freedom1.position() -
                           degrees_of_freedom2.position()).Norm();
    EXPECT_THAT(length,
                AlmostEquals(2.0 * sqrt(5.0) * i * SIUnit<Length>(), 0, 2));
  }
}

}  // namespace physics
}  // namespace principia
<|endoftext|>


<commit_before>// Copyright (C) 2017 Jérôme Leclercq
// This file is part of the "Nazara Development Kit"
// For conditions of distribution and use, see copyright notice in Prerequisites.hpp

#include <NDK/Systems/PhysicsSystem2D.hpp>
#include <Nazara/Physics2D/RigidBody2D.hpp>
#include <NDK/World.hpp>
#include <NDK/Components/CollisionComponent2D.hpp>
#include <NDK/Components/NodeComponent.hpp>
#include <NDK/Components/PhysicsComponent2D.hpp>
#include <NDK/Components/PhysicsComponent3D.hpp>

namespace Ndk
{
	/*!
	* \ingroup NDK
	* \class Ndk::PhysicsSystem2D
	* \brief NDK class that represents a two-dimensional physics system
	*
	* \remark This system is enabled if the entity has the trait: NodeComponent and any of these two: CollisionComponent3D or PhysicsComponent3D
	* \remark Static objects do not have a velocity specified by the physical engine
	*/

	/*!
	* \brief Constructs an PhysicsSystem object by default
	*/

	PhysicsSystem2D::PhysicsSystem2D()
	{
		Requires<NodeComponent>();
		RequiresAny<CollisionComponent2D, PhysicsComponent2D>();
		Excludes<PhysicsComponent3D>();
	}

	void PhysicsSystem2D::CreatePhysWorld() const
	{
		NazaraAssert(!m_physWorld, "Physics world should not be created twice");

		m_physWorld = std::make_unique<Nz::PhysWorld2D>();
	}

	void PhysicsSystem2D::DebugDraw(const DebugDrawOptions& options, bool drawShapes, bool drawConstraints, bool drawCollisions)
	{
		Nz::PhysWorld2D::DebugDrawOptions worldOptions{ options.constraintColor, options.collisionPointColor, options.shapeOutlineColor };

		if (options.colorCallback)
		{
			worldOptions.colorCallback = [&options, this](Nz::RigidBody2D& body, std::size_t shapeIndex, void* userdata)
			{
				return options.colorCallback(GetEntityFromBody(body), shapeIndex, userdata);
			};
		}

		worldOptions.circleCallback = options.circleCallback;
		worldOptions.dotCallback = options.dotCallback;
		worldOptions.polygonCallback = options.polygonCallback;
		worldOptions.segmentCallback = options.segmentCallback;
		worldOptions.thickSegmentCallback = options.thickSegmentCallback;

		worldOptions.userdata = options.userdata;

		GetPhysWorld().DebugDraw(worldOptions, drawShapes, drawConstraints, drawCollisions);
	}

	const EntityHandle& PhysicsSystem2D::GetEntityFromBody(const Nz::RigidBody2D& body) const
	{
		auto entityId = static_cast<EntityId>(reinterpret_cast<std::uintptr_t>(body.GetUserdata()));

		auto& world = GetWorld();

		NazaraAssert(world.IsEntityIdValid(entityId), "All Bodies of this world must be part of the physics world by using PhysicsComponent");

		return world.GetEntity(entityId);
	}

	bool PhysicsSystem2D::NearestBodyQuery(const Nz::Vector2f& from, float maxDistance, Nz::UInt32 collisionGroup, Nz::UInt32 categoryMask, Nz::UInt32 collisionMask, EntityHandle* nearestBody)
	{
		Nz::RigidBody2D* body;
		bool res = GetPhysWorld().NearestBodyQuery(from, maxDistance, collisionGroup, categoryMask, collisionMask, &body);

		(*nearestBody) = GetEntityFromBody(*body);

		return res;
	}

	bool PhysicsSystem2D::NearestBodyQuery(const Nz::Vector2f& from, float maxDistance, Nz::UInt32 collisionGroup, Nz::UInt32 categoryMask, Nz::UInt32 collisionMask, NearestQueryResult* result)
	{
		Nz::PhysWorld2D::NearestQueryResult queryResult;
		bool res = GetPhysWorld().NearestBodyQuery(from, maxDistance, collisionGroup, categoryMask, collisionMask, &queryResult);

		result->nearestBody = GetEntityFromBody(*queryResult.nearestBody);
		result->closestPoint = std::move(queryResult.closestPoint);
		result->fraction = std::move(queryResult.fraction);
		result->distance = queryResult.distance;

		return res;
	}

	bool PhysicsSystem2D::RaycastQuery(const Nz::Vector2f& from, const Nz::Vector2f& to, float radius, Nz::UInt32 collisionGroup, Nz::UInt32 categoryMask, Nz::UInt32 collisionMask, std::vector<RaycastHit>* hitInfos)
	{
		std::vector<Nz::PhysWorld2D::RaycastHit> queryResult;
		bool res = GetPhysWorld().RaycastQuery(from, to, radius, collisionGroup, categoryMask, collisionMask, &queryResult);

		for (auto& hitResult : queryResult)
		{
			hitInfos->push_back({
				GetEntityFromBody(*hitResult.nearestBody),
				std::move(hitResult.hitPos),
				std::move(hitResult.hitNormal),
				hitResult.fraction
				});
		}

		return res;
	}

	bool PhysicsSystem2D::RaycastQueryFirst(const Nz::Vector2f& from, const Nz::Vector2f& to, float radius, Nz::UInt32 collisionGroup, Nz::UInt32 categoryMask, Nz::UInt32 collisionMask, RaycastHit* hitInfo)
	{
		Nz::PhysWorld2D::RaycastHit queryResult;
		bool res = GetPhysWorld().RaycastQueryFirst(from, to, radius, collisionGroup, categoryMask, collisionMask, &queryResult);

		hitInfo->body = GetEntityFromBody(*queryResult.nearestBody);
		hitInfo->hitPos = std::move(queryResult.hitPos);
		hitInfo->hitNormal = std::move(queryResult.hitNormal);
		hitInfo->fraction = queryResult.fraction;

		return res;
	}

	void PhysicsSystem2D::RegionQuery(const Nz::Rectf& boundingBox, Nz::UInt32 collisionGroup, Nz::UInt32 categoryMask, Nz::UInt32 collisionMask, std::vector<EntityHandle>* bodies)
	{
		std::vector<Nz::RigidBody2D*> queryResult;
		GetPhysWorld().RegionQuery(boundingBox, collisionGroup, categoryMask, collisionMask, &queryResult);

		for (auto& body : queryResult)
		{
			bodies->emplace_back(GetEntityFromBody(*body));
		}
	}

	/*!
	* \brief Operation to perform when entity is validated for the system
	*
	* \param entity Pointer to the entity
	* \param justAdded Is the entity newly added
	*/

	void PhysicsSystem2D::OnEntityValidation(Entity* entity, bool justAdded)
	{
		// It's possible our entity got revalidated because of the addition/removal of a PhysicsComponent3D
		if (!justAdded)
		{
			// We take the opposite array from which the entity should belong to
			auto& entities = (entity->HasComponent<PhysicsComponent2D>()) ? m_staticObjects : m_dynamicObjects;
			entities.Remove(entity);
		}

		auto& entities = (entity->HasComponent<PhysicsComponent2D>()) ? m_dynamicObjects : m_staticObjects;
		entities.Insert(entity);

		if (!m_physWorld)
			CreatePhysWorld();
	}

	/*!
	* \brief Operation to perform when system is updated
	*
	* \param elapsedTime Delta time used for the update
	*/

	void PhysicsSystem2D::OnUpdate(float elapsedTime)
	{
		if (!m_physWorld)
			return;

		m_physWorld->Step(elapsedTime);

		for (const Ndk::EntityHandle& entity : m_dynamicObjects)
		{
			NodeComponent& node = entity->GetComponent<NodeComponent>();
			PhysicsComponent2D& phys = entity->GetComponent<PhysicsComponent2D>();

			Nz::RigidBody2D* body = phys.GetRigidBody();
			node.SetRotation(Nz::EulerAnglesf(0.f, 0.f, body->GetRotation()), Nz::CoordSys_Global);
			node.SetPosition(Nz::Vector3f(body->GetPosition(), node.GetPosition(Nz::CoordSys_Global).z), Nz::CoordSys_Global);
		}

		float invElapsedTime = 1.f / elapsedTime;
		for (const Ndk::EntityHandle& entity : m_staticObjects)
		{
			CollisionComponent2D& collision = entity->GetComponent<CollisionComponent2D>();
			NodeComponent& node = entity->GetComponent<NodeComponent>();

			Nz::RigidBody2D* body = collision.GetStaticBody();

			Nz::Vector2f oldPosition = body->GetPosition();
			Nz::Vector2f newPosition = Nz::Vector2f(node.GetPosition(Nz::CoordSys_Global));

			// To move static objects and ensure their collisions, we have to specify them a velocity
			// (/!\: the physical motor does not apply the speed on static objects)
			if (newPosition != oldPosition)
			{
				body->SetPosition(newPosition);
				body->SetVelocity((newPosition - oldPosition) * invElapsedTime);
			}
			else
				body->SetVelocity(Nz::Vector2f::Zero());

/*
			if (newRotation != oldRotation)
			{
				Nz::Quaternionf transition = newRotation * oldRotation.GetConjugate();
				Nz::EulerAnglesf angles = transition.ToEulerAngles();
				Nz::Vector3f angularVelocity(Nz::ToRadians(angles.pitch * invElapsedTime),
				                             Nz::ToRadians(angles.yaw * invElapsedTime),
				                             Nz::ToRadians(angles.roll * invElapsedTime));

				physObj->SetRotation(oldRotation);
				physObj->SetAngularVelocity(angularVelocity);
			}
			else
				physObj->SetAngularVelocity(Nz::Vector3f::Zero());
*/
		}
	}

	void PhysicsSystem2D::RegisterCallbacks(unsigned int collisionId, const Callback& callbacks)
	{
		Nz::PhysWorld2D::Callback worldCallbacks;

		if (callbacks.endCallback)
		{
			worldCallbacks.endCallback = [&callbacks, this](Nz::PhysWorld2D& world, Nz::Arbiter2D& arbiter, Nz::RigidBody2D& bodyA, Nz::RigidBody2D& bodyB, void* userdata)
			{
				callbacks.endCallback(*this, arbiter, GetEntityFromBody(bodyA), GetEntityFromBody(bodyB), userdata);
			};
		}
		if (callbacks.preSolveCallback)
		{
			worldCallbacks.preSolveCallback = [&callbacks, this](Nz::PhysWorld2D& world, Nz::Arbiter2D& arbiter, Nz::RigidBody2D& bodyA, Nz::RigidBody2D& bodyB, void* userdata)
			{
				return callbacks.preSolveCallback(*this, arbiter, GetEntityFromBody(bodyA), GetEntityFromBody(bodyB), userdata);
			};
		}
		if (callbacks.postSolveCallback)
		{
			worldCallbacks.postSolveCallback = [&callbacks, this](Nz::PhysWorld2D& world, Nz::Arbiter2D& arbiter, Nz::RigidBody2D& bodyA, Nz::RigidBody2D& bodyB, void* userdata)
			{
				callbacks.preSolveCallback(*this, arbiter, GetEntityFromBody(bodyA), GetEntityFromBody(bodyB), userdata);
			};
		}
		if (callbacks.startCallback)
		{
			worldCallbacks.startCallback = [&callbacks, this](Nz::PhysWorld2D& world, Nz::Arbiter2D& arbiter, Nz::RigidBody2D& bodyA, Nz::RigidBody2D& bodyB, void* userdata)
			{
				return callbacks.startCallback(*this, arbiter, GetEntityFromBody(bodyA), GetEntityFromBody(bodyB), userdata);
			};
		}

		worldCallbacks.userdata = callbacks.userdata;

		m_physWorld->RegisterCallbacks(collisionId, worldCallbacks);
	}

	void PhysicsSystem2D::RegisterCallbacks(unsigned int collisionIdA, unsigned int collisionIdB, const Callback& callbacks)
	{
		Nz::PhysWorld2D::Callback worldCallbacks{};

		if (callbacks.endCallback)
		{
			worldCallbacks.endCallback = [&callbacks, this](Nz::PhysWorld2D& world, Nz::Arbiter2D& arbiter, Nz::RigidBody2D& bodyA, Nz::RigidBody2D& bodyB, void* userdata)
			{
				callbacks.endCallback(*this, arbiter, GetEntityFromBody(bodyA), GetEntityFromBody(bodyB), userdata);
			};
		}
		if (callbacks.preSolveCallback)
		{
			worldCallbacks.preSolveCallback = [&callbacks, this](Nz::PhysWorld2D& world, Nz::Arbiter2D& arbiter, Nz::RigidBody2D& bodyA, Nz::RigidBody2D& bodyB, void* userdata)
			{
				return callbacks.preSolveCallback(*this, arbiter, GetEntityFromBody(bodyA), GetEntityFromBody(bodyB), userdata);
			};
		}
		if (callbacks.postSolveCallback)
		{
			worldCallbacks.postSolveCallback = [&callbacks, this](Nz::PhysWorld2D& world, Nz::Arbiter2D& arbiter, Nz::RigidBody2D& bodyA, Nz::RigidBody2D& bodyB, void* userdata)
			{
				callbacks.preSolveCallback(*this, arbiter, GetEntityFromBody(bodyA), GetEntityFromBody(bodyB), userdata);
			};
		}
		if (callbacks.startCallback)
		{
			worldCallbacks.startCallback = [&callbacks, this](Nz::PhysWorld2D& world, Nz::Arbiter2D& arbiter, Nz::RigidBody2D& bodyA, Nz::RigidBody2D& bodyB, void* userdata)
			{
				return callbacks.startCallback(*this, arbiter, GetEntityFromBody(bodyA), GetEntityFromBody(bodyB), userdata);
			};
		}

		worldCallbacks.userdata = callbacks.userdata;

		m_physWorld->RegisterCallbacks(collisionIdA, collisionIdB, worldCallbacks);
	}

	SystemIndex PhysicsSystem2D::systemIndex;
}
<commit_msg>Fix (#184)<commit_after>// Copyright (C) 2017 Jérôme Leclercq
// This file is part of the "Nazara Development Kit"
// For conditions of distribution and use, see copyright notice in Prerequisites.hpp

#include <NDK/Systems/PhysicsSystem2D.hpp>
#include <Nazara/Physics2D/RigidBody2D.hpp>
#include <NDK/World.hpp>
#include <NDK/Components/CollisionComponent2D.hpp>
#include <NDK/Components/NodeComponent.hpp>
#include <NDK/Components/PhysicsComponent2D.hpp>
#include <NDK/Components/PhysicsComponent3D.hpp>

namespace Ndk
{
	/*!
	* \ingroup NDK
	* \class Ndk::PhysicsSystem2D
	* \brief NDK class that represents a two-dimensional physics system
	*
	* \remark This system is enabled if the entity has the trait: NodeComponent and any of these two: CollisionComponent3D or PhysicsComponent3D
	* \remark Static objects do not have a velocity specified by the physical engine
	*/

	/*!
	* \brief Constructs an PhysicsSystem object by default
	*/

	PhysicsSystem2D::PhysicsSystem2D()
	{
		Requires<NodeComponent>();
		RequiresAny<CollisionComponent2D, PhysicsComponent2D>();
		Excludes<PhysicsComponent3D>();
	}

	void PhysicsSystem2D::CreatePhysWorld() const
	{
		NazaraAssert(!m_physWorld, "Physics world should not be created twice");

		m_physWorld = std::make_unique<Nz::PhysWorld2D>();
	}

	void PhysicsSystem2D::DebugDraw(const DebugDrawOptions& options, bool drawShapes, bool drawConstraints, bool drawCollisions)
	{
		Nz::PhysWorld2D::DebugDrawOptions worldOptions{ options.constraintColor, options.collisionPointColor, options.shapeOutlineColor };

		if (options.colorCallback)
		{
			worldOptions.colorCallback = [&options, this](Nz::RigidBody2D& body, std::size_t shapeIndex, void* userdata)
			{
				return options.colorCallback(GetEntityFromBody(body), shapeIndex, userdata);
			};
		}

		worldOptions.circleCallback = options.circleCallback;
		worldOptions.dotCallback = options.dotCallback;
		worldOptions.polygonCallback = options.polygonCallback;
		worldOptions.segmentCallback = options.segmentCallback;
		worldOptions.thickSegmentCallback = options.thickSegmentCallback;

		worldOptions.userdata = options.userdata;

		GetPhysWorld().DebugDraw(worldOptions, drawShapes, drawConstraints, drawCollisions);
	}

	const EntityHandle& PhysicsSystem2D::GetEntityFromBody(const Nz::RigidBody2D& body) const
	{
		auto entityId = static_cast<EntityId>(reinterpret_cast<std::uintptr_t>(body.GetUserdata()));

		auto& world = GetWorld();

		NazaraAssert(world.IsEntityIdValid(entityId), "All Bodies of this world must be part of the physics world by using PhysicsComponent");

		return world.GetEntity(entityId);
	}

	bool PhysicsSystem2D::NearestBodyQuery(const Nz::Vector2f& from, float maxDistance, Nz::UInt32 collisionGroup, Nz::UInt32 categoryMask, Nz::UInt32 collisionMask, EntityHandle* nearestBody)
	{
		Nz::RigidBody2D* body;
		bool res = GetPhysWorld().NearestBodyQuery(from, maxDistance, collisionGroup, categoryMask, collisionMask, &body);

		(*nearestBody) = GetEntityFromBody(*body);

		return res;
	}

	bool PhysicsSystem2D::NearestBodyQuery(const Nz::Vector2f& from, float maxDistance, Nz::UInt32 collisionGroup, Nz::UInt32 categoryMask, Nz::UInt32 collisionMask, NearestQueryResult* result)
	{
		Nz::PhysWorld2D::NearestQueryResult queryResult;
		bool res = GetPhysWorld().NearestBodyQuery(from, maxDistance, collisionGroup, categoryMask, collisionMask, &queryResult);

		result->nearestBody = GetEntityFromBody(*queryResult.nearestBody);
		result->closestPoint = std::move(queryResult.closestPoint);
		result->fraction = std::move(queryResult.fraction);
		result->distance = queryResult.distance;

		return res;
	}

	bool PhysicsSystem2D::RaycastQuery(const Nz::Vector2f& from, const Nz::Vector2f& to, float radius, Nz::UInt32 collisionGroup, Nz::UInt32 categoryMask, Nz::UInt32 collisionMask, std::vector<RaycastHit>* hitInfos)
	{
		std::vector<Nz::PhysWorld2D::RaycastHit> queryResult;
		bool res = GetPhysWorld().RaycastQuery(from, to, radius, collisionGroup, categoryMask, collisionMask, &queryResult);

		for (auto& hitResult : queryResult)
		{
			hitInfos->push_back({
				GetEntityFromBody(*hitResult.nearestBody),
				std::move(hitResult.hitPos),
				std::move(hitResult.hitNormal),
				hitResult.fraction
				});
		}

		return res;
	}

	bool PhysicsSystem2D::RaycastQueryFirst(const Nz::Vector2f& from, const Nz::Vector2f& to, float radius, Nz::UInt32 collisionGroup, Nz::UInt32 categoryMask, Nz::UInt32 collisionMask, RaycastHit* hitInfo)
	{
		Nz::PhysWorld2D::RaycastHit queryResult;
		bool res = GetPhysWorld().RaycastQueryFirst(from, to, radius, collisionGroup, categoryMask, collisionMask, &queryResult);

		hitInfo->body = GetEntityFromBody(*queryResult.nearestBody);
		hitInfo->hitPos = std::move(queryResult.hitPos);
		hitInfo->hitNormal = std::move(queryResult.hitNormal);
		hitInfo->fraction = queryResult.fraction;

		return res;
	}

	void PhysicsSystem2D::RegionQuery(const Nz::Rectf& boundingBox, Nz::UInt32 collisionGroup, Nz::UInt32 categoryMask, Nz::UInt32 collisionMask, std::vector<EntityHandle>* bodies)
	{
		std::vector<Nz::RigidBody2D*> queryResult;
		GetPhysWorld().RegionQuery(boundingBox, collisionGroup, categoryMask, collisionMask, &queryResult);

		for (auto& body : queryResult)
		{
			bodies->emplace_back(GetEntityFromBody(*body));
		}
	}

	/*!
	* \brief Operation to perform when entity is validated for the system
	*
	* \param entity Pointer to the entity
	* \param justAdded Is the entity newly added
	*/

	void PhysicsSystem2D::OnEntityValidation(Entity* entity, bool justAdded)
	{
		// It's possible our entity got revalidated because of the addition/removal of a PhysicsComponent3D
		if (!justAdded)
		{
			// We take the opposite array from which the entity should belong to
			auto& entities = (entity->HasComponent<PhysicsComponent2D>()) ? m_staticObjects : m_dynamicObjects;
			entities.Remove(entity);
		}

		auto& entities = (entity->HasComponent<PhysicsComponent2D>()) ? m_dynamicObjects : m_staticObjects;
		entities.Insert(entity);

		if (!m_physWorld)
			CreatePhysWorld();
	}

	/*!
	* \brief Operation to perform when system is updated
	*
	* \param elapsedTime Delta time used for the update
	*/

	void PhysicsSystem2D::OnUpdate(float elapsedTime)
	{
		if (!m_physWorld)
			return;

		m_physWorld->Step(elapsedTime);

		for (const Ndk::EntityHandle& entity : m_dynamicObjects)
		{
			NodeComponent& node = entity->GetComponent<NodeComponent>();
			PhysicsComponent2D& phys = entity->GetComponent<PhysicsComponent2D>();

			Nz::RigidBody2D* body = phys.GetRigidBody();
			node.SetRotation(Nz::EulerAnglesf(0.f, 0.f, body->GetRotation()), Nz::CoordSys_Global);
			node.SetPosition(Nz::Vector3f(body->GetPosition(), node.GetPosition(Nz::CoordSys_Global).z), Nz::CoordSys_Global);
		}

		float invElapsedTime = 1.f / elapsedTime;
		for (const Ndk::EntityHandle& entity : m_staticObjects)
		{
			CollisionComponent2D& collision = entity->GetComponent<CollisionComponent2D>();
			NodeComponent& node = entity->GetComponent<NodeComponent>();

			Nz::RigidBody2D* body = collision.GetStaticBody();

			Nz::Vector2f oldPosition = body->GetPosition();
			Nz::Vector2f newPosition = Nz::Vector2f(node.GetPosition(Nz::CoordSys_Global));

			// To move static objects and ensure their collisions, we have to specify them a velocity
			// (/!\: the physical motor does not apply the speed on static objects)
			if (newPosition != oldPosition)
			{
				body->SetPosition(newPosition);
				body->SetVelocity((newPosition - oldPosition) * invElapsedTime);
			}
			else
				body->SetVelocity(Nz::Vector2f::Zero());

/*
			if (newRotation != oldRotation)
			{
				Nz::Quaternionf transition = newRotation * oldRotation.GetConjugate();
				Nz::EulerAnglesf angles = transition.ToEulerAngles();
				Nz::Vector3f angularVelocity(Nz::ToRadians(angles.pitch * invElapsedTime),
				                             Nz::ToRadians(angles.yaw * invElapsedTime),
				                             Nz::ToRadians(angles.roll * invElapsedTime));

				physObj->SetRotation(oldRotation);
				physObj->SetAngularVelocity(angularVelocity);
			}
			else
				physObj->SetAngularVelocity(Nz::Vector3f::Zero());
*/
		}
	}

	void PhysicsSystem2D::RegisterCallbacks(unsigned int collisionId, const Callback& callbacks)
	{
		Nz::PhysWorld2D::Callback worldCallbacks;

		if (callbacks.endCallback)
		{
			worldCallbacks.endCallback = [&callbacks, this](Nz::PhysWorld2D& world, Nz::Arbiter2D& arbiter, Nz::RigidBody2D& bodyA, Nz::RigidBody2D& bodyB, void* userdata)
			{
				callbacks.endCallback(*this, arbiter, GetEntityFromBody(bodyA), GetEntityFromBody(bodyB), userdata);
			};
		}
		if (callbacks.preSolveCallback)
		{
			worldCallbacks.preSolveCallback = [&callbacks, this](Nz::PhysWorld2D& world, Nz::Arbiter2D& arbiter, Nz::RigidBody2D& bodyA, Nz::RigidBody2D& bodyB, void* userdata)
			{
				return callbacks.preSolveCallback(*this, arbiter, GetEntityFromBody(bodyA), GetEntityFromBody(bodyB), userdata);
			};
		}
		if (callbacks.postSolveCallback)
		{
			worldCallbacks.postSolveCallback = [&callbacks, this](Nz::PhysWorld2D& world, Nz::Arbiter2D& arbiter, Nz::RigidBody2D& bodyA, Nz::RigidBody2D& bodyB, void* userdata)
			{
				callbacks.postSolveCallback(*this, arbiter, GetEntityFromBody(bodyA), GetEntityFromBody(bodyB), userdata);
			};
		}
		if (callbacks.startCallback)
		{
			worldCallbacks.startCallback = [&callbacks, this](Nz::PhysWorld2D& world, Nz::Arbiter2D& arbiter, Nz::RigidBody2D& bodyA, Nz::RigidBody2D& bodyB, void* userdata)
			{
				return callbacks.startCallback(*this, arbiter, GetEntityFromBody(bodyA), GetEntityFromBody(bodyB), userdata);
			};
		}

		worldCallbacks.userdata = callbacks.userdata;

		m_physWorld->RegisterCallbacks(collisionId, worldCallbacks);
	}

	void PhysicsSystem2D::RegisterCallbacks(unsigned int collisionIdA, unsigned int collisionIdB, const Callback& callbacks)
	{
		Nz::PhysWorld2D::Callback worldCallbacks{};

		if (callbacks.endCallback)
		{
			worldCallbacks.endCallback = [&callbacks, this](Nz::PhysWorld2D& world, Nz::Arbiter2D& arbiter, Nz::RigidBody2D& bodyA, Nz::RigidBody2D& bodyB, void* userdata)
			{
				callbacks.endCallback(*this, arbiter, GetEntityFromBody(bodyA), GetEntityFromBody(bodyB), userdata);
			};
		}
		if (callbacks.preSolveCallback)
		{
			worldCallbacks.preSolveCallback = [&callbacks, this](Nz::PhysWorld2D& world, Nz::Arbiter2D& arbiter, Nz::RigidBody2D& bodyA, Nz::RigidBody2D& bodyB, void* userdata)
			{
				return callbacks.preSolveCallback(*this, arbiter, GetEntityFromBody(bodyA), GetEntityFromBody(bodyB), userdata);
			};
		}
		if (callbacks.postSolveCallback)
		{
			worldCallbacks.postSolveCallback = [&callbacks, this](Nz::PhysWorld2D& world, Nz::Arbiter2D& arbiter, Nz::RigidBody2D& bodyA, Nz::RigidBody2D& bodyB, void* userdata)
			{
				callbacks.postSolveCallback(*this, arbiter, GetEntityFromBody(bodyA), GetEntityFromBody(bodyB), userdata);
			};
		}
		if (callbacks.startCallback)
		{
			worldCallbacks.startCallback = [&callbacks, this](Nz::PhysWorld2D& world, Nz::Arbiter2D& arbiter, Nz::RigidBody2D& bodyA, Nz::RigidBody2D& bodyB, void* userdata)
			{
				return callbacks.startCallback(*this, arbiter, GetEntityFromBody(bodyA), GetEntityFromBody(bodyB), userdata);
			};
		}

		worldCallbacks.userdata = callbacks.userdata;

		m_physWorld->RegisterCallbacks(collisionIdA, collisionIdB, worldCallbacks);
	}

	SystemIndex PhysicsSystem2D::systemIndex;
}
<|endoftext|>


<commit_before>#include <limits>
#include <cstdlib>
#include <ctype.h>
#define R_NO_REMAP
#include <R.h>
#include <Rinternals.h>
#include "omxState.h"
#include "omxMatrix.h"
#include "glue.h"
#include "nloptcpp.h"
#include "finiteDifferences.h"

#include "nlopt.h"

struct fit_functional {
	GradientOptimizerContext &goc;

	fit_functional(GradientOptimizerContext &goc) : goc(goc) {};

	template <typename T1>
	double operator()(Eigen::MatrixBase<T1>& x) const {
		int mode = 0;
		return goc.solFun(x.derived().data(), &mode);
	}
};

static double nloptObjectiveFunction(unsigned n, const double *x, double *grad, void *f_data)
{
	GradientOptimizerContext *goc = (GradientOptimizerContext *) f_data;
	nlopt_opt opt = (nlopt_opt) goc->extraData;
	FitContext *fc = goc->fc;
	assert(n == fc->numParam);
	int mode = 0;
	double fit = goc->solFun((double*) x, &mode);
	if (grad) {
		fc->iterations += 1;
		if (goc->maxMajorIterations != -1 && fc->iterations >= goc->maxMajorIterations) {
			nlopt_force_stop(opt);
		}
	}
	if (grad && goc->verbose >= 2) {
		mxLog("major iteration fit=%.12f", fit);
	}
	if (mode == -1) {
		if (!goc->feasible) {
			nlopt_force_stop(opt);
		}
		return nan("infeasible");
	}
	if (!grad) return fit;

	Eigen::Map< Eigen::VectorXd > Epoint((double*) x, n);
	Eigen::Map< Eigen::VectorXd > Egrad(grad, n);
	if (fc->wanted & FF_COMPUTE_GRADIENT) {
		Egrad = fc->grad;
	} else if (fc->CI && fc->CI->varIndex >= 0) {
		Egrad.setZero();
		Egrad[fc->CI->varIndex] = fc->lowerBound? 1 : -1;
		fc->grad = Egrad;
	} else {
		if (goc->verbose >= 3) mxLog("fd_gradient start");
		fit_functional ff(*goc);
		gradient_with_ref(goc->gradientAlgo, goc->gradientIterations, goc->gradientStepSize,
				  ff, fit, Epoint, Egrad);
		fc->grad = Egrad;
	}
	if (goc->verbose >= 3) {
		mxPrintMat("gradient", Egrad);
	}
	return fit;
}

struct equality_functional {
	GradientOptimizerContext &goc;

	equality_functional(GradientOptimizerContext &goc) : goc(goc) {};

	template <typename T1, typename T2>
	void operator()(Eigen::MatrixBase<T1> &x, Eigen::MatrixBase<T2> &result) const {
		memcpy(goc.fc->est, x.derived().data(), sizeof(double) * goc.fc->numParam);
		goc.fc->copyParamToModel();
		goc.solEqBFun();
		result = goc.equality;
	}
};

static void nloptEqualityFunction(unsigned m, double* result, unsigned n, const double* x, double* grad, void* f_data)
{
	GradientOptimizerContext *goc = (GradientOptimizerContext *) f_data;
	assert(n == goc->fc->numParam);
	Eigen::Map< Eigen::VectorXd > Epoint((double*)x, n);
	Eigen::Map< Eigen::VectorXd > Eresult(result, m);
	Eigen::Map< Eigen::MatrixXd > jacobian(grad, n, m);
	equality_functional ff(*goc);
	fd_jacobian(ff, Epoint, Eresult, grad==NULL, jacobian);
	//if (goc->verbose >= 3 && grad) std::cout << "equality:\n" << Eresult << "\n" << jacobian << "\n";
}

struct inequality_functional {
	GradientOptimizerContext &goc;

	inequality_functional(GradientOptimizerContext &goc) : goc(goc) {};

	template <typename T1, typename T2>
	void operator()(Eigen::MatrixBase<T1> &x, Eigen::MatrixBase<T2> &result) const {
		memcpy(goc.fc->est, x.derived().data(), sizeof(double) * goc.fc->numParam);
		goc.fc->copyParamToModel();
		goc.myineqFun();
		result = goc.inequality;
	}
};

static void nloptInequalityFunction(unsigned m, double *result, unsigned n, const double* x, double* grad, void* f_data)
{
	GradientOptimizerContext *goc = (GradientOptimizerContext *) f_data;
	assert(n == goc->fc->numParam);
	Eigen::Map< Eigen::VectorXd > Epoint((double*)x, n);
	Eigen::Map< Eigen::VectorXd > Eresult(result, m);
	Eigen::Map< Eigen::MatrixXd > jacobian(grad, n, m);
	if (grad && goc->verbose >= 2) {
		if (m == 1) {
			mxLog("major iteration ineq=%.12f", Eresult[0]);
		} else {
			mxPrintMat("major iteration ineq", Eresult);
		}
	}
	inequality_functional ff(*goc);
	fd_jacobian(ff, Epoint, Eresult, grad==NULL, jacobian);
	if (grad && !std::isfinite(Eresult.sum())) {
		// infeasible at start of major iteration
		nlopt_opt opt = (nlopt_opt) goc->extraData;
		nlopt_force_stop(opt);
	}
	if (goc->verbose >= 3 && grad) {
		mxPrintMat("inequality jacobian", jacobian);
	}
}

void omxInvokeNLOPT(double *est, GradientOptimizerContext &goc)
{
	goc.optName = "SLSQP";
	goc.setupSimpleBounds();
	goc.useGradient = true;

	FitContext *fc = goc.fc;
	int oldWanted = fc->wanted;
	fc->wanted = 0;
	omxState *globalState = fc->state;

        nlopt_opt opt = nlopt_create(NLOPT_LD_SLSQP, fc->numParam);
	goc.extraData = opt;
        //local_opt = nlopt_create(NLOPT_LD_SLSQP, n); // Subsidiary algorithm

        //nlopt_set_local_optimizer(opt, local_opt);
        nlopt_set_lower_bounds(opt, goc.solLB.data());
        nlopt_set_upper_bounds(opt, goc.solUB.data());

	int eq, ieq;
	globalState->countNonlinearConstraints(eq, ieq);

	if (fc->CI) {
		nlopt_set_xtol_rel(opt, 5e-3);
		std::vector<double> tol(fc->numParam, std::numeric_limits<double>::epsilon());
		nlopt_set_xtol_abs(opt, tol.data());
	} else {
		// The 0.01 factor is a bit ridiculous. NPSOL doesn't
		// have the same definition of relative tolerance
		// compare to SLSQP.
		nlopt_set_ftol_rel(opt, goc.ControlTolerance * 0.01);
		nlopt_set_ftol_abs(opt, std::numeric_limits<double>::epsilon());
	}

	nlopt_set_min_objective(opt, nloptObjectiveFunction, &goc);

        if (eq + ieq) {
		double feasibilityTolerance = Global->feasibilityTolerance;
                if (ieq > 0){
			goc.inequality.resize(ieq);
			std::vector<double> tol(ieq, feasibilityTolerance);
			nlopt_add_inequality_mconstraint(opt, ieq, nloptInequalityFunction, &goc, tol.data());
                }

                if (eq > 0){
			goc.equality.resize(eq);
			std::vector<double> tol(eq, feasibilityTolerance);
			nlopt_add_equality_mconstraint(opt, eq, nloptEqualityFunction, &goc, tol.data());
                }
	}

	int code = nlopt_optimize(opt, est, &fc->fit);
	if (goc.verbose >= 2) mxLog("nlopt_optimize returned %d", code);

        nlopt_destroy(opt);

	fc->wanted = oldWanted;

	if (code == NLOPT_INVALID_ARGS) {
		Rf_error("NLOPT invoked with invalid arguments");
	} else if (code == NLOPT_OUT_OF_MEMORY) {
		Rf_error("NLOPT ran out of memory");
	} else if (code == NLOPT_FORCED_STOP) {
		if (goc.maxMajorIterations != -1 && fc->iterations >= goc.maxMajorIterations) {
			goc.informOut = INFORM_ITERATION_LIMIT;
		} else {
			goc.informOut = INFORM_STARTING_VALUES_INFEASIBLE;
		}
	} else if (code == NLOPT_ROUNDOFF_LIMITED) {
		goc.informOut = INFORM_NOT_AT_OPTIMUM;  // is this correct? TODO
	} else if (code < 0) {
		Rf_error("NLOPT fatal error %d", code);
	} else if (eq + ieq == 0 && (fc->grad.array().abs() > 0.1).any()) {
		goc.informOut = INFORM_NOT_AT_OPTIMUM;
	} else if (code == NLOPT_MAXEVAL_REACHED) {
		goc.informOut = INFORM_ITERATION_LIMIT;
	} else {
		goc.informOut = INFORM_CONVERGED_OPTIMUM;
	}
}
<commit_msg>SLSQP: Increase default precision<commit_after>#include <limits>
#include <cstdlib>
#include <ctype.h>
#define R_NO_REMAP
#include <R.h>
#include <Rinternals.h>
#include "omxState.h"
#include "omxMatrix.h"
#include "glue.h"
#include "nloptcpp.h"
#include "finiteDifferences.h"

#include "nlopt.h"

struct fit_functional {
	GradientOptimizerContext &goc;

	fit_functional(GradientOptimizerContext &goc) : goc(goc) {};

	template <typename T1>
	double operator()(Eigen::MatrixBase<T1>& x) const {
		int mode = 0;
		return goc.solFun(x.derived().data(), &mode);
	}
};

static double nloptObjectiveFunction(unsigned n, const double *x, double *grad, void *f_data)
{
	GradientOptimizerContext *goc = (GradientOptimizerContext *) f_data;
	nlopt_opt opt = (nlopt_opt) goc->extraData;
	FitContext *fc = goc->fc;
	assert(n == fc->numParam);
	int mode = 0;
	double fit = goc->solFun((double*) x, &mode);
	if (grad) {
		fc->iterations += 1;
		if (goc->maxMajorIterations != -1 && fc->iterations >= goc->maxMajorIterations) {
			nlopt_force_stop(opt);
		}
	}
	if (grad && goc->verbose >= 2) {
		mxLog("major iteration fit=%.12f", fit);
	}
	if (mode == -1) {
		if (!goc->feasible) {
			nlopt_force_stop(opt);
		}
		return nan("infeasible");
	}
	if (!grad) return fit;

	Eigen::Map< Eigen::VectorXd > Epoint((double*) x, n);
	Eigen::Map< Eigen::VectorXd > Egrad(grad, n);
	if (fc->wanted & FF_COMPUTE_GRADIENT) {
		Egrad = fc->grad;
	} else if (fc->CI && fc->CI->varIndex >= 0) {
		Egrad.setZero();
		Egrad[fc->CI->varIndex] = fc->lowerBound? 1 : -1;
		fc->grad = Egrad;
	} else {
		if (goc->verbose >= 3) mxLog("fd_gradient start");
		fit_functional ff(*goc);
		gradient_with_ref(goc->gradientAlgo, goc->gradientIterations, goc->gradientStepSize,
				  ff, fit, Epoint, Egrad);
		fc->grad = Egrad;
	}
	if (goc->verbose >= 3) {
		mxPrintMat("gradient", Egrad);
	}
	return fit;
}

struct equality_functional {
	GradientOptimizerContext &goc;

	equality_functional(GradientOptimizerContext &goc) : goc(goc) {};

	template <typename T1, typename T2>
	void operator()(Eigen::MatrixBase<T1> &x, Eigen::MatrixBase<T2> &result) const {
		memcpy(goc.fc->est, x.derived().data(), sizeof(double) * goc.fc->numParam);
		goc.fc->copyParamToModel();
		goc.solEqBFun();
		result = goc.equality;
	}
};

static void nloptEqualityFunction(unsigned m, double* result, unsigned n, const double* x, double* grad, void* f_data)
{
	GradientOptimizerContext *goc = (GradientOptimizerContext *) f_data;
	assert(n == goc->fc->numParam);
	Eigen::Map< Eigen::VectorXd > Epoint((double*)x, n);
	Eigen::Map< Eigen::VectorXd > Eresult(result, m);
	Eigen::Map< Eigen::MatrixXd > jacobian(grad, n, m);
	equality_functional ff(*goc);
	fd_jacobian(ff, Epoint, Eresult, grad==NULL, jacobian);
	//if (goc->verbose >= 3 && grad) std::cout << "equality:\n" << Eresult << "\n" << jacobian << "\n";
}

struct inequality_functional {
	GradientOptimizerContext &goc;

	inequality_functional(GradientOptimizerContext &goc) : goc(goc) {};

	template <typename T1, typename T2>
	void operator()(Eigen::MatrixBase<T1> &x, Eigen::MatrixBase<T2> &result) const {
		memcpy(goc.fc->est, x.derived().data(), sizeof(double) * goc.fc->numParam);
		goc.fc->copyParamToModel();
		goc.myineqFun();
		result = goc.inequality;
	}
};

static void nloptInequalityFunction(unsigned m, double *result, unsigned n, const double* x, double* grad, void* f_data)
{
	GradientOptimizerContext *goc = (GradientOptimizerContext *) f_data;
	assert(n == goc->fc->numParam);
	Eigen::Map< Eigen::VectorXd > Epoint((double*)x, n);
	Eigen::Map< Eigen::VectorXd > Eresult(result, m);
	Eigen::Map< Eigen::MatrixXd > jacobian(grad, n, m);
	if (grad && goc->verbose >= 2) {
		if (m == 1) {
			mxLog("major iteration ineq=%.12f", Eresult[0]);
		} else {
			mxPrintMat("major iteration ineq", Eresult);
		}
	}
	inequality_functional ff(*goc);
	fd_jacobian(ff, Epoint, Eresult, grad==NULL, jacobian);
	if (grad && !std::isfinite(Eresult.sum())) {
		// infeasible at start of major iteration
		nlopt_opt opt = (nlopt_opt) goc->extraData;
		nlopt_force_stop(opt);
	}
	if (goc->verbose >= 3 && grad) {
		mxPrintMat("inequality jacobian", jacobian);
	}
}

void omxInvokeNLOPT(double *est, GradientOptimizerContext &goc)
{
	goc.optName = "SLSQP";
	goc.setupSimpleBounds();
	goc.useGradient = true;

	FitContext *fc = goc.fc;
	int oldWanted = fc->wanted;
	fc->wanted = 0;
	omxState *globalState = fc->state;

        nlopt_opt opt = nlopt_create(NLOPT_LD_SLSQP, fc->numParam);
	goc.extraData = opt;
        //local_opt = nlopt_create(NLOPT_LD_SLSQP, n); // Subsidiary algorithm

        //nlopt_set_local_optimizer(opt, local_opt);
        nlopt_set_lower_bounds(opt, goc.solLB.data());
        nlopt_set_upper_bounds(opt, goc.solUB.data());

	int eq, ieq;
	globalState->countNonlinearConstraints(eq, ieq);

	if (fc->CI) {
		nlopt_set_xtol_rel(opt, 5e-3);
		std::vector<double> tol(fc->numParam, std::numeric_limits<double>::epsilon());
		nlopt_set_xtol_abs(opt, tol.data());
	} else {
		// The 0.005 factor is a bit ridiculous. NPSOL doesn't
		// have the same definition of relative tolerance
		// compare to SLSQP.
		nlopt_set_ftol_rel(opt, goc.ControlTolerance * 0.005);
		nlopt_set_ftol_abs(opt, std::numeric_limits<double>::epsilon());
	}

	nlopt_set_min_objective(opt, nloptObjectiveFunction, &goc);

        if (eq + ieq) {
		double feasibilityTolerance = Global->feasibilityTolerance;
                if (ieq > 0){
			goc.inequality.resize(ieq);
			std::vector<double> tol(ieq, feasibilityTolerance);
			nlopt_add_inequality_mconstraint(opt, ieq, nloptInequalityFunction, &goc, tol.data());
                }

                if (eq > 0){
			goc.equality.resize(eq);
			std::vector<double> tol(eq, feasibilityTolerance);
			nlopt_add_equality_mconstraint(opt, eq, nloptEqualityFunction, &goc, tol.data());
                }
	}

	int code = nlopt_optimize(opt, est, &fc->fit);
	if (goc.verbose >= 2) mxLog("nlopt_optimize returned %d", code);

        nlopt_destroy(opt);

	fc->wanted = oldWanted;

	if (code == NLOPT_INVALID_ARGS) {
		Rf_error("NLOPT invoked with invalid arguments");
	} else if (code == NLOPT_OUT_OF_MEMORY) {
		Rf_error("NLOPT ran out of memory");
	} else if (code == NLOPT_FORCED_STOP) {
		if (goc.maxMajorIterations != -1 && fc->iterations >= goc.maxMajorIterations) {
			goc.informOut = INFORM_ITERATION_LIMIT;
		} else {
			goc.informOut = INFORM_STARTING_VALUES_INFEASIBLE;
		}
	} else if (code == NLOPT_ROUNDOFF_LIMITED) {
		goc.informOut = INFORM_NOT_AT_OPTIMUM;  // is this correct? TODO
	} else if (code < 0) {
		Rf_error("NLOPT fatal error %d", code);
	} else if (eq + ieq == 0 && (fc->grad.array().abs() > 0.1).any()) {
		goc.informOut = INFORM_NOT_AT_OPTIMUM;
	} else if (code == NLOPT_MAXEVAL_REACHED) {
		goc.informOut = INFORM_ITERATION_LIMIT;
	} else {
		goc.informOut = INFORM_CONVERGED_OPTIMUM;
	}
}
<|endoftext|>


<commit_before>#include "role.hpp"

void
Role :: periodic(uint64_t ts) {
	switch (m_state) {
	case Leader:
		periodic_leader(ts);
		break;
	case PotentialLeader:
		periodic_potential_leader(ts);
		break;
	case Follower:
		periodic_follower(ts);
		break;
	}
}

void
Role :: periodic_leader(uint64_t ts) {
	if (m_leader_data->m_pending_round == 0) {
		// No pending round.
		if (ts - m_leader_data->m_last_broadcast < 50e6) {
			// Not enough time has passed to send a regular heartbeat.
			return;
		}
	}
	if (m_leader_data->m_acks.size() >= m_cluster_size/2) {
		// Received required votes.
		uint64_t max_round = m_round;
		for (auto it = m_leader_data->m_acks.begin(); it != m_leader_data->m_acks.end(); ++it) {
			if (it->second > max_round) {
				max_round = it->second;
			}
		}
		auto pending_round_votes = 0;
		for (auto it = m_leader_data->m_acks.begin(); it != m_leader_data->m_acks.end(); ++it) {
			if (it->second == max_round) {
				pending_round_votes++;
			}
		}

		if (m_leader_data->m_pending_round > 0) {
			// There's a pending append.
			if (max_round != m_leader_data->m_pending_round ||
				pending_round_votes < m_cluster_size/2) {
				// Not enough votes for the pending append
				// We need to forfeit leadership.
				if (m_leader_data->m_callback != nullptr) {
					// Append was not confirmed by a majority.
					m_leader_data->m_callback(-1, m_leader_data->m_callback_data);
					m_leader_data->m_callback = nullptr;
					m_leader_data->m_callback_data = nullptr;
					m_leader_data->m_pending_round = 0;
				}
				m_leader_data = nullptr;
				m_state = PotentialLeader;
				m_potential_leader_data = std::make_unique<PotentialLeaderData>();
				return;
			}

			if (m_leader_data->m_callback != nullptr) {
				// Append was confirmed by a majority.
				m_leader_data->m_callback(0, m_leader_data->m_callback_data);
				m_leader_data->m_callback = nullptr;
				m_leader_data->m_callback_data = nullptr;
				m_round = m_leader_data->m_pending_round;
				m_leader_data->m_pending_round = 0;
			}
		} else {
			// No pending append
			// This is just a regular heartbeat ack.
			if (max_round > m_round) {
				m_round = max_round;
			}
		}

		++m_seq;
		auto round = m_round;
		if (m_leader_data->m_pending_round > 0) {
			round = m_leader_data->m_pending_round;
		}
		LeaderActiveMessage msg(m_id, m_seq, round);
		m_registry.broadcast(&msg);
		m_leader_data->m_last_broadcast = ts;
		m_leader_data->m_acks.clear();
		return;
	}
	if (ts - m_leader_data->m_last_broadcast > 300e6) {
		// It's been over 300 ms since the last broadcast.
		// Didn't get a majority. We're not a leader anymore.
		if (m_leader_data->m_callback != nullptr) {
			// Append was not confirmed by a majority.
			m_leader_data->m_callback(-1, m_leader_data->m_callback_data);
			m_leader_data->m_callback = nullptr;
			m_leader_data->m_callback_data = nullptr;
			m_leader_data->m_pending_round = 0;
		}
		if (m_client_callbacks.lost_leadership != nullptr) {
			m_client_callbacks.lost_leadership(m_client_callbacks_data);
		}
		m_leader_data = nullptr;
		m_state = PotentialLeader;
		m_potential_leader_data = std::make_unique<PotentialLeaderData>();
		return;
	}
}

void
Role :: periodic_potential_leader(uint64_t ts) {
	if (ts - m_potential_leader_data->m_last_broadcast > 300e6) {
		// It's been over 300 ms since the last broadcast.
		if (m_potential_leader_data->m_acks.size() >= m_cluster_size/2) {
			// Got a majority. We're now a leader.
			if (m_client_callbacks.gained_leadership != nullptr) {
				m_client_callbacks.gained_leadership(m_client_callbacks_data);
			}
			m_leader_data = std::make_unique<LeaderData>();
			m_leader_data->m_last_broadcast = m_potential_leader_data->m_last_broadcast;
			m_leader_data->m_acks = m_potential_leader_data->m_acks;
			m_potential_leader_data = nullptr;
			m_state = Leader;
			return;
		}

		// Try again.
		++m_seq;
		m_potential_leader_data->m_acks.clear();
		LeaderActiveMessage msg(m_id, m_seq, m_round);
		m_registry.broadcast(&msg);
		m_potential_leader_data->m_last_broadcast = ts;
	}
}

void
Role :: periodic_follower(uint64_t ts) {
	if (m_follower_data->m_last_leader_active == 0) {
		// Initialize m_last_leader_active.
		m_follower_data->m_last_leader_active = ts;
		return;
	}

	if (ts - m_follower_data->m_last_leader_active > 1000e6) {
		// Leader hasn't been active for over 1000 ms
		m_follower_data = nullptr;
		m_state = PotentialLeader;
		m_potential_leader_data = std::make_unique<PotentialLeaderData>();
	}
}

void
Role :: handle_leader_active(uint64_t ts, const LeaderActiveMessage& msg) {
	if (m_state != Follower) {
		if (msg.id < m_id) {
			// Other node has more authority. Drop down to follower state.
			if (m_state == Leader) {
				if (m_leader_data->m_callback != nullptr) {
					// Append was not confirmed by a majority.
					m_leader_data->m_callback(-1, m_leader_data->m_callback_data);
					m_leader_data->m_callback = nullptr;
					m_leader_data->m_callback_data = nullptr;
				}
				if (m_client_callbacks.lost_leadership != nullptr) {
					m_client_callbacks.lost_leadership(m_client_callbacks_data);
				}
				m_leader_data = nullptr;
			}
			m_follower_data = std::make_unique<FollowerData>();
			m_potential_leader_data = nullptr;
			m_state = Follower;
			m_follower_data->m_current_leader = msg.id;
			if (m_client_callbacks.on_leader_change != nullptr) {
				m_client_callbacks.on_leader_change(msg.id, m_client_callbacks_data);
			}
		} else {
			return;
		}
	}

	if (m_id < msg.id) {
		// We're more authoritative.
		// Ignore this message.
		return;
	}

	if (m_follower_data->m_current_leader > msg.id) {
		// Our current leader is less authoritative. Replace.
		m_follower_data->m_current_leader = msg.id;
		if (m_client_callbacks.on_leader_change != nullptr) {
			m_client_callbacks.on_leader_change(msg.id, m_client_callbacks_data);
		}
		m_follower_data->m_pending_round = 0;
	} else if (m_follower_data->m_current_leader < msg.id) {
		// Less authoritative than the current leader.
		// Ignore this message.
		return;
	}

	if (msg.round > m_round) {
		m_round = msg.round;
	}

	if (m_follower_data->m_pending_round != 0) {
		// We haven't confirmed a previous append.
		// Ignore all other messages.
	}

	if (msg.next != 0) {
		// Append message
		if (m_client_callbacks.on_append != nullptr) {
			m_seq = msg.seq;
			m_client_callbacks.on_append(msg.next, msg.next_content.c_str(),
				msg.next_content.size(), m_client_callbacks_data);
			m_follower_data->m_last_leader_active = ts;
			m_follower_data->m_pending_round = msg.next;
			return;
		}
	}

	// Normal heartbeat
	// Send ack
	LeaderActiveAck ack(m_id, msg.seq, m_round);
	m_registry.send_to_index(msg.source, &ack);
	if (m_follower_data->m_current_leader != msg.id) {
		if (m_client_callbacks.on_leader_change != nullptr) {
			m_client_callbacks.on_leader_change(msg.id, m_client_callbacks_data);
		}
	}
	m_follower_data->m_current_leader = msg.id;
	m_follower_data->m_last_leader_active = ts;
}

void
Role :: handle_leader_active_ack(uint64_t ts, const LeaderActiveAck& msg) {
	if (m_state == Follower) {
		// Nothing to do.
		return;
	}

	if (msg.seq != m_seq) {
		// message is too old
		return;
	}

	if (m_state == Leader) {
		m_leader_data->m_acks[msg.id] = msg.round;
		periodic_leader(ts);
	} else {
		m_potential_leader_data->m_acks[msg.id] = msg.round;
	}
}
<commit_msg>allow single-node cluster appends (#43)<commit_after>#include "role.hpp"

void
Role :: periodic(uint64_t ts) {
	switch (m_state) {
	case Leader:
		periodic_leader(ts);
		break;
	case PotentialLeader:
		periodic_potential_leader(ts);
		break;
	case Follower:
		periodic_follower(ts);
		break;
	}
}

void
Role :: periodic_leader(uint64_t ts) {
	if (m_leader_data->m_pending_round == 0) {
		// No pending round.
		if (ts - m_leader_data->m_last_broadcast < 50e6) {
			// Not enough time has passed to send a regular heartbeat.
			return;
		}
	}
	if (m_leader_data->m_acks.size() >= m_cluster_size/2) {
		// Received required votes.
		uint64_t max_round = m_round;
		for (auto it = m_leader_data->m_acks.begin(); it != m_leader_data->m_acks.end(); ++it) {
			if (it->second > max_round) {
				max_round = it->second;
			}
		}
		auto pending_round_votes = 0;
		for (auto it = m_leader_data->m_acks.begin(); it != m_leader_data->m_acks.end(); ++it) {
			if (it->second == max_round) {
				pending_round_votes++;
			}
		}

		if (m_leader_data->m_pending_round > 0) {
			// There's a pending append.
			if ((max_round != m_leader_data->m_pending_round ||
				pending_round_votes < m_cluster_size/2) &&
				m_cluster_size > 1) {
				// Not enough votes for the pending append
				// We need to forfeit leadership.
				if (m_leader_data->m_callback != nullptr) {
					// Append was not confirmed by a majority.
					m_leader_data->m_callback(-1, m_leader_data->m_callback_data);
					m_leader_data->m_callback = nullptr;
					m_leader_data->m_callback_data = nullptr;
					m_leader_data->m_pending_round = 0;
				}
				m_leader_data = nullptr;
				m_state = PotentialLeader;
				m_potential_leader_data = std::make_unique<PotentialLeaderData>();
				return;
			}

			if (m_leader_data->m_callback != nullptr) {
				// Append was confirmed by a majority.
				m_leader_data->m_callback(0, m_leader_data->m_callback_data);
				m_leader_data->m_callback = nullptr;
				m_leader_data->m_callback_data = nullptr;
				m_round = m_leader_data->m_pending_round;
				m_leader_data->m_pending_round = 0;
			}
		} else {
			// No pending append
			// This is just a regular heartbeat ack.
			if (max_round > m_round) {
				m_round = max_round;
			}
		}

		++m_seq;
		auto round = m_round;
		if (m_leader_data->m_pending_round > 0) {
			round = m_leader_data->m_pending_round;
		}
		LeaderActiveMessage msg(m_id, m_seq, round);
		m_registry.broadcast(&msg);
		m_leader_data->m_last_broadcast = ts;
		m_leader_data->m_acks.clear();
		return;
	}
	if (ts - m_leader_data->m_last_broadcast > 300e6) {
		// It's been over 300 ms since the last broadcast.
		// Didn't get a majority. We're not a leader anymore.
		if (m_leader_data->m_callback != nullptr) {
			// Append was not confirmed by a majority.
			m_leader_data->m_callback(-1, m_leader_data->m_callback_data);
			m_leader_data->m_callback = nullptr;
			m_leader_data->m_callback_data = nullptr;
			m_leader_data->m_pending_round = 0;
		}
		if (m_client_callbacks.lost_leadership != nullptr) {
			m_client_callbacks.lost_leadership(m_client_callbacks_data);
		}
		m_leader_data = nullptr;
		m_state = PotentialLeader;
		m_potential_leader_data = std::make_unique<PotentialLeaderData>();
		return;
	}
}

void
Role :: periodic_potential_leader(uint64_t ts) {
	if (ts - m_potential_leader_data->m_last_broadcast > 300e6) {
		// It's been over 300 ms since the last broadcast.
		if (m_potential_leader_data->m_acks.size() >= m_cluster_size/2) {
			// Got a majority. We're now a leader.
			if (m_client_callbacks.gained_leadership != nullptr) {
				m_client_callbacks.gained_leadership(m_client_callbacks_data);
			}
			m_leader_data = std::make_unique<LeaderData>();
			m_leader_data->m_last_broadcast = m_potential_leader_data->m_last_broadcast;
			m_leader_data->m_acks = m_potential_leader_data->m_acks;
			m_potential_leader_data = nullptr;
			m_state = Leader;
			return;
		}

		// Try again.
		++m_seq;
		m_potential_leader_data->m_acks.clear();
		LeaderActiveMessage msg(m_id, m_seq, m_round);
		m_registry.broadcast(&msg);
		m_potential_leader_data->m_last_broadcast = ts;
	}
}

void
Role :: periodic_follower(uint64_t ts) {
	if (m_follower_data->m_last_leader_active == 0) {
		// Initialize m_last_leader_active.
		m_follower_data->m_last_leader_active = ts;
		return;
	}

	if (ts - m_follower_data->m_last_leader_active > 1000e6) {
		// Leader hasn't been active for over 1000 ms
		m_follower_data = nullptr;
		m_state = PotentialLeader;
		m_potential_leader_data = std::make_unique<PotentialLeaderData>();
	}
}

void
Role :: handle_leader_active(uint64_t ts, const LeaderActiveMessage& msg) {
	if (m_state != Follower) {
		if (msg.id < m_id) {
			// Other node has more authority. Drop down to follower state.
			if (m_state == Leader) {
				if (m_leader_data->m_callback != nullptr) {
					// Append was not confirmed by a majority.
					m_leader_data->m_callback(-1, m_leader_data->m_callback_data);
					m_leader_data->m_callback = nullptr;
					m_leader_data->m_callback_data = nullptr;
				}
				if (m_client_callbacks.lost_leadership != nullptr) {
					m_client_callbacks.lost_leadership(m_client_callbacks_data);
				}
				m_leader_data = nullptr;
			}
			m_follower_data = std::make_unique<FollowerData>();
			m_potential_leader_data = nullptr;
			m_state = Follower;
			m_follower_data->m_current_leader = msg.id;
			if (m_client_callbacks.on_leader_change != nullptr) {
				m_client_callbacks.on_leader_change(msg.id, m_client_callbacks_data);
			}
		} else {
			return;
		}
	}

	if (m_id < msg.id) {
		// We're more authoritative.
		// Ignore this message.
		return;
	}

	if (m_follower_data->m_current_leader > msg.id) {
		// Our current leader is less authoritative. Replace.
		m_follower_data->m_current_leader = msg.id;
		if (m_client_callbacks.on_leader_change != nullptr) {
			m_client_callbacks.on_leader_change(msg.id, m_client_callbacks_data);
		}
		m_follower_data->m_pending_round = 0;
	} else if (m_follower_data->m_current_leader < msg.id) {
		// Less authoritative than the current leader.
		// Ignore this message.
		return;
	}

	if (msg.round > m_round) {
		m_round = msg.round;
	}

	if (m_follower_data->m_pending_round != 0) {
		// We haven't confirmed a previous append.
		// Ignore all other messages.
	}

	if (msg.next != 0) {
		// Append message
		if (m_client_callbacks.on_append != nullptr) {
			m_seq = msg.seq;
			m_client_callbacks.on_append(msg.next, msg.next_content.c_str(),
				msg.next_content.size(), m_client_callbacks_data);
			m_follower_data->m_last_leader_active = ts;
			m_follower_data->m_pending_round = msg.next;
			return;
		}
	}

	// Normal heartbeat
	// Send ack
	LeaderActiveAck ack(m_id, msg.seq, m_round);
	m_registry.send_to_index(msg.source, &ack);
	if (m_follower_data->m_current_leader != msg.id) {
		if (m_client_callbacks.on_leader_change != nullptr) {
			m_client_callbacks.on_leader_change(msg.id, m_client_callbacks_data);
		}
	}
	m_follower_data->m_current_leader = msg.id;
	m_follower_data->m_last_leader_active = ts;
}

void
Role :: handle_leader_active_ack(uint64_t ts, const LeaderActiveAck& msg) {
	if (m_state == Follower) {
		// Nothing to do.
		return;
	}

	if (msg.seq != m_seq) {
		// message is too old
		return;
	}

	if (m_state == Leader) {
		m_leader_data->m_acks[msg.id] = msg.round;
		periodic_leader(ts);
	} else {
		m_potential_leader_data->m_acks[msg.id] = msg.round;
	}
}
<|endoftext|>


<commit_before>/* mbed Microcontroller Library
 * Copyright (c) 2017 ARM Limited
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "utest/utest.h"
#include "unity/unity.h"
#include "greentea-client/test_env.h"

#include "mbed.h"
#include "mbed_mktime.h"

#define LAST_VALID_YEAR 206

using namespace utest::v1;

static rtc_leap_year_support_t rtc_leap_year_support;

/*  Regular is_leap_year, see platform/mbed_mktime.c for the optimised version. */
bool is_leap_year(int year)
{
    year = 1900 + year;
    if (year % 4) {
        return false;
    } else if (year % 100) {
        return true;
    } else if (year % 400) {
        return false;
    }
    return true;
}

/* Test the optimised version of _rtc_is_leap_year() against the generic version.
 *
 * Note: This test case is designed for both types of RTC devices:
 *       - RTC devices which handle correctly leap years in whole range (1970 - 2106).
 *       - RTC devices which does not handle correctly leap years in whole range (1970 - 2106).
 *         This RTC devices uses simpler leap year detection and incorrectly treat 2100 as a leap year.
 *       rtc_leap_year_support variable specifies which device is tested.
 *
 * Given is year in valid range.
 * When _rtc_is_leap_year() function is called.
 * Then _rtc_is_leap_year() returns true if given year is a leap year; false otherwise.
 */
void test_is_leap_year()
{
    for (int i = 70; i <= LAST_VALID_YEAR; ++i) {
        bool expected = is_leap_year(i);

        /* Add exception for year 2100. */
        if (rtc_leap_year_support == RTC_4_YEAR_LEAP_YEAR_SUPPORT && i == 200) {
            expected = true;
        }

        bool actual_value = _rtc_is_leap_year(i, rtc_leap_year_support);

        if (expected != actual_value) {
            printf("Leap year failed with i = %d\r\n", i);
        }
        TEST_ASSERT_EQUAL(expected, actual_value);
    }
}

/* Structure to test border values for _rtc_maketime(). */
typedef struct {
    struct tm timeinfo;
    time_t exp_seconds;  // if result is false then exp_seconds is irrelevant
    bool result;
} test_mk_time_struct;

/* Test boundary values for _rtc_maketime().
 *
 * Note: This test case is designed for both types of RTC devices:
 *       - RTC devices which handle correctly leap years in whole range (1970 - 2106).
 *       - RTC devices which does not handle correctly leap years in whole range (1970 - 2106).
 *         This RTC devices uses simpler leap year detection and incorrectly treat 2100 as a leap year.
 *       rtc_leap_year_support variable specifies which device is tested.
 *
 * Given is boundary calendar time.
 * When _rtc_maketime() function is called to convert the calendar time into timestamp.
 * Then if given calendar time is valid function returns true and conversion result, otherwise returns false.
 */
void test_mk_time_boundary()
{
    test_mk_time_struct *pTestCases;

    /* Array which contains data to test boundary values for the RTC devices which handles correctly leap years in
     * whole range (1970 - 2106).
     * Expected range: the 1st of January 1970 at 00:00:00 (seconds: 0) to the 7th of February 2106 at 06:28:15 (seconds: UINT_MAX).
     */
    test_mk_time_struct test_mk_time_arr_full[] = {
        {{ 0, 0, 0, 1, 0, 70, 0, 0, 0 }, (time_t) 0, true},               // valid lower bound - the 1st of January 1970 at 00:00:00
        {{ 59, 59, 23, 31, 11, 59, 0, 0, 0 }, (time_t) 0, false },        // invalid lower bound - the 31st of December 1969 at 23:59:59

        {{ 15, 28, 6, 7, 1, 206, 0, 0, 0 }, (time_t)(UINT_MAX), true },   // valid upper bound - the 7th of February 2106 at 06:28:15
        {{ 16, 28, 6, 7, 1, 206, 0, 0, 0 }, (time_t) 0, false },          // invalid upper bound - the 7th of February 2106 at 06:28:16
    };

    /* Array which contains data to test boundary values for the RTC devices which does not handle correctly leap years in
     * whole range (1970 - 2106). On this platforms we will be one day off after 28.02.2100 since 2100 year will be
     * incorrectly treated as a leap year.
     * Expected range: the 1st of January 1970 at 00:00:00 (seconds: 0) to the 6th of February 2106 at 06:28:15 (seconds: UINT_MAX).
     */
    test_mk_time_struct test_mk_time_arr_partial[] = {
        {{ 0, 0, 0, 1, 0, 70, 0, 0, 0 }, (time_t) 0, true},               // valid lower bound - the 1st of January 1970 at 00:00:00
        {{ 59, 59, 23, 31, 11, 59, 0, 0, 0 }, (time_t) 0, false },        // invalid lower bound - the 31st of December 1969 at 23:59:59

        {{ 15, 28, 6, 6, 1, 206, 0, 0, 0 }, (time_t)(UINT_MAX), true },   // valid upper bound - the 6th of February 2106 at 06:28:15
        {{ 16, 28, 6, 6, 1, 206, 0, 0, 0 }, (time_t) 0, false },          // invalid upper bound - the 6th of February 2106 at 06:28:16
    };

    /* Select array with test cases. */
    if (rtc_leap_year_support == RTC_FULL_LEAP_YEAR_SUPPORT) {
        pTestCases = test_mk_time_arr_full;
    } else {
        pTestCases = test_mk_time_arr_partial;
    }

    for (int i = 0; i < (sizeof(test_mk_time_arr_full) / (sizeof(test_mk_time_struct))); i++) {
        time_t seconds;
        bool result = _rtc_maketime(&pTestCases[i].timeinfo, &seconds, rtc_leap_year_support);

        TEST_ASSERT_EQUAL(pTestCases[i].result, result);

        /* If the result is false, then we have conversion error - skip checking seconds. */
        if (pTestCases[i].result) {
            TEST_ASSERT_EQUAL_UINT32(pTestCases[i].exp_seconds, seconds);
        }
    }
}

/* Test _rtc_maketime() function - call with invalid parameters.
 *
 * Given is _rtc_maketime() function.
 * When _rtc_maketime() function is called with invalid parameter.
 * Then _rtc_maketime() function returns false.
 */
void test_mk_time_invalid_param()
{
    time_t seconds;
    struct tm timeinfo;

    TEST_ASSERT_EQUAL(false, _rtc_maketime(NULL, &seconds, RTC_FULL_LEAP_YEAR_SUPPORT));
    TEST_ASSERT_EQUAL(false, _rtc_maketime(NULL, &seconds, RTC_4_YEAR_LEAP_YEAR_SUPPORT));
    TEST_ASSERT_EQUAL(false, _rtc_maketime(&timeinfo, NULL, RTC_FULL_LEAP_YEAR_SUPPORT));
    TEST_ASSERT_EQUAL(false, _rtc_maketime(&timeinfo, NULL, RTC_4_YEAR_LEAP_YEAR_SUPPORT));
}

/* Test _rtc_localtime() function - call with invalid parameters.
 *
 * Given is _rtc_localtime() function.
 * When _rtc_localtime() function is called with invalid parameter.
 * Then _rtc_localtime() function returns false.
 */
void test_local_time_invalid_param()
{
    TEST_ASSERT_EQUAL(false, _rtc_localtime(1, NULL, RTC_FULL_LEAP_YEAR_SUPPORT));
    TEST_ASSERT_EQUAL(false, _rtc_localtime(1, NULL, RTC_4_YEAR_LEAP_YEAR_SUPPORT));
}

utest::v1::status_t teardown_handler_t(const Case *const source, const size_t passed, const size_t failed,
                                       const failure_t reason)
{
    return greentea_case_teardown_handler(source, passed, failed, reason);
}

utest::v1::status_t full_leap_year_case_setup_handler_t(const Case *const source, const size_t index_of_case)
{
    rtc_leap_year_support = RTC_FULL_LEAP_YEAR_SUPPORT;

    return greentea_case_setup_handler(source, index_of_case);
}

utest::v1::status_t partial_leap_year_case_setup_handler_t(const Case *const source, const size_t index_of_case)
{
    rtc_leap_year_support = RTC_4_YEAR_LEAP_YEAR_SUPPORT;

    return greentea_case_setup_handler(source, index_of_case);
}

Case cases[] = {
    Case("test is leap year - RTC leap years full support", full_leap_year_case_setup_handler_t, test_is_leap_year, teardown_handler_t),
    Case("test is leap year - RTC leap years partial support", partial_leap_year_case_setup_handler_t, test_is_leap_year, teardown_handler_t),
    Case("test make time boundary values - RTC leap years full support", full_leap_year_case_setup_handler_t, test_mk_time_boundary, teardown_handler_t),
    Case("test make time boundary values - RTC leap years partial support", partial_leap_year_case_setup_handler_t, test_mk_time_boundary, teardown_handler_t),
    Case("test make time - invalid param", test_mk_time_invalid_param, teardown_handler_t),
    Case("test local time - invalid param", test_local_time_invalid_param, teardown_handler_t),
};

utest::v1::status_t greentea_test_setup(const size_t number_of_cases)
{
    GREENTEA_SETUP(20, "default_auto");
    return greentea_test_setup_handler(number_of_cases);
}

Specification specification(greentea_test_setup, cases, greentea_test_teardown_handler);

int main()
{
    return Harness::run(specification);
}
<commit_msg>tests-mbed_hal-rtc_time: use templates instead of global variable<commit_after>/* mbed Microcontroller Library
 * Copyright (c) 2017 ARM Limited
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "utest/utest.h"
#include "unity/unity.h"
#include "greentea-client/test_env.h"

#include "mbed.h"
#include "mbed_mktime.h"

#define LAST_VALID_YEAR 206

using namespace utest::v1;

/*  Regular is_leap_year, see platform/mbed_mktime.c for the optimised version. */
bool is_leap_year(int year)
{
    year = 1900 + year;
    if (year % 4) {
        return false;
    } else if (year % 100) {
        return true;
    } else if (year % 400) {
        return false;
    }
    return true;
}

/* Test the optimised version of _rtc_is_leap_year() against the generic version.
 *
 * Note: This test case is designed for both types of RTC devices:
 *       - RTC devices which handle correctly leap years in whole range (1970 - 2106).
 *       - RTC devices which does not handle correctly leap years in whole range (1970 - 2106).
 *         This RTC devices uses simpler leap year detection and incorrectly treat 2100 as a leap year.
 *       rtc_leap_year_support variable specifies which device is tested.
 *
 * Given is year in valid range.
 * When _rtc_is_leap_year() function is called.
 * Then _rtc_is_leap_year() returns true if given year is a leap year; false otherwise.
 */
template <rtc_leap_year_support_t rtc_leap_year_support>
void test_is_leap_year()
{
    for (int i = 70; i <= LAST_VALID_YEAR; ++i) {
        bool expected = is_leap_year(i);

        /* Add exception for year 2100. */
        if (rtc_leap_year_support == RTC_4_YEAR_LEAP_YEAR_SUPPORT && i == 200) {
            expected = true;
        }

        bool actual_value = _rtc_is_leap_year(i, rtc_leap_year_support);

        if (expected != actual_value) {
            printf("Leap year failed with i = %d\r\n", i);
        }
        TEST_ASSERT_EQUAL(expected, actual_value);
    }
}

/* Structure to test border values for _rtc_maketime(). */
typedef struct {
    struct tm timeinfo;
    time_t exp_seconds;  // if result is false then exp_seconds is irrelevant
    bool result;
} test_mk_time_struct;

/* Test boundary values for _rtc_maketime().
 *
 * Note: This test case is designed for both types of RTC devices:
 *       - RTC devices which handle correctly leap years in whole range (1970 - 2106).
 *       - RTC devices which does not handle correctly leap years in whole range (1970 - 2106).
 *         This RTC devices uses simpler leap year detection and incorrectly treat 2100 as a leap year.
 *       rtc_leap_year_support variable specifies which device is tested.
 *
 * Given is boundary calendar time.
 * When _rtc_maketime() function is called to convert the calendar time into timestamp.
 * Then if given calendar time is valid function returns true and conversion result, otherwise returns false.
 */
template <rtc_leap_year_support_t rtc_leap_year_support>
void test_mk_time_boundary()
{
    test_mk_time_struct *pTestCases;

    /* Array which contains data to test boundary values for the RTC devices which handles correctly leap years in
     * whole range (1970 - 2106).
     * Expected range: the 1st of January 1970 at 00:00:00 (seconds: 0) to the 7th of February 2106 at 06:28:15 (seconds: UINT_MAX).
     */
    test_mk_time_struct test_mk_time_arr_full[] = {
        {{ 0, 0, 0, 1, 0, 70, 0, 0, 0 }, (time_t) 0, true},               // valid lower bound - the 1st of January 1970 at 00:00:00
        {{ 59, 59, 23, 31, 11, 59, 0, 0, 0 }, (time_t) 0, false },        // invalid lower bound - the 31st of December 1969 at 23:59:59

        {{ 15, 28, 6, 7, 1, 206, 0, 0, 0 }, (time_t)(UINT_MAX), true },   // valid upper bound - the 7th of February 2106 at 06:28:15
        {{ 16, 28, 6, 7, 1, 206, 0, 0, 0 }, (time_t) 0, false },          // invalid upper bound - the 7th of February 2106 at 06:28:16
    };

    /* Array which contains data to test boundary values for the RTC devices which does not handle correctly leap years in
     * whole range (1970 - 2106). On this platforms we will be one day off after 28.02.2100 since 2100 year will be
     * incorrectly treated as a leap year.
     * Expected range: the 1st of January 1970 at 00:00:00 (seconds: 0) to the 6th of February 2106 at 06:28:15 (seconds: UINT_MAX).
     */
    test_mk_time_struct test_mk_time_arr_partial[] = {
        {{ 0, 0, 0, 1, 0, 70, 0, 0, 0 }, (time_t) 0, true},               // valid lower bound - the 1st of January 1970 at 00:00:00
        {{ 59, 59, 23, 31, 11, 59, 0, 0, 0 }, (time_t) 0, false },        // invalid lower bound - the 31st of December 1969 at 23:59:59

        {{ 15, 28, 6, 6, 1, 206, 0, 0, 0 }, (time_t)(UINT_MAX), true },   // valid upper bound - the 6th of February 2106 at 06:28:15
        {{ 16, 28, 6, 6, 1, 206, 0, 0, 0 }, (time_t) 0, false },          // invalid upper bound - the 6th of February 2106 at 06:28:16
    };

    /* Select array with test cases. */
    if (rtc_leap_year_support == RTC_FULL_LEAP_YEAR_SUPPORT) {
        pTestCases = test_mk_time_arr_full;
    } else {
        pTestCases = test_mk_time_arr_partial;
    }

    for (int i = 0; i < (sizeof(test_mk_time_arr_full) / (sizeof(test_mk_time_struct))); i++) {
        time_t seconds;
        bool result = _rtc_maketime(&pTestCases[i].timeinfo, &seconds, rtc_leap_year_support);

        TEST_ASSERT_EQUAL(pTestCases[i].result, result);

        /* If the result is false, then we have conversion error - skip checking seconds. */
        if (pTestCases[i].result) {
            TEST_ASSERT_EQUAL_UINT32(pTestCases[i].exp_seconds, seconds);
        }
    }
}

/* Test _rtc_maketime() function - call with invalid parameters.
 *
 * Given is _rtc_maketime() function.
 * When _rtc_maketime() function is called with invalid parameter.
 * Then _rtc_maketime() function returns false.
 */
void test_mk_time_invalid_param()
{
    time_t seconds;
    struct tm timeinfo;

    TEST_ASSERT_EQUAL(false, _rtc_maketime(NULL, &seconds, RTC_FULL_LEAP_YEAR_SUPPORT));
    TEST_ASSERT_EQUAL(false, _rtc_maketime(NULL, &seconds, RTC_4_YEAR_LEAP_YEAR_SUPPORT));
    TEST_ASSERT_EQUAL(false, _rtc_maketime(&timeinfo, NULL, RTC_FULL_LEAP_YEAR_SUPPORT));
    TEST_ASSERT_EQUAL(false, _rtc_maketime(&timeinfo, NULL, RTC_4_YEAR_LEAP_YEAR_SUPPORT));
}

/* Test _rtc_localtime() function - call with invalid parameters.
 *
 * Given is _rtc_localtime() function.
 * When _rtc_localtime() function is called with invalid parameter.
 * Then _rtc_localtime() function returns false.
 */
void test_local_time_invalid_param()
{
    TEST_ASSERT_EQUAL(false, _rtc_localtime(1, NULL, RTC_FULL_LEAP_YEAR_SUPPORT));
    TEST_ASSERT_EQUAL(false, _rtc_localtime(1, NULL, RTC_4_YEAR_LEAP_YEAR_SUPPORT));
}

Case cases[] = {
    Case("test is leap year - RTC leap years full support", test_is_leap_year<RTC_FULL_LEAP_YEAR_SUPPORT>),
    Case("test is leap year - RTC leap years partial support", test_is_leap_year<RTC_4_YEAR_LEAP_YEAR_SUPPORT>),
    Case("test make time boundary values - RTC leap years full support", test_mk_time_boundary<RTC_FULL_LEAP_YEAR_SUPPORT>),
    Case("test make time boundary values - RTC leap years partial support", test_mk_time_boundary<RTC_4_YEAR_LEAP_YEAR_SUPPORT>),
    Case("test make time - invalid param", test_mk_time_invalid_param),
    Case("test local time - invalid param", test_local_time_invalid_param),
};

utest::v1::status_t greentea_test_setup(const size_t number_of_cases)
{
    GREENTEA_SETUP(20, "default_auto");
    return greentea_test_setup_handler(number_of_cases);
}

Specification specification(greentea_test_setup, cases, greentea_test_teardown_handler);

int main()
{
    return Harness::run(specification);
}
<|endoftext|>