Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/math/matrix.hpp	Lines:	14	14	100.0 %
Date:	2023-08-09 08:43:58	Branches:	3	6	50.0 %


//
// Copyright (c) 2016-2020 CNRS INRIA
//

#ifndef __pinocchio_math_matrix_hpp__
#define __pinocchio_math_matrix_hpp__

#include "pinocchio/macros.hpp"
#include "pinocchio/math/fwd.hpp"

#include <Eigen/Core>
#include <boost/type_traits.hpp>

namespace pinocchio
{

  template<typename Derived>
  inline bool hasNaN(const Eigen::DenseBase<Derived> & m)
  {
    return !((m.derived().array()==m.derived().array()).all());
  }

  namespace internal
  {
    template<typename MatrixLike, bool value = is_floating_point<typename MatrixLike::Scalar>::value>
    struct isZeroAlgo
    {
      typedef typename MatrixLike::Scalar Scalar;
      typedef typename MatrixLike::RealScalar RealScalar;

      static bool run(const Eigen::MatrixBase<MatrixLike> & mat,
                      const RealScalar & prec =
                      Eigen::NumTraits< Scalar >::dummy_precision())
      {
        return mat.isZero(prec);
      }
    };

    template<typename MatrixLike>
    struct isZeroAlgo<MatrixLike,false>
    {
      typedef typename MatrixLike::Scalar Scalar;
      typedef typename MatrixLike::RealScalar RealScalar;

      static bool run(const Eigen::MatrixBase<MatrixLike> & /*vec*/,
                      const RealScalar & prec =
                      Eigen::NumTraits< Scalar >::dummy_precision())
      {
        PINOCCHIO_UNUSED_VARIABLE(prec);
        return true;
      }
    };
  }

