MatrixXd ConstraintDynamics::getContactMatrix() const {
MatrixXd E(MatrixXd::Zero(getNumContacts() * mNumDir, getNumContacts()));
MatrixXd column(MatrixXd::Ones(mNumDir, 1));
for (int i = 0; i < getNumContacts(); i++) {
E.block(i * mNumDir, i, mNumDir, 1) = column;
}
return E;
}
/*
void ContactDynamics::updateBasisMatrix() {
mB = MatrixXd::Zero(getNumTotalDofs(), getNumContacts() * getNumContactDirections());
for (int i = 0; i < getNumContacts(); i++) {
ContactPoint& c = mCollisionChecker->getContact(i);
Vector3d p = c.point;
int skelID1 = mBodyIndexToSkelIndex[c.bdID1];
int skelID2 = mBodyIndexToSkelIndex[c.bdID2];
MatrixXd B21;
MatrixXd B12;
if (!mSkels[skelID1]->getImmobileState()) {
int index1 = mIndices[skelID1];
int NDOF1 = c.bd1->getSkel()->getNumDofs();
B21 = getTangentBasisMatrix(p, c.normal);
MatrixXd J21 = getJacobian(c.bd1, p);
mB.block(index1, i * getNumContactDirections(), NDOF1, getNumContactDirections()) = J21.transpose() * B21;
// cout << "B21: " << B21 << endl;
}
if (!mSkels[skelID2]->getImmobileState()) {
int index2 = mIndices[skelID2];
int NDOF2 = c.bd2->getSkel()->getNumDofs();
if (B21.rows() == 0)
B12 = getTangentBasisMatrix(p, -c.normal);
else
B12 = -B21;
MatrixXd J12 = getJacobian(c.bd2, p);
mB.block(index2, i * getNumContactDirections(), NDOF2, getNumContactDirections()) = J12.transpose() * B12;
// cout << "B12: " << B12 << endl;
}
}
return;
}
*/
MatrixXd ContactDynamics::getContactMatrix() const {
int numDir = getNumContactDirections();
MatrixXd E(MatrixXd::Zero(getNumContacts() * numDir, getNumContacts()));
MatrixXd column(MatrixXd::Ones(numDir, 1));
for (int i = 0; i < getNumContacts(); i++) {
E.block(i * numDir, i, numDir, 1) = column;
}
// return E / (mDt * mDt);
return E;
}
void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btTransform& trB)
{
int i;
#ifdef DEBUG_PERSISTENCY
printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n",
trA.getOrigin().getX(),
trA.getOrigin().getY(),
trA.getOrigin().getZ(),
trB.getOrigin().getX(),
trB.getOrigin().getY(),
trB.getOrigin().getZ());
#endif //DEBUG_PERSISTENCY
/// first refresh worldspace positions and distance
for (i=getNumContacts()-1;i>=0;i--)
{
btManifoldPoint &manifoldPoint = m_pointCache[i];
manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA );
manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB );
manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA - manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
manifoldPoint.m_lifeTime++;
}
/// then
btScalar distance2d;
btVector3 projectedDifference,projectedPoint;
for (i=getNumContacts()-1;i>=0;i--)
{
btManifoldPoint &manifoldPoint = m_pointCache[i];
//contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
if (!validContactDistance(manifoldPoint))
{
removeContactPoint(i);
} else
{
//contact also becomes invalid when relative movement orthogonal to normal exceeds margin
projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
distance2d = projectedDifference.dot(projectedDifference);
if (distance2d > getContactBreakingThreshold()*getContactBreakingThreshold() )
{
removeContactPoint(i);
} else
{
//contact point processed callback
if (gContactProcessedCallback)
(*gContactProcessedCallback)(manifoldPoint,m_body0,m_body1);
}
}
}
#ifdef DEBUG_PERSISTENCY
DebugPersistency();
#endif //
}
typename BVolCollision<BasicTraits,BVOL>::ResultT
BVolCollision<BasicTraits,BVOL>::BVolBVolCollision
(GroupType* g0, GroupType* g1)
{
++m_numBVolBVolTests;
// perform BV-BV overlap test
Real t = bvolIntersect(g0->getBVol(), g1->getBVol());
if (t < 0.0f) {
return DoubleTraverserBase<BasicTraits>::QUIT;
}
const DoubleTraverserFixedBase<BasicTraits>* trav
= dynamic_cast<const DoubleTraverserFixedBase<BasicTraits>*>(getTraverser());
if (trav != NULL
&& trav->getDepth0() == getCurrentDepth0(trav, g0)
&& trav->getDepth1() == getCurrentDepth1(trav, g1)) {
// Keep track of colliding pairs
// check if first collision already found
if (getNumContacts() == 0) {
m_contacts.push_back(CollisionPair((AdapterType*)g0->findLeaf(),
(AdapterType*)g1->findLeaf()));
CollisionInterface<OpenSGTriangleBase<BasicTraits>,Real>
result(m_contacts[m_contacts.size()-1]);
result.getData() = t;
}
CollisionInterface<OpenSGTriangleBase<BasicTraits>,Real>
result(m_contacts[0]);
if (t < result.getData()) {
result.getData() = t;
m_contacts[0].setFirst((AdapterType*)g0->findLeaf());
m_contacts[0].setSecond((AdapterType*)g1->findLeaf());
}
}
return DoubleTraverserBase<BasicTraits>::CONTINUE;
}
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];
}
}
int btPersistentManifold::addManifoldPoint(const btManifoldPoint& newPoint)
{
btAssert(validContactDistance(newPoint));
int insertIndex = getNumContacts();
if (insertIndex == MANIFOLD_CACHE_SIZE)
{
#if MANIFOLD_CACHE_SIZE >= 4
//sort cache so best points come first, based on area
insertIndex = sortCachedPoints(newPoint);
#else
insertIndex = 0;
#endif
clearUserCache(m_pointCache[insertIndex]);
} else
{
m_cachedPoints++;
}
if (insertIndex<0)
insertIndex=0;
btAssert(m_pointCache[insertIndex].m_userPersistentData==0);
m_pointCache[insertIndex] = newPoint;
return insertIndex;
}
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];
}
}
typename PrecomputedAlignCollision<BasicTraits,BVOL>::ResultT
PrecomputedAlignCollision<BasicTraits,BVOL>::BVolBVolCollision (GroupType* g0, GroupType* g1)
{
// check if first collision already found
if (getStopFirst() && getNumContacts()>0) {
return DoubleTraverserBase<BasicTraits>::QUIT;
}
++m_numBVolBVolTests;
// perform BV-BV overlap test
const BVOL& dop1 = g0->getBVol();
const BVOL& dop2 = g1->getBVol();
const Real* center = g1->getData().getRotationCenter();
u32 k, kk, mink, maxk;
Real min2, max2, correct;
for (k=0, kk=BVOL::Size; k<BVOL::Size; ++k, ++kk) {
maxk = m_perm[k];
mink = m_perm[kk];
if (maxk < BVOL::Size) {
max2 = dop2.maxVector()[maxk] - center[maxk];
min2 = dop2.minVector()[maxk] - center[maxk];
} else {
max2 = -dop2.minVector()[mink] + center[mink];
min2 = -dop2.maxVector()[mink] + center[mink];
}
min2 *= m_scale;
max2 *= m_scale;
correct = m_proj[k].dot(g1->getCenter());
#ifdef GV_WITH_SUBCONES
min2 = min2*BVOL::unitDopAngleTable(g1->getData().getOccupancy(mink)[m_submask[kk]],
m_mask[kk],
g1->getData().getOuterMost(mink)[m_submask[kk]])
+ correct;
max2 = max2*BVOL::unitDopAngleTable(g1->getData().getOccupancy(maxk)[m_submask[k]],
m_mask[k],
g1->getData().getOuterMost(maxk)[m_submask[k]])
+ correct;
#else
min2 = min2*BVOL::unitDopAngleTable(g1->getData().getOccupancy(mink)[0],
m_mask[kk],
g1->getData().getOuterMost(mink)[0])
+ correct;
max2 = max2*BVOL::unitDopAngleTable(g1->getData().getOccupancy(maxk)[0],
m_mask[k],
g1->getData().getOuterMost(maxk)[0])
+ correct;
#endif
if (stdMin(dop1.maxVector()[k], max2+m_M1Offset[k]) < stdMax(dop1.minVector()[k], min2+m_M1Offset[k])) {
return DoubleTraverserBase<BasicTraits>::QUIT;
}
}
return DoubleTraverserBase<BasicTraits>::CONTINUE;
}
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;
}
}
}
typename BVolCollision<BasicTraits,BVOL>::ResultT
BVolCollision<BasicTraits,BVOL>::PrimBVolCollision
(AdapterType* p0, GroupType* g1)
{
++m_numPrimBVolTests;
// perform BV-BV overlap test
#if defined(GV_BVOLS_IN_MIXEDTESTS)
Real t = bvolIntersect(p0->getBVol(), g1->getBVol());
if (t < 0.0f) {
return DoubleTraverserBase<BasicTraits>::QUIT;
}
const DoubleTraverserFixedBase<BasicTraits>* trav
= dynamic_cast<const DoubleTraverserFixedBase<BasicTraits>*>(getTraverser());
if (trav != NULL
&& trav->getDepth1() == getCurrentDepth1(trav, g1)) {
// Keep track of colliding pairs
// check if first collision already found
if (getNumContacts() == 0) {
m_contacts.push_back(CollisionPair(p0, (AdapterType*)g1->findLeaf()));
CollisionInterface<OpenSGTriangleBase<BasicTraits>,Real>
result(m_contacts[m_contacts.size()-1]);
result.getData() = t;
}
CollisionInterface<OpenSGTriangleBase<BasicTraits>,Real>
result(m_contacts[0]);
if (t < result.getData()) {
result.getData() = t;
m_contacts[0].setFirst(p0);
m_contacts[0].setSecond((AdapterType*)g1->findLeaf());
}
}
#else
// Transform from model space 0 to model space 1
PointClass d0, d1, d2;
m_M0ToM1.mult(p0->getPosition(0), d0);
m_M0ToM1.mult(p0->getPosition(1), d1);
m_M0ToM1.mult(p0->getPosition(2), d2);
if (!g1->getBVol().checkIntersect(d0, d1, d2)) {
return DoubleTraverserBase<BasicTraits>::QUIT;
}
#endif
return DoubleTraverserBase<BasicTraits>::CONTINUE;
}
typename BVolCollision<BasicTraits,BVOL>::ResultT
BVolCollision<BasicTraits,BVOL>::PrimPrimCollision
(AdapterType* p0, AdapterType* p1)
{
++m_numPrimPrimTests;
#if defined(GV_BVOLS_IN_MIXEDTESTS)
Real t = (this->*f_primIntersect)(p0, p1);
if (t >= 0.0f) {
// Keep track of colliding pairs
// check if first collision already found
if (!getStopFirst() || getNumContacts() == 0) {
m_contacts.push_back(CollisionPair(p0, p1));
CollisionInterface<OpenSGTriangleBase<BasicTraits>,Real>
result(m_contacts[m_contacts.size()-1]);
result.getData() = t;
}
CollisionInterface<OpenSGTriangleBase<BasicTraits>,Real>
result(m_contacts[0]);
if (t < result.getData()) {
result.getData() = t;
m_contacts[0].setFirst(p0);
m_contacts[0].setSecond(p1);
}
}
#else
// Transform from model space 1 to model space 0
PointClass d0, d1, d2;
m_M1ToM0.mult(p1->getPosition(0), d0);
m_M1ToM0.mult(p1->getPosition(1), d1);
m_M1ToM0.mult(p1->getPosition(2), d2);
// Perform triangle-triangle overlap test
if (genvis::triTriOverlap(p0->getPosition(0),
p0->getPosition(1),
p0->getPosition(2),
d0,
d1,
d2)) {
// Keep track of colliding pairs
m_contacts.push_back(CollisionPair(p0, p1));
}
#endif
return DoubleTraverserBase<BasicTraits>::CONTINUE;
}
int btPersistentManifold::getCacheEntry(const btManifoldPoint& newPoint) const
{
btScalar shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold();
int size = getNumContacts();
int nearestPoint = -1;
for( int i = 0; i < size; i++ )
{
const btManifoldPoint &mp = m_pointCache[i];
btVector3 diffA = mp.m_localPointA- newPoint.m_localPointA;
const btScalar distToManiPoint = diffA.dot(diffA);
if( distToManiPoint < shortestDist )
{
shortestDist = distToManiPoint;
nearestPoint = i;
}
}
return nearestPoint;
}
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();
}
}
MatrixXd ContactDynamics::getMuMatrix() const {
int c = getNumContacts();
// TESTING CODE BEGIN (create a frictionless node)
/* MatrixXd mat(MatrixXd::Identity(c, c));
double mu;
for (int i = 0; i < c; i++) {
ContactPoint& contact = mCollisionChecker->getContact(i);
if (contact.bdID1 == 0 || contact.bdID2 == 0)
mu = 0.0;
else
mu = mMu;
mat(i, i) = mu;
}
return mat / (mDt * mDt);
// TESTING CODE END
*/
// return (MatrixXd::Identity(c, c) * mMu / (mDt * mDt));
return MatrixXd::Identity(c, c) * mMu;
}
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);
}
MatrixXd ConstraintDynamics::getMuMatrix() const {
int c = getNumContacts();
return MatrixXd::Identity(c, c) * mMu;
}
bool ContactDynamics::solve() {
lcpsolver::LCPSolver solver = lcpsolver::LCPSolver();
bool b = solver.Solve(mA, mQBar, mX, getNumContacts(), mMu, mNumDir, true);
return b;
}
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;
}
typename PrecomputedAlignCollision<BasicTraits,BVOL>::ResultT
PrecomputedAlignCollision<BasicTraits,BVOL>::BVolPrimCollision (GroupType* g0, AdapterType* p1)
{
// check if first collision already found
if (getStopFirst() && getNumContacts()>0) {
return DoubleTraverserBase<BasicTraits>::QUIT;
}
++m_numBVolPrimTests;
#if 0
// perform BV-BV overlap test
const BVOL& dop1 = g0->getBVol();
const BVOL& dop2 = p1->getBVol();
const Real* center = p1->getData().getRotationCenter();
u32 k, kk, mink, maxk;
Real min2, max2, correct;
for (k=0, kk=BVOL::Size; k<BVOL::Size; ++k, ++kk) {
maxk = m_perm[k];
mink = m_perm[kk];
if (maxk < BVOL::Size) {
max2 = dop2.maxVector()[maxk] - center[maxk];
min2 = dop2.minVector()[maxk] - center[maxk];
} else {
max2 = -dop2.minVector()[mink] + center[mink];
min2 = -dop2.maxVector()[mink] + center[mink];
}
min2 *= m_scale;
max2 *= m_scale;
correct = m_proj[k].dot(p1->getCenter());
#ifdef GV_WITH_SUBCONES
min2 = min2*BVOL::unitDopAngleTable(p1->getData().getOccupancy(mink)[m_submask[kk]],
m_mask[kk],
p1->getData().getOuterMost(mink)[m_submask[kk]])
+ correct;
max2 = max2*BVOL::unitDopAngleTable(p1->getData().getOccupancy(maxk)[m_submask[k]],
m_mask[k],
p1->getData().getOuterMost(maxk)[m_submask[k]])
+ correct;
#else
min2 = min2*BVOL::unitDopAngleTable(p1->getData().getOccupancy(mink)[0],
m_mask[kk],
p1->getData().getOuterMost(mink)[0])
+ correct;
max2 = max2*BVOL::unitDopAngleTable(p1->getData().getOccupancy(maxk)[0],
m_mask[k],
p1->getData().getOuterMost(maxk)[0])
+ correct;
#endif
if (stdMin(dop1.maxVector()[k], max2+m_M1Offset[k]) < stdMax(dop1.minVector()[k], min2+m_M1Offset[k])) {
return DoubleTraverserBase<BasicTraits>::QUIT;
}
}
#else
// Transform from model space 1 to model space 0
PointClass d0, d1, d2;
m_M1ToM0.mult(p1->getPosition(0), d0);
m_M1ToM0.mult(p1->getPosition(1), d1);
m_M1ToM0.mult(p1->getPosition(2), d2);
if (!g0->getBVol().checkIntersect(d0, d1, d2)) {
return DoubleTraverserBase<BasicTraits>::QUIT;
}
#endif
return DoubleTraverserBase<BasicTraits>::CONTINUE;
}
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;
}
