void BodyNode::updateFirstDerivatives() {
const int numLocalDofs = getNumLocalDofs();
const int numParentDofs = getNumDependentDofs()-numLocalDofs;
// Update Local Derivatives
for(int i = 0; i < numLocalDofs; i++) {
mTq.at(i) = getLocalDeriv(getDof(i));
}
// Update World Derivatives
// parent dofs
for (int i = 0; i < numParentDofs; i++) {
assert(mNodeParent); // should always have a parent if enters this for loop
if(i<mNumRootTrans)
mWq.at(i) = mNodeParent->mWq.at(i); // in turn its equal to dT/dqi where T is the translation 4x4 matrix for the first 3 dofs
else
mWq.at(i) = mNodeParent->mWq.at(i) * mT;
}
// local dofs
for(int i = 0; i < numLocalDofs; i++){
if(mNodeParent)
mWq.at(numParentDofs+i) = mNodeParent->mW * mTq.at(i);
else
mWq.at(i) = mTq.at(i);
}
evalJacLin();
evalJacAng();
}
//==============================================================================
Eigen::VectorXd Recording::getConfig(int _frameIdx, int _skelIdx) const
{
int index = 0;
for (int i = 0; i < _skelIdx; i++)
index += mNumGenCoordsForSkeletons[i];
return mBakedStates[_frameIdx].segment(index, getDof(_skelIdx));
}
void BodyNode::setDependDofList() {
mDependentDofs.clear();
if (mNodeParent != NULL) {
mDependentDofs.insert(mDependentDofs.end(), mNodeParent->mDependentDofs.begin(), mNodeParent->mDependentDofs.end());
}
for (int i = 0; i < getNumLocalDofs(); i++) {
int dofID = getDof(i)->getSkelIndex();
mDependentDofs.push_back(dofID);
}
#if _DEBUG
for (int i = 0; i < (int)mDependentDofs.size() - 1; i++) {
int now = mDependentDofs[i];
int next = mDependentDofs[i + 1];
if (now > next) {
cerr << "Array not sorted!!!" << endl;
exit(0);
}
}
#endif
}
