17 #ifndef _IGNITION_MASSMATRIX3_HH_
18 #define _IGNITION_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];
327 T tol = _tol * this->Ixxyyzz.Max();
331 return this->Ixxyyzz;
342 T c = Id[0]*Id[1] - std::pow(Ip[0], 2)
343 + Id[0]*Id[2] - std::pow(Ip[1], 2)
344 + Id[1]*Id[2] - std::pow(Ip[2], 2);
346 T d = Id[0]*std::pow(Ip[2], 2)
347 + Id[1]*std::pow(Ip[1], 2)
348 + Id[2]*std::pow(Ip[0], 2)
350 - 2*Ip[0]*Ip[1]*Ip[2];
352 T p = std::pow(b, 2) - 3*c;
361 if (p < std::pow(tol, 2))
365 T q = 2*std::pow(b, 3) - 9*b*c - 27*d;
369 T delta = acos(clamp<T>(0.5 * q / std::pow(p, 1.5), -1, 1));
372 T moment0 = (b + 2*sqrt(p) * cos(delta / 3.0)) / 3.0;
373 T moment1 = (b + 2*sqrt(p) * cos((delta + 2*M_PI)/3.0)) / 3.0;
374 T moment2 = (b + 2*sqrt(p) * cos((delta - 2*M_PI)/3.0)) / 3.0;
375 sort3(moment0, moment1, moment2);
bool operator==(const MassMatrix3< T > &_m) const
Equality comparison operator.
Definition: MassMatrix3.hh:266
T IXY() const
Get IXY.
Definition: MassMatrix3.hh:152
T Mass() const
Get the mass.
Definition: MassMatrix3.hh:76
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
MassMatrix3()
Default Constructor.
Definition: MassMatrix3.hh:42
bool operator!=(const MassMatrix3< T > &_m) const
Inequality test operator.
Definition: MassMatrix3.hh:276
T Sum() const
Return the sum of the values.
Definition: Vector3.hh:87
T IXZ() const
Get IXZ.
Definition: MassMatrix3.hh:159
MassMatrix3(const MassMatrix3< T > &_m)
Copy constructor.
Definition: MassMatrix3.hh:57
bool OffDiagonalMoments(const Vector3< T > &_ixyxzyz)
Set the off-diagonal moments of inertia (Ixy, Ixz, Iyz).
Definition: MassMatrix3.hh:123
bool IZZ(const T &_v)
Set IZZ.
Definition: MassMatrix3.hh:192
A class for inertial information about a rigid body consisting of the scalar mass and a 3x3 symmetric...
Definition: MassMatrix3.hh:39
MassMatrix3 & operator=(const MassMatrix3< T > &_massMatrix)
Equal operator.
Definition: MassMatrix3.hh:253
Vector3< T > OffDiagonalMoments() const
Get the off-diagonal moments of inertia (Ixy, Ixz, Iyz).
Definition: MassMatrix3.hh:106
Vector3< T > DiagonalMoments() const
Get the diagonal moments of inertia (Ixx, Iyy, Izz).
Definition: MassMatrix3.hh:99
bool IsPositive() const
Verify that inertia values are positive definite.
Definition: MassMatrix3.hh:284
bool DiagonalMoments(const Vector3< T > &_ixxyyzz)
Set the diagonal moments of inertia (Ixx, Iyy, Izz).
Definition: MassMatrix3.hh:114
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:324
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
A 3x3 matrix class.
Definition: Matrix3.hh:33
Matrix3< T > MOI() const
returns Moments of Inertia as a Matrix3
Definition: MassMatrix3.hh:227
T IZZ() const
Get IZZ.
Definition: MassMatrix3.hh:145
bool IXY(const T &_v)
Set IXY.
Definition: MassMatrix3.hh:201
bool IYY(const T &_v)
Set IYY.
Definition: MassMatrix3.hh:183
T IYZ() const
Get IYZ.
Definition: MassMatrix3.hh:166
bool IXZ(const T &_v)
Set IXZ.
Definition: MassMatrix3.hh:210
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:37
MassMatrix3< double > MassMatrix3d
Definition: MassMatrix3.hh:393
virtual ~MassMatrix3()
Destructor.
Definition: MassMatrix3.hh:63
bool IsValid() const
Verify that inertia values are positive definite and satisfy the triangle inequality.
Definition: MassMatrix3.hh:298
MassMatrix3(const T &_mass, const Vector3< T > &_ixxyyzz, const Vector3< T > &_ixyxzyz)
Constructor.
Definition: MassMatrix3.hh:49
bool MOI(const Matrix3< T > &_moi)
Sets Moments of Inertia (MOI) from a Matrix3.
Definition: MassMatrix3.hh:240
MassMatrix3< float > MassMatrix3f
Definition: MassMatrix3.hh:394
T IXX() const
Get IXX.
Definition: MassMatrix3.hh:131
bool IXX(const T &_v)
Set IXX.
Definition: MassMatrix3.hh:174
Definition: AffineException.hh:30
T IYY() const
Get IYY.
Definition: MassMatrix3.hh:138
bool Mass(const T &_m)
Set the mass.
Definition: MassMatrix3.hh:68
void sort3(T &_a, T &_b, T &_c)
Sort three numbers, such that _a <= _b <= _c.
Definition: Helpers.hh:291