void ContactDynamics::applySolution() {
int c = getNumContacts();
// First compute the external forces
int nRows = mMInv.rows(); // a hacky way to get the dimension
VectorXd forces(VectorXd::Zero(nRows));
VectorXd f_n = mX.head(c);
VectorXd f_d = mX.segment(c, c * mNumDir);
VectorXd lambda = mX.tail(c);
forces = (mN * f_n) + (mB * f_d);
// Next, apply the external forces skeleton by skeleton.
int startRow = 0;
for (int i = 0; i < getNumSkels(); i++) {
if (mSkels[i]->getImmobileState())
continue;
int nDof = mSkels[i]->getNumDofs();
mConstrForces[i] = forces.segment(startRow, nDof);
startRow += nDof;
}
for (int i = 0; i < c; i++) {
ContactPoint& contact = mCollisionChecker->getContact(i);
contact.force = getTangentBasisMatrix(contact.point, contact.normal) * f_d.segment(i * mNumDir, mNumDir) + contact.normal * f_n[i];
}
}
void ContactDynamics::applySolution() {
const int c = getNumContacts();
// First compute the external forces
VectorXd f_n = mX.head(c);
VectorXd f_d = mX.segment(c, c * mNumDir);
VectorXd lambda = mX.tail(c);
VectorXd forces = mN * f_n;
forces.noalias() += mB * f_d;
// Next, apply the external forces skeleton by skeleton.
int startRow = 0;
for (int i = 0; i < getNumSkels(); i++) {
if (mSkels[i]->getImmobileState())
continue;
int nDof = mSkels[i]->getNumDofs();
mConstrForces[i] = forces.segment(startRow, nDof);
startRow += nDof;
}
for (int i = 0; i < c; i++) {
Contact& contact = mCollisionChecker->getContact(i);
contact.force.noalias() = getTangentBasisMatrix(contact.point, contact.normal) * f_d.segment(i * mNumDir, mNumDir);
contact.force += contact.normal * f_n[i];
}
}
void ConstraintDynamics::applySolution() {
VectorXd contactForces(VectorXd::Zero(getTotalNumDofs()));
VectorXd jointLimitForces(VectorXd::Zero(getTotalNumDofs()));
if (getNumContacts() > 0) {
VectorXd f_n = mX.head(getNumContacts());
VectorXd f_d = mX.segment(getNumContacts(), getNumContacts() * mNumDir);
contactForces.noalias() = mN * f_n;
contactForces.noalias() += mB * f_d;
for (int i = 0; i < getNumContacts(); i++) {
Contact& contact = mCollisionChecker->getContact(i);
contact.force.noalias() = getTangentBasisMatrix(contact.point, contact.normal) * f_d.segment(i * mNumDir, mNumDir);
contact.force.noalias() += contact.normal * f_n[i];
}
}
for (int i = 0; i < mLimitingDofIndex.size(); i++) {
if (mLimitingDofIndex[i] > 0) { // hitting upper bound
jointLimitForces[mLimitingDofIndex[i] - 1] = -mX[getNumContacts() * (2 + mNumDir) + i];
}else{
jointLimitForces[abs(mLimitingDofIndex[i]) - 1] = mX[getNumContacts() * (2 + mNumDir) + i];
}
}
VectorXd lambda = VectorXd::Zero(mGInv.rows());
for (int i = 0; i < mSkels.size(); i++) {
if (mSkels[i]->getImmobileState())
continue;
mContactForces[i] = contactForces.segment(mIndices[i], mSkels[i]->getNumDofs());
mTotalConstrForces[i] = mContactForces[i] + jointLimitForces.segment(mIndices[i], mSkels[i]->getNumDofs());
if (mConstraints.size() > 0) {
VectorXd tempVec = mGInv * (mTauHat - mJMInv[i] * (contactForces.segment(mIndices[i], mSkels[i]->getNumDofs()) + jointLimitForces.segment(mIndices[i], mSkels[i]->getNumDofs())));
mTotalConstrForces[i] += mJ[i].transpose() * tempVec;
lambda += tempVec;
}
}
int count = 0;
for (int i = 0; i < mConstraints.size(); i++) {
mConstraints[i]->setLagrangeMultipliers(lambda.segment(count, mConstraints[i]->getNumRows()));
count += mConstraints[i]->getNumRows();
}
}
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;
}
}
}
