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 this->pose = _inertial.
Pose();
113 return (this->pose == _inertial.
Pose()) &&
122 return !(*
this == _inertial);
132 T m1 = this->massMatrix.Mass();
144 auto com1 = this->
Pose().Pos();
145 auto com2 = _inertial.
Pose().Pos();
147 auto com = (m1*com1 + m2*com2) / mass;
154 auto moi = this->
MOI() + _inertial.
MOI();
155 ixxyyzz =
Vector3<T>(moi(0, 0), moi(1, 1), moi(2, 2));
156 ixyxzyz =
Vector3<T>(moi(0, 1), moi(0, 2), moi(1, 2));
160 auto dc = com1 - com;
161 ixxyyzz.
X() += m1 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
162 ixxyyzz.
Y() += m1 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
163 ixxyyzz.
Z() += m1 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
164 ixxyyzz.
X() -= m1 * dc[0] * dc[1];
165 ixxyyzz.
Y() -= m1 * dc[0] * dc[2];
166 ixxyyzz.
Z() -= m1 * dc[1] * dc[2];
169 auto dc = com2 - com;
170 ixxyyzz.
X() += m2 * (std::pow(dc[1], 2) + std::pow(dc[2], 2));
171 ixxyyzz.
Y() += m2 * (std::pow(dc[2], 2) + std::pow(dc[0], 2));
172 ixxyyzz.
Z() += m2 * (std::pow(dc[0], 2) + std::pow(dc[1], 2));
173 ixxyyzz.
X() -= m2 * dc[0] * dc[1];
174 ixxyyzz.
Y() -= m2 * dc[0] * dc[2];
175 ixxyyzz.
Z() -= m2 * dc[1] * dc[2];
const Inertial< T > operator+(const Inertial< T > &_inertial) const
Adds inertial properties to current object.
Definition: Inertial.hh:188
bool SetMassMatrix(const MassMatrix3< T > &_m)
Set the mass and inertia matrix.
Definition: Inertial.hh:58
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:130
Encapsulates a position and rotation in three space.
Definition: Pose3.hh:30
T X() const
Get the x value.
Definition: Vector3.hh:639
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
bool operator!=(const Inertial< T > &_inertial) const
Inequality test operator.
Definition: Inertial.hh:120
Matrix3< T > MOI() const
Get the moment of inertia matrix expressed in the base coordinate frame.
Definition: Inertial.hh:90
T Y() const
Get the y value.
Definition: Vector3.hh:646
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:202
Inertial()
Default Constructor.
Definition: Inertial.hh:35
A 3x3 matrix class.
Definition: Matrix3.hh:35
virtual ~Inertial()
Destructor.
Definition: Inertial.hh:53
Inertial(const Inertial< T > &_inertial)
Copy constructor.
Definition: Inertial.hh:48
T Z() const
Get the z value.
Definition: Vector3.hh:653
Inertial & operator=(const Inertial< T > &_inertial)
Equal operator.
Definition: Inertial.hh:99
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:37
bool operator==(const Inertial< T > &_inertial) const
Equality comparison operator.
Definition: Inertial.hh:111
const MassMatrix3< T > & MassMatrix() const
Get the mass and inertia matrix.
Definition: Inertial.hh:66
const Pose3< T > & Pose() const
Get the pose of center of mass reference frame.
Definition: Inertial.hh:82
Definition: AffineException.hh:30
A quaternion class.
Definition: Matrix3.hh:30
Inertial(const MassMatrix3< T > &_massMatrix, const Pose3< T > &_pose)
Constructor.
Definition: Inertial.hh:41
Inertial< float > Inertialf
Definition: Inertial.hh:203