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