testGeometry.cpp

/*
 * (c) Copyright 2020 CORSIKA Project, corsika-project@lists.kit.edu
 *
 * This software is distributed under the terms of the GNU General Public
 * Licence version 3 (GPL Version 3). See file LICENSE for a full version of
 * the license.
 */

#include <catch2/catch.hpp>

#include <cmath>
#include <corsika/framework/core/PhysicalUnits.hpp>
#include <corsika/framework/geometry/CoordinateSystem.hpp>
#include <corsika/framework/geometry/Line.hpp>
#include <corsika/framework/geometry/Point.hpp>
#include <corsika/framework/geometry/RootCoordinateSystem.hpp>
#include <corsika/framework/geometry/Sphere.hpp>
#include <corsika/framework/geometry/Trajectory.hpp>

using namespace corsika;

double constexpr absMargin = 1.0e-8;

TEST_CASE("transformations between CoordinateSystems") {
  CoordinateSystemPtr rootCS = get_root_CoordinateSystem();

  QuantityVector<length_d> const coordinates{0_m, 0_m, 0_m};
  Point p1(rootCS, coordinates);

  QuantityVector<magnetic_flux_density_d> components{1. * tesla, 0. * tesla, 0. * tesla};
  Vector<magnetic_flux_density_d> v1(rootCS, components);

  CHECK((p1.getCoordinates() - coordinates).getNorm().magnitude() ==
        Approx(0).margin(absMargin));
  CHECK((p1.getCoordinates(rootCS) - coordinates).getNorm().magnitude() ==
        Approx(0).margin(absMargin));

  SECTION("translations") {
    QuantityVector<length_d> const translationVector{0_m, 4_m, 0_m};

    CoordinateSystemPtr translatedCS = make_translation(rootCS, translationVector);

    CHECK(*translatedCS->getReferenceCS() == *rootCS);

    CHECK((p1.getCoordinates(translatedCS) + translationVector).getNorm().magnitude() ==
          Approx(0).margin(absMargin));

    // Vectors are not subject to translations
    CHECK((v1.getComponents(rootCS) - v1.getComponents(translatedCS))
              .getNorm()
              .magnitude() == Approx(0).margin(absMargin));

    Point p2(translatedCS, {0_m, 0_m, 0_m});
    CHECK(((p2 - p1).getComponents() - translationVector).getNorm().magnitude() ==
          Approx(0).margin(absMargin));
  }

  SECTION("multiple translations") {
    QuantityVector<length_d> const tv1{0_m, 5_m, 0_m};
    CoordinateSystemPtr cs2 = make_translation(rootCS, tv1);

    QuantityVector<length_d> const tv2{3_m, 0_m, 0_m};
    CoordinateSystemPtr cs3 = make_translation(rootCS, tv2);

    QuantityVector<length_d> const tv3{0_m, 0_m, 2_m};
    CoordinateSystemPtr cs4 = make_translation(cs3, tv3);

    CHECK(*cs4->getReferenceCS()->getReferenceCS() == *rootCS);

    CHECK(get_transformation(cs3, cs2).isApprox(
        make_translation(rootCS, {3_m, -5_m, 0_m})->getTransform()));
    CHECK(get_transformation(cs2, cs3).isApprox(
        make_translation(rootCS, {-3_m, +5_m, 0_m})->getTransform()));
  }

  SECTION("rotations") {
    QuantityVector<length_d> const axis{0_m, 0_m, 1_km};
    double const angle = 90. / 180. * M_PI;

    CoordinateSystemPtr rotatedCS = make_rotation(rootCS, axis, angle);
    CHECK(*rotatedCS->getReferenceCS() == *rootCS);

    CHECK(v1.getComponents(rotatedCS)[1].magnitude() ==
          Approx((-1. * tesla).magnitude()));

    // vector norm invariant under rotation
    CHECK(v1.getComponents(rotatedCS).getNorm().magnitude() ==
          Approx(v1.getComponents(rootCS).getNorm().magnitude()));
  }

  SECTION("multiple rotations") {
    QuantityVector<length_d> const zAxis{0_m, 0_m, 1_km};
    QuantityVector<length_d> const yAxis{0_m, 7_nm, 0_m};
    QuantityVector<length_d> const xAxis{2_m, 0_nm, 0_m};

    QuantityVector<magnetic_flux_density_d> components{1. * tesla, 2. * tesla,
                                                       3. * tesla};
    Vector<magnetic_flux_density_d> v1(rootCS, components);

    double const angle = 90. / 180. * M_PI;

    CoordinateSystemPtr rotated1 = make_rotation(rootCS, zAxis, angle);
    CoordinateSystemPtr rotated2 = make_rotation(rotated1, yAxis, angle);
    CoordinateSystemPtr rotated3 = make_rotation(rotated2, zAxis, -angle);

    CoordinateSystemPtr combined = make_rotation(rootCS, xAxis, -angle);

    auto comp1 = v1.getComponents(rotated3);
    auto comp3 = v1.getComponents(combined);
    CHECK((comp1 - comp3).getNorm().magnitude() == Approx(0).margin(absMargin));
  }

  SECTION("RotateToZ positive") {
    Vector const v{rootCS, 0_m, 1_m, 1_m};
    auto const csPrime = make_rotationToZ(rootCS, v);
    Vector const zPrime{csPrime, 0_m, 0_m, 5_m};
    Vector const xPrime{csPrime, 5_m, 0_m, 0_m};
    Vector const yPrime{csPrime, 0_m, 5_m, 0_m};

    CHECK(xPrime.dot(v).magnitude() == Approx(0).margin(absMargin));
    CHECK(yPrime.dot(v).magnitude() == Approx(0).margin(absMargin));
    CHECK((zPrime.dot(v) / 1_m).magnitude() == Approx(5 * sqrt(2)));

    CHECK(zPrime.getComponents(rootCS)[1].magnitude() ==
          Approx(zPrime.getComponents(rootCS)[2].magnitude()));
    CHECK(zPrime.getComponents(rootCS)[0].magnitude() == Approx(0));

    CHECK(xPrime.getComponents(rootCS).getEigenVector().dot(
              yPrime.getComponents(rootCS).getEigenVector()) == Approx(0));
    CHECK(zPrime.getComponents(rootCS).getEigenVector().dot(
              xPrime.getComponents(rootCS).getEigenVector()) == Approx(0));
    CHECK(yPrime.getComponents(rootCS).getEigenVector().dot(
              zPrime.getComponents(rootCS).getEigenVector()) == Approx(0));

    CHECK(yPrime.getComponents(rootCS).getEigenVector().dot(
              yPrime.getComponents(rootCS).getEigenVector()) ==
          Approx((5_m * 5_m).magnitude()));
    CHECK(xPrime.getComponents(rootCS).getEigenVector().dot(
              xPrime.getComponents(rootCS).getEigenVector()) ==
          Approx((5_m * 5_m).magnitude()));
    CHECK(zPrime.getComponents(rootCS).getEigenVector().dot(
              zPrime.getComponents(rootCS).getEigenVector()) ==
          Approx((5_m * 5_m).magnitude()));
  }

  SECTION("RotateToZ negative") {
    Vector const v{rootCS, 0_m, 0_m, -1_m};
    auto const csPrime = make_rotationToZ(rootCS, v);
    Vector const zPrime{csPrime, 0_m, 0_m, 5_m};
    Vector const xPrime{csPrime, 5_m, 0_m, 0_m};
    Vector const yPrime{csPrime, 0_m, 5_m, 0_m};

    CHECK(zPrime.dot(v).magnitude() > 0);
    CHECK(xPrime.getComponents(rootCS).getEigenVector().dot(
              v.getComponents().getEigenVector()) == Approx(0));
    CHECK(yPrime.getComponents(rootCS).getEigenVector().dot(
              v.getComponents().getEigenVector()) == Approx(0));

    CHECK(xPrime.getComponents(rootCS).getEigenVector().dot(
              yPrime.getComponents(rootCS).getEigenVector()) == Approx(0));
    CHECK(zPrime.getComponents(rootCS).getEigenVector().dot(
              xPrime.getComponents(rootCS).getEigenVector()) == Approx(0));
    CHECK(yPrime.getComponents(rootCS).getEigenVector().dot(
              zPrime.getComponents(rootCS).getEigenVector()) == Approx(0));

    CHECK(yPrime.getComponents(rootCS).getEigenVector().dot(
              yPrime.getComponents(rootCS).getEigenVector()) ==
          Approx((5_m * 5_m).magnitude()));
    CHECK(xPrime.getComponents(rootCS).getEigenVector().dot(
              xPrime.getComponents(rootCS).getEigenVector()) ==
          Approx((5_m * 5_m).magnitude()));
    CHECK(zPrime.getComponents(rootCS).getEigenVector().dot(
              zPrime.getComponents(rootCS).getEigenVector()) ==
          Approx((5_m * 5_m).magnitude()));
  }

  SECTION("RotateToZ positive") {
    Vector const v{rootCS, 0_m, 1_m, 1_m};
    auto const csPrime = rootCS.RotateToZ(v);
    Vector const zPrime{csPrime, 0_m, 0_m, 5_m};
    Vector const xPrime{csPrime, 5_m, 0_m, 0_m};
    Vector const yPrime{csPrime, 0_m, 5_m, 0_m};

    CHECK(xPrime.dot(v).magnitude() == Approx(0).margin(absMargin));
    CHECK(yPrime.dot(v).magnitude() == Approx(0).margin(absMargin));
    CHECK((zPrime.dot(v) / 1_m).magnitude() == Approx(5 * sqrt(2)));

    CHECK(zPrime.GetComponents(rootCS)[1].magnitude() ==
          Approx(zPrime.GetComponents(rootCS)[2].magnitude()));
    CHECK(zPrime.GetComponents(rootCS)[0].magnitude() == Approx(0));

    CHECK(xPrime.GetComponents(rootCS).eVector.dot(
              yPrime.GetComponents(rootCS).eVector) == Approx(0));
    CHECK(zPrime.GetComponents(rootCS).eVector.dot(
              xPrime.GetComponents(rootCS).eVector) == Approx(0));
    CHECK(yPrime.GetComponents(rootCS).eVector.dot(
              zPrime.GetComponents(rootCS).eVector) == Approx(0));

    CHECK(yPrime.GetComponents(rootCS).eVector.dot(
              yPrime.GetComponents(rootCS).eVector) == Approx((5_m * 5_m).magnitude()));
    CHECK(xPrime.GetComponents(rootCS).eVector.dot(
              xPrime.GetComponents(rootCS).eVector) == Approx((5_m * 5_m).magnitude()));
    CHECK(zPrime.GetComponents(rootCS).eVector.dot(
              zPrime.GetComponents(rootCS).eVector) == Approx((5_m * 5_m).magnitude()));
  }

  SECTION("RotateToZ negative") {
    Vector const v{rootCS, 0_m, 0_m, -1_m};
    auto const csPrime = rootCS.RotateToZ(v);
    Vector const zPrime{csPrime, 0_m, 0_m, 5_m};
    Vector const xPrime{csPrime, 5_m, 0_m, 0_m};
    Vector const yPrime{csPrime, 0_m, 5_m, 0_m};

    CHECK(zPrime.dot(v).magnitude() > 0);
    CHECK(xPrime.GetComponents(rootCS).eVector.dot(v.GetComponents().eVector) ==
          Approx(0));
    CHECK(yPrime.GetComponents(rootCS).eVector.dot(v.GetComponents().eVector) ==
          Approx(0));

    CHECK(xPrime.GetComponents(rootCS).eVector.dot(
              yPrime.GetComponents(rootCS).eVector) == Approx(0));
    CHECK(zPrime.GetComponents(rootCS).eVector.dot(
              xPrime.GetComponents(rootCS).eVector) == Approx(0));
    CHECK(yPrime.GetComponents(rootCS).eVector.dot(
              zPrime.GetComponents(rootCS).eVector) == Approx(0));

    CHECK(yPrime.GetComponents(rootCS).eVector.dot(
              yPrime.GetComponents(rootCS).eVector) == Approx((5_m * 5_m).magnitude()));
    CHECK(xPrime.GetComponents(rootCS).eVector.dot(
              xPrime.GetComponents(rootCS).eVector) == Approx((5_m * 5_m).magnitude()));
    CHECK(zPrime.GetComponents(rootCS).eVector.dot(
              zPrime.GetComponents(rootCS).eVector) == Approx((5_m * 5_m).magnitude()));
  }
}

