Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/spatial/symmetric3.hpp	Lines:	194	204	95.1 %
Date:	2024-01-23 21:41:47	Branches:	143	294	48.6 %


//
// Copyright (c) 2014-2019 CNRS INRIA
//

#ifndef __pinocchio_symmetric3_hpp__
#define __pinocchio_symmetric3_hpp__

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

namespace pinocchio
{

  template<typename _Scalar, int _Options>
  class Symmetric3Tpl
  {
  public:
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
    typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
    typedef Eigen::Matrix<Scalar,2,2,Options> Matrix2;
    typedef Eigen::Matrix<Scalar,3,2,Options> Matrix32;

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  public:
    Symmetric3Tpl(): m_data() {}

    template<typename Sc,int Opt>
    explicit Symmetric3Tpl(const Eigen::Matrix<Sc,3,3,Opt> & I)
    {
      assert( (I-I.transpose()).isMuchSmallerThan(I) );
      m_data(0) = I(0,0);
      m_data(1) = I(1,0); m_data(2) = I(1,1);
      m_data(3) = I(2,0); m_data(4) = I(2,1); m_data(5) = I(2,2);
    }

    explicit Symmetric3Tpl(const Vector6 & I) : m_data(I) {}

    Symmetric3Tpl(const Scalar & a0, const Scalar & a1, const Scalar & a2,
		  const Scalar & a3, const Scalar & a4, const Scalar & a5)
    { m_data << a0,a1,a2,a3,a4,a5; }

    static Symmetric3Tpl Zero()     { return Symmetric3Tpl(Vector6::Zero());  }
    void setZero() { m_data.setZero(); }

    static Symmetric3Tpl Random()   { return RandomPositive();  }
    void setRandom()
    {
      Scalar
      a = Scalar(std::rand())/RAND_MAX*2.0-1.0,
      b = Scalar(std::rand())/RAND_MAX*2.0-1.0,
      c = Scalar(std::rand())/RAND_MAX*2.0-1.0,
      d = Scalar(std::rand())/RAND_MAX*2.0-1.0,
      e = Scalar(std::rand())/RAND_MAX*2.0-1.0,
      f = Scalar(std::rand())/RAND_MAX*2.0-1.0;

      m_data << a, b, c, d, e, f;
    }

    static Symmetric3Tpl Identity() { return Symmetric3Tpl(1, 0, 1, 0, 0, 1);  }
    void setIdentity() { m_data << 1, 0, 1, 0, 0, 1; }

    /* Required by Inertia::operator== */
    bool operator==(const Symmetric3Tpl & other) const
    { return m_data == other.m_data; }

    bool operator!=(const Symmetric3Tpl & other) const
    { return !(*this == other); }

