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