  template<typename MatrixLike>
  inline bool isZero(const Eigen::MatrixBase<MatrixLike> & m,
                     const typename MatrixLike::RealScalar & prec =
                     Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
  {
    return internal::isZeroAlgo<MatrixLike>::run(m,prec);
  }

  template<typename M1, typename M2>
  struct MatrixMatrixProduct
  {
#if EIGEN_VERSION_AT_LEAST(3,2,90)
    typedef typename Eigen::Product<M1,M2> type;
#else
    typedef typename Eigen::ProductReturnType<M1,M2>::Type type;
#endif
  };

  template<typename Scalar, typename Matrix>
  struct ScalarMatrixProduct
  {
#if EIGEN_VERSION_AT_LEAST(3,3,0)
    typedef Eigen::CwiseBinaryOp<EIGEN_CAT(EIGEN_CAT(Eigen::internal::scalar_,product),_op)<Scalar,typename Eigen::internal::traits<Matrix>::Scalar>,
    const typename Eigen::internal::plain_constant_type<Matrix,Scalar>::type, const Matrix> type;
#elif EIGEN_VERSION_AT_LEAST(3,2,90)
    typedef Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type;
#else
    typedef const Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type;
#endif
  };

  template<typename Matrix, typename Scalar>
  struct MatrixScalarProduct
  {
#if EIGEN_VERSION_AT_LEAST(3,3,0)
    typedef Eigen::CwiseBinaryOp<EIGEN_CAT(EIGEN_CAT(Eigen::internal::scalar_,product),_op)<typename Eigen::internal::traits<Matrix>::Scalar,Scalar>,
    const Matrix, const typename Eigen::internal::plain_constant_type<Matrix,Scalar>::type> type;
#elif EIGEN_VERSION_AT_LEAST(3,2,90)
    typedef Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type;
#else
    typedef const Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type;
#endif
  };

  namespace internal
  {
    template<typename MatrixLike, bool value = is_floating_point<typename MatrixLike::Scalar>::value>
    struct isUnitaryAlgo
    {
      typedef typename MatrixLike::Scalar Scalar;
      typedef typename MatrixLike::RealScalar RealScalar;

      static bool run(const Eigen::MatrixBase<MatrixLike> & mat,
                      const RealScalar & prec =
                      Eigen::NumTraits< Scalar >::dummy_precision())
      {
        return mat.isUnitary(prec);
      }
    };

    template<typename MatrixLike>
    struct isUnitaryAlgo<MatrixLike,false>
    {
      typedef typename MatrixLike::Scalar Scalar;
      typedef typename MatrixLike::RealScalar RealScalar;

      static bool run(const Eigen::MatrixBase<MatrixLike> & /*vec*/,
                      const RealScalar & prec =
                      Eigen::NumTraits< Scalar >::dummy_precision())
      {
        PINOCCHIO_UNUSED_VARIABLE(prec);
        return true;
      }
    };
  }

  ///
  /// \brief Check whether the input matrix is Unitary within the given precision.
  ///
  /// \param[in] mat Input matrix
  /// \param[in] prec Required precision
  ///
  /// \returns true if mat is unitary within the precision prec
  ///
  template<typename MatrixLike>
  inline bool isUnitary(const Eigen::MatrixBase<MatrixLike> & mat,
                        const typename MatrixLike::RealScalar & prec =
                        Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
  {
    return internal::isUnitaryAlgo<MatrixLike>::run(mat,prec);
  }

  namespace internal
  {
    template<typename VectorLike, bool value = is_floating_point<typename VectorLike::Scalar>::value>
    struct isNormalizedAlgo
    {
      typedef typename VectorLike::Scalar Scalar;
      typedef typename VectorLike::RealScalar RealScalar;

      static bool run(const Eigen::MatrixBase<VectorLike> & vec,
                      const RealScalar & prec =
                      Eigen::NumTraits<RealScalar>::dummy_precision())
      {
        return math::fabs(static_cast<RealScalar>(vec.norm() - RealScalar(1))) <= prec;
      }
    };

    template<typename VectorLike>
    struct isNormalizedAlgo<VectorLike,false>
    {
      typedef typename VectorLike::Scalar Scalar;
      typedef typename VectorLike::RealScalar RealScalar;

      static bool run(const Eigen::MatrixBase<VectorLike> & /*vec*/,
                      const RealScalar & prec =
                      Eigen::NumTraits<RealScalar>::dummy_precision())
      {
        PINOCCHIO_UNUSED_VARIABLE(prec);
        return true;
      }
    };
  }

  ///
  /// \brief Check whether the input vector is Normalized within the given precision.
  ///
  /// \param[in] vec Input vector
  /// \param[in] prec Required precision
  ///
  /// \returns true if vec is normalized within the precision prec.
  ///
  template<typename VectorLike>
  inline bool isNormalized(const Eigen::MatrixBase<VectorLike> & vec,
                           const typename VectorLike::RealScalar & prec =
                           Eigen::NumTraits< typename VectorLike::Scalar >::dummy_precision())
  {
    EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike);
    return internal::isNormalizedAlgo<VectorLike>::run(vec,prec);
  }

  namespace internal
  {
    template<typename Scalar>
    struct CallCorrectMatrixInverseAccordingToScalar
    {
      template<typename MatrixIn, typename MatrixOut>
      static void run(const Eigen::MatrixBase<MatrixIn> & m_in,
                      const Eigen::MatrixBase<MatrixOut> & dest)
      {
        MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut,dest);
        dest_.noalias() = m_in.inverse();
      }
    };

  }

  template<typename MatrixIn, typename MatrixOut>
  inline void inverse(const Eigen::MatrixBase<MatrixIn> & m_in,
                      const Eigen::MatrixBase<MatrixOut> & dest)
  {
    MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut,dest);
    internal::CallCorrectMatrixInverseAccordingToScalar<typename MatrixIn::Scalar>::run(m_in,dest_);
  }

}

