17 #ifndef IGNITION_MATH_INERTIAL_HH_ 18 #define IGNITION_MATH_INERTIAL_HH_ 43 : massMatrix(_massMatrix), pose(_pose)
60 this->massMatrix = _m;
61 return this->massMatrix.IsValid();
68 return this->massMatrix;
77 return this->massMatrix.IsValid();
93 return R * this->massMatrix.MOI() * R.Transposed();
102 auto moi = this->
MOI();
103 this->pose.Rot() = _q;
105 return this->massMatrix.MOI(R.Transposed() * moi * R);
123 this->pose.Rot() *= this->
MassMatrix().PrincipalAxesOffset(_tol) *
125 const auto moments = this->
MassMatrix().PrincipalMoments(_tol);
131 return this->massMatrix.MOI(R * diag * R.
Transposed());
140 this->pose = _inertial.
Pose();
151 return (this->pose == _inertial.
Pose()) &&
160 return !(*
this == _inertial);
170 T m1 = this->massMatrix.Mass();
182 auto com1 = this->
Pose().Pos();
183 auto com2 = _inertial.
Pose().Pos();
185 auto com = (m1*com1 + m2*com2) / mass;
192 auto moi = this->
MOI() + _inertial.
MOI();
193 ixxyyzz =
Vector3<T>(moi(0, 0), moi(1, 1), moi(2, 2));
194 ixyxzyz =
Vector3<T>(moi(0, 1), moi(0, 2), moi(1, 2));
198 auto dc = com1 - com;
199 ixxyyzz.
X() += m1 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
200 ixxyyzz.
Y() += m1 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
201 ixxyyzz.
Z() += m1 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
202 ixxyyzz.
X() -= m1 * dc[0] * dc[1];
203 ixxyyzz.
Y() -= m1 * dc[0] * dc[2];
204 ixxyyzz.
Z() -= m1 * dc[1] * dc[2];
207 auto dc = com2 - com;
208 ixxyyzz.
X() += m2 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
209 ixxyyzz.
Y() += m2 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
210 ixxyyzz.
Z() += m2 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
211 ixxyyzz.
X() -= m2 * dc[0] * dc[1];
212 ixxyyzz.
Y() -= m2 * dc[0] * dc[2];
213 ixxyyzz.
Z() -= m2 * dc[1] * dc[2];
T X() const
Get the x value.
Definition: Vector3.hh:635
const MassMatrix3< T > & MassMatrix() const
Get the mass and inertia matrix.
Definition: Inertial.hh:66
bool SetInertialRotation(const Quaternion< T > &_q)
Set the inertial pose rotation without affecting the MOI in the base coordinate frame.
Definition: Inertial.hh:100
bool SetMassMatrix(const MassMatrix3< T > &_m)
Set the mass and inertia matrix.
Definition: Inertial.hh:58
bool operator==(const Inertial< T > &_inertial) const
Equality comparison operator.
Definition: Inertial.hh:149
A class for inertial information about a rigid body consisting of the scalar mass and a 3x3 symmetric...
Definition: MassMatrix3.hh:39
Inertial< T > & operator+=(const Inertial< T > &_inertial)
Adds inertial properties to current object.
Definition: Inertial.hh:168
Encapsulates a position and rotation in three space.
Definition: Pose3.hh:30
A class for inertial information about a rigid body consisting of the scalar mass, a 3x3 symmetric moment of inertia matrix, and center of mass reference frame pose.
Definition: Inertial.hh:32
Matrix3< T > MOI() const
Get the moment of inertia matrix expressed in the base coordinate frame.
Definition: Inertial.hh:90
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:120
bool SetPose(const Pose3< T > &_pose)
Set the pose of center of mass reference frame.
Definition: Inertial.hh:74
Inertial< double > Inertiald
Definition: Inertial.hh:240
Inertial()
Default Constructor.
Definition: Inertial.hh:35
A 3x3 matrix class.
Definition: Matrix3.hh:35
virtual ~Inertial()
Destructor.
Definition: Inertial.hh:53
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
Inertial(const Inertial< T > &_inertial)
Copy constructor.
Definition: Inertial.hh:48
Quaternion< T > Inverse() const
Get the inverse of this quaternion.
Definition: Quaternion.hh:128
Inertial & operator=(const Inertial< T > &_inertial)
Equal operator.
Definition: Inertial.hh:137
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:36
const Pose3< T > & Pose() const
Get the pose of center of mass reference frame.
Definition: Inertial.hh:82
const Inertial< T > operator+(const Inertial< T > &_inertial) const
Adds inertial properties to current object.
Definition: Inertial.hh:226
T Z() const
Get the z value.
Definition: Vector3.hh:649
A quaternion class.
Definition: Matrix3.hh:30
Inertial(const MassMatrix3< T > &_massMatrix, const Pose3< T > &_pose)
Constructor.
Definition: Inertial.hh:41
bool operator!=(const Inertial< T > &_inertial) const
Inequality test operator.
Definition: Inertial.hh:158
Inertial< float > Inertialf
Definition: Inertial.hh:241