30 #ifndef _RL_MATH_RIGIDBODYINERTIA_H_
31 #define _RL_MATH_RIGIDBODYINERTIA_H_
43 template<
typename Scalar >
47 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
69 template<
typename OtherDerived >
72 inertiaData(other.template topLeftCorner< 3, 3 >()),
73 massData(other.template bottomRightCorner< 3, 3 >().diagonal().mean())
114 res.template topLeftCorner< 3, 3 >() =
inertia();
115 res.template topRightCorner< 3, 3 >() =
cog().cross33();
116 res.template bottomLeftCorner< 3, 3 >() = -
cog().cross33();
117 res.template bottomRightCorner< 3, 3 >() = ::Eigen::Matrix< Scalar, 3, 3 >::Identity() *
mass();
121 template<
typename OtherDerived >
124 cog() = other.template topRightCorner< 3, 3 >().
cross3();
125 inertia() = other.template topLeftCorner< 3, 3 >();
126 mass() = other.template bottomRightCorner< 3, 3 >().diagonal().mean();
133 res.cog() =
cog() + other.cog();
134 res.inertia() =
inertia() + other.inertia();
135 res.mass() =
mass() + other.mass();
142 res.cog() =
cog() - other.cog();
143 res.inertia() =
inertia() - other.inertia();
144 res.mass() =
mass() - other.mass();
148 template<
typename OtherScalar >
152 res.cog() =
cog() * other;
153 res.inertia() =
inertia() * other;
154 res.mass() =
mass() * other;
158 template<
typename OtherScalar >
161 template<
typename OtherScalar >
165 res.cog() =
cog() / other;
166 res.inertia() =
inertia() / other;
167 res.mass() =
mass() / other;
198 #endif // _RL_MATH_RIGIDBODYINERTIA_H_