void SPATIAL_RB_INERTIA::inverse_mult_spatial(const SVECTOR6& w, const MATRIX3& iJ, const MATRIX3& hx, const MATRIX3& hxiJ, REAL inv_m, SVECTOR6& result) const
{
// get the components of the force
ORIGIN3 top(w.get_upper());
ORIGIN3 bot(w.get_lower());
// do the arithmetic
VECTOR3 ttop(hxiJ.transpose_mult(top) + iJ*bot, pose);
VECTOR3 tbot(top*inv_m + hx*(hxiJ.transpose_mult(top)) + hxiJ.mult(bot), pose);
result.pose = pose;
result.set_upper(ttop);
result.set_lower(tbot);
}
/// Multiplies a spatial vector
void SPATIAL_RB_INERTIA::mult_spatial(const SVECTOR6& t, SVECTOR6& result) const
{
// get necessary components of t
ORIGIN3 ttop(t.get_upper());
ORIGIN3 tbot(t.get_lower());
// do some precomputation
MATRIX3 hx = MATRIX3::skew_symmetric(h);
MATRIX3 hxm = MATRIX3::skew_symmetric(h*m);
// Featherstone's code uses this format:
// J-hx*hx*m hx*m
// -hx*m mI
// our format is:
// -hx*m mI
// J-hx*hx*m hx*m
// compute result
VECTOR3 rtop(tbot*m + hxm.transpose_mult(ttop), pose);
VECTOR3 rbot(J*ttop-(hx*(hxm*ttop)) + hxm*tbot, pose);
result.pose = pose;
result.set_upper(rtop);
result.set_lower(rbot);
}
/// Multiplies a spatial vector
void SPATIAL_RB_INERTIA::mult_spatial(const SVECTOR6& t, const MATRIX3& hxm, const MATRIX3& Jmod, SVECTOR6& result) const
{
// get necessary components of t
ORIGIN3 ttop(t.get_upper());
ORIGIN3 tbot(t.get_lower());
// compute result
VECTOR3 rtop(tbot*m + hxm.transpose_mult(ttop), pose);
VECTOR3 rbot(Jmod*ttop + hxm*tbot, pose);
result.pose = pose;
result.set_upper(rtop);
result.set_lower(rbot);
}
