void ContactDynamics::initialize() {
// Allocate the Collision Detection class
//mCollisionChecker = new FCLCollisionDetector();
mCollisionChecker = new FCLMESHCollisionDetector();
mBodyIndexToSkelIndex.clear();
// Add all body nodes into mCollisionChecker
for (int i = 0; i < getNumSkels(); i++) {
SkeletonDynamics* skel = mSkels[i];
int nNodes = skel->getNumNodes();
for (int j = 0; j < nNodes; j++) {
mCollisionChecker->addCollisionSkeletonNode(skel->getNode(j));
mBodyIndexToSkelIndex.push_back(i);
}
}
mConstrForces.resize(getNumSkels());
for (int i = 0; i < getNumSkels(); i++){
if (!mSkels[i]->getImmobileState())
mConstrForces[i] = VectorXd::Zero(mSkels[i]->getNumDofs());
}
mIndices.clear();
int sumNDofs = 0;
mIndices.push_back(sumNDofs);
for (int i = 0; i < getNumSkels(); i++) {
if (!mSkels[i]->getImmobileState())
sumNDofs += mSkels[i]->getNumDofs();
mIndices.push_back(sumNDofs);
}
mTauStar = VectorXd::Zero(getNumTotalDofs());
}
void ContactDynamics::initialize() {
// Allocate the Collision Detection class
mCollisionChecker = new SkeletonCollision();
mBodyIndexToSkelIndex.clear();
// Add all body nodes into mCollisionChecker
int rows = 0;
int cols = 0;
for (int i = 0; i < getNumSkels(); i++) {
SkeletonDynamics* skel = mSkels[i];
int nNodes = skel->getNumNodes();
for (int j = 0; j < nNodes; j++) {
kinematics::BodyNode* node = skel->getNode(j);
if (node->getShape()->getShapeType() != kinematics::Shape::P_UNDEFINED)
{
mCollisionChecker->addCollisionSkeletonNode(node);
mBodyIndexToSkelIndex.push_back(i);
}
}
if (!mSkels[i]->getImmobileState()) {
// Immobile objets have mass of infinity
rows += skel->getMassMatrix().rows();
cols += skel->getMassMatrix().cols();
}
}
mConstrForces.resize(getNumSkels());
for (int i = 0; i < getNumSkels(); i++){
if (!mSkels[i]->getImmobileState())
mConstrForces[i] = VectorXd::Zero(mSkels[i]->getNumDofs());
}
mMInv = MatrixXd::Zero(rows, cols);
mTauStar = VectorXd::Zero(rows);
// Initialize the index vector:
// If we have 3 skeletons,
// mIndices[0] = 0
// mIndices[1] = nDof0
// mIndices[2] = nDof0 + nDof1
// mIndices[3] = nDof0 + nDof1 + nDof2
mIndices.clear();
int sumNDofs = 0;
mIndices.push_back(sumNDofs);
for (int i = 0; i < getNumSkels(); i++) {
SkeletonDynamics* skel = mSkels[i];
int nDofs = skel->getNumDofs();
if (mSkels[i]->getImmobileState())
nDofs = 0;
sumNDofs += nDofs;
mIndices.push_back(sumNDofs);
}
}
void ContactDynamics::initialize() {
// Allocate the Collision Detection class
//mCollisionChecker = new FCLCollisionDetector();
mCollisionChecker = new FCLMESHCollisionDetector();
mBodyIndexToSkelIndex.clear();
// Add all body nodes into mCollisionChecker
for (int i = 0; i < getNumSkels(); i++) {
SkeletonDynamics* skel = mSkels[i];
int nNodes = skel->getNumNodes();
for (int j = 0; j < nNodes; j++) {
kinematics::BodyNode* node = skel->getNode(j);
// If the collision shape of the node is NULL, then the node is
// uncollidable object. We don't care uncollidable object in
// ContactDynamics.
if (node->getCollisionShape() == NULL)
continue;
if (node->getCollisionShape()->getShapeType() != kinematics::Shape::P_UNDEFINED)
{
mCollisionChecker->addCollisionSkeletonNode(node);
mBodyIndexToSkelIndex.push_back(i);
}
}
}
mConstrForces.resize(getNumSkels());
for (int i = 0; i < getNumSkels(); i++){
if (!mSkels[i]->getImmobileState())
mConstrForces[i] = VectorXd::Zero(mSkels[i]->getNumDofs());
}
mIndices.clear();
int sumNDofs = 0;
mIndices.push_back(sumNDofs);
for (int i = 0; i < getNumSkels(); i++) {
if (!mSkels[i]->getImmobileState())
sumNDofs += mSkels[i]->getNumDofs();
mIndices.push_back(sumNDofs);
}
mTauStar = VectorXd::Zero(getNumTotalDofs());
}