TEST_CASE("CoordinateSystem hirarchy") {

  CoordinateSystemPtr rootCS = get_root_CoordinateSystem();

  CHECK(get_transformation(rootCS, rootCS).isApprox(EigenTransform::Identity()));

  // define the root coordinate system
  CoordinateSystemPtr root = get_root_CoordinateSystem();
  Point const p1(root, {0_m, 0_m, 0_m}); // the origin of the root CS

  // root -> cs2
  CoordinateSystemPtr cs2 = make_translation(root, {0_m, 0_m, 1_m});
  Point const p2(cs2, {0_m, 0_m, -1_m});

  // root -> cs2 -> cs3
  CoordinateSystemPtr cs3 = make_translation(cs2, {0_m, 0_m, -1_m});
  Point const p3(cs3, {0_m, 0_m, 0_m});

  // root -> cs2 -> cs4
  CoordinateSystemPtr cs4 = make_translation(cs2, {0_m, 0_m, -1_m});
  Point const p4(cs4, {0_m, 0_m, 0_m});

  // root -> cs2 -> cs4 -> cs5
  CoordinateSystemPtr cs5 =
      make_rotation(cs4, QuantityVector<length_d>{1_m, 0_m, 0_m}, 90 * degree_angle);
  Point const p5(cs5, {0_m, 0_m, 0_m});

  // root -> cs6
  CoordinateSystemPtr cs6 =
      make_rotation(root, QuantityVector<length_d>{1_m, 0_m, 0_m}, 90 * degree_angle);
  Point const p6(cs6, {0_m, 0_m, 0_m}); // the origin of the root CS

  // all points should be on top of each other

  CHECK_FALSE(get_transformation(root, cs2).isApprox(EigenTransform::Identity()));
  CHECK(get_transformation(root, cs3).isApprox(EigenTransform::Identity()));
  CHECK(get_transformation(root, cs4).isApprox(EigenTransform::Identity()));
  CHECK(get_transformation(cs5, cs6).isApprox(EigenTransform::Identity()));

  CHECK((p1 - p2).getNorm().magnitude() == Approx(0).margin(absMargin));
  CHECK((p1 - p3).getNorm().magnitude() == Approx(0).margin(absMargin));
  CHECK((p1 - p4).getNorm().magnitude() == Approx(0).margin(absMargin));
  CHECK((p1 - p5).getNorm().magnitude() == Approx(0).margin(absMargin));
  CHECK((p1 - p6).getNorm().magnitude() == Approx(0).margin(absMargin));
}