    bool isApprox(const Symmetric3Tpl & other,
                  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
    { return m_data.isApprox(other.m_data,prec); }

    bool isZero(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
    { return m_data.isZero(prec); }

    void fill(const Scalar value) { m_data.fill(value); }

    struct SkewSquare
    {
      const Vector3 & v;
      SkewSquare( const Vector3 & v ) : v(v) {}
      operator Symmetric3Tpl () const
      {
        const Scalar & x = v[0], & y = v[1], & z = v[2];
        return Symmetric3Tpl( -y*y-z*z,
                             x*y    ,  -x*x-z*z,
                             x*z    ,   y*z    ,  -x*x-y*y );
      }
    }; // struct SkewSquare

    Symmetric3Tpl operator- (const SkewSquare & v) const
    {
      const Scalar & x = v.v[0], & y = v.v[1], & z = v.v[2];
      return Symmetric3Tpl(m_data[0]+y*y+z*z,
                           m_data[1]-x*y,m_data[2]+x*x+z*z,
                           m_data[3]-x*z,m_data[4]-y*z,m_data[5]+x*x+y*y);
    }

    Symmetric3Tpl& operator-= (const SkewSquare & v)
    {
      const Scalar & x = v.v[0], & y = v.v[1], & z = v.v[2];
      m_data[0]+=y*y+z*z;
      m_data[1]-=x*y; m_data[2]+=x*x+z*z;
      m_data[3]-=x*z; m_data[4]-=y*z; m_data[5]+=x*x+y*y;
      return *this;
    }

    template<typename D>
    friend Matrix3 operator- (const Symmetric3Tpl & S, const Eigen::MatrixBase <D> & M)
    {
      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(D,3,3);
      Matrix3 result (S.matrix());
      result -= M;

      return result;
    }

    struct AlphaSkewSquare
    {
      const Scalar & m;
      const Vector3 & v;

      AlphaSkewSquare(const Scalar & m, const SkewSquare & v) : m(m),v(v.v) {}
      AlphaSkewSquare(const Scalar & m, const Vector3 & v) : m(m),v(v) {}

      operator Symmetric3Tpl () const
      {
        const Scalar & x = v[0], & y = v[1], & z = v[2];
        return Symmetric3Tpl(-m*(y*y+z*z),
                             m* x*y,-m*(x*x+z*z),
                             m* x*z,m* y*z,-m*(x*x+y*y));
      }
    };

    friend AlphaSkewSquare operator* (const Scalar & m, const SkewSquare & sk)
    { return AlphaSkewSquare(m,sk); }

    Symmetric3Tpl operator- (const AlphaSkewSquare & v) const
    {
      const Scalar & x = v.v[0], & y = v.v[1], & z = v.v[2];
      return Symmetric3Tpl(m_data[0]+v.m*(y*y+z*z),
                           m_data[1]-v.m* x*y, m_data[2]+v.m*(x*x+z*z),
                           m_data[3]-v.m* x*z, m_data[4]-v.m* y*z,
                           m_data[5]+v.m*(x*x+y*y));
    }

    Symmetric3Tpl& operator-= (const AlphaSkewSquare & v)
    {
      const Scalar & x = v.v[0], & y = v.v[1], & z = v.v[2];
      m_data[0]+=v.m*(y*y+z*z);
      m_data[1]-=v.m* x*y; m_data[2]+=v.m*(x*x+z*z);
      m_data[3]-=v.m* x*z; m_data[4]-=v.m* y*z; m_data[5]+=v.m*(x*x+y*y);
      return *this;
    }

    const Vector6 & data () const {return m_data;}
    Vector6 & data () {return m_data;}

    // static Symmetric3Tpl SkewSq( const Vector3 & v )
    // {
    //   const Scalar & x = v[0], & y = v[1], & z = v[2];
    //   return Symmetric3Tpl(-y*y-z*z,
    // 			    x*y, -x*x-z*z,
    // 			    x*z, y*z, -x*x-y*y );
    // }

    /* Shoot a positive definite matrix. */
    static Symmetric3Tpl RandomPositive()
    {
      Scalar
      a = Scalar(std::rand())/RAND_MAX*2.0-1.0,
      b = Scalar(std::rand())/RAND_MAX*2.0-1.0,
      c = Scalar(std::rand())/RAND_MAX*2.0-1.0,
      d = Scalar(std::rand())/RAND_MAX*2.0-1.0,
      e = Scalar(std::rand())/RAND_MAX*2.0-1.0,
      f = Scalar(std::rand())/RAND_MAX*2.0-1.0;
      return Symmetric3Tpl(a*a+b*b+d*d,
			   a*b+b*c+d*e, b*b+c*c+e*e,
			   a*d+b*e+d*f, b*d+c*e+e*f,  d*d+e*e+f*f );
    }

    Matrix3 matrix() const
    {
      Matrix3 res;
      res(0,0) = m_data(0); res(0,1) = m_data(1); res(0,2) = m_data(3);
      res(1,0) = m_data(1); res(1,1) = m_data(2); res(1,2) = m_data(4);
      res(2,0) = m_data(3); res(2,1) = m_data(4); res(2,2) = m_data(5);
      return res;
    }
    operator Matrix3 () const { return matrix(); }

    Scalar vtiv (const Vector3 & v) const
    {
      const Scalar & x = v[0];
      const Scalar & y = v[1];
      const Scalar & z = v[2];

      const Scalar xx = x*x;
      const Scalar xy = x*y;
      const Scalar xz = x*z;
      const Scalar yy = y*y;
      const Scalar yz = y*z;
      const Scalar zz = z*z;

      return m_data(0)*xx + m_data(2)*yy + m_data(5)*zz + 2.*(m_data(1)*xy + m_data(3)*xz + m_data(4)*yz);
    }

    ///
    /// \brief Performs the operation \f$ M = [v]_{\times} S_{3} \f$.
    ///        This operation is equivalent to applying the cross product of v on each column of S.
    ///
    /// \tparam Vector3, Matrix3
    ///
    /// \param[in]  v  a vector of dimension 3.
    /// \param[in]  S3 a symmetric matrix of dimension 3x3.
    /// \param[out] M  an output matrix of dimension 3x3.
    ///
    template<typename Vector3, typename Matrix3>
    static void vxs(const Eigen::MatrixBase<Vector3> & v,
                    const Symmetric3Tpl & S3,
                    const Eigen::MatrixBase<Matrix3> & M)
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);

      const Scalar & a = S3.data()[0];
      const Scalar & b = S3.data()[1];
      const Scalar & c = S3.data()[2];
      const Scalar & d = S3.data()[3];
      const Scalar & e = S3.data()[4];
      const Scalar & f = S3.data()[5];


      const typename Vector3::RealScalar & v0 = v[0];
      const typename Vector3::RealScalar & v1 = v[1];
      const typename Vector3::RealScalar & v2 = v[2];

      Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,M);
      M_(0,0) = d * v1 - b * v2;
      M_(1,0) = a * v2 - d * v0;
      M_(2,0) = b * v0 - a * v1;

      M_(0,1) = e * v1 - c * v2;
      M_(1,1) = b * v2 - e * v0;
      M_(2,1) = c * v0 - b * v1;

      M_(0,2) = f * v1 - e * v2;
      M_(1,2) = d * v2 - f * v0;
      M_(2,2) = e * v0 - d * v1;
    }

