IAP GITLAB

CoordinateSystem.inl 4.64 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12
/*
 * (c) Copyright 2018 CORSIKA Project, corsika-project@lists.kit.edu
 *
 * See file AUTHORS for a list of contributors.
 *
 * 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
13
#include <corsika/framework/geometry/RootCoordinateSystem.hpp>
14 15 16
#include <corsika/framework/geometry/QuantityVector.hpp>
#include <corsika/framework/core/PhysicalUnits.hpp>

17
#include <Eigen/Dense>
18

19 20
#include <memory>
#include <stdexcept>
21

22
namespace corsika {
23

Ralf Ulrich's avatar
Ralf Ulrich committed
24
  CoordinateSystemPtr CoordinateSystem::getReferenceCS() const { return referenceCS_; }
25

26
  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) {
Ralf Ulrich's avatar
Ralf Ulrich committed
47
        a = a->getReferenceCS().get();
Ralf Ulrich's avatar
Ralf Ulrich committed
48
      }
49

50
      if (a == b) break;
51

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

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

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

Ralf Ulrich's avatar
Ralf Ulrich committed
67
    p = &pTo;
68

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

74 75
    return t;
  }
76

Ralf Ulrich's avatar
Ralf Ulrich committed
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 141 142
  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));
  }

143
} // namespace corsika