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 divideJacobianInertia(const MomentumJacobian& src, const RigidBodyInertia& I, Jacobian& dest)
{
/** \todo if the inertia matrix is singular ? */
if(src.columns()!=dest.columns() || I.getMass() == 0)
return false;
for(unsigned int i=0;i<src.columns();i++)
dest.setColumn(i,src.getColumn(i)/I);
return true;
}