Gazebo Math

API Reference

6.15.1
gz/math/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 GZ_MATH_MASSMATRIX3_HH_
18 #define GZ_MATH_MASSMATRIX3_HH_
19 
20 #include <algorithm>
21 #include <limits>
22 #include <string>
23 #include <vector>
24 
25 #include <gz/math/config.hh>
26 #include "gz/math/Helpers.hh"
27 #include "gz/math/Material.hh"
28 #include "gz/math/Quaternion.hh"
29 #include "gz/math/Vector2.hh"
30 #include "gz/math/Vector3.hh"
31 #include "gz/math/Matrix3.hh"
32 
33 namespace ignition
34 {
35  namespace math
36  {
37  // Inline bracket to help doxygen filtering.
38  inline namespace IGNITION_MATH_VERSION_NAMESPACE {
39  //
44  template<typename T>
46  {
49  public: MassMatrix3() : mass(0)
50  {}
51 
56  public: MassMatrix3(const T &_mass,
57  const Vector3<T> &_ixxyyzz,
58  const Vector3<T> &_ixyxzyz)
59  : mass(_mass), Ixxyyzz(_ixxyyzz), Ixyxzyz(_ixyxzyz)
60  {}
61 
64  public: MassMatrix3(const MassMatrix3<T> &_m)
65  : mass(_m.Mass()), Ixxyyzz(_m.DiagonalMoments()),
66  Ixyxzyz(_m.OffDiagonalMoments())
67  {}
68 
70  public: virtual ~MassMatrix3() {}
71 
76  public: bool IGN_DEPRECATED(5.0) Mass(const T &_m)
77  {
78  return this->SetMass(_m);
79  }
80 
84  public: bool SetMass(const T &_m)
85  {
86  this->mass = _m;
87  return this->IsValid();
88  }
89 
92  public: T Mass() const
93  {
94  return this->mass;
95  }
96 
107  public: bool IGN_DEPRECATED(5.0) InertiaMatrix(
108  const T &_ixx, const T &_iyy, const T &_izz,
109  const T &_ixy, const T &_ixz, const T &_iyz)
110  {
111  return this->SetInertiaMatrix(_ixx, _iyy, _izz, _ixy, _ixz, _iyz);
112  }
113 
122  public: bool SetInertiaMatrix(
123  const T &_ixx, const T &_iyy, const T &_izz,
124  const T &_ixy, const T &_ixz, const T &_iyz)
125  {
126  this->Ixxyyzz.Set(_ixx, _iyy, _izz);
127  this->Ixyxzyz.Set(_ixy, _ixz, _iyz);
128  return this->IsValid();
129  }
130 
133  public: Vector3<T> DiagonalMoments() const
134  {
135  return this->Ixxyyzz;
136  }
137 
141  {
142  return this->Ixyxzyz;
143  }
144 
149  public: bool IGN_DEPRECATED(5.0) DiagonalMoments(
150  const Vector3<T> &_ixxyyzz)
151  {
152  return this->SetDiagonalMoments(_ixxyyzz);
153  }
154 
158  public: bool SetDiagonalMoments(const Vector3<T> &_ixxyyzz)
159  {
160  this->Ixxyyzz = _ixxyyzz;
161  return this->IsValid();
162  }
163 
168  public: bool IGN_DEPRECATED(5.0) OffDiagonalMoments(
169  const Vector3<T> &_ixyxzyz)
170  {
171  return this->SetOffDiagonalMoments(_ixyxzyz);
172  }
173 
177  public: bool SetOffDiagonalMoments(const Vector3<T> &_ixyxzyz)
178  {
179  this->Ixyxzyz = _ixyxzyz;
180  return this->IsValid();
181  }
182 
186  public: T IGN_DEPRECATED(5.0) IXX() const
187  {
188  return this->Ixx();
189  }
190 
193  public: T Ixx() const
194  {
195  return this->Ixxyyzz[0];
196  }
197 
201  public: T IGN_DEPRECATED(5.0) IYY() const
202  {
203  return this->Iyy();
204  }
205 
208  public: T Iyy() const
209  {
210  return this->Ixxyyzz[1];
211  }
212 
216  public: T IGN_DEPRECATED(5.0) IZZ() const
217  {
218  return this->Izz();
219  }
220 
223  public: T Izz() const
224  {
225  return this->Ixxyyzz[2];
226  }
227 
231  public: T IGN_DEPRECATED(5.0) IXY() const
232  {
233  return this->Ixy();
234  }
235 
238  public: T Ixy() const
239  {
240  return this->Ixyxzyz[0];
241  }
242 
246  public: T IGN_DEPRECATED(5.0) IXZ() const
247  {
248  return this->Ixz();
249  }
250 
253  public: T Ixz() const
254  {
255  return this->Ixyxzyz[1];
256  }
257 
261  public: T IGN_DEPRECATED(5.0) IYZ() const
262  {
263  return this->Iyz();
264  }
265 
268  public: T Iyz() const
269  {
270  return this->Ixyxzyz[2];
271  }
272 
277  public: bool IGN_DEPRECATED(5.0) IXX(const T &_v)
278  {
279  return this->SetIxx(_v);
280  }
281 
285  public: bool SetIxx(const T &_v)
286  {
287  this->Ixxyyzz.X(_v);
288  return this->IsValid();
289  }
290 
295  public: bool IGN_DEPRECATED(5.0) IYY(const T &_v)
296  {
297  return this->SetIyy(_v);
298  }
299 
303  public: bool SetIyy(const T &_v)
304  {
305  this->Ixxyyzz.Y(_v);
306  return this->IsValid();
307  }
308 
313  public: bool IGN_DEPRECATED(5.0) IZZ(const T &_v)
314  {
315  return this->SetIzz(_v);
316  }
317 
321  public: bool SetIzz(const T &_v)
322  {
323  this->Ixxyyzz.Z(_v);
324  return this->IsValid();
325  }
326 
331  public: bool IGN_DEPRECATED(5.0) IXY(const T &_v)
332  {
333  return this->SetIxy(_v);
334  }
335 
339  public: bool SetIxy(const T &_v)
340  {
341  this->Ixyxzyz.X(_v);
342  return this->IsValid();
343  }
344 
349  public: bool IGN_DEPRECATED(5.0) IXZ(const T &_v)
350  {
351  return this->SetIxz(_v);
352  }
353 
357  public: bool SetIxz(const T &_v)
358  {
359  this->Ixyxzyz.Y(_v);
360  return this->IsValid();
361  }
362 
367  public: bool IGN_DEPRECATED(5.0) IYZ(const T &_v)
368  {
369  return this->SetIyz(_v);
370  }
371 
375  public: bool SetIyz(const T &_v)
376  {
377  this->Ixyxzyz.Z(_v);
378  return this->IsValid();
379  }
380 
384  public: Matrix3<T> IGN_DEPRECATED(5.0) MOI() const
385  {
386  return this->Moi();
387  }
388 
391  public: Matrix3<T> Moi() const
392  {
393  return Matrix3<T>(
394  this->Ixxyyzz[0], this->Ixyxzyz[0], this->Ixyxzyz[1],
395  this->Ixyxzyz[0], this->Ixxyyzz[1], this->Ixyxzyz[2],
396  this->Ixyxzyz[1], this->Ixyxzyz[2], this->Ixxyyzz[2]);
397  }
398 
405  public: bool IGN_DEPRECATED(5.0) MOI(const Matrix3<T> &_moi)
406  {
407  return this->SetMoi(_moi);
408  }
409 
415  public: bool SetMoi(const Matrix3<T> &_moi)
416  {
417  this->Ixxyyzz.Set(_moi(0, 0), _moi(1, 1), _moi(2, 2));
418  this->Ixyxzyz.Set(
419  0.5*(_moi(0, 1) + _moi(1, 0)),
420  0.5*(_moi(0, 2) + _moi(2, 0)),
421  0.5*(_moi(1, 2) + _moi(2, 1)));
422  return this->IsValid();
423  }
424 
428  public: MassMatrix3 &operator=(const MassMatrix3<T> &_massMatrix)
429  {
430  this->mass = _massMatrix.Mass();
431  this->Ixxyyzz = _massMatrix.DiagonalMoments();
432  this->Ixyxzyz = _massMatrix.OffDiagonalMoments();
433 
434  return *this;
435  }
436 
441  public: bool operator==(const MassMatrix3<T> &_m) const
442  {
443  return equal<T>(this->mass, _m.Mass()) &&
444  (this->Ixxyyzz == _m.DiagonalMoments()) &&
445  (this->Ixyxzyz == _m.OffDiagonalMoments());
446  }
447 
451  public: bool operator!=(const MassMatrix3<T> &_m) const
452  {
453  return !(*this == _m);
454  }
455 
475  public: bool IsNearPositive(const T _tolerance =
476  IGN_MASSMATRIX3_DEFAULT_TOLERANCE<T>) const
477  {
478  const T epsilon = this->Epsilon(_tolerance);
479 
480  // Check if mass and determinants of all upper left submatrices
481  // of moment of inertia matrix are positive
482  return (this->mass >= 0) &&
483  (this->Ixx() + epsilon >= 0) &&
484  (this->Ixx() * this->Iyy() - std::pow(this->Ixy(), 2) +
485  epsilon >= 0) &&
486  (this->Moi().Determinant() + epsilon >= 0);
487  }
488 
508  public: bool IsPositive(const T _tolerance =
509  IGN_MASSMATRIX3_DEFAULT_TOLERANCE<T>) const
510  {
511  const T epsilon = this->Epsilon(_tolerance);
512 
513  // Check if mass and determinants of all upper left submatrices
514  // of moment of inertia matrix are positive
515  return (this->mass > 0) &&
516  (this->Ixx() + epsilon > 0) &&
517  (this->Ixx() * this->Iyy() - std::pow(this->Ixy(), 2) +
518  epsilon > 0) &&
519  (this->Moi().Determinant() + epsilon > 0);
520  }
521 
529  public: T Epsilon(const T _tolerance =
530  IGN_MASSMATRIX3_DEFAULT_TOLERANCE<T>) const
531  {
532  return Epsilon(this->DiagonalMoments(), _tolerance);
533  }
534 
556  public: static T Epsilon(const Vector3<T> &_moments,
557  const T _tolerance =
558  IGN_MASSMATRIX3_DEFAULT_TOLERANCE<T>)
559  {
560  // The following was borrowed heavily from:
561  // https://github.com/RobotLocomotion/drake/blob/v0.27.0/multibody/tree/rotational_inertia.h
562 
563  // Compute the maximum possible moment of inertia, which will be
564  // used to compute whether the moments are valid.
565  //
566  // The maximum moment of inertia is bounded by:
567  // trace / 3 <= maxPossibleMoi <= trace / 2.
568  //
569  // The trace of a matrix is the sum of the coefficients on the
570  // main diagonal. For a mass matrix, this is equal to
571  // ixx + iyy + izz, or _moments.Sum() for this function's
572  // implementation.
573  //
574  // It is okay if maxPossibleMoi == zero.
575  T maxPossibleMoI = 0.5 * std::abs(_moments.Sum());
576 
577  // In order to check validity of the moments we need to use an
578  // epsilon value that is related to machine precision
579  // multiplied by the largest possible moment of inertia.
580  return _tolerance *
581  std::numeric_limits<T>::epsilon() * maxPossibleMoI;
582  }
583 
595  public: bool IsValid(const T _tolerance =
596  IGN_MASSMATRIX3_DEFAULT_TOLERANCE<T>) const
597  {
598  return this->IsNearPositive(_tolerance) &&
599  ValidMoments(this->PrincipalMoments(), _tolerance);
600  }
601 
622  public: static bool ValidMoments(const Vector3<T> &_moments,
623  const T _tolerance = IGN_MASSMATRIX3_DEFAULT_TOLERANCE<T>)
624  {
625  T epsilon = Epsilon(_moments, _tolerance);
626 
627  return _moments[0] + epsilon >= 0 &&
628  _moments[1] + epsilon >= 0 &&
629  _moments[2] + epsilon >= 0 &&
630  _moments[0] + _moments[1] + epsilon >= _moments[2] &&
631  _moments[1] + _moments[2] + epsilon >= _moments[0] &&
632  _moments[2] + _moments[0] + epsilon >= _moments[1];
633  }
634 
646  public: Vector3<T> PrincipalMoments(const T _tol = 1e-6) const
647  {
648  // Compute tolerance relative to maximum value of inertia diagonal
649  T tol = _tol * this->Ixxyyzz.Max();
650  if (this->Ixyxzyz.Equal(Vector3<T>::Zero, tol))
651  {
652  // Matrix is already diagonalized, return diagonal moments
653  return this->Ixxyyzz;
654  }
655 
656  // Algorithm based on http://arxiv.org/abs/1306.6291v4
657  // A Method for Fast Diagonalization of a 2x2 or 3x3 Real Symmetric
658  // Matrix, by Maarten Kronenburg
659  Vector3<T> Id(this->Ixxyyzz);
660  Vector3<T> Ip(this->Ixyxzyz);
661  // b = Ixx + Iyy + Izz
662  T b = Id.Sum();
663  // c = Ixx*Iyy - Ixy^2 + Ixx*Izz - Ixz^2 + Iyy*Izz - Iyz^2
664  T c = Id[0]*Id[1] - std::pow(Ip[0], 2)
665  + Id[0]*Id[2] - std::pow(Ip[1], 2)
666  + Id[1]*Id[2] - std::pow(Ip[2], 2);
667  // d = Ixx*Iyz^2 + Iyy*Ixz^2 + Izz*Ixy^2 - Ixx*Iyy*Izz - 2*Ixy*Ixz*Iyz
668  T d = Id[0]*std::pow(Ip[2], 2)
669  + Id[1]*std::pow(Ip[1], 2)
670  + Id[2]*std::pow(Ip[0], 2)
671  - Id[0]*Id[1]*Id[2]
672  - 2*Ip[0]*Ip[1]*Ip[2];
673  // p = b^2 - 3c
674  T p = std::pow(b, 2) - 3*c;
675 
676  // At this point, it is important to check that p is not close
677  // to zero, since its inverse is used to compute delta.
678  // In equation 4.7, p is expressed as a sum of squares
679  // that is only zero if the matrix is diagonal
680  // with identical principal moments.
681  // This check has no test coverage, since this function returns
682  // immediately if a diagonal matrix is detected.
683  if (p < std::pow(tol, 2))
684  return b / 3.0 * Vector3<T>::One;
685 
686  // q = 2b^3 - 9bc - 27d
687  T q = 2*std::pow(b, 3) - 9*b*c - 27*d;
688 
689  // delta = acos(q / (2 * p^(1.5)))
690  // additionally clamp the argument to [-1,1]
691  T delta = acos(clamp<T>(0.5 * q / std::pow(p, 1.5), -1, 1));
692 
693  // sort the moments from smallest to largest
694  T moment0 = (b + 2*sqrt(p) * cos(delta / 3.0)) / 3.0;
695  T moment1 = (b + 2*sqrt(p) * cos((delta + 2*IGN_PI)/3.0)) / 3.0;
696  T moment2 = (b + 2*sqrt(p) * cos((delta - 2*IGN_PI)/3.0)) / 3.0;
697  sort3(moment0, moment1, moment2);
698  return Vector3<T>(moment0, moment1, moment2);
699  }
700 
712  public: Quaternion<T> PrincipalAxesOffset(const T _tol = 1e-6) const
713  {
714  Vector3<T> moments = this->PrincipalMoments(_tol);
715  // Compute tolerance relative to maximum value of inertia diagonal
716  T tol = _tol * this->Ixxyyzz.Max();
717  if (moments.Equal(this->Ixxyyzz, tol) ||
718  (math::equal<T>(moments[0], moments[1], std::abs(tol)) &&
719  math::equal<T>(moments[0], moments[2], std::abs(tol))))
720  {
721  // matrix is already aligned with principal axes
722  // or all three moments are approximately equal
723  // return identity rotation
725  }
726 
727  // Algorithm based on http://arxiv.org/abs/1306.6291v4
728  // A Method for Fast Diagonalization of a 2x2 or 3x3 Real Symmetric
729  // Matrix, by Maarten Kronenburg
730  // A real, symmetric matrix can be diagonalized by an orthogonal matrix
731  // (due to the finite-dimensional spectral theorem
732  // https://en.wikipedia.org/wiki/Spectral_theorem
733  // #Hermitian_maps_and_Hermitian_matrices ),
734  // and another name for orthogonal matrix is rotation matrix.
735  // Section 5 of the paper shows how to compute Euler angles
736  // phi1, phi2, and phi3 that map to a rotation matrix.
737  // In some cases, there are multiple possible values for a given angle,
738  // such as phi1, that are denoted as phi11, phi12, phi11a, phi12a, etc.
739  // Similar variable names are used to the paper so that the paper
740  // can be used as an additional reference.
741 
742  // f1, f2 defined in equations 5.5, 5.6
743  Vector2<T> f1(this->Ixyxzyz[0], -this->Ixyxzyz[1]);
744  Vector2<T> f2(this->Ixxyyzz[1] - this->Ixxyyzz[2],
745  -2*this->Ixyxzyz[2]);
746 
747  // Check if two moments are equal, since different equations are used
748  // The moments vector is already sorted, so just check adjacent values.
749  Vector2<T> momentsDiff(moments[0] - moments[1],
750  moments[1] - moments[2]);
751 
752  // index of unequal moment
753  int unequalMoment = -1;
754  if (equal<T>(momentsDiff[0], 0, std::abs(tol)))
755  unequalMoment = 2;
756  else if (equal<T>(momentsDiff[1], 0, std::abs(tol)))
757  unequalMoment = 0;
758 
759  if (unequalMoment >= 0)
760  {
761  // moments[1] is the repeated value
762  // it is not equal to moments[unequalMoment]
763  // momentsDiff3 = lambda - lambda3
764  T momentsDiff3 = moments[1] - moments[unequalMoment];
765  // eq 5.21:
766  // s = cos(phi2)^2 = (A_11 - lambda3) / (lambda - lambda3)
767  // s >= 0 since A_11 is in range [lambda, lambda3]
768  T s = (this->Ixxyyzz[0] - moments[unequalMoment]) / momentsDiff3;
769  // set phi3 to zero for repeated moments (eq 5.23)
770  T phi3 = 0;
771  // phi2 = +- acos(sqrt(s))
772  // start with just the positive value
773  // also clamp the acos argument to prevent NaN's
774  T phi2 = acos(clamp<T>(ClampedSqrt(s), -1, 1));
775 
776  // The paper defines variables phi11 and phi12
777  // which are candidate values of angle phi1.
778  // phi12 is straightforward to compute as a function of f2 and g2.
779  // eq 5.25:
780  Vector2<T> g2(momentsDiff3 * s, 0);
781  // combining eq 5.12 and 5.14, and subtracting psi2
782  // instead of multiplying by its rotation matrix:
783  math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
784  phi12.Normalize();
785 
786  // The paragraph prior to equation 5.16 describes how to choose
787  // the candidate value of phi1 based on the length
788  // of the f1 and f2 vectors.
789  // * When |f1| != 0 and |f2| != 0, then one should choose the
790  // value of phi2 so that phi11 = phi12
791  // * When |f1| == 0 and f2 != 0, then phi1 = phi12
792  // phi11 can be ignored, and either sign of phi2 can be used
793  // * The case of |f2| == 0 can be ignored at this point in the code
794  // since having a repeated moment when |f2| == 0 implies that
795  // the matrix is diagonal. But this function returns a unit
796  // quaternion for diagonal matrices, so we can assume |f2| != 0
797  // See MassMatrix3.ipynb for a more complete discussion.
798  //
799  // Since |f2| != 0, we only need to consider |f1|
800  // * |f1| == 0: phi1 = phi12
801  // * |f1| != 0: choose phi2 so that phi11 == phi12
802  // In either case, phi1 = phi12,
803  // and the sign of phi2 must be chosen to make phi11 == phi12
804  T phi1 = phi12.Radian();
805 
806  bool f1small = f1.SquaredLength() < std::pow(tol, 2);
807  if (!f1small)
808  {
809  // a: phi2 > 0
810  // eq. 5.24
811  Vector2<T> g1a(0, 0.5*momentsDiff3 * sin(2*phi2));
812  // combining eq 5.11 and 5.13, and subtracting psi1
813  // instead of multiplying by its rotation matrix:
814  math::Angle phi11a(Angle2(g1a, tol) - Angle2(f1, tol));
815  phi11a.Normalize();
816 
817  // b: phi2 < 0
818  // eq. 5.24
819  Vector2<T> g1b(0, 0.5*momentsDiff3 * sin(-2*phi2));
820  // combining eq 5.11 and 5.13, and subtracting psi1
821  // instead of multiplying by its rotation matrix:
822  math::Angle phi11b(Angle2(g1b, tol) - Angle2(f1, tol));
823  phi11b.Normalize();
824 
825  // choose sign of phi2
826  // based on whether phi11a or phi11b is closer to phi12
827  // use sin and cos to account for angle wrapping
828  T erra = std::pow(sin(phi1) - sin(phi11a.Radian()), 2)
829  + std::pow(cos(phi1) - cos(phi11a.Radian()), 2);
830  T errb = std::pow(sin(phi1) - sin(phi11b.Radian()), 2)
831  + std::pow(cos(phi1) - cos(phi11b.Radian()), 2);
832  if (errb < erra)
833  {
834  phi2 *= -1;
835  }
836  }
837 
838  // I determined these arguments using trial and error
839  Quaternion<T> result = Quaternion<T>(-phi1, -phi2, -phi3).Inverse();
840 
841  // Previous equations assume repeated moments are at the beginning
842  // of the moments vector (moments[0] == moments[1]).
843  // We have the vectors sorted by size, so it's possible that the
844  // repeated moments are at the end (moments[1] == moments[2]).
845  // In this case (unequalMoment == 0), we apply an extra
846  // rotation that exchanges moment[0] and moment[2]
847  // Rotation matrix = [ 0 0 1]
848  // [ 0 1 0]
849  // [-1 0 0]
850  // That is equivalent to a 90 degree pitch
851  if (unequalMoment == 0)
852  result *= Quaternion<T>(0, IGN_PI_2, 0);
853 
854  return result;
855  }
856 
857  // No repeated principal moments
858  // eq 5.1:
859  T v = (std::pow(this->Ixyxzyz[0], 2) + std::pow(this->Ixyxzyz[1], 2)
860  +(this->Ixxyyzz[0] - moments[2])
861  *(this->Ixxyyzz[0] + moments[2] - moments[0] - moments[1]))
862  / ((moments[1] - moments[2]) * (moments[2] - moments[0]));
863  // value of w depends on v
864  T w;
865  if (v < std::abs(tol))
866  {
867  // first sentence after eq 5.4:
868  // "In the case that v = 0, then w = 1."
869  w = 1;
870  }
871  else
872  {
873  // eq 5.2:
874  w = (this->Ixxyyzz[0] - moments[2] + (moments[2] - moments[1])*v)
875  / ((moments[0] - moments[1]) * v);
876  }
877  // initialize values of angle phi1, phi2, phi3
878  T phi1 = 0;
879  // eq 5.3: start with positive value
880  T phi2 = acos(clamp<T>(ClampedSqrt(v), -1, 1));
881  // eq 5.4: start with positive value
882  T phi3 = acos(clamp<T>(ClampedSqrt(w), -1, 1));
883 
884  // compute g1, g2 for phi2,phi3 >= 0
885  // equations 5.7, 5.8
886  Vector2<T> g1(
887  0.5* (moments[0]-moments[1])*ClampedSqrt(v)*sin(2*phi3),
888  0.5*((moments[0]-moments[1])*w + moments[1]-moments[2])*sin(2*phi2));
889  Vector2<T> g2(
890  (moments[0]-moments[1])*(1 + (v-2)*w) + (moments[1]-moments[2])*v,
891  (moments[0]-moments[1])*sin(phi2)*sin(2*phi3));
892 
893  // The paragraph prior to equation 5.16 describes how to choose
894  // the candidate value of phi1 based on the length
895  // of the f1 and f2 vectors.
896  // * The case of |f1| == |f2| == 0 implies a repeated moment,
897  // which should not be possible at this point in the code
898  // * When |f1| != 0 and |f2| != 0, then one should choose the
899  // value of phi2 so that phi11 = phi12
900  // * When |f1| == 0 and f2 != 0, then phi1 = phi12
901  // phi11 can be ignored, and either sign of phi2, phi3 can be used
902  // * When |f2| == 0 and f1 != 0, then phi1 = phi11
903  // phi12 can be ignored, and either sign of phi2, phi3 can be used
904  bool f1small = f1.SquaredLength() < std::pow(tol, 2);
905  bool f2small = f2.SquaredLength() < std::pow(tol, 2);
906  if (f1small && f2small)
907  {
908  // this should never happen
909  // f1small && f2small implies a repeated moment
910  // return invalid quaternion
912  return Quaternion<T>::Zero;
913  }
914  else if (f1small)
915  {
916  // use phi12 (equations 5.12, 5.14)
917  math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
918  phi12.Normalize();
919  phi1 = phi12.Radian();
920  }
921  else if (f2small)
922  {
923  // use phi11 (equations 5.11, 5.13)
924  math::Angle phi11(Angle2(g1, tol) - Angle2(f1, tol));
925  phi11.Normalize();
926  phi1 = phi11.Radian();
927  }
928  else
929  {
930  // check for when phi11 == phi12
931  // eqs 5.11, 5.13:
932  math::Angle phi11(Angle2(g1, tol) - Angle2(f1, tol));
933  phi11.Normalize();
934  // eqs 5.12, 5.14:
935  math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
936  phi12.Normalize();
937  T err = std::pow(sin(phi11.Radian()) - sin(phi12.Radian()), 2)
938  + std::pow(cos(phi11.Radian()) - cos(phi12.Radian()), 2);
939  phi1 = phi11.Radian();
940  math::Vector2<T> signsPhi23(1, 1);
941  // case a: phi2 <= 0
942  {
943  Vector2<T> g1a = Vector2<T>(1, -1) * g1;
944  Vector2<T> g2a = Vector2<T>(1, -1) * g2;
945  math::Angle phi11a(Angle2(g1a, tol) - Angle2(f1, tol));
946  math::Angle phi12a(0.5*(Angle2(g2a, tol) - Angle2(f2, tol)));
947  phi11a.Normalize();
948  phi12a.Normalize();
949  T erra = std::pow(sin(phi11a.Radian()) - sin(phi12a.Radian()), 2)
950  + std::pow(cos(phi11a.Radian()) - cos(phi12a.Radian()), 2);
951  if (erra < err)
952  {
953  err = erra;
954  phi1 = phi11a.Radian();
955  signsPhi23.Set(-1, 1);
956  }
957  }
958  // case b: phi3 <= 0
959  {
960  Vector2<T> g1b = Vector2<T>(-1, 1) * g1;
961  Vector2<T> g2b = Vector2<T>(1, -1) * g2;
962  math::Angle phi11b(Angle2(g1b, tol) - Angle2(f1, tol));
963  math::Angle phi12b(0.5*(Angle2(g2b, tol) - Angle2(f2, tol)));
964  phi11b.Normalize();
965  phi12b.Normalize();
966  T errb = std::pow(sin(phi11b.Radian()) - sin(phi12b.Radian()), 2)
967  + std::pow(cos(phi11b.Radian()) - cos(phi12b.Radian()), 2);
968  if (errb < err)
969  {
970  err = errb;
971  phi1 = phi11b.Radian();
972  signsPhi23.Set(1, -1);
973  }
974  }
975  // case c: phi2,phi3 <= 0
976  {
977  Vector2<T> g1c = Vector2<T>(-1, -1) * g1;
978  Vector2<T> g2c = g2;
979  math::Angle phi11c(Angle2(g1c, tol) - Angle2(f1, tol));
980  math::Angle phi12c(0.5*(Angle2(g2c, tol) - Angle2(f2, tol)));
981  phi11c.Normalize();
982  phi12c.Normalize();
983  T errc = std::pow(sin(phi11c.Radian()) - sin(phi12c.Radian()), 2)
984  + std::pow(cos(phi11c.Radian()) - cos(phi12c.Radian()), 2);
985  if (errc < err)
986  {
987  phi1 = phi11c.Radian();
988  signsPhi23.Set(-1, -1);
989  }
990  }
991 
992  // apply sign changes
993  phi2 *= signsPhi23[0];
994  phi3 *= signsPhi23[1];
995  }
996 
997  // I determined these arguments using trial and error
998  return Quaternion<T>(-phi1, -phi2, -phi3).Inverse();
999  }
1000 
1010  public: bool EquivalentBox(Vector3<T> &_size,
1011  Quaternion<T> &_rot,
1012  const T _tol = 1e-6) const
1013  {
1014  if (!this->IsPositive(0))
1015  {
1016  // inertia is not positive, cannot compute equivalent box
1017  return false;
1018  }
1019 
1020  Vector3<T> moments = this->PrincipalMoments(_tol);
1021  if (!ValidMoments(moments))
1022  {
1023  // principal moments don't satisfy the triangle identity
1024  return false;
1025  }
1026 
1027  // The reason for checking that the principal moments satisfy
1028  // the triangle inequality
1029  // I1 + I2 - I3 >= 0
1030  // is to ensure that the arguments to sqrt in these equations
1031  // are positive and the box size is real.
1032  _size.X(sqrt(6*(moments.Y() + moments.Z() - moments.X()) / this->mass));
1033  _size.Y(sqrt(6*(moments.Z() + moments.X() - moments.Y()) / this->mass));
1034  _size.Z(sqrt(6*(moments.X() + moments.Y() - moments.Z()) / this->mass));
1035 
1036  _rot = this->PrincipalAxesOffset(_tol);
1037 
1038  if (_rot == Quaternion<T>::Zero)
1039  {
1040  // _rot is an invalid quaternion
1042  return false;
1043  }
1044 
1045  return true;
1046  }
1047 
1054  public: bool SetFromBox(const Material &_mat,
1055  const Vector3<T> &_size,
1056  const Quaternion<T> &_rot = Quaternion<T>::Identity)
1057  {
1058  T volume = _size.X() * _size.Y() * _size.Z();
1059  return this->SetFromBox(_mat.Density() * volume, _size, _rot);
1060  }
1061 
1067  public: bool SetFromBox(const T _mass,
1068  const Vector3<T> &_size,
1069  const Quaternion<T> &_rot = Quaternion<T>::Identity)
1070  {
1071  // Check that _mass and _size are strictly positive
1072  // and that quaternion is valid
1073  if (_mass <= 0 || _size.Min() <= 0 || _rot == Quaternion<T>::Zero)
1074  {
1075  return false;
1076  }
1077  this->SetMass(_mass);
1078  return this->SetFromBox(_size, _rot);
1079  }
1080 
1086  public: bool SetFromBox(const Vector3<T> &_size,
1087  const Quaternion<T> &_rot = Quaternion<T>::Identity)
1088  {
1089  // Check that _mass and _size are strictly positive
1090  // and that quaternion is valid
1091  if (this->Mass() <= 0 || _size.Min() <= 0 ||
1092  _rot == Quaternion<T>::Zero)
1093  {
1094  return false;
1095  }
1096 
1097  // Diagonal matrix L with principal moments
1098  Matrix3<T> L;
1099  T x2 = std::pow(_size.X(), 2);
1100  T y2 = std::pow(_size.Y(), 2);
1101  T z2 = std::pow(_size.Z(), 2);
1102  L(0, 0) = this->mass / 12.0 * (y2 + z2);
1103  L(1, 1) = this->mass / 12.0 * (z2 + x2);
1104  L(2, 2) = this->mass / 12.0 * (x2 + y2);
1105  Matrix3<T> R(_rot);
1106  return this->SetMoi(R * L * R.Transposed());
1107  }
1108 
1117  public: bool SetFromCylinderZ(const Material &_mat,
1118  const T _length,
1119  const T _radius,
1120  const Quaternion<T> &_rot = Quaternion<T>::Identity)
1121  {
1122  // Check that density, _radius and _length are strictly positive
1123  // and that quaternion is valid
1124  if (_mat.Density() <= 0 || _length <= 0 || _radius <= 0 ||
1125  _rot == Quaternion<T>::Zero)
1126  {
1127  return false;
1128  }
1129  T volume = IGN_PI * _radius * _radius * _length;
1130  return this->SetFromCylinderZ(_mat.Density() * volume,
1131  _length, _radius, _rot);
1132  }
1133 
1141  public: bool SetFromCylinderZ(const T _mass,
1142  const T _length,
1143  const T _radius,
1144  const Quaternion<T> &_rot = Quaternion<T>::Identity)
1145  {
1146  // Check that _mass, _radius and _length are strictly positive
1147  // and that quaternion is valid
1148  if (_mass <= 0 || _length <= 0 || _radius <= 0 ||
1149  _rot == Quaternion<T>::Zero)
1150  {
1151  return false;
1152  }
1153  this->SetMass(_mass);
1154  return this->SetFromCylinderZ(_length, _radius, _rot);
1155  }
1156 
1163  public: bool SetFromCylinderZ(const T _length,
1164  const T _radius,
1165  const Quaternion<T> &_rot)
1166  {
1167  // Check that _mass and _size are strictly positive
1168  // and that quaternion is valid
1169  if (this->Mass() <= 0 || _length <= 0 || _radius <= 0 ||
1170  _rot == Quaternion<T>::Zero)
1171  {
1172  return false;
1173  }
1174 
1175  // Diagonal matrix L with principal moments
1176  T radius2 = std::pow(_radius, 2);
1177  Matrix3<T> L;
1178  L(0, 0) = this->mass / 12.0 * (3*radius2 + std::pow(_length, 2));
1179  L(1, 1) = L(0, 0);
1180  L(2, 2) = this->mass / 2.0 * radius2;
1181  Matrix3<T> R(_rot);
1182  return this->SetMoi(R * L * R.Transposed());
1183  }
1184 
1191  public: bool SetFromSphere(const Material &_mat, const T _radius)
1192  {
1193  // Check that the density and _radius are strictly positive
1194  if (_mat.Density() <= 0 || _radius <= 0)
1195  {
1196  return false;
1197  }
1198 
1199  T volume = (4.0/3.0) * IGN_PI * std::pow(_radius, 3);
1200  return this->SetFromSphere(_mat.Density() * volume, _radius);
1201  }
1202 
1207  public: bool SetFromSphere(const T _mass, const T _radius)
1208  {
1209  // Check that _mass and _radius are strictly positive
1210  if (_mass <= 0 || _radius <= 0)
1211  {
1212  return false;
1213  }
1214  this->SetMass(_mass);
1215  return this->SetFromSphere(_radius);
1216  }
1217 
1222  public: bool SetFromSphere(const T _radius)
1223  {
1224  // Check that _mass and _radius are strictly positive
1225  if (this->Mass() <= 0 || _radius <= 0)
1226  {
1227  return false;
1228  }
1229 
1230  // Diagonal matrix L with principal moments
1231  T radius2 = std::pow(_radius, 2);
1232  Matrix3<T> L;
1233  L(0, 0) = 0.4 * this->mass * radius2;
1234  L(1, 1) = 0.4 * this->mass * radius2;
1235  L(2, 2) = 0.4 * this->mass * radius2;
1236  return this->SetMoi(L);
1237  }
1238 
1242  private: static inline T ClampedSqrt(const T &_x)
1243  {
1244  if (_x <= 0)
1245  return 0;
1246  return sqrt(_x);
1247  }
1248 
1254  private: static T Angle2(const Vector2<T> &_v, const T _eps = 1e-6)
1255  {
1256  if (_v.SquaredLength() < std::pow(_eps, 2))
1257  return 0;
1258  return atan2(_v[1], _v[0]);
1259  }
1260 
1262  private: T mass;
1263 
1267  private: Vector3<T> Ixxyyzz;
1268 
1272  private: Vector3<T> Ixyxzyz;
1273  };
1274 
1277  }
1278  }
1279 }
1280 #endif
bool DiagonalMoments(const Vector3< T > &_ixxyyzz)
Set the diagonal moments of inertia (Ixx, Iyy, Izz).
Definition: gz/math/MassMatrix3.hh:149
T Iyy() const
Get IYY.
Definition: gz/math/MassMatrix3.hh:208
T IXY() const
Get IXY.
Definition: gz/math/MassMatrix3.hh:231
bool operator!=(const MassMatrix3< T > &_m) const
Inequality test operator.
Definition: gz/math/MassMatrix3.hh:451
Two dimensional (x, y) vector.
Definition: gz/math/Vector2.hh:37
bool IXX(const T &_v)
Set IXX.
Definition: gz/math/MassMatrix3.hh:277
Definition: gz/math/AdditivelySeparableScalarField3.hh:27
MassMatrix3< float > MassMatrix3f
Definition: gz/math/MassMatrix3.hh:1276
#define IGN_PI
Define IGN_PI, IGN_PI_2, and IGN_PI_4. This was put here for Windows support.
Definition: gz/math/Helpers.hh:184
static bool ValidMoments(const Vector3< T > &_moments, const T _tolerance=IGN_MASSMATRIX3_DEFAULT_TOLERANCE< T >)
Verify that principal moments are positive and satisfy the triangle inequality.
Definition: gz/math/MassMatrix3.hh:622
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: gz/math/MassMatrix3.hh:1163
T IXX() const
Get IXX.
Definition: gz/math/MassMatrix3.hh:186
void Radian(double _radian)
Set the value from an angle in radians.
MassMatrix3(const T &_mass, const Vector3< T > &_ixxyyzz, const Vector3< T > &_ixyxzyz)
Constructor.
Definition: gz/math/MassMatrix3.hh:56
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: gz/math/MassMatrix3.hh:1141
T Ixx() const
Get IXX.
Definition: gz/math/MassMatrix3.hh:193
bool SetIzz(const T &_v)
Set IZZ.
Definition: gz/math/MassMatrix3.hh:321
bool IsValid(const T _tolerance=IGN_MASSMATRIX3_DEFAULT_TOLERANCE< T >) const
Verify that inertia values are positive semi-definite and satisfy the triangle inequality.
Definition: gz/math/MassMatrix3.hh:595
Contains information about a single material.
Definition: gz/math/Material.hh:65
T X() const
Get the x value.
Definition: gz/math/Vector3.hh:654
Matrix3< T > Moi() const
returns Moments of Inertia as a Matrix3
Definition: gz/math/MassMatrix3.hh:391
bool IYY(const T &_v)
Set IYY.
Definition: gz/math/MassMatrix3.hh:295
A 3x3 matrix class.
Definition: gz/math/Matrix3.hh:40
bool SetIyy(const T &_v)
Set IYY.
Definition: gz/math/MassMatrix3.hh:303
T Iyz() const
Get IYZ.
Definition: gz/math/MassMatrix3.hh:268
Matrix3< T > MOI() const
returns Moments of Inertia as a Matrix3
Definition: gz/math/MassMatrix3.hh:384
MassMatrix3< double > MassMatrix3d
Definition: gz/math/MassMatrix3.hh:1275
static T Epsilon(const Vector3< T > &_moments, const T _tolerance=IGN_MASSMATRIX3_DEFAULT_TOLERANCE< T >)
Get an epsilon value that represents the amount of acceptable error in a MassMatrix3....
Definition: gz/math/MassMatrix3.hh:556
bool SetOffDiagonalMoments(const Vector3< T > &_ixyxzyz)
Set the off-diagonal moments of inertia (Ixy, Ixz, Iyz).
Definition: gz/math/MassMatrix3.hh:177
virtual ~MassMatrix3()
Destructor.
Definition: gz/math/MassMatrix3.hh:70
bool IsNearPositive(const T _tolerance=IGN_MASSMATRIX3_DEFAULT_TOLERANCE< T >) const
Verify that inertia values are positive semidefinite.
Definition: gz/math/MassMatrix3.hh:475
bool SetInertiaMatrix(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: gz/math/MassMatrix3.hh:122
void Min(const Vector3< T > &_v)
Set this vector's components to the minimum of itself and the passed in vector.
Definition: gz/math/Vector3.hh:294
bool IYZ(const T &_v)
Set IYZ.
Definition: gz/math/MassMatrix3.hh:367
bool SetFromCylinderZ(const Material &_mat, const T _length, const T _radius, const Quaternion< T > &_rot=Quaternion< T >::Identity)
Set inertial properties based on a Material and equivalent cylinder aligned with Z axis.
Definition: gz/math/MassMatrix3.hh:1117
bool SetFromSphere(const Material &_mat, const T _radius)
Set inertial properties based on a material and equivalent sphere.
Definition: gz/math/MassMatrix3.hh:1191
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: gz/math/MassMatrix3.hh:646
T Z() const
Get the z value.
Definition: gz/math/Vector3.hh:668
bool SetMoi(const Matrix3< T > &_moi)
Sets Moments of Inertia (MOI) from a Matrix3. Symmetric component of input matrix is used by averagin...
Definition: gz/math/MassMatrix3.hh:415
bool MOI(const Matrix3< T > &_moi)
Sets Moments of Inertia (MOI) from a Matrix3. Symmetric component of input matrix is used by averagin...
Definition: gz/math/MassMatrix3.hh:405
T IZZ() const
Get IZZ.
Definition: gz/math/MassMatrix3.hh:216
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: gz/math/MassMatrix3.hh:1010
T SquaredLength() const
Returns the square of the length (magnitude) of the vector.
Definition: gz/math/Vector2.hh:100
bool Mass(const T &_m)
Set the mass.
Definition: gz/math/MassMatrix3.hh:76
bool IXY(const T &_v)
Set IXY.
Definition: gz/math/MassMatrix3.hh:331
double Density() const
Get the density value of the material in kg/m^3.
MassMatrix3 & operator=(const MassMatrix3< T > &_massMatrix)
Equal operator.
Definition: gz/math/MassMatrix3.hh:428
The Vector3 class represents the generic vector containing 3 elements. Since it's commonly used to ke...
Definition: gz/math/Vector3.hh:41
MassMatrix3()
Default Constructor, which inializes the mass and moments to zero.
Definition: gz/math/MassMatrix3.hh:49
Vector3< T > DiagonalMoments() const
Get the diagonal moments of inertia (Ixx, Iyy, Izz).
Definition: gz/math/MassMatrix3.hh:133
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: gz/math/MassMatrix3.hh:1086
Quaternion< T > PrincipalAxesOffset(const T _tol=1e-6) const
Compute rotational offset of principal axes.
Definition: gz/math/MassMatrix3.hh:712
void Set(T _x, T _y)
Set the contents of the vector.
Definition: gz/math/Vector2.hh:149
bool SetFromSphere(const T _mass, const T _radius)
Set inertial properties based on mass and equivalent sphere.
Definition: gz/math/MassMatrix3.hh:1207
bool SetIxz(const T &_v)
Set IXZ.
Definition: gz/math/MassMatrix3.hh:357
bool OffDiagonalMoments(const Vector3< T > &_ixyxzyz)
Set the off-diagonal moments of inertia (Ixy, Ixz, Iyz).
Definition: gz/math/MassMatrix3.hh:168
bool SetDiagonalMoments(const Vector3< T > &_ixxyyzz)
Set the diagonal moments of inertia (Ixx, Iyy, Izz).
Definition: gz/math/MassMatrix3.hh:158
T epsilon(T... args)
bool IsPositive(const T _tolerance=IGN_MASSMATRIX3_DEFAULT_TOLERANCE< T >) const
Verify that inertia values are positive definite.
Definition: gz/math/MassMatrix3.hh:508
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: gz/math/MassMatrix3.hh:107
T Epsilon(const T _tolerance=IGN_MASSMATRIX3_DEFAULT_TOLERANCE< T >) const
Get an epsilon value that represents the amount of acceptable error in a MassMatrix3....
Definition: gz/math/MassMatrix3.hh:529
bool Equal(const Vector3 &_v, const T &_tol) const
Equality test with tolerance.
Definition: gz/math/Vector3.hh:565
T IYZ() const
Get IYZ.
Definition: gz/math/MassMatrix3.hh:261
bool SetIxy(const T &_v)
Set IXY.
Definition: gz/math/MassMatrix3.hh:339
void Normalize()
Normalize the angle in the range -Pi to Pi. This modifies the value contained in this Angle instance.
T Izz() const
Get IZZ.
Definition: gz/math/MassMatrix3.hh:223
bool SetMass(const T &_m)
Set the mass.
Definition: gz/math/MassMatrix3.hh:84
MassMatrix3(const MassMatrix3< T > &_m)
Copy constructor.
Definition: gz/math/MassMatrix3.hh:64
The Angle class is used to simplify and clarify the use of radians and degrees measurements....
Definition: gz/math/Angle.hh:61
bool IXZ(const T &_v)
Set IXZ.
Definition: gz/math/MassMatrix3.hh:349
T Sum() const
Return the sum of the values.
Definition: gz/math/Vector3.hh:94
T Mass() const
Get the mass.
Definition: gz/math/MassMatrix3.hh:92
Quaternion< T > Inverse() const
Get the inverse of this quaternion.
Definition: gz/math/Quaternion.hh:132
void sort3(T &_a, T &_b, T &_c)
Sort three numbers, such that _a <= _b <= _c.
Definition: gz/math/Helpers.hh:612
bool SetIxx(const T &_v)
Set IXX.
Definition: gz/math/MassMatrix3.hh:285
Vector3< T > OffDiagonalMoments() const
Get the off-diagonal moments of inertia (Ixy, Ixz, Iyz).
Definition: gz/math/MassMatrix3.hh:140
bool SetIyz(const T &_v)
Set IYZ.
Definition: gz/math/MassMatrix3.hh:375
T Ixz() const
Get IXZ.
Definition: gz/math/MassMatrix3.hh:253
bool SetFromBox(const Material &_mat, const Vector3< T > &_size, const Quaternion< T > &_rot=Quaternion< T >::Identity)
Set inertial properties based on a Material and equivalent box.
Definition: gz/math/MassMatrix3.hh:1054
bool operator==(const MassMatrix3< T > &_m) const
Equality comparison operator.
Definition: gz/math/MassMatrix3.hh:441
T IXZ() const
Get IXZ.
Definition: gz/math/MassMatrix3.hh:246
T Y() const
Get the y value.
Definition: gz/math/Vector3.hh:661
#define IGN_PI_2
Definition: gz/math/Helpers.hh:185
bool IZZ(const T &_v)
Set IZZ.
Definition: gz/math/MassMatrix3.hh:313
T IYY() const
Get IYY.
Definition: gz/math/MassMatrix3.hh:201
bool SetFromSphere(const T _radius)
Set inertial properties based on equivalent sphere using the current mass value.
Definition: gz/math/MassMatrix3.hh:1222
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: gz/math/MassMatrix3.hh:1067
A class for inertial information about a rigid body consisting of the scalar mass and a 3x3 symmetric...
Definition: gz/math/MassMatrix3.hh:45
Matrix3< T > Transposed() const
Return the transpose of this matrix.
Definition: gz/math/Matrix3.hh:480
T Ixy() const
Get IXY.
Definition: gz/math/MassMatrix3.hh:238
T pow(T... args)