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 ConstraintDynamics::computeConstraintForces() {
// static Timer t1("t1");
if (getTotalNumDofs() == 0)
return;
mCollisionChecker->clearAllContacts();
mCollisionChecker->checkCollision(true, true);
// t1.startTimer();
mLimitingDofIndex.clear();
for (int i = 0; i < mSkels.size(); i++) {
if (mSkels[i]->getImmobileState() || !mSkels[i]->getJointLimitState())
continue;
for (int j = 0; j < mSkels[i]->getNumDofs(); j++) {
double val = mSkels[i]->getDof(j)->getValue();
double ub = mSkels[i]->getDof(j)->getMax();
double lb = mSkels[i]->getDof(j)->getMin();
if (val >= ub){
mLimitingDofIndex.push_back(mIndices[i] + j + 1);
// cout << "Skeleton " << i << " Dof " << j << " hits upper bound" << endl;
}
if (val <= lb){
mLimitingDofIndex.push_back(-(mIndices[i] + j + 1));
// cout << "Skeleton " << i << " Dof " << j << " hits lower bound" << endl;
}
}
}
if (mCollisionChecker->getNumContacts() == 0 && mLimitingDofIndex.size() == 0) {
for (int i = 0; i < mSkels.size(); i++)
mContactForces[i].setZero();
if (mConstraints.size() == 0) {
for (int i = 0; i < mSkels.size(); i++)
mTotalConstrForces[i].setZero();
} else {
computeConstraintWithoutContact();
}
} else {
fillMatrices();
solve();
applySolution();
}
// t1.stopTimer();
// t1.printScreen();
}
void Rubik3D::display()
{
if(AutoPlayOn)
{
AutoPlayOn=false;
Singleton->resolver();
}
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glLoadIdentity();
glPushMatrix();
gluLookAt(cameraX,cameraY,cameraZ,0.0f,0.0f,0.0f,upX,upY,upZ);
applySolution();
showCube();
glFlush();
glutSwapBuffers();
}
