C++ (Cpp) SkeletonDynamics::get_dqの例

コード例 #1

ファイル: PointConstraint.cpp プロジェクト: bingjeff/dart

 void PointConstraint::updateDynamics(std::vector<Eigen::MatrixXd> & _J, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
     getJacobian();
     SkeletonDynamics *skel = (SkeletonDynamics*)mBody->getSkel();
     _J[mSkelIndex].block(_rowIndex, 0, 3, skel->getNumDofs()) = mJ;
     Vector3d worldP = xformHom(mBody->getWorldTransform(), mOffset);
     VectorXd qDot = skel->get_dq();
     _C.segment(_rowIndex, 3) = worldP - mTarget;
     _CDot.segment(_rowIndex, 3) = mJ * qDot;
 }