    ///
    /// \brief Performs the operation \f$ [v]_{\times} S \f$.
    ///        This operation is equivalent to applying the cross product of v on each column of S.
    ///
    /// \tparam Vector3
    ///
    /// \param[in]  v  a vector of dimension 3.
    ///
    /// \returns the result \f$ [v]_{\times} S \f$.
    ///
    template<typename Vector3>
    Matrix3 vxs(const Eigen::MatrixBase<Vector3> & v) const
    {
      Matrix3 M;
      vxs(v,*this,M);
      return M;
    }

    ///
    /// \brief Performs the operation \f$ M = S_{3} [v]_{\times} \f$.
    ///
    /// \tparam Vector3, Matrix3
    ///
    /// \param[in]  v  a vector of dimension 3.
    /// \param[in]  S3 a symmetric matrix of dimension 3x3.
    /// \param[out] M  an output matrix of dimension 3x3.
    ///
    template<typename Vector3, typename Matrix3>
    static void svx(const Eigen::MatrixBase<Vector3> & v,
                    const Symmetric3Tpl & S3,
                    const Eigen::MatrixBase<Matrix3> & M)
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);

      const Scalar & a = S3.data()[0];
      const Scalar & b = S3.data()[1];
      const Scalar & c = S3.data()[2];
      const Scalar & d = S3.data()[3];
      const Scalar & e = S3.data()[4];
      const Scalar & f = S3.data()[5];

      const typename Vector3::RealScalar & v0 = v[0];
      const typename Vector3::RealScalar & v1 = v[1];
      const typename Vector3::RealScalar & v2 = v[2];

      Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,M);
      M_(0,0) = b * v2 - d * v1;
      M_(1,0) = c * v2 - e * v1;
      M_(2,0) = e * v2 - f * v1;

      M_(0,1) = d * v0 - a * v2;
      M_(1,1) = e * v0 - b * v2;
      M_(2,1) = f * v0 - d * v2;

      M_(0,2) = a * v1 - b * v0;
      M_(1,2) = b * v1 - c * v0;
      M_(2,2) = d * v1 - e * v0;
    }

    /// \brief Performs the operation \f$ M = S_{3} [v]_{\times} \f$.
    ///
    /// \tparam Vector3
    ///
    /// \param[in]  v  a vector of dimension 3.
    ///
    /// \returns the result \f$ S [v]_{\times} \f$.
    ///
    template<typename Vector3>
    Matrix3 svx(const Eigen::MatrixBase<Vector3> & v) const
    {
      Matrix3 M;
      svx(v,*this,M);
      return M;
    }

    Symmetric3Tpl operator+(const Symmetric3Tpl & s2) const
    {
      return Symmetric3Tpl((m_data+s2.m_data).eval());
    }

    Symmetric3Tpl & operator+=(const Symmetric3Tpl & s2)
    {
      m_data += s2.m_data; return *this;
    }

    template<typename V3in, typename V3out>
    static void rhsMult(const Symmetric3Tpl & S3,
                        const Eigen::MatrixBase<V3in> & vin,
                        const Eigen::MatrixBase<V3out> & vout)
    {
      EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(V3in,Vector3);
      EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(V3out,Vector3);

      V3out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3out,vout);

      vout_[0] = S3.m_data(0) * vin[0] + S3.m_data(1) * vin[1] + S3.m_data(3) * vin[2];
      vout_[1] = S3.m_data(1) * vin[0] + S3.m_data(2) * vin[1] + S3.m_data(4) * vin[2];
      vout_[2] = S3.m_data(3) * vin[0] + S3.m_data(4) * vin[1] + S3.m_data(5) * vin[2];
    }

    template<typename V3>
    Vector3 operator*(const Eigen::MatrixBase<V3> & v) const
    {
      Vector3 res;
      rhsMult(*this,v,res);
      return res;
    }

    // Matrix3 operator*(const Matrix3 &a) const
    // {
    //   Matrix3 r;
    //   for(unsigned int i=0; i<3; ++i)
    //     {
    //       r(0,i) = m_data(0) * a(0,i) + m_data(1) * a(1,i) + m_data(3) * a(2,i);
    //       r(1,i) = m_data(1) * a(0,i) + m_data(2) * a(1,i) + m_data(4) * a(2,i);
    //       r(2,i) = m_data(3) * a(0,i) + m_data(4) * a(1,i) + m_data(5) * a(2,i);
    //     }
    //   return r;
    // }

    const Scalar& operator()(const int &i,const int &j) const
    {
      return ((i!=2)&&(j!=2)) ? m_data[i+j] : m_data[i+j+1];
    }

    Symmetric3Tpl operator-(const Matrix3 &S) const
    {
      assert( (S-S.transpose()).isMuchSmallerThan(S) );
      return Symmetric3Tpl( m_data(0)-S(0,0),
			    m_data(1)-S(1,0), m_data(2)-S(1,1),
			    m_data(3)-S(2,0), m_data(4)-S(2,1), m_data(5)-S(2,2) );
    }

    Symmetric3Tpl operator+(const Matrix3 &S) const
    {
      assert( (S-S.transpose()).isMuchSmallerThan(S) );
      return Symmetric3Tpl( m_data(0)+S(0,0),
			    m_data(1)+S(1,0), m_data(2)+S(1,1),
			    m_data(3)+S(2,0), m_data(4)+S(2,1), m_data(5)+S(2,2) );
    }

    /* --- Symmetric R*S*R' and R'*S*R products --- */
  public: //private:

    /** \brief Computes L for a symmetric matrix A.
     */
    Matrix32  decomposeltI() const
    {
      Matrix32 L;
      L <<
      m_data(0) - m_data(5),    m_data(1),
      m_data(1),              m_data(2) - m_data(5),
      2*m_data(3),            m_data(4) + m_data(4);
      return L;
    }

    /* R*S*R' */
    template<typename D>
    Symmetric3Tpl rotate(const Eigen::MatrixBase<D> & R) const
    {
      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(D,3,3);
      assert(isUnitary(R.transpose()*R) && "R is not a Unitary matrix");

      Symmetric3Tpl Sres;

      // 4 a
      const Matrix32 L( decomposeltI() );

      // Y = R' L   ===> (12 m + 8 a)
      const Matrix2 Y( R.template block<2,3>(1,0) * L );

      // Sres= Y R  ===> (16 m + 8a)
      Sres.m_data(1) = Y(0,0)*R(0,0) + Y(0,1)*R(0,1);
      Sres.m_data(2) = Y(0,0)*R(1,0) + Y(0,1)*R(1,1);
      Sres.m_data(3) = Y(1,0)*R(0,0) + Y(1,1)*R(0,1);
      Sres.m_data(4) = Y(1,0)*R(1,0) + Y(1,1)*R(1,1);
      Sres.m_data(5) = Y(1,0)*R(2,0) + Y(1,1)*R(2,1);

      // r=R' v ( 6m + 3a)
      const Vector3 r(-R(0,0)*m_data(4) + R(0,1)*m_data(3),
                      -R(1,0)*m_data(4) + R(1,1)*m_data(3),
                      -R(2,0)*m_data(4) + R(2,1)*m_data(3));

      // Sres_11 (3a)
      Sres.m_data(0) = L(0,0) + L(1,1) - Sres.m_data(2) - Sres.m_data(5);

      // Sres + D + (Ev)x ( 9a)
      Sres.m_data(0) += m_data(5);
      Sres.m_data(1) += r(2); Sres.m_data(2)+= m_data(5);
      Sres.m_data(3) +=-r(1); Sres.m_data(4)+= r(0); Sres.m_data(5) += m_data(5);

      return Sres;
    }

    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    Symmetric3Tpl<NewScalar,Options> cast() const
    {
      return Symmetric3Tpl<NewScalar,Options>(m_data.template cast<NewScalar>());
    }

    // TODO: adjust code
