IAP GITLAB

CoordinateSystem.inl 4.6 KB
Newer Older
1
/*
Ralf Ulrich's avatar
Ralf Ulrich committed
2
 * (c) Copyright 2020 CORSIKA Project, corsika-project@lists.kit.edu
3 4 5 6 7 8 9 10
 *
 * 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.
 */

#pragma once

Ralf Ulrich's avatar
Ralf Ulrich committed
11
#include <corsika/framework/geometry/RootCoordinateSystem.hpp>
12 13 14
#include <corsika/framework/geometry/QuantityVector.hpp>
#include <corsika/framework/core/PhysicalUnits.hpp>

15
#include <Eigen/Dense>
16

17 18
#include <memory>
#include <stdexcept>
19

20
namespace corsika {
21

22 23 24
  inline CoordinateSystemPtr CoordinateSystem::getReferenceCS() const {
    return referenceCS_;
  }
25

26
  inline EigenTransform const& CoordinateSystem::getTransform() const { return transf_; }
27

28 29 30
  inline bool CoordinateSystem::operator==(CoordinateSystem const& cs) const {
    return referenceCS_ == cs.referenceCS_ && transf_.matrix() == cs.transf_.matrix();
  }
31

32 33 34
  inline bool CoordinateSystem::operator!=(CoordinateSystem const& cs) const {
    return !(cs == *this);
  }
35

Ralf Ulrich's avatar
Ralf Ulrich committed
36
  /// find transformation between two CS, using most optimal common base
Ralf Ulrich's avatar
Ralf Ulrich committed
37 38 39 40
  inline EigenTransform get_transformation(CoordinateSystem const& pFrom,
                                           CoordinateSystem const& pTo) {
    CoordinateSystem const* a{&pFrom};
    CoordinateSystem const* b{&pTo};
41

Ralf Ulrich's avatar
Ralf Ulrich committed
42
    while (a != b && b) {
43

Ralf Ulrich's avatar
Ralf Ulrich committed
44
      // traverse pFrom
Ralf Ulrich's avatar
Ralf Ulrich committed
45
      a = &pFrom;
Ralf Ulrich's avatar
Ralf Ulrich committed
46
      while (a != b && a) { a = a->getReferenceCS().get(); }
47

48
      if (a == b) break;
49

Ralf Ulrich's avatar
Ralf Ulrich committed
50
      b = b->getReferenceCS().get();
51
    }
52

Ralf Ulrich's avatar
Ralf Ulrich committed
53
    if (a != b || a == nullptr) {
54 55
      throw std::runtime_error("no connection between coordinate systems found!");
    }
56

Ralf Ulrich's avatar
Ralf Ulrich committed
57 58
    CoordinateSystem const* commonBase = a;
    CoordinateSystem const* p = &pFrom;
59 60 61
    EigenTransform t = EigenTransform::Identity();
    while ((*p) != (*commonBase)) {
      t = p->getTransform() * t;
Ralf Ulrich's avatar
Ralf Ulrich committed
62
      p = p->getReferenceCS().get();
63
    }
64

Ralf Ulrich's avatar
Ralf Ulrich committed
65
    p = &pTo;
66

67 68
    while (*p != *commonBase) {
      t = t * p->getTransform().inverse(Eigen::TransformTraits::Isometry);
Ralf Ulrich's avatar
Ralf Ulrich committed
69
      p = p->getReferenceCS().get();
70 71
    }

72 73
    return t;
  }
74

Ralf Ulrich's avatar
Ralf Ulrich committed
75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140
  inline CoordinateSystemPtr make_translation(CoordinateSystemPtr const& cs,
                                              QuantityVector<length_d> const& vector) {
    EigenTransform const translation{EigenTranslation(vector.getEigenVector())};
    return std::make_shared<CoordinateSystem const>(CoordinateSystem(cs, translation));
  }

  template <typename TDim>
  inline CoordinateSystemPtr make_rotationToZ(CoordinateSystemPtr const& cs,
                                              Vector<TDim> const& vVec) {
    auto const a = vVec.normalized().getComponents(cs).getEigenVector();
    auto const a1 = a(0), a2 = a(1), a3 = a(2);

    Eigen::Matrix3d A, B;

    if (a3 > 0) {
      auto const c = 1 / (1 + a3);
      A << 1, 0, a1,                      // comment to prevent clang-format
          0, 1, a2,                       // .
          -a1, -a2, 1;                    // .
      B << -a1 * a1 * c, -a1 * a2 * c, 0, // .
          -a1 * a2 * c, -a2 * a2 * c, 0,  // .
          0, 0, -(a1 * a1 + a2 * a2) * c; // .

    } else {
      auto const c = 1 / (1 - a3);
      A << 1, 0, a1,                      // .
          0, -1, a2,                      // .
          a1, -a2, -1;                    // .
      B << -a1 * a1 * c, +a1 * a2 * c, 0, // .
          -a1 * a2 * c, +a2 * a2 * c, 0,  // .
          0, 0, (a1 * a1 + a2 * a2) * c;  // .
    }

    return std::make_shared<CoordinateSystem const>(
        CoordinateSystem(cs, EigenTransform(A + B)));
  }

  template <typename TDim>
  inline CoordinateSystemPtr make_rotation(CoordinateSystemPtr const& cs,
                                           QuantityVector<TDim> const& axis,
                                           double const angle) {
    if (axis.getEigenVector().isZero()) {
      throw std::runtime_error("null-vector given as axis parameter");
    }

    EigenTransform const rotation{
        Eigen::AngleAxisd(angle, axis.getEigenVector().normalized())};

    return std::make_shared<CoordinateSystem const>(CoordinateSystem(cs, rotation));
  }

  template <typename TDim>
  inline CoordinateSystemPtr make_translationAndRotation(
      CoordinateSystemPtr const& cs, QuantityVector<length_d> const& translation,
      QuantityVector<TDim> const& axis, double const angle) {
    if (axis.getEigenVector().isZero()) {
      throw std::runtime_error("null-vector given as axis parameter");
    }

    EigenTransform const transf{
        Eigen::AngleAxisd(angle, axis.getEigenVector().normalized()) *
        EigenTranslation(translation.getEigenVector())};

    return std::make_shared<CoordinateSystem const>(CoordinateSystem(cs, transf));
  }

141
} // namespace corsika