Matrix4x4 Affine::ExtractInverseTransformMatrix() const
{
if(mIsRS)
{
// return transpose only
float invScale = 1.0f / mScale;
return Matrix4x4(mUnitAxis[0][0]*invScale,
mUnitAxis[0][1],
mUnitAxis[0][2],
0.0f,
mUnitAxis[1][0],
mUnitAxis[1][1]*invScale,
mUnitAxis[1][2],
0.0f,
mUnitAxis[2][0],
mUnitAxis[2][1],
mUnitAxis[2][2]*invScale,
0.0f,
0.0f , 0.0f, 0.0f, 1.0f);
}
else
{
// compute full inverse
return ExtractTransformMatrix().Inverse();
}
}
string __str__() {
TransformMatrix tm = ExtractTransformMatrix(transform);
return boost::str(boost::format("<joint %s, transform [[%f, %f, %f, %f], [%f, %f, %f, %f], [%f, %f, %f, %f]], parentindex %d>")%strPyKinBodyJoint(joint)%tm.m[0]%tm.m[1]%tm.m[2]%tm.trans[0]%tm.m[4]%tm.m[5]%tm.m[6]%tm.trans[1]%tm.m[8]%tm.m[9]%tm.m[10]%tm.trans[2]%parentindex);
}
