bool changeRefPoint(const MomentumJacobian& src1, const Vector& base_AB, MomentumJacobian& dest)
{
if(src1.columns()!=dest.columns())
return false;
for(unsigned int i=0;i<src1.columns();i++)
dest.setColumn(i,src1.getColumn(i).RefPoint(base_AB));
return true;
}
bool changeBase(const MomentumJacobian& src1, const Rotation& rot, MomentumJacobian& dest)
{
if(src1.columns()!=dest.columns())
return false;
for(unsigned int i=0;i<src1.columns();i++)
dest.setColumn(i,rot*src1.getColumn(i));;
return true;
}
bool changeRefFrame(const MomentumJacobian& src1,const Frame& frame, MomentumJacobian& dest)
{
if(src1.columns()!=dest.columns())
return false;
for(unsigned int i=0;i<src1.columns();i++)
dest.setColumn(i,frame*src1.getColumn(i));
return true;
}
bool multiplyInertiaJacobian(const Jacobian& src, const RigidBodyInertia& I, MomentumJacobian& dest)
{
if(src.columns()!=dest.columns())
return false;
for(unsigned int i=0;i<src.columns();i++)
dest.setColumn(i,I*src.getColumn(i));;
return true;
}