Example #1
0
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());
}
Example #2
0
        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);
            }
        }
Example #3
0
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());
}