TEST_CASE("Sphere") {
  CoordinateSystemPtr rootCS = get_root_CoordinateSystem();
  Point center(rootCS, {0_m, 3_m, 4_m});
  Sphere sphere(center, 5_m);

  SECTION("getCenter") {
    CHECK((sphere.getCenter().getCoordinates(rootCS) -
           QuantityVector<length_d>(0_m, 3_m, 4_m))
              .getNorm()
              .magnitude() == Approx(0).margin(absMargin));
    CHECK(sphere.getRadius() / 5_m == Approx(1));
  }

  SECTION("isInside") {
    CHECK_FALSE(sphere.isInside(Point(rootCS, {100_m, 0_m, 0_m})));
    CHECK(sphere.isInside(Point(rootCS, {2_m, 3_m, 4_m})));
  }
}

TEST_CASE("Trajectories") {
  CoordinateSystemPtr rootCS = get_root_CoordinateSystem();
  Point r0(rootCS, {0_m, 0_m, 0_m});

  SECTION("Line") {
    Vector<SpeedType::dimension_type> v0(rootCS,
                                         {3_m / second, 0_m / second, 0_m / second});

    Line const line(r0, v0);
    CHECK(
        (line.getPosition(2_s).getCoordinates() - QuantityVector<length_d>(6_m, 0_m, 0_m))
            .getNorm()
            .magnitude() == Approx(0).margin(absMargin));

    CHECK((line.getPositionFromArclength(4_m).getCoordinates() -
           QuantityVector<length_d>(4_m, 0_m, 0_m))
              .getNorm()
              .magnitude() == Approx(0).margin(absMargin));

    CHECK((line.getPosition(7_s) -
           line.getPositionFromArclength(line.getArcLength(0_s, 7_s)))
              .getNorm()
              .magnitude() == Approx(0).margin(absMargin));

    auto const t = 1_s;
    Trajectory<Line> base(line, t);
    CHECK(line.getPosition(t).getCoordinates() == base.getPosition(1.).getCoordinates());

    CHECK(base.getArcLength(1_s, 2_s) / 1_m == Approx(3));

    CHECK((base.getNormalizedDirection().getComponents(rootCS) -
           QuantityVector<dimensionless_d>{1, 0, 0})
              .getNorm() == Approx(0).margin(absMargin));
  }
}