本文整理汇总了C++中Transform3f::transformInv方法的典型用法代码示例。如果您正苦于以下问题：C++ Transform3f::transformInv方法的具体用法？C++ Transform3f::transformInv怎么用？C++ Transform3f::transformInv使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Transform3f的用法示例。
在下文中一共展示了Transform3f::transformInv方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的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;
}示例2: computeFootIK
//.........这里部分代码省略.........
if (swap) {
std::swap(hip_rx_angles[0], hip_rx_angles[1]);
std::swap(badness[0], badness[1]);
std::swap(flags[0], flags[1]);
}
int hip_solution_cnt = 2;
if (badness[0] == 0 && badness[1] != 0) {
hip_solution_cnt = 1;
}
debug << "hip_rx_angles[0]=" << hip_rx_angles[0]
<< ", badness=" << badness[0]
<< ", flags=" << flags[0] << "\n";
debug << "hip_rx_angles[1]=" << hip_rx_angles[1]
<< ", badness=" << badness[1]
<< ", flags=" << flags[1] << "\n";
debug << "hip_solution_cnt = " << hip_solution_cnt << "\n";
vec3f qfwd[2], qrear[2];
for (int i=0; i<hip_solution_cnt; ++i) {
debug << "** computing ll solution " << (i+1) << " of " << (hip_solution_cnt) << "\n";
float hip_rx = hip_rx_angles[i];
// now make inv. transform to get rid of hip rotation
Transform3f tx = Transform3f::rx(hip_rx, jo(_kc, leg, HIP_RX_OFFSET, _centeredFootIK));
vec3f ptx = tx.transformInv(orig);
debug << "tx=[" << tx.translation() << ", " << tx.rotation() << "], ptx = " << ptx << "\n";
// calculate lengths for cosine law
float l1sqr = ol2(_kc, leg, KNEE_RY_OFFSET, _centeredFootIK);
float l2sqr = ol2(_kc, leg, FOOT_OFFSET, _centeredFootIK);
float l1 = ol(_kc, leg, KNEE_RY_OFFSET, _centeredFootIK);
float l2 = ol(_kc, leg, FOOT_OFFSET, _centeredFootIK);
float ksqr = ptx[0]*ptx[0] + ptx[2]*ptx[2];
float k = sqrt(ksqr);
debug << "l1=" << l1 << ", l2=" << l2 << ", k=" << k << "\n";
// check triangle inequality
if (k > l1 + l2) {
debug << "oops, violated the triangle inequality for lower segments: "
<< "k = " << k << ", "
<< "l1 + l2 = " << l1 + l2 << "\n";
if (k - (l1 + l2) > 1e-4) {
flags[i] = flags[i] | IK_LOWER_DISTANCE;
}
k = l1 + l2;
ksqr = k * k;
}
// 2*theta is the acute angle formed by the spread
// of the two hip rotations...
float costheta = (l1sqr + ksqr - l2sqr) / (2 * l1 * k);
if (fabs(costheta) > 1) {
debug << "costheta = " << costheta << " > 1\n";
if (fabs(costheta) - 1 > 1e-4) {