예제 #1
0
 void PointConstraint::updateDynamics(std::vector<Eigen::MatrixXd> & _J, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
     getJacobian();
     SkeletonDynamics *skel = (SkeletonDynamics*)mBody->getSkel();
     _J[mSkelIndex].block(_rowIndex, 0, 3, skel->getNumDofs()) = mJ;
     Vector3d worldP = xformHom(mBody->getWorldTransform(), mOffset);
     VectorXd qDot = skel->getQDotVector();
     _C.segment(_rowIndex, 3) = worldP - mTarget;
     _CDot.segment(_rowIndex, 3) = mJ * qDot;
 }
예제 #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);
            }
        }
예제 #3
0
        void ConstraintDynamics::addSkeleton(SkeletonDynamics* _newSkel)
        {
            mSkels.push_back(_newSkel);

            int nSkels = mSkels.size();
            int nNodes = _newSkel->getNumNodes();

            for (int j = 0; j < nNodes; j++)
                {
                    kinematics::BodyNode* node = _newSkel->getNode(j);
                    // TODO: (test)
                    if (node->getCollisionShape() == NULL)
                        continue;
                    mCollisionChecker->addCollisionSkeletonNode(node);
                    mBodyIndexToSkelIndex.push_back(nSkels-1);
                }

            // Add all body nodes into mCollisionChecker
            int rows = mMInv.rows();
            int cols = mMInv.cols();

            if (!_newSkel->getImmobileState())
                {
                    // Immobile objets have mass of infinity
                    rows += _newSkel->getMassMatrix().rows();
                    cols += _newSkel->getMassMatrix().cols();
                }

            Eigen::VectorXd newConstrForce;
            if (!_newSkel->getImmobileState())
                newConstrForce = VectorXd::Zero(_newSkel->getNumDofs());
            mContactForces.push_back(newConstrForce);
            mTotalConstrForces.push_back(newConstrForce);

            mMInv = MatrixXd::Zero(rows, cols);
            mTauStar = VectorXd::Zero(rows);

            mIndices.clear();
            int sumNDofs = 0;
            mIndices.push_back(sumNDofs);

            for (int i = 0; i < mSkels.size(); i++) {
                SkeletonDynamics* skel = mSkels[i];
                int nDofs = skel->getNumDofs();
                if (mSkels[i]->getImmobileState())
                    nDofs = 0;
                sumNDofs += nDofs;
                mIndices.push_back(sumNDofs);
            }
            mJ.resize(mSkels.size());
            mPreJ.resize(mSkels.size());
            mJMInv.resize(mSkels.size());
            mZ = MatrixXd(rows, cols);
        }