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::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);
}
}
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::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 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::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::updateMassMat() {
int startRow = 0;
int startCol = 0;
for (int i = 0; i < getNumSkels(); i++) {
if (mSkels[i]->getImmobileState())
continue;
MatrixXd skelMassInv = mSkels[i]->getInvMassMatrix();
mMInv.block(startRow, startCol, skelMassInv.rows(), skelMassInv.cols()) = skelMassInv;
startRow+= skelMassInv.rows();
startCol+= skelMassInv.cols();
}
}
void ContactDynamics::updateTauStar() {
int startRow = 0;
for (int i = 0; i < getNumSkels(); i++) {
if (mSkels[i]->getImmobileState())
continue;
VectorXd tau = mSkels[i]->getExternalForces() + mSkels[i]->getInternalForces();
VectorXd tauStar = (mSkels[i]->getMassMatrix() * mSkels[i]->getQDotVector()) - (mDt * (mSkels[i]->getCombinedVector() - tau));
mTauStar.segment(startRow, tauStar.rows()) = tauStar;
startRow += tauStar.rows();
}
}
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::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;
}