bool KDLtoYarp(const KDL::Rotation & kdlRotation, yarp::sig::Matrix & yarpMatrix3_3)
{
if( yarpMatrix3_3.rows() != 3 || yarpMatrix3_3.cols() != 3 ) { yarpMatrix3_3.resize(3,3); }
//Both kdl and yarp store the rotation matrix in row major order
memcpy(yarpMatrix3_3.data(),kdlRotation.data,3*3*sizeof(double));
return true;
}
bool yarpWholeBodyModelV1::convertBasePose(const Frame &xBase, yarp::sig::Matrix & H_world_base)
{
if( H_world_base.cols() != 4 || H_world_base.rows() != 4 )
H_world_base.resize(4,4);
xBase.get4x4Matrix(H_world_base.data());
return true;
}
bool Matrix::operator==(const yarp::sig::Matrix &r) const
{
//check dimensions first
if ( (rows()!=r.rows()) || (cols()!=r.cols()))
return false;
const double *tmp1=data();
const double *tmp2=r.data();
int c=rows()*cols();
while(c--)
{
if (*tmp1++!=*tmp2++)
return false;
}
return true;
}
