Fixed spatial rb inertia; fixed serious bug in initializing Matrix3 types