Gazebo Math
API Reference
8.0.0
insert_drive_file
Tutorials
library_books
Classes
toc
Namespaces
insert_drive_file
Files
launch
Gazebo Website
Index
List
Hierarchy
Members: All
Members: Functions
Members: Variables
Members: Typedefs
Members: Enumerations
Members: Enumerator
List
Members
Functions
Typedefs
Variables
Enumerations
Enumerator
src
gz-math
include
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 <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
34
namespace
gz::math
35
{
36
// Inline bracket to help doxygen filtering.
37
inline
namespace
GZ_MATH_VERSION_NAMESPACE {
38
//
43
template
<
typename
T>
44
class
MassMatrix3
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
96
public
:
Vector3<T>
DiagonalMoments
()
const
97
{
98
return
this->Ixxyyzz;
99
}
100
103
public
:
Vector3<T>
OffDiagonalMoments
()
const
104
{
105
return
this->Ixyxzyz;
106
}
107
111
public
:
bool
SetDiagonalMoments
(
const
Vector3<T>
&
_ixxyyzz
)
112
{
113
this->Ixxyyzz =
_ixxyyzz
;
114
return
this->IsValid();
115
}
116
120
public
:
bool
SetOffDiagonalMoments
(
const
Vector3<T>
&
_ixyxzyz
)
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
=
286
GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>
)
const
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
=
319
GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>
)
const
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
=
340
GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>
)
const
341
{
342
return
Epsilon(this->DiagonalMoments(),
_tolerance
);
343
}
344
366
public
:
static
T
Epsilon
(
const
Vector3<T>
&
_moments
,
367
const
T
_tolerance
=
368
GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>
)
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
*
391
std::numeric_limits<T>::epsilon
() *
maxPossibleMoI
;
392
}
393
405
public
:
bool
IsValid
(
const
T
_tolerance
=
406
GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>
)
const
407
{
408
return
this->IsNearPositive(
_tolerance
) &&
409
ValidMoments(this->PrincipalMoments(),
_tolerance
);
410
}
411
432
public
:
static
bool
ValidMoments
(
const
Vector3<T>
&
_moments
,
433
const
T
_tolerance
=
GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>
)
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
= 1
e
-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;
507
sort3
(
moment0
,
moment1
,
moment2
);
508
return
Vector3<T>
(
moment0
,
moment1
,
moment2
);
509
}
510
522
public
:
Quaternion<T>
PrincipalAxesOffset
(
const
T
_tol
= 1
e
-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
534
return
Quaternion<T>::Identity
;
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.
559
Vector2<T>
momentsDiff
(
moments
[0] -
moments
[1],
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
574
T
momentsDiff3
=
moments
[1] -
moments
[
unequalMoment
];
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
649
Quaternion<T>
result
=
Quaternion<T>
(-
phi1
, -
phi2
, -
phi3
).Inverse();
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
696
Vector2<T>
g1
(
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
));
699
Vector2<T>
g2
(
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();
750
math::Vector2<T>
signsPhi23
(1, 1);
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
820
public
:
bool
EquivalentBox
(
Vector3<T>
&
_size
,
821
Quaternion<T>
&
_rot
,
822
const
T
_tol
= 1
e
-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
848
if
(
_rot
==
Quaternion<T>::Zero
)
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
,
866
const
Quaternion<T>
&
_rot
=
Quaternion<T>::Identity
)
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
,
879
const
Quaternion<T>
&
_rot
=
Quaternion<T>::Identity
)
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
,
897
const
Quaternion<T>
&
_rot
=
Quaternion<T>::Identity
)
898
{
899
// Check that _mass and _size are strictly positive
900
// and that quaternion is valid
901
if
(this->Mass() <= 0 ||
_size
.Min() <= 0 ||
902
_rot
==
Quaternion<T>::Zero
)
903
{
904
return
false
;
905
}
906
907
// Diagonal matrix L with principal moments
908
Matrix3<T>
L
;
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
=
931
Quaternion<T>::Identity
)
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 ||
936
_rot
==
Quaternion<T>::Zero
)
937
{
938
return
false
;
939
}
940
T
volume
=
GZ_PI
*
_radius
*
_radius
*
_length
/ 3.0;
941
return
this->SetFromConeZ(
_mat
.Density() *
volume
,
942
_length
,
_radius
,
_rot
);
943
}
944
952
public
:
bool
SetFromConeZ
(
const
T
_mass
,
953
const
T
_length
,
954
const
T
_radius
,
955
const
Quaternion<T>
&
_rot
=
956
Quaternion<T>::Identity
)
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 ||
961
_rot
==
Quaternion<T>::Zero
)
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 ||
982
_rot
==
Quaternion<T>::Zero
)
983
{
984
return
false
;
985
}
986
987
// Diagonal matrix L with principal moments
988
T
radius2
=
std::pow
(
_radius
, 2);
989
Matrix3<T>
L
;
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
,
1009
const
Quaternion<T>
&
_rot
=
Quaternion<T>::Identity
)
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 ||
1014
_rot
==
Quaternion<T>::Zero
)
1015
{
1016
return
false
;
1017
}
1018
T
volume
=
GZ_PI
*
_radius
*
_radius
*
_length
;
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
,
1033
const
Quaternion<T>
&
_rot
=
Quaternion<T>::Identity
)
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 ||
1038
_rot
==
Quaternion<T>::Zero
)
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 ||
1059
_rot
==
Quaternion<T>::Zero
)
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
1164
typedef
MassMatrix3<double>
MassMatrix3d
;
1165
typedef
MassMatrix3<float>
MassMatrix3f
;
1166
}
// namespace GZ_MATH_VERSION_NAMESPACE
1167
}
// namespace gz::math
1168
#endif
// GZ_MATH_MASSMATRIX3_HH_