Gazebo Math

API Reference

8.1.0
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 <cmath>
22#include <limits>
23#include <string>
24#include <vector>
25
26#include <gz/math/config.hh>
27#include "gz/math/Helpers.hh"
28#include "gz/math/Material.hh"
29#include "gz/math/Quaternion.hh"
30#include "gz/math/Vector2.hh"
31#include "gz/math/Vector3.hh"
32#include "gz/math/Matrix3.hh"
33
34namespace gz::math
35{
36 // Inline bracket to help doxygen filtering.
37 inline namespace GZ_MATH_VERSION_NAMESPACE {
38 //
43 template<typename T>
45 {
48 public: MassMatrix3() : mass(0)
49 {}
50
55 public: MassMatrix3(const T &_mass,
56 const Vector3<T> &_ixxyyzz,
57 const Vector3<T> &_ixyxzyz)
58 : mass(_mass), Ixxyyzz(_ixxyyzz), Ixyxzyz(_ixyxzyz)
59 {}
60
64 public: bool SetMass(const T &_m)
65 {
66 this->mass = _m;
67 return this->IsValid();
68 }
69
72 public: T Mass() const
73 {
74 return this->mass;
75 }
76
85 public: bool SetInertiaMatrix(
86 const T &_ixx, const T &_iyy, const T &_izz,
87 const T &_ixy, const T &_ixz, const T &_iyz)
88 {
89 this->Ixxyyzz.Set(_ixx, _iyy, _izz);
90 this->Ixyxzyz.Set(_ixy, _ixz, _iyz);
91 return this->IsValid();
92 }
93
97 {
98 return this->Ixxyyzz;
99 }
100
104 {
105 return this->Ixyxzyz;
106 }
107
112 {
113 this->Ixxyyzz = _ixxyyzz;
114 return this->IsValid();
115 }
116
121 {
122 this->Ixyxzyz = _ixyxzyz;
123 return this->IsValid();
124 }
125
128 public: T Ixx() const
129 {
130 return this->Ixxyyzz[0];
131 }
132
135 public: T Iyy() const
136 {
137 return this->Ixxyyzz[1];
138 }
139
142 public: T Izz() const
143 {
144 return this->Ixxyyzz[2];
145 }
146
149 public: T Ixy() const
150 {
151 return this->Ixyxzyz[0];
152 }
153
156 public: T Ixz() const
157 {
158 return this->Ixyxzyz[1];
159 }
160
163 public: T Iyz() const
164 {
165 return this->Ixyxzyz[2];
166 }
167
171 public: bool SetIxx(const T &_v)
172 {
173 this->Ixxyyzz.X(_v);
174 return this->IsValid();
175 }
176
180 public: bool SetIyy(const T &_v)
181 {
182 this->Ixxyyzz.Y(_v);
183 return this->IsValid();
184 }
185
189 public: bool SetIzz(const T &_v)
190 {
191 this->Ixxyyzz.Z(_v);
192 return this->IsValid();
193 }
194
198 public: bool SetIxy(const T &_v)
199 {
200 this->Ixyxzyz.X(_v);
201 return this->IsValid();
202 }
203
207 public: bool SetIxz(const T &_v)
208 {
209 this->Ixyxzyz.Y(_v);
210 return this->IsValid();
211 }
212
216 public: bool SetIyz(const T &_v)
217 {
218 this->Ixyxzyz.Z(_v);
219 return this->IsValid();
220 }
221
224 public: Matrix3<T> Moi() const
225 {
226 return Matrix3<T>(
227 this->Ixxyyzz[0], this->Ixyxzyz[0], this->Ixyxzyz[1],
228 this->Ixyxzyz[0], this->Ixxyyzz[1], this->Ixyxzyz[2],
229 this->Ixyxzyz[1], this->Ixyxzyz[2], this->Ixxyyzz[2]);
230 }
231
237 public: bool SetMoi(const Matrix3<T> &_moi)
238 {
239 this->Ixxyyzz.Set(_moi(0, 0), _moi(1, 1), _moi(2, 2));
240 this->Ixyxzyz.Set(
241 0.5*(_moi(0, 1) + _moi(1, 0)),
242 0.5*(_moi(0, 2) + _moi(2, 0)),
243 0.5*(_moi(1, 2) + _moi(2, 1)));
244 return this->IsValid();
245 }
246
251 public: bool operator==(const MassMatrix3<T> &_m) const
252 {
253 return equal<T>(this->mass, _m.Mass()) &&
254 (this->Ixxyyzz == _m.DiagonalMoments()) &&
255 (this->Ixyxzyz == _m.OffDiagonalMoments());
256 }
257
261 public: bool operator!=(const MassMatrix3<T> &_m) const
262 {
263 return !(*this == _m);
264 }
265
285 public: bool IsNearPositive(const T _tolerance =
287 {
288 const T epsilon = this->Epsilon(_tolerance);
289
290 // Check if mass and determinants of all upper left submatrices
291 // of moment of inertia matrix are positive
292 return (this->mass >= 0) &&
293 (this->Ixx() + epsilon >= 0) &&
294 (this->Ixx() * this->Iyy() - std::pow(this->Ixy(), 2) +
295 epsilon >= 0) &&
296 (this->Moi().Determinant() + epsilon >= 0);
297 }
298
318 public: bool IsPositive(const T _tolerance =
320 {
321 const T epsilon = this->Epsilon(_tolerance);
322
323 // Check if mass and determinants of all upper left submatrices
324 // of moment of inertia matrix are positive
325 return (this->mass > 0) &&
326 (this->Ixx() + epsilon > 0) &&
327 (this->Ixx() * this->Iyy() - std::pow(this->Ixy(), 2) +
328 epsilon > 0) &&
329 (this->Moi().Determinant() + epsilon > 0);
330 }
331
339 public: T Epsilon(const T _tolerance =
341 {
342 return Epsilon(this->DiagonalMoments(), _tolerance);
343 }
344
366 public: static T Epsilon(const Vector3<T> &_moments,
367 const T _tolerance =
369 {
370 // The following was borrowed heavily from:
371 // https://github.com/RobotLocomotion/drake/blob/v0.27.0/multibody/tree/rotational_inertia.h
372
373 // Compute the maximum possible moment of inertia, which will be
374 // used to compute whether the moments are valid.
375 //
376 // The maximum moment of inertia is bounded by:
377 // trace / 3 <= maxPossibleMoi <= trace / 2.
378 //
379 // The trace of a matrix is the sum of the coefficients on the
380 // main diagonal. For a mass matrix, this is equal to
381 // ixx + iyy + izz, or _moments.Sum() for this function's
382 // implementation.
383 //
384 // It is okay if maxPossibleMoi == zero.
385 T maxPossibleMoI = 0.5 * std::abs(_moments.Sum());
386
387 // In order to check validity of the moments we need to use an
388 // epsilon value that is related to machine precision
389 // multiplied by the largest possible moment of inertia.
390 return _tolerance *
392 }
393
405 public: bool IsValid(const T _tolerance =
407 {
408 return this->IsNearPositive(_tolerance) &&
409 ValidMoments(this->PrincipalMoments(), _tolerance);
410 }
411
432 public: static bool ValidMoments(const Vector3<T> &_moments,
434 {
435 T epsilon = Epsilon(_moments, _tolerance);
436
437 return _moments[0] + epsilon >= 0 &&
438 _moments[1] + epsilon >= 0 &&
439 _moments[2] + epsilon >= 0 &&
440 _moments[0] + _moments[1] + epsilon >= _moments[2] &&
441 _moments[1] + _moments[2] + epsilon >= _moments[0] &&
442 _moments[2] + _moments[0] + epsilon >= _moments[1];
443 }
444
456 public: Vector3<T> PrincipalMoments(const T _tol = 1e-6) const
457 {
458 // Compute tolerance relative to maximum value of inertia diagonal
459 T tol = _tol * this->Ixxyyzz.Max();
460 if (this->Ixyxzyz.Equal(Vector3<T>::Zero, tol))
461 {
462 // Matrix is already diagonalized, return diagonal moments
463 return this->Ixxyyzz;
464 }
465
466 // Algorithm based on http://arxiv.org/abs/1306.6291v4
467 // A Method for Fast Diagonalization of a 2x2 or 3x3 Real Symmetric
468 // Matrix, by Maarten Kronenburg
469 Vector3<T> Id(this->Ixxyyzz);
470 Vector3<T> Ip(this->Ixyxzyz);
471 // b = Ixx + Iyy + Izz
472 T b = Id.Sum();
473 // c = Ixx*Iyy - Ixy^2 + Ixx*Izz - Ixz^2 + Iyy*Izz - Iyz^2
474 T c = Id[0]*Id[1] - std::pow(Ip[0], 2)
475 + Id[0]*Id[2] - std::pow(Ip[1], 2)
476 + Id[1]*Id[2] - std::pow(Ip[2], 2);
477 // d = Ixx*Iyz^2 + Iyy*Ixz^2 + Izz*Ixy^2 - Ixx*Iyy*Izz - 2*Ixy*Ixz*Iyz
478 T d = Id[0]*std::pow(Ip[2], 2)
479 + Id[1]*std::pow(Ip[1], 2)
480 + Id[2]*std::pow(Ip[0], 2)
481 - Id[0]*Id[1]*Id[2]
482 - 2*Ip[0]*Ip[1]*Ip[2];
483 // p = b^2 - 3c
484 T p = std::pow(b, 2) - 3*c;
485
486 // At this point, it is important to check that p is not close
487 // to zero, since its inverse is used to compute delta.
488 // In equation 4.7, p is expressed as a sum of squares
489 // that is only zero if the matrix is diagonal
490 // with identical principal moments.
491 // This check has no test coverage, since this function returns
492 // immediately if a diagonal matrix is detected.
493 if (p < std::pow(tol, 2))
494 return b / 3.0 * Vector3<T>::One;
495
496 // q = 2b^3 - 9bc - 27d
497 T q = 2*std::pow(b, 3) - 9*b*c - 27*d;
498
499 // delta = acos(q / (2 * p^(1.5)))
500 // additionally clamp the argument to [-1,1]
501 T delta = acos(clamp<T>(0.5 * q / std::pow(p, 1.5), -1, 1));
502
503 // sort the moments from smallest to largest
504 T moment0 = (b + 2*sqrt(p) * cos(delta / 3.0)) / 3.0;
505 T moment1 = (b + 2*sqrt(p) * cos((delta + 2*GZ_PI)/3.0)) / 3.0;
506 T moment2 = (b + 2*sqrt(p) * cos((delta - 2*GZ_PI)/3.0)) / 3.0;
509 }
510
522 public: Quaternion<T> PrincipalAxesOffset(const T _tol = 1e-6) const
523 {
524 Vector3<T> moments = this->PrincipalMoments(_tol);
525 // Compute tolerance relative to maximum value of inertia diagonal
526 T tol = _tol * this->Ixxyyzz.Max();
527 if (moments.Equal(this->Ixxyyzz, tol) ||
528 (math::equal<T>(moments[0], moments[1], std::abs(tol)) &&
529 math::equal<T>(moments[0], moments[2], std::abs(tol))))
530 {
531 // matrix is already aligned with principal axes
532 // or all three moments are approximately equal
533 // return identity rotation
535 }
536
537 // Algorithm based on http://arxiv.org/abs/1306.6291v4
538 // A Method for Fast Diagonalization of a 2x2 or 3x3 Real Symmetric
539 // Matrix, by Maarten Kronenburg
540 // A real, symmetric matrix can be diagonalized by an orthogonal matrix
541 // (due to the finite-dimensional spectral theorem
542 // https://en.wikipedia.org/wiki/Spectral_theorem
543 // #Hermitian_maps_and_Hermitian_matrices ),
544 // and another name for orthogonal matrix is rotation matrix.
545 // Section 5 of the paper shows how to compute Euler angles
546 // phi1, phi2, and phi3 that map to a rotation matrix.
547 // In some cases, there are multiple possible values for a given angle,
548 // such as phi1, that are denoted as phi11, phi12, phi11a, phi12a, etc.
549 // Similar variable names are used to the paper so that the paper
550 // can be used as an additional reference.
551
552 // f1, f2 defined in equations 5.5, 5.6
553 Vector2<T> f1(this->Ixyxzyz[0], -this->Ixyxzyz[1]);
554 Vector2<T> f2(this->Ixxyyzz[1] - this->Ixxyyzz[2],
555 -2*this->Ixyxzyz[2]);
556
557 // Check if two moments are equal, since different equations are used
558 // The moments vector is already sorted, so just check adjacent values.
560 moments[1] - moments[2]);
561
562 // index of unequal moment
563 int unequalMoment = -1;
564 if (equal<T>(momentsDiff[0], 0, std::abs(tol)))
565 unequalMoment = 2;
566 else if (equal<T>(momentsDiff[1], 0, std::abs(tol)))
567 unequalMoment = 0;
568
569 if (unequalMoment >= 0)
570 {
571 // moments[1] is the repeated value
572 // it is not equal to moments[unequalMoment]
573 // momentsDiff3 = lambda - lambda3
575 // eq 5.21:
576 // s = cos(phi2)^2 = (A_11 - lambda3) / (lambda - lambda3)
577 // s >= 0 since A_11 is in range [lambda, lambda3]
578 T s = (this->Ixxyyzz[0] - moments[unequalMoment]) / momentsDiff3;
579 // set phi3 to zero for repeated moments (eq 5.23)
580 T phi3 = 0;
581 // phi2 = +- acos(sqrt(s))
582 // start with just the positive value
583 // also clamp the acos argument to prevent NaN's
584 T phi2 = acos(clamp<T>(ClampedSqrt(s), -1, 1));
585
586 // The paper defines variables phi11 and phi12
587 // which are candidate values of angle phi1.
588 // phi12 is straightforward to compute as a function of f2 and g2.
589 // eq 5.25:
590 Vector2<T> g2(momentsDiff3 * s, 0);
591 // combining eq 5.12 and 5.14, and subtracting psi2
592 // instead of multiplying by its rotation matrix:
593 math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
594 phi12.Normalize();
595
596 // The paragraph prior to equation 5.16 describes how to choose
597 // the candidate value of phi1 based on the length
598 // of the f1 and f2 vectors.
599 // * When |f1| != 0 and |f2| != 0, then one should choose the
600 // value of phi2 so that phi11 = phi12
601 // * When |f1| == 0 and f2 != 0, then phi1 = phi12
602 // phi11 can be ignored, and either sign of phi2 can be used
603 // * The case of |f2| == 0 can be ignored at this point in the code
604 // since having a repeated moment when |f2| == 0 implies that
605 // the matrix is diagonal. But this function returns a unit
606 // quaternion for diagonal matrices, so we can assume |f2| != 0
607 // See MassMatrix3.ipynb for a more complete discussion.
608 //
609 // Since |f2| != 0, we only need to consider |f1|
610 // * |f1| == 0: phi1 = phi12
611 // * |f1| != 0: choose phi2 so that phi11 == phi12
612 // In either case, phi1 = phi12,
613 // and the sign of phi2 must be chosen to make phi11 == phi12
614 T phi1 = phi12.Radian();
615
616 bool f1small = f1.SquaredLength() < std::pow(tol, 2);
617 if (!f1small)
618 {
619 // a: phi2 > 0
620 // eq. 5.24
621 Vector2<T> g1a(0, 0.5*momentsDiff3 * sin(2*phi2));
622 // combining eq 5.11 and 5.13, and subtracting psi1
623 // instead of multiplying by its rotation matrix:
624 math::Angle phi11a(Angle2(g1a, tol) - Angle2(f1, tol));
625 phi11a.Normalize();
626
627 // b: phi2 < 0
628 // eq. 5.24
629 Vector2<T> g1b(0, 0.5*momentsDiff3 * sin(-2*phi2));
630 // combining eq 5.11 and 5.13, and subtracting psi1
631 // instead of multiplying by its rotation matrix:
632 math::Angle phi11b(Angle2(g1b, tol) - Angle2(f1, tol));
633 phi11b.Normalize();
634
635 // choose sign of phi2
636 // based on whether phi11a or phi11b is closer to phi12
637 // use sin and cos to account for angle wrapping
638 T erra = std::pow(sin(phi1) - sin(phi11a.Radian()), 2)
639 + std::pow(cos(phi1) - cos(phi11a.Radian()), 2);
640 T errb = std::pow(sin(phi1) - sin(phi11b.Radian()), 2)
641 + std::pow(cos(phi1) - cos(phi11b.Radian()), 2);
642 if (errb < erra)
643 {
644 phi2 *= -1;
645 }
646 }
647
648 // I determined these arguments using trial and error
650
651 // Previous equations assume repeated moments are at the beginning
652 // of the moments vector (moments[0] == moments[1]).
653 // We have the vectors sorted by size, so it's possible that the
654 // repeated moments are at the end (moments[1] == moments[2]).
655 // In this case (unequalMoment == 0), we apply an extra
656 // rotation that exchanges moment[0] and moment[2]
657 // Rotation matrix = [ 0 0 1]
658 // [ 0 1 0]
659 // [-1 0 0]
660 // That is equivalent to a 90 degree pitch
661 if (unequalMoment == 0)
662 result *= Quaternion<T>(0, GZ_PI_2, 0);
663
664 return result;
665 }
666
667 // No repeated principal moments
668 // eq 5.1:
669 T v = (std::pow(this->Ixyxzyz[0], 2) + std::pow(this->Ixyxzyz[1], 2)
670 +(this->Ixxyyzz[0] - moments[2])
671 *(this->Ixxyyzz[0] + moments[2] - moments[0] - moments[1]))
672 / ((moments[1] - moments[2]) * (moments[2] - moments[0]));
673 // value of w depends on v
674 T w;
675 if (v < std::abs(tol))
676 {
677 // first sentence after eq 5.4:
678 // "In the case that v = 0, then w = 1."
679 w = 1;
680 }
681 else
682 {
683 // eq 5.2:
684 w = (this->Ixxyyzz[0] - moments[2] + (moments[2] - moments[1])*v)
685 / ((moments[0] - moments[1]) * v);
686 }
687 // initialize values of angle phi1, phi2, phi3
688 T phi1 = 0;
689 // eq 5.3: start with positive value
690 T phi2 = acos(clamp<T>(ClampedSqrt(v), -1, 1));
691 // eq 5.4: start with positive value
692 T phi3 = acos(clamp<T>(ClampedSqrt(w), -1, 1));
693
694 // compute g1, g2 for phi2,phi3 >= 0
695 // equations 5.7, 5.8
697 0.5* (moments[0]-moments[1])*ClampedSqrt(v)*sin(2*phi3),
698 0.5*((moments[0]-moments[1])*w + moments[1]-moments[2])*sin(2*phi2));
700 (moments[0]-moments[1])*(1 + (v-2)*w) + (moments[1]-moments[2])*v,
701 (moments[0]-moments[1])*sin(phi2)*sin(2*phi3));
702
703 // The paragraph prior to equation 5.16 describes how to choose
704 // the candidate value of phi1 based on the length
705 // of the f1 and f2 vectors.
706 // * The case of |f1| == |f2| == 0 implies a repeated moment,
707 // which should not be possible at this point in the code
708 // * When |f1| != 0 and |f2| != 0, then one should choose the
709 // value of phi2 so that phi11 = phi12
710 // * When |f1| == 0 and f2 != 0, then phi1 = phi12
711 // phi11 can be ignored, and either sign of phi2, phi3 can be used
712 // * When |f2| == 0 and f1 != 0, then phi1 = phi11
713 // phi12 can be ignored, and either sign of phi2, phi3 can be used
714 bool f1small = f1.SquaredLength() < std::pow(tol, 2);
715 bool f2small = f2.SquaredLength() < std::pow(tol, 2);
716 if (f1small && f2small)
717 {
718 // this should never happen
719 // f1small && f2small implies a repeated moment
720 // return invalid quaternion
722 return Quaternion<T>::Zero;
723 }
724 else if (f1small)
725 {
726 // use phi12 (equations 5.12, 5.14)
727 math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
728 phi12.Normalize();
729 phi1 = phi12.Radian();
730 }
731 else if (f2small)
732 {
733 // use phi11 (equations 5.11, 5.13)
734 math::Angle phi11(Angle2(g1, tol) - Angle2(f1, tol));
735 phi11.Normalize();
736 phi1 = phi11.Radian();
737 }
738 else
739 {
740 // check for when phi11 == phi12
741 // eqs 5.11, 5.13:
742 math::Angle phi11(Angle2(g1, tol) - Angle2(f1, tol));
743 phi11.Normalize();
744 // eqs 5.12, 5.14:
745 math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
746 phi12.Normalize();
747 T err = std::pow(sin(phi11.Radian()) - sin(phi12.Radian()), 2)
748 + std::pow(cos(phi11.Radian()) - cos(phi12.Radian()), 2);
749 phi1 = phi11.Radian();
751 // case a: phi2 <= 0
752 {
753 Vector2<T> g1a = Vector2<T>(1, -1) * g1;
754 Vector2<T> g2a = Vector2<T>(1, -1) * g2;
755 math::Angle phi11a(Angle2(g1a, tol) - Angle2(f1, tol));
756 math::Angle phi12a(0.5*(Angle2(g2a, tol) - Angle2(f2, tol)));
757 phi11a.Normalize();
758 phi12a.Normalize();
759 T erra = std::pow(sin(phi11a.Radian()) - sin(phi12a.Radian()), 2)
760 + std::pow(cos(phi11a.Radian()) - cos(phi12a.Radian()), 2);
761 if (erra < err)
762 {
763 err = erra;
764 phi1 = phi11a.Radian();
765 signsPhi23.Set(-1, 1);
766 }
767 }
768 // case b: phi3 <= 0
769 {
770 Vector2<T> g1b = Vector2<T>(-1, 1) * g1;
771 Vector2<T> g2b = Vector2<T>(1, -1) * g2;
772 math::Angle phi11b(Angle2(g1b, tol) - Angle2(f1, tol));
773 math::Angle phi12b(0.5*(Angle2(g2b, tol) - Angle2(f2, tol)));
774 phi11b.Normalize();
775 phi12b.Normalize();
776 T errb = std::pow(sin(phi11b.Radian()) - sin(phi12b.Radian()), 2)
777 + std::pow(cos(phi11b.Radian()) - cos(phi12b.Radian()), 2);
778 if (errb < err)
779 {
780 err = errb;
781 phi1 = phi11b.Radian();
782 signsPhi23.Set(1, -1);
783 }
784 }
785 // case c: phi2,phi3 <= 0
786 {
787 Vector2<T> g1c = Vector2<T>(-1, -1) * g1;
788 Vector2<T> g2c = g2;
789 math::Angle phi11c(Angle2(g1c, tol) - Angle2(f1, tol));
790 math::Angle phi12c(0.5*(Angle2(g2c, tol) - Angle2(f2, tol)));
791 phi11c.Normalize();
792 phi12c.Normalize();
793 T errc = std::pow(sin(phi11c.Radian()) - sin(phi12c.Radian()), 2)
794 + std::pow(cos(phi11c.Radian()) - cos(phi12c.Radian()), 2);
795 if (errc < err)
796 {
797 phi1 = phi11c.Radian();
798 signsPhi23.Set(-1, -1);
799 }
800 }
801
802 // apply sign changes
803 phi2 *= signsPhi23[0];
804 phi3 *= signsPhi23[1];
805 }
806
807 // I determined these arguments using trial and error
808 return Quaternion<T>(-phi1, -phi2, -phi3).Inverse();
809 }
810
822 const T _tol = 1e-6) const
823 {
824 if (!this->IsPositive(0))
825 {
826 // inertia is not positive, cannot compute equivalent box
827 return false;
828 }
829
830 Vector3<T> moments = this->PrincipalMoments(_tol);
831 if (!ValidMoments(moments))
832 {
833 // principal moments don't satisfy the triangle identity
834 return false;
835 }
836
837 // The reason for checking that the principal moments satisfy
838 // the triangle inequality
839 // I1 + I2 - I3 >= 0
840 // is to ensure that the arguments to sqrt in these equations
841 // are positive and the box size is real.
842 _size.X(sqrt(6*(moments.Y() + moments.Z() - moments.X()) / this->mass));
843 _size.Y(sqrt(6*(moments.Z() + moments.X() - moments.Y()) / this->mass));
844 _size.Z(sqrt(6*(moments.X() + moments.Y() - moments.Z()) / this->mass));
845
846 _rot = this->PrincipalAxesOffset(_tol);
847
849 {
850 // _rot is an invalid quaternion
852 return false;
853 }
854
855 return true;
856 }
857
864 public: bool SetFromBox(const Material &_mat,
865 const Vector3<T> &_size,
867 {
868 T volume = _size.X() * _size.Y() * _size.Z();
869 return this->SetFromBox(_mat.Density() * volume, _size, _rot);
870 }
871
877 public: bool SetFromBox(const T _mass,
878 const Vector3<T> &_size,
880 {
881 // Check that _mass and _size are strictly positive
882 // and that quaternion is valid
883 if (_mass <= 0 || _size.Min() <= 0 || _rot == Quaternion<T>::Zero)
884 {
885 return false;
886 }
887 this->SetMass(_mass);
888 return this->SetFromBox(_size, _rot);
889 }
890
896 public: bool SetFromBox(const Vector3<T> &_size,
898 {
899 // Check that _mass and _size are strictly positive
900 // and that quaternion is valid
901 if (this->Mass() <= 0 || _size.Min() <= 0 ||
903 {
904 return false;
905 }
906
907 // Diagonal matrix L with principal moments
909 T x2 = std::pow(_size.X(), 2);
910 T y2 = std::pow(_size.Y(), 2);
911 T z2 = std::pow(_size.Z(), 2);
912 L(0, 0) = this->mass / 12.0 * (y2 + z2);
913 L(1, 1) = this->mass / 12.0 * (z2 + x2);
914 L(2, 2) = this->mass / 12.0 * (x2 + y2);
915 Matrix3<T> R(_rot);
916 return this->SetMoi(R * L * R.Transposed());
917 }
918
927 public: bool SetFromConeZ(const Material &_mat,
928 const T _length,
929 const T _radius,
930 const Quaternion<T> &_rot =
932 {
933 // Check that density, _radius and _length are strictly positive
934 // and that quaternion is valid
935 if (_mat.Density() <= 0 || _length <= 0 || _radius <= 0 ||
937 {
938 return false;
939 }
940 T volume = GZ_PI * _radius * _radius * _length / 3.0;
941 return this->SetFromConeZ(_mat.Density() * volume,
943 }
944
952 public: bool SetFromConeZ(const T _mass,
953 const T _length,
954 const T _radius,
955 const Quaternion<T> &_rot =
957 {
958 // Check that _mass, _radius and _length are strictly positive
959 // and that quaternion is valid
960 if (_mass <= 0 || _length <= 0 || _radius <= 0 ||
962 {
963 return false;
964 }
965 this->SetMass(_mass);
966 return this->SetFromConeZ(_length, _radius, _rot);
967 }
968
975 public: bool SetFromConeZ(const T _length,
976 const T _radius,
977 const Quaternion<T> &_rot)
978 {
979 // Check that _mass and _size are strictly positive
980 // and that quaternion is valid
981 if (this->Mass() <= 0 || _length <= 0 || _radius <= 0 ||
983 {
984 return false;
985 }
986
987 // Diagonal matrix L with principal moments
988 T radius2 = std::pow(_radius, 2);
990 L(0, 0) = 3.0 * this->mass * (4.0 * radius2 +
991 std::pow(_length, 2)) / 80.0;
992 L(1, 1) = L(0, 0);
993 L(2, 2) = 3.0 * this->mass * radius2 / 10.0;
994 Matrix3<T> R(_rot);
995 return this->SetMoi(R * L * R.Transposed());
996 }
997
1006 public: bool SetFromCylinderZ(const Material &_mat,
1007 const T _length,
1008 const T _radius,
1010 {
1011 // Check that density, _radius and _length are strictly positive
1012 // and that quaternion is valid
1013 if (_mat.Density() <= 0 || _length <= 0 || _radius <= 0 ||
1015 {
1016 return false;
1017 }
1019 return this->SetFromCylinderZ(_mat.Density() * volume,
1020 _length, _radius, _rot);
1021 }
1022
1030 public: bool SetFromCylinderZ(const T _mass,
1031 const T _length,
1032 const T _radius,
1034 {
1035 // Check that _mass, _radius and _length are strictly positive
1036 // and that quaternion is valid
1037 if (_mass <= 0 || _length <= 0 || _radius <= 0 ||
1039 {
1040 return false;
1041 }
1042 this->SetMass(_mass);
1043 return this->SetFromCylinderZ(_length, _radius, _rot);
1044 }
1045
1052 public: bool SetFromCylinderZ(const T _length,
1053 const T _radius,
1054 const Quaternion<T> &_rot)
1055 {
1056 // Check that _mass and _size are strictly positive
1057 // and that quaternion is valid
1058 if (this->Mass() <= 0 || _length <= 0 || _radius <= 0 ||
1060 {
1061 return false;
1062 }
1063
1064 // Diagonal matrix L with principal moments
1065 T radius2 = std::pow(_radius, 2);
1066 Matrix3<T> L;
1067 L(0, 0) = this->mass / 12.0 * (3*radius2 + std::pow(_length, 2));
1068 L(1, 1) = L(0, 0);
1069 L(2, 2) = this->mass / 2.0 * radius2;
1070 Matrix3<T> R(_rot);
1071 return this->SetMoi(R * L * R.Transposed());
1072 }
1073
1080 public: bool SetFromSphere(const Material &_mat, const T _radius)
1081 {
1082 // Check that the density and _radius are strictly positive
1083 if (_mat.Density() <= 0 || _radius <= 0)
1084 {
1085 return false;
1086 }
1087
1088 T volume = (4.0/3.0) * GZ_PI * std::pow(_radius, 3);
1089 return this->SetFromSphere(_mat.Density() * volume, _radius);
1090 }
1091
1096 public: bool SetFromSphere(const T _mass, const T _radius)
1097 {
1098 // Check that _mass and _radius are strictly positive
1099 if (_mass <= 0 || _radius <= 0)
1100 {
1101 return false;
1102 }
1103 this->SetMass(_mass);
1104 return this->SetFromSphere(_radius);
1105 }
1106
1111 public: bool SetFromSphere(const T _radius)
1112 {
1113 // Check that _mass and _radius are strictly positive
1114 if (this->Mass() <= 0 || _radius <= 0)
1115 {
1116 return false;
1117 }
1118
1119 // Diagonal matrix L with principal moments
1120 T radius2 = std::pow(_radius, 2);
1121 Matrix3<T> L;
1122 L(0, 0) = 0.4 * this->mass * radius2;
1123 L(1, 1) = 0.4 * this->mass * radius2;
1124 L(2, 2) = 0.4 * this->mass * radius2;
1125 return this->SetMoi(L);
1126 }
1127
1131 private: static inline T ClampedSqrt(const T &_x)
1132 {
1133 if (_x <= 0)
1134 return 0;
1135 return sqrt(_x);
1136 }
1137
1143 private: static T Angle2(const Vector2<T> &_v, const T _eps = 1e-6)
1144 {
1145 if (_v.SquaredLength() < std::pow(_eps, 2))
1146 return 0;
1147 return atan2(_v[1], _v[0]);
1148 }
1149
1151 private: T mass;
1152
1156 private: Vector3<T> Ixxyyzz;
1157
1161 private: Vector3<T> Ixyxzyz;
1162 };
1163
1166 } // namespace GZ_MATH_VERSION_NAMESPACE
1167} // namespace gz::math
1168#endif // GZ_MATH_MASSMATRIX3_HH_