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::addSkeleton(SkeletonDynamics* _newSkel) { // mSkels.push_back(_newSkel); int nSkels = mSkels.size(); int nNodes = _newSkel->getNumNodes(); for (int j = 0; j < nNodes; j++) { mCollisionChecker->addCollisionSkeletonNode(_newSkel->getNode(j)); mBodyIndexToSkelIndex.push_back(nSkels-1); } Eigen::VectorXd newConstrForce; if (!_newSkel->getImmobileState()) newConstrForce = VectorXd::Zero(_newSkel->getNumDofs()); mConstrForces.push_back(newConstrForce); // TODO: World already has this indices. // 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++) { if (!mSkels[i]->getImmobileState()) sumNDofs += mSkels[i]->getNumDofs(); mIndices.push_back(sumNDofs); } mTauStar = VectorXd::Zero(getNumTotalDofs()); }
void ContactDynamics::updateNBMatrices() { mN = MatrixXd::Zero(getNumTotalDofs(), getNumContacts()); mB = MatrixXd::Zero(getNumTotalDofs(), getNumContacts() * getNumContactDirections()); for (int i = 0; i < getNumContacts(); i++) { Contact& c = mCollisionChecker->getContact(i); Vector3d p = c.point; int skelID1 = mBodyIndexToSkelIndex[c.collisionNode1->getIndex()]; int skelID2 = mBodyIndexToSkelIndex[c.collisionNode2->getIndex()]; Vector3d N21 = c.normal; Vector3d N12 = -c.normal; MatrixXd B21 = getTangentBasisMatrix(p, N21); MatrixXd B12 = -B21; if (!mSkels[skelID1]->getImmobileState()) { int index1 = mIndices[skelID1]; int NDOF1 = c.collisionNode1->getBodyNode()->getSkel()->getNumDofs(); // Vector3d N21 = c.normal; MatrixXd J21t = getJacobian(c.collisionNode1->getBodyNode(), p); mN.block(index1, i, NDOF1, 1).noalias() = J21t * N21; //B21 = getTangentBasisMatrix(p, N21); mB.block(index1, i * getNumContactDirections(), NDOF1, getNumContactDirections()).noalias() = J21t * B21; } if (!mSkels[skelID2]->getImmobileState()) { int index2 = mIndices[skelID2]; int NDOF2 = c.collisionNode2->getBodyNode()->getSkel()->getNumDofs(); //Vector3d N12 = -c.normal; //if (B21.rows() == 0) // B12 = getTangentBasisMatrix(p, N12); //else // B12 = -B21; MatrixXd J12t = getJacobian(c.collisionNode2->getBodyNode(), p); mN.block(index2, i, NDOF2, 1).noalias() = J12t * N12; mB.block(index2, i * getNumContactDirections(), NDOF2, getNumContactDirections()).noalias() = J12t * B12; } } }
void ContactDynamics::applyContactForces() { if (getNumTotalDofs() == 0) return; mCollisionChecker->clearAllContacts(); mCollisionChecker->checkCollision(true, true); for (int i = 0; i < getNumSkels(); i++) mConstrForces[i].setZero(); if (mCollisionChecker->getNumContact() == 0) return; fillMatrices(); solve(); applySolution(); }
void ContactDynamics::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); // 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(nSkels-1); } } Eigen::VectorXd newConstrForce; if (!_newSkel->getImmobileState()) newConstrForce = VectorXd::Zero(_newSkel->getNumDofs()); mConstrForces.push_back(newConstrForce); // TODO: World already has this indices. // 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++) { 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 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()); }
void ContactDynamics::fillMatrices() { updateTauStar(); updateNBMatrices(); // updateNormalMatrix(); // updateBasisMatrix(); MatrixXd E = getContactMatrix(); int c = getNumContacts(); int cd = c * mNumDir; // Construct the intermediary blocks. // nTmInv = mN.transpose() * MInv // bTmInv = mB.transpose() * MInv // Where MInv is the imaginary diagonal block matrix that combines the inverted mass matrices of all skeletons. // Muliplying each block independently is more efficient that multiplyting the whole MInv matrix. MatrixXd nTmInv(c, getNumTotalDofs()); MatrixXd bTmInv(cd, getNumTotalDofs()); for (int i = 0; i < getNumSkels(); i++) { if (mSkels[i]->getImmobileState()) { assert(mIndices[i] == mIndices[i+1]); // If the user sets a skeleton to be immobile without reinitializing ContactDynamics, this assertion will fail. continue; } const MatrixXd skelMInv = mSkels[i]->getInvMassMatrix(); const int skelNumDofs = mSkels[i]->getNumDofs(); nTmInv.middleCols(mIndices[i], skelNumDofs).noalias() = mN.transpose().middleCols(mIndices[i], skelNumDofs) * skelMInv; bTmInv.middleCols(mIndices[i], skelNumDofs).noalias() = mB.transpose().middleCols(mIndices[i], skelNumDofs) * skelMInv; } // Construct int dimA = c * (2 + mNumDir); // dimension of A is c + cd + c mA.resize(dimA, dimA); mA.topLeftCorner(c, c).triangularView<Upper>() = nTmInv * mN; mA.topLeftCorner(c, c).triangularView<StrictlyLower>() = mA.topLeftCorner(c, c).transpose(); mA.block(0, c, c, cd).noalias() = nTmInv * mB; mA.block(c, 0, cd, c) = mA.block(0, c, c, cd).transpose(); // since B^T * Minv * N = (N^T * Minv * B)^T mA.block(c, c, cd, cd).triangularView<Upper>() = bTmInv * mB; mA.block(c, c, cd, cd).triangularView<StrictlyLower>() = mA.block(c, c, cd, cd).transpose(); // mA.block(c, c + cd, cd, c) = E * (mDt * mDt); mA.block(c, c + cd, cd, c) = E; // mA.block(c + cd, 0, c, c) = mu * (mDt * mDt); mA.bottomLeftCorner(c, c) = getMuMatrix(); // Note: mu is a diagonal matrix, but we also set the surrounding zeros // mA.block(c + cd, c, c, cd) = -E.transpose() * (mDt * mDt); mA.block(c + cd, c, c, cd) = -E.transpose(); mA.topRightCorner(c, c).setZero(); mA.bottomRightCorner(c, c).setZero(); int cfmSize = getNumContacts() * (1 + mNumDir); for (int i = 0; i < cfmSize; ++i) //add small values to diagnal to keep it away from singular, similar to cfm varaible in ODE mA(i, i) += 0.001 * mA(i, i); // Construct Q mQBar = VectorXd::Zero(dimA); /* VectorXd MinvTauStar(mN.rows()); int rowStart = 0; for (int i = 0; i < mSkels.size(); i++) { int nDof = mSkels[i]->getNumDofs(); if (mSkels[i]->getImmobileState()) { continue; } else { MinvTauStar.segment(rowStart, nDof) = mMInv.block(rowStart, rowStart, nDof, nDof) * mTauStar.segment(rowStart, nDof); } rowStart += nDof; } */ //mQBar.block(0, 0, c, 1) = mN.transpose() * MinvTauStar; //mQBar.block(c, 0, cd, 1) = mB.transpose() * MinvTauStar; mQBar.head(c).noalias() = nTmInv * mTauStar; mQBar.segment(c,cd).noalias() = bTmInv * mTauStar; mQBar /= mDt; }