#endif //#ifndef __pinocchio_math_matrix_hpp__


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2016-2020 CNRS INRIA
3			//
4
5			#ifndef __pinocchio_math_matrix_hpp__
6			#define __pinocchio_math_matrix_hpp__
7
8			#include "pinocchio/macros.hpp"
9			#include "pinocchio/math/fwd.hpp"
10
11			#include <Eigen/Core>
12			#include <boost/type_traits.hpp>
13
14			namespace pinocchio
15			{
16
17			template<typename Derived>
18		4304	inline bool hasNaN(const Eigen::DenseBase<Derived> & m)
19			{
20	✓✗✓✗ ✓✗	4304	return !((m.derived().array()==m.derived().array()).all());
21			}
22
23			namespace internal
24			{
25			template<typename MatrixLike, bool value = is_floating_point<typename MatrixLike::Scalar>::value>
26			struct isZeroAlgo
27			{
28			typedef typename MatrixLike::Scalar Scalar;
29			typedef typename MatrixLike::RealScalar RealScalar;
30
31		5386	static bool run(const Eigen::MatrixBase<MatrixLike> & mat,
32			const RealScalar & prec =
33			Eigen::NumTraits< Scalar >::dummy_precision())
34			{
35		5386	return mat.isZero(prec);
36			}
37			};
38
39			template<typename MatrixLike>
40			struct isZeroAlgo<MatrixLike,false>
41			{
42			typedef typename MatrixLike::Scalar Scalar;
43			typedef typename MatrixLike::RealScalar RealScalar;
44
45			static bool run(const Eigen::MatrixBase<MatrixLike> & /vec/,
46			const RealScalar & prec =
47			Eigen::NumTraits< Scalar >::dummy_precision())
48			{
49			PINOCCHIO_UNUSED_VARIABLE(prec);
50			return true;
51			}
52			};
53			}
54
55			template<typename MatrixLike>
56		5386	inline bool isZero(const Eigen::MatrixBase<MatrixLike> & m,
57			const typename MatrixLike::RealScalar & prec =
58			Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
59			{
60		5386	return internal::isZeroAlgo<MatrixLike>::run(m,prec);
61			}
62
63			template<typename M1, typename M2>
64			struct MatrixMatrixProduct
65			{
66			#if EIGEN_VERSION_AT_LEAST(3,2,90)
67			typedef typename Eigen::Product<M1,M2> type;
68			#else
69			typedef typename Eigen::ProductReturnType<M1,M2>::Type type;
70			#endif
71			};
72
73			template<typename Scalar, typename Matrix>
74			struct ScalarMatrixProduct
75			{
76			#if EIGEN_VERSION_AT_LEAST(3,3,0)
77			typedef Eigen::CwiseBinaryOp<EIGEN_CAT(EIGEN_CAT(Eigen::internal::scalar_,product),_op)<Scalar,typename Eigen::internal::traits<Matrix>::Scalar>,
78			const typename Eigen::internal::plain_constant_type<Matrix,Scalar>::type, const Matrix> type;
79			#elif EIGEN_VERSION_AT_LEAST(3,2,90)
80			typedef Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type;
81			#else
82			typedef const Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type;
83			#endif
84			};
85
86			template<typename Matrix, typename Scalar>
87			struct MatrixScalarProduct
88			{
89			#if EIGEN_VERSION_AT_LEAST(3,3,0)
90			typedef Eigen::CwiseBinaryOp<EIGEN_CAT(EIGEN_CAT(Eigen::internal::scalar_,product),_op)<typename Eigen::internal::traits<Matrix>::Scalar,Scalar>,
91			const Matrix, const typename Eigen::internal::plain_constant_type<Matrix,Scalar>::type> type;
92			#elif EIGEN_VERSION_AT_LEAST(3,2,90)
93			typedef Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type;
94			#else
95			typedef const Eigen::CwiseUnaryOp<Eigen::internal::scalar_multiple_op<Scalar>, const Matrix> type;
96			#endif
97			};
98
99			namespace internal
100			{
101			template<typename MatrixLike, bool value = is_floating_point<typename MatrixLike::Scalar>::value>
102			struct isUnitaryAlgo
103			{
104			typedef typename MatrixLike::Scalar Scalar;
105			typedef typename MatrixLike::RealScalar RealScalar;
106
107		379245	static bool run(const Eigen::MatrixBase<MatrixLike> & mat,
108			const RealScalar & prec =
109			Eigen::NumTraits< Scalar >::dummy_precision())
110			{
111		379245	return mat.isUnitary(prec);
112			}
113			};
114
115			template<typename MatrixLike>
116			struct isUnitaryAlgo<MatrixLike,false>
117			{
118			typedef typename MatrixLike::Scalar Scalar;
119			typedef typename MatrixLike::RealScalar RealScalar;
120
121			static bool run(const Eigen::MatrixBase<MatrixLike> & /vec/,
122			const RealScalar & prec =
123			Eigen::NumTraits< Scalar >::dummy_precision())
124			{
125			PINOCCHIO_UNUSED_VARIABLE(prec);
126			return true;
127			}
128			};
129			}
130
131			///
132			/// \brief Check whether the input matrix is Unitary within the given precision.
133			///
134			/// \param[in] mat Input matrix
135			/// \param[in] prec Required precision
136			///
137			/// \returns true if mat is unitary within the precision prec
138			///
139			template<typename MatrixLike>
140		273622	inline bool isUnitary(const Eigen::MatrixBase<MatrixLike> & mat,
141			const typename MatrixLike::RealScalar & prec =
142			Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
143			{
144		273622	return internal::isUnitaryAlgo<MatrixLike>::run(mat,prec);
145			}
146
147			namespace internal
148			{
149			template<typename VectorLike, bool value = is_floating_point<typename VectorLike::Scalar>::value>
150			struct isNormalizedAlgo
151			{
152			typedef typename VectorLike::Scalar Scalar;
153			typedef typename VectorLike::RealScalar RealScalar;
154
155		155174	static bool run(const Eigen::MatrixBase<VectorLike> & vec,
156			const RealScalar & prec =
157			Eigen::NumTraits<RealScalar>::dummy_precision())
158			{
159		155174	return math::fabs(static_cast<RealScalar>(vec.norm() - RealScalar(1))) <= prec;
160			}
161			};
162
163			template<typename VectorLike>
164			struct isNormalizedAlgo<VectorLike,false>
165			{
166			typedef typename VectorLike::Scalar Scalar;
167			typedef typename VectorLike::RealScalar RealScalar;
168
169			static bool run(const Eigen::MatrixBase<VectorLike> & /vec/,
170			const RealScalar & prec =
171			Eigen::NumTraits<RealScalar>::dummy_precision())
172			{
173			PINOCCHIO_UNUSED_VARIABLE(prec);
174			return true;
175			}
176			};
177			}
178
179			///
180			/// \brief Check whether the input vector is Normalized within the given precision.
181			///
182			/// \param[in] vec Input vector
183			/// \param[in] prec Required precision
184			///
185			/// \returns true if vec is normalized within the precision prec.
186			///
187			template<typename VectorLike>
188		77860	inline bool isNormalized(const Eigen::MatrixBase<VectorLike> & vec,
189			const typename VectorLike::RealScalar & prec =
190			Eigen::NumTraits< typename VectorLike::Scalar >::dummy_precision())
191			{
192			EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorLike);
193		77860	return internal::isNormalizedAlgo<VectorLike>::run(vec,prec);
194			}
195
196			namespace internal
197			{
198			template<typename Scalar>
199			struct CallCorrectMatrixInverseAccordingToScalar
200			{
201			template<typename MatrixIn, typename MatrixOut>
202			static void run(const Eigen::MatrixBase<MatrixIn> & m_in,
203			const Eigen::MatrixBase<MatrixOut> & dest)
204			{
205			MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut,dest);
206			dest_.noalias() = m_in.inverse();
207			}
208			};
209
210			}
211
212			template<typename MatrixIn, typename MatrixOut>
213			inline void inverse(const Eigen::MatrixBase<MatrixIn> & m_in,
214			const Eigen::MatrixBase<MatrixOut> & dest)
215			{
216			MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut,dest);
217			internal::CallCorrectMatrixInverseAccordingToScalar<typename MatrixIn::Scalar>::run(m_in,dest_);
218			}
219
220			}
221
222			#endif //#ifndef __pinocchio_math_matrix_hpp__