symmetric3.hpp 14.8 KB
Newer Older
jcarpent's avatar
jcarpent committed
1
//
2
// Copyright (c) 2014-2019 CNRS INRIA
jcarpent's avatar
jcarpent committed
3
//
4

5
6
#ifndef __pinocchio_symmetric3_hpp__
#define __pinocchio_symmetric3_hpp__
7

8
#include "pinocchio/macros.hpp"
9
#include "pinocchio/math/matrix.hpp"
10

11
namespace pinocchio
12
13
14
15
16
17
18
19
20
21
22
23
24
{

  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;
25
26
    
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
27
28

  public:    
29
    Symmetric3Tpl(): m_data() {}
30

31
32
    template<typename Sc,int Opt>
    explicit Symmetric3Tpl(const Eigen::Matrix<Sc,3,3,Opt> & I)
33
34
    {
      assert( (I-I.transpose()).isMuchSmallerThan(I) );
35
36
37
      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);
38
    }
39
    
40
    explicit Symmetric3Tpl(const Vector6 & I) : m_data(I) {}
41
42
43
    
    Symmetric3Tpl(const Scalar & a0, const Scalar & a1, const Scalar & a2,
		  const Scalar & a3, const Scalar & a4, const Scalar & a5)
44
    { m_data << a0,a1,a2,a3,a4,a5; }
45

46
    static Symmetric3Tpl Zero()     { return Symmetric3Tpl(Vector6::Zero());  }
47
    void setZero() { m_data.setZero(); }
48
    
49
50
    static Symmetric3Tpl Random()   { return RandomPositive();  }
    void setRandom()
51
    {
52
53
54
55
56
57
58
      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;
59
      
60
      m_data << a, b, c, d, e, f;
61
62
    }
    
63
    static Symmetric3Tpl Identity() { return Symmetric3Tpl(1, 0, 1, 0, 0, 1);  }
64
    void setIdentity() { m_data << 1, 0, 1, 0, 0, 1; }
65

Sébastien Barthélémy's avatar
Sébastien Barthélémy committed
66
    /* Required by Inertia::operator== */
67
68
69
70
71
    bool operator==(const Symmetric3Tpl & other) const
    { return m_data == other.m_data; }
    
    bool operator!=(const Symmetric3Tpl & other) const
    { return !(*this == other); }
72
    
73
74
    bool isApprox(const Symmetric3Tpl & other,
                  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
75
    { return m_data.isApprox(other.m_data,prec); }
76
    
Justin Carpentier's avatar
Justin Carpentier committed
77
78
79
    bool isZero(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
    { return m_data.isZero(prec); }
    
80
    void fill(const Scalar value) { m_data.fill(value); }
81
    
82
83
84
85
86
87
    struct SkewSquare
    {
      const Vector3 & v;
      SkewSquare( const Vector3 & v ) : v(v) {}
      operator Symmetric3Tpl () const 
      {
88
89
90
91
        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 );
92
      }
93
94
    }; // struct SkewSquare
    
95
96
    Symmetric3Tpl operator- (const SkewSquare & v) const
    {
97
      const Scalar & x = v.v[0], & y = v.v[1], & z = v.v[2];
98
99
100
      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);
101
    }
102
    
103
104
    Symmetric3Tpl& operator-= (const SkewSquare & v)
    {
105
      const Scalar & x = v.v[0], & y = v.v[1], & z = v.v[2];
106
107
108
      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;
109
      return *this;
110
    }
jcarpent's avatar
jcarpent committed
111
112
113
114
115
116
117
118
119
120
    
    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;
    }
121
122
123

    struct AlphaSkewSquare
    {
124
125
126
127
      const Scalar & m;
      const Vector3 & v;
      
      AlphaSkewSquare(const Scalar & m, const SkewSquare & v) : m(m),v(v.v) {}
128
129
      AlphaSkewSquare(const Scalar & m, const Vector3 & v) : m(m),v(v) {}
      
130
131
      operator Symmetric3Tpl () const 
      {
132
133
134
135
        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));
136
137
      }
    };
138
139
    
    friend AlphaSkewSquare operator* (const Scalar & m, const SkewSquare & sk)
140
    { return AlphaSkewSquare(m,sk); }
141
    
142
143
    Symmetric3Tpl operator- (const AlphaSkewSquare & v) const
    {
144
      const Scalar & x = v.v[0], & y = v.v[1], & z = v.v[2];
145
146
147
148
      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));
149
    }
150
    
151
152
    Symmetric3Tpl& operator-= (const AlphaSkewSquare & v)
    {
153
      const Scalar & x = v.v[0], & y = v.v[1], & z = v.v[2];
154
155
156
      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);
157
158
159
      return *this;
    }

160
161
    const Vector6 & data () const {return m_data;}
    Vector6 & data () {return m_data;}
162
    
163
164
    // static Symmetric3Tpl SkewSq( const Vector3 & v )
    // { 
165
166
167
168
    //   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 );
169
170
    // }

171
172
173
    /* Shoot a positive definite matrix. */
    static Symmetric3Tpl RandomPositive() 
    { 
174
175
176
177
178
179
180
      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;
181
182
183
184
      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 );
    }
185
    
