/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
inline Eigen::Matrix <double, 6, 3> operator* (const Inertia & Y, const ConstraintRotationalSubspace & )
{
Eigen::Matrix <double, 6, 3> M;
// M.block <3,3> (Inertia::LINEAR, 0) = - Y.mass () * skew(Y.lever ());
M.block <3,3> (Inertia::LINEAR, 0) = alphaSkew ( -Y.mass (), Y.lever ());
M.block <3,3> (Inertia::ANGULAR, 0) = (Inertia::Matrix3)(Y.inertia () - Symmetric3::AlphaSkewSquare(Y.mass (), Y.lever ()));
return M;
}
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
inline Eigen::Matrix<double,6,1>
operator*( const Inertia& Y,const ConstraintRevoluteUnaligned & cru)
{
/* YS = [ m -mcx ; mcx I-mcxcx ] [ 0 ; w ] = [ mcxw ; Iw -mcxcxw ] */
const double &m = Y.mass();
const Inertia::Vector3 & c = Y.lever();
const Inertia::Symmetric3 & I = Y.inertia();
Eigen::Matrix<double,6,1> res;
res.head<3>() = -m*c.cross(cru.axis);
res.tail<3>() = I*cru.axis + c.cross(res.head<3>());
return res;
}
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
Eigen::Matrix <Inertia::Scalar, 6, 3> operator* (const Inertia & Y, const JointPlanar::ConstraintPlanar &)
{
Eigen::Matrix <Inertia::Scalar, 6, 3> M;
const double mass = Y.mass ();
const Inertia::Vector3 & com = Y.lever ();
const Symmetric3 & inertia = Y.inertia ();
M.topLeftCorner <3,3> ().setZero ();
M.topLeftCorner <2,2> ().diagonal ().fill (mass);
Inertia::Vector3 mc (mass * com);
M.rightCols <1> ().head <2> () << mc(1), - mc(0);
M.bottomLeftCorner <3,2> () << 0., mc(2), - mc(1), 0., mc(1), -mc(0);
M.rightCols <1> ().tail <3> () = inertia.data ().tail <3> ();
M.rightCols <1> ().head <2> () = mc.head <2> () * com(2);
M.rightCols <1> ().tail <1> () = -mc.head <2> ().transpose () * com.head <2> ();
return M;
}