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; }
