GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/pinocchio/spatial/symmetric3.hpp Lines: 194 204 95.1 %
Date: 2024-04-26 13:14:21 Branches: 143 294 48.6 %

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
252713
    Symmetric3Tpl(): m_data() {}
30
31
    template<typename Sc,int Opt>
32
1590
    explicit Symmetric3Tpl(const Eigen::Matrix<Sc,3,3,Opt> & I)
33
1590
    {
34

1590
      assert( (I-I.transpose()).isMuchSmallerThan(I) );
35
1590
      m_data(0) = I(0,0);
36
1590
      m_data(1) = I(1,0); m_data(2) = I(1,1);
37
1590
      m_data(3) = I(2,0); m_data(4) = I(2,1); m_data(5) = I(2,2);
38
1590
    }
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
7991
    bool operator==(const Symmetric3Tpl & other) const
68
7991
    { 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
16
    bool isZero(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
78
16
    { 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
51374
      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( -y*y-z*z,
90
                             x*y    ,  -x*x-z*z,
91
62
                             x*z    ,   y*z    ,  -x*x-y*y );
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]+y*y+z*z,
99

1
                           m_data[1]-x*y,m_data[2]+x*x+z*z,
100


3
                           m_data[3]-x*z,m_data[4]-y*z,m_data[5]+x*x+y*y);
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]+=y*y+z*z;
107
1
      m_data[1]-=x*y; m_data[2]+=x*x+z*z;
108
1
      m_data[3]-=x*z; m_data[4]-=y*z; m_data[5]+=x*x+y*y;
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
51341
      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*(y*y+z*z),
134
                             m* x*y,-m*(x*x+z*z),
135
264
                             m* x*z,m* y*z,-m*(x*x+y*y));
136
      }
137
    };
138
139
51341
    friend AlphaSkewSquare operator* (const Scalar & m, const SkewSquare & sk)
140
51341
    { 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*(y*y+z*z),
146

19380
                           m_data[1]-v.m* x*y, m_data[2]+v.m*(x*x+z*z),
147

19380
                           m_data[3]-v.m* x*z, m_data[4]-v.m* y*z,
148

58140
                           m_data[5]+v.m*(x*x+y*y));
149
    }
150
151
51335
    Symmetric3Tpl& operator-= (const AlphaSkewSquare & v)
152
    {
153
51335
      const Scalar & x = v.v[0], & y = v.v[1], & z = v.v[2];
154
51335
      m_data[0]+=v.m*(y*y+z*z);
155
51335
      m_data[1]-=v.m* x*y; m_data[2]+=v.m*(x*x+z*z);
156
51335
      m_data[3]-=v.m* x*z; m_data[4]-=v.m* y*z; m_data[5]+=v.m*(x*x+y*y);
157
51335
      return *this;
158
    }
159
160
4846
    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(-y*y-z*z,
167
    // 			    x*y, -x*x-z*z,
168
    // 			    x*z, y*z, -x*x-y*y );
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(a*a+b*b+d*d,
182
			   a*b+b*c+d*e, b*b+c*c+e*e,
183
12022
			   a*d+b*e+d*f, b*d+c*e+e*f,  d*d+e*e+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
51336
    Symmetric3Tpl & operator+=(const Symmetric3Tpl & s2)
338
    {
339
51336
      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 R*S*R' and R'*S*R products --- */
399
  public: //private:
400
401
    /** \brief Computes L for a symmetric matrix A.
402
     */
403
151644
    Matrix32  decomposeltI() const
404
    {
405
151644
      Matrix32 L;
406
      L <<
407


303288
      m_data(0) - m_data(5),    m_data(1),
408


303288
      m_data(1),              m_data(2) - m_data(5),
409


151644
      2*m_data(3),            m_data(4) + m_data(4);
410
151644
      return L;
411
    }
412
413
    /* R*S*R' */
414
    template<typename D>
415
252735
    Symmetric3Tpl rotate(const Eigen::MatrixBase<D> & R) const
416
    {
417
      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(D,3,3);
418


252735
      assert(isUnitary(R.transpose()*R) && "R is not a Unitary matrix");
419
420
252735
      Symmetric3Tpl Sres;
421
422
      // 4 a
423
252735
      const Matrix32 L( decomposeltI() );
424
425
      // Y = R' L   ===> (12 m + 8 a)
426

252735
      const Matrix2 Y( R.template block<2,3>(1,0) * L );
427
428
      // Sres= Y R  ===> (16 m + 8a)
429


252735
      Sres.m_data(1) = Y(0,0)*R(0,0) + Y(0,1)*R(0,1);
430


252735
      Sres.m_data(2) = Y(0,0)*R(1,0) + Y(0,1)*R(1,1);
431


252735
      Sres.m_data(3) = Y(1,0)*R(0,0) + Y(1,1)*R(0,1);
432


252735
      Sres.m_data(4) = Y(1,0)*R(1,0) + Y(1,1)*R(1,1);
433


252735
      Sres.m_data(5) = Y(1,0)*R(2,0) + Y(1,1)*R(2,1);
434
435
      // r=R' v ( 6m + 3a)
436


252735
      const Vector3 r(-R(0,0)*m_data(4) + R(0,1)*m_data(3),
437


252735
                      -R(1,0)*m_data(4) + R(1,1)*m_data(3),
438


252735
                      -R(2,0)*m_data(4) + R(2,1)*m_data(3));
439
440
      // Sres_11 (3a)
441


252735
      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

252735
      Sres.m_data(0) += m_data(5);
445


252735
      Sres.m_data(1) += r(2); Sres.m_data(2)+= m_data(5);
446



252735
      Sres.m_data(3) +=-r(1); Sres.m_data(4)+= r(0); Sres.m_data(5) += m_data(5);
447
448
505470
      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