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;
}
}
}
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
}
