17#ifndef IGNITION_MATH_INERTIAL_HH_
18#define IGNITION_MATH_INERTIAL_HH_
20#include <ignition/math/config.hh>
28 inline namespace IGNITION_MATH_VERSION_NAMESPACE
46 : massMatrix(_massMatrix), pose(_pose)
63 this->massMatrix = _m;
64 return this->massMatrix.IsValid();
71 return this->massMatrix;
80 return this->massMatrix.IsValid();
96 return R * this->massMatrix.MOI() * R.Transposed();
105 auto moi = this->
MOI();
106 this->pose.Rot() = _q;
108 return this->massMatrix.MOI(R.Transposed() * moi * R);
126 this->pose.Rot() *= this->
MassMatrix().PrincipalAxesOffset(_tol) *
128 const auto moments = this->
MassMatrix().PrincipalMoments(_tol);
134 return this->massMatrix.MOI(R * diag * R.Transposed());
143 this->pose = _inertial.
Pose();
154 return (this->pose == _inertial.
Pose()) &&
163 return !(*
this == _inertial);
173 T m1 = this->massMatrix.Mass();
185 auto com1 = this->
Pose().Pos();
186 auto com2 = _inertial.
Pose().Pos();
188 auto com = (m1*com1 + m2*com2) / mass;
195 auto moi = this->
MOI() + _inertial.
MOI();
196 ixxyyzz =
Vector3<T>(moi(0, 0), moi(1, 1), moi(2, 2));
197 ixyxzyz =
Vector3<T>(moi(0, 1), moi(0, 2), moi(1, 2));
201 auto dc = com1 - com;
202 ixxyyzz.
X() += m1 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
203 ixxyyzz.
Y() += m1 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
204 ixxyyzz.
Z() += m1 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
205 ixxyyzz.
X() -= m1 * dc[0] * dc[1];
206 ixxyyzz.
Y() -= m1 * dc[0] * dc[2];
207 ixxyyzz.
Z() -= m1 * dc[1] * dc[2];
210 auto dc = com2 - com;
211 ixxyyzz.
X() += m2 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
212 ixxyyzz.
Y() += m2 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
213 ixxyyzz.
Z() += m2 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
214 ixxyyzz.
X() -= m2 * dc[0] * dc[1];
215 ixxyyzz.
Y() -= m2 * dc[0] * dc[2];
216 ixxyyzz.
Z() -= m2 * dc[1] * dc[2];
A class for inertial information about a rigid body consisting of the scalar mass,...
Definition Inertial.hh:36
bool operator==(const Inertial< T > &_inertial) const
Equality comparison operator.
Definition Inertial.hh:152
Matrix3< T > MOI() const
Get the moment of inertia matrix expressed in the base coordinate frame.
Definition Inertial.hh:93
Inertial()
Default Constructor.
Definition Inertial.hh:38
const Pose3< T > & Pose() const
Get the pose of center of mass reference frame.
Definition Inertial.hh:85
virtual ~Inertial()
Destructor.
Definition Inertial.hh:56
bool operator!=(const Inertial< T > &_inertial) const
Inequality test operator.
Definition Inertial.hh:161
const MassMatrix3< T > & MassMatrix() const
Get the mass and inertia matrix.
Definition Inertial.hh:69
Inertial< T > & operator+=(const Inertial< T > &_inertial)
Adds inertial properties to current object.
Definition Inertial.hh:171
Inertial & operator=(const Inertial< T > &_inertial)
Equal operator.
Definition Inertial.hh:140
Inertial(const Inertial< T > &_inertial)
Copy constructor.
Definition Inertial.hh:51
bool SetInertialRotation(const Quaternion< T > &_q)
Set the inertial pose rotation without affecting the MOI in the base coordinate frame.
Definition Inertial.hh:103
bool SetMassMatrix(const MassMatrix3< T > &_m)
Set the mass and inertia matrix.
Definition Inertial.hh:61
Inertial(const MassMatrix3< T > &_massMatrix, const Pose3< T > &_pose)
Constructor.
Definition Inertial.hh:44
bool SetPose(const Pose3< T > &_pose)
Set the pose of center of mass reference frame.
Definition Inertial.hh:77
const Inertial< T > operator+(const Inertial< T > &_inertial) const
Adds inertial properties to current object.
Definition Inertial.hh:229
bool SetMassMatrixRotation(const Quaternion< T > &_q, const T _tol=1e-6)
Set the MassMatrix rotation (eigenvectors of inertia matrix) without affecting the MOI in the base co...
Definition Inertial.hh:123
A class for inertial information about a rigid body consisting of the scalar mass and a 3x3 symmetric...
Definition MassMatrix3.hh:43
A 3x3 matrix class.
Definition Quaternion.hh:32
Encapsulates a position and rotation in three space.
Definition Pose3.hh:34
A quaternion class.
Definition Quaternion.hh:38
Quaternion< T > Inverse() const
Get the inverse of this quaternion.
Definition Quaternion.hh:131
The Vector3 class represents the generic vector containing 3 elements.
Definition Vector3.hh:40
T Z() const
Get the z value.
Definition Vector3.hh:661
T Y() const
Get the y value.
Definition Vector3.hh:654
T X() const
Get the x value.
Definition Vector3.hh:647
Inertial< double > Inertiald
Definition Inertial.hh:243
Inertial< float > Inertialf
Definition Inertial.hh:244