JacobianMatrixUtil.h

/**************************************************************************
 * basf2 (Belle II Analysis Software Framework)                           *
 * Author: The Belle II Collaboration                                     *
 *                                                                        *
 * See git log for contributors and copyright holders.                    *
 * This file is licensed under LGPL-3.0, see LICENSE.md.                  *
 **************************************************************************/
#pragma once
 
#include <tracking/trackingUtilities/numerics/JacobianMatrix.h>
#include <tracking/trackingUtilities/numerics/ParameterVector.h>
 
#include <tracking/trackingUtilities/geometry/VectorUtil.h>
 
#include <tracking/trackingUtilities/numerics/EigenView.h>
 
#include <Math/Vector2D.h>
 
#include <Eigen/Core>
 
namespace Belle2 {
  namespace TrackingUtilities {

    struct JacobianMatrixUtil {

      template <unsigned int M, unsigned int N = M>
      static JacobianMatrix<M, N> zero()
      {
        return JacobianMatrix<M, N>::Zero();
      }

      template <int M, int N = M>
      static JacobianMatrix<M, N> identity()
      {
        return JacobianMatrix<M, N>::Identity();
      }

      template <int N>
      static JacobianMatrix<N, N> passiveRotation(int i, int j, double phi)
      {
        return passiveRotation<N>(i, j, VectorUtil::Phi(phi));
      }

      template <int N>
      static JacobianMatrix<N, N> passiveRotation(int i, int j, const ROOT::Math::XYVector& phiVec)
      {
        assert(i < N);
        assert(j < N);
        JacobianMatrix<N, N> result = identity<N>();
        result(i, i) = phiVec.x();
        result(i, j) = phiVec.y();
        result(j, i) = -phiVec.y();
        result(j, j) = phiVec.x();
        return result;
      }

      template <int N>
      static JacobianMatrix<N> scale(const ParameterVector<N>& scales)
      {
        JacobianMatrix<N> result = zero<N>();
        mapToEigen(result).diagonal() = mapToEigen(scales);
        return result;
      }

      template <int M1, int N1, int M2, int N2>
      static JacobianMatrix < M1 + M2, N1 + N2 > stackBlocks(const JacobianMatrix<M1, N1>& block1,
                                                             const JacobianMatrix<M2, N2>& block2)
      {
        JacobianMatrix < M1 + M2, N1 + N2 > result;
        mapToEigen(result) << mapToEigen(block1), Eigen::Matrix<double, M1, N2>::Zero(),
                   Eigen::Matrix<double, M2, N1>::Zero(), mapToEigen(block2);
        return result;
      }
    };
 
  }
}