From c610ba49df6f3fccee903869a6ec94b9bcbcd7e9 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Sat, 5 Oct 2019 17:42:37 -0700 Subject: [PATCH] fix bug in computing rhs in momentum solve --- examples/DeformableDemo/DeformableContact.cpp | 2 +- examples/DeformableDemo/DeformableRigid.cpp | 2 +- .../DeformableSelfCollision.cpp | 2 +- examples/DeformableDemo/GraspDeformable.cpp | 75 ++++++++++--------- examples/DeformableDemo/PinchFriction.cpp | 2 +- .../DeformableDemo/VolumetricDeformable.cpp | 2 +- src/BulletSoftBody/btConjugateGradient.h | 2 +- src/BulletSoftBody/btDeformableBodySolver.cpp | 3 +- .../btDeformableMultiBodyConstraintSolver.cpp | 7 +- 9 files changed, 50 insertions(+), 47 deletions(-) diff --git a/examples/DeformableDemo/DeformableContact.cpp b/examples/DeformableDemo/DeformableContact.cpp index 12a81543b..3efa8e470 100644 --- a/examples/DeformableDemo/DeformableContact.cpp +++ b/examples/DeformableDemo/DeformableContact.cpp @@ -196,7 +196,7 @@ void DeformableContact::initPhysics() getDeformableDynamicsWorld()->addForce(psb2, gravity_force2); m_forces.push_back(gravity_force2); } - getDeformableDynamicsWorld()->setImplicit(true); + getDeformableDynamicsWorld()->setImplicit(false); getDeformableDynamicsWorld()->setLineSearch(false); m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); } diff --git a/examples/DeformableDemo/DeformableRigid.cpp b/examples/DeformableDemo/DeformableRigid.cpp index 1bd437a49..748f1f4c8 100644 --- a/examples/DeformableDemo/DeformableRigid.cpp +++ b/examples/DeformableDemo/DeformableRigid.cpp @@ -231,7 +231,7 @@ void DeformableRigid::initPhysics() // add a few rigid bodies Ctor_RbUpStack(1); } - getDeformableDynamicsWorld()->setImplicit(true); + getDeformableDynamicsWorld()->setImplicit(false); getDeformableDynamicsWorld()->setLineSearch(false); m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); } diff --git a/examples/DeformableDemo/DeformableSelfCollision.cpp b/examples/DeformableDemo/DeformableSelfCollision.cpp index be09af17c..cb4f405c2 100644 --- a/examples/DeformableDemo/DeformableSelfCollision.cpp +++ b/examples/DeformableDemo/DeformableSelfCollision.cpp @@ -175,7 +175,7 @@ void DeformableSelfCollision::initPhysics() getDeformableDynamicsWorld()->addForce(psb, gravity_force); m_forces.push_back(gravity_force); } - getDeformableDynamicsWorld()->setImplicit(true); + getDeformableDynamicsWorld()->setImplicit(false); getDeformableDynamicsWorld()->setLineSearch(false); m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); } diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index 41f75a4cf..970fde5f4 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -269,6 +269,7 @@ void GraspDeformable::initPhysics() } // create a soft block + if(0) { char relative_path[1024]; // b3FileUtils::findFile("banana.vtk", relative_path, 1024); @@ -291,7 +292,7 @@ void GraspDeformable::initPhysics() // psb->scale(btVector3(30, 30, 30)); // for banana // psb->scale(btVector3(.2, .2, .2)); // psb->scale(btVector3(2, 2, 2)); - psb->scale(btVector3(.3, .3, .3)); // for tube, torus, boot + psb->scale(btVector3(.1, .1, .1)); // for tube, torus, boot // psb->scale(btVector3(.1, .1, .1)); // for ditto // psb->translate(btVector3(.25, 2, 0.4)); psb->getCollisionShape()->setMargin(0.01); @@ -306,44 +307,46 @@ void GraspDeformable::initPhysics() getDeformableDynamicsWorld()->addForce(psb, gravity_force); m_forces.push_back(gravity_force); - btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(10,20, 0.01); + btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(100,200, 0.05); getDeformableDynamicsWorld()->addForce(psb, neohookean); m_forces.push_back(neohookean); } - -// // create a piece of cloth -// { -// bool onGround = false; -// const btScalar s = .4; -// btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s), -// btVector3(+s, 0, -s), -// btVector3(-s, 0, +s), -// btVector3(+s, 0, +s), -// 20,20, -// 0, true); -// -// if (onGround) -// psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s), -// btVector3(+s, 0, -s), -// btVector3(-s, 0, +s), -// btVector3(+s, 0, +s), -// // 20,20, -// 2,2, -// 0, true); -// -// psb->getCollisionShape()->setMargin(0.02); -// psb->generateBendingConstraints(2); -// psb->setTotalMass(.01); -// psb->setSpringStiffness(5); -// psb->setDampingCoefficient(0.05); -// psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects -// psb->m_cfg.kCHR = 1; // collision hardness with rigid body -// psb->m_cfg.kDF = 1; -// psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; -// getDeformableDynamicsWorld()->addSoftBody(psb); -// getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(.2,0.02, true)); -// getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); -// } + getDeformableDynamicsWorld()->setImplicit(false); + + // create a piece of cloth + { + bool onGround = false; + const btScalar s = .4; + btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s), + btVector3(+s, 0, -s), + btVector3(-s, 0, +s), + btVector3(+s, 0, +s), + 20,20, + 0, true); + + if (onGround) + psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s), + btVector3(+s, 0, -s), + btVector3(-s, 0, +s), + btVector3(+s, 0, +s), + // 20,20, + 2,2, + 0, true); + + psb->getCollisionShape()->setMargin(0.005); + psb->generateBendingConstraints(2); + psb->setTotalMass(.01); + psb->setSpringStiffness(5); + psb->setDampingCoefficient(0.05); + psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb->m_cfg.kCHR = 1; // collision hardness with rigid body + psb->m_cfg.kDF = 1; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; + getDeformableDynamicsWorld()->addSoftBody(psb); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(.2,0.02, true)); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); + } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); diff --git a/examples/DeformableDemo/PinchFriction.cpp b/examples/DeformableDemo/PinchFriction.cpp index 72e864ac3..1cbc7e43d 100644 --- a/examples/DeformableDemo/PinchFriction.cpp +++ b/examples/DeformableDemo/PinchFriction.cpp @@ -350,7 +350,7 @@ void PinchFriction::initPhysics() getDeformableDynamicsWorld()->addForce(psb3, neohookean); m_forces.push_back(neohookean); } - + getDeformableDynamicsWorld()->setImplicit(false); // add a pair of grippers createGrip(); m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); diff --git a/examples/DeformableDemo/VolumetricDeformable.cpp b/examples/DeformableDemo/VolumetricDeformable.cpp index c1213d520..d3a37d5bf 100644 --- a/examples/DeformableDemo/VolumetricDeformable.cpp +++ b/examples/DeformableDemo/VolumetricDeformable.cpp @@ -228,7 +228,7 @@ void VolumetricDeformable::initPhysics() m_forces.push_back(neohookean); } - getDeformableDynamicsWorld()->setImplicit(true); + getDeformableDynamicsWorld()->setImplicit(false); getDeformableDynamicsWorld()->setLineSearch(false); // add a few rigid bodies Ctor_RbUpStack(4); diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h index c1e41a0fd..c1816600e 100644 --- a/src/BulletSoftBody/btConjugateGradient.h +++ b/src/BulletSoftBody/btConjugateGradient.h @@ -66,7 +66,6 @@ public: // temp = A*p A.multiply(p, temp); A.project(temp); - // alpha = r^T * z / (p^T * A * p) if (dot(p,temp) < SIMD_EPSILON) { if (verbose) @@ -77,6 +76,7 @@ public: } return k; } + // alpha = r^T * z / (p^T * A * p) btScalar alpha = r_dot_z_new / dot(p, temp); // x += alpha * p; multAndAddTo(alpha, p, x); diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 10d3775d3..197ef7221 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -21,7 +21,7 @@ btDeformableBodySolver::btDeformableBodySolver() : m_numNodes(0) -, m_cg(20) +, m_cg(200) , m_maxNewtonIterations(5) , m_newtonTolerance(1e-4) , m_lineSearch(true) @@ -301,6 +301,7 @@ void btDeformableBodySolver::setupDeformableSolve(bool implicit) m_backupVelocity[counter] = psb->m_nodes[j].m_vn; } m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter]; + psb->m_nodes[j].m_v = m_backupVelocity[counter]; ++counter; } } diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index 362451c0e..2567cecab 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -23,7 +23,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration ///this is a special step to resolve penetrations (just for contacts) solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); - m_maxOverrideNumSolverIterations = 50; +// m_maxOverrideNumSolverIterations = 10; int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; for (int iteration = 0; iteration < maxIterations; iteration++) { @@ -33,10 +33,9 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration 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(); // update rigid body velocity in rigid/deformable contact - m_leastSquaresResidual = btMax(m_leastSquaresResidual, m_deformableSolver->solveContactConstraints()); - + m_leastSquaresResidual = btMax(m_leastSquaresResidual, deformableResidual); // solver body velocity <- rigid body velocity writeToSolverBody(bodies, numBodies, infoGlobal);