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