diff --git a/examples/DeformableDemo/DeformableSelfCollision.cpp b/examples/DeformableDemo/DeformableSelfCollision.cpp index f2734371d..9806bdb37 100644 --- a/examples/DeformableDemo/DeformableSelfCollision.cpp +++ b/examples/DeformableDemo/DeformableSelfCollision.cpp @@ -157,7 +157,7 @@ void DeformableSelfCollision::addCloth(btVector3 origin) getDeformableDynamicsWorld()->addSoftBody(psb); psb->setSelfCollision(true); - btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2,0.02, true); + btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2,0.2, true); psb->setSpringStiffness(4); getDeformableDynamicsWorld()->addForce(psb, mass_spring); m_forces.push_back(mass_spring); diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index 6c8fb26fb..a773ab430 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -363,10 +363,10 @@ void GraspDeformable::initPhysics() psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF; - psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; +// psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; getDeformableDynamicsWorld()->addSoftBody(psb); // getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(.0,0.0, true)); - getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(10,0.05, true)); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(10,1, true)); getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); } diff --git a/src/BulletSoftBody/btConjugateResidual.h b/src/BulletSoftBody/btConjugateResidual.h index 0f2b79713..7b211c417 100644 --- a/src/BulletSoftBody/btConjugateResidual.h +++ b/src/BulletSoftBody/btConjugateResidual.h @@ -32,14 +32,10 @@ class btConjugateResidual // z = M^(-1) * temp_p = M^(-1) * A * p int max_iterations; btScalar tolerance_squared, best_r; - int count; - int total_it; public: btConjugateResidual(const int max_it_in) : max_iterations(max_it_in) { - count = 0; - total_it = 0; tolerance_squared = 1e-2; } @@ -48,11 +44,6 @@ public: // return the number of iterations taken int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false) { - ++count; - if (count == 1000) - { - printf("total_it = %d", total_it); - } BT_PROFILE("CRSolve"); btAssert(x.size() == b.size()); reinitialize(b); @@ -79,7 +70,6 @@ public: temp_r = temp_p; r_dot_Ar = dot(r, temp_r); for (int k = 1; k <= max_iterations; k++) { - ++total_it; // z = M^(-1) * Ap A.precondition(temp_p, z); // alpha = r^T * A * r / (Ap)^T * M^-1 * Ap) diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index 3a26e05d5..5381ee626 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -23,13 +23,15 @@ btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAligned , m_backupVelocity(backup_v) , m_implicit(false) { -// m_preconditioner = new MassPreconditioner(m_softBodies); - m_preconditioner = new DiagonalPreconditioner(m_softBodies, m_projection, m_lf, m_dt, m_implicit); + m_massPreconditioner = new MassPreconditioner(m_softBodies); + m_KKTPreconditioner = new KKTPreconditioner(m_softBodies, m_projection, m_lf, m_dt, m_implicit); + m_preconditioner = m_KKTPreconditioner; } btDeformableBackwardEulerObjective::~btDeformableBackwardEulerObjective() { - delete m_preconditioner; + delete m_KKTPreconditioner; + delete m_massPreconditioner; } void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar dt) diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index dcabc5ef4..86579e71a 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -40,6 +40,8 @@ public: const TVStack& m_backupVelocity; btAlignedObjectArray m_nodes; bool m_implicit; + MassPreconditioner* m_massPreconditioner; + KKTPreconditioner* m_KKTPreconditioner; btDeformableBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v); diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index c3a7051e5..132699c54 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -26,6 +26,7 @@ btDeformableBodySolver::btDeformableBodySolver() , m_maxNewtonIterations(5) , m_newtonTolerance(1e-4) , m_lineSearch(false) +, m_useProjection(false) { m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity); } @@ -42,15 +43,21 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt) { m_objective->computeResidual(solverdt, m_residual); m_objective->applyDynamicFriction(m_residual); -// computeStep(m_dv, m_residual); - TVStack rhs, x; - m_objective->addLagrangeMultiplierRHS(m_residual, m_dv, rhs); - m_objective->addLagrangeMultiplier(m_dv, x); - m_objective->m_preconditioner->reinitialize(true); - computeStep(x, rhs); - for (int i = 0; iaddLagrangeMultiplierRHS(m_residual, m_dv, rhs); + m_objective->addLagrangeMultiplier(m_dv, x); + m_objective->m_preconditioner->reinitialize(true); + computeStep(x, rhs); + for (int i = 0; icomputeResidual(solverdt, m_residual); if (m_objective->computeNorm(m_residual) < m_newtonTolerance && i > 0) { @@ -210,7 +217,10 @@ void btDeformableBodySolver::updateDv(btScalar scale) void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual) { - m_cr.solve(*m_objective, ddv, residual, false); + if (m_useProjection) + m_cg.solve(*m_objective, ddv, residual, false); + else + m_cr.solve(*m_objective, ddv, residual, false); } void btDeformableBodySolver::reinitialize(const btAlignedObjectArray& softBodies, btScalar dt) diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index 235e8f2be..d4e5f4c60 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -49,6 +49,7 @@ protected: public: // handles data related to objective function btDeformableBackwardEulerObjective* m_objective; + bool m_useProjection; btDeformableBodySolver(); diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index 046a770cf..2864446de 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -141,7 +141,8 @@ btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btS m_total_normal_dv.setZero(); m_total_tangent_dv.setZero(); // The magnitude of penetration is the depth of penetration. - m_penetration = btMin(btScalar(0),c.m_cti.m_offset); + m_penetration = c.m_cti.m_offset; +// m_penetration = btMin(btScalar(0),c.m_cti.m_offset); } btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other) @@ -326,14 +327,16 @@ void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impul } /* ================ Face vs. Rigid =================== */ -btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal) +btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting) : m_face(contact.m_face) +, m_useStrainLimiting(useStrainLimiting) , btDeformableRigidContactConstraint(contact, infoGlobal) { } btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other) : m_face(other.m_face) +, m_useStrainLimiting(other.m_useStrainLimiting) , btDeformableRigidContactConstraint(other) { } @@ -380,60 +383,62 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul v1 -= dv * contact->m_weights[1]; if (im2 > 0) v2 -= dv * contact->m_weights[2]; - - btScalar relaxation = 1./btScalar(m_infoGlobal->m_numIterations); - btScalar m01 = (relaxation/(im0 + im1)); - btScalar m02 = (relaxation/(im0 + im2)); - btScalar m12 = (relaxation/(im1 + im2)); - #ifdef USE_STRAIN_RATE_LIMITING - // apply strain limiting to prevent the new velocity to change the current length of the edge by more than 1%. - btScalar p = 0.01; - btVector3& x0 = face->m_n[0]->m_x; - btVector3& x1 = face->m_n[1]->m_x; - btVector3& x2 = face->m_n[2]->m_x; - const btVector3 x_diff[3] = {x1-x0, x2-x0, x2-x1}; - const btVector3 v_diff[3] = {v1-v0, v2-v0, v2-v1}; - btVector3 u[3]; - btScalar x_diff_dot_u, dn[3]; - btScalar dt = m_infoGlobal->m_timeStep; - for (int i = 0; i < 3; ++i) + if (m_useStrainLimiting) { - btScalar x_diff_norm = x_diff[i].safeNorm(); - btScalar x_diff_norm_new = (x_diff[i] + v_diff[i] * dt).safeNorm(); - btScalar strainRate = x_diff_norm_new/x_diff_norm; - u[i] = v_diff[i]; - u[i].safeNormalize(); - if (x_diff_norm == 0 || (1-p <= strainRate && strainRate <= 1+p)) + btScalar relaxation = 1./btScalar(m_infoGlobal->m_numIterations); + btScalar m01 = (relaxation/(im0 + im1)); + btScalar m02 = (relaxation/(im0 + im2)); + btScalar m12 = (relaxation/(im1 + im2)); + #ifdef USE_STRAIN_RATE_LIMITING + // apply strain limiting to prevent the new velocity to change the current length of the edge by more than 1%. + btScalar p = 0.01; + btVector3& x0 = face->m_n[0]->m_x; + btVector3& x1 = face->m_n[1]->m_x; + btVector3& x2 = face->m_n[2]->m_x; + const btVector3 x_diff[3] = {x1-x0, x2-x0, x2-x1}; + const btVector3 v_diff[3] = {v1-v0, v2-v0, v2-v1}; + btVector3 u[3]; + btScalar x_diff_dot_u, dn[3]; + btScalar dt = m_infoGlobal->m_timeStep; + for (int i = 0; i < 3; ++i) { - dn[i] = 0; - continue; + btScalar x_diff_norm = x_diff[i].safeNorm(); + btScalar x_diff_norm_new = (x_diff[i] + v_diff[i] * dt).safeNorm(); + btScalar strainRate = x_diff_norm_new/x_diff_norm; + u[i] = v_diff[i]; + u[i].safeNormalize(); + if (x_diff_norm == 0 || (1-p <= strainRate && strainRate <= 1+p)) + { + dn[i] = 0; + continue; + } + x_diff_dot_u = btDot(x_diff[i], u[i]); + btScalar s; + if (1-p > strainRate) + { + s = 1/dt * (-x_diff_dot_u - btSqrt(x_diff_dot_u*x_diff_dot_u + (p*p-2*p) * x_diff_norm * x_diff_norm)); + } + else + { + s = 1/dt * (-x_diff_dot_u + btSqrt(x_diff_dot_u*x_diff_dot_u + (p*p+2*p) * x_diff_norm * x_diff_norm)); + } + // x_diff_norm_new = (x_diff[i] + s * u[i] * dt).safeNorm(); + // strainRate = x_diff_norm_new/x_diff_norm; + dn[i] = s - v_diff[i].safeNorm(); } - x_diff_dot_u = btDot(x_diff[i], u[i]); - btScalar s; - if (1-p > strainRate) - { - s = 1/dt * (-x_diff_dot_u - btSqrt(x_diff_dot_u*x_diff_dot_u + (p*p-2*p) * x_diff_norm * x_diff_norm)); - } - else - { - s = 1/dt * (-x_diff_dot_u + btSqrt(x_diff_dot_u*x_diff_dot_u + (p*p+2*p) * x_diff_norm * x_diff_norm)); - } - // x_diff_norm_new = (x_diff[i] + s * u[i] * dt).safeNorm(); - // strainRate = x_diff_norm_new/x_diff_norm; - dn[i] = s - v_diff[i].safeNorm(); + btVector3 dv0 = im0 * (m01 * u[0]*(-dn[0]) + m02 * u[1]*-(dn[1])); + btVector3 dv1 = im1 * (m01 * u[0]*(dn[0]) + m12 * u[2]*(-dn[2])); + btVector3 dv2 = im2 * (m12 * u[2]*(dn[2]) + m02 * u[1]*(dn[1])); + #else + // apply strain limiting to prevent undamped modes + btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0)); + btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1)); + btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2)); + #endif + v0 += dv0; + v1 += dv1; + v2 += dv2; } - btVector3 dv0 = im0 * (m01 * u[0]*(-dn[0]) + m02 * u[1]*-(dn[1])); - btVector3 dv1 = im1 * (m01 * u[0]*(dn[0]) + m12 * u[2]*(-dn[2])); - btVector3 dv2 = im2 * (m12 * u[2]*(dn[2]) + m02 * u[1]*(dn[1])); -#else - // apply strain limiting to prevent undamped modes - btVector3 dv0 = im0 * (m01 * (v1-v0) + m02 * (v2-v0)); - btVector3 dv1 = im1 * (m01 * (v0-v1) + m12 * (v2-v1)); - btVector3 dv2 = im2 * (m12 * (v1-v2) + m02 * (v0-v2)); -#endif -// v0 += dv0; -// v1 += dv1; -// v2 += dv2; } /* ================ Face vs. Node =================== */ diff --git a/src/BulletSoftBody/btDeformableContactConstraint.h b/src/BulletSoftBody/btDeformableContactConstraint.h index abcac438c..9f9d5bf0a 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.h +++ b/src/BulletSoftBody/btDeformableContactConstraint.h @@ -204,9 +204,10 @@ class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactCo { public: const btSoftBody::Face* m_face; - btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal); + bool m_useStrainLimiting; + btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact, const btContactSolverInfo& infoGlobal, bool useStrainLimiting); btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other); - btDeformableFaceRigidContactConstraint(){} + btDeformableFaceRigidContactConstraint(): m_useStrainLimiting(false) {} virtual ~btDeformableFaceRigidContactConstraint() { } diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 0eab2d1e0..22ca8bf58 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -142,7 +142,7 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in { continue; } - btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal); + btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal, m_useStrainLimiting); btVector3 va = constraint.getVa(); btVector3 vb = constraint.getVb(); const btVector3 vr = vb - va; @@ -158,6 +158,42 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in void btDeformableContactProjection::project(TVStack& x) { +#ifndef USE_MGS + const int dim = 3; + for (int index = 0; index < m_projectionsDict.size(); ++index) + { + btAlignedObjectArray& projectionDirs = *m_projectionsDict.getAtIndex(index); + size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1(); + if (projectionDirs.size() >= dim) + { + // static node + x[i].setZero(); + continue; + } + else if (projectionDirs.size() == 2) + { + btVector3 dir0 = projectionDirs[0]; + btVector3 dir1 = projectionDirs[1]; + btVector3 free_dir = btCross(dir0, dir1); + if (free_dir.safeNorm() < SIMD_EPSILON) + { + x[i] -= x[i].dot(dir0) * dir0; + x[i] -= x[i].dot(dir1) * dir1; + } + else + { + free_dir.normalize(); + x[i] = x[i].dot(free_dir) * free_dir; + } + } + else + { + btAssert(projectionDirs.size() == 1); + btVector3 dir0 = projectionDirs[0]; + x[i] -= x[i].dot(dir0) * dir0; + } + } +#else btReducedVector p(x.size()); for (int i = 0; i < m_projections.size(); ++i) { @@ -167,48 +203,138 @@ void btDeformableContactProjection::project(TVStack& x) { x[p.m_indices[i]] -= p.m_vecs[i]; } +#endif } -//void btDeformableContactProjection::project(TVStack& x) -//{ -// const int dim = 3; -// for (int index = 0; index < m_projectionsDict.size(); ++index) -// { -// btAlignedObjectArray& projectionDirs = *m_projectionsDict.getAtIndex(index); -// size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1(); -// if (projectionDirs.size() >= dim) -// { -// // static node -// x[i].setZero(); -// continue; -// } -// else if (projectionDirs.size() == 2) -// { -// btVector3 dir0 = projectionDirs[0]; -// btVector3 dir1 = projectionDirs[1]; -// btVector3 free_dir = btCross(dir0, dir1); -// if (free_dir.safeNorm() < SIMD_EPSILON) -// { -// x[i] -= x[i].dot(dir0) * dir0; -// x[i] -= x[i].dot(dir1) * dir1; -// } -// else -// { -// free_dir.normalize(); -// x[i] = x[i].dot(free_dir) * free_dir; -// } -// } -// else -// { -// btAssert(projectionDirs.size() == 1); -// btVector3 dir0 = projectionDirs[0]; -// x[i] -= x[i].dot(dir0) * dir0; -// } -// } -//} - void btDeformableContactProjection::setProjection() { +#ifndef USE_MGS + BT_PROFILE("btDeformableContactProjection::setProjection"); + btAlignedObjectArray units; + units.push_back(btVector3(1,0,0)); + units.push_back(btVector3(0,1,0)); + units.push_back(btVector3(0,0,1)); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < m_staticConstraints[i].size(); ++j) + { + int index = m_staticConstraints[i][j].m_node->index; + m_staticConstraints[i][j].m_node->m_penetration = SIMD_INFINITY; + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } + } + } + for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) + { + int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; + m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_penetration = SIMD_INFINITY; + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } + } + } + for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) + { + int index = m_nodeRigidConstraints[i][j].m_node->index; + m_nodeRigidConstraints[i][j].m_node->m_penetration = -m_nodeRigidConstraints[i][j].getContact()->m_cti.m_offset; + if (m_nodeRigidConstraints[i][j].m_static) + { + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } + } + } + else + { + if (m_projectionsDict.find(index) == NULL) + { + btAlignedObjectArray projections; + projections.push_back(m_nodeRigidConstraints[i][j].m_normal); + m_projectionsDict.insert(index, projections); + } + else + { + btAlignedObjectArray& projections = *m_projectionsDict[index]; + projections.push_back(m_nodeRigidConstraints[i][j].m_normal); + } + } + } + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) + { + const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; + btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset; + for (int k = 0; k < 3; ++k) + { + face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration); + } + for (int k = 0; k < 3; ++k) + { + btSoftBody::Node* node = face->m_n[k]; + node->m_penetration = true; + int index = node->index; + if (m_faceRigidConstraints[i][j].m_static) + { + if (m_projectionsDict.find(index) == NULL) + { + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } + } + } + else + { + if (m_projectionsDict.find(index) == NULL) + { + btAlignedObjectArray projections; + projections.push_back(m_faceRigidConstraints[i][j].m_normal); + m_projectionsDict.insert(index, projections); + } + else + { + btAlignedObjectArray& projections = *m_projectionsDict[index]; + projections.push_back(m_faceRigidConstraints[i][j].m_normal); + } + } + } + } + } +#else int dof = 0; for (int i = 0; i < m_softBodies.size(); ++i) { @@ -277,11 +403,15 @@ void btDeformableContactProjection::setProjection() const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; btVector3 bary = m_faceRigidConstraints[i][j].getContact()->m_bary; btScalar penetration = -m_faceRigidConstraints[i][j].getContact()->m_cti.m_offset; + for (int k = 0; k < 3; ++k) + { + face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration); + } if (m_faceRigidConstraints[i][j].m_static) { for (int l = 0; l < 3; ++l) { - face->m_n[l]->m_penetration = penetration; + btReducedVector rv(dof); for (int k = 0; k < 3; ++k) { @@ -310,6 +440,7 @@ void btDeformableContactProjection::setProjection() btModifiedGramSchmidt mgs(m_projections); mgs.solve(); m_projections = mgs.m_out; +#endif } void btDeformableContactProjection::checkConstraints(const TVStack& x) @@ -398,7 +529,7 @@ void btDeformableContactProjection::setLagrangeMultiplier() lm.m_num_nodes = 3; for (int k = 0; k<3; ++k) { - face->m_n[k]->m_penetration = penetration; + face->m_n[k]->m_penetration = btMax(face->m_n[k]->m_penetration, penetration); lm.m_indices[k] = face->m_n[k]->index; lm.m_weights[k] = bary[k]; } @@ -419,131 +550,7 @@ void btDeformableContactProjection::setLagrangeMultiplier() } } -//void btDeformableContactProjection::setProjection() -//{ -// BT_PROFILE("btDeformableContactProjection::setProjection"); -// btAlignedObjectArray units; -// units.push_back(btVector3(1,0,0)); -// units.push_back(btVector3(0,1,0)); -// units.push_back(btVector3(0,0,1)); -// for (int i = 0; i < m_softBodies.size(); ++i) -// { -// btSoftBody* psb = m_softBodies[i]; -// if (!psb->isActive()) -// { -// continue; -// } -// for (int j = 0; j < m_staticConstraints[i].size(); ++j) -// { -// int index = m_staticConstraints[i][j].m_node->index; -// m_staticConstraints[i][j].m_node->m_constrained = true; -// if (m_projectionsDict.find(index) == NULL) -// { -// m_projectionsDict.insert(index, units); -// } -// else -// { -// btAlignedObjectArray& projections = *m_projectionsDict[index]; -// for (int k = 0; k < 3; ++k) -// { -// projections.push_back(units[k]); -// } -// } -// } -// for (int j = 0; j < m_nodeAnchorConstraints[i].size(); ++j) -// { -// int index = m_nodeAnchorConstraints[i][j].m_anchor->m_node->index; -// m_nodeAnchorConstraints[i][j].m_anchor->m_node->m_constrained = true; -// if (m_projectionsDict.find(index) == NULL) -// { -// m_projectionsDict.insert(index, units); -// } -// else -// { -// btAlignedObjectArray& projections = *m_projectionsDict[index]; -// for (int k = 0; k < 3; ++k) -// { -// projections.push_back(units[k]); -// } -// } -// } -// for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) -// { -// int index = m_nodeRigidConstraints[i][j].m_node->index; -// m_nodeRigidConstraints[i][j].m_node->m_constrained = true; -// if (m_nodeRigidConstraints[i][j].m_static) -// { -// if (m_projectionsDict.find(index) == NULL) -// { -// m_projectionsDict.insert(index, units); -// } -// else -// { -// btAlignedObjectArray& projections = *m_projectionsDict[index]; -// for (int k = 0; k < 3; ++k) -// { -// projections.push_back(units[k]); -// } -// } -// } -// else -// { -// if (m_projectionsDict.find(index) == NULL) -// { -// btAlignedObjectArray projections; -// projections.push_back(m_nodeRigidConstraints[i][j].m_normal); -// m_projectionsDict.insert(index, projections); -// } -// else -// { -// btAlignedObjectArray& projections = *m_projectionsDict[index]; -// projections.push_back(m_nodeRigidConstraints[i][j].m_normal); -// } -// } -// } -// for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) -// { -// const btSoftBody::Face* face = m_faceRigidConstraints[i][j].m_face; -// for (int k = 0; k < 3; ++k) -// { -// btSoftBody::Node* node = face->m_n[k]; -// node->m_constrained = true; -// int index = node->index; -// if (m_faceRigidConstraints[i][j].m_static) -// { -// if (m_projectionsDict.find(index) == NULL) -// { -// m_projectionsDict.insert(index, units); -// } -// else -// { -// btAlignedObjectArray& projections = *m_projectionsDict[index]; -// for (int k = 0; k < 3; ++k) -// { -// projections.push_back(units[k]); -// } -// } -// } -// else -// { -// if (m_projectionsDict.find(index) == NULL) -// { -// btAlignedObjectArray projections; -// projections.push_back(m_faceRigidConstraints[i][j].m_normal); -// m_projectionsDict.insert(index, projections); -// } -// else -// { -// btAlignedObjectArray& projections = *m_projectionsDict[index]; -// projections.push_back(m_faceRigidConstraints[i][j].m_normal); -// } -// } -// } -// } -// } -//} - - +// void btDeformableContactProjection::applyDynamicFriction(TVStack& f) { for (int i = 0; i < m_softBodies.size(); ++i) @@ -615,8 +622,11 @@ void btDeformableContactProjection::reinitialize(bool nodeUpdated) m_faceRigidConstraints[i].clear(); m_deformableConstraints[i].clear(); } -// m_projectionsDict.clear(); +#ifndef USE_MGS + m_projectionsDict.clear(); +#else m_projections.clear(); +#endif m_lagrangeMultipliers.clear(); } diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index c64d35e97..8d7e94d4f 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -43,11 +43,13 @@ public: // all constraints involving face btAlignedObjectArray m_allFaceConstraints; - +#ifndef USE_MGS // map from node index to projection directions -// btHashMap > m_projectionsDict; - + btHashMap > m_projectionsDict; +#else btAlignedObjectArray m_projections; +#endif + btAlignedObjectArray m_lagrangeMultipliers; // map from node index to static constraint @@ -61,6 +63,8 @@ public: // map from node index to node anchor constraint btAlignedObjectArray > m_nodeAnchorConstraints; + bool m_useStrainLimiting; + btDeformableContactProjection(btAlignedObjectArray& softBodies) : m_softBodies(softBodies) { diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 834ae880a..6b742978e 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -61,6 +61,7 @@ m_deformableBodySolver(deformableBodySolver), m_solverCallback(0) m_internalTime = 0.0; m_implicit = false; m_lineSearch = false; + m_useProjection = true; m_ccdIterations = 5; m_solverDeformableBodyIslandCallback = new DeformableBodyInplaceSolverIslandCallback(constraintSolver, dispatcher); } @@ -253,18 +254,6 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim } } } - - for (int i = 0; i < m_softBodies.size(); ++i) - { - btSoftBody* psb = m_softBodies[i]; - if (psb->isActive() && psb->m_usePostCollisionDamping) - { - for (int j = 0; j < psb->m_nodes.size(); ++j) - { - psb->m_nodes[j].m_v *= psb->m_dampingCoefficient; - } - } - } } void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision() @@ -399,9 +388,11 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) solveContactConstraints(); // set up the directions in which the velocity does not change in the momentum solve -// m_deformableBodySolver->m_objective->m_projection.setProjection(); - m_deformableBodySolver->m_objective->m_projection.setLagrangeMultiplier(); - + if (m_useProjection) + m_deformableBodySolver->m_objective->m_projection.setProjection(); + else + m_deformableBodySolver->m_objective->m_projection.setLagrangeMultiplier(); + // for explicit scheme, m_backupVelocity = v_{n+1}^* // for implicit scheme, m_backupVelocity = v_n // Here, set dv = v_{n+1} - v_n for nodes in contact @@ -538,6 +529,17 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep) dispatchInfo.m_stepCount = 0; dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer(); btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep; + if (m_useProjection) + { + m_deformableBodySolver->m_useProjection = true; +// m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true; + m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_massPreconditioner; + } + else + { + m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_KKTPreconditioner; + } + } diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index 8f88e0d8a..76b58a037 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -49,6 +49,7 @@ class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld int m_ccdIterations; bool m_implicit; bool m_lineSearch; + bool m_useProjection; DeformableBodyInplaceSolverIslandCallback* m_solverDeformableBodyIslandCallback; typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world); diff --git a/src/BulletSoftBody/btPreconditioner.h b/src/BulletSoftBody/btPreconditioner.h index 2799e13e0..c2db448ef 100644 --- a/src/BulletSoftBody/btPreconditioner.h +++ b/src/BulletSoftBody/btPreconditioner.h @@ -81,7 +81,7 @@ public: }; -class DiagonalPreconditioner : public Preconditioner +class KKTPreconditioner : public Preconditioner { const btAlignedObjectArray& m_softBodies; const btDeformableContactProjection& m_projections; @@ -90,7 +90,7 @@ class DiagonalPreconditioner : public Preconditioner const btScalar& m_dt; const bool& m_implicit; public: - DiagonalPreconditioner(const btAlignedObjectArray& softBodies, const btDeformableContactProjection& projections, const btAlignedObjectArray& lf, const btScalar& dt, const bool& implicit) + KKTPreconditioner(const btAlignedObjectArray& softBodies, const btDeformableContactProjection& projections, const btAlignedObjectArray& lf, const btScalar& dt, const bool& implicit) : m_softBodies(softBodies) , m_projections(projections) , m_lf(lf) diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index def4cefbd..c3cb53806 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -220,7 +220,6 @@ void btSoftBody::initDefaults() m_dampingCoefficient = 1.0; m_sleepingThreshold = .4; m_useSelfCollision = false; - m_usePostCollisionDamping = false; m_collisionFlags = 0; m_softSoftCollision = false; } @@ -4052,8 +4051,6 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap if (pcoWrap->getCollisionObject()->isActive() || this->isActive()) { const btTransform wtr = pcoWrap->getWorldTransform(); -// const btTransform ctr = pcoWrap->getWorldTransform(); -// const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length(); const btScalar timemargin = 0; const btScalar basemargin = getCollisionShape()->getMargin(); btVector3 mins; @@ -4065,14 +4062,17 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap maxs); volume = btDbvtVolume::FromMM(mins, maxs); volume.Expand(btVector3(basemargin, basemargin, basemargin)); - btSoftColliders::CollideSDF_RD docollideNode; - docollideNode.psb = this; - docollideNode.m_colObj1Wrap = pcoWrap; - docollideNode.m_rigidBody = prb1; - docollideNode.dynmargin = basemargin + timemargin; - docollideNode.stamargin = basemargin; -// m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode); - + if (m_cfg.collisions & fCollision::SDF_RDN) + { + btSoftColliders::CollideSDF_RD docollideNode; + docollideNode.psb = this; + docollideNode.m_colObj1Wrap = pcoWrap; + docollideNode.m_rigidBody = prb1; + docollideNode.dynmargin = basemargin + timemargin; + docollideNode.stamargin = basemargin; + m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode); + } + if (((pcoWrap->getCollisionObject()->getInternalType() == CO_RIGID_BODY) && (m_cfg.collisions & fCollision::SDF_RDF)) || ((pcoWrap->getCollisionObject()->getInternalType() == CO_FEATHERSTONE_LINK) && (m_cfg.collisions & fCollision::SDF_MDF))) { btSoftColliders::CollideSDF_RDF docollideFace; diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 11055fc24..b03af0736 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -163,7 +163,7 @@ public: RVSmask = 0x000f, ///Rigid versus soft mask SDF_RS = 0x0001, ///SDF based rigid vs soft CL_RS = 0x0002, ///Cluster vs convex rigid vs soft - SDF_RD = 0x0004, ///SDF based rigid vs deformable + SDF_RD = 0x0004, ///rigid vs deformable SVSmask = 0x00f0, ///Rigid versus soft mask VF_SS = 0x0010, ///Vertex vs face soft vs soft handling @@ -172,8 +172,9 @@ public: VF_DD = 0x0080, ///Vertex vs face soft vs soft handling RVDFmask = 0x0f00, /// Rigid versus deformable face mask - SDF_RDF = 0x0100, /// SDF based Rigid vs. deformable face - SDF_MDF = 0x0200, /// SDF based Multibody vs. deformable face + SDF_RDF = 0x0100, /// GJK based Rigid vs. deformable face + SDF_MDF = 0x0200, /// GJK based Multibody vs. deformable face + SDF_RDN = 0x0400, /// SDF based Rigid vs. deformable node /* presets */ Default = SDF_RS, END @@ -817,7 +818,6 @@ public: btAlignedObjectArray m_z; // vertical distance used in extrapolation bool m_useSelfCollision; bool m_softSoftCollision; - bool m_usePostCollisionDamping; btAlignedObjectArray m_clusterConnectivity; //cluster connectivity, for self-collision @@ -1308,9 +1308,9 @@ public: face_penetration = btMax(face_penetration, face->m_n[i]->m_penetration); btScalar I_tilde = .5 *I /(1.0+w.length2()); - // double the impulse if node or face is constrained. -// if (face_penetration > 0 || node_penetration > 0) -// I_tilde *= 2.0; +// double the impulse if node or face is constrained. + if (face_penetration > 0 || node_penetration > 0) + I_tilde *= 2.0; if (face_penetration <= node_penetration) { for (int j = 0; j < 3; ++j)