17 #ifndef GZ_MATH_INERTIAL_HH_
18 #define GZ_MATH_INERTIAL_HH_
22 #include <gz/math/config.hh>
31 inline namespace GZ_MATH_VERSION_NAMESPACE {
67 : massMatrix(_massMatrix), pose(_pose)
84 : massMatrix(_massMatrix), pose(_pose), addedMass(_addedMass)
105 const T _tolerance = GZ_MASSMATRIX3_DEFAULT_TOLERANCE<T>)
107 this->massMatrix = _m;
108 return this->massMatrix.
IsValid(_tolerance);
117 return this->massMatrix;
126 return this->massMatrix.IsValid();
147 this->addedMass = _m;
148 return this->addedMass.value() == this->addedMass.value().
Transposed();
156 return this->addedMass;
167 return R * this->massMatrix.Moi() * R.Transposed();
194 auto x = this->pose.Pos().X();
195 auto y = this->pose.Pos().Y();
196 auto z = this->pose.Pos().Z();
200 -y, x, 0) * this->massMatrix.Mass();
216 return this->addedMass.has_value() ?
217 this->BodyMatrix() + this->addedMass.value() : this->BodyMatrix();
226 auto moi = this->Moi();
227 this->pose.Rot() = _q;
229 return this->massMatrix.SetMoi(R.Transposed() * moi * R);
247 this->pose.Rot() *= this->MassMatrix().PrincipalAxesOffset(_tol) *
249 const auto moments = this->MassMatrix().PrincipalMoments(_tol);
255 return this->massMatrix.SetMoi(R * diag * R.Transposed());
269 return (this->pose == _inertial.
Pose()) &&
270 (this->massMatrix == _inertial.
MassMatrix()) &&
279 return !(*
this == _inertial);
289 T m1 = this->massMatrix.Mass();
301 auto com1 = this->Pose().Pos();
302 auto com2 = _inertial.
Pose().Pos();
304 auto com = (m1*com1 + m2*com2) / mass;
311 auto moi = this->Moi() + _inertial.
Moi();
312 ixxyyzz =
Vector3<T>(moi(0, 0), moi(1, 1), moi(2, 2));
313 ixyxzyz =
Vector3<T>(moi(0, 1), moi(0, 2), moi(1, 2));
317 auto dc = com1 - com;
321 ixyxzyz.
X() -= m1 * dc[0] * dc[1];
322 ixyxzyz.
Y() -= m1 * dc[0] * dc[2];
323 ixyxzyz.
Z() -= m1 * dc[1] * dc[2];
326 auto dc = com2 - com;
330 ixyxzyz.
X() -= m2 * dc[0] * dc[1];
331 ixyxzyz.
Y() -= m2 * dc[0] * dc[2];
332 ixyxzyz.
Z() -= m2 * dc[1] * dc[2];
347 T m = this->massMatrix.Mass();
359 auto com = this->Pose().Pos();
360 auto com2 = _inertial.
Pose().Pos();
362 auto com1 = (m*com - m2*com2)/m1;
370 auto moi = this->Moi() - _inertial.
Moi();
371 ixxyyzz =
Vector3<T>(moi(0, 0), moi(1, 1), moi(2, 2));
372 ixyxzyz =
Vector3<T>(moi(0, 1), moi(0, 2), moi(1, 2));
376 auto dc = com1 - com;
380 ixyxzyz.
X() += m1 * dc[0] * dc[1];
381 ixyxzyz.
Y() += m1 * dc[0] * dc[2];
382 ixyxzyz.
Z() += m1 * dc[1] * dc[2];
385 auto dc = com2 - com;
389 ixyxzyz.
X() += m2 * dc[0] * dc[1];
390 ixyxzyz.
Y() += m2 * dc[0] * dc[2];
391 ixyxzyz.
Z() += m2 * dc[1] * dc[2];
428 private: std::optional<Matrix6<T>> addedMass;