void ContactDynamics::fillMatrices() { updateMassMat(); updateTauStar(); updateNBMatrices(); // updateNormalMatrix(); // updateBasisMatrix(); MatrixXd E = getContactMatrix(); MatrixXd mu = getMuMatrix(); // Construct the intermediary blocks. MatrixXd Ntranspose = mN.transpose(); MatrixXd Btranspose = mB.transpose(); MatrixXd nTmInv = Ntranspose * mMInv; MatrixXd bTmInv = Btranspose * mMInv; // Construct int c = getNumContacts(); int cd = c * mNumDir; int dimA = c * (2 + mNumDir); // dimension of A is c + cd + c mA.resize(dimA, dimA); mA.topLeftCorner(c, c) = nTmInv * mN; mA.block(0, c, c, cd) = nTmInv * mB; mA.block(c, 0, cd, c) = bTmInv * mN; mA.block(c, c, cd, cd) = bTmInv * mB; // 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) = mu; // 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) = Ntranspose * MinvTauStar; //mQBar.block(c, 0, cd, 1) = Btranspose * MinvTauStar; mQBar.head(c) = nTmInv * mTauStar; mQBar.segment(c,cd) = bTmInv * mTauStar; mQBar /= mDt; }
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; }
void ConstraintDynamics::fillMatrices() { int nContacts = getNumContacts(); int nJointLimits = mLimitingDofIndex.size(); int nConstrs = mConstraints.size(); int cd = nContacts * mNumDir; int dimA = nContacts * (2 + mNumDir) + nJointLimits; mA = MatrixXd::Zero(dimA, dimA); mQBar = VectorXd::Zero(dimA); updateMassMat(); updateTauStar(); MatrixXd augMInv = mMInv; VectorXd tauVec = VectorXd::Zero(getTotalNumDofs()); if (nConstrs > 0) { updateConstraintTerms(); augMInv -= mZ; VectorXd tempVec = mDt * mGInv * mTauHat; for (int i = 0; i < mSkels.size(); i++) { if (mSkels[i]->getImmobileState()) continue; tauVec.segment(mIndices[i], mSkels[i]->getNumDofs()) = mJ[i].transpose() * tempVec; } } tauVec = mMInv * (tauVec + mTauStar); MatrixXd Ntranspose(nContacts, getTotalNumDofs()); MatrixXd Btranspose(cd, getTotalNumDofs()); MatrixXd NTerm(getTotalNumDofs(), nContacts); MatrixXd BTerm(getTotalNumDofs(), cd); if (nContacts > 0) { updateNBMatrices(); MatrixXd E = getContactMatrix(); MatrixXd mu = getMuMatrix(); // Construct the intermediary blocks. Ntranspose = mN.transpose(); Btranspose = mB.transpose(); // Compute NTerm and BTerm NTerm = augMInv * mN; BTerm = augMInv * mB; mA.block(0, 0, nContacts, nContacts) = Ntranspose * NTerm; mA.block(0, nContacts, nContacts, cd) = Ntranspose * BTerm; mA.block(nContacts, 0, cd, nContacts) = Btranspose * NTerm; mA.block(nContacts, nContacts, cd, cd) = Btranspose * BTerm; mA.block(nContacts, nContacts + cd, cd, nContacts) = E; mA.block(nContacts + cd, 0, nContacts, nContacts) = mu; mA.block(nContacts + cd, nContacts, nContacts, cd) = -E.transpose(); mQBar.segment(0, nContacts) = Ntranspose * tauVec; mQBar.segment(nContacts, cd) = Btranspose * tauVec; } if (nJointLimits > 0) { int jointStart = 2 * nContacts + cd; for (int i = 0; i < nJointLimits; i++) for (int j = 0; j < nJointLimits; j++) { if (mLimitingDofIndex[i] * mLimitingDofIndex[j] < 0) mA(jointStart + i, jointStart + j) = -augMInv(abs(mLimitingDofIndex[i]) - 1, abs(mLimitingDofIndex[j]) - 1); else mA(jointStart + i, jointStart + j) = augMInv(abs(mLimitingDofIndex[i]) - 1, abs(mLimitingDofIndex[j]) - 1); } for (int i = 0; i < nJointLimits; i++) { if (mLimitingDofIndex[i] > 0) // hitting upper bound mQBar[jointStart + i] = -tauVec[abs(mLimitingDofIndex[i]) - 1]; else // hitting lower bound mQBar[jointStart + i] = tauVec[abs(mLimitingDofIndex[i]) - 1]; } if (nContacts > 0) { MatrixXd STerm(mMInv.rows(), nJointLimits); for (int i = 0; i < nJointLimits; i++) { if (mLimitingDofIndex[i] > 0) // hitting upper bound STerm.col(i) = -augMInv.col(mLimitingDofIndex[i] - 1); else STerm.col(i) = augMInv.col(abs(mLimitingDofIndex[i]) - 1); } mA.block(0, jointStart, nContacts, nJointLimits) = Ntranspose * STerm; mA.block(nContacts, jointStart, cd, nJointLimits) = Btranspose * STerm; for (int i = 0; i < nJointLimits; i++) { if (mLimitingDofIndex[i] > 0) { //hitting uppder bound mA.block(jointStart + i, 0, 1, nContacts) = -NTerm.row(mLimitingDofIndex[i] - 1); mA.block(jointStart + i, nContacts, 1, cd) = -BTerm.row(mLimitingDofIndex[i] - 1); } else { mA.block(jointStart + i, 0, 1, nContacts) = NTerm.row(abs(mLimitingDofIndex[i]) - 1); mA.block(jointStart + i, nContacts, 1, cd) = BTerm.row(abs(mLimitingDofIndex[i]) - 1); } } } } mQBar /= mDt; 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); }