inline typename Eigen::Matrix<S1,6,3,O1> operator*(const InertiaTpl<S1,O1> & Y, const ConstraintPlanarTpl<S2,O2> &) { typedef InertiaTpl<S1,O1> Inertia; typedef typename Inertia::Scalar Scalar; typedef Eigen::Matrix<S1,6,3,O1> ReturnType; ReturnType M; const Scalar mass = Y.mass(); const typename Inertia::Vector3 & com = Y.lever(); const typename Inertia::Symmetric3 & inertia = Y.inertia(); M.template topLeftCorner<3,3>().setZero(); M.template topLeftCorner<2,2>().diagonal().fill(mass); const typename Inertia::Vector3 mc(mass * com); M.template rightCols<1>().template head<2>() << -mc(1), mc(0); M.template bottomLeftCorner<3,2>() << (Scalar)0, -mc(2), mc(2), (Scalar)0, -mc(1), mc(0); M.template rightCols<1>().template tail<3>() = inertia.data().template tail<3>(); M.template rightCols<1>()[3] -= mc(0)*com(2); M.template rightCols<1>()[4] -= mc(1)*com(2); M.template rightCols<1>()[5] += mass*(com(0)*com(0) + com(1)*com(1)); return M; }
inline Eigen::Matrix<S2,6,1,O2> operator*(const InertiaTpl<S1,O1> & Y,const ConstraintRevoluteTpl<S2,O2,2> &) { typedef InertiaTpl<S1,O1> Inertia; /* Y(:,5) = (-y, x, 0, I20-xz , I21-yz , I22+xx+yy) */ const S1 &m = Y.mass(), &x = Y.lever()[0], &y = Y.lever()[1], &z = Y.lever()[2]; const typename Inertia::Symmetric3 & I = Y.inertia(); Eigen::Matrix<S2,6,1,O2> res; res << -m*y,m*x,(S2)0, I(2,0)-m*x*z, I(2,1)-m*y*z, I(2,2)+m*(x*x+y*y) ; return res; }
inline Eigen::Matrix<S2,6,1,O2> operator*(const InertiaTpl<S1,O1> & Y,const ConstraintRevoluteTpl<S2,O2,1> &) { typedef InertiaTpl<S1,O1> Inertia; /* Y(:,4) = ( z, 0,-x, I10-xy , I11+xx+zz, I12-yz ) */ const S1 &m = Y.mass(), &x = Y.lever()[0], &y = Y.lever()[1], &z = Y.lever()[2]; const typename Inertia::Symmetric3 & I = Y.inertia(); Eigen::Matrix<S2,6,1,O2> res; res << m*z,(S2)0,-m*x, I(1,0)-m*x*y, I(1,1)+m*(x*x+z*z), I(1,2)-m*y*z ; return res; }
inline Eigen::Matrix<S2,6,1,O2> operator*(const InertiaTpl<S1,O1> & Y,const ConstraintRevoluteTpl<S2,O2,0> &) { typedef InertiaTpl<S1,O1> Inertia; /* Y(:,3) = ( 0,-z, y, I00+yy+zz, I01-xy , I02-xz ) */ const S1 &m = Y.mass(), &x = Y.lever()[0], &y = Y.lever()[1], &z = Y.lever()[2]; const typename Inertia::Symmetric3 & I = Y.inertia(); Eigen::Matrix<S2,6,1,O2> res; res << (S2)0,-m*z,m*y, I(0,0)+m*(y*y+z*z), I(0,1)-m*x*y, I(0,2)-m*x*z ; return res; }
//template<int Dim,typename Scalar,int Options> friend Eigen::Matrix<_Scalar,6,_Dim> operator*( const InertiaTpl<_Scalar,_Options> & Y,const ConstraintTpl<_Dim,_Scalar,_Options> & S) { return Y.matrix()*S.S; }