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;
}
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 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);
}
