Gazebo Math

API Reference

7.4.0
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
33namespace gz
34{
35 namespace math
36 {
37 // Inline bracket to help doxygen filtering.
38 inline namespace GZ_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) = default;
65
67 public: ~MassMatrix3() = default;
68
72 public: bool SetMass(const T &_m)
73 {
74 this->mass = _m;
75 return this->IsValid();
76 }
77
80 public: T Mass() const
81 {
82 return this->mass;
83 }
84
93 public: bool SetInertiaMatrix(
94 const T &_ixx, const T &_iyy, const T &_izz,
95 const T &_ixy, const T &_ixz, const T &_iyz)
96 {
97 this->Ixxyyzz.Set(_ixx, _iyy, _izz);
98 this->Ixyxzyz.Set(_ixy, _ixz, _iyz);
99 return this->IsValid();
100 }
101
105 {
106 return this->Ixxyyzz;
107 }
108
112 {
113 return this->Ixyxzyz;
114 }
115
119 public: bool SetDiagonalMoments(const Vector3<T> &_ixxyyzz)
120 {
121 this->Ixxyyzz = _ixxyyzz;
122 return this->IsValid();
123 }
124
128 public: bool SetOffDiagonalMoments(const Vector3<T> &_ixyxzyz)
129 {
130 this->Ixyxzyz = _ixyxzyz;
131 return this->IsValid();
132 }
133
136 public: T Ixx() const
137 {
138 return this->Ixxyyzz[0];
139 }
140
143 public: T Iyy() const
144 {
145 return this->Ixxyyzz[1];
146 }
147
150 public: T Izz() const
151 {
152 return this->Ixxyyzz[2];
153 }
154
157 public: T Ixy() const
158 {
159 return this->Ixyxzyz[0];
160 }
161
164 public: T Ixz() const
165 {
166 return this->Ixyxzyz[1];
167 }
168
171 public: T Iyz() const
172 {
173 return this->Ixyxzyz[2];
174 }
175
179 public: bool SetIxx(const T &_v)
180 {
181 this->Ixxyyzz.X(_v);
182 return this->IsValid();
183 }
184
188 public: bool SetIyy(const T &_v)
189 {
190 this->Ixxyyzz.Y(_v);
191 return this->IsValid();
192 }
193
197 public: bool SetIzz(const T &_v)
198 {
199 this->Ixxyyzz.Z(_v);
200 return this->IsValid();
201 }
202
206 public: bool SetIxy(const T &_v)
207 {
208 this->Ixyxzyz.X(_v);
209 return this->IsValid();
210 }
211
215 public: bool SetIxz(const T &_v)
216 {
217 this->Ixyxzyz.Y(_v);
218 return this->IsValid();
219 }
220
224 public: bool SetIyz(const T &_v)
225 {
226 this->Ixyxzyz.Z(_v);
227 return this->IsValid();
228 }
229
232 public: Matrix3<T> Moi() const
233 {
234 return Matrix3<T>(
235 this->Ixxyyzz[0], this->Ixyxzyz[0], this->Ixyxzyz[1],
236 this->Ixyxzyz[0], this->Ixxyyzz[1], this->Ixyxzyz[2],
237 this->Ixyxzyz[1], this->Ixyxzyz[2], this->Ixxyyzz[2]);
238 }
239
245 public: bool SetMoi(const Matrix3<T> &_moi)
246 {
247 this->Ixxyyzz.Set(_moi(0, 0), _moi(1, 1), _moi(2, 2));
248 this->Ixyxzyz.Set(
249 0.5*(_moi(0, 1) + _moi(1, 0)),
250 0.5*(_moi(0, 2) + _moi(2, 0)),
251 0.5*(_moi(1, 2) + _moi(2, 1)));
252 return this->IsValid();
253 }
254
258 public: MassMatrix3 &operator=(const MassMatrix3<T> &_massMatrix)
259 = default;
260
265 public: bool operator==(const MassMatrix3<T> &_m) const
266 {
267 return equal<T>(this->mass, _m.Mass()) &&
268 (this->Ixxyyzz == _m.DiagonalMoments()) &&
269 (this->Ixyxzyz == _m.OffDiagonalMoments());
270 }
271
275 public: bool operator!=(const MassMatrix3<T> &_m) const
276 {
277 return !(*this == _m);
278 }
279
299 public: bool IsNearPositive(const T _tolerance =
300 GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>) const
301 {
302 const T epsilon = this->Epsilon(_tolerance);
303
304 // Check if mass and determinants of all upper left submatrices
305 // of moment of inertia matrix are positive
306 return (this->mass >= 0) &&
307 (this->Ixx() + epsilon >= 0) &&
308 (this->Ixx() * this->Iyy() - std::pow(this->Ixy(), 2) +
309 epsilon >= 0) &&
310 (this->Moi().Determinant() + epsilon >= 0);
311 }
312
332 public: bool IsPositive(const T _tolerance =
333 GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>) const
334 {
335 const T epsilon = this->Epsilon(_tolerance);
336
337 // Check if mass and determinants of all upper left submatrices
338 // of moment of inertia matrix are positive
339 return (this->mass > 0) &&
340 (this->Ixx() + epsilon > 0) &&
341 (this->Ixx() * this->Iyy() - std::pow(this->Ixy(), 2) +
342 epsilon > 0) &&
343 (this->Moi().Determinant() + epsilon > 0);
344 }
345
353 public: T Epsilon(const T _tolerance =
354 GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>) const
355 {
356 return Epsilon(this->DiagonalMoments(), _tolerance);
357 }
358
380 public: static T Epsilon(const Vector3<T> &_moments,
381 const T _tolerance =
382 GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>)
383 {
384 // The following was borrowed heavily from:
385 // https://github.com/RobotLocomotion/drake/blob/v0.27.0/multibody/tree/rotational_inertia.h
386
387 // Compute the maximum possible moment of inertia, which will be
388 // used to compute whether the moments are valid.
389 //
390 // The maximum moment of inertia is bounded by:
391 // trace / 3 <= maxPossibleMoi <= trace / 2.
392 //
393 // The trace of a matrix is the sum of the coefficients on the
394 // main diagonal. For a mass matrix, this is equal to
395 // ixx + iyy + izz, or _moments.Sum() for this function's
396 // implementation.
397 //
398 // It is okay if maxPossibleMoi == zero.
399 T maxPossibleMoI = 0.5 * std::abs(_moments.Sum());
400
401 // In order to check validity of the moments we need to use an
402 // epsilon value that is related to machine precision
403 // multiplied by the largest possible moment of inertia.
404 return _tolerance *
405 std::numeric_limits<T>::epsilon() * maxPossibleMoI;
406 }
407
419 public: bool IsValid(const T _tolerance =
420 GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>) const
421 {
422 return this->IsNearPositive(_tolerance) &&
423 ValidMoments(this->PrincipalMoments(), _tolerance);
424 }
425
446 public: static bool ValidMoments(const Vector3<T> &_moments,
447 const T _tolerance = GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>)
448 {
449 T epsilon = Epsilon(_moments, _tolerance);
450
451 return _moments[0] + epsilon >= 0 &&
452 _moments[1] + epsilon >= 0 &&
453 _moments[2] + epsilon >= 0 &&
454 _moments[0] + _moments[1] + epsilon >= _moments[2] &&
455 _moments[1] + _moments[2] + epsilon >= _moments[0] &&
456 _moments[2] + _moments[0] + epsilon >= _moments[1];
457 }
458
470 public: Vector3<T> PrincipalMoments(const T _tol = 1e-6) const
471 {
472 // Compute tolerance relative to maximum value of inertia diagonal
473 T tol = _tol * this->Ixxyyzz.Max();
474 if (this->Ixyxzyz.Equal(Vector3<T>::Zero, tol))
475 {
476 // Matrix is already diagonalized, return diagonal moments
477 return this->Ixxyyzz;
478 }
479
480 // Algorithm based on http://arxiv.org/abs/1306.6291v4
481 // A Method for Fast Diagonalization of a 2x2 or 3x3 Real Symmetric
482 // Matrix, by Maarten Kronenburg
483 Vector3<T> Id(this->Ixxyyzz);
484 Vector3<T> Ip(this->Ixyxzyz);
485 // b = Ixx + Iyy + Izz
486 T b = Id.Sum();
487 // c = Ixx*Iyy - Ixy^2 + Ixx*Izz - Ixz^2 + Iyy*Izz - Iyz^2
488 T c = Id[0]*Id[1] - std::pow(Ip[0], 2)
489 + Id[0]*Id[2] - std::pow(Ip[1], 2)
490 + Id[1]*Id[2] - std::pow(Ip[2], 2);
491 // d = Ixx*Iyz^2 + Iyy*Ixz^2 + Izz*Ixy^2 - Ixx*Iyy*Izz - 2*Ixy*Ixz*Iyz
492 T d = Id[0]*std::pow(Ip[2], 2)
493 + Id[1]*std::pow(Ip[1], 2)
494 + Id[2]*std::pow(Ip[0], 2)
495 - Id[0]*Id[1]*Id[2]
496 - 2*Ip[0]*Ip[1]*Ip[2];
497 // p = b^2 - 3c
498 T p = std::pow(b, 2) - 3*c;
499
500 // At this point, it is important to check that p is not close
501 // to zero, since its inverse is used to compute delta.
502 // In equation 4.7, p is expressed as a sum of squares
503 // that is only zero if the matrix is diagonal
504 // with identical principal moments.
505 // This check has no test coverage, since this function returns
506 // immediately if a diagonal matrix is detected.
507 if (p < std::pow(tol, 2))
508 return b / 3.0 * Vector3<T>::One;
509
510 // q = 2b^3 - 9bc - 27d
511 T q = 2*std::pow(b, 3) - 9*b*c - 27*d;
512
513 // delta = acos(q / (2 * p^(1.5)))
514 // additionally clamp the argument to [-1,1]
515 T delta = acos(clamp<T>(0.5 * q / std::pow(p, 1.5), -1, 1));
516
517 // sort the moments from smallest to largest
518 T moment0 = (b + 2*sqrt(p) * cos(delta / 3.0)) / 3.0;
519 T moment1 = (b + 2*sqrt(p) * cos((delta + 2*GZ_PI)/3.0)) / 3.0;
520 T moment2 = (b + 2*sqrt(p) * cos((delta - 2*GZ_PI)/3.0)) / 3.0;
521 sort3(moment0, moment1, moment2);
522 return Vector3<T>(moment0, moment1, moment2);
523 }
524
536 public: Quaternion<T> PrincipalAxesOffset(const T _tol = 1e-6) const
537 {
538 Vector3<T> moments = this->PrincipalMoments(_tol);
539 // Compute tolerance relative to maximum value of inertia diagonal
540 T tol = _tol * this->Ixxyyzz.Max();
541 if (moments.Equal(this->Ixxyyzz, tol) ||
542 (math::equal<T>(moments[0], moments[1], std::abs(tol)) &&
543 math::equal<T>(moments[0], moments[2], std::abs(tol))))
544 {
545 // matrix is already aligned with principal axes
546 // or all three moments are approximately equal
547 // return identity rotation
549 }
550
551 // Algorithm based on http://arxiv.org/abs/1306.6291v4
552 // A Method for Fast Diagonalization of a 2x2 or 3x3 Real Symmetric
553 // Matrix, by Maarten Kronenburg
554 // A real, symmetric matrix can be diagonalized by an orthogonal matrix
555 // (due to the finite-dimensional spectral theorem
556 // https://en.wikipedia.org/wiki/Spectral_theorem
557 // #Hermitian_maps_and_Hermitian_matrices ),
558 // and another name for orthogonal matrix is rotation matrix.
559 // Section 5 of the paper shows how to compute Euler angles
560 // phi1, phi2, and phi3 that map to a rotation matrix.
561 // In some cases, there are multiple possible values for a given angle,
562 // such as phi1, that are denoted as phi11, phi12, phi11a, phi12a, etc.
563 // Similar variable names are used to the paper so that the paper
564 // can be used as an additional reference.
565
566 // f1, f2 defined in equations 5.5, 5.6
567 Vector2<T> f1(this->Ixyxzyz[0], -this->Ixyxzyz[1]);
568 Vector2<T> f2(this->Ixxyyzz[1] - this->Ixxyyzz[2],
569 -2*this->Ixyxzyz[2]);
570
571 // Check if two moments are equal, since different equations are used
572 // The moments vector is already sorted, so just check adjacent values.
573 Vector2<T> momentsDiff(moments[0] - moments[1],
574 moments[1] - moments[2]);
575
576 // index of unequal moment
577 int unequalMoment = -1;
578 if (equal<T>(momentsDiff[0], 0, std::abs(tol)))
579 unequalMoment = 2;
580 else if (equal<T>(momentsDiff[1], 0, std::abs(tol)))
581 unequalMoment = 0;
582
583 if (unequalMoment >= 0)
584 {
585 // moments[1] is the repeated value
586 // it is not equal to moments[unequalMoment]
587 // momentsDiff3 = lambda - lambda3
588 T momentsDiff3 = moments[1] - moments[unequalMoment];
589 // eq 5.21:
590 // s = cos(phi2)^2 = (A_11 - lambda3) / (lambda - lambda3)
591 // s >= 0 since A_11 is in range [lambda, lambda3]
592 T s = (this->Ixxyyzz[0] - moments[unequalMoment]) / momentsDiff3;
593 // set phi3 to zero for repeated moments (eq 5.23)
594 T phi3 = 0;
595 // phi2 = +- acos(sqrt(s))
596 // start with just the positive value
597 // also clamp the acos argument to prevent NaN's
598 T phi2 = acos(clamp<T>(ClampedSqrt(s), -1, 1));
599
600 // The paper defines variables phi11 and phi12
601 // which are candidate values of angle phi1.
602 // phi12 is straightforward to compute as a function of f2 and g2.
603 // eq 5.25:
604 Vector2<T> g2(momentsDiff3 * s, 0);
605 // combining eq 5.12 and 5.14, and subtracting psi2
606 // instead of multiplying by its rotation matrix:
607 math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
608 phi12.Normalize();
609
610 // The paragraph prior to equation 5.16 describes how to choose
611 // the candidate value of phi1 based on the length
612 // of the f1 and f2 vectors.
613 // * When |f1| != 0 and |f2| != 0, then one should choose the
614 // value of phi2 so that phi11 = phi12
615 // * When |f1| == 0 and f2 != 0, then phi1 = phi12
616 // phi11 can be ignored, and either sign of phi2 can be used
617 // * The case of |f2| == 0 can be ignored at this point in the code
618 // since having a repeated moment when |f2| == 0 implies that
619 // the matrix is diagonal. But this function returns a unit
620 // quaternion for diagonal matrices, so we can assume |f2| != 0
621 // See MassMatrix3.ipynb for a more complete discussion.
622 //
623 // Since |f2| != 0, we only need to consider |f1|
624 // * |f1| == 0: phi1 = phi12
625 // * |f1| != 0: choose phi2 so that phi11 == phi12
626 // In either case, phi1 = phi12,
627 // and the sign of phi2 must be chosen to make phi11 == phi12
628 T phi1 = phi12.Radian();
629
630 bool f1small = f1.SquaredLength() < std::pow(tol, 2);
631 if (!f1small)
632 {
633 // a: phi2 > 0
634 // eq. 5.24
635 Vector2<T> g1a(0, 0.5*momentsDiff3 * sin(2*phi2));
636 // combining eq 5.11 and 5.13, and subtracting psi1
637 // instead of multiplying by its rotation matrix:
638 math::Angle phi11a(Angle2(g1a, tol) - Angle2(f1, tol));
639 phi11a.Normalize();
640
641 // b: phi2 < 0
642 // eq. 5.24
643 Vector2<T> g1b(0, 0.5*momentsDiff3 * sin(-2*phi2));
644 // combining eq 5.11 and 5.13, and subtracting psi1
645 // instead of multiplying by its rotation matrix:
646 math::Angle phi11b(Angle2(g1b, tol) - Angle2(f1, tol));
647 phi11b.Normalize();
648
649 // choose sign of phi2
650 // based on whether phi11a or phi11b is closer to phi12
651 // use sin and cos to account for angle wrapping
652 T erra = std::pow(sin(phi1) - sin(phi11a.Radian()), 2)
653 + std::pow(cos(phi1) - cos(phi11a.Radian()), 2);
654 T errb = std::pow(sin(phi1) - sin(phi11b.Radian()), 2)
655 + std::pow(cos(phi1) - cos(phi11b.Radian()), 2);
656 if (errb < erra)
657 {
658 phi2 *= -1;
659 }
660 }
661
662 // I determined these arguments using trial and error
663 Quaternion<T> result = Quaternion<T>(-phi1, -phi2, -phi3).Inverse();
664
665 // Previous equations assume repeated moments are at the beginning
666 // of the moments vector (moments[0] == moments[1]).
667 // We have the vectors sorted by size, so it's possible that the
668 // repeated moments are at the end (moments[1] == moments[2]).
669 // In this case (unequalMoment == 0), we apply an extra
670 // rotation that exchanges moment[0] and moment[2]
671 // Rotation matrix = [ 0 0 1]
672 // [ 0 1 0]
673 // [-1 0 0]
674 // That is equivalent to a 90 degree pitch
675 if (unequalMoment == 0)
676 result *= Quaternion<T>(0, GZ_PI_2, 0);
677
678 return result;
679 }
680
681 // No repeated principal moments
682 // eq 5.1:
683 T v = (std::pow(this->Ixyxzyz[0], 2) + std::pow(this->Ixyxzyz[1], 2)
684 +(this->Ixxyyzz[0] - moments[2])
685 *(this->Ixxyyzz[0] + moments[2] - moments[0] - moments[1]))
686 / ((moments[1] - moments[2]) * (moments[2] - moments[0]));
687 // value of w depends on v
688 T w;
689 if (v < std::abs(tol))
690 {
691 // first sentence after eq 5.4:
692 // "In the case that v = 0, then w = 1."
693 w = 1;
694 }
695 else
696 {
697 // eq 5.2:
698 w = (this->Ixxyyzz[0] - moments[2] + (moments[2] - moments[1])*v)
699 / ((moments[0] - moments[1]) * v);
700 }
701 // initialize values of angle phi1, phi2, phi3
702 T phi1 = 0;
703 // eq 5.3: start with positive value
704 T phi2 = acos(clamp<T>(ClampedSqrt(v), -1, 1));
705 // eq 5.4: start with positive value
706 T phi3 = acos(clamp<T>(ClampedSqrt(w), -1, 1));
707
708 // compute g1, g2 for phi2,phi3 >= 0
709 // equations 5.7, 5.8
710 Vector2<T> g1(
711 0.5* (moments[0]-moments[1])*ClampedSqrt(v)*sin(2*phi3),
712 0.5*((moments[0]-moments[1])*w + moments[1]-moments[2])*sin(2*phi2));
713 Vector2<T> g2(
714 (moments[0]-moments[1])*(1 + (v-2)*w) + (moments[1]-moments[2])*v,
715 (moments[0]-moments[1])*sin(phi2)*sin(2*phi3));
716
717 // The paragraph prior to equation 5.16 describes how to choose
718 // the candidate value of phi1 based on the length
719 // of the f1 and f2 vectors.
720 // * The case of |f1| == |f2| == 0 implies a repeated moment,
721 // which should not be possible at this point in the code
722 // * When |f1| != 0 and |f2| != 0, then one should choose the
723 // value of phi2 so that phi11 = phi12
724 // * When |f1| == 0 and f2 != 0, then phi1 = phi12
725 // phi11 can be ignored, and either sign of phi2, phi3 can be used
726 // * When |f2| == 0 and f1 != 0, then phi1 = phi11
727 // phi12 can be ignored, and either sign of phi2, phi3 can be used
728 bool f1small = f1.SquaredLength() < std::pow(tol, 2);
729 bool f2small = f2.SquaredLength() < std::pow(tol, 2);
730 if (f1small && f2small)
731 {
732 // this should never happen
733 // f1small && f2small implies a repeated moment
734 // return invalid quaternion
736 return Quaternion<T>::Zero;
737 }
738 else if (f1small)
739 {
740 // use phi12 (equations 5.12, 5.14)
741 math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
742 phi12.Normalize();
743 phi1 = phi12.Radian();
744 }
745 else if (f2small)
746 {
747 // use phi11 (equations 5.11, 5.13)
748 math::Angle phi11(Angle2(g1, tol) - Angle2(f1, tol));
749 phi11.Normalize();
750 phi1 = phi11.Radian();
751 }
752 else
753 {
754 // check for when phi11 == phi12
755 // eqs 5.11, 5.13:
756 math::Angle phi11(Angle2(g1, tol) - Angle2(f1, tol));
757 phi11.Normalize();
758 // eqs 5.12, 5.14:
759 math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
760 phi12.Normalize();
761 T err = std::pow(sin(phi11.Radian()) - sin(phi12.Radian()), 2)
762 + std::pow(cos(phi11.Radian()) - cos(phi12.Radian()), 2);
763 phi1 = phi11.Radian();
764 math::Vector2<T> signsPhi23(1, 1);
765 // case a: phi2 <= 0
766 {
767 Vector2<T> g1a = Vector2<T>(1, -1) * g1;
768 Vector2<T> g2a = Vector2<T>(1, -1) * g2;
769 math::Angle phi11a(Angle2(g1a, tol) - Angle2(f1, tol));
770 math::Angle phi12a(0.5*(Angle2(g2a, tol) - Angle2(f2, tol)));
771 phi11a.Normalize();
772 phi12a.Normalize();
773 T erra = std::pow(sin(phi11a.Radian()) - sin(phi12a.Radian()), 2)
774 + std::pow(cos(phi11a.Radian()) - cos(phi12a.Radian()), 2);
775 if (erra < err)
776 {
777 err = erra;
778 phi1 = phi11a.Radian();
779 signsPhi23.Set(-1, 1);
780 }
781 }
782 // case b: phi3 <= 0
783 {
784 Vector2<T> g1b = Vector2<T>(-1, 1) * g1;
785 Vector2<T> g2b = Vector2<T>(1, -1) * g2;
786 math::Angle phi11b(Angle2(g1b, tol) - Angle2(f1, tol));
787 math::Angle phi12b(0.5*(Angle2(g2b, tol) - Angle2(f2, tol)));
788 phi11b.Normalize();
789 phi12b.Normalize();
790 T errb = std::pow(sin(phi11b.Radian()) - sin(phi12b.Radian()), 2)
791 + std::pow(cos(phi11b.Radian()) - cos(phi12b.Radian()), 2);
792 if (errb < err)
793 {
794 err = errb;
795 phi1 = phi11b.Radian();
796 signsPhi23.Set(1, -1);
797 }
798 }
799 // case c: phi2,phi3 <= 0
800 {
801 Vector2<T> g1c = Vector2<T>(-1, -1) * g1;
802 Vector2<T> g2c = g2;
803 math::Angle phi11c(Angle2(g1c, tol) - Angle2(f1, tol));
804 math::Angle phi12c(0.5*(Angle2(g2c, tol) - Angle2(f2, tol)));
805 phi11c.Normalize();
806 phi12c.Normalize();
807 T errc = std::pow(sin(phi11c.Radian()) - sin(phi12c.Radian()), 2)
808 + std::pow(cos(phi11c.Radian()) - cos(phi12c.Radian()), 2);
809 if (errc < err)
810 {
811 phi1 = phi11c.Radian();
812 signsPhi23.Set(-1, -1);
813 }
814 }
815
816 // apply sign changes
817 phi2 *= signsPhi23[0];
818 phi3 *= signsPhi23[1];
819 }
820
821 // I determined these arguments using trial and error
822 return Quaternion<T>(-phi1, -phi2, -phi3).Inverse();
823 }
824
834 public: bool EquivalentBox(Vector3<T> &_size,
835 Quaternion<T> &_rot,
836 const T _tol = 1e-6) const
837 {
838 if (!this->IsPositive(0))
839 {
840 // inertia is not positive, cannot compute equivalent box
841 return false;
842 }
843
844 Vector3<T> moments = this->PrincipalMoments(_tol);
845 if (!ValidMoments(moments))
846 {
847 // principal moments don't satisfy the triangle identity
848 return false;
849 }
850
851 // The reason for checking that the principal moments satisfy
852 // the triangle inequality
853 // I1 + I2 - I3 >= 0
854 // is to ensure that the arguments to sqrt in these equations
855 // are positive and the box size is real.
856 _size.X(sqrt(6*(moments.Y() + moments.Z() - moments.X()) / this->mass));
857 _size.Y(sqrt(6*(moments.Z() + moments.X() - moments.Y()) / this->mass));
858 _size.Z(sqrt(6*(moments.X() + moments.Y() - moments.Z()) / this->mass));
859
860 _rot = this->PrincipalAxesOffset(_tol);
861
862 if (_rot == Quaternion<T>::Zero)
863 {
864 // _rot is an invalid quaternion
866 return false;
867 }
868
869 return true;
870 }
871
878 public: bool SetFromBox(const Material &_mat,
879 const Vector3<T> &_size,
881 {
882 T volume = _size.X() * _size.Y() * _size.Z();
883 return this->SetFromBox(_mat.Density() * volume, _size, _rot);
884 }
885
891 public: bool SetFromBox(const T _mass,
892 const Vector3<T> &_size,
894 {
895 // Check that _mass and _size are strictly positive
896 // and that quaternion is valid
897 if (_mass <= 0 || _size.Min() <= 0 || _rot == Quaternion<T>::Zero)
898 {
899 return false;
900 }
901 this->SetMass(_mass);
902 return this->SetFromBox(_size, _rot);
903 }
904
910 public: bool SetFromBox(const Vector3<T> &_size,
912 {
913 // Check that _mass and _size are strictly positive
914 // and that quaternion is valid
915 if (this->Mass() <= 0 || _size.Min() <= 0 ||
916 _rot == Quaternion<T>::Zero)
917 {
918 return false;
919 }
920
921 // Diagonal matrix L with principal moments
922 Matrix3<T> L;
923 T x2 = std::pow(_size.X(), 2);
924 T y2 = std::pow(_size.Y(), 2);
925 T z2 = std::pow(_size.Z(), 2);
926 L(0, 0) = this->mass / 12.0 * (y2 + z2);
927 L(1, 1) = this->mass / 12.0 * (z2 + x2);
928 L(2, 2) = this->mass / 12.0 * (x2 + y2);
929 Matrix3<T> R(_rot);
930 return this->SetMoi(R * L * R.Transposed());
931 }
932
941 public: bool SetFromCylinderZ(const Material &_mat,
942 const T _length,
943 const T _radius,
945 {
946 // Check that density, _radius and _length are strictly positive
947 // and that quaternion is valid
948 if (_mat.Density() <= 0 || _length <= 0 || _radius <= 0 ||
949 _rot == Quaternion<T>::Zero)
950 {
951 return false;
952 }
953 T volume = GZ_PI * _radius * _radius * _length;
954 return this->SetFromCylinderZ(_mat.Density() * volume,
955 _length, _radius, _rot);
956 }
957
965 public: bool SetFromCylinderZ(const T _mass,
966 const T _length,
967 const T _radius,
969 {
970 // Check that _mass, _radius and _length are strictly positive
971 // and that quaternion is valid
972 if (_mass <= 0 || _length <= 0 || _radius <= 0 ||
973 _rot == Quaternion<T>::Zero)
974 {
975 return false;
976 }
977 this->SetMass(_mass);
978 return this->SetFromCylinderZ(_length, _radius, _rot);
979 }
980
987 public: bool SetFromCylinderZ(const T _length,
988 const T _radius,
989 const Quaternion<T> &_rot)
990 {
991 // Check that _mass and _size are strictly positive
992 // and that quaternion is valid
993 if (this->Mass() <= 0 || _length <= 0 || _radius <= 0 ||
994 _rot == Quaternion<T>::Zero)
995 {
996 return false;
997 }
998
999 // Diagonal matrix L with principal moments
1000 T radius2 = std::pow(_radius, 2);
1001 Matrix3<T> L;
1002 L(0, 0) = this->mass / 12.0 * (3*radius2 + std::pow(_length, 2));
1003 L(1, 1) = L(0, 0);
1004 L(2, 2) = this->mass / 2.0 * radius2;
1005 Matrix3<T> R(_rot);
1006 return this->SetMoi(R * L * R.Transposed());
1007 }
1008
1015 public: bool SetFromSphere(const Material &_mat, const T _radius)
1016 {
1017 // Check that the density and _radius are strictly positive
1018 if (_mat.Density() <= 0 || _radius <= 0)
1019 {
1020 return false;
1021 }
1022
1023 T volume = (4.0/3.0) * GZ_PI * std::pow(_radius, 3);
1024 return this->SetFromSphere(_mat.Density() * volume, _radius);
1025 }
1026
1031 public: bool SetFromSphere(const T _mass, const T _radius)
1032 {
1033 // Check that _mass and _radius are strictly positive
1034 if (_mass <= 0 || _radius <= 0)
1035 {
1036 return false;
1037 }
1038 this->SetMass(_mass);
1039 return this->SetFromSphere(_radius);
1040 }
1041
1046 public: bool SetFromSphere(const T _radius)
1047 {
1048 // Check that _mass and _radius are strictly positive
1049 if (this->Mass() <= 0 || _radius <= 0)
1050 {
1051 return false;
1052 }
1053
1054 // Diagonal matrix L with principal moments
1055 T radius2 = std::pow(_radius, 2);
1056 Matrix3<T> L;
1057 L(0, 0) = 0.4 * this->mass * radius2;
1058 L(1, 1) = 0.4 * this->mass * radius2;
1059 L(2, 2) = 0.4 * this->mass * radius2;
1060 return this->SetMoi(L);
1061 }
1062
1066 private: static inline T ClampedSqrt(const T &_x)
1067 {
1068 if (_x <= 0)
1069 return 0;
1070 return sqrt(_x);
1071 }
1072
1078 private: static T Angle2(const Vector2<T> &_v, const T _eps = 1e-6)
1079 {
1080 if (_v.SquaredLength() < std::pow(_eps, 2))
1081 return 0;
1082 return atan2(_v[1], _v[0]);
1083 }
1084
1086 private: T mass;
1087
1091 private: Vector3<T> Ixxyyzz;
1092
1096 private: Vector3<T> Ixyxzyz;
1097 };
1098
1101 }
1102 }
1103}
1104#endif