MassMatrix3.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015-2016 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_MATH_MASSMATRIX3_HH_
18 #define IGNITION_MATH_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 
329  public: Vector3<T> PrincipalMoments(const T _tol = 1e-6) const
330  {
331  // Compute tolerance relative to maximum value of inertia diagonal
332  T tol = _tol * this->Ixxyyzz.Max();
333  if (this->Ixyxzyz.Equal(Vector3<T>::Zero, tol))
334  {
335  // Matrix is already diagonalized, return diagonal moments
336  return this->Ixxyyzz;
337  }
338 
339  // Algorithm based on http://arxiv.org/abs/1306.6291v4
340  // A Method for Fast Diagonalization of a 2x2 or 3x3 Real Symmetric
341  // Matrix, by Maarten Kronenburg
342  Vector3<T> Id(this->Ixxyyzz);
343  Vector3<T> Ip(this->Ixyxzyz);
344  // b = Ixx + Iyy + Izz
345  T b = Id.Sum();
346  // c = Ixx*Iyy - Ixy^2 + Ixx*Izz - Ixz^2 + Iyy*Izz - Iyz^2
347  T c = Id[0]*Id[1] - std::pow(Ip[0], 2)
348  + Id[0]*Id[2] - std::pow(Ip[1], 2)
349  + Id[1]*Id[2] - std::pow(Ip[2], 2);
350  // d = Ixx*Iyz^2 + Iyy*Ixz^2 + Izz*Ixy^2 - Ixx*Iyy*Izz - 2*Ixy*Ixz*Iyz
351  T d = Id[0]*std::pow(Ip[2], 2)
352  + Id[1]*std::pow(Ip[1], 2)
353  + Id[2]*std::pow(Ip[0], 2)
354  - Id[0]*Id[1]*Id[2]
355  - 2*Ip[0]*Ip[1]*Ip[2];
356  // p = b^2 - 3c
357  T p = std::pow(b, 2) - 3*c;
358 
359  // At this point, it is important to check that p is not close
360  // to zero, since its inverse is used to compute delta.
361  // In equation 4.7, p is expressed as a sum of squares
362  // that is only zero if the matrix is diagonal
363  // with identical principal moments.
364  // This check has no test coverage, since this function returns
365  // immediately if a diagonal matrix is detected.
366  if (p < std::pow(tol, 2))
367  return b / 3.0 * Vector3<T>::One;
368 
369  // q = 2b^3 - 9bc - 27d
370  T q = 2*std::pow(b, 3) - 9*b*c - 27*d;
371 
372  // delta = acos(q / (2 * p^(1.5)))
373  // additionally clamp the argument to [-1,1]
374  T delta = acos(clamp<T>(0.5 * q / std::pow(p, 1.5), -1, 1));
375 
376  // sort the moments from smallest to largest
377  T moment0 = (b + 2*sqrt(p) * cos(delta / 3.0)) / 3.0;
378  T moment1 = (b + 2*sqrt(p) * cos((delta + 2*IGN_PI)/3.0)) / 3.0;
379  T moment2 = (b + 2*sqrt(p) * cos((delta - 2*IGN_PI)/3.0)) / 3.0;
380  sort3(moment0, moment1, moment2);
381  return Vector3<T>(moment0, moment1, moment2);
382  }
383 
395  public: Quaternion<T> PrincipalAxesOffset(const T _tol = 1e-6) const
396  {
397  // Compute tolerance relative to maximum value of inertia diagonal
398  T tol = _tol * this->Ixxyyzz.Max();
399  Vector3<T> moments = this->PrincipalMoments(tol);
400  if (moments.Equal(this->Ixxyyzz, tol) ||
401  (math::equal<T>(moments[0], moments[1], std::abs(tol)) &&
402  math::equal<T>(moments[0], moments[2], std::abs(tol))))
403  {
404  // matrix is already aligned with principal axes
405  // or all three moments are approximately equal
406  // return identity rotation
408  }
409 
410  // Algorithm based on http://arxiv.org/abs/1306.6291v4
411  // A Method for Fast Diagonalization of a 2x2 or 3x3 Real Symmetric
412  // Matrix, by Maarten Kronenburg
413  // A real, symmetric matrix can be diagonalized by an orthogonal matrix
414  // (due to the finite-dimensional spectral theorem
415  // https://en.wikipedia.org/wiki/Spectral_theorem
416  // #Hermitian_maps_and_Hermitian_matrices ),
417  // and another name for orthogonal matrix is rotation matrix.
418  // Section 5 of the paper shows how to compute Euler angles
419  // phi1, phi2, and phi3 that map to a rotation matrix.
420  // In some cases, there are multiple possible values for a given angle,
421  // such as phi1, that are denoted as phi11, phi12, phi11a, phi12a, etc.
422  // Similar variable names are used to the paper so that the paper
423  // can be used as an additional reference.
424 
425  // f1, f2 defined in equations 5.5, 5.6
426  Vector2<T> f1(this->Ixyxzyz[0], -this->Ixyxzyz[1]);
427  Vector2<T> f2(this->Ixxyyzz[1] - this->Ixxyyzz[2],
428  -2*this->Ixyxzyz[2]);
429 
430  // Check if two moments are equal, since different equations are used
431  // The moments vector is already sorted, so just check adjacent values.
432  Vector2<T> momentsDiff(moments[0] - moments[1],
433  moments[1] - moments[2]);
434 
435  // index of unequal moment
436  int unequalMoment = -1;
437  if (equal<T>(momentsDiff[0], 0, std::abs(tol)))
438  unequalMoment = 2;
439  else if (equal<T>(momentsDiff[1], 0, std::abs(tol)))
440  unequalMoment = 0;
441 
442  if (unequalMoment >= 0)
443  {
444  // moments[1] is the repeated value
445  // it is not equal to moments[unequalMoment]
446  // momentsDiff3 = lambda - lambda3
447  T momentsDiff3 = moments[1] - moments[unequalMoment];
448  // eq 5.21:
449  // s = cos(phi2)^2 = (A_11 - lambda3) / (lambda - lambda3)
450  // s >= 0 since A_11 is in range [lambda, lambda3]
451  T s = (this->Ixxyyzz[0] - moments[unequalMoment]) / momentsDiff3;
452  // set phi3 to zero for repeated moments (eq 5.23)
453  T phi3 = 0;
454  // phi2 = +- acos(sqrt(s))
455  // start with just the positive value
456  // also clamp the acos argument to prevent NaN's
457  T phi2 = acos(clamp<T>(ClampedSqrt(s), -1, 1));
458 
459  // The paper defines variables phi11 and phi12
460  // which are candidate values of angle phi1.
461  // phi12 is straightforward to compute as a function of f2 and g2.
462  // eq 5.25:
463  Vector2<T> g2(momentsDiff3 * s, 0);
464  // combining eq 5.12 and 5.14, and subtracting psi2
465  // instead of multiplying by its rotation matrix:
466  math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
467  phi12.Normalize();
468 
469  // The paragraph prior to equation 5.16 describes how to choose
470  // the candidate value of phi1 based on the length
471  // of the f1 and f2 vectors.
472  // * When |f1| != 0 and |f2| != 0, then one should choose the
473  // value of phi2 so that phi11 = phi12
474  // * When |f1| == 0 and f2 != 0, then phi1 = phi12
475  // phi11 can be ignored, and either sign of phi2 can be used
476  // * The case of |f2| == 0 can be ignored at this point in the code
477  // since having a repeated moment when |f2| == 0 implies that
478  // the matrix is diagonal. But this function returns a unit
479  // quaternion for diagonal matrices, so we can assume |f2| != 0
480  // See MassMatrix3.ipynb for a more complete discussion.
481  //
482  // Since |f2| != 0, we only need to consider |f1|
483  // * |f1| == 0: phi1 = phi12
484  // * |f1| != 0: choose phi2 so that phi11 == phi12
485  // In either case, phi1 = phi12,
486  // and the sign of phi2 must be chosen to make phi11 == phi12
487  T phi1 = phi12.Radian();
488 
489  bool f1small = f1.SquaredLength() < std::pow(tol, 2);
490  if (!f1small)
491  {
492  // a: phi2 > 0
493  // eq. 5.24
494  Vector2<T> g1a(0, 0.5*momentsDiff3 * sin(2*phi2));
495  // combining eq 5.11 and 5.13, and subtracting psi1
496  // instead of multiplying by its rotation matrix:
497  math::Angle phi11a(Angle2(g1a, tol) - Angle2(f1, tol));
498  phi11a.Normalize();
499 
500  // b: phi2 < 0
501  // eq. 5.24
502  Vector2<T> g1b(0, 0.5*momentsDiff3 * sin(-2*phi2));
503  // combining eq 5.11 and 5.13, and subtracting psi1
504  // instead of multiplying by its rotation matrix:
505  math::Angle phi11b(Angle2(g1b, tol) - Angle2(f1, tol));
506  phi11b.Normalize();
507 
508  // choose sign of phi2
509  // based on whether phi11a or phi11b is closer to phi12
510  // use sin and cos to account for angle wrapping
511  T erra = std::pow(sin(phi1) - sin(phi11a.Radian()), 2)
512  + std::pow(cos(phi1) - cos(phi11a.Radian()), 2);
513  T errb = std::pow(sin(phi1) - sin(phi11b.Radian()), 2)
514  + std::pow(cos(phi1) - cos(phi11b.Radian()), 2);
515  if (errb < erra)
516  {
517  phi2 *= -1;
518  }
519  }
520 
521  // I determined these arguments using trial and error
522  Quaternion<T> result = Quaternion<T>(-phi1, -phi2, -phi3).Inverse();
523 
524  // Previous equations assume repeated moments are at the beginning
525  // of the moments vector (moments[0] == moments[1]).
526  // We have the vectors sorted by size, so it's possible that the
527  // repeated moments are at the end (moments[1] == moments[2]).
528  // In this case (unequalMoment == 0), we apply an extra
529  // rotation that exchanges moment[0] and moment[2]
530  // Rotation matrix = [ 0 0 1]
531  // [ 0 1 0]
532  // [-1 0 0]
533  // That is equivalent to a 90 degree pitch
534  if (unequalMoment == 0)
535  result *= Quaternion<T>(0, IGN_PI_2, 0);
536 
537  return result;
538  }
539 
540  // No repeated principal moments
541  // eq 5.1:
542  T v = (std::pow(this->Ixyxzyz[0], 2) + std::pow(this->Ixyxzyz[1], 2)
543  +(this->Ixxyyzz[0] - moments[2])
544  *(this->Ixxyyzz[0] + moments[2] - moments[0] - moments[1]))
545  / ((moments[1] - moments[2]) * (moments[2] - moments[0]));
546  // value of w depends on v
547  T w;
548  if (v < std::abs(tol))
549  {
550  // first sentence after eq 5.4:
551  // "In the case that v = 0, then w = 1."
552  w = 1;
553  }
554  else
555  {
556  // eq 5.2:
557  w = (this->Ixxyyzz[0] - moments[2] + (moments[2] - moments[1])*v)
558  / ((moments[0] - moments[1]) * v);
559  }
560  // initialize values of angle phi1, phi2, phi3
561  T phi1 = 0;
562  // eq 5.3: start with positive value
563  T phi2 = acos(clamp<T>(ClampedSqrt(v), -1, 1));
564  // eq 5.4: start with positive value
565  T phi3 = acos(clamp<T>(ClampedSqrt(w), -1, 1));
566 
567  // compute g1, g2 for phi2,phi3 >= 0
568  // equations 5.7, 5.8
569  Vector2<T> g1(
570  0.5* (moments[0]-moments[1])*ClampedSqrt(v)*sin(2*phi3),
571  0.5*((moments[0]-moments[1])*w + moments[1]-moments[2])*sin(2*phi2));
572  Vector2<T> g2(
573  (moments[0]-moments[1])*(1 + (v-2)*w) + (moments[1]-moments[2])*v,
574  (moments[0]-moments[1])*sin(phi2)*sin(2*phi3));
575 
576  // The paragraph prior to equation 5.16 describes how to choose
577  // the candidate value of phi1 based on the length
578  // of the f1 and f2 vectors.
579  // * The case of |f1| == |f2| == 0 implies a repeated moment,
580  // which should not be possible at this point in the code
581  // * When |f1| != 0 and |f2| != 0, then one should choose the
582  // value of phi2 so that phi11 = phi12
583  // * When |f1| == 0 and f2 != 0, then phi1 = phi12
584  // phi11 can be ignored, and either sign of phi2, phi3 can be used
585  // * When |f2| == 0 and f1 != 0, then phi1 = phi11
586  // phi12 can be ignored, and either sign of phi2, phi3 can be used
587  bool f1small = f1.SquaredLength() < std::pow(tol, 2);
588  bool f2small = f2.SquaredLength() < std::pow(tol, 2);
589  if (f1small && f2small)
590  {
591  // this should never happen
592  // f1small && f2small implies a repeated moment
593  // return invalid quaternion
594  return Quaternion<T>::Zero;
595  }
596  else if (f1small)
597  {
598  // use phi12 (equations 5.12, 5.14)
599  math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
600  phi12.Normalize();
601  phi1 = phi12.Radian();
602  }
603  else if (f2small)
604  {
605  // use phi11 (equations 5.11, 5.13)
606  math::Angle phi11(Angle2(g1, tol) - Angle2(f1, tol));
607  phi11.Normalize();
608  phi1 = phi11.Radian();
609  }
610  else
611  {
612  // check for when phi11 == phi12
613  // eqs 5.11, 5.13:
614  math::Angle phi11(Angle2(g1, tol) - Angle2(f1, tol));
615  phi11.Normalize();
616  // eqs 5.12, 5.14:
617  math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
618  phi12.Normalize();
619  T err = std::pow(sin(phi11.Radian()) - sin(phi12.Radian()), 2)
620  + std::pow(cos(phi11.Radian()) - cos(phi12.Radian()), 2);
621  phi1 = phi11.Radian();
622  math::Vector2<T> signsPhi23(1, 1);
623  // case a: phi2 <= 0
624  {
625  Vector2<T> g1a = Vector2<T>(1, -1) * g1;
626  Vector2<T> g2a = Vector2<T>(1, -1) * g2;
627  math::Angle phi11a(Angle2(g1a, tol) - Angle2(f1, tol));
628  math::Angle phi12a(0.5*(Angle2(g2a, tol) - Angle2(f2, tol)));
629  phi11a.Normalize();
630  phi12a.Normalize();
631  T erra = std::pow(sin(phi11a.Radian()) - sin(phi12a.Radian()), 2)
632  + std::pow(cos(phi11a.Radian()) - cos(phi12a.Radian()), 2);
633  if (erra < err)
634  {
635  err = erra;
636  phi1 = phi11a.Radian();
637  signsPhi23.Set(-1, 1);
638  }
639  }
640  // case b: phi3 <= 0
641  {
642  Vector2<T> g1b = Vector2<T>(-1, 1) * g1;
643  Vector2<T> g2b = Vector2<T>(1, -1) * g2;
644  math::Angle phi11b(Angle2(g1b, tol) - Angle2(f1, tol));
645  math::Angle phi12b(0.5*(Angle2(g2b, tol) - Angle2(f2, tol)));
646  phi11b.Normalize();
647  phi12b.Normalize();
648  T errb = std::pow(sin(phi11b.Radian()) - sin(phi12b.Radian()), 2)
649  + std::pow(cos(phi11b.Radian()) - cos(phi12b.Radian()), 2);
650  if (errb < err)
651  {
652  err = errb;
653  phi1 = phi11b.Radian();
654  signsPhi23.Set(1, -1);
655  }
656  }
657  // case c: phi2,phi3 <= 0
658  {
659  Vector2<T> g1c = Vector2<T>(-1, -1) * g1;
660  Vector2<T> g2c = g2;
661  math::Angle phi11c(Angle2(g1c, tol) - Angle2(f1, tol));
662  math::Angle phi12c(0.5*(Angle2(g2c, tol) - Angle2(f2, tol)));
663  phi11c.Normalize();
664  phi12c.Normalize();
665  T errc = std::pow(sin(phi11c.Radian()) - sin(phi12c.Radian()), 2)
666  + std::pow(cos(phi11c.Radian()) - cos(phi12c.Radian()), 2);
667  if (errc < err)
668  {
669  err = errc;
670  phi1 = phi11c.Radian();
671  signsPhi23.Set(-1, -1);
672  }
673  }
674 
675  // apply sign changes
676  phi2 *= signsPhi23[0];
677  phi3 *= signsPhi23[1];
678  }
679 
680  // I determined these arguments using trial and error
681  return Quaternion<T>(-phi1, -phi2, -phi3).Inverse();
682  }
683 
693  public: bool EquivalentBox(Vector3<T> &_size,
694  Quaternion<T> &_rot,
695  const T _tol = 1e-6) const
696  {
697  if (!this->IsPositive())
698  {
699  // inertia is not positive, cannot compute equivalent box
700  return false;
701  }
702 
703  Vector3<T> moments = this->PrincipalMoments(_tol);
704  if (!ValidMoments(moments))
705  {
706  // principal moments don't satisfy the triangle identity
707  return false;
708  }
709 
710  // The reason for checking that the principal moments satisfy
711  // the triangle inequality
712  // I1 + I2 - I3 >= 0
713  // is to ensure that the arguments to sqrt in these equations
714  // are positive and the box size is real.
715  _size.X(sqrt(6*(moments.Y() + moments.Z() - moments.X()) / this->mass));
716  _size.Y(sqrt(6*(moments.Z() + moments.X() - moments.Y()) / this->mass));
717  _size.Z(sqrt(6*(moments.X() + moments.Y() - moments.Z()) / this->mass));
718 
719  _rot = this->PrincipalAxesOffset(_tol);
720 
721  if (_rot == Quaternion<T>::Zero)
722  {
723  // _rot is an invalid quaternion
724  return false;
725  }
726 
727  return true;
728  }
729 
735  public: bool SetFromBox(const T _mass,
736  const Vector3<T> &_size,
738  {
739  // Check that _mass and _size are strictly positive
740  // and that quatenion is valid
741  if (_mass <= 0 || _size.Min() <= 0 || _rot == Quaternion<T>::Zero)
742  {
743  return false;
744  }
745  this->Mass(_mass);
746  return this->SetFromBox(_size, _rot);
747  }
748 
754  public: bool SetFromBox(const Vector3<T> &_size,
756  {
757  // Check that _mass and _size are strictly positive
758  // and that quatenion is valid
759  if (this->Mass() <= 0 || _size.Min() <= 0 ||
760  _rot == Quaternion<T>::Zero)
761  {
762  return false;
763  }
764 
765  // Diagonal matrix L with principal moments
766  Matrix3<T> L;
767  T x2 = std::pow(_size.X(), 2);
768  T y2 = std::pow(_size.Y(), 2);
769  T z2 = std::pow(_size.Z(), 2);
770  L(0, 0) = this->mass / 12.0 * (y2 + z2);
771  L(1, 1) = this->mass / 12.0 * (z2 + x2);
772  L(2, 2) = this->mass / 12.0 * (x2 + y2);
773  Matrix3<T> R(_rot);
774  return this->MOI(R * L * R.Transposed());
775  }
776 
784  public: bool SetFromCylinderZ(const T _mass,
785  const T _length,
786  const T _radius,
788  {
789  // Check that _mass, _radius and _length are strictly positive
790  // and that quatenion is valid
791  if (_mass <= 0 || _length <= 0 || _radius <= 0 ||
792  _rot == Quaternion<T>::Zero)
793  {
794  return false;
795  }
796  this->Mass(_mass);
797  return this->SetFromCylinderZ(_length, _radius, _rot);
798  }
799 
806  public: bool SetFromCylinderZ(const T _length,
807  const T _radius,
808  const Quaternion<T> &_rot)
809  {
810  // Check that _mass and _size are strictly positive
811  // and that quatenion is valid
812  if (this->Mass() <= 0 || _length <= 0 || _radius <= 0 ||
813  _rot == Quaternion<T>::Zero)
814  {
815  return false;
816  }
817 
818  // Diagonal matrix L with principal moments
819  T radius2 = std::pow(_radius, 2);
820  Matrix3<T> L;
821  L(0, 0) = this->mass / 12.0 * (3*radius2 + std::pow(_length, 2));
822  L(1, 1) = L(0, 0);
823  L(2, 2) = this->mass / 2.0 * radius2;
824  Matrix3<T> R(_rot);
825  return this->MOI(R * L * R.Transposed());
826  }
827 
832  public: bool SetFromSphere(const T _mass, const T _radius)
833  {
834  // Check that _mass and _radius are strictly positive
835  if (_mass <= 0 || _radius <= 0)
836  {
837  return false;
838  }
839  this->Mass(_mass);
840  return this->SetFromSphere(_radius);
841  }
842 
847  public: bool SetFromSphere(const T _radius)
848  {
849  // Check that _mass and _radius are strictly positive
850  if (this->Mass() <= 0 || _radius <= 0)
851  {
852  return false;
853  }
854 
855  // Diagonal matrix L with principal moments
856  T radius2 = std::pow(_radius, 2);
857  Matrix3<T> L;
858  L(0, 0) = 0.4 * this->mass * radius2;
859  L(1, 1) = 0.4 * this->mass * radius2;
860  L(2, 2) = 0.4 * this->mass * radius2;
861  return this->MOI(L);
862  }
863 
867  private: static inline T ClampedSqrt(const T &_x)
868  {
869  if (_x <= 0)
870  return 0;
871  return sqrt(_x);
872  }
873 
879  private: static T Angle2(const Vector2<T> &_v, const T _eps = 1e-6)
880  {
881  if (_v.SquaredLength() < std::pow(_eps, 2))
882  return 0;
883  return atan2(_v[1], _v[0]);
884  }
885 
887  private: T mass;
888 
892  private: Vector3<T> Ixxyyzz;
893 
897  private: Vector3<T> Ixyxzyz;
898  };
899 
902  }
903 }
904 #endif
An angle and related functions.
Definition: Angle.hh:44
T X() const
Get the x value.
Definition: Vector3.hh:635
Vector3< T > OffDiagonalMoments() const
Get the off-diagonal moments of inertia (Ixy, Ixz, Iyz).
Definition: MassMatrix3.hh:106
bool SetFromBox(const Vector3< T > &_size, const Quaternion< T > &_rot=Quaternion< T >::Identity)
Set inertial properties based on equivalent box using the current mass value.
Definition: MassMatrix3.hh:754
bool Equal(const Vector3 &_v, const T &_tol) const
Equality test with tolerance.
Definition: Vector3.hh:555
bool SetFromSphere(const T _mass, const T _radius)
Set inertial properties based on mass and equivalent sphere.
Definition: MassMatrix3.hh:832
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
T IZZ() const
Get IZZ.
Definition: MassMatrix3.hh:145
MassMatrix3()
Default Constructor.
Definition: MassMatrix3.hh:42
bool SetFromCylinderZ(const T _mass, const T _length, const T _radius, const Quaternion< T > &_rot=Quaternion< T >::Identity)
Set inertial properties based on mass and equivalent cylinder aligned with Z axis.
Definition: MassMatrix3.hh:784
MassMatrix3(const MassMatrix3< T > &_m)
Copy constructor.
Definition: MassMatrix3.hh:57
void Set(T _x, T _y)
Set the contents of the vector.
Definition: Vector2.hh:103
bool EquivalentBox(Vector3< T > &_size, Quaternion< T > &_rot, const T _tol=1e-6) const
Get dimensions and rotation offset of uniform box with equivalent mass and moment of inertia...
Definition: MassMatrix3.hh:693
void Normalize()
Normalize the angle in the range -Pi to Pi.
bool IsPositive() const
Verify that inertia values are positive definite.
Definition: MassMatrix3.hh:284
T IYZ() const
Get IYZ.
Definition: MassMatrix3.hh:166
bool OffDiagonalMoments(const Vector3< T > &_ixyxzyz)
Set the off-diagonal moments of inertia (Ixy, Ixz, Iyz).
Definition: MassMatrix3.hh:123
T SquaredLength() const
Returns the square of the length (magnitude) of the vector.
Definition: Vector2.hh:82
bool IZZ(const T &_v)
Set IZZ.
Definition: MassMatrix3.hh:192
bool operator==(const MassMatrix3< T > &_m) const
Equality comparison operator.
Definition: MassMatrix3.hh:266
A class for inertial information about a rigid body consisting of the scalar mass and a 3x3 symmetric...
Definition: MassMatrix3.hh:39
Two dimensional (x, y) vector.
Definition: Vector2.hh:29
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:329
MassMatrix3 & operator=(const MassMatrix3< T > &_massMatrix)
Equal operator.
Definition: MassMatrix3.hh:253
Vector3< T > DiagonalMoments() const
Get the diagonal moments of inertia (Ixx, Iyy, Izz).
Definition: MassMatrix3.hh:99
T IXY() const
Get IXY.
Definition: MassMatrix3.hh:152
T IYY() const
Get IYY.
Definition: MassMatrix3.hh:138
bool DiagonalMoments(const Vector3< T > &_ixxyyzz)
Set the diagonal moments of inertia (Ixx, Iyy, Izz).
Definition: MassMatrix3.hh:114
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
void Radian(double _radian)
Set the value from an angle in radians.
A 3x3 matrix class.
Definition: Matrix3.hh:35
void Min(const Vector3< T > &_v)
Set this vector&#39;s components to the minimum of itself and the passed in vector.
Definition: Vector3.hh:284
bool SetFromSphere(const T _radius)
Set inertial properties based on equivalent sphere using the current mass value.
Definition: MassMatrix3.hh:847
Matrix3< T > Transposed() const
Return the transpose of this matrix.
Definition: Matrix3.hh:475
T Y() const
Get the y value.
Definition: Vector3.hh:642
bool IXY(const T &_v)
Set IXY.
Definition: MassMatrix3.hh:201
bool IYY(const T &_v)
Set IYY.
Definition: MassMatrix3.hh:183
bool operator!=(const MassMatrix3< T > &_m) const
Inequality test operator.
Definition: MassMatrix3.hh:276
Quaternion< T > PrincipalAxesOffset(const T _tol=1e-6) const
Compute rotational offset of principal axes.
Definition: MassMatrix3.hh:395
Matrix3< T > MOI() const
returns Moments of Inertia as a Matrix3
Definition: MassMatrix3.hh:227
bool IXZ(const T &_v)
Set IXZ.
Definition: MassMatrix3.hh:210
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:36
MassMatrix3< double > MassMatrix3d
Definition: MassMatrix3.hh:900
virtual ~MassMatrix3()
Destructor.
Definition: MassMatrix3.hh:63
MassMatrix3(const T &_mass, const Vector3< T > &_ixxyyzz, const Vector3< T > &_ixyxzyz)
Constructor.
Definition: MassMatrix3.hh:49
T Z() const
Get the z value.
Definition: Vector3.hh:649
bool MOI(const Matrix3< T > &_moi)
Sets Moments of Inertia (MOI) from a Matrix3.
Definition: MassMatrix3.hh:240
void Normalize()
Normalize the vector length.
Definition: Vector2.hh:89
MassMatrix3< float > MassMatrix3f
Definition: MassMatrix3.hh:901
T Mass() const
Get the mass.
Definition: MassMatrix3.hh:76
bool IsValid() const
Verify that inertia values are positive definite and satisfy the triangle inequality.
Definition: MassMatrix3.hh:298
bool SetFromBox(const T _mass, const Vector3< T > &_size, const Quaternion< T > &_rot=Quaternion< T >::Identity)
Set inertial properties based on mass and equivalent box.
Definition: MassMatrix3.hh:735
#define IGN_PI_2
Definition: Helpers.hh:174
bool SetFromCylinderZ(const T _length, const T _radius, const Quaternion< T > &_rot)
Set inertial properties based on equivalent cylinder aligned with Z axis using the current mass value...
Definition: MassMatrix3.hh:806
bool IXX(const T &_v)
Set IXX.
Definition: MassMatrix3.hh:174
Definition: Angle.hh:38
A quaternion class.
Definition: Matrix3.hh:30
#define IGN_PI
Define IGN_PI, IGN_PI_2, and IGN_PI_4.
Definition: Helpers.hh:173
T IXZ() const
Get IXZ.
Definition: MassMatrix3.hh:159
bool Mass(const T &_m)
Set the mass.
Definition: MassMatrix3.hh:68
T IXX() const
Get IXX.
Definition: MassMatrix3.hh:131
void sort3(T &_a, T &_b, T &_c)
Sort three numbers, such that _a <= _b <= _c.
Definition: Helpers.hh:574
T Sum() const
Return the sum of the values.
Definition: Vector3.hh:86