IAP GITLAB

CoordinateSystem.inl 4.83 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15
/*
 * (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

#include <corsika/framework/geometry/QuantityVector.hpp>
#include <corsika/framework/core/PhysicalUnits.hpp>

16
#include <Eigen/Dense>
17

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

21
namespace corsika {
22

23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56
  inline CoordinateSystemPtr CoordinateSystem::translate(
      QuantityVector<length_d> vector) const {
    EigenTransform const translation{EigenTranslation(vector.eigenVector_)};

    return std::make_shared<CoordinateSystem const>(*(new CoordinateSystem(
        std::make_shared<CoordinateSystem const>(*this), translation)));
  }

  template <typename TDim>
  CoordinateSystemPtr CoordinateSystem::rotateToZ(Vector<TDim> vVec) const {
    auto const a = vVec.normalized()
                       .getComponents(std::make_shared<CoordinateSystem const>(*this))
                       .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;  // .
57 58
    }

59 60 61
    return std::make_shared<CoordinateSystem const>(*(new CoordinateSystem(
        std::make_shared<CoordinateSystem const>(*this), EigenTransform(A + B))));
  }
62

63 64 65 66 67
  template <typename TDim>
  CoordinateSystemPtr CoordinateSystem::rotate(QuantityVector<TDim> axis,
                                               double angle) const {
    if (axis.eigenVector_.isZero()) {
      throw std::runtime_error("null-vector given as axis parameter");
68 69
    }

70 71
    EigenTransform const rotation{
        Eigen::AngleAxisd(angle, axis.eigenVector_.normalized())};
72

73 74 75
    return std::make_shared<CoordinateSystem const>(
        CoordinateSystem(std::make_shared<CoordinateSystem const>(*this), rotation));
  }
76

77 78 79 80 81
  template <typename TDim>
  CoordinateSystemPtr CoordinateSystem::translateAndRotate(
      QuantityVector<length_d> translation, QuantityVector<TDim> axis, double angle) {
    if (axis.eigenVector_.isZero()) {
      throw std::runtime_error("null-vector given as axis parameter");
82 83
    }

84 85
    EigenTransform const transf{Eigen::AngleAxisd(angle, axis.eigenVector_.normalized()) *
                                EigenTranslation(translation.eigenVector_)};
86

87 88
    return std::make_shared<CoordinateSystem const>(CoordinateSystem(*this, transf));
  }
89

90 91 92
  CoordinateSystemPtr CoordinateSystem::getReferenceCS() const {
    return referenceCS_; //*(referenceCS_.get());
  }
93

94
  EigenTransform const& CoordinateSystem::getTransform() const { return transf_; }
95

96 97 98
  inline bool CoordinateSystem::operator==(CoordinateSystem const& cs) const {
    return referenceCS_ == cs.referenceCS_ && transf_.matrix() == cs.transf_.matrix();
  }
99

100 101 102
  inline bool CoordinateSystem::operator!=(CoordinateSystem const& cs) const {
    return !(cs == *this);
  }
103

104 105 106 107 108 109 110 111 112 113 114 115
  /**
   * returns the transformation matrix necessary to transform primitives with coordinates
   * in \a pFrom to \a pTo, e.g.
   * \f$ \vec{v}^{\text{(to)}} = \mathcal{M} \vec{v}^{\text{(from)}} \f$
   * (\f$ \vec{v}^{(.)} \f$ denotes the coordinates/components of the component in
   * the indicated CoordinateSystem).
   */
  inline EigenTransform getTransformation(CoordinateSystemPtr pFrom,
                                          CoordinateSystemPtr pTo) {
    CoordinateSystemPtr a{pFrom};
    CoordinateSystemPtr b{pTo};
    CoordinateSystemPtr commonBase{nullptr};
116

117 118
    while (a != b && b != nullptr) {
      a = pFrom;
119

120
      while (a != b && a != nullptr) { a = a->getReferenceCS(); }
121

122
      if (a == b) break;
123

124 125
      b = b->getReferenceCS();
    }
126

127 128
    if (a == b && a != nullptr) {
      commonBase = a;
129

130 131 132
    } else {
      throw std::runtime_error("no connection between coordinate systems found!");
    }
133

134 135
    EigenTransform t = EigenTransform::Identity();
    CoordinateSystemPtr p = pFrom;
136

137 138 139 140
    while ((*p) != (*commonBase)) {
      t = p->getTransform() * t;
      p = p->getReferenceCS();
    }
141

142
    p = pTo;
143

144 145 146
    while (*p != *commonBase) {
      t = t * p->getTransform().inverse(Eigen::TransformTraits::Isometry);
      p = p->getReferenceCS();
147 148
    }

149 150
    return t;
  }
151

152
} // namespace corsika