//    bool isValid() const
//    {
//      return
//         m_data(0) >= Scalar(0)
//      && m_data(2) >= Scalar(0)
//      && m_data(5) >= Scalar(0);
//    }

  protected:
    Vector6 m_data;

  };

} // namespace pinocchio

#endif // ifndef __pinocchio_symmetric3_hpp__



Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2014-2019 CNRS INRIA
3			//
4
5			#ifndef __pinocchio_symmetric3_hpp__
6			#define __pinocchio_symmetric3_hpp__
7
8			#include "pinocchio/macros.hpp"
9			#include "pinocchio/math/matrix.hpp"
10
11			namespace pinocchio
12			{
13
14			template<typename _Scalar, int _Options>
15			class Symmetric3Tpl
16			{
17			public:
18			typedef _Scalar Scalar;
19			enum { Options = _Options };
20			typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
21			typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
22			typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
23			typedef Eigen::Matrix<Scalar,2,2,Options> Matrix2;
24			typedef Eigen::Matrix<Scalar,3,2,Options> Matrix32;
25
26			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
27
28			public:
29		152703	Symmetric3Tpl(): m_data() {}
30
31			template<typename Sc,int Opt>
32		1588	explicit Symmetric3Tpl(const Eigen::Matrix<Sc,3,3,Opt> & I)
33		1588	{
34	✓✗✓✗ ✗✓	1588	assert( (I-I.transpose()).isMuchSmallerThan(I) );
35		1588	m_data(0) = I(0,0);
36		1588	m_data(1) = I(1,0); m_data(2) = I(1,1);
37		1588	m_data(3) = I(2,0); m_data(4) = I(2,1); m_data(5) = I(2,2);
38		1588	}
39
40		42908	explicit Symmetric3Tpl(const Vector6 & I) : m_data(I) {}
41
42		25572	Symmetric3Tpl(const Scalar & a0, const Scalar & a1, const Scalar & a2,
43			const Scalar & a3, const Scalar & a4, const Scalar & a5)
44	✓✗✓✗ ✓✗✓✗ ✓✗	25572	{ m_data << a0,a1,a2,a3,a4,a5; }
45
46	✓✗✓✗	40120	static Symmetric3Tpl Zero() { return Symmetric3Tpl(Vector6::Zero()); }
47		287	void setZero() { m_data.setZero(); }
48
49		7	static Symmetric3Tpl Random() { return RandomPositive(); }
50		1	void setRandom()
51			{
52			Scalar
53		1	a = Scalar(std::rand())/RAND_MAX*2.0-1.0,
54		1	b = Scalar(std::rand())/RAND_MAX*2.0-1.0,
55		1	c = Scalar(std::rand())/RAND_MAX*2.0-1.0,
56		1	d = Scalar(std::rand())/RAND_MAX*2.0-1.0,
57		1	e = Scalar(std::rand())/RAND_MAX*2.0-1.0,
58		1	f = Scalar(std::rand())/RAND_MAX*2.0-1.0;
59
60	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	1	m_data << a, b, c, d, e, f;
61		1	}
62
63	✓✗	4	static Symmetric3Tpl Identity() { return Symmetric3Tpl(1, 0, 1, 0, 0, 1); }
64			void setIdentity() { m_data << 1, 0, 1, 0, 0, 1; }
65
66			/* Required by Inertia::operator== */
67		7993	bool operator==(const Symmetric3Tpl & other) const
68		7993	{ return m_data == other.m_data; }
69
70		1	bool operator!=(const Symmetric3Tpl & other) const
71		1	{ return !(*this == other); }
72
73		151	bool isApprox(const Symmetric3Tpl & other,
74			const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
75		151	{ return m_data.isApprox(other.m_data,prec); }
76
77		25	bool isZero(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
78		25	{ return m_data.isZero(prec); }
79
80			void fill(const Scalar value) { m_data.fill(value); }
81
82			struct SkewSquare
83			{
84			const Vector3 & v;
85		51363	SkewSquare( const Vector3 & v ) : v(v) {}
86		31	operator Symmetric3Tpl () const
87			{
88		31	const Scalar & x = v[0], & y = v[1], & z = v[2];
89			return Symmetric3Tpl( -yy-zz,
90			xy , -xx-z*z,
91	✓✗	62	xz , yz , -xx-yy );
92			}
93			}; // struct SkewSquare
94
95		1	Symmetric3Tpl operator- (const SkewSquare & v) const
96			{
97		1	const Scalar & x = v.v[0], & y = v.v[1], & z = v.v[2];
98			return Symmetric3Tpl(m_data[0]+yy+zz,
99	✓✗✓✗	1	m_data[1]-xy,m_data[2]+xx+z*z,
100	✓✗✓✗ ✓✗✓✗	3	m_data[3]-xz,m_data[4]-yz,m_data[5]+xx+yy);
101			}
102
103		1	Symmetric3Tpl& operator-= (const SkewSquare & v)
104			{
105		1	const Scalar & x = v.v[0], & y = v.v[1], & z = v.v[2];
106		1	m_data[0]+=yy+zz;
107		1	m_data[1]-=xy; m_data[2]+=xx+z*z;
108		1	m_data[3]-=xz; m_data[4]-=yz; m_data[5]+=xx+yy;
109		1	return *this;
110			}
111
112			template<typename D>
113			friend Matrix3 operator- (const Symmetric3Tpl & S, const Eigen::MatrixBase <D> & M)
114			{
115			EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(D,3,3);
116			Matrix3 result (S.matrix());
117			result -= M;
118
119			return result;
120			}
121
122			struct AlphaSkewSquare
123			{
124			const Scalar & m;
125			const Vector3 & v;
126
127		51330	AlphaSkewSquare(const Scalar & m, const SkewSquare & v) : m(m),v(v.v) {}
128		19508	AlphaSkewSquare(const Scalar & m, const Vector3 & v) : m(m),v(v) {}
129
130		132	operator Symmetric3Tpl () const
131			{
132		132	const Scalar & x = v[0], & y = v[1], & z = v[2];
133			return Symmetric3Tpl(-m(yy+z*z),
134			m* xy,-m(xx+zz),
135	✓✗	264	m* xz,m yz,-m(xx+yy));
136			}
137			};
138
139		51330	friend AlphaSkewSquare operator* (const Scalar & m, const SkewSquare & sk)
140		51330	{ return AlphaSkewSquare(m,sk); }
141
142		19380	Symmetric3Tpl operator- (const AlphaSkewSquare & v) const
143			{
144		19380	const Scalar & x = v.v[0], & y = v.v[1], & z = v.v[2];
145			return Symmetric3Tpl(m_data[0]+v.m(yy+z*z),
146	✓✗✓✗	19380	m_data[1]-v.m* xy, m_data[2]+v.m(xx+zz),
147	✓✗✓✗	19380	m_data[3]-v.m* xz, m_data[4]-v.m y*z,
148	✓✗✓✗	58140	m_data[5]+v.m(xx+y*y));
149			}
150
151		51324	Symmetric3Tpl& operator-= (const AlphaSkewSquare & v)
152			{
153		51324	const Scalar & x = v.v[0], & y = v.v[1], & z = v.v[2];
154		51324	m_data[0]+=v.m(yy+z*z);
155		51324	m_data[1]-=v.m* xy; m_data[2]+=v.m(xx+zz);
156		51324	m_data[3]-=v.m* xz; m_data[4]-=v.m yz; m_data[5]+=v.m(xx+yy);
157		51324	return *this;
158			}
159
160		4845	const Vector6 & data () const {return m_data;}
161		2333	Vector6 & data () {return m_data;}
162
163			// static Symmetric3Tpl SkewSq( const Vector3 & v )
164			// {
165			// const Scalar & x = v[0], & y = v[1], & z = v[2];
166			// return Symmetric3Tpl(-yy-zz,
167			// xy, -xx-z*z,
168			// xz, yz, -xx-yy );
169			// }
170
171			/* Shoot a positive definite matrix. */
172		6011	static Symmetric3Tpl RandomPositive()
173			{
174			Scalar
175		6011	a = Scalar(std::rand())/RAND_MAX*2.0-1.0,
176		6011	b = Scalar(std::rand())/RAND_MAX*2.0-1.0,
177		6011	c = Scalar(std::rand())/RAND_MAX*2.0-1.0,
178		6011	d = Scalar(std::rand())/RAND_MAX*2.0-1.0,
179		6011	e = Scalar(std::rand())/RAND_MAX*2.0-1.0,
180		6011	f = Scalar(std::rand())/RAND_MAX*2.0-1.0;
181			return Symmetric3Tpl(aa+bb+d*d,
182			ab+bc+de, bb+cc+ee,
183	✓✗	12022	ad+be+df, bd+ce+ef, dd+ee+f*f );
184			}
185
186		19414	Matrix3 matrix() const
187			{
188		19414	Matrix3 res;
189		19414	res(0,0) = m_data(0); res(0,1) = m_data(1); res(0,2) = m_data(3);
190		19414	res(1,0) = m_data(1); res(1,1) = m_data(2); res(1,2) = m_data(4);
191		19414	res(2,0) = m_data(3); res(2,1) = m_data(4); res(2,2) = m_data(5);
192		19414	return res;
193			}
194		1	operator Matrix3 () const { return matrix(); }
195
196		5649	Scalar vtiv (const Vector3 & v) const
197			{
198		5649	const Scalar & x = v[0];
199		5649	const Scalar & y = v[1];
200		5649	const Scalar & z = v[2];
201
202		5649	const Scalar xx = x*x;
203		5649	const Scalar xy = x*y;
204		5649	const Scalar xz = x*z;
205		5649	const Scalar yy = y*y;
206		5649	const Scalar yz = y*z;
207		5649	const Scalar zz = z*z;
208
209		5649	return m_data(0)xx + m_data(2)yy + m_data(5)zz + 2.(m_data(1)xy + m_data(3)xz + m_data(4)*yz);
210			}
211
212			///
213			/// \brief Performs the operation \f$ M = [v]_{\times} S_{3} \f$.
214			/// This operation is equivalent to applying the cross product of v on each column of S.
215			///
216			/// \tparam Vector3, Matrix3
217			///
218			/// \param[in] v a vector of dimension 3.
219			/// \param[in] S3 a symmetric matrix of dimension 3x3.
220			/// \param[out] M an output matrix of dimension 3x3.
221			///
222			template<typename Vector3, typename Matrix3>
223		264	static void vxs(const Eigen::MatrixBase<Vector3> & v,
224			const Symmetric3Tpl & S3,
225			const Eigen::MatrixBase<Matrix3> & M)
226			{
227			EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
228			EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
229
230		264	const Scalar & a = S3.data()[0];
231		264	const Scalar & b = S3.data()[1];
232		264	const Scalar & c = S3.data()[2];
233		264	const Scalar & d = S3.data()[3];
234		264	const Scalar & e = S3.data()[4];
235		264	const Scalar & f = S3.data()[5];
236
237
238		264	const typename Vector3::RealScalar & v0 = v[0];
239		264	const typename Vector3::RealScalar & v1 = v[1];
240		264	const typename Vector3::RealScalar & v2 = v[2];
241
242		264	Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,M);
243		264	M_(0,0) = d * v1 - b * v2;
244		264	M_(1,0) = a * v2 - d * v0;
245		264	M_(2,0) = b * v0 - a * v1;
246
247		264	M_(0,1) = e * v1 - c * v2;
248		264	M_(1,1) = b * v2 - e * v0;
249		264	M_(2,1) = c * v0 - b * v1;
250
251		264	M_(0,2) = f * v1 - e * v2;
252		264	M_(1,2) = d * v2 - f * v0;
253		264	M_(2,2) = e * v0 - d * v1;
254		264	}
255
256			///
257			/// \brief Performs the operation \f$ [v]_{\times} S \f$.
258			/// This operation is equivalent to applying the cross product of v on each column of S.
259			///
260			/// \tparam Vector3
261			///
262			/// \param[in] v a vector of dimension 3.
263			///
264			/// \returns the result \f$ [v]_{\times} S \f$.
265			///
266			template<typename Vector3>
267		263	Matrix3 vxs(const Eigen::MatrixBase<Vector3> & v) const
268			{
269		263	Matrix3 M;
270		263	vxs(v,*this,M);
271		263	return M;
272			}
273
274			///
275			/// \brief Performs the operation \f$ M = S_{3} [v]_{\times} \f$.
276			///
277			/// \tparam Vector3, Matrix3
278			///
279			/// \param[in] v a vector of dimension 3.
280			/// \param[in] S3 a symmetric matrix of dimension 3x3.
281			/// \param[out] M an output matrix of dimension 3x3.
282			///
283			template<typename Vector3, typename Matrix3>
284		5	static void svx(const Eigen::MatrixBase<Vector3> & v,
285			const Symmetric3Tpl & S3,
286			const Eigen::MatrixBase<Matrix3> & M)
287			{
288			EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
289			EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
290
291		5	const Scalar & a = S3.data()[0];
292		5	const Scalar & b = S3.data()[1];
293		5	const Scalar & c = S3.data()[2];
294		5	const Scalar & d = S3.data()[3];
295		5	const Scalar & e = S3.data()[4];
296		5	const Scalar & f = S3.data()[5];
297
298		5	const typename Vector3::RealScalar & v0 = v[0];
299		5	const typename Vector3::RealScalar & v1 = v[1];
300		5	const typename Vector3::RealScalar & v2 = v[2];
301
302		5	Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,M);
303		5	M_(0,0) = b * v2 - d * v1;
304		5	M_(1,0) = c * v2 - e * v1;
305		5	M_(2,0) = e * v2 - f * v1;
306
307		5	M_(0,1) = d * v0 - a * v2;
308		5	M_(1,1) = e * v0 - b * v2;
309		5	M_(2,1) = f * v0 - d * v2;
310
311		5	M_(0,2) = a * v1 - b * v0;
312		5	M_(1,2) = b * v1 - c * v0;
313		5	M_(2,2) = d * v1 - e * v0;
314		5	}
315
316			/// \brief Performs the operation \f$ M = S_{3} [v]_{\times} \f$.
317			///
318			/// \tparam Vector3
319			///
320			/// \param[in] v a vector of dimension 3.
321			///
322			/// \returns the result \f$ S [v]_{\times} \f$.
323			///
324			template<typename Vector3>
325		4	Matrix3 svx(const Eigen::MatrixBase<Vector3> & v) const
326			{
327		4	Matrix3 M;
328		4	svx(v,*this,M);
329		4	return M;
330			}
331
332		4	Symmetric3Tpl operator+(const Symmetric3Tpl & s2) const
333			{
334	✓✗✓✗	4	return Symmetric3Tpl((m_data+s2.m_data).eval());
335			}
336
337		51325	Symmetric3Tpl & operator+=(const Symmetric3Tpl & s2)
338			{
339		51325	m_data += s2.m_data; return *this;
340			}
341
342			template<typename V3in, typename V3out>
343		192540	static void rhsMult(const Symmetric3Tpl & S3,
344			const Eigen::MatrixBase<V3in> & vin,
345			const Eigen::MatrixBase<V3out> & vout)
346			{
347			EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(V3in,Vector3);
348			EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(V3out,Vector3);
349
350		192540	V3out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3out,vout);
351
352		192540	vout_[0] = S3.m_data(0) * vin[0] + S3.m_data(1) * vin[1] + S3.m_data(3) * vin[2];
353		192540	vout_[1] = S3.m_data(1) * vin[0] + S3.m_data(2) * vin[1] + S3.m_data(4) * vin[2];
354		192540	vout_[2] = S3.m_data(3) * vin[0] + S3.m_data(4) * vin[1] + S3.m_data(5) * vin[2];
355		192540	}
356
357			template<typename V3>
358		6384	Vector3 operator*(const Eigen::MatrixBase<V3> & v) const
359			{
360		6384	Vector3 res;
361		6384	rhsMult(*this,v,res);
362		6384	return res;
363			}
364
365			// Matrix3 operator*(const Matrix3 &a) const
366			// {
367			// Matrix3 r;
368			// for(unsigned int i=0; i<3; ++i)
369			// {
370			// r(0,i) = m_data(0) * a(0,i) + m_data(1) * a(1,i) + m_data(3) * a(2,i);
371			// r(1,i) = m_data(1) * a(0,i) + m_data(2) * a(1,i) + m_data(4) * a(2,i);
372			// r(2,i) = m_data(3) * a(0,i) + m_data(4) * a(1,i) + m_data(5) * a(2,i);
373			// }
374			// return r;
375			// }
376
377		3084	const Scalar& operator()(const int &i,const int &j) const
378			{
379	✓✓✓✓	3084	return ((i!=2)&&(j!=2)) ? m_data[i+j] : m_data[i+j+1];
380			}
381
382		1	Symmetric3Tpl operator-(const Matrix3 &S) const
383			{
384	✓✗✓✗ ✗✓	1	assert( (S-S.transpose()).isMuchSmallerThan(S) );
385	✓✗	1	return Symmetric3Tpl( m_data(0)-S(0,0),
386	✓✗✓✗ ✓✗✓✗	1	m_data(1)-S(1,0), m_data(2)-S(1,1),
387	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	3	m_data(3)-S(2,0), m_data(4)-S(2,1), m_data(5)-S(2,2) );
388			}
389
390		2	Symmetric3Tpl operator+(const Matrix3 &S) const
391			{
392	✓✗✓✗ ✗✓	2	assert( (S-S.transpose()).isMuchSmallerThan(S) );
393	✓✗	2	return Symmetric3Tpl( m_data(0)+S(0,0),
394	✓✗✓✗ ✓✗✓✗	2	m_data(1)+S(1,0), m_data(2)+S(1,1),
395	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	6	m_data(3)+S(2,0), m_data(4)+S(2,1), m_data(5)+S(2,2) );
396			}
397
398			/* --- Symmetric RSR' and R'SR products --- */
399			public: //private:
400
401			/** \brief Computes L for a symmetric matrix A.
402			*/
403		151633	Matrix32 decomposeltI() const
404			{
405		151633	Matrix32 L;
406			L <<
407	✓✗✓✗ ✓✗✓✗	303266	m_data(0) - m_data(5), m_data(1),
408	✓✗✓✗ ✓✗✓✗	303266	m_data(1), m_data(2) - m_data(5),
409	✓✗✓✗ ✓✗✓✗ ✓✗	151633	2*m_data(3), m_data(4) + m_data(4);
410		151633	return L;
411			}
412
413			/* RSR' */
414			template<typename D>
415		252724	Symmetric3Tpl rotate(const Eigen::MatrixBase<D> & R) const
416			{
417			EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(D,3,3);
418	✓✗✓✗ ✓✗✓✗	252724	assert(isUnitary(R.transpose()*R) && "R is not a Unitary matrix");
419
420	✓✗	252724	Symmetric3Tpl Sres;
421
422			// 4 a
423	✓✗	252724	const Matrix32 L( decomposeltI() );
424
425			// Y = R' L ===> (12 m + 8 a)
426	✓✗✓✗ ✓✗	252724	const Matrix2 Y( R.template block<2,3>(1,0) * L );
427
428			// Sres= Y R ===> (16 m + 8a)
429	✓✗✓✗ ✓✗✓✗ ✓✗	252724	Sres.m_data(1) = Y(0,0)R(0,0) + Y(0,1)R(0,1);
430	✓✗✓✗ ✓✗✓✗ ✓✗	252724	Sres.m_data(2) = Y(0,0)R(1,0) + Y(0,1)R(1,1);
431	✓✗✓✗ ✓✗✓✗ ✓✗	252724	Sres.m_data(3) = Y(1,0)R(0,0) + Y(1,1)R(0,1);
432	✓✗✓✗ ✓✗✓✗ ✓✗	252724	Sres.m_data(4) = Y(1,0)R(1,0) + Y(1,1)R(1,1);
433	✓✗✓✗ ✓✗✓✗ ✓✗	252724	Sres.m_data(5) = Y(1,0)R(2,0) + Y(1,1)R(2,1);
434
435			// r=R' v ( 6m + 3a)
436	✓✗✓✗ ✓✗✓✗	252724	const Vector3 r(-R(0,0)m_data(4) + R(0,1)m_data(3),
437	✓✗✓✗ ✓✗✓✗	252724	-R(1,0)m_data(4) + R(1,1)m_data(3),
438	✓✗✓✗ ✓✗✓✗ ✓✗	252724	-R(2,0)m_data(4) + R(2,1)m_data(3));
439
440			// Sres_11 (3a)
441	✓✗✓✗ ✓✗✓✗ ✓✗	252724	Sres.m_data(0) = L(0,0) + L(1,1) - Sres.m_data(2) - Sres.m_data(5);
442
443			// Sres + D + (Ev)x ( 9a)
444	✓✗✓✗	252724	Sres.m_data(0) += m_data(5);
445	✓✗✓✗ ✓✗✓✗	252724	Sres.m_data(1) += r(2); Sres.m_data(2)+= m_data(5);
446	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	252724	Sres.m_data(3) +=-r(1); Sres.m_data(4)+= r(0); Sres.m_data(5) += m_data(5);
447
448		505448	return Sres;
449			}
450
451			/// \returns An expression of *this with the Scalar type casted to NewScalar.
452			template<typename NewScalar>
453		1185	Symmetric3Tpl<NewScalar,Options> cast() const
454			{
455		1185	return Symmetric3Tpl<NewScalar,Options>(m_data.template cast<NewScalar>());
456			}
457
458			// TODO: adjust code
459			// bool isValid() const
460			// {
461			// return
462			// m_data(0) >= Scalar(0)
463			// && m_data(2) >= Scalar(0)
464			// && m_data(5) >= Scalar(0);
465			// }
466
467			protected:
468			Vector6 m_data;
469
470			};
471
472			} // namespace pinocchio
473
474			#endif // ifndef __pinocchio_symmetric3_hpp__
475