From 657a7468b3bdedcf8586d8c5fae8408715470f16 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 20 Sep 2019 13:54:17 -0700 Subject: [PATCH] documentation and optimization --- .../DeformableSelfCollision.cpp | 4 +- .../DeformableDemo/VolumetricDeformable.cpp | 13 ++-- src/BulletSoftBody/btConjugateGradient.h | 2 +- src/BulletSoftBody/btDeformableBodySolver.cpp | 5 +- src/BulletSoftBody/btDeformableBodySolver.h | 75 ++++++++++++------- .../btDeformableContactProjection.h | 6 +- .../btDeformableLagrangianForce.h | 13 ++-- .../btDeformableMassSpringForce.h | 2 + .../btDeformableMultiBodyConstraintSolver.cpp | 64 ++++++++++++++++ .../btDeformableMultiBodyConstraintSolver.h | 57 +++----------- .../btDeformableNeoHookeanForce.h | 31 +++++--- 11 files changed, 162 insertions(+), 110 deletions(-) diff --git a/examples/DeformableDemo/DeformableSelfCollision.cpp b/examples/DeformableDemo/DeformableSelfCollision.cpp index eb963ae05..be09af17c 100644 --- a/examples/DeformableDemo/DeformableSelfCollision.cpp +++ b/examples/DeformableDemo/DeformableSelfCollision.cpp @@ -153,7 +153,7 @@ void DeformableSelfCollision::initPhysics() btVector3(-s, h, +4*s), btVector3(+s, h, +4*s), 10,40, - 0, true, 0.1); + 0, true, 0.01); psb->getCollisionShape()->setMargin(0.2); @@ -161,7 +161,7 @@ void DeformableSelfCollision::initPhysics() psb->setTotalMass(1); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body - psb->m_cfg.kDF = 0.02; + psb->m_cfg.kDF = 0.2; psb->rotate(btQuaternion(0,SIMD_PI / 2, 0)); psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; diff --git a/examples/DeformableDemo/VolumetricDeformable.cpp b/examples/DeformableDemo/VolumetricDeformable.cpp index 0c4de59b1..c1213d520 100644 --- a/examples/DeformableDemo/VolumetricDeformable.cpp +++ b/examples/DeformableDemo/VolumetricDeformable.cpp @@ -91,7 +91,7 @@ public: void Ctor_RbUpStack(int count) { - float mass = 0.02; + float mass = 1; btCompoundShape* cylinderCompound = new btCompoundShape; btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(2, .5, .5)); @@ -166,7 +166,7 @@ void VolumetricDeformable::initPhysics() m_solver = sol; m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); - btVector3 gravity = btVector3(0, -10, 0); + btVector3 gravity = btVector3(0, -100, 0); m_dynamicsWorld->setGravity(gravity); getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); @@ -196,7 +196,6 @@ void VolumetricDeformable::initPhysics() //add the ground to the dynamics world m_dynamicsWorld->addRigidBody(body); } - getDeformableDynamicsWorld()->setLineSearch(false); createStaticBox(btVector3(1, 5, 5), btVector3(-5,0,0)); createStaticBox(btVector3(1, 5, 5), btVector3(5,0,0)); @@ -220,19 +219,17 @@ void VolumetricDeformable::initPhysics() psb->m_cfg.kDF = 0.5; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; - btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(0,0.03); - getDeformableDynamicsWorld()->addForce(psb, mass_spring); - m_forces.push_back(mass_spring); - btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); getDeformableDynamicsWorld()->addForce(psb, gravity_force); m_forces.push_back(gravity_force); - btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(.5,2.5); + btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(30,100,0.02); getDeformableDynamicsWorld()->addForce(psb, neohookean); m_forces.push_back(neohookean); } + getDeformableDynamicsWorld()->setImplicit(true); + getDeformableDynamicsWorld()->setLineSearch(false); // add a few rigid bodies Ctor_RbUpStack(4); diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h index 38158dc09..77fb3dc72 100644 --- a/src/BulletSoftBody/btConjugateGradient.h +++ b/src/BulletSoftBody/btConjugateGradient.h @@ -69,7 +69,7 @@ public: if (dot(p,temp) < SIMD_EPSILON) { if (verbose) - std::cout << "Encountered negative direction in CG!"<m_fdbvt.optimizeIncremental(1); } + void btDeformableBodySolver::updateSoftBodies() { for (int i = 0; i < m_softBodies.size(); i++) @@ -419,7 +420,7 @@ void btDeformableBodySolver::updateSoftBodies() btSoftBody *psb = (btSoftBody *)m_softBodies[i]; if (psb->isActive()) { - psb->updateNormals(); // normal is updated here + psb->updateNormals(); } } } diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index b0c458029..dc68157d5 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -29,26 +29,24 @@ class btDeformableMultiBodyDynamicsWorld; class btDeformableBodySolver : public btSoftBodySolver { -// using TVStack = btAlignedObjectArray; typedef btAlignedObjectArray TVStack; protected: - int m_numNodes; - TVStack m_dv; - TVStack m_backup_dv; - TVStack m_ddv; - TVStack m_residual; - btAlignedObjectArray m_softBodies; - - btAlignedObjectArray m_backupVelocity; - btScalar m_dt; - btScalar m_contact_iterations; - btConjugateGradient m_cg; - bool m_implicit; - int m_maxNewtonIterations; - btScalar m_newtonTolerance; - bool m_lineSearch; + int m_numNodes; // total number of deformable body nodes + TVStack m_dv; // v_{n+1} - v_n + TVStack m_backup_dv; // backed up dv + TVStack m_ddv; // incremental dv + TVStack m_residual; // rhs of the linear solve + btAlignedObjectArray m_softBodies; // all deformable bodies + TVStack m_backupVelocity; // backed up v, equals v_n for implicit, equals v_{n+1}^* for explicit + btScalar m_dt; // dt + btConjugateGradient m_cg; // CG solver + bool m_implicit; // use implicit scheme if true, explicit scheme if false + int m_maxNewtonIterations; // max number of newton iterations + btScalar m_newtonTolerance; // stop newton iterations if f(x) < m_newtonTolerance + bool m_lineSearch; // If true, use newton's method with line search under implicit scheme public: + // handles data related to objective function btDeformableBackwardEulerObjective* m_objective; btDeformableBodySolver(); @@ -60,49 +58,62 @@ public: return DEFORMABLE_SOLVER; } + // update soft body normals virtual void updateSoftBodies(); - - virtual void copyBackToSoftBodies(bool bMove = true) {} + // solve the momentum equation virtual void solveDeformableConstraints(btScalar solverdt); + // solve the contact between deformable and rigid as well as among deformables btScalar solveContactConstraints(); - - virtual void solveConstraints(btScalar dt){} - + + // resize/clear data structures void reinitialize(const btAlignedObjectArray& softBodies, btScalar dt); + // set up contact constraints void setConstraints(); + // add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion + virtual void predictMotion(btScalar solverdt); + + // move to temporary position x_{n+1}^* = x_n + dt * v_{n+1}^* + // x_{n+1}^* is stored in m_q void predictDeformableMotion(btSoftBody* psb, btScalar dt); + // save the current velocity to m_backupVelocity void backupVelocity(); + + // set m_dv and m_backupVelocity to desired value to prepare for momentum solve void setupDeformableSolve(bool implicit); + + // set the current velocity to that backed up in m_backupVelocity void revertVelocity(); + + // set velocity to m_dv + m_backupVelocity void updateVelocity(); + // update the node count bool updateNodes(); - void computeStep(TVStack& dv, const TVStack& residual); + // calculate the change in dv resulting from the momentum solve + void computeStep(TVStack& ddv, const TVStack& residual); + // calculate the change in dv resulting from the momentum solve when line search is turned on btScalar computeDescentStep(TVStack& ddv, const TVStack& residual); - - virtual void predictMotion(btScalar solverdt); virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {} + // process collision between deformable and rigid virtual void processCollision(btSoftBody * softBody, const btCollisionObjectWrapper * collisionObjectWrap) { softBody->defaultCollisionHandler(collisionObjectWrap); } - + + // process collision between deformable and deformable virtual void processCollision(btSoftBody * softBody, btSoftBody * otherSoftBody) { softBody->defaultCollisionHandler(otherSoftBody); } - virtual void optimize(btAlignedObjectArray &softBodies, bool forceUpdate = false){} - - virtual bool checkInitialized(){return true;} - + // If true, implicit time stepping scheme is used. // Otherwise, explicit time stepping scheme is used void setImplicit(bool implicit); @@ -136,6 +147,12 @@ public: // 1/2 * dv^T * M * dv // used in line search btScalar kineticEnergy(); + + // unused functions + virtual void optimize(btAlignedObjectArray &softBodies, bool forceUpdate = false){} + virtual void solveConstraints(btScalar dt){} + virtual bool checkInitialized(){return true;} + virtual void copyBackToSoftBodies(bool bMove = true) {} }; #endif /* btDeformableBodySolver_h */ diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index 6c91fd584..fa5eb662b 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -26,8 +26,6 @@ class btDeformableContactProjection { public: typedef btAlignedObjectArray TVStack; - typedef btAlignedObjectArray > TVArrayStack; - typedef btAlignedObjectArray > TArrayStack; btAlignedObjectArray& m_softBodies; // map from node index to static constraint @@ -54,10 +52,10 @@ public: { } - // apply the constraints to the rhs + // apply the constraints to the rhs of the linear solve virtual void project(TVStack& x); - // add to friction + // add friction force to the rhs of the linear solve virtual void applyDynamicFriction(TVStack& f); // update the constraints diff --git a/src/BulletSoftBody/btDeformableLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h index f65569451..cab48837d 100644 --- a/src/BulletSoftBody/btDeformableLagrangianForce.h +++ b/src/BulletSoftBody/btDeformableLagrangianForce.h @@ -67,6 +67,7 @@ public: { } + // get number of nodes that have the force virtual int getNumNodes() { int numNodes = 0; @@ -94,10 +95,7 @@ public: btVector3 c1 = dx[id1] - dx[id0]; btVector3 c2 = dx[id2] - dx[id0]; btVector3 c3 = dx[id3] - dx[id0]; - btMatrix3x3 dF(c1.getX(), c2.getX(), c3.getX(), - c1.getY(), c2.getY(), c3.getY(), - c1.getZ(), c2.getZ(), c3.getZ()); - return dF; + return btMatrix3x3(c1,c2,c3).transpose(); } // Calculate the incremental deformable generated from the current velocity @@ -106,12 +104,10 @@ public: btVector3 c1 = n1->m_v - n0->m_v; btVector3 c2 = n2->m_v - n0->m_v; btVector3 c3 = n3->m_v - n0->m_v; - btMatrix3x3 dF(c1.getX(), c2.getX(), c3.getX(), - c1.getY(), c2.getY(), c3.getY(), - c1.getZ(), c2.getZ(), c3.getZ()); - return dF; + return btMatrix3x3(c1,c2,c3).transpose(); } + // test for addScaledElasticForce function virtual void testDerivative() { for (int i = 0; igetInvMass()) + { + btSolverBody& solverBody = siData.m_tmpSolverBodyPool[bodyId]; + solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity; + solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity; + } + } +} + +void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactSolverInfo& infoGlobal) +{ + for (int i = 0; i < m_tmpSolverBodyPool.size(); i++) + { + btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody; + if (body) + { + m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity); + m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity+m_tmpSolverBodyPool[i].m_deltaAngularVelocity); + } + } +} diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h index d28aa204e..cac82f98b 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h @@ -22,6 +22,13 @@ class btDeformableBodySolver; +// btDeformableMultiBodyConstraintSolver extendsn btMultiBodyConstraintSolver to solve for the contact among rigid/multibody and deformable bodies. Notice that the following constraints +// 1. rigid/multibody against rigid/multibody +// 2. rigid/multibody against deforamble +// 3. deformable against deformable +// 4. deformable self collision +// 5. joint constraints +// are all coupled in this solve. ATTRIBUTE_ALIGNED16(class) btDeformableMultiBodyConstraintSolver : public btMultiBodyConstraintSolver { @@ -32,34 +39,10 @@ protected: virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); // write the velocity of the the solver body to the underlying rigid body - void solverBodyWriteBack(const btContactSolverInfo& infoGlobal) - { - for (int i = 0; i < m_tmpSolverBodyPool.size(); i++) - { - btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody; - if (body) - { - m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity); - m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity+m_tmpSolverBodyPool[i].m_deltaAngularVelocity); - } - } - } - - void writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal) - { - for (int i = 0; i < numBodies; i++) - { - int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep); - - btRigidBody* body = btRigidBody::upcast(bodies[i]); - if (body && body->getInvMass()) - { - btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; - solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity; - solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity; - } - } - } + void solverBodyWriteBack(const btContactSolverInfo& infoGlobal); + + // write the velocity of the underlying rigid body to the the the solver body + void writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal); public: BT_DECLARE_ALIGNED_ALLOCATOR(); @@ -69,23 +52,7 @@ public: m_deformableSolver = deformableSolver; } - virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) - { - m_tmpMultiBodyConstraints = multiBodyConstraints; - m_tmpNumMultiBodyConstraints = numMultiBodyConstraints; - - // inherited from MultiBodyConstraintSolver - solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); - - // overriden - solveGroupCacheFriendlyIterations(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer); - - // inherited from MultiBodyConstraintSolver - solveGroupCacheFriendlyFinish(bodies, numBodies, info); - - m_tmpMultiBodyConstraints = 0; - m_tmpNumMultiBodyConstraints = 0; - } + virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); }; #endif /* BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H */ diff --git a/src/BulletSoftBody/btDeformableNeoHookeanForce.h b/src/BulletSoftBody/btDeformableNeoHookeanForce.h index 5957c77aa..66affa3d3 100644 --- a/src/BulletSoftBody/btDeformableNeoHookeanForce.h +++ b/src/BulletSoftBody/btDeformableNeoHookeanForce.h @@ -17,7 +17,7 @@ subject to the following restrictions: #define BT_NEOHOOKEAN_H #include "btDeformableLagrangianForce.h" - +#include "LinearMath/btQuickprof.h" // This energy is as described in https://graphics.pixar.com/library/StableElasticity/paper.pdf class btDeformableNeoHookeanForce : public btDeformableLagrangianForce { @@ -155,8 +155,9 @@ public: btSoftBody::Tetra& tetra = psb->m_tetras[j]; btMatrix3x3 P; firstPiola(psb->m_tetraScratches[j],P); - btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); +// btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose(); + btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col; btSoftBody::Node* node0 = tetra.m_n[0]; btSoftBody::Node* node1 = tetra.m_n[1]; @@ -202,8 +203,9 @@ public: btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse; btMatrix3x3 dP; firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); - btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); +// btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; // damping force differential btScalar scale1 = scale * tetra.m_element_measure; @@ -237,8 +239,9 @@ public: btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse; btMatrix3x3 dP; firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP); - btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); +// btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; // elastic force differential btScalar scale1 = scale * tetra.m_element_measure; @@ -252,27 +255,33 @@ public: void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P) { - P = s.m_F * m_mu * ( 1. - 1. / (s.m_trace + 1.)) + s.m_cofF * (m_lambda * (s.m_J - 1.) - 0.75 * m_mu); + btScalar c1 = (m_mu * ( 1. - 1. / (s.m_trace + 1.))); + btScalar c2 = (m_lambda * (s.m_J - 1.) - 0.75 * m_mu); + P = s.m_F * c1 + s.m_cofF * c2; } // Let P be the first piola stress. // This function calculates the dP = dP/dF * dF void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) { - dP = dF * m_mu * ( 1. - 1. / (s.m_trace + 1.)) + s.m_F * (2*m_mu) * DotProduct(s.m_F, dF) * (1./((1.+s.m_trace)*(1.+s.m_trace))); - + btScalar c1 = m_mu * ( 1. - 1. / (s.m_trace + 1.)); + btScalar c2 = (2.*m_mu) * DotProduct(s.m_F, dF) * (1./((1.+s.m_trace)*(1.+s.m_trace))); + btScalar c3 = (m_lambda * DotProduct(s.m_cofF, dF)); + dP = dF * c1 + s.m_F * c2; addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda*(s.m_J-1.) - 0.75*m_mu, dP); - dP += s.m_cofF * m_lambda * DotProduct(s.m_cofF, dF); + dP += s.m_cofF * c3; } // Let Q be the damping stress. // This function calculates the dP = dQ/dF * dF void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) { - dP = dF * m_mu_damp * ( 1. - 1. / (s.m_trace + 1.)) + s.m_F * (2*m_mu_damp) * DotProduct(s.m_F, dF) * (1./((1.+s.m_trace)*(1.+s.m_trace))); - + btScalar c1 = (m_mu_damp * ( 1. - 1. / (s.m_trace + 1.))); + btScalar c2 = ((2.*m_mu_damp) * DotProduct(s.m_F, dF) *(1./((1.+s.m_trace)*(1.+s.m_trace)))); + btScalar c3 = (m_lambda_damp * DotProduct(s.m_cofF, dF)); + dP = dF * c1 + s.m_F * c2; addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda_damp*(s.m_J-1.) - 0.75*m_mu_damp, dP); - dP += s.m_cofF * m_lambda_damp * DotProduct(s.m_cofF, dF); + dP += s.m_cofF * c3; } btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B)