IAP GITLAB

CoordinateSystem.inl 4.36 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39
/*
 * (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 <Eigen/Dense>
#include <stdexcept>
#include <corsika/framework/core/PhysicalUnits.hpp>


namespace corsika {


    CoordinateSystem& CoordinateSystem::operator=(const corsika::CoordinateSystem& pCS)
    {
      reference = pCS.reference;
      transf = pCS.transf;
      return *this;
    }

    inline CoordinateSystem CoordinateSystem::translate(QuantityVector<length_d> vector) const
    {
      EigenTransform const translation{EigenTranslation(vector.eVector)};

      return CoordinateSystem(*this, translation);
    }

    template <typename TDim>
    auto CoordinateSystem::RotateToZ(Vector<TDim> vVec) const
    {
      auto const a = vVec.normalized().GetComponents(*this).eVector;
40
      auto const a1 = a(0), a2 = a(1), a3 = a(2);
41 42 43

      Eigen::Matrix3d A, B;

44 45
      if (a3 > 0) {
        auto const c = 1 / (1 + a3);
46 47 48 49 50 51 52 53
        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 {
54
        auto const c = 1 / (1 - a3);
55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78
        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 CoordinateSystem(*this, EigenTransform(A + B));
    }

    template <typename TDim>
    auto CoordinateSystem::rotate(QuantityVector<TDim> axis, double angle) const
    {
      if (axis.eVector.isZero()) {
        throw std::runtime_error("null-vector given as axis parameter");
      }

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

      return CoordinateSystem(*this, rotation);
    }

    template <typename TDim>
Ralf Ulrich's avatar
Ralf Ulrich committed
79
    auto CoordinateSystem::translateAndRotate(QuantityVector<length_d> translation, QuantityVector<TDim> axis, double angle)
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 143 144 145 146 147 148 149 150
    {
      if (axis.eVector.isZero()) {
        throw std::runtime_error("null-vector given as axis parameter");
      }

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

      return CoordinateSystem(*this, transf);
    }

    CoordinateSystem const* CoordinateSystem::GetReference() const
    {
    	return reference;
    }

    const EigenTransform& CoordinateSystem::GetTransform() const
    {
    	return transf;
    }

    /**
     * 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(CoordinateSystem const& pFrom,
                                                                                          CoordinateSystem const& pTo) {
      CoordinateSystem const* a{&pFrom};
      CoordinateSystem const* b{&pTo};
      CoordinateSystem const* commonBase{nullptr};

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

        while (a != b && a != nullptr) { a = a->GetReference(); }

        if (a == b) break;

        b = b->GetReference();
      }

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

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

      EigenTransform t = EigenTransform::Identity();
      auto* p = &pFrom;

      while (p != commonBase) {
        t = p->GetTransform() * t;
        p = p->GetReference();
      }

      p = &pTo;

      while (p != commonBase) {
        t = t * p->GetTransform().inverse(Eigen::TransformTraits::Isometry);
        p = p->GetReference();
      }

      return t;
    }

} // namespace corsika