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 37 38
  /// find transformation between two CS, using most optimal common base
  inline EigenTransform get_transformation(CoordinateSystemPtr const& pFrom,
                                           CoordinateSystemPtr const& pTo) {
39 40 41
    CoordinateSystemPtr a{pFrom};
    CoordinateSystemPtr b{pTo};
    CoordinateSystemPtr commonBase{nullptr};
42

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

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

51
      if (a == b) break;
52

53 54
      b = b->getReferenceCS();
    }
55

Ralf Ulrich's avatar
Ralf Ulrich committed
56
    if (a == b && a) {
57
      commonBase = a;
58

59 60 61
    } else {
      throw std::runtime_error("no connection between coordinate systems found!");
    }
62

63 64
    EigenTransform t = EigenTransform::Identity();
    CoordinateSystemPtr p = pFrom;
65

66 67 68 69
    while ((*p) != (*commonBase)) {
      t = p->getTransform() * t;
      p = p->getReferenceCS();
    }
70

71
    p = pTo;
72

73 74 75
    while (*p != *commonBase) {
      t = t * p->getTransform().inverse(Eigen::TransformTraits::Isometry);
      p = p->getReferenceCS();
76 77
    }

78 79
    return t;
  }
80

Ralf Ulrich's avatar
Ralf Ulrich committed
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 143 144 145 146
  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));
  }

147
} // namespace corsika