diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index 7150657e5..e7d63d52d 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -275,12 +275,12 @@ void GraspDeformable::initPhysics() { char absolute_path[1024]; b3BulletDefaultFileIO fileio; -// fileio.findResourcePath("ditto.vtk", absolute_path, 1024); + fileio.findResourcePath("ditto.vtk", absolute_path, 1024); // fileio.findResourcePath("banana.vtk", absolute_path, 1024); // fileio.findResourcePath("ball.vtk", absolute_path, 1024); // fileio.findResourcePath("deformable_crumpled_napkin_sim.vtk", absolute_path, 1024); // fileio.findResourcePath("single_tet.vtk", absolute_path, 1024); - fileio.findResourcePath("tube.vtk", absolute_path, 1024); +// fileio.findResourcePath("tube.vtk", absolute_path, 1024); // fileio.findResourcePath("torus.vtk", absolute_path, 1024); // fileio.findResourcePath("paper_roll.vtk", absolute_path, 1024); // fileio.findResourcePath("bread.vtk", absolute_path, 1024); @@ -295,8 +295,8 @@ void GraspDeformable::initPhysics() // psb->scale(btVector3(30, 30, 30)); // for banana psb->scale(btVector3(.7, .7, .7)); // psb->scale(btVector3(2, 2, 2)); - psb->scale(btVector3(.3, .3, .3)); // for tube, torus, boot -// psb->scale(btVector3(.1, .1, .1)); // for ditto +// psb->scale(btVector3(.3, .3, .3)); // for tube, torus, boot + psb->scale(btVector3(.1, .1, .1)); // for ditto // psb->translate(btVector3(.25, 10, 0.4)); psb->getCollisionShape()->setMargin(0.0005); psb->setMaxStress(50); diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 810168614..808fce32c 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -46,7 +46,7 @@ struct btContactSolverInfoData btScalar m_sor; //successive over-relaxation term btScalar m_erp; //error reduction for non-contact constraints btScalar m_erp2; //error reduction for contact constraints - btScalar m_deformable_erp; //error reduction for deformable constraints + btScalar m_deformable_erp; //error reduction for deformable constraints btScalar m_globalCfm; //constraint force mixing for contacts and non-contacts btScalar m_frictionERP; //error reduction for friction constraints btScalar m_frictionCFM; //constraint force mixing for friction constraints @@ -82,7 +82,7 @@ struct btContactSolverInfo : public btContactSolverInfoData m_numIterations = 10; m_erp = btScalar(0.2); m_erp2 = btScalar(0.2); - m_deformable_erp = btScalar(0.0); + m_deformable_erp = btScalar(0.1); m_globalCfm = btScalar(0.); m_frictionERP = btScalar(0.2); //positional friction 'anchors' are disabled by default m_frictionCFM = btScalar(0.); diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 3972a48b2..6404ce6ee 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -234,10 +234,10 @@ void btDeformableBodySolver::setConstraints(const btContactSolverInfo& infoGloba m_objective->setConstraints(infoGlobal); } -btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies) +btScalar btDeformableBodySolver::solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal) { BT_PROFILE("solveContactConstraints"); - btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies); + btScalar maxSquaredResidual = m_objective->m_projection.update(deformableBodies,numDeformableBodies, infoGlobal); return maxSquaredResidual; } diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index bd09eda2f..bce0d30b3 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -65,7 +65,7 @@ public: virtual void solveDeformableConstraints(btScalar solverdt); // solve the contact between deformable and rigid as well as among deformables - btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies); + btScalar solveContactConstraints(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal); // solve the position error between deformable and rigid as well as among deformables; btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index c11b84464..61eaae5a8 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -79,7 +79,7 @@ btVector3 btDeformableNodeAnchorConstraint::getVa() const return va; } -btScalar btDeformableNodeAnchorConstraint::solveConstraint() +btScalar btDeformableNodeAnchorConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) { const btSoftBody::sCti& cti = m_anchor->m_cti; btVector3 va = getVa(); @@ -141,7 +141,7 @@ 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 = c.m_cti.m_offset; + m_penetration = btMin(btScalar(0),c.m_cti.m_offset); } btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other) @@ -206,16 +206,16 @@ btVector3 btDeformableRigidContactConstraint::getVa() const return va; } -btScalar btDeformableRigidContactConstraint::solveConstraint() +btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) { const btSoftBody::sCti& cti = m_contact->m_cti; btVector3 va = getVa(); btVector3 vb = getVb(); btVector3 vr = vb - va; - const btScalar dn = btDot(vr, cti.m_normal); + const btScalar dn = btDot(vr, cti.m_normal) + m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep; // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt btScalar residualSquare = dn*dn; - btVector3 impulse = m_contact->m_c0 * vr; + btVector3 impulse = m_contact->m_c0 * (vr + m_penetration * infoGlobal.m_deformable_erp / infoGlobal.m_timeStep * cti.m_normal) ; const btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn); btVector3 impulse_tangent = impulse - impulse_normal; btVector3 old_total_tangent_dv = m_total_tangent_dv; @@ -457,9 +457,9 @@ void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impul btScalar m02 = (relaxation/(im0 + im2)); btScalar m12 = (relaxation/(im1 + im2)); - // 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)); +// 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)); 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])); @@ -530,7 +530,7 @@ btVector3 btDeformableFaceNodeContactConstraint::getDv(const btSoftBody::Node* n return dv * contact->m_weights[2]; } -btScalar btDeformableFaceNodeContactConstraint::solveConstraint() +btScalar btDeformableFaceNodeContactConstraint::solveConstraint(const btContactSolverInfo& infoGlobal) { btVector3 va = getVa(); btVector3 vb = getVb(); diff --git a/src/BulletSoftBody/btDeformableContactConstraint.h b/src/BulletSoftBody/btDeformableContactConstraint.h index 8ede39a19..7acf9aa69 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.h +++ b/src/BulletSoftBody/btDeformableContactConstraint.h @@ -50,7 +50,7 @@ public: // solve the constraint with inelastic impulse and return the error, which is the square of normal component of velocity diffrerence // the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact - virtual btScalar solveConstraint() = 0; + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) = 0; // solve the position error by applying an inelastic impulse that changes only the position (not velocity) virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal) = 0; @@ -93,7 +93,7 @@ public: virtual ~btDeformableStaticConstraint(){} - virtual btScalar solveConstraint() + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal) { return 0; } @@ -136,7 +136,7 @@ public: virtual ~btDeformableNodeAnchorConstraint() { } - virtual btScalar solveConstraint(); + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal) { // todo xuchenhan@ @@ -179,7 +179,7 @@ public: // object A is the rigid/multi body, and object B is the deformable node/face virtual btVector3 getVa() const; - virtual btScalar solveConstraint(); + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); @@ -264,7 +264,7 @@ public: btDeformableFaceNodeContactConstraint(){} virtual ~btDeformableFaceNodeContactConstraint(){} - virtual btScalar solveConstraint(); + virtual btScalar solveConstraint(const btContactSolverInfo& infoGlobal); virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal) { diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index e1666fb86..a59177e16 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -17,7 +17,7 @@ #include "btDeformableMultiBodyDynamicsWorld.h" #include #include -btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies) +btScalar btDeformableContactProjection::update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal) { btScalar residualSquare = 0; for (int i = 0; i < numDeformableBodies; ++i) @@ -32,25 +32,25 @@ btScalar btDeformableContactProjection::update(btCollisionObject** deformableBod for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k) { btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k]; - btScalar localResidualSquare = constraint.solveConstraint(); + btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); residualSquare = btMax(residualSquare, localResidualSquare); } for (int k = 0; k < m_nodeAnchorConstraints[j].size(); ++k) { btDeformableNodeAnchorConstraint& constraint = m_nodeAnchorConstraints[j][k]; - btScalar localResidualSquare = constraint.solveConstraint(); + btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); residualSquare = btMax(residualSquare, localResidualSquare); } for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k) { btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k]; - btScalar localResidualSquare = constraint.solveConstraint(); + btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); residualSquare = btMax(residualSquare, localResidualSquare); } for (int k = 0; k < m_deformableConstraints[j].size(); ++k) { btDeformableFaceNodeContactConstraint& constraint = m_deformableConstraints[j][k]; - btScalar localResidualSquare = constraint.solveConstraint(); + btScalar localResidualSquare = constraint.solveConstraint(infoGlobal); residualSquare = btMax(residualSquare, localResidualSquare); } } diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index 618eea4c3..32d6c4d75 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -72,7 +72,7 @@ public: virtual void applyDynamicFriction(TVStack& f); // update and solve the constraints - virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies); + virtual btScalar update(btCollisionObject** deformableBodies,int numDeformableBodies, const btContactSolverInfo& infoGlobal); // solve the position error using split impulse virtual btScalar solveSplitImpulse(const btContactSolverInfo& infoGlobal); diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index aa70139bf..c8cc47923 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -32,7 +32,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveDeformableGroupIterations(b m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); // solver body velocity -> rigid body velocity solverBodyWriteBack(infoGlobal); - btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies,numDeformableBodies); + btScalar deformableResidual = m_deformableSolver->solveContactConstraints(deformableBodies,numDeformableBodies, infoGlobal); // update rigid body velocity in rigid/deformable contact m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual); // solver body velocity <- rigid body velocity @@ -112,7 +112,7 @@ void btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlySplitImpulseI if (infoGlobal.m_splitImpulse) { { - m_deformableSolver->splitImpulseSetup(infoGlobal); +// m_deformableSolver->splitImpulseSetup(infoGlobal); for (iteration = 0; iteration < infoGlobal.m_numIterations; iteration++) { btScalar leastSquaresResidual = 0.f; diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 40376e96d..ca7eec13d 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -53,7 +53,7 @@ btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btV n.m_material = pm; } updateBounds(); - setCollisionQuadrature(2); + setCollisionQuadrature(3); } btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo) @@ -2459,7 +2459,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO : colObjWrap->getWorldTransform(); btScalar dst; -#define USE_QUADRATURE 1 +//#define USE_QUADRATURE 1 //#define CACHE_PREV_COLLISION // use the contact position of the previous collision