17 #ifndef IGNITION_MATH_MASSMATRIX3_HH_ 18 #define IGNITION_MATH_MASSMATRIX3_HH_ 52 : mass(_mass), Ixxyyzz(_ixxyyzz), Ixyxzyz(_ixyxzyz)
68 public:
bool Mass(
const T &_m)
90 const T &_ixy,
const T &_ixz,
const T &_iyz)
92 this->Ixxyyzz.Set(_ixx, _iyy, _izz);
93 this->Ixyxzyz.Set(_ixy, _ixz, _iyz);
101 return this->Ixxyyzz;
108 return this->Ixyxzyz;
116 this->Ixxyyzz = _ixxyyzz;
125 this->Ixyxzyz = _ixyxzyz;
133 return this->Ixxyyzz[0];
140 return this->Ixxyyzz[1];
147 return this->Ixxyyzz[2];
154 return this->Ixyxzyz[0];
161 return this->Ixyxzyz[1];
168 return this->Ixyxzyz[2];
174 public:
bool IXX(
const T &_v)
183 public:
bool IYY(
const T &_v)
192 public:
bool IZZ(
const T &_v)
201 public:
bool IXY(
const T &_v)
210 public:
bool IXZ(
const T &_v)
219 public:
bool IYZ(
const T &_v)
230 this->Ixxyyzz[0], this->Ixyxzyz[0], this->Ixyxzyz[1],
231 this->Ixyxzyz[0], this->Ixxyyzz[1], this->Ixyxzyz[2],
232 this->Ixyxzyz[1], this->Ixyxzyz[2], this->Ixxyyzz[2]);
242 this->Ixxyyzz.Set(_moi(0, 0), _moi(1, 1), _moi(2, 2));
244 0.5*(_moi(0, 1) + _moi(1, 0)),
245 0.5*(_moi(0, 2) + _moi(2, 0)),
246 0.5*(_moi(1, 2) + _moi(2, 1)));
255 this->mass = _massMatrix.
Mass();
268 return equal<T>(this->mass, _m.
Mass()) &&
278 return !(*
this == _m);
288 return (this->mass > 0) &&
290 (this->
IXX()*this->
IYY() - std::pow(this->
IXY(), 2) > 0) &&
291 (this->
MOI().Determinant() > 0);
310 return _moments[0] > 0 &&
313 _moments[0] + _moments[1] > _moments[2] &&
314 _moments[1] + _moments[2] > _moments[0] &&
315 _moments[2] + _moments[0] > _moments[1];
332 T tol = _tol * this->Ixxyyzz.Max();
336 return this->Ixxyyzz;
347 T c = Id[0]*Id[1] - std::pow(Ip[0], 2)
348 + Id[0]*Id[2] - std::pow(Ip[1], 2)
349 + Id[1]*Id[2] - std::pow(Ip[2], 2);
351 T d = Id[0]*std::pow(Ip[2], 2)
352 + Id[1]*std::pow(Ip[1], 2)
353 + Id[2]*std::pow(Ip[0], 2)
355 - 2*Ip[0]*Ip[1]*Ip[2];
357 T p = std::pow(b, 2) - 3*c;
366 if (p < std::pow(tol, 2))
370 T q = 2*std::pow(b, 3) - 9*b*c - 27*d;
374 T delta = acos(clamp<T>(0.5 * q / std::pow(p, 1.5), -1, 1));
377 T moment0 = (b + 2*sqrt(p) * cos(delta / 3.0)) / 3.0;
378 T moment1 = (b + 2*sqrt(p) * cos((delta + 2*
IGN_PI)/3.0)) / 3.0;
379 T moment2 = (b + 2*sqrt(p) * cos((delta - 2*
IGN_PI)/3.0)) / 3.0;
380 sort3(moment0, moment1, moment2);
398 T tol = _tol * this->Ixxyyzz.Max();
400 if (moments.
Equal(this->Ixxyyzz, tol) ||
401 (math::equal<T>(moments[0], moments[1], std::abs(tol)) &&
402 math::equal<T>(moments[0], moments[2], std::abs(tol))))
426 Vector2<T> f1(this->Ixyxzyz[0], -this->Ixyxzyz[1]);
427 Vector2<T> f2(this->Ixxyyzz[1] - this->Ixxyyzz[2],
428 -2*this->Ixyxzyz[2]);
432 Vector2<T> momentsDiff(moments[0] - moments[1],
433 moments[1] - moments[2]);
436 int unequalMoment = -1;
437 if (equal<T>(momentsDiff[0], 0, std::abs(tol)))
439 else if (equal<T>(momentsDiff[1], 0, std::abs(tol)))
442 if (unequalMoment >= 0)
447 T momentsDiff3 = moments[1] - moments[unequalMoment];
451 T s = (this->Ixxyyzz[0] - moments[unequalMoment]) / momentsDiff3;
457 T phi2 = acos(clamp<T>(ClampedSqrt(s), -1, 1));
466 math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
494 Vector2<T> g1a(0, 0.5*momentsDiff3 * sin(2*phi2));
497 math::Angle phi11a(Angle2(g1a, tol) - Angle2(f1, tol));
502 Vector2<T> g1b(0, 0.5*momentsDiff3 * sin(-2*phi2));
505 math::Angle phi11b(Angle2(g1b, tol) - Angle2(f1, tol));
511 T erra = std::pow(sin(phi1) - sin(phi11a.
Radian()), 2)
512 + std::pow(cos(phi1) - cos(phi11a.
Radian()), 2);
513 T errb = std::pow(sin(phi1) - sin(phi11b.
Radian()), 2)
514 + std::pow(cos(phi1) - cos(phi11b.
Radian()), 2);
534 if (unequalMoment == 0)
542 T v = (std::pow(this->Ixyxzyz[0], 2) + std::pow(this->Ixyxzyz[1], 2)
543 +(this->Ixxyyzz[0] - moments[2])
544 *(this->Ixxyyzz[0] + moments[2] - moments[0] - moments[1]))
545 / ((moments[1] - moments[2]) * (moments[2] - moments[0]));
548 if (v < std::abs(tol))
557 w = (this->Ixxyyzz[0] - moments[2] + (moments[2] - moments[1])*v)
558 / ((moments[0] - moments[1]) * v);
563 T phi2 = acos(clamp<T>(ClampedSqrt(v), -1, 1));
565 T phi3 = acos(clamp<T>(ClampedSqrt(w), -1, 1));
570 0.5* (moments[0]-moments[1])*ClampedSqrt(v)*sin(2*phi3),
571 0.5*((moments[0]-moments[1])*w + moments[1]-moments[2])*sin(2*phi2));
573 (moments[0]-moments[1])*(1 + (v-2)*w) + (moments[1]-moments[2])*v,
574 (moments[0]-moments[1])*sin(phi2)*sin(2*phi3));
589 if (f1small && f2small)
599 math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
606 math::Angle phi11(Angle2(g1, tol) - Angle2(f1, tol));
614 math::Angle phi11(Angle2(g1, tol) - Angle2(f1, tol));
617 math::Angle phi12(0.5*(Angle2(g2, tol) - Angle2(f2, tol)));
619 T err = std::pow(sin(phi11.
Radian()) - sin(phi12.
Radian()), 2)
620 + std::pow(cos(phi11.
Radian()) - cos(phi12.
Radian()), 2);
627 math::Angle phi11a(Angle2(g1a, tol) - Angle2(f1, tol));
628 math::Angle phi12a(0.5*(Angle2(g2a, tol) - Angle2(f2, tol)));
631 T erra = std::pow(sin(phi11a.Radian()) - sin(phi12a.
Radian()), 2)
632 + std::pow(cos(phi11a.Radian()) - cos(phi12a.
Radian()), 2);
636 phi1 = phi11a.Radian();
637 signsPhi23.
Set(-1, 1);
644 math::Angle phi11b(Angle2(g1b, tol) - Angle2(f1, tol));
645 math::Angle phi12b(0.5*(Angle2(g2b, tol) - Angle2(f2, tol)));
648 T errb = std::pow(sin(phi11b.Radian()) - sin(phi12b.
Radian()), 2)
649 + std::pow(cos(phi11b.Radian()) - cos(phi12b.
Radian()), 2);
653 phi1 = phi11b.Radian();
654 signsPhi23.
Set(1, -1);
661 math::Angle phi11c(Angle2(g1c, tol) - Angle2(f1, tol));
662 math::Angle phi12c(0.5*(Angle2(g2c, tol) - Angle2(f2, tol)));
665 T errc = std::pow(sin(phi11c.
Radian()) - sin(phi12c.
Radian()), 2)
666 + std::pow(cos(phi11c.
Radian()) - cos(phi12c.
Radian()), 2);
671 signsPhi23.
Set(-1, -1);
676 phi2 *= signsPhi23[0];
677 phi3 *= signsPhi23[1];
695 const T _tol = 1e-6)
const 715 _size.
X(sqrt(6*(moments.
Y() + moments.
Z() - moments.
X()) / this->mass));
716 _size.
Y(sqrt(6*(moments.
Z() + moments.
X() - moments.
Y()) / this->mass));
717 _size.
Z(sqrt(6*(moments.
X() + moments.
Y() - moments.
Z()) / this->mass));
759 if (this->
Mass() <= 0 || _size.
Min() <= 0 ||
767 T x2 = std::pow(_size.
X(), 2);
768 T y2 = std::pow(_size.
Y(), 2);
769 T z2 = std::pow(_size.
Z(), 2);
770 L(0, 0) = this->mass / 12.0 * (y2 + z2);
771 L(1, 1) = this->mass / 12.0 * (z2 + x2);
772 L(2, 2) = this->mass / 12.0 * (x2 + y2);
791 if (_mass <= 0 || _length <= 0 || _radius <= 0 ||
812 if (this->
Mass() <= 0 || _length <= 0 || _radius <= 0 ||
819 T radius2 = std::pow(_radius, 2);
821 L(0, 0) = this->mass / 12.0 * (3*radius2 + std::pow(_length, 2));
823 L(2, 2) = this->mass / 2.0 * radius2;
835 if (_mass <= 0 || _radius <= 0)
850 if (this->
Mass() <= 0 || _radius <= 0)
856 T radius2 = std::pow(_radius, 2);
858 L(0, 0) = 0.4 * this->mass * radius2;
859 L(1, 1) = 0.4 * this->mass * radius2;
860 L(2, 2) = 0.4 * this->mass * radius2;
867 private:
static inline T ClampedSqrt(
const T &_x)
879 private:
static T Angle2(
const Vector2<T> &_v,
const T _eps = 1e-6)
881 if (_v.SquaredLength() < std::pow(_eps, 2))
883 return atan2(_v[1], _v[0]);
892 private: Vector3<T> Ixxyyzz;
897 private: Vector3<T> Ixyxzyz;
An angle and related functions.
Definition: Angle.hh:44
T X() const
Get the x value.
Definition: Vector3.hh:635
Vector3< T > OffDiagonalMoments() const
Get the off-diagonal moments of inertia (Ixy, Ixz, Iyz).
Definition: MassMatrix3.hh:106
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:754
bool Equal(const Vector3 &_v, const T &_tol) const
Equality test with tolerance.
Definition: Vector3.hh:555
bool SetFromSphere(const T _mass, const T _radius)
Set inertial properties based on mass and equivalent sphere.
Definition: MassMatrix3.hh:832
bool IYZ(const T &_v)
Set IYZ.
Definition: MassMatrix3.hh:219
static bool ValidMoments(const Vector3< T > &_moments)
Verify that principal moments are positive and satisfy the triangle inequality.
Definition: MassMatrix3.hh:308
T IZZ() const
Get IZZ.
Definition: MassMatrix3.hh:145
MassMatrix3()
Default Constructor.
Definition: MassMatrix3.hh:42
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:784
MassMatrix3(const MassMatrix3< T > &_m)
Copy constructor.
Definition: MassMatrix3.hh:57
void Set(T _x, T _y)
Set the contents of the vector.
Definition: Vector2.hh:103
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:693
void Normalize()
Normalize the angle in the range -Pi to Pi.
bool IsPositive() const
Verify that inertia values are positive definite.
Definition: MassMatrix3.hh:284
T IYZ() const
Get IYZ.
Definition: MassMatrix3.hh:166
bool OffDiagonalMoments(const Vector3< T > &_ixyxzyz)
Set the off-diagonal moments of inertia (Ixy, Ixz, Iyz).
Definition: MassMatrix3.hh:123
T SquaredLength() const
Returns the square of the length (magnitude) of the vector.
Definition: Vector2.hh:82
bool IZZ(const T &_v)
Set IZZ.
Definition: MassMatrix3.hh:192
bool operator==(const MassMatrix3< T > &_m) const
Equality comparison operator.
Definition: MassMatrix3.hh:266
A class for inertial information about a rigid body consisting of the scalar mass and a 3x3 symmetric...
Definition: MassMatrix3.hh:39
Two dimensional (x, y) vector.
Definition: Vector2.hh:29
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:329
MassMatrix3 & operator=(const MassMatrix3< T > &_massMatrix)
Equal operator.
Definition: MassMatrix3.hh:253
Vector3< T > DiagonalMoments() const
Get the diagonal moments of inertia (Ixx, Iyy, Izz).
Definition: MassMatrix3.hh:99
T IXY() const
Get IXY.
Definition: MassMatrix3.hh:152
T IYY() const
Get IYY.
Definition: MassMatrix3.hh:138
bool DiagonalMoments(const Vector3< T > &_ixxyyzz)
Set the diagonal moments of inertia (Ixx, Iyy, Izz).
Definition: MassMatrix3.hh:114
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:89
void Radian(double _radian)
Set the value from an angle in radians.
A 3x3 matrix class.
Definition: Matrix3.hh:35
void Min(const Vector3< T > &_v)
Set this vector's components to the minimum of itself and the passed in vector.
Definition: Vector3.hh:284
bool SetFromSphere(const T _radius)
Set inertial properties based on equivalent sphere using the current mass value.
Definition: MassMatrix3.hh:847
Matrix3< T > Transposed() const
Return the transpose of this matrix.
Definition: Matrix3.hh:475
T Y() const
Get the y value.
Definition: Vector3.hh:642
bool IXY(const T &_v)
Set IXY.
Definition: MassMatrix3.hh:201
bool IYY(const T &_v)
Set IYY.
Definition: MassMatrix3.hh:183
bool operator!=(const MassMatrix3< T > &_m) const
Inequality test operator.
Definition: MassMatrix3.hh:276
Quaternion< T > PrincipalAxesOffset(const T _tol=1e-6) const
Compute rotational offset of principal axes.
Definition: MassMatrix3.hh:395
Matrix3< T > MOI() const
returns Moments of Inertia as a Matrix3
Definition: MassMatrix3.hh:227
bool IXZ(const T &_v)
Set IXZ.
Definition: MassMatrix3.hh:210
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:36
MassMatrix3< double > MassMatrix3d
Definition: MassMatrix3.hh:900
virtual ~MassMatrix3()
Destructor.
Definition: MassMatrix3.hh:63
MassMatrix3(const T &_mass, const Vector3< T > &_ixxyyzz, const Vector3< T > &_ixyxzyz)
Constructor.
Definition: MassMatrix3.hh:49
T Z() const
Get the z value.
Definition: Vector3.hh:649
bool MOI(const Matrix3< T > &_moi)
Sets Moments of Inertia (MOI) from a Matrix3.
Definition: MassMatrix3.hh:240
void Normalize()
Normalize the vector length.
Definition: Vector2.hh:89
MassMatrix3< float > MassMatrix3f
Definition: MassMatrix3.hh:901
T Mass() const
Get the mass.
Definition: MassMatrix3.hh:76
bool IsValid() const
Verify that inertia values are positive definite and satisfy the triangle inequality.
Definition: MassMatrix3.hh:298
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:735
#define IGN_PI_2
Definition: Helpers.hh:174
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:806
bool IXX(const T &_v)
Set IXX.
Definition: MassMatrix3.hh:174
A quaternion class.
Definition: Matrix3.hh:30
#define IGN_PI
Define IGN_PI, IGN_PI_2, and IGN_PI_4.
Definition: Helpers.hh:173
T IXZ() const
Get IXZ.
Definition: MassMatrix3.hh:159
bool Mass(const T &_m)
Set the mass.
Definition: MassMatrix3.hh:68
T IXX() const
Get IXX.
Definition: MassMatrix3.hh:131
void sort3(T &_a, T &_b, T &_c)
Sort three numbers, such that _a <= _b <= _c.
Definition: Helpers.hh:598
T Sum() const
Return the sum of the values.
Definition: Vector3.hh:86