void BodyNode::init() {
assert(mSkel);
if(mVizShape && mI.isZero()) {
mI = mVizShape->computeInertia(mMass);
}
mT = Matrix4d::Identity();
mW = Matrix4d::Identity();
mIc = Matrix3d::Zero();
const int numLocalDofs = getNumLocalDofs();
mTq.resize(numLocalDofs, Matrix4d::Zero());
const int numDepDofs = getNumDependentDofs();
mWq.resize(numDepDofs, Matrix4d::Zero());
mJv = MatrixXd::Zero(3, numDepDofs);
mJw = MatrixXd::Zero(3, numDepDofs);
mNumRootTrans = 0;
for(int i=0; i<mSkel->getNumDofs(); i++){
if(!(mSkel->getDof(i)->getTrans()->getType()==Transformation::T_TRANSLATE
|| mSkel->getDof(i)->getTrans()->getType()==Transformation::T_TRANSLATEX
|| mSkel->getDof(i)->getTrans()->getType()==Transformation::T_TRANSLATEY
|| mSkel->getDof(i)->getTrans()->getType()==Transformation::T_TRANSLATEZ)) {
mNumRootTrans = i;
break;
}
}
}
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();
}
void BodyNode::init() {
if (mShape != NULL) {
mMass = mShape->getMass();
} else {
mMass = 0;
}
mT = Matrix4d::Identity();
mW = Matrix4d::Identity();
mIc = Matrix3d::Zero();
const int numLocalDofs = getNumLocalDofs();
mTq.resize(numLocalDofs, Matrix4d::Zero());
const int numDepDofs = getNumDependentDofs();
mWq.resize(numDepDofs, Matrix4d::Zero());
mJv = MatrixXd::Zero(3, numDepDofs);
mJw = MatrixXd::Zero(3, numDepDofs);
mNumRootTrans = 0;
for(int i=0; i<mSkel->getNumDofs(); i++){
if(!(mSkel->getDof(i)->getTrans()->getType()==Transformation::T_TRANSLATE
|| mSkel->getDof(i)->getTrans()->getType()==Transformation::T_TRANSLATEX
|| mSkel->getDof(i)->getTrans()->getType()==Transformation::T_TRANSLATEY
|| mSkel->getDof(i)->getTrans()->getType()==Transformation::T_TRANSLATEZ)) {
mNumRootTrans = i;
break;
}
}
}
