C++ Transform::GetDof方法代码示例

/*
* Jacobian recursive worker method
*/
void Solver::computeJ(Matd &J, TransformNode *node, Vec4d &pos, Mat4d &trans) {

	// Get initial transform data
	std::vector<Transform *> transforms = node->mTransforms;

	// LEFT MATRIX
	Mat4d leftMat;
	leftMat.MakeDiag(1.0);	// identity
	for (int i = 0; i < transforms.size(); i++)
	{
		Transform *tr = transforms[i];
		// Only need to calculate if DOF
		if (tr->IsDof())
		{
			for (int j = 0; j < tr->GetDofCount(); j++)
			{
				// Get DOF
				Dof *dof = tr->GetDof(j);
				int dofId = dof->mId;

				// Add to our map
				leftMap[dofId] = leftMat;
			}
		}
		// Now update leftMat
		leftMat = leftMat * tr->GetTransform();
	}

	// RIGHT MATRIX
	Mat4d rightMat;
	rightMat.MakeDiag(1.0);	// identity
	for (int i = transforms.size()-1; i >= 0; i--)
	{
		Transform *tr = transforms[i];
		// Only need to calculate if DOF
		if (tr->IsDof())
		{
			for (int j = tr->GetDofCount()- 1; j >= 0 ; j--)
			{
				// Get DOF
				Dof *dof = tr->GetDof(j);
				int dofId = dof->mId;

				// Add to our map
				rightMap[dofId] = rightMat;
			}
		}
		// Now update leftMat
		rightMat = tr->GetTransform() * rightMat;
	}

	// JACOBIAN
	Mat4d pTrans = node->mParentTransform;

	// New identity matrix for later use
	Mat4d newTransform;
	newTransform.MakeDiag(1.0);

	for (int i = 0; i < transforms.size(); i++)
	{		
		// Check if DOF, if so compute derivative
		Transform *tr = transforms[i];
		if (tr->IsDof())
		{
			for (int j = 0; j < tr->GetDofCount(); j++)
			{
				Mat4d deriv = tr->GetDeriv(j);

				// Get row
				Dof * dof = tr->GetDof(j);
				int dofId = dof->mId;

				Mat4d leftMat = leftMap[dofId];
				Mat4d rightMat = rightMap[dofId];
				Vec4d value = pTrans * leftMat * deriv * rightMat * trans * pos;

				J[dofId] = value;
			}
		}

		newTransform = newTransform * tr->GetTransform();
	}
	// Calculate J[] values for parent if necessary
	TransformNode *parent = node->mParentNode;
	// If the parent is non-null and not the current node
	if (parent != NULL && parent != node)
	{
		newTransform = newTransform * trans;
		computeJ(J, parent, pos, newTransform);
	}
}

std::vector<Vec4d*> TransformNode::ComputeJacobian(Matd* J, C3dFileInfo* c3d, int frameNum) {
  // Initialize handles list (transformed)
  std::vector<Vec4d*> hLocals (c3d->GetHandleCount());
  for (int i = 0; i < hLocals.size(); i++) {
    hLocals[i] = (Vec4d*)NULL;
  }

  // std::cout << mName << " Working on TransformNode " << std::endl;

  // Call all children first
  for (int i = 0; i < mChildren.size(); i++) {
    // We should get all child handles and their transforms
    std::vector<Vec4d*> cLocals = static_cast<TransformNode*>(mChildren[i])->ComputeJacobian(J, c3d, frameNum);
    // Copy handle transforms
    for (int j = 0; j < cLocals.size(); j++) {
      if (cLocals[j] != (Vec4d*)NULL) {
        hLocals[j] = new Vec4d(*cLocals[j]);
      }
    }
  }

  // std::cout << mName << " Called children of " << std::endl;

  // Now add all our handles to the list
  for (int i = 0; i < mHandles.size(); i++) {
    hLocals[mHandles[i]->mMarkerOrder] = new Vec4d(mHandles[i]->mOffset, 1);
  }

  // std::cout << mName << " has handle list:" << std::endl;
  // for (int i = 0; i < hLocals.size(); i++) {
  //   if (hLocals[i] != (Vec4d*)NULL) {
  //     std::cout << *hLocals[i] << std::endl;
  //   } else {
  //     std::cout << "NULL" << std::endl;
  //   }
  // }

  for (int i = 0; i < GetSize(); i++) {
    if (!mTransforms[i]->IsDof()) continue;

    for (int j = 0; j < mTransforms[i]->GetDofCount(); j++) {
      // std::cout << mName << " Working on " << i << " transform " << j << " DOF" << std::endl;
      // Which column of J should this DOF be in?
      Transform* trans = mTransforms[i];
      int c = trans->GetDof(j)->mId;

      // Derivative of this local transform
      // Some transforms have many DOFs
      // Multiply all transforms;
      // If we come to this current transform i,
      // Take the derivative of it with respect to jth DOF.
      // Otherwise, just get regular transform
      Mat4d T = vl_I;
      for (int k = 0; k < GetSize(); k++) {
        if (k == i)
          T *= mTransforms[i]->GetDeriv(j);
        else
          T *= mTransforms[i]->GetTransform();
      }

      // Now T is our local transform with derivative
      // taken with respect to the jth DOF.

      // Now the partial transforms of all handles
      // on children is populated on model for use.
      for (int r = 0; r < hLocals.size(); r++) {
        // Make homogeneous coordinate of child handles
        // Vec4d h = Vec4d(c3d->GetMarkerPos(frameNum, r), 1);
        Vec4d h = vl_zero;
        if (hLocals[r] != (Vec4d*)NULL)
          h = *hLocals[r];

        // Derivative transform
        h = T * h;

        // Each row is an x,y, or z of a handle
        // All child handles use the transform
        // Otherwise, all 0s are output
        (*J)[r*3+0][c] = h[0];
        (*J)[r*3+1][c] = h[1];
        (*J)[r*3+2][c] = h[2];
      }
    }
  }

  // Now apply local transform to all handles
  for (int i = 0; i < hLocals.size(); i++) {
    if (hLocals[i] == (Vec4d*)NULL) continue;

    Vec4d* ht = new Vec4d(mLocalTransform * *hLocals[i]);
    hLocals[i] = ht;
  }

  return hLocals;
}