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_MATH_MASSMATRIX3_HH_
18 #define IGNITION_MATH_MASSMATRIX3_HH_
19 
20 #include <algorithm>
21 #include <string>
22 #include <vector>
23 
24 #include <ignition/math/config.hh>
25 #include "ignition/math/Helpers.hh"
27 #include "ignition/math/Vector2.hh"
28 #include "ignition/math/Vector3.hh"
29 #include "ignition/math/Matrix3.hh"
30 
31 namespace ignition
32 {
33  namespace math
34  {
35  inline namespace IGNITION_MATH_VERSION_NAMESPACE
36  {
41  template<typename T>
43  {
45  public: MassMatrix3() : mass(0)
46  {}
47 
52  public: MassMatrix3(const T &_mass,
53  const Vector3<T> &_ixxyyzz,
54  const Vector3<T> &_ixyxzyz)
55  : mass(_mass), Ixxyyzz(_ixxyyzz), Ixyxzyz(_ixyxzyz)
56  {}
57 
60  public: MassMatrix3(const MassMatrix3<T> &_m)
61  : mass(_m.Mass()), Ixxyyzz(_m.DiagonalMoments()),
62  Ixyxzyz(_m.OffDiagonalMoments())
63  {}
64 
66  public: virtual ~MassMatrix3() {}
67 
71  public: bool Mass(const T &_m)
72  {
73  this->mass = _m;
74  return this->IsValid();
75  }
76 
79  public: T Mass() const
80  {
81  return this->mass;
82  }
83 
92  public: bool InertiaMatrix(const T &_ixx, const T &_iyy, const T &_izz,
93  const T &_ixy, const T &_ixz, const T &_iyz)
94  {
95  this->Ixxyyzz.Set(_ixx, _iyy, _izz);
96  this->Ixyxzyz.Set(_ixy, _ixz, _iyz);
97  return this->IsValid();
98  }
99 
102  public: Vector3<T> DiagonalMoments() const
103  {
104  return this->Ixxyyzz;
105  }
106 
110  {
111  return this->Ixyxzyz;
112  }
113 
117  public: bool DiagonalMoments(const Vector3<T> &_ixxyyzz)
118  {
119  this->Ixxyyzz = _ixxyyzz;
120  return this->IsValid();
121  }
122 
126  public: bool OffDiagonalMoments(const Vector3<T> &_ixyxzyz)
127  {
128  this->Ixyxzyz = _ixyxzyz;
129  return this->IsValid();
130  }
131 
134  public: T IXX() const
135  {
136  return this->Ixxyyzz[0];
137  }
138 
141  public: T IYY() const
142  {
143  return this->Ixxyyzz[1];
144  }
145 
148  public: T IZZ() const
149  {
150  return this->Ixxyyzz[2];
151  }
152 
155  public: T IXY() const
156  {
157  return this->Ixyxzyz[0];
158  }
159 
162  public: T IXZ() const
163  {
164  return this->Ixyxzyz[1];
165  }
166 
169  public: T IYZ() const
170  {
171  return this->Ixyxzyz[2];
172  }
173 
177  public: bool IXX(const T &_v)
178  {
179  this->Ixxyyzz.X(_v);
180  return this->IsValid();
181  }
182 
186  public: bool IYY(const T &_v)
187  {
188  this->Ixxyyzz.Y(_v);
189  return this->IsValid();
190  }
191 
195  public: bool IZZ(const T &_v)
196  {
197  this->Ixxyyzz.Z(_v);
198  return this->IsValid();
199  }
200 
204  public: bool IXY(const T &_v)
205  {
206  this->Ixyxzyz.X(_v);
207  return this->IsValid();
208  }
209 
213  public: bool IXZ(const T &_v)
214  {
215  this->Ixyxzyz.Y(_v);
216  return this->IsValid();
217  }
218 
222  public: bool IYZ(const T &_v)
223  {
224  this->Ixyxzyz.Z(_v);
225  return this->IsValid();
226  }
227 
230  public: Matrix3<T> MOI() const
231  {
232  return Matrix3<T>(
233  this->Ixxyyzz[0], this->Ixyxzyz[0], this->Ixyxzyz[1],
234  this->Ixyxzyz[0], this->Ixxyyzz[1], this->Ixyxzyz[2],
235  this->Ixyxzyz[1], this->Ixyxzyz[2], this->Ixxyyzz[2]);
236  }
237 
243  public: bool MOI(const Matrix3<T> &_moi)
244  {
245  this->Ixxyyzz.Set(_moi(0, 0), _moi(1, 1), _moi(2, 2));
246  this->Ixyxzyz.Set(
247  0.5*(_moi(0, 1) + _moi(1, 0)),
248  0.5*(_moi(0, 2) + _moi(2, 0)),
249  0.5*(_moi(1, 2) + _moi(2, 1)));
250  return this->IsValid();
251  }
252 
256  public: MassMatrix3 &operator=(const MassMatrix3<T> &_massMatrix)
257  {
258  this->mass = _massMatrix.Mass();
259  this->Ixxyyzz = _massMatrix.DiagonalMoments();
260  this->Ixyxzyz = _massMatrix.OffDiagonalMoments();
261 
262  return *this;
263  }
264 
269  public: bool operator==(const MassMatrix3<T> &_m) const
270  {
271  return equal<T>(this->mass, _m.Mass()) &&
272  (this->Ixxyyzz == _m.DiagonalMoments()) &&
273  (this->Ixyxzyz == _m.OffDiagonalMoments());
274  }
275 
279  public: bool operator!=(const MassMatrix3<T> &_m) const
280  {
281  return !(*this == _m);
282  }
283 
287  public: bool IsPositive() const
288  {
289  // Check if mass and determinants of all upper left submatrices
290  // of moment of inertia matrix are positive
291  return (this->mass > 0) &&
292  (this->IXX() > 0) &&
293  (this->IXX()*this->IYY() - std::pow(this->IXY(), 2) > 0) &&
294  (this->MOI().Determinant() > 0);
295  }
296 
301  public: bool IsValid() const
302  {
303  return this->IsPositive() && ValidMoments(this->PrincipalMoments());
304  }
305 
311  public: static bool ValidMoments(const Vector3<T> &_moments)
312  {
313  return _moments[0] > 0 &&
314  _moments[1] > 0 &&
315  _moments[2] > 0 &&
316  _moments[0] + _moments[1] > _moments[2] &&
317  _moments[1] + _moments[2] > _moments[0] &&
318  _moments[2] + _moments[0] > _moments[1];
319  }
320 
332  public: Vector3<T> PrincipalMoments(const T _tol = 1e-6) const
333  {
334  // Compute tolerance relative to maximum value of inertia diagonal
335  T tol = _tol * this->Ixxyyzz.Max();
336  if (this->Ixyxzyz.Equal(Vector3<T>::Zero, tol))
337  {
338  // Matrix is already diagonalized, return diagonal moments
339  return this->Ixxyyzz;
340  }
341 
342  // Algorithm based on http://arxiv.org/abs/1306.6291v4
343  // A Method for Fast Diagonalization of a 2x2 or 3x3 Real Symmetric
344  // Matrix, by Maarten Kronenburg
345  Vector3<T> Id(this->Ixxyyzz);
346  Vector3<T> Ip(this->Ixyxzyz);
347  // b = Ixx + Iyy + Izz
348  T b = Id.Sum();
349  // c = Ixx*Iyy - Ixy^2 + Ixx*Izz - Ixz^2 + Iyy*Izz - Iyz^2
350  T c = Id[0]*Id[1] - std::pow(Ip[0], 2)
351  + Id[0]*Id[2] - std::pow(Ip[1], 2)
352  + Id[1]*Id[2] - std::pow(Ip[2], 2);
353  // d = Ixx*Iyz^2 + Iyy*Ixz^2 + Izz*Ixy^2 - Ixx*Iyy*Izz - 2*Ixy*Ixz*Iyz
354  T d = Id[0]*std::pow(Ip[2], 2)
355  + Id[1]*std::pow(Ip[1], 2)
356  + Id[2]*std::pow(Ip[0], 2)
357  - Id[0]*Id[1]*Id[2]
358  - 2*Ip[0]*Ip[1]*Ip[2];
359  // p = b^2 - 3c
360  T p = std::pow(b, 2) - 3*c;
361 
362  // At this point, it is important to check that p is not close
363  // to zero, since its inverse is used to compute delta.
364  // In equation 4.7, p is expressed as a sum of squares
365  // that is only zero if the matrix is diagonal
366  // with identical principal moments.
367  // This check has no test coverage, since this function returns
368  // immediately if a diagonal matrix is detected.
369  if (p < std::pow(tol, 2))
370  return b / 3.0 * Vector3<T>::One;
371 
372  // q = 2b^3 - 9bc - 27d
373  T q = 2*std::pow(b, 3) - 9*b*c - 27*d;
374 
375  // delta = acos(q / (2 * p^(1.5)))
376  // additionally clamp the argument to [-1,1]
377  T delta = acos(clamp<T>(0.5 * q / std::pow(p, 1.5), -1, 1));
378 
379  // sort the moments from smallest to largest
380  T moment0 = (b + 2*sqrt(p) * cos(delta / 3.0)) / 3.0;
381  T moment1 = (b + 2*sqrt(p) * cos((delta + 2*IGN_PI)/3.0)) / 3.0;
382  T moment2 = (b + 2*sqrt(p) * cos((delta - 2*IGN_PI)/3.0)) / 3.0;
383  sort3(moment0, moment1, moment2);
384  return Vector3<T>(moment0, moment1, moment2);
385  }
386 
398  public: Quaternion<T> PrincipalAxesOffset(const T _tol = 1e-6) const
399  {
400  // Compute tolerance relative to maximum value of inertia diagonal
401  T tol = _tol * this->Ixxyyzz.Max();
402  Vector3<T> moments = this->PrincipalMoments(tol);
403  if (moments.Equal(this->Ixxyyzz, tol) ||
404  (math::equal<T>(moments[0], moments[1], std::abs(tol)) &&
405  math::equal<T>(moments[0], moments[2], std::abs(tol))))
406  {
407  // matrix is already aligned with principal axes
408  // or all three moments are approximately equal
409  // return identity rotation
411  }
412 
413  // Algorithm based on http://arxiv.org/abs/1306.6291v4
414  // A Method for Fast Diagonalization of a 2x2 or 3x3 Real Symmetric
415  // Matrix, by Maarten Kronenburg
416  // A real, symmetric matrix can be diagonalized by an orthogonal matrix
417  // (due to the finite-dimensional spectral theorem
418  // https://en.wikipedia.org/wiki/Spectral_theorem
419  // #Hermitian_maps_and_Hermitian_matrices ),
420  // and another name for orthogonal matrix is rotation matrix.
421  // Section 5 of the paper shows how to compute Euler angles
422  // phi1, phi2, and phi3 that map to a rotation matrix.
423  // In some cases, there are multiple possible values for a given angle,
424  // such as phi1, that are denoted as phi11, phi12, phi11a, phi12a, etc.
425  // Similar variable names are used to the paper so that the paper
426  // can be used as an additional reference.
427 
428  // f1, f2 defined in equations 5.5, 5.6
429  Vector2<T> f1(this->Ixyxzyz[0], -this->Ixyxzyz[1]);
430  Vector2<T> f2(this->Ixxyyzz[1] - this->Ixxyyzz[2],
431  -2*this->Ixyxzyz[2]);
432 
433  // Check if two moments are equal, since different equations are used
434  // The moments vector is already sorted, so just check adjacent values.
435  Vector2<T> momentsDiff(moments[0] - moments[1],
436  moments[1] - moments[2]);
437 
438  // index of unequal moment
439  int unequalMoment = -1;
440  if (equal<T>(momentsDiff[0], 0, std::abs(tol)))
441  unequalMoment = 2;
442  else if (equal<T>(momentsDiff[1], 0, std::abs(tol)))
443  unequalMoment = 0;
444 
445  if (unequalMoment >= 0)
446  {
447  // moments[1] is the repeated value
448  // it is not equal to moments[unequalMoment]
449  // momentsDiff3 = lambda - lambda3
450  T momentsDiff3 = moments[1] - moments[unequalMoment];
451  // eq 5.21:
452  // s = cos(phi2)^2 = (A_11 - lambda3) / (lambda - lambda3)
453  // s >= 0 since A_11 is in range [lambda, lambda3]
454  T s = (this->Ixxyyzz[0] - moments[unequalMoment]) / momentsDiff3;
455  // set phi3 to zero for repeated moments (eq 5.23)
456  T phi3 = 0;
457  // phi2 = +- acos(sqrt(s))
458  // start with just the positive value
459  // also clamp the acos argument to prevent NaN's
460  T phi2 = acos(clamp<T>(ClampedSqrt(s), -1, 1));
461 
462  // The paper defines variables phi11 and phi12
463  // which are candidate values of angle phi1.
464  // phi12 is straightforward to compute as a function of f2 and g2.
465  // eq 5.25:
466  Vector2<T> g2(momentsDiff3 * s, 0);
467  // combining eq 5.12 and 5.14, and subtracting psi2
468  // instead of multiplying by its rotation matrix:
469  math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
470  phi12.Normalize();
471 
472  // The paragraph prior to equation 5.16 describes how to choose
473  // the candidate value of phi1 based on the length
474  // of the f1 and f2 vectors.
475  // * When |f1| != 0 and |f2| != 0, then one should choose the
476  // value of phi2 so that phi11 = phi12
477  // * When |f1| == 0 and f2 != 0, then phi1 = phi12
478  // phi11 can be ignored, and either sign of phi2 can be used
479  // * The case of |f2| == 0 can be ignored at this point in the code
480  // since having a repeated moment when |f2| == 0 implies that
481  // the matrix is diagonal. But this function returns a unit
482  // quaternion for diagonal matrices, so we can assume |f2| != 0
483  // See MassMatrix3.ipynb for a more complete discussion.
484  //
485  // Since |f2| != 0, we only need to consider |f1|
486  // * |f1| == 0: phi1 = phi12
487  // * |f1| != 0: choose phi2 so that phi11 == phi12
488  // In either case, phi1 = phi12,
489  // and the sign of phi2 must be chosen to make phi11 == phi12
490  T phi1 = phi12.Radian();
491 
492  bool f1small = f1.SquaredLength() < std::pow(tol, 2);
493  if (!f1small)
494  {
495  // a: phi2 > 0
496  // eq. 5.24
497  Vector2<T> g1a(0, 0.5*momentsDiff3 * sin(2*phi2));
498  // combining eq 5.11 and 5.13, and subtracting psi1
499  // instead of multiplying by its rotation matrix:
500  math::Angle phi11a(Angle2(g1a, tol) - Angle2(f1, tol));
501  phi11a.Normalize();
502 
503  // b: phi2 < 0
504  // eq. 5.24
505  Vector2<T> g1b(0, 0.5*momentsDiff3 * sin(-2*phi2));
506  // combining eq 5.11 and 5.13, and subtracting psi1
507  // instead of multiplying by its rotation matrix:
508  math::Angle phi11b(Angle2(g1b, tol) - Angle2(f1, tol));
509  phi11b.Normalize();
510 
511  // choose sign of phi2
512  // based on whether phi11a or phi11b is closer to phi12
513  // use sin and cos to account for angle wrapping
514  T erra = std::pow(sin(phi1) - sin(phi11a.Radian()), 2)
515  + std::pow(cos(phi1) - cos(phi11a.Radian()), 2);
516  T errb = std::pow(sin(phi1) - sin(phi11b.Radian()), 2)
517  + std::pow(cos(phi1) - cos(phi11b.Radian()), 2);
518  if (errb < erra)
519  {
520  phi2 *= -1;
521  }
522  }
523 
524  // I determined these arguments using trial and error
525  Quaternion<T> result = Quaternion<T>(-phi1, -phi2, -phi3).Inverse();
526 
527  // Previous equations assume repeated moments are at the beginning
528  // of the moments vector (moments[0] == moments[1]).
529  // We have the vectors sorted by size, so it's possible that the
530  // repeated moments are at the end (moments[1] == moments[2]).
531  // In this case (unequalMoment == 0), we apply an extra
532  // rotation that exchanges moment[0] and moment[2]
533  // Rotation matrix = [ 0 0 1]
534  // [ 0 1 0]
535  // [-1 0 0]
536  // That is equivalent to a 90 degree pitch
537  if (unequalMoment == 0)
538  result *= Quaternion<T>(0, IGN_PI_2, 0);
539 
540  return result;
541  }
542 
543  // No repeated principal moments
544  // eq 5.1:
545  T v = (std::pow(this->Ixyxzyz[0], 2) + std::pow(this->Ixyxzyz[1], 2)
546  +(this->Ixxyyzz[0] - moments[2])
547  *(this->Ixxyyzz[0] + moments[2] - moments[0] - moments[1]))
548  / ((moments[1] - moments[2]) * (moments[2] - moments[0]));
549  // value of w depends on v
550  T w;
551  if (v < std::abs(tol))
552  {
553  // first sentence after eq 5.4:
554  // "In the case that v = 0, then w = 1."
555  w = 1;
556  }
557  else
558  {
559  // eq 5.2:
560  w = (this->Ixxyyzz[0] - moments[2] + (moments[2] - moments[1])*v)
561  / ((moments[0] - moments[1]) * v);
562  }
563  // initialize values of angle phi1, phi2, phi3
564  T phi1 = 0;
565  // eq 5.3: start with positive value
566  T phi2 = acos(clamp<T>(ClampedSqrt(v), -1, 1));
567  // eq 5.4: start with positive value
568  T phi3 = acos(clamp<T>(ClampedSqrt(w), -1, 1));
569 
570  // compute g1, g2 for phi2,phi3 >= 0
571  // equations 5.7, 5.8
572  Vector2<T> g1(
573  0.5* (moments[0]-moments[1])*ClampedSqrt(v)*sin(2*phi3),
574  0.5*((moments[0]-moments[1])*w + moments[1]-moments[2])*sin(2*phi2));
575  Vector2<T> g2(
576  (moments[0]-moments[1])*(1 + (v-2)*w) + (moments[1]-moments[2])*v,
577  (moments[0]-moments[1])*sin(phi2)*sin(2*phi3));
578 
579  // The paragraph prior to equation 5.16 describes how to choose
580  // the candidate value of phi1 based on the length
581  // of the f1 and f2 vectors.
582  // * The case of |f1| == |f2| == 0 implies a repeated moment,
583  // which should not be possible at this point in the code
584  // * When |f1| != 0 and |f2| != 0, then one should choose the
585  // value of phi2 so that phi11 = phi12
586  // * When |f1| == 0 and f2 != 0, then phi1 = phi12
587  // phi11 can be ignored, and either sign of phi2, phi3 can be used
588  // * When |f2| == 0 and f1 != 0, then phi1 = phi11
589  // phi12 can be ignored, and either sign of phi2, phi3 can be used
590  bool f1small = f1.SquaredLength() < std::pow(tol, 2);
591  bool f2small = f2.SquaredLength() < std::pow(tol, 2);
592  if (f1small && f2small)
593  {
594  // this should never happen
595  // f1small && f2small implies a repeated moment
596  // return invalid quaternion
598  return Quaternion<T>::Zero;
599  }
600  else if (f1small)
601  {
602  // use phi12 (equations 5.12, 5.14)
603  math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
604  phi12.Normalize();
605  phi1 = phi12.Radian();
606  }
607  else if (f2small)
608  {
609  // use phi11 (equations 5.11, 5.13)
610  math::Angle phi11(Angle2(g1, tol) - Angle2(f1, tol));
611  phi11.Normalize();
612  phi1 = phi11.Radian();
613  }
614  else
615  {
616  // check for when phi11 == phi12
617  // eqs 5.11, 5.13:
618  math::Angle phi11(Angle2(g1, tol) - Angle2(f1, tol));
619  phi11.Normalize();
620  // eqs 5.12, 5.14:
621  math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
622  phi12.Normalize();
623  T err = std::pow(sin(phi11.Radian()) - sin(phi12.Radian()), 2)
624  + std::pow(cos(phi11.Radian()) - cos(phi12.Radian()), 2);
625  phi1 = phi11.Radian();
626  math::Vector2<T> signsPhi23(1, 1);
627  // case a: phi2 <= 0
628  {
629  Vector2<T> g1a = Vector2<T>(1, -1) * g1;
630  Vector2<T> g2a = Vector2<T>(1, -1) * g2;
631  math::Angle phi11a(Angle2(g1a, tol) - Angle2(f1, tol));
632  math::Angle phi12a(0.5*(Angle2(g2a, tol) - Angle2(f2, tol)));
633  phi11a.Normalize();
634  phi12a.Normalize();
635  T erra = std::pow(sin(phi11a.Radian()) - sin(phi12a.Radian()), 2)
636  + std::pow(cos(phi11a.Radian()) - cos(phi12a.Radian()), 2);
637  if (erra < err)
638  {
639  err = erra;
640  phi1 = phi11a.Radian();
641  signsPhi23.Set(-1, 1);
642  }
643  }
644  // case b: phi3 <= 0
645  {
646  Vector2<T> g1b = Vector2<T>(-1, 1) * g1;
647  Vector2<T> g2b = Vector2<T>(1, -1) * g2;
648  math::Angle phi11b(Angle2(g1b, tol) - Angle2(f1, tol));
649  math::Angle phi12b(0.5*(Angle2(g2b, tol) - Angle2(f2, tol)));
650  phi11b.Normalize();
651  phi12b.Normalize();
652  T errb = std::pow(sin(phi11b.Radian()) - sin(phi12b.Radian()), 2)
653  + std::pow(cos(phi11b.Radian()) - cos(phi12b.Radian()), 2);
654  if (errb < err)
655  {
656  err = errb;
657  phi1 = phi11b.Radian();
658  signsPhi23.Set(1, -1);
659  }
660  }
661  // case c: phi2,phi3 <= 0
662  {
663  Vector2<T> g1c = Vector2<T>(-1, -1) * g1;
664  Vector2<T> g2c = g2;
665  math::Angle phi11c(Angle2(g1c, tol) - Angle2(f1, tol));
666  math::Angle phi12c(0.5*(Angle2(g2c, tol) - Angle2(f2, tol)));
667  phi11c.Normalize();
668  phi12c.Normalize();
669  T errc = std::pow(sin(phi11c.Radian()) - sin(phi12c.Radian()), 2)
670  + std::pow(cos(phi11c.Radian()) - cos(phi12c.Radian()), 2);
671  if (errc < err)
672  {
673  err = errc;
674  phi1 = phi11c.Radian();
675  signsPhi23.Set(-1, -1);
676  }
677  }
678 
679  // apply sign changes
680  phi2 *= signsPhi23[0];
681  phi3 *= signsPhi23[1];
682  }
683 
684  // I determined these arguments using trial and error
685  return Quaternion<T>(-phi1, -phi2, -phi3).Inverse();
686  }
687 
697  public: bool EquivalentBox(Vector3<T> &_size,
698  Quaternion<T> &_rot,
699  const T _tol = 1e-6) const
700  {
701  if (!this->IsPositive())
702  {
703  // inertia is not positive, cannot compute equivalent box
704  return false;
705  }
706 
707  Vector3<T> moments = this->PrincipalMoments(_tol);
708  if (!ValidMoments(moments))
709  {
710  // principal moments don't satisfy the triangle identity
711  return false;
712  }
713 
714  // The reason for checking that the principal moments satisfy
715  // the triangle inequality
716  // I1 + I2 - I3 >= 0
717  // is to ensure that the arguments to sqrt in these equations
718  // are positive and the box size is real.
719  _size.X(sqrt(6*(moments.Y() + moments.Z() - moments.X()) / this->mass));
720  _size.Y(sqrt(6*(moments.Z() + moments.X() - moments.Y()) / this->mass));
721  _size.Z(sqrt(6*(moments.X() + moments.Y() - moments.Z()) / this->mass));
722 
723  _rot = this->PrincipalAxesOffset(_tol);
724 
725  if (_rot == Quaternion<T>::Zero)
726  {
727  // _rot is an invalid quaternion
729  return false;
730  }
731 
732  return true;
733  }
734 
740  public: bool SetFromBox(const T _mass,
741  const Vector3<T> &_size,
743  {
744  // Check that _mass and _size are strictly positive
745  // and that quatenion is valid
746  if (_mass <= 0 || _size.Min() <= 0 || _rot == Quaternion<T>::Zero)
747  {
748  return false;
749  }
750  this->Mass(_mass);
751  return this->SetFromBox(_size, _rot);
752  }
753 
759  public: bool SetFromBox(const Vector3<T> &_size,
761  {
762  // Check that _mass and _size are strictly positive
763  // and that quatenion is valid
764  if (this->Mass() <= 0 || _size.Min() <= 0 ||
765  _rot == Quaternion<T>::Zero)
766  {
767  return false;
768  }
769 
770  // Diagonal matrix L with principal moments
771  Matrix3<T> L;
772  T x2 = std::pow(_size.X(), 2);
773  T y2 = std::pow(_size.Y(), 2);
774  T z2 = std::pow(_size.Z(), 2);
775  L(0, 0) = this->mass / 12.0 * (y2 + z2);
776  L(1, 1) = this->mass / 12.0 * (z2 + x2);
777  L(2, 2) = this->mass / 12.0 * (x2 + y2);
778  Matrix3<T> R(_rot);
779  return this->MOI(R * L * R.Transposed());
780  }
781 
789  public: bool SetFromCylinderZ(const T _mass,
790  const T _length,
791  const T _radius,
793  {
794  // Check that _mass, _radius and _length are strictly positive
795  // and that quatenion is valid
796  if (_mass <= 0 || _length <= 0 || _radius <= 0 ||
797  _rot == Quaternion<T>::Zero)
798  {
799  return false;
800  }
801  this->Mass(_mass);
802  return this->SetFromCylinderZ(_length, _radius, _rot);
803  }
804 
811  public: bool SetFromCylinderZ(const T _length,
812  const T _radius,
813  const Quaternion<T> &_rot)
814  {
815  // Check that _mass and _size are strictly positive
816  // and that quatenion is valid
817  if (this->Mass() <= 0 || _length <= 0 || _radius <= 0 ||
818  _rot == Quaternion<T>::Zero)
819  {
820  return false;
821  }
822 
823  // Diagonal matrix L with principal moments
824  T radius2 = std::pow(_radius, 2);
825  Matrix3<T> L;
826  L(0, 0) = this->mass / 12.0 * (3*radius2 + std::pow(_length, 2));
827  L(1, 1) = L(0, 0);
828  L(2, 2) = this->mass / 2.0 * radius2;
829  Matrix3<T> R(_rot);
830  return this->MOI(R * L * R.Transposed());
831  }
832 
837  public: bool SetFromSphere(const T _mass, const T _radius)
838  {
839  // Check that _mass and _radius are strictly positive
840  if (_mass <= 0 || _radius <= 0)
841  {
842  return false;
843  }
844  this->Mass(_mass);
845  return this->SetFromSphere(_radius);
846  }
847 
852  public: bool SetFromSphere(const T _radius)
853  {
854  // Check that _mass and _radius are strictly positive
855  if (this->Mass() <= 0 || _radius <= 0)
856  {
857  return false;
858  }
859 
860  // Diagonal matrix L with principal moments
861  T radius2 = std::pow(_radius, 2);
862  Matrix3<T> L;
863  L(0, 0) = 0.4 * this->mass * radius2;
864  L(1, 1) = 0.4 * this->mass * radius2;
865  L(2, 2) = 0.4 * this->mass * radius2;
866  return this->MOI(L);
867  }
868 
872  private: static inline T ClampedSqrt(const T &_x)
873  {
874  if (_x <= 0)
875  return 0;
876  return sqrt(_x);
877  }
878 
884  private: static T Angle2(const Vector2<T> &_v, const T _eps = 1e-6)
885  {
886  if (_v.SquaredLength() < std::pow(_eps, 2))
887  return 0;
888  return atan2(_v[1], _v[0]);
889  }
890 
892  private: T mass;
893 
897  private: Vector3<T> Ixxyyzz;
898 
902  private: Vector3<T> Ixyxzyz;
903  };
904 
907  }
908  }
909 }
910 #endif
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::IYY
bool IYY(const T &_v)
Set IYY.
Definition: MassMatrix3.hh:186
Helpers.hh
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::IZZ
T IZZ() const
Get IZZ.
Definition: MassMatrix3.hh:148
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector2::SquaredLength
T SquaredLength() const
Returns the square of the length (magnitude) of the vector.
Definition: Vector2.hh:85
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector3::Equal
bool Equal(const Vector3 &_v, const T &_tol) const
Equality test with tolerance.
Definition: Vector3.hh:558
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::OffDiagonalMoments
bool OffDiagonalMoments(const Vector3< T > &_ixyxzyz)
Set the off-diagonal moments of inertia (Ixy, Ixz, Iyz).
Definition: MassMatrix3.hh:126
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector3::Min
void Min(const Vector3< T > &_v)
Set this vector's components to the minimum of itself and the passed in vector.
Definition: Vector3.hh:287
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Angle
An angle and related functions.
Definition: Angle.hh:47
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector2::Set
void Set(T _x, T _y)
Set the contents of the vector.
Definition: Vector2.hh:106
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Matrix3
A 3x3 matrix class.
Definition: Matrix3.hh:38
ignition
Definition: Angle.hh:39
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::SetFromBox
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:740
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::DiagonalMoments
bool DiagonalMoments(const Vector3< T > &_ixxyyzz)
Set the diagonal moments of inertia (Ixx, Iyy, Izz).
Definition: MassMatrix3.hh:117
Vector3.hh
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::PrincipalMoments
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:332
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::SetFromBox
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:759
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::MassMatrix3
MassMatrix3(const MassMatrix3< T > &_m)
Copy constructor.
Definition: MassMatrix3.hh:60
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::IYY
T IYY() const
Get IYY.
Definition: MassMatrix3.hh:141
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::IYZ
T IYZ() const
Get IYZ.
Definition: MassMatrix3.hh:169
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Quaternion< T >
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3d
MassMatrix3< double > MassMatrix3d
Definition: MassMatrix3.hh:905
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::SetFromCylinderZ
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:811
Quaternion.hh
IGN_PI
#define IGN_PI
Define IGN_PI, IGN_PI_2, and IGN_PI_4.
Definition: Helpers.hh:174
Vector2.hh
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::Mass
bool Mass(const T &_m)
Set the mass.
Definition: MassMatrix3.hh:71
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::IsValid
bool IsValid() const
Verify that inertia values are positive definite and satisfy the triangle inequality.
Definition: MassMatrix3.hh:301
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::MassMatrix3
MassMatrix3(const T &_mass, const Vector3< T > &_ixxyyzz, const Vector3< T > &_ixyxzyz)
Constructor.
Definition: MassMatrix3.hh:52
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::operator=
MassMatrix3 & operator=(const MassMatrix3< T > &_massMatrix)
Equal operator.
Definition: MassMatrix3.hh:256
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector3::Sum
T Sum() const
Return the sum of the values.
Definition: Vector3.hh:89
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::IXY
T IXY() const
Get IXY.
Definition: MassMatrix3.hh:155
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::PrincipalAxesOffset
Quaternion< T > PrincipalAxesOffset(const T _tol=1e-6) const
Compute rotational offset of principal axes.
Definition: MassMatrix3.hh:398
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::EquivalentBox
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:697
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Angle::Radian
void Radian(double _radian)
Set the value from an angle in radians.
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::SetFromSphere
bool SetFromSphere(const T _mass, const T _radius)
Set inertial properties based on mass and equivalent sphere.
Definition: MassMatrix3.hh:837
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::DiagonalMoments
Vector3< T > DiagonalMoments() const
Get the diagonal moments of inertia (Ixx, Iyy, Izz).
Definition: MassMatrix3.hh:102
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::MOI
bool MOI(const Matrix3< T > &_moi)
Sets Moments of Inertia (MOI) from a Matrix3.
Definition: MassMatrix3.hh:243
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector3
The Vector3 class represents the generic vector containing 3 elements. Since it's commonly used to ke...
Definition: Vector3.hh:39
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Quaternion::Inverse
Quaternion< T > Inverse() const
Get the inverse of this quaternion.
Definition: Quaternion.hh:131
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::IXZ
T IXZ() const
Get IXZ.
Definition: MassMatrix3.hh:162
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::IXY
bool IXY(const T &_v)
Set IXY.
Definition: MassMatrix3.hh:204
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::sort3
void sort3(T &_a, T &_b, T &_c)
Sort three numbers, such that _a <= _b <= _c.
Definition: Helpers.hh:601
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::IXZ
bool IXZ(const T &_v)
Set IXZ.
Definition: MassMatrix3.hh:213
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3f
MassMatrix3< float > MassMatrix3f
Definition: MassMatrix3.hh:906
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::operator==
bool operator==(const MassMatrix3< T > &_m) const
Equality comparison operator.
Definition: MassMatrix3.hh:269
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3
A class for inertial information about a rigid body consisting of the scalar mass and a 3x3 symmetric...
Definition: MassMatrix3.hh:42
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::MOI
Matrix3< T > MOI() const
returns Moments of Inertia as a Matrix3
Definition: MassMatrix3.hh:230
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::Mass
T Mass() const
Get the mass.
Definition: MassMatrix3.hh:79
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::~MassMatrix3
virtual ~MassMatrix3()
Destructor.
Definition: MassMatrix3.hh:66
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::OffDiagonalMoments
Vector3< T > OffDiagonalMoments() const
Get the off-diagonal moments of inertia (Ixy, Ixz, Iyz).
Definition: MassMatrix3.hh:109
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::IXX
T IXX() const
Get IXX.
Definition: MassMatrix3.hh:134
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::MassMatrix3
MassMatrix3()
Default Constructor.
Definition: MassMatrix3.hh:45
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::operator!=
bool operator!=(const MassMatrix3< T > &_m) const
Inequality test operator.
Definition: MassMatrix3.hh:279
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Angle::Normalize
void Normalize()
Normalize the angle in the range -Pi to Pi.
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::IXX
bool IXX(const T &_v)
Set IXX.
Definition: MassMatrix3.hh:177
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::SetFromCylinderZ
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:789
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::ValidMoments
static bool ValidMoments(const Vector3< T > &_moments)
Verify that principal moments are positive and satisfy the triangle inequality.
Definition: MassMatrix3.hh:311
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::IZZ
bool IZZ(const T &_v)
Set IZZ.
Definition: MassMatrix3.hh:195
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::IsPositive
bool IsPositive() const
Verify that inertia values are positive definite.
Definition: MassMatrix3.hh:287
IGN_PI_2
#define IGN_PI_2
Definition: Helpers.hh:175
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector2
Two dimensional (x, y) vector.
Definition: Vector2.hh:32
Matrix3.hh
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::InertiaMatrix
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:92
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::SetFromSphere
bool SetFromSphere(const T _radius)
Set inertial properties based on equivalent sphere using the current mass value.
Definition: MassMatrix3.hh:852
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Matrix3::Transposed
Matrix3< T > Transposed() const
Return the transpose of this matrix.
Definition: Matrix3.hh:478
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector3::X
T X() const
Get the x value.
Definition: Vector3.hh:647
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector3::Z
T Z() const
Get the z value.
Definition: Vector3.hh:661
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::Vector3::Y
T Y() const
Get the y value.
Definition: Vector3.hh:654
ignition::math::IGNITION_MATH_VERSION_NAMESPACE::MassMatrix3::IYZ
bool IYZ(const T &_v)
Set IYZ.
Definition: MassMatrix3.hh:222