186
187
188
    Matrix3 matrix() const
    {
      Matrix3 res;
189
190
191
      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);
192
193
194
      return res;
    }
    operator Matrix3 () const { return matrix(); }
jcarpent's avatar
jcarpent committed
195
196
197
198
199
200
201
202
203
204
205
206
207
208
    
    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;
      
209
      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);
jcarpent's avatar
jcarpent committed
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
217
218
219
220
221
222
    ///
    /// \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>
223
224
225
    static void vxs(const Eigen::MatrixBase<Vector3> & v,
                    const Symmetric3Tpl & S3,
                    const Eigen::MatrixBase<Matrix3> & M)
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
    {
      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];
      
242
      Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,M);
243
244
245
246
247
248
249
250
251
252
253
254
255
256
      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;
    }
    
    ///
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
261
262
263
    ///
    /// \tparam Vector3
    ///
    /// \param[in]  v  a vector of dimension 3.
    ///
264
    /// \returns the result \f$ [v]_{\times} S \f$.
265
266
    ///
    template<typename Vector3>
267
    Matrix3 vxs(const Eigen::MatrixBase<Vector3> & v) const
268
269
    {
      Matrix3 M;
270
      vxs(v,*this,M);
271
272
      return M;
    }
273
274
    
    ///
Gabriele Buondonno's avatar
Gabriele Buondonno committed
275
    /// \brief Performs the operation \f$ M = S_{3} [v]_{\times} \f$.
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
    ///
    /// \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];
      
302
      Matrix3 & M_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,M);
303
304
305
306
307
308
309
310
311
312
313
314
315
      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;
    }
    
Gabriele Buondonno's avatar
Gabriele Buondonno committed
316
    /// \brief Performs the operation \f$ M = S_{3} [v]_{\times} \f$.
317
318
319
320
321
    ///
    /// \tparam Vector3
    ///
    /// \param[in]  v  a vector of dimension 3.
    ///
322
    /// \returns the result \f$ S [v]_{\times} \f$.
323
324
325
326
327
328
329
330
    ///
    template<typename Vector3>
    Matrix3 svx(const Eigen::MatrixBase<Vector3> & v) const
    {
      Matrix3 M;
      svx(v,*this,M);
      return M;
    }
331
332
333

    Symmetric3Tpl operator+(const Symmetric3Tpl & s2) const
    {
334
      return Symmetric3Tpl((m_data+s2.m_data).eval());
335
336
337
338
    }

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

342
343
344
345
    template<typename V3in, typename V3out>
    static void rhsMult(const Symmetric3Tpl & S3,
                        const Eigen::MatrixBase<V3in> & vin,
                        const Eigen::MatrixBase<V3out> & vout)
346
    {
347
348
349
      EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(V3in,Vector3);
      EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(V3out,Vector3);
      
350
      V3out & vout_ = PINOCCHIO_EIGEN_CONST_CAST(V3out,vout);
351
      
352
353
354
      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];
355
356
    }

357
358
359
360
361
362
363
364
    template<typename V3>
    Vector3 operator*(const Eigen::MatrixBase<V3> & v) const
    {
      Vector3 res;
      rhsMult(*this,v,res);
      return res;
    }
    
365
366
367
368
369
    // Matrix3 operator*(const Matrix3 &a) const
    // {
    //   Matrix3 r;
    //   for(unsigned int i=0; i<3; ++i)
    //     {
370
371
372
    //       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);
373
374
375
376
377
378
    //     }
    //   return r;
    // }

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

    Symmetric3Tpl operator-(const Matrix3 &S) const
    {
      assert( (S-S.transpose()).isMuchSmallerThan(S) );
385
386
387
      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) );
388
389
390
391
392
    }

    Symmetric3Tpl operator+(const Matrix3 &S) const
    {
      assert( (S-S.transpose()).isMuchSmallerThan(S) );
393
394
395
      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) );
396
397
398
399
400
401
402
403
404
405
406
    }

    /* --- 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 << 
407
408
409
      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);
410
411
412
413
414
      return L;
    }

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

420
421
422
      Symmetric3Tpl Sres;
      
      // 4 a
423
      const Matrix32 L( decomposeltI() );
424
425
      
      // Y = R' L   ===> (12 m + 8 a)
426
      const Matrix2 Y( R.template block<2,3>(1,0) * L );
427
	
428
      // Sres= Y R  ===> (16 m + 8a)
429
430
431
432
433
      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);
434
435

      // r=R' v ( 6m + 3a)
436
437
438
      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));
439
440

      // Sres_11 (3a)
441
      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
445
446
      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);
447

448
      return Sres;
449
    }
450
451
452
453
454
455
456
    
    /// \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>());
    }
457
    
458
459
460
461
462
463
464
465
    // TODO: adjust code
//    bool isValid() const
//    {
//      return
//         m_data(0) >= Scalar(0)
//      && m_data(2) >= Scalar(0)
//      && m_data(5) >= Scalar(0);
//    }
466

jcarpent's avatar
jcarpent committed
467
  protected:
468
    Vector6 m_data;
jcarpent's avatar
jcarpent committed
469
    
470
471
  };

472
} // namespace pinocchio
473

474
#endif // ifndef __pinocchio_symmetric3_hpp__
475