==============================================================================
| 1528 | |
| 1529 | //============================================================================== |
| 1530 | Eigen::VectorXd Skeleton::getPositionDifferences( |
| 1531 | const Eigen::VectorXd& _q2, const Eigen::VectorXd& _q1) const |
| 1532 | { |
| 1533 | if (static_cast<std::size_t>(_q2.size()) != getNumDofs() |
| 1534 | || static_cast<std::size_t>(_q1.size()) != getNumDofs()) { |
| 1535 | dterr << "Skeleton::getPositionsDifference: q1's size[" << _q1.size() |
| 1536 | << "] or q2's size[" << _q2.size() << "is different with the dof [" |
| 1537 | << getNumDofs() << "]." << std::endl; |
| 1538 | return Eigen::VectorXd::Zero(getNumDofs()); |
| 1539 | } |
| 1540 | |
| 1541 | Eigen::VectorXd dq(getNumDofs()); |
| 1542 | |
| 1543 | for (const auto& bodyNode : mSkelCache.mBodyNodes) { |
| 1544 | const Joint* joint = bodyNode->getParentJoint(); |
| 1545 | const std::size_t dof = joint->getNumDofs(); |
| 1546 | |
| 1547 | if (dof) { |
| 1548 | std::size_t index = joint->getDof(0)->getIndexInSkeleton(); |
| 1549 | const Eigen::VectorXd& q2Seg = _q2.segment(index, dof); |
| 1550 | const Eigen::VectorXd& q1Seg = _q1.segment(index, dof); |
| 1551 | dq.segment(index, dof) = joint->getPositionDifferences(q2Seg, q1Seg); |
| 1552 | } |
| 1553 | } |
| 1554 | |
| 1555 | return dq; |
| 1556 | } |
| 1557 | |
| 1558 | //============================================================================== |
| 1559 | Eigen::VectorXd Skeleton::getVelocityDifferences( |