MassMatrix3.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 #ifndef _IGNITION_MASSMATRIX3_HH_
18 #define _IGNITION_MASSMATRIX3_HH_
19 
20 #include <algorithm>
21 #include <string>
22 #include <vector>
23 
24 #include "ignition/math/Helpers.hh"
26 #include "ignition/math/Vector2.hh"
27 #include "ignition/math/Vector3.hh"
28 #include "ignition/math/Matrix3.hh"
29 
30 namespace ignition
31 {
32  namespace math
33  {
38  template<typename T>
40  {
42  public: MassMatrix3() : mass(0)
43  {}
44 
49  public: MassMatrix3(const T &_mass,
50  const Vector3<T> &_ixxyyzz,
51  const Vector3<T> &_ixyxzyz )
52  : mass(_mass), Ixxyyzz(_ixxyyzz), Ixyxzyz(_ixyxzyz)
53  {}
54 
57  public: MassMatrix3(const MassMatrix3<T> &_m)
58  : mass(_m.Mass()), Ixxyyzz(_m.DiagonalMoments()),
59  Ixyxzyz(_m.OffDiagonalMoments())
60  {}
61 
63  public: virtual ~MassMatrix3() {}
64 
68  public: bool Mass(const T &_m)
69  {
70  this->mass = _m;
71  return this->IsValid();
72  }
73 
76  public: T Mass() const
77  {
78  return this->mass;
79  }
80 
89  public: bool InertiaMatrix(const T &_ixx, const T &_iyy, const T &_izz,
90  const T &_ixy, const T &_ixz, const T &_iyz)
91  {
92  this->Ixxyyzz.Set(_ixx, _iyy, _izz);
93  this->Ixyxzyz.Set(_ixy, _ixz, _iyz);
94  return this->IsValid();
95  }
96 
99  public: Vector3<T> DiagonalMoments() const
100  {
101  return this->Ixxyyzz;
102  }
103 
107  {
108  return this->Ixyxzyz;
109  }
110 
114  public: bool DiagonalMoments(const Vector3<T> &_ixxyyzz)
115  {
116  this->Ixxyyzz = _ixxyyzz;
117  return this->IsValid();
118  }
119 
123  public: bool OffDiagonalMoments(const Vector3<T> &_ixyxzyz)
124  {
125  this->Ixyxzyz = _ixyxzyz;
126  return this->IsValid();
127  }
128 
131  public: T IXX() const
132  {
133  return this->Ixxyyzz[0];
134  }
135 
138  public: T IYY() const
139  {
140  return this->Ixxyyzz[1];
141  }
142 
145  public: T IZZ() const
146  {
147  return this->Ixxyyzz[2];
148  }
149 
152  public: T IXY() const
153  {
154  return this->Ixyxzyz[0];
155  }
156 
159  public: T IXZ() const
160  {
161  return this->Ixyxzyz[1];
162  }
163 
166  public: T IYZ() const
167  {
168  return this->Ixyxzyz[2];
169  }
170 
174  public: bool IXX(const T &_v)
175  {
176  this->Ixxyyzz.X(_v);
177  return this->IsValid();
178  }
179 
183  public: bool IYY(const T &_v)
184  {
185  this->Ixxyyzz.Y(_v);
186  return this->IsValid();
187  }
188 
192  public: bool IZZ(const T &_v)
193  {
194  this->Ixxyyzz.Z(_v);
195  return this->IsValid();
196  }
197 
201  public: bool IXY(const T &_v)
202  {
203  this->Ixyxzyz.X(_v);
204  return this->IsValid();
205  }
206 
210  public: bool IXZ(const T &_v)
211  {
212  this->Ixyxzyz.Y(_v);
213  return this->IsValid();
214  }
215 
219  public: bool IYZ(const T &_v)
220  {
221  this->Ixyxzyz.Z(_v);
222  return this->IsValid();
223  }
224 
227  public: Matrix3<T> MOI() const
228  {
229  return Matrix3<T>(
230  this->Ixxyyzz[0], this->Ixyxzyz[0], this->Ixyxzyz[1],
231  this->Ixyxzyz[0], this->Ixxyyzz[1], this->Ixyxzyz[2],
232  this->Ixyxzyz[1], this->Ixyxzyz[2], this->Ixxyyzz[2]);
233  }
234 
240  public: bool MOI(const Matrix3<T> &_moi)
241  {
242  this->Ixxyyzz.Set(_moi(0, 0), _moi(1, 1), _moi(2, 2));
243  this->Ixyxzyz.Set(
244  0.5*(_moi(0, 1) + _moi(1, 0)),
245  0.5*(_moi(0, 2) + _moi(2, 0)),
246  0.5*(_moi(1, 2) + _moi(2, 1)));
247  return this->IsValid();
248  }
249 
253  public: MassMatrix3 &operator=(const MassMatrix3<T> &_massMatrix)
254  {
255  this->mass = _massMatrix.Mass();
256  this->Ixxyyzz = _massMatrix.DiagonalMoments();
257  this->Ixyxzyz = _massMatrix.OffDiagonalMoments();
258 
259  return *this;
260  }
261 
266  public: bool operator==(const MassMatrix3<T> &_m) const
267  {
268  return equal<T>(this->mass, _m.Mass()) &&
269  (this->Ixxyyzz == _m.DiagonalMoments()) &&
270  (this->Ixyxzyz == _m.OffDiagonalMoments());
271  }
272 
276  public: bool operator!=(const MassMatrix3<T> &_m) const
277  {
278  return !(*this == _m);
279  }
280 
284  public: bool IsPositive() const
285  {
286  // Check if mass and determinants of all upper left submatrices
287  // of moment of inertia matrix are positive
288  return (this->mass > 0) &&
289  (this->IXX() > 0) &&
290  (this->IXX()*this->IYY() - std::pow(this->IXY(), 2) > 0) &&
291  (this->MOI().Determinant() > 0);
292  }
293 
298  public: bool IsValid() const
299  {
300  return this->IsPositive() && ValidMoments(this->PrincipalMoments());
301  }
302 
308  public: static bool ValidMoments(const Vector3<T> &_moments)
309  {
310  return _moments[0] > 0 &&
311  _moments[1] > 0 &&
312  _moments[2] > 0 &&
313  _moments[0] + _moments[1] > _moments[2] &&
314  _moments[1] + _moments[2] > _moments[0] &&
315  _moments[2] + _moments[0] > _moments[1];
316  }
317 
324  public: Vector3<T> PrincipalMoments(const T _tol = 1e-6) const
325  {
326  // Compute tolerance relative to maximum value of inertia diagonal
327  T tol = _tol * this->Ixxyyzz.Max();
328  if (this->Ixyxzyz.Equal(Vector3<T>::Zero, tol))
329  {
330  // Matrix is already diagonalized, return diagonal moments
331  return this->Ixxyyzz;
332  }
333 
334  // Algorithm based on http://arxiv.org/abs/1306.6291v4
335  // A Method for Fast Diagonalization of a 2x2 or 3x3 Real Symmetric
336  // Matrix, by Maarten Kronenburg
337  Vector3<T> Id(this->Ixxyyzz);
338  Vector3<T> Ip(this->Ixyxzyz);
339  // b = Ixx + Iyy + Izz
340  T b = Id.Sum();
341  // c = Ixx*Iyy - Ixy^2 + Ixx*Izz - Ixz^2 + Iyy*Izz - Iyz^2
342  T c = Id[0]*Id[1] - std::pow(Ip[0], 2)
343  + Id[0]*Id[2] - std::pow(Ip[1], 2)
344  + Id[1]*Id[2] - std::pow(Ip[2], 2);
345  // d = Ixx*Iyz^2 + Iyy*Ixz^2 + Izz*Ixy^2 - Ixx*Iyy*Izz - 2*Ixy*Ixz*Iyz
346  T d = Id[0]*std::pow(Ip[2], 2)
347  + Id[1]*std::pow(Ip[1], 2)
348  + Id[2]*std::pow(Ip[0], 2)
349  - Id[0]*Id[1]*Id[2]
350  - 2*Ip[0]*Ip[1]*Ip[2];
351  // p = b^2 - 3c
352  T p = std::pow(b, 2) - 3*c;
353 
354  // At this point, it is important to check that p is not close
355  // to zero, since its inverse is used to compute delta.
356  // In equation 4.7, p is expressed as a sum of squares
357  // that is only zero if the matrix is diagonal
358  // with identical principal moments.
359  // This check has no test coverage, since this function returns
360  // immediately if a diagonal matrix is detected.
361  if (p < std::pow(tol, 2))
362  return b / 3.0 * Vector3<T>::One;
363 
364  // q = 2b^3 - 9bc - 27d
365  T q = 2*std::pow(b, 3) - 9*b*c - 27*d;
366 
367  // delta = acos(q / (2 * p^(1.5)))
368  // additionally clamp the argument to [-1,1]
369  T delta = acos(clamp<T>(0.5 * q / std::pow(p, 1.5), -1, 1));
370 
371  // sort the moments from smallest to largest
372  T moment0 = (b + 2*sqrt(p) * cos(delta / 3.0)) / 3.0;
373  T moment1 = (b + 2*sqrt(p) * cos((delta + 2*M_PI)/3.0)) / 3.0;
374  T moment2 = (b + 2*sqrt(p) * cos((delta - 2*M_PI)/3.0)) / 3.0;
375  sort3(moment0, moment1, moment2);
376  return Vector3<T>(moment0, moment1, moment2);
377  }
378 
380  private: T mass;
381 
385  private: Vector3<T> Ixxyyzz;
386 
390  private: Vector3<T> Ixyxzyz;
391  };
392 
395  }
396 }
397 #endif
bool operator==(const MassMatrix3< T > &_m) const
Equality comparison operator.
Definition: MassMatrix3.hh:266
T IXY() const
Get IXY.
Definition: MassMatrix3.hh:152
T Mass() const
Get the mass.
Definition: MassMatrix3.hh:76
bool IYZ(const T &_v)
Set IYZ.
Definition: MassMatrix3.hh:219
static bool ValidMoments(const Vector3< T > &_moments)
Verify that principal moments are positive and satisfy the triangle inequality.
Definition: MassMatrix3.hh:308
MassMatrix3()
Default Constructor.
Definition: MassMatrix3.hh:42
bool operator!=(const MassMatrix3< T > &_m) const
Inequality test operator.
Definition: MassMatrix3.hh:276
T Sum() const
Return the sum of the values.
Definition: Vector3.hh:87
T IXZ() const
Get IXZ.
Definition: MassMatrix3.hh:159
MassMatrix3(const MassMatrix3< T > &_m)
Copy constructor.
Definition: MassMatrix3.hh:57
bool OffDiagonalMoments(const Vector3< T > &_ixyxzyz)
Set the off-diagonal moments of inertia (Ixy, Ixz, Iyz).
Definition: MassMatrix3.hh:123
bool IZZ(const T &_v)
Set IZZ.
Definition: MassMatrix3.hh:192
A class for inertial information about a rigid body consisting of the scalar mass and a 3x3 symmetric...
Definition: MassMatrix3.hh:39
MassMatrix3 & operator=(const MassMatrix3< T > &_massMatrix)
Equal operator.
Definition: MassMatrix3.hh:253
Vector3< T > OffDiagonalMoments() const
Get the off-diagonal moments of inertia (Ixy, Ixz, Iyz).
Definition: MassMatrix3.hh:106
Vector3< T > DiagonalMoments() const
Get the diagonal moments of inertia (Ixx, Iyy, Izz).
Definition: MassMatrix3.hh:99
bool IsPositive() const
Verify that inertia values are positive definite.
Definition: MassMatrix3.hh:284
bool DiagonalMoments(const Vector3< T > &_ixxyyzz)
Set the diagonal moments of inertia (Ixx, Iyy, Izz).
Definition: MassMatrix3.hh:114
Vector3< T > PrincipalMoments(const T _tol=1e-6) const
Compute principal moments of inertia, which are the eigenvalues of the moment of inertia matrix...
Definition: MassMatrix3.hh:324
bool InertiaMatrix(const T &_ixx, const T &_iyy, const T &_izz, const T &_ixy, const T &_ixz, const T &_iyz)
Set the moment of inertia matrix.
Definition: MassMatrix3.hh:89
A 3x3 matrix class.
Definition: Matrix3.hh:33
Matrix3< T > MOI() const
returns Moments of Inertia as a Matrix3
Definition: MassMatrix3.hh:227
T IZZ() const
Get IZZ.
Definition: MassMatrix3.hh:145
bool IXY(const T &_v)
Set IXY.
Definition: MassMatrix3.hh:201
bool IYY(const T &_v)
Set IYY.
Definition: MassMatrix3.hh:183
T IYZ() const
Get IYZ.
Definition: MassMatrix3.hh:166
bool IXZ(const T &_v)
Set IXZ.
Definition: MassMatrix3.hh:210
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:37
MassMatrix3< double > MassMatrix3d
Definition: MassMatrix3.hh:393
virtual ~MassMatrix3()
Destructor.
Definition: MassMatrix3.hh:63
bool IsValid() const
Verify that inertia values are positive definite and satisfy the triangle inequality.
Definition: MassMatrix3.hh:298
MassMatrix3(const T &_mass, const Vector3< T > &_ixxyyzz, const Vector3< T > &_ixyxzyz)
Constructor.
Definition: MassMatrix3.hh:49
bool MOI(const Matrix3< T > &_moi)
Sets Moments of Inertia (MOI) from a Matrix3.
Definition: MassMatrix3.hh:240
MassMatrix3< float > MassMatrix3f
Definition: MassMatrix3.hh:394
T IXX() const
Get IXX.
Definition: MassMatrix3.hh:131
bool IXX(const T &_v)
Set IXX.
Definition: MassMatrix3.hh:174
Definition: AffineException.hh:30
T IYY() const
Get IYY.
Definition: MassMatrix3.hh:138
bool Mass(const T &_m)
Set the mass.
Definition: MassMatrix3.hh:68
void sort3(T &_a, T &_b, T &_c)
Sort three numbers, such that _a <= _b <= _c.
Definition: Helpers.hh:291