本文整理汇总了C++中Transform3f::rotInv方法的典型用法代码示例。如果您正苦于以下问题：C++ Transform3f::rotInv方法的具体用法？C++ Transform3f::rotInv怎么用？C++ Transform3f::rotInv使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Transform3f的用法示例。
在下文中一共展示了Transform3f::rotInv方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: computeWorldPointFootDK
CMUK_ERROR_CODE cmuk::computeWorldPointFootDK( LegIndex leg,
JointOffset link,
const KState& state,
const vec3f& pworld,
mat3f* J_trans,
mat3f* J_rot,
float* det,
float det_tol,
float lambda ) const {
if ((int)leg < 0 || (int)leg >= NUM_LEGS) {
return CMUK_BAD_LEG_INDEX;
}
if (!J_trans || !J_rot) {
return CMUK_INSUFFICIENT_ARGUMENTS;
}
Transform3f xform = state.xform();
vec3f pbody = xform.transformInv(pworld);
vec3f fbody;
mat3f J;
CMUK_ERROR_CODE err =
computePointFootDK( leg, link, state.leg_rot[leg], pbody, &J,
&fbody, det, det_tol, lambda );
if (err != CMUK_OKAY && err != CMUK_SINGULAR_MATRIX) {
return err;
}
const mat3f& R = xform.rotFwd();
const mat3f& Rinv = xform.rotInv();
*J_trans = mat3f::identity() - R * J * Rinv;
//*J_rot = R * (-mat3f::cross(pbody) + J * mat3f::cross(fbody));
*J_rot = R * ( J * mat3f::cross(fbody) - mat3f::cross(pbody) ) * Rinv;
return err;
}