From ec91a0ffa40555aba556c40bea295f3f09064ad2 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 4 Oct 2019 11:00:22 -0700 Subject: [PATCH 01/29] configure damping coefficients for neohookean models --- examples/SharedMemory/PhysicsClientC_API.cpp | 3 ++- examples/SharedMemory/PhysicsClientC_API.h | 2 +- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 7 ++++--- examples/SharedMemory/SharedMemoryCommands.h | 1 + 4 files changed, 8 insertions(+), 5 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 1963c3473..608a577b2 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -348,12 +348,13 @@ B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle co return 0; } -B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda) +B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda, double NeoHookeanDamping) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; b3Assert(command->m_type == CMD_LOAD_SOFT_BODY); command->m_loadSoftBodyArguments.m_NeoHookeanMu = NeoHookeanMu; command->m_loadSoftBodyArguments.m_NeoHookeanLambda = NeoHookeanLambda; + command->m_loadSoftBodyArguments.m_NeoHookeanDamping = NeoHookeanDamping; command->m_updateFlags |= LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE; return 0; } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 64ef41bc7..3ef510afc 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -632,7 +632,7 @@ extern "C" B3_SHARED_API int b3LoadSoftBodySetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX, double startPosY, double startPosZ); B3_SHARED_API int b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW); B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda); - B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda); + B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda, double NeoHookeanDamping = 0); B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness); B3_SHARED_API int b3LoadSoftBodyAddGravityForce(b3SharedMemoryCommandHandle commandHandle, double gravityX, double gravityY, double gravityZ); B3_SHARED_API int b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle commandHandle, double collisionHardness); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 78087a686..8fa68afca 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8019,7 +8019,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar const std::string& error_message_prefix = ""; std::string out_found_filename; std::string out_found_sim_filename; - int out_type, out_sim_type; + int out_type(0), out_sim_type(0); bool render_mesh_is_sim_mesh = true; @@ -8081,12 +8081,13 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar corotated_lambda = clientCmd.m_loadSoftBodyArguments.m_corotatedLambda; m_data->m_dynamicsWorld->addForce(psb, new btDeformableCorotatedForce(corotated_mu, corotated_lambda)); } - btScalar neohookean_mu, neohookean_lambda; + btScalar neohookean_mu, neohookean_lambda, neohookean_damping; if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE) { neohookean_mu = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanMu; neohookean_lambda = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanLambda; - m_data->m_dynamicsWorld->addForce(psb, new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda)); + neohookean_damping = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanDamping; + m_data->m_dynamicsWorld->addForce(psb, new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda, neohookean_damping)); } btScalar spring_elastic_stiffness, spring_damping_stiffness; if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE) diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 74e79537a..7fd317aa4 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -528,6 +528,7 @@ struct LoadSoftBodyArgs double m_frictionCoeff; double m_NeoHookeanMu; double m_NeoHookeanLambda; + double m_NeoHookeanDamping; }; struct b3LoadSoftBodyResultArgs From d0e4bbf04d4a63d741b82451fc61a2668da4088f Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 4 Oct 2019 11:21:34 -0700 Subject: [PATCH 02/29] fix gravity set up --- examples/SharedMemory/PhysicsClientC_API.h | 2 +- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 5 +---- src/BulletSoftBody/btSoftBody.cpp | 8 ++++---- 3 files changed, 6 insertions(+), 9 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 3ef510afc..39da0f455 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -632,7 +632,7 @@ extern "C" B3_SHARED_API int b3LoadSoftBodySetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX, double startPosY, double startPosZ); B3_SHARED_API int b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW); B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda); - B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda, double NeoHookeanDamping = 0); + B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda, double NeoHookeanDamping); B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness); B3_SHARED_API int b3LoadSoftBodyAddGravityForce(b3SharedMemoryCommandHandle commandHandle, double gravityX, double gravityY, double gravityZ); B3_SHARED_API int b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle commandHandle, double collisionHardness); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 8fa68afca..2d8bc9bd3 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8113,10 +8113,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar psb->m_renderNodes.resize(0); } btVector3 gravity = m_data->m_dynamicsWorld->getGravity(); - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_GRAVITY_FORCE) - { - m_data->m_dynamicsWorld->addForce(psb, new btDeformableGravityForce(gravity)); - } + m_data->m_dynamicsWorld->addForce(psb, new btDeformableGravityForce(gravity)); btScalar collision_hardness = 1; psb->m_cfg.kKHR = collision_hardness; psb->m_cfg.kCHR = collision_hardness; diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 0da85ff43..6f46b63b7 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -2306,10 +2306,10 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject(); // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect // but resolve contact at x_n - btTransform wtr = (predict) ? - (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) - : colObjWrap->getWorldTransform(); - //const btTransform& wtr = colObjWrap->getWorldTransform(); + //btTransform wtr = (predict) ? + //(colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) + //: colObjWrap->getWorldTransform(); + const btTransform& wtr = colObjWrap->getWorldTransform(); btScalar dst = m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x), From 4220c7f94c20698098f71dfd073b9c3e47f2bb25 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 4 Oct 2019 17:53:30 -0700 Subject: [PATCH 03/29] tune CG tolerance --- src/BulletSoftBody/btConjugateGradient.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h index a73199f90..c1e41a0fd 100644 --- a/src/BulletSoftBody/btConjugateGradient.h +++ b/src/BulletSoftBody/btConjugateGradient.h @@ -32,7 +32,7 @@ public: btConjugateGradient(const int max_it_in) : max_iterations(max_it_in) { - tolerance = 1024 * std::numeric_limits::epsilon(); + tolerance = 1e-5; } virtual ~btConjugateGradient(){} @@ -52,7 +52,7 @@ public: A.project(z); btScalar r_dot_z = dot(z,r); btScalar local_tolerance = tolerance; - if (std::sqrt(r_dot_z) <= local_tolerance) { + if (r_dot_z <= local_tolerance) { if (verbose) { std::cout << "Iteration = 0" << std::endl; @@ -86,7 +86,7 @@ public: A.precondition(r, z); r_dot_z = r_dot_z_new; r_dot_z_new = dot(r,z); - if (std::sqrt(r_dot_z_new) < local_tolerance) { + if (r_dot_z_new < local_tolerance) { if (verbose) { std::cout << "ConjugateGradient iterations " << k << std::endl; From e13578fee3f8f638c8722be684ccce5a06560834 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 4 Oct 2019 17:53:53 -0700 Subject: [PATCH 04/29] add option to turn self-collision on/off --- examples/SharedMemory/PhysicsClientC_API.cpp | 9 +++++ examples/SharedMemory/PhysicsClientC_API.h | 1 + .../PhysicsServerCommandProcessor.cpp | 6 ++++ examples/SharedMemory/SharedMemoryCommands.h | 2 ++ src/BulletSoftBody/btSoftBody.cpp | 33 +++++++++++++------ src/BulletSoftBody/btSoftBody.h | 3 ++ 6 files changed, 44 insertions(+), 10 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 608a577b2..914e47037 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -386,6 +386,15 @@ B3_SHARED_API int b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle return 0; } +B3_SHARED_API int b3LoadSoftBodySetSelfCollision(b3SharedMemoryCommandHandle commandHandle, bool useSelfCollision) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command->m_type == CMD_LOAD_SOFT_BODY); + command->m_loadSoftBodyArguments.m_useSelfCollision = useSelfCollision; + command->m_updateFlags |= LOAD_SOFT_BODY_SET_SELF_COLLISION; + return 0; +} + B3_SHARED_API int b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHandle commandHandle, double frictionCoefficient) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 39da0f455..52569807b 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -636,6 +636,7 @@ extern "C" B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness); B3_SHARED_API int b3LoadSoftBodyAddGravityForce(b3SharedMemoryCommandHandle commandHandle, double gravityX, double gravityY, double gravityZ); B3_SHARED_API int b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle commandHandle, double collisionHardness); + B3_SHARED_API int b3LoadSoftBodySetSelfCollision(b3SharedMemoryCommandHandle commandHandle, bool useSelfCollision); B3_SHARED_API int b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHandle commandHandle, double frictionCoefficient); B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle commandHandle, int useBendingSprings); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 2d8bc9bd3..e5dc10bb4 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8144,6 +8144,12 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; psb->setCollisionFlags(0); psb->setTotalMass(mass); + bool use_self_collision = false; + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_USE_SELF_COLLISION) + { + use_self_collision = loadSoftBodyArgs.m_useSelfCollision; + } + psb->setSelfCollision(use_self_collision); #else btSoftBody::Material* pm = psb->appendMaterial(); pm->m_kLST = 0.5; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 7fd317aa4..d23ac1594 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -501,6 +501,7 @@ enum EnumLoadSoftBodyUpdateFlags LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT = 1<<10, LOAD_SOFT_BODY_ADD_BENDING_SPRINGS = 1<<11, LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE = 1<<12, + LOAD_SOFT_BODY_SET_SELF_COLLISION = 1<<13, }; enum EnumSimParamInternalSimFlags @@ -525,6 +526,7 @@ struct LoadSoftBodyArgs double m_corotatedLambda; bool m_useBendingSprings; double m_collisionHardness; + double m_useSelfCollision; double m_frictionCoeff; double m_NeoHookeanMu; double m_NeoHookeanLambda; diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 6f46b63b7..ef3a201c5 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -3378,6 +3378,16 @@ btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver) return (0); } +void btSoftBody::setSelfCollision(bool useSelfCollision) +{ + m_useSelfCollision = useSelfCollision; +} + +bool btSoftBody::useSelfCollision() +{ + return m_useSelfCollision; +} + // void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap) { @@ -3563,16 +3573,19 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb) } else { - btSoftColliders::CollideFF_DD docollide; - docollide.mrg = getCollisionShape()->getMargin() + - psb->getCollisionShape()->getMargin(); - docollide.psb[0] = this; - docollide.psb[1] = psb; - /* psb0 faces vs psb0 faces */ - btDbvntNode* root = copyToDbvnt(this->m_fdbvt.m_root); - calculateNormalCone(root); - this->m_fdbvt.selfCollideT(root,docollide); - delete root; + if (psb->useSelfCollision()) + { + btSoftColliders::CollideFF_DD docollide; + docollide.mrg = getCollisionShape()->getMargin() + + psb->getCollisionShape()->getMargin(); + docollide.psb[0] = this; + docollide.psb[1] = psb; + /* psb0 faces vs psb0 faces */ + btDbvntNode* root = copyToDbvnt(this->m_fdbvt.m_root); + calculateNormalCone(root); + this->m_fdbvt.selfCollideT(root,docollide); + delete root; + } // btSoftColliders::CollideFF_DD docollide; // /* common */ diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 1016fe9f6..8acf22a21 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -790,6 +790,7 @@ public: btAlignedObjectArray m_renderNodesInterpolationWeights; btAlignedObjectArray > m_renderNodesParents; + bool m_useSelfCollision; btAlignedObjectArray m_clusterConnectivity; //cluster connectivity, for self-collision @@ -1006,6 +1007,8 @@ public: /* defaultCollisionHandlers */ void defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap); void defaultCollisionHandler(btSoftBody* psb); + void setSelfCollision(bool useSelfCollision); + bool useSelfCollision(); // // Functionality to deal with new accelerated solvers. From 0cb7cb2445f734e72b1e3e2ba9b81a55497ec963 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 4 Oct 2019 18:31:29 -0700 Subject: [PATCH 05/29] bool->int --- examples/SharedMemory/PhysicsClientC_API.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 52569807b..752789428 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -636,7 +636,7 @@ extern "C" B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness); B3_SHARED_API int b3LoadSoftBodyAddGravityForce(b3SharedMemoryCommandHandle commandHandle, double gravityX, double gravityY, double gravityZ); B3_SHARED_API int b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle commandHandle, double collisionHardness); - B3_SHARED_API int b3LoadSoftBodySetSelfCollision(b3SharedMemoryCommandHandle commandHandle, bool useSelfCollision); + B3_SHARED_API int b3LoadSoftBodySetSelfCollision(b3SharedMemoryCommandHandle commandHandle, int useSelfCollision); B3_SHARED_API int b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHandle commandHandle, double frictionCoefficient); B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle commandHandle, int useBendingSprings); From c610ba49df6f3fccee903869a6ec94b9bcbcd7e9 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Sat, 5 Oct 2019 17:42:37 -0700 Subject: [PATCH 06/29] 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); From 3b945597d1ddbbc2bc7e37c0797b5a3a50165029 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 7 Oct 2019 14:40:11 -0700 Subject: [PATCH 07/29] prevent narrow phase collision detection between rigid and soft when both are sleeping --- src/BulletSoftBody/btSoftBody.cpp | 67 +++++++++++++----------- src/BulletSoftBody/btSoftBodyInternals.h | 2 + 2 files changed, 37 insertions(+), 32 deletions(-) diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index ef3a201c5..debe95d61 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -2306,9 +2306,9 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject(); // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect // but resolve contact at x_n - //btTransform wtr = (predict) ? - //(colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) - //: colObjWrap->getWorldTransform(); +// btTransform wtr = (predict) ? +// (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform()) +// : colObjWrap->getWorldTransform(); const btTransform& wtr = colObjWrap->getWorldTransform(); btScalar dst = m_worldInfo->m_sparsesdf.Evaluate( @@ -3430,35 +3430,38 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap { btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject()); - btTransform wtr = pcoWrap->getWorldTransform(); - - const btTransform ctr = pcoWrap->getWorldTransform(); - const btScalar timemargin = (wtr.getOrigin() - ctr.getOrigin()).length(); - const btScalar basemargin = getCollisionShape()->getMargin(); - btVector3 mins; - btVector3 maxs; - ATTRIBUTE_ALIGNED16(btDbvtVolume) - volume; - pcoWrap->getCollisionShape()->getAabb(pcoWrap->getWorldTransform(), - mins, - 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); - - btSoftColliders::CollideSDF_RDF docollideFace; - docollideFace.psb = this; - docollideFace.m_colObj1Wrap = pcoWrap; - docollideFace.m_rigidBody = prb1; - docollideFace.dynmargin = basemargin + timemargin; - docollideFace.stamargin = basemargin; - m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace); + 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; + btVector3 maxs; + ATTRIBUTE_ALIGNED16(btDbvtVolume) + volume; + pcoWrap->getCollisionShape()->getAabb(wtr, + mins, + 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); + + btSoftColliders::CollideSDF_RDF docollideFace; + docollideFace.psb = this; + docollideFace.m_colObj1Wrap = pcoWrap; + docollideFace.m_rigidBody = prb1; + docollideFace.dynmargin = basemargin + timemargin; + docollideFace.stamargin = basemargin; + m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace); + } } break; } diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index ff074eb43..afe9c7ea8 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -1074,6 +1074,7 @@ struct btSoftColliders if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ true) || psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true)) { const btScalar ima = n.m_im; + // todo: collision between multibody and fixed deformable node will be missed. const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; const btScalar ms = ima + imb; if (ms > 0) @@ -1169,6 +1170,7 @@ struct btSoftColliders { btScalar ima = n0->m_im + n1->m_im + n2->m_im; const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; + // todo: collision between multibody and fixed deformable face will be missed. const btScalar ms = ima + imb; if (ms > 0) { From ca92cf067ee8cd32b7dbcfcaa40d9ae1cb8ee6b4 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 7 Oct 2019 14:48:36 -0700 Subject: [PATCH 08/29] fix removeCollisionObject for Deformable world --- .../btDeformableMultiBodyDynamicsWorld.cpp | 9 +++++++++ src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h | 2 ++ 2 files changed, 11 insertions(+) diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 595ed737c..73e534d09 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -386,3 +386,12 @@ void btDeformableMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body) // force a reinitialize so that node indices get updated. m_deformableBodySolver->reinitialize(m_softBodies, btScalar(-1)); } + +void btDeformableMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject) +{ + btSoftBody* body = btSoftBody::upcast(collisionObject); + if (body) + removeSoftBody(body); + else + btDiscreteDynamicsWorld::removeCollisionObject(collisionObject); +} diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index e66e060f2..f618c989d 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -147,6 +147,8 @@ public: void removeSoftBody(btSoftBody* body); + void removeCollisionObject(btCollisionObject* collisionObject); + int getDrawFlags() const { return (m_drawFlags); } void setDrawFlags(int f) { m_drawFlags = f; } From 94facf002927f8d54c144a6bcde9803483009551 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 7 Oct 2019 17:24:22 -0700 Subject: [PATCH 09/29] typo fix --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index e5dc10bb4..e87251ede 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8145,7 +8145,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar psb->setCollisionFlags(0); psb->setTotalMass(mass); bool use_self_collision = false; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_USE_SELF_COLLISION) + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_SELF_COLLISION) { use_self_collision = loadSoftBodyArgs.m_useSelfCollision; } From 58a67f82fbe1b4668c67b199893d393c878d4963 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 7 Oct 2019 17:32:08 -0700 Subject: [PATCH 10/29] set default integration scheme to explicit --- src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index f618c989d..72cfa4fc5 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -83,7 +83,8 @@ public: m_sbi.water_normal = btVector3(0, 0, 0); m_sbi.m_gravity.setValue(0, -10, 0); m_internalTime = 0.0; - m_implicit = true; + m_implicit = false; + m_lineSearch = false; m_selfCollision = true; } From c808bb78c7ef6f6c178f4e504d6e46c00ce202bf Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 7 Oct 2019 19:30:45 -0700 Subject: [PATCH 11/29] delete lagrangian force when deleting dynamics world from command processor --- .../PhysicsServerCommandProcessor.cpp | 46 +++++++++++++------ 1 file changed, 32 insertions(+), 14 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index e87251ede..1f095105a 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -105,7 +105,7 @@ #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #endif -#define SKIP_DEFORMABLE_BODY 1 +//#define SKIP_DEFORMABLE_BODY 1 int gInternalSimFlags = 0; bool gResetSimulation = 0; @@ -1631,6 +1631,7 @@ struct PhysicsServerCommandProcessorInternalData #ifndef SKIP_DEFORMABLE_BODY btDeformableMultiBodyDynamicsWorld* m_dynamicsWorld; btDeformableBodySolver* m_deformablebodySolver; + btAlignedObjectArray m_lf; #else btSoftMultiBodyDynamicsWorld* m_dynamicsWorld; btSoftBodySolver* m_softbodySolver; @@ -2790,6 +2791,13 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() m_data->m_dynamicsWorld->removeMultiBody(mb); delete mb; } +#ifndef SKIP_DEFORMABLE + for (int j = 0; j < m_data->m_lf.size(); j++) + { + btDeformableLagrangianForce* force = m_data->m_lf[j]; + delete force; + } +#endif #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD for (i = m_data->m_dynamicsWorld->getSoftBodyArray().size() - 1; i >= 0; i--) { @@ -8066,7 +8074,9 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar { spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness; spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness; - m_data->m_dynamicsWorld->addForce(psb, new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, false)); + btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, false); + m_data->m_dynamicsWorld->addForce(psb, springForce); + m_data->m_lf.push_back(springForce); } #endif } @@ -8077,24 +8087,30 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar btScalar corotated_mu, corotated_lambda; if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE) { - corotated_mu = clientCmd.m_loadSoftBodyArguments.m_corotatedMu; - corotated_lambda = clientCmd.m_loadSoftBodyArguments.m_corotatedLambda; - m_data->m_dynamicsWorld->addForce(psb, new btDeformableCorotatedForce(corotated_mu, corotated_lambda)); + corotated_mu = clientCmd.m_loadSoftBodyArguments.m_corotatedMu; + corotated_lambda = clientCmd.m_loadSoftBodyArguments.m_corotatedLambda; + btDeformableLagrangianForce* corotatedForce = new btDeformableCorotatedForce(corotated_mu, corotated_lambda); + m_data->m_dynamicsWorld->addForce(psb, corotatedForce); + m_data->m_lf.push_back(corotatedForce); } btScalar neohookean_mu, neohookean_lambda, neohookean_damping; if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE) { - neohookean_mu = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanMu; - neohookean_lambda = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanLambda; - neohookean_damping = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanDamping; - m_data->m_dynamicsWorld->addForce(psb, new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda, neohookean_damping)); + neohookean_mu = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanMu; + neohookean_lambda = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanLambda; + neohookean_damping = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanDamping; + btDeformableLagrangianForce* neohookeanForce = new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda, neohookean_damping); + m_data->m_dynamicsWorld->addForce(psb, neohookeanForce); + m_data->m_lf.push_back(neohookeanForce); } btScalar spring_elastic_stiffness, spring_damping_stiffness; if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE) { - spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness; - spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness; - m_data->m_dynamicsWorld->addForce(psb, new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true)); + spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness; + spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness; + btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true); + m_data->m_dynamicsWorld->addForce(psb, springForce); + m_data->m_lf.push_back(springForce); } #endif } @@ -8113,7 +8129,9 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar psb->m_renderNodes.resize(0); } btVector3 gravity = m_data->m_dynamicsWorld->getGravity(); - m_data->m_dynamicsWorld->addForce(psb, new btDeformableGravityForce(gravity)); + btDeformableLagrangianForce* gravityForce = new btDeformableGravityForce(gravity); + m_data->m_dynamicsWorld->addForce(psb, gravityForce); + m_data->m_lf.push_back(gravityForce); btScalar collision_hardness = 1; psb->m_cfg.kKHR = collision_hardness; psb->m_cfg.kCHR = collision_hardness; @@ -8143,7 +8161,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar // collion between deformable and deformable and self-collision psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; psb->setCollisionFlags(0); - psb->setTotalMass(mass); + psb->setTotalMass(mass); bool use_self_collision = false; if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_SELF_COLLISION) { From 0d7ff567e66a55f8a219caaf46614b613c58a6c4 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 9 Oct 2019 00:38:13 -0700 Subject: [PATCH 12/29] bug fix in deformable predict motion --- src/BulletSoftBody/btDeformableBodySolver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 197ef7221..1dba88698 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -336,14 +336,14 @@ bool btDeformableBodySolver::updateNodes() void btDeformableBodySolver::predictMotion(btScalar solverdt) { + // apply explicit forces to velocity + m_objective->applyExplicitForce(m_residual); for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody *psb = m_softBodies[i]; if (psb->isActive()) { - // apply explicit forces to velocity - m_objective->applyExplicitForce(m_residual); // predict motion for collision detection predictDeformableMotion(psb, solverdt); } From 25a566c378a31f1d97530ffc9c45976e20e08f96 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 10 Oct 2019 13:12:40 -0700 Subject: [PATCH 13/29] finish deformable sleeping and add option for drag --- .../btDeformableBackwardEulerObjective.cpp | 4 ++++ src/BulletSoftBody/btDeformableBodySolver.cpp | 1 + src/BulletSoftBody/btDeformableGravityForce.h | 4 ++++ .../btDeformableMassSpringForce.h | 24 +++++++++++++++++++ .../btDeformableNeoHookeanForce.h | 8 +++++++ src/BulletSoftBody/btSoftBody.cpp | 1 + src/BulletSoftBody/btSoftBody.h | 1 + 7 files changed, 43 insertions(+) diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index 49232725a..fee353a60 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -101,6 +101,10 @@ void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } for (int j = 0; j < psb->m_nodes.size(); ++j) { btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im; diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 1dba88698..dc0b024f3 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -376,6 +376,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i) { btSoftBody::Node& n = psb->m_nodes[i]; + n.m_v *= (1 - psb->m_cfg.drag); n.m_q = n.m_x + n.m_v * dt; } /* Bounds */ diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h index d9bef8b07..abc120252 100644 --- a/src/BulletSoftBody/btDeformableGravityForce.h +++ b/src/BulletSoftBody/btDeformableGravityForce.h @@ -80,6 +80,10 @@ public: for (int i = 0; iisActive()) + { + continue; + } for (int j = 0; j < psb->m_nodes.size(); ++j) { const btSoftBody::Node& node = psb->m_nodes[j]; diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h index 9e7b3594c..44ac91c62 100644 --- a/src/BulletSoftBody/btDeformableMassSpringForce.h +++ b/src/BulletSoftBody/btDeformableMassSpringForce.h @@ -51,6 +51,10 @@ public: for (int i = 0; i < m_softBodies.size(); ++i) { const btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } for (int j = 0; j < psb->m_links.size(); ++j) { const btSoftBody::Link& link = psb->m_links[j]; @@ -83,6 +87,10 @@ public: for (int i = 0; i < m_softBodies.size(); ++i) { const btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } for (int j = 0; j < psb->m_links.size(); ++j) { const btSoftBody::Link& link = psb->m_links[j]; @@ -108,6 +116,10 @@ public: for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } btScalar scaled_k_damp = m_dampingStiffness * scale; for (int j = 0; j < psb->m_links.size(); ++j) { @@ -138,6 +150,10 @@ public: for (int i = 0; i < m_softBodies.size(); ++i) { const btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } for (int j = 0; j < psb->m_links.size(); ++j) { const btSoftBody::Link& link = psb->m_links[j]; @@ -160,6 +176,10 @@ public: for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } for (int j = 0; j < psb->m_nodes.size(); ++j) { sz = btMax(sz, psb->m_nodes[j].index); @@ -188,6 +208,10 @@ public: for (int i = 0; i < m_softBodies.size(); ++i) { const btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } btScalar scaled_k = m_elasticStiffness * scale; for (int j = 0; j < psb->m_links.size(); ++j) { diff --git a/src/BulletSoftBody/btDeformableNeoHookeanForce.h b/src/BulletSoftBody/btDeformableNeoHookeanForce.h index c40aaf4a7..797addf8b 100644 --- a/src/BulletSoftBody/btDeformableNeoHookeanForce.h +++ b/src/BulletSoftBody/btDeformableNeoHookeanForce.h @@ -93,6 +93,10 @@ public: for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } for (int j = 0; j < psb->m_tetraScratches.size(); ++j) { btSoftBody::Tetra& tetra = psb->m_tetras[j]; @@ -111,6 +115,10 @@ public: for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } for (int j = 0; j < psb->m_nodes.size(); ++j) { sz = btMax(sz, psb->m_nodes[j].index); diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index debe95d61..f63955470 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -89,6 +89,7 @@ void btSoftBody::initDefaults() m_cfg.piterations = 1; m_cfg.diterations = 0; m_cfg.citerations = 4; + m_cfg.drag = 0.01; m_cfg.collisions = fCollision::Default; m_cfg.collisions |= fCollision::VF_DD; m_pose.m_bvolume = false; diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 8acf22a21..31ddfabb9 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -702,6 +702,7 @@ public: tVSolverArray m_vsequence; // Velocity solvers sequence tPSolverArray m_psequence; // Position solvers sequence tPSolverArray m_dsequence; // Drift solvers sequence + btScalar drag; // deformable air drag }; /* SolverState */ struct SolverState From 992e1454b63a06d62fe2a1e7f55c5850ebe320cf Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 10 Oct 2019 13:13:14 -0700 Subject: [PATCH 14/29] turn on SKIP_DEFORMABLE --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 1f095105a..e9d93eff0 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -105,7 +105,7 @@ #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #endif -//#define SKIP_DEFORMABLE_BODY 1 +#define SKIP_DEFORMABLE_BODY 1 int gInternalSimFlags = 0; bool gResetSimulation = 0; @@ -2797,6 +2797,7 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() btDeformableLagrangianForce* force = m_data->m_lf[j]; delete force; } + m_data->m_lf.clear(); #endif #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD for (i = m_data->m_dynamicsWorld->getSoftBodyArray().size() - 1; i >= 0; i--) From 0d742273c1cbc9135394f6db50fd7dfb519bb399 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 10 Oct 2019 13:14:26 -0700 Subject: [PATCH 15/29] set default drag coefficient to 0 --- src/BulletSoftBody/btSoftBody.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index f63955470..f3d3f0e4b 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -89,7 +89,7 @@ void btSoftBody::initDefaults() m_cfg.piterations = 1; m_cfg.diterations = 0; m_cfg.citerations = 4; - m_cfg.drag = 0.01; + m_cfg.drag = 0; m_cfg.collisions = fCollision::Default; m_cfg.collisions |= fCollision::VF_DD; m_pose.m_bvolume = false; From 45c44977119e8a1fda1c0fdd3467572cad63816d Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 10 Oct 2019 13:32:11 -0700 Subject: [PATCH 16/29] fix compile --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index e9d93eff0..9e5799c36 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2791,7 +2791,7 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() m_data->m_dynamicsWorld->removeMultiBody(mb); delete mb; } -#ifndef SKIP_DEFORMABLE +#ifndef SKIP_DEFORMABLE_BODY for (int j = 0; j < m_data->m_lf.size(); j++) { btDeformableLagrangianForce* force = m_data->m_lf[j]; From 87546bb7c320f366ac46fd2aa5717d9d517ead7f Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 10 Oct 2019 14:26:44 -0700 Subject: [PATCH 17/29] prevent velocity of deformable from getting too big from explicit force --- src/BulletSoftBody/btDeformableBodySolver.cpp | 15 +++- src/BulletSoftBody/btSoftBody.cpp | 82 +++++++++++++------ 2 files changed, 70 insertions(+), 27 deletions(-) diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index dc0b024f3..618bf7ed5 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -372,15 +372,26 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d psb->m_sst.velmrg = psb->m_sst.sdt * 3; psb->m_sst.radmrg = psb->getCollisionShape()->getMargin(); psb->m_sst.updmrg = psb->m_sst.radmrg * (btScalar)0.25; + /* Bounds */ + psb->updateBounds(); + /* Integrate */ + // do not allow particles to move more than 10% of the bounding box size + btScalar max_v = 0.1 * (psb->m_bounds[1]-psb->m_bounds[0]).norm() / dt; for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i) { btSoftBody::Node& n = psb->m_nodes[i]; + // apply drag n.m_v *= (1 - psb->m_cfg.drag); + // scale velocity back + if (n.m_v.norm() > max_v) + { + n.m_v.safeNormalize(); + n.m_v *= max_v; + } n.m_q = n.m_x + n.m_v * dt; } - /* Bounds */ - psb->updateBounds(); + /* Nodes */ ATTRIBUTE_ALIGNED16(btDbvtVolume) vol; diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index f3d3f0e4b..c3d99590a 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -2428,31 +2428,63 @@ void btSoftBody::updateBounds() m_bounds[1] = btVector3(1000, 1000, 1000); } else {*/ - if (m_ndbvt.m_root) - { - const btVector3& mins = m_ndbvt.m_root->volume.Mins(); - const btVector3& maxs = m_ndbvt.m_root->volume.Maxs(); - const btScalar csm = getCollisionShape()->getMargin(); - const btVector3 mrg = btVector3(csm, - csm, - csm) * - 1; // ??? to investigate... - m_bounds[0] = mins - mrg; - m_bounds[1] = maxs + mrg; - if (0 != getBroadphaseHandle()) - { - m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(), - m_bounds[0], - m_bounds[1], - m_worldInfo->m_dispatcher); - } - } - else - { - m_bounds[0] = - m_bounds[1] = btVector3(0, 0, 0); - } - //} +// if (m_ndbvt.m_root) +// { +// const btVector3& mins = m_ndbvt.m_root->volume.Mins(); +// const btVector3& maxs = m_ndbvt.m_root->volume.Maxs(); +// const btScalar csm = getCollisionShape()->getMargin(); +// const btVector3 mrg = btVector3(csm, +// csm, +// csm) * +// 1; // ??? to investigate... +// m_bounds[0] = mins - mrg; +// m_bounds[1] = maxs + mrg; +// if (0 != getBroadphaseHandle()) +// { +// m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(), +// m_bounds[0], +// m_bounds[1], +// m_worldInfo->m_dispatcher); +// } +// } +// else +// { +// m_bounds[0] = +// m_bounds[1] = btVector3(0, 0, 0); +// } + if (m_nodes.size()) + { + btVector3 mins = m_nodes[0].m_x; + btVector3 maxs = m_nodes[0].m_x; + for (int i = 1; i < m_nodes.size(); ++i) + { + for (int d = 0; d < 3; ++d) + { + if (m_nodes[i].m_x[d] > maxs[d]) + maxs[d] = m_nodes[i].m_x[d]; + if (m_nodes[i].m_x[d] < mins[d]) + mins[d] = m_nodes[i].m_x[d]; + } + } + const btScalar csm = getCollisionShape()->getMargin(); + const btVector3 mrg = btVector3(csm, + csm, + csm); + m_bounds[0] = mins - mrg; + m_bounds[1] = maxs + mrg; + if (0 != getBroadphaseHandle()) + { + m_worldInfo->m_broadphase->setAabb(getBroadphaseHandle(), + m_bounds[0], + m_bounds[1], + m_worldInfo->m_dispatcher); + } + } + else + { + m_bounds[0] = + m_bounds[1] = btVector3(0, 0, 0); + } } // From e87df1854405c6a6e5af0468e3e1059f92afc751 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 10 Oct 2019 22:55:11 -0700 Subject: [PATCH 18/29] switch to damping model from Irvine 05 for its SPDness --- src/BulletSoftBody/btDeformableBodySolver.cpp | 4 ++-- src/BulletSoftBody/btDeformableNeoHookeanForce.h | 10 ++++++++-- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 618bf7ed5..676df528d 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -376,8 +376,8 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d psb->updateBounds(); /* Integrate */ - // do not allow particles to move more than 10% of the bounding box size - btScalar max_v = 0.1 * (psb->m_bounds[1]-psb->m_bounds[0]).norm() / dt; + // do not allow particles to move more than the bounding box size + btScalar max_v = (psb->m_bounds[1]-psb->m_bounds[0]).norm() / dt; for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i) { btSoftBody::Node& n = psb->m_nodes[i]; diff --git a/src/BulletSoftBody/btDeformableNeoHookeanForce.h b/src/BulletSoftBody/btDeformableNeoHookeanForce.h index 797addf8b..816d214ef 100644 --- a/src/BulletSoftBody/btDeformableNeoHookeanForce.h +++ b/src/BulletSoftBody/btDeformableNeoHookeanForce.h @@ -73,7 +73,10 @@ public: size_t id3 = node3->index; btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse; btMatrix3x3 dP; - firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); +// firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); + btMatrix3x3 I; + I.setIdentity(); + dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp; 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(); @@ -210,7 +213,10 @@ public: size_t id3 = node3->index; btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse; btMatrix3x3 dP; - firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); +// firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); + btMatrix3x3 I; + I.setIdentity(); + dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp; // 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; From 9546390fd68b4233245f907b68cc2568f446f91c Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 11 Oct 2019 16:24:07 -0700 Subject: [PATCH 19/29] clamp stress for NeoHookean in singular value space --- .../btDeformableNeoHookeanForce.h | 55 +- src/LinearMath/CMakeLists.txt | 1 + src/LinearMath/btImplicitQRSVD.h | 892 ++++++++++++++++++ 3 files changed, 946 insertions(+), 2 deletions(-) create mode 100644 src/LinearMath/btImplicitQRSVD.h diff --git a/src/BulletSoftBody/btDeformableNeoHookeanForce.h b/src/BulletSoftBody/btDeformableNeoHookeanForce.h index 816d214ef..e0f1b76d1 100644 --- a/src/BulletSoftBody/btDeformableNeoHookeanForce.h +++ b/src/BulletSoftBody/btDeformableNeoHookeanForce.h @@ -18,6 +18,8 @@ subject to the following restrictions: #include "btDeformableLagrangianForce.h" #include "LinearMath/btQuickprof.h" +#include "LinearMath/btImplicitQRSVD.h" +#include "Eigen" // This energy is as described in https://graphics.pixar.com/library/StableElasticity/paper.pdf class btDeformableNeoHookeanForce : public btDeformableLagrangianForce { @@ -72,11 +74,12 @@ public: size_t id2 = node2->index; size_t id3 = node3->index; btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse; + btMatrix3x3 C = dF + dF.transpose(); btMatrix3x3 dP; // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); btMatrix3x3 I; I.setIdentity(); - dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp; + dP = C * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp; 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(); @@ -155,6 +158,7 @@ public: virtual void addScaledElasticForce(btScalar scale, TVStack& force) { +// btScalar max_p = 0; int numNodes = getNumNodes(); btAssert(numNodes <= force.size()); btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); @@ -166,6 +170,51 @@ public: btSoftBody::Tetra& tetra = psb->m_tetras[j]; btMatrix3x3 P; firstPiola(psb->m_tetraScratches[j],P); + + btMatrix3x3 U, V; + btVector3 sigma; + singularValueDecomposition(P, U, sigma, V); + sigma[0] = btMin(sigma[0], (btScalar)1000); + sigma[1] = btMin(sigma[1], (btScalar)1000); + sigma[2] = btMin(sigma[2], (btScalar)1000); + sigma[0] = btMax(sigma[0], (btScalar)-1000); + sigma[1] = btMax(sigma[1], (btScalar)-1000); + sigma[2] = btMax(sigma[2], (btScalar)-1000); + btMatrix3x3 Sigma; + Sigma.setIdentity(); + Sigma[0][0] = sigma[0]; + Sigma[1][1] = sigma[1]; + Sigma[2][2] = sigma[2]; +// max_p = btMax(max_p, sigma[0]); +// max_p = btMax(max_p, sigma[1]); +// max_p = btMax(max_p, sigma[2]); + P = U * Sigma * V.transpose(); + +// Eigen::Matrix eigenP; +// eigenP << P[0][0], P[0][1], P[0][2], +// P[1][0],P[1][1],P[1][2], +// P[2][0],P[2][1],P[2][2]; +// Eigen::JacobiSVD > svd(eigenP, Eigen::ComputeFullU | Eigen::ComputeFullV); +// Eigen::Matrix P_hat = svd.singularValues().asDiagonal(); +// P_hat(0,0) = btMin((btScalar)P_hat(0,0), (btScalar)20); +// P_hat(1,1) = btMin((btScalar)P_hat(1,1), (btScalar)20); +// P_hat(2,2) = btMin((btScalar)P_hat(2,2), (btScalar)20); +// eigenP = svd.matrixU() * P_hat * svd.matrixV().transpose(); +// max_p = btMax(max_p, (btScalar)P_hat(0,0)); +// max_p = btMax(max_p, (btScalar)P_hat(1,1)); +// max_p = btMax(max_p, (btScalar)P_hat(2,2)); +// +// P[0][0] = eigenP(0,0); +// P[0][1] = eigenP(0,1); +// P[0][2] = eigenP(0,2); +// P[1][0] = eigenP(1,0); +// P[1][1] = eigenP(1,1); +// P[1][2] = eigenP(1,2); +// P[2][0] = eigenP(2,0); +// P[2][1] = eigenP(2,1); +// P[2][2] = eigenP(2,2); + + // 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; @@ -187,6 +236,7 @@ public: force[id3] -= scale1 * force_on_node123.getColumn(2); } } +// std::cout << max_p << std::endl; } // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search @@ -212,11 +262,12 @@ public: size_t id2 = node2->index; size_t id3 = node3->index; btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse; + btMatrix3x3 C = dF + dF.transpose(); btMatrix3x3 dP; // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); btMatrix3x3 I; I.setIdentity(); - dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp; + dP = C * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp; // 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; diff --git a/src/LinearMath/CMakeLists.txt b/src/LinearMath/CMakeLists.txt index 0c8c0133a..a5491c325 100644 --- a/src/LinearMath/CMakeLists.txt +++ b/src/LinearMath/CMakeLists.txt @@ -32,6 +32,7 @@ SET(LinearMath_HDRS btIDebugDraw.h btList.h btMatrix3x3.h + btImplicitQRSVD.h btMinMax.h btMotionState.h btPolarDecomposition.h diff --git a/src/LinearMath/btImplicitQRSVD.h b/src/LinearMath/btImplicitQRSVD.h new file mode 100644 index 000000000..0785a6259 --- /dev/null +++ b/src/LinearMath/btImplicitQRSVD.h @@ -0,0 +1,892 @@ +/** + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + + Copyright (c) 2016 Theodore Gast, Chuyuan Fu, Chenfanfu Jiang, Joseph Teran + + Permission is hereby granted, free of charge, to any person obtaining a copy of + this software and associated documentation files (the "Software"), to deal in + the Software without restriction, including without limitation the rights to + use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies + of the Software, and to permit persons to whom the Software is furnished to do + so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + If the code is used in an article, the following paper shall be cited: + @techreport{qrsvd:2016, + title={Implicit-shifted Symmetric QR Singular Value Decomposition of 3x3 Matrices}, + author={Gast, Theodore and Fu, Chuyuan and Jiang, Chenfanfu and Teran, Joseph}, + year={2016}, + institution={University of California Los Angeles} + } + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. +**/ + +#ifndef btImplicitQRSVD_h +#define btImplicitQRSVD_h + +#include "btMatrix3x3.h" +class btMatrix2x2 +{ +public: + btScalar m_00, m_01, m_10, m_11; + btMatrix2x2(): m_00(0), m_10(0), m_01(0), m_11(0) + { + } + btMatrix2x2(const btMatrix2x2& other): m_00(other.m_00),m_01(other.m_01),m_10(other.m_10),m_11(other.m_11) + {} + btScalar& operator()(int i, int j) + { + if (i == 0 && j == 0) + return m_00; + if (i == 1 && j == 0) + return m_10; + if (i == 0 && j == 1) + return m_01; + if (i == 1 && j == 1) + return m_11; + btAssert(false); + return m_00; + } + const btScalar& operator()(int i, int j) const + { + if (i == 0 && j == 0) + return m_00; + if (i == 1 && j == 0) + return m_10; + if (i == 0 && j == 1) + return m_01; + if (i == 1 && j == 1) + return m_11; + btAssert(false); + return m_00; + } + void setIdentity() + { + m_00 = 1; + m_11 = 1; + m_01 = 0; + m_10 = 0; + } +}; + +/** + Class for givens rotation. + Row rotation G*A corresponds to something like + c -s 0 + ( s c 0 ) A + 0 0 1 + Column rotation A G' corresponds to something like + c -s 0 + A ( s c 0 ) + 0 0 1 + + c and s are always computed so that + ( c -s ) ( a ) = ( * ) + s c b ( 0 ) + + Assume rowi(R); + A.setIdentity(); + A[rowi][rowi] = c; + A[rowk][rowi] = -s; + A[rowi][rowk] = s; + A[rowk][rowk] = c; + } + + inline void fill(const btMatrix2x2& R) const + { + btMatrix2x2& A = const_cast(R); + A(rowi,rowi) = c; + A(rowk,rowi) = -s; + A(rowi,rowk) = s; + A(rowk,rowk) = c; + } + + /** + This function does something like + c -s 0 + ( s c 0 ) A -> A + 0 0 1 + It only affects row i and row k of A. + */ + inline void rowRotation(btMatrix3x3& A) const + { + for (int j = 0; j < 3; j++) { + btScalar tau1 = A[rowi][j]; + btScalar tau2 = A[rowk][j]; + A[rowi][j] = c * tau1 - s * tau2; + A[rowk][j] = s * tau1 + c * tau2; + } + } + inline void rowRotation(btMatrix2x2& A) const + { + for (int j = 0; j < 2; j++) { + btScalar tau1 = A(rowi,j); + btScalar tau2 = A(rowk,j); + A(rowi,j) = c * tau1 - s * tau2; + A(rowk,j) = s * tau1 + c * tau2; + } + } + + /** + This function does something like + c s 0 + A ( -s c 0 ) -> A + 0 0 1 + It only affects column i and column k of A. + */ + inline void columnRotation(btMatrix3x3& A) const + { + for (int j = 0; j < 3; j++) { + btScalar tau1 = A[j][rowi]; + btScalar tau2 = A[j][rowk]; + A[j][rowi] = c * tau1 - s * tau2; + A[j][rowk] = s * tau1 + c * tau2; + } + } + inline void columnRotation(btMatrix2x2& A) const + { + for (int j = 0; j < 2; j++) { + btScalar tau1 = A(j,rowi); + btScalar tau2 = A(j,rowk); + A(j,rowi) = c * tau1 - s * tau2; + A(j,rowk) = s * tau1 + c * tau2; + } + } + + /** + Multiply givens must be for same row and column + **/ + inline void operator*=(const GivensRotation& A) + { + btScalar new_c = c * A.c - s * A.s; + btScalar new_s = s * A.c + c * A.s; + c = new_c; + s = new_s; + } + + /** + Multiply givens must be for same row and column + **/ + inline GivensRotation operator*(const GivensRotation& A) const + { + GivensRotation r(*this); + r *= A; + return r; + } +}; + +/** + \brief zero chasing the 3X3 matrix to bidiagonal form + original form of H: x x 0 + x x x + 0 0 x + after zero chase: + x x 0 + 0 x x + 0 0 x + */ +inline void zeroChase(btMatrix3x3& H, btMatrix3x3& U, btMatrix3x3& V) +{ + + /** + Reduce H to of form + x x + + 0 x x + 0 0 x + */ + GivensRotation r1(H[0][0], H[1][0], 0, 1); + /** + Reduce H to of form + x x 0 + 0 x x + 0 + x + Can calculate r2 without multiplying by r1 since both entries are in first two + rows thus no need to divide by sqrt(a^2+b^2) + */ + GivensRotation r2(1, 2); + if (H[1][0] != 0) + r2.compute(H[0][0] * H[0][1] + H[1][0] * H[1][1], H[0][0] * H[0][2] + H[1][0] * H[1][2]); + else + r2.compute(H[0][1], H[0][2]); + + r1.rowRotation(H); + + /* GivensRotation r2(H(0, 1), H(0, 2), 1, 2); */ + r2.columnRotation(H); + r2.columnRotation(V); + + /** + Reduce H to of form + x x 0 + 0 x x + 0 0 x + */ + GivensRotation r3(H[1][1], H[2][1], 1, 2); + r3.rowRotation(H); + + // Save this till end for better cache coherency + // r1.rowRotation(u_transpose); + // r3.rowRotation(u_transpose); + r1.columnRotation(U); + r3.columnRotation(U); +} + +/** + \brief make a 3X3 matrix to upper bidiagonal form + original form of H: x x x + x x x + x x x + after zero chase: + x x 0 + 0 x x + 0 0 x + */ +inline void makeUpperBidiag(btMatrix3x3& H, btMatrix3x3& U, btMatrix3x3& V) +{ + U.setIdentity(); + V.setIdentity(); + + /** + Reduce H to of form + x x x + x x x + 0 x x + */ + + GivensRotation r(H[1][0], H[2][0], 1, 2); + r.rowRotation(H); + // r.rowRotation(u_transpose); + r.columnRotation(U); + // zeroChase(H, u_transpose, V); + zeroChase(H, U, V); +} + +/** + \brief make a 3X3 matrix to lambda shape + original form of H: x x x + * x x x + * x x x + after : + * x 0 0 + * x x 0 + * x 0 x + */ +inline void makeLambdaShape(btMatrix3x3& H, btMatrix3x3& U, btMatrix3x3& V) +{ + U.setIdentity(); + V.setIdentity(); + + /** + Reduce H to of form + * x x 0 + * x x x + * x x x + */ + + GivensRotation r1(H[0][1], H[0][2], 1, 2); + r1.columnRotation(H); + r1.columnRotation(V); + + /** + Reduce H to of form + * x x 0 + * x x 0 + * x x x + */ + + r1.computeUnconventional(H[1][2], H[2][2]); + r1.rowRotation(H); + r1.columnRotation(U); + + /** + Reduce H to of form + * x x 0 + * x x 0 + * x 0 x + */ + + GivensRotation r2(H[2][0], H[2][1], 0, 1); + r2.columnRotation(H); + r2.columnRotation(V); + + /** + Reduce H to of form + * x 0 0 + * x x 0 + * x 0 x + */ + r2.computeUnconventional(H[0][1], H[1][1]); + r2.rowRotation(H); + r2.columnRotation(U); +} + +/** + \brief 2x2 polar decomposition. + \param[in] A matrix. + \param[out] R Robustly a rotation matrix. + \param[out] S_Sym Symmetric. Whole matrix is stored + + Polar guarantees negative sign is on the small magnitude singular value. + S is guaranteed to be the closest one to identity. + R is guaranteed to be the closest rotation to A. + */ +inline void polarDecomposition(const btMatrix2x2& A, + GivensRotation& R, + const btMatrix2x2& S_Sym) +{ + btScalar a = (A(0, 0) + A(1, 1)), b = (A(1, 0) - A(0, 1)); + btScalar denominator = btSqrt(a*a+b*b); + R.c = (btScalar)1; + R.s = (btScalar)0; + if (denominator != 0) { + /* + No need to use a tolerance here because x(0) and x(1) always have + smaller magnitude then denominator, therefore overflow never happens. + */ + R.c = a / denominator; + R.s = -b / denominator; + } + btMatrix2x2& S = const_cast(S_Sym); + S = A; + R.rowRotation(S); +} + +inline void polarDecomposition(const btMatrix2x2& A, + const btMatrix2x2& R, + const btMatrix2x2& S_Sym) +{ + GivensRotation r(0, 1); + polarDecomposition(A, r, S_Sym); + r.fill(R); +} + +/** + \brief 2x2 SVD (singular value decomposition) A=USV' + \param[in] A Input matrix. + \param[out] U Robustly a rotation matrix in Givens form + \param[out] Sigma matrix of singular values sorted with decreasing magnitude. The second one can be negative. + \param[out] V Robustly a rotation matrix in Givens form + */ +inline void singularValueDecomposition( + const btMatrix2x2& A, + GivensRotation& U, + const btMatrix2x2& Sigma, + GivensRotation& V, + const btScalar tol = 64 * std::numeric_limits::epsilon()) +{ + btMatrix2x2& sigma = const_cast(Sigma); + sigma.setIdentity(); + btMatrix2x2 S_Sym; + polarDecomposition(A, U, S_Sym); + btScalar cosine, sine; + btScalar x = S_Sym(0, 0); + btScalar y = S_Sym(0, 1); + btScalar z = S_Sym(1, 1); + if (y == 0) { + // S is already diagonal + cosine = 1; + sine = 0; + sigma(0,0) = x; + sigma(1,1) = z; + } + else { + btScalar tau = 0.5 * (x - z); + btScalar w = btSqrt(tau * tau + y * y); + // w > y > 0 + btScalar t; + if (tau > 0) { + // tau + w > w > y > 0 ==> division is safe + t = y / (tau + w); + } + else { + // tau - w < -w < -y < 0 ==> division is safe + t = y / (tau - w); + } + cosine = btScalar(1) / btSqrt(t * t + btScalar(1)); + sine = -t * cosine; + /* + V = [cosine -sine; sine cosine] + Sigma = V'SV. Only compute the diagonals for efficiency. + Also utilize symmetry of S and don't form V yet. + */ + btScalar c2 = cosine * cosine; + btScalar csy = 2 * cosine * sine * y; + btScalar s2 = sine * sine; + sigma(0,0) = c2 * x - csy + s2 * z; + sigma(1,1) = s2 * x + csy + c2 * z; + } + + // Sorting + // Polar already guarantees negative sign is on the small magnitude singular value. + if (sigma(0,0) < sigma(1,1)) { + std::swap(sigma(0,0), sigma(1,1)); + V.c = -sine; + V.s = cosine; + } + else { + V.c = cosine; + V.s = sine; + } + U *= V; +} + +/** + \brief 2x2 SVD (singular value decomposition) A=USV' + \param[in] A Input matrix. + \param[out] U Robustly a rotation matrix. + \param[out] Sigma Vector of singular values sorted with decreasing magnitude. The second one can be negative. + \param[out] V Robustly a rotation matrix. + */ +inline void singularValueDecomposition( + const btMatrix2x2& A, + const btMatrix2x2& U, + const btMatrix2x2& Sigma, + const btMatrix2x2& V, + const btScalar tol = 64 * std::numeric_limits::epsilon()) +{ + GivensRotation gv(0, 1); + GivensRotation gu(0, 1); + singularValueDecomposition(A, gu, Sigma, gv); + + gu.fill(U); + gv.fill(V); +} + +/** + \brief compute wilkinsonShift of the block + a1 b1 + b1 a2 + based on the wilkinsonShift formula + mu = c + d - sign (d) \ sqrt (d*d + b*b), where d = (a-c)/2 + + */ +inline btScalar wilkinsonShift(const btScalar a1, const btScalar b1, const btScalar a2) +{ + using std::sqrt; + using std::fabs; + using std::copysign; + + btScalar d = (btScalar)0.5 * (a1 - a2); + btScalar bs = b1 * b1; + + btScalar mu = a2 - copysign(bs / (fabs(d) + sqrt(d * d + bs)), d); + // T mu = a2 - bs / ( d + sign_d*sqrt (d*d + bs)); + return mu; +} + +/** + \brief Helper function of 3X3 SVD for processing 2X2 SVD + */ +template +inline void process(btMatrix3x3& B, btMatrix3x3& U, btVector3& sigma, btMatrix3x3& V) +{ + int other = (t == 1) ? 0 : 2; + GivensRotation u(0, 1); + GivensRotation v(0, 1); + sigma[other] = B[other][other]; + + btMatrix2x2 B_sub, sigma_sub; + if (t == 0) + { + B_sub.m_00 = B[0][0]; + B_sub.m_10 = B[1][0]; + B_sub.m_01 = B[0][1]; + B_sub.m_11 = B[1][1]; + sigma_sub.m_00 = sigma[0]; + sigma_sub.m_11 = sigma[1]; +// singularValueDecomposition(B.template block<2, 2>(t, t), u, sigma.template block<2, 1>(t, 0), v); + singularValueDecomposition(B_sub, u, sigma_sub, v); + B[0][0] = B_sub.m_00; + B[1][0] = B_sub.m_10; + B[0][1] = B_sub.m_01; + B[1][1] = B_sub.m_11; + sigma[0] = sigma_sub.m_00; + sigma[1] = sigma_sub.m_11; + } + else + { + B_sub.m_00 = B[1][1]; + B_sub.m_10 = B[2][1]; + B_sub.m_01 = B[1][2]; + B_sub.m_11 = B[2][2]; + sigma_sub.m_00 = sigma[1]; + sigma_sub.m_11 = sigma[2]; + // singularValueDecomposition(B.template block<2, 2>(t, t), u, sigma.template block<2, 1>(t, 0), v); + singularValueDecomposition(B_sub, u, sigma_sub, v); + B[1][1] = B_sub.m_00; + B[2][1] = B_sub.m_10; + B[1][2] = B_sub.m_01; + B[2][2] = B_sub.m_11; + sigma[1] = sigma_sub.m_00; + sigma[2] = sigma_sub.m_11; + } + u.rowi += t; + u.rowk += t; + v.rowi += t; + v.rowk += t; + u.columnRotation(U); + v.columnRotation(V); +} + +/** + \brief Helper function of 3X3 SVD for flipping signs due to flipping signs of sigma + */ +inline void flipSign(int i, btMatrix3x3& U, btVector3& sigma) +{ + sigma[i] = -sigma[i]; + U[0][i] = -U[0][i]; + U[1][i] = -U[1][i]; + U[2][i] = -U[2][i]; +} + +inline void flipSign(int i, btMatrix3x3& U) +{ + U[0][i] = -U[0][i]; + U[1][i] = -U[1][i]; + U[2][i] = -U[2][i]; +} + +inline void swapCol(btMatrix3x3& A, int i, int j) +{ + for (int d = 0; d < 3; ++d) + std::swap(A[d][i], A[d][j]); +} +/** + \brief Helper function of 3X3 SVD for sorting singular values + */ +inline void sort(btMatrix3x3& U, btVector3& sigma, btMatrix3x3& V, int t) +{ + using std::fabs; + + if (t == 0) + { + // Case: sigma(0) > |sigma(1)| >= |sigma(2)| + if (fabs(sigma[1]) >= fabs(sigma[2])) { + if (sigma[1] < 0) { + flipSign(1, U, sigma); + flipSign(2, U, sigma); + } + return; + } + + //fix sign of sigma for both cases + if (sigma[2] < 0) { + flipSign(1, U, sigma); + flipSign(2, U, sigma); + } + + //swap sigma(1) and sigma(2) for both cases + std::swap(sigma[1], sigma[2]); + // swap the col 1 and col 2 for U,V + swapCol(U,1,2); + swapCol(V,1,2); + + // Case: |sigma(2)| >= sigma(0) > |simga(1)| + if (sigma[1] > sigma[0]) { + std::swap(sigma[0], sigma[1]); + swapCol(U,0,1); + swapCol(V,0,1); + } + + // Case: sigma(0) >= |sigma(2)| > |simga(1)| + else { + flipSign(2, U); + flipSign(2, V); + } + } + else if (t == 1) + { + // Case: |sigma(0)| >= sigma(1) > |sigma(2)| + if (fabs(sigma[0]) >= sigma[1]) { + if (sigma[0] < 0) { + flipSign(0, U, sigma); + flipSign(2, U, sigma); + } + return; + } + + //swap sigma(0) and sigma(1) for both cases + std::swap(sigma[0], sigma[1]); + swapCol(U, 0, 1); + swapCol(V, 0, 1); + + // Case: sigma(1) > |sigma(2)| >= |sigma(0)| + if (fabs(sigma[1]) < fabs(sigma[2])) { + std::swap(sigma[1], sigma[2]); + swapCol(U, 1, 2); + swapCol(V, 1, 2); + } + + // Case: sigma(1) >= |sigma(0)| > |sigma(2)| + else { + flipSign(1, U); + flipSign(1, V); + } + + // fix sign for both cases + if (sigma[1] < 0) { + flipSign(1, U, sigma); + flipSign(2, U, sigma); + } + } +} + +/** + \brief 3X3 SVD (singular value decomposition) A=USV' + \param[in] A Input matrix. + \param[out] U is a rotation matrix. + \param[out] sigma Diagonal matrix, sorted with decreasing magnitude. The third one can be negative. + \param[out] V is a rotation matrix. + */ +inline int singularValueDecomposition(const btMatrix3x3& A, + btMatrix3x3& U, + btVector3& sigma, + btMatrix3x3& V, + btScalar tol = 128*std::numeric_limits::epsilon()) +{ + using std::fabs; + using std::sqrt; + using std::max; + btMatrix3x3 B = A; + U.setIdentity(); + V.setIdentity(); + + makeUpperBidiag(B, U, V); + + int count = 0; + btScalar mu = (btScalar)0; + GivensRotation r(0, 1); + + btScalar alpha_1 = B[0][0]; + btScalar beta_1 = B[0][1]; + btScalar alpha_2 = B[1][1]; + btScalar alpha_3 = B[2][2]; + btScalar beta_2 = B[1][2]; + btScalar gamma_1 = alpha_1 * beta_1; + btScalar gamma_2 = alpha_2 * beta_2; + tol *= max((btScalar)0.5 * sqrt(alpha_1 * alpha_1 + alpha_2 * alpha_2 + alpha_3 * alpha_3 + beta_1 * beta_1 + beta_2 * beta_2), (btScalar)1); + + /** + Do implicit shift QR until A^T A is block diagonal + */ + + while (fabs(beta_2) > tol && fabs(beta_1) > tol + && fabs(alpha_1) > tol && fabs(alpha_2) > tol + && fabs(alpha_3) > tol) { + mu = wilkinsonShift(alpha_2 * alpha_2 + beta_1 * beta_1, gamma_2, alpha_3 * alpha_3 + beta_2 * beta_2); + + r.compute(alpha_1 * alpha_1 - mu, gamma_1); + r.columnRotation(B); + + r.columnRotation(V); + zeroChase(B, U, V); + + alpha_1 = B[0][0]; + beta_1 = B[0][1]; + alpha_2 = B[1][1]; + alpha_3 = B[2][2]; + beta_2 = B[1][2]; + gamma_1 = alpha_1 * beta_1; + gamma_2 = alpha_2 * beta_2; + count++; + } + /** + Handle the cases of one of the alphas and betas being 0 + Sorted by ease of handling and then frequency + of occurrence + + If B is of form + x x 0 + 0 x 0 + 0 0 x + */ + if (fabs(beta_2) <= tol) { + process<0>(B, U, sigma, V); + sort(U, sigma, V,0); + } + /** + If B is of form + x 0 0 + 0 x x + 0 0 x + */ + else if (fabs(beta_1) <= tol) { + process<1>(B, U, sigma, V); + sort(U, sigma, V,1); + } + /** + If B is of form + x x 0 + 0 0 x + 0 0 x + */ + else if (fabs(alpha_2) <= tol) { + /** + Reduce B to + x x 0 + 0 0 0 + 0 0 x + */ + GivensRotation r1(1, 2); + r1.computeUnconventional(B[1][2], B[2][2]); + r1.rowRotation(B); + r1.columnRotation(U); + + process<0>(B, U, sigma, V); + sort(U, sigma, V, 0); + } + /** + If B is of form + x x 0 + 0 x x + 0 0 0 + */ + else if (fabs(alpha_3) <= tol) { + /** + Reduce B to + x x + + 0 x 0 + 0 0 0 + */ + GivensRotation r1(1, 2); + r1.compute(B[1][1], B[1][2]); + r1.columnRotation(B); + r1.columnRotation(V); + /** + Reduce B to + x x 0 + + x 0 + 0 0 0 + */ + GivensRotation r2(0, 2); + r2.compute(B[0][0], B[0][2]); + r2.columnRotation(B); + r2.columnRotation(V); + + process<0>(B, U, sigma, V); + sort(U, sigma, V, 0); + } + /** + If B is of form + 0 x 0 + 0 x x + 0 0 x + */ + else if (fabs(alpha_1) <= tol) { + /** + Reduce B to + 0 0 + + 0 x x + 0 0 x + */ + GivensRotation r1(0, 1); + r1.computeUnconventional(B[0][1], B[1][1]); + r1.rowRotation(B); + r1.columnRotation(U); + + /** + Reduce B to + 0 0 0 + 0 x x + 0 + x + */ + GivensRotation r2(0, 2); + r2.computeUnconventional(B[0][2], B[2][2]); + r2.rowRotation(B); + r2.columnRotation(U); + + process<1>(B, U, sigma, V); + sort(U, sigma, V, 1); + } + + return count; +} +#endif /* btImplicitQRSVD_h */ From 9e29f7108d8ba2c991e6b98eabeb354e9c085c28 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 11 Oct 2019 16:34:56 -0700 Subject: [PATCH 20/29] remove Eigen --- src/BulletSoftBody/btDeformableNeoHookeanForce.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/BulletSoftBody/btDeformableNeoHookeanForce.h b/src/BulletSoftBody/btDeformableNeoHookeanForce.h index e0f1b76d1..589abf2e3 100644 --- a/src/BulletSoftBody/btDeformableNeoHookeanForce.h +++ b/src/BulletSoftBody/btDeformableNeoHookeanForce.h @@ -19,7 +19,6 @@ subject to the following restrictions: #include "btDeformableLagrangianForce.h" #include "LinearMath/btQuickprof.h" #include "LinearMath/btImplicitQRSVD.h" -#include "Eigen" // This energy is as described in https://graphics.pixar.com/library/StableElasticity/paper.pdf class btDeformableNeoHookeanForce : public btDeformableLagrangianForce { From a7222d8a9f4f287976043d53788e37b308370d18 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Sat, 12 Oct 2019 18:48:56 -0700 Subject: [PATCH 21/29] add option to set stress clamping limit --- examples/DeformableDemo/GraspDeformable.cpp | 37 ++++++----- .../btDeformableNeoHookeanForce.h | 65 ++++++------------- src/BulletSoftBody/btSoftBody.cpp | 7 ++ src/BulletSoftBody/btSoftBody.h | 2 + 4 files changed, 49 insertions(+), 62 deletions(-) diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index 970fde5f4..98fb63a8d 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -190,6 +190,7 @@ void GraspDeformable::initPhysics() m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); // build a gripper + if(1) { bool damping = true; bool gyro = false; @@ -269,18 +270,18 @@ void GraspDeformable::initPhysics() } // create a soft block - if(0) + if(1) { char relative_path[1024]; // b3FileUtils::findFile("banana.vtk", relative_path, 1024); // b3FileUtils::findFile("ball.vtk", relative_path, 1024); -// b3FileUtils::findFile("paper_collision.vtk", relative_path, 1024); +// b3FileUtils::findFile("deformable_crumpled_napkin_sim.vtk", relative_path, 1024); // b3FileUtils::findFile("single_tet.vtk", relative_path, 1024); - b3FileUtils::findFile("tube.vtk", relative_path, 1024); +// b3FileUtils::findFile("tube.vtk", relative_path, 1024); // b3FileUtils::findFile("torus.vtk", relative_path, 1024); // b3FileUtils::findFile("paper_roll.vtk", relative_path, 1024); // b3FileUtils::findFile("bread.vtk", relative_path, 1024); -// b3FileUtils::findFile("ditto.vtk", relative_path, 1024); + b3FileUtils::findFile("ditto.vtk", relative_path, 1024); // b3FileUtils::findFile("boot.vtk", relative_path, 1024); // btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(), // TetraCube::getElements(), @@ -290,12 +291,13 @@ void GraspDeformable::initPhysics() btSoftBody* psb = btSoftBodyHelpers::CreateFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), relative_path); // psb->scale(btVector3(30, 30, 30)); // for banana -// psb->scale(btVector3(.2, .2, .2)); + psb->scale(btVector3(.7, .7, .7)); // psb->scale(btVector3(2, 2, 2)); - 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); +// 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); psb->setTotalMass(.01); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body @@ -307,20 +309,22 @@ void GraspDeformable::initPhysics() getDeformableDynamicsWorld()->addForce(psb, gravity_force); m_forces.push_back(gravity_force); - btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(100,200, 0.05); + btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(8,32, .05); getDeformableDynamicsWorld()->addForce(psb, neohookean); m_forces.push_back(neohookean); } getDeformableDynamicsWorld()->setImplicit(false); // create a piece of cloth + if(0) { 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), + const btScalar s = .1; + const btScalar h = 1; + btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s), + btVector3(+s, h, -s), + btVector3(-s, h, +s), + btVector3(+s, h, +s), 20,20, 0, true); @@ -344,7 +348,8 @@ void GraspDeformable::initPhysics() 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 btDeformableMassSpringForce(.0,0.0, true)); + getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(1,0.05, false)); getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); } diff --git a/src/BulletSoftBody/btDeformableNeoHookeanForce.h b/src/BulletSoftBody/btDeformableNeoHookeanForce.h index 589abf2e3..26f738fda 100644 --- a/src/BulletSoftBody/btDeformableNeoHookeanForce.h +++ b/src/BulletSoftBody/btDeformableNeoHookeanForce.h @@ -33,7 +33,7 @@ public: m_lambda_damp = damping * m_lambda; } - btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0): m_mu(mu), m_lambda(lambda) + btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.05): m_mu(mu), m_lambda(lambda) { m_mu_damp = damping * m_mu; m_lambda_damp = damping * m_lambda; @@ -73,12 +73,10 @@ public: size_t id2 = node2->index; size_t id3 = node3->index; btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse; - btMatrix3x3 C = dF + dF.transpose(); - btMatrix3x3 dP; -// firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); btMatrix3x3 I; I.setIdentity(); - dP = C * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp; + btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp; +// firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); 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(); @@ -157,13 +155,17 @@ public: virtual void addScaledElasticForce(btScalar scale, TVStack& force) { -// btScalar max_p = 0; int numNodes = getNumNodes(); btAssert(numNodes <= force.size()); btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + btScalar max_p = psb->m_cfg.m_maxStress; for (int j = 0; j < psb->m_tetras.size(); ++j) { btSoftBody::Tetra& tetra = psb->m_tetras[j]; @@ -173,47 +175,21 @@ public: btMatrix3x3 U, V; btVector3 sigma; singularValueDecomposition(P, U, sigma, V); - sigma[0] = btMin(sigma[0], (btScalar)1000); - sigma[1] = btMin(sigma[1], (btScalar)1000); - sigma[2] = btMin(sigma[2], (btScalar)1000); - sigma[0] = btMax(sigma[0], (btScalar)-1000); - sigma[1] = btMax(sigma[1], (btScalar)-1000); - sigma[2] = btMax(sigma[2], (btScalar)-1000); + if (max_p > 0) + { + sigma[0] = btMin(sigma[0], max_p); + sigma[1] = btMin(sigma[1], max_p); + sigma[2] = btMin(sigma[2], max_p); + sigma[0] = btMax(sigma[0], -max_p); + sigma[1] = btMax(sigma[1], -max_p); + sigma[2] = btMax(sigma[2], -max_p); + } btMatrix3x3 Sigma; Sigma.setIdentity(); Sigma[0][0] = sigma[0]; Sigma[1][1] = sigma[1]; Sigma[2][2] = sigma[2]; -// max_p = btMax(max_p, sigma[0]); -// max_p = btMax(max_p, sigma[1]); -// max_p = btMax(max_p, sigma[2]); P = U * Sigma * V.transpose(); - -// Eigen::Matrix eigenP; -// eigenP << P[0][0], P[0][1], P[0][2], -// P[1][0],P[1][1],P[1][2], -// P[2][0],P[2][1],P[2][2]; -// Eigen::JacobiSVD > svd(eigenP, Eigen::ComputeFullU | Eigen::ComputeFullV); -// Eigen::Matrix P_hat = svd.singularValues().asDiagonal(); -// P_hat(0,0) = btMin((btScalar)P_hat(0,0), (btScalar)20); -// P_hat(1,1) = btMin((btScalar)P_hat(1,1), (btScalar)20); -// P_hat(2,2) = btMin((btScalar)P_hat(2,2), (btScalar)20); -// eigenP = svd.matrixU() * P_hat * svd.matrixV().transpose(); -// max_p = btMax(max_p, (btScalar)P_hat(0,0)); -// max_p = btMax(max_p, (btScalar)P_hat(1,1)); -// max_p = btMax(max_p, (btScalar)P_hat(2,2)); -// -// P[0][0] = eigenP(0,0); -// P[0][1] = eigenP(0,1); -// P[0][2] = eigenP(0,2); -// P[1][0] = eigenP(1,0); -// P[1][1] = eigenP(1,1); -// P[1][2] = eigenP(1,2); -// P[2][0] = eigenP(2,0); -// P[2][1] = eigenP(2,1); -// P[2][2] = eigenP(2,2); - - // 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; @@ -235,7 +211,6 @@ public: force[id3] -= scale1 * force_on_node123.getColumn(2); } } -// std::cout << max_p << std::endl; } // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search @@ -261,12 +236,10 @@ public: size_t id2 = node2->index; size_t id3 = node3->index; btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse; - btMatrix3x3 C = dF + dF.transpose(); - btMatrix3x3 dP; -// firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); btMatrix3x3 I; I.setIdentity(); - dP = C * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp; + btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0]+dF[1][1]+dF[2][2]) * m_lambda_damp; +// firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); // 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; diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index c3d99590a..253de645d 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -90,6 +90,7 @@ void btSoftBody::initDefaults() m_cfg.diterations = 0; m_cfg.citerations = 4; m_cfg.drag = 0; + m_cfg.m_maxStress = 0; m_cfg.collisions = fCollision::Default; m_cfg.collisions |= fCollision::VF_DD; m_pose.m_bvolume = false; @@ -3195,6 +3196,12 @@ void btSoftBody::applyForces() } } +// +void btSoftBody::setMaxStress(btScalar maxStress) +{ + m_cfg.m_maxStress = maxStress; +} + // void btSoftBody::interpolateRenderMesh() { diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 31ddfabb9..4f4f6f22b 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -703,6 +703,7 @@ public: tPSolverArray m_psequence; // Position solvers sequence tPSolverArray m_dsequence; // Drift solvers sequence btScalar drag; // deformable air drag + btScalar m_maxStress; // Maximum principle first Piola stress }; /* SolverState */ struct SolverState @@ -1107,6 +1108,7 @@ public: void updateDeformation(); void advanceDeformation(); void applyForces(); + void setMaxStress(btScalar maxStress); void interpolateRenderMesh(); static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti); static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti); From 30238b2fbcb7eacbd06b1cebe35536ef347d41f4 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Sat, 12 Oct 2019 19:19:11 -0700 Subject: [PATCH 22/29] remove std::copysign --- examples/DeformableDemo/GraspDeformable.cpp | 2 +- src/LinearMath/btImplicitQRSVD.h | 11 +++++++---- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index 98fb63a8d..ddb47954b 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -340,7 +340,7 @@ void GraspDeformable::initPhysics() psb->getCollisionShape()->setMargin(0.005); psb->generateBendingConstraints(2); psb->setTotalMass(.01); - psb->setSpringStiffness(5); + psb->setSpringStiffness(10); psb->setDampingCoefficient(0.05); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body diff --git a/src/LinearMath/btImplicitQRSVD.h b/src/LinearMath/btImplicitQRSVD.h index 0785a6259..6af525993 100644 --- a/src/LinearMath/btImplicitQRSVD.h +++ b/src/LinearMath/btImplicitQRSVD.h @@ -87,6 +87,12 @@ public: } }; +static inline btScalar copySign(btScalar x, btScalar y) { + if ((x < 0 && y > 0) || (x > 0 && y < 0)) + return -x; + return x; +} + /** Class for givens rotation. Row rotation G*A corresponds to something like @@ -550,14 +556,11 @@ inline void singularValueDecomposition( */ inline btScalar wilkinsonShift(const btScalar a1, const btScalar b1, const btScalar a2) { - using std::sqrt; using std::fabs; - using std::copysign; btScalar d = (btScalar)0.5 * (a1 - a2); btScalar bs = b1 * b1; - - btScalar mu = a2 - copysign(bs / (fabs(d) + sqrt(d * d + bs)), d); + btScalar mu = a2 - copysign(bs / (btFabs(d) + btSqrt(d * d + bs)), d); // T mu = a2 - bs / ( d + sign_d*sqrt (d*d + bs)); return mu; } From a1afc66817818f4b2650df3379e43b9d447f902d Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 14 Oct 2019 09:16:11 -0700 Subject: [PATCH 23/29] remove std::fabs and std::max --- src/LinearMath/btImplicitQRSVD.h | 30 ++++++++++++------------------ 1 file changed, 12 insertions(+), 18 deletions(-) diff --git a/src/LinearMath/btImplicitQRSVD.h b/src/LinearMath/btImplicitQRSVD.h index 6af525993..cf96d4f72 100644 --- a/src/LinearMath/btImplicitQRSVD.h +++ b/src/LinearMath/btImplicitQRSVD.h @@ -556,8 +556,6 @@ inline void singularValueDecomposition( */ inline btScalar wilkinsonShift(const btScalar a1, const btScalar b1, const btScalar a2) { - using std::fabs; - btScalar d = (btScalar)0.5 * (a1 - a2); btScalar bs = b1 * b1; btScalar mu = a2 - copysign(bs / (btFabs(d) + btSqrt(d * d + bs)), d); @@ -647,12 +645,10 @@ inline void swapCol(btMatrix3x3& A, int i, int j) */ inline void sort(btMatrix3x3& U, btVector3& sigma, btMatrix3x3& V, int t) { - using std::fabs; - if (t == 0) { // Case: sigma(0) > |sigma(1)| >= |sigma(2)| - if (fabs(sigma[1]) >= fabs(sigma[2])) { + if (btFabs(sigma[1]) >= btFabs(sigma[2])) { if (sigma[1] < 0) { flipSign(1, U, sigma); flipSign(2, U, sigma); @@ -688,7 +684,7 @@ inline void sort(btMatrix3x3& U, btVector3& sigma, btMatrix3x3& V, int t) else if (t == 1) { // Case: |sigma(0)| >= sigma(1) > |sigma(2)| - if (fabs(sigma[0]) >= sigma[1]) { + if (btFabs(sigma[0]) >= sigma[1]) { if (sigma[0] < 0) { flipSign(0, U, sigma); flipSign(2, U, sigma); @@ -702,7 +698,7 @@ inline void sort(btMatrix3x3& U, btVector3& sigma, btMatrix3x3& V, int t) swapCol(V, 0, 1); // Case: sigma(1) > |sigma(2)| >= |sigma(0)| - if (fabs(sigma[1]) < fabs(sigma[2])) { + if (btFabs(sigma[1]) < btFabs(sigma[2])) { std::swap(sigma[1], sigma[2]); swapCol(U, 1, 2); swapCol(V, 1, 2); @@ -736,8 +732,6 @@ inline int singularValueDecomposition(const btMatrix3x3& A, btScalar tol = 128*std::numeric_limits::epsilon()) { using std::fabs; - using std::sqrt; - using std::max; btMatrix3x3 B = A; U.setIdentity(); V.setIdentity(); @@ -755,15 +749,15 @@ inline int singularValueDecomposition(const btMatrix3x3& A, btScalar beta_2 = B[1][2]; btScalar gamma_1 = alpha_1 * beta_1; btScalar gamma_2 = alpha_2 * beta_2; - tol *= max((btScalar)0.5 * sqrt(alpha_1 * alpha_1 + alpha_2 * alpha_2 + alpha_3 * alpha_3 + beta_1 * beta_1 + beta_2 * beta_2), (btScalar)1); + tol *= btMax((btScalar)0.5 * btSqrt(alpha_1 * alpha_1 + alpha_2 * alpha_2 + alpha_3 * alpha_3 + beta_1 * beta_1 + beta_2 * beta_2), (btScalar)1); /** Do implicit shift QR until A^T A is block diagonal */ - while (fabs(beta_2) > tol && fabs(beta_1) > tol - && fabs(alpha_1) > tol && fabs(alpha_2) > tol - && fabs(alpha_3) > tol) { + while (btFabs(beta_2) > tol && btFabs(beta_1) > tol + && btFabs(alpha_1) > tol && btFabs(alpha_2) > tol + && btFabs(alpha_3) > tol) { mu = wilkinsonShift(alpha_2 * alpha_2 + beta_1 * beta_1, gamma_2, alpha_3 * alpha_3 + beta_2 * beta_2); r.compute(alpha_1 * alpha_1 - mu, gamma_1); @@ -791,7 +785,7 @@ inline int singularValueDecomposition(const btMatrix3x3& A, 0 x 0 0 0 x */ - if (fabs(beta_2) <= tol) { + if (btFabs(beta_2) <= tol) { process<0>(B, U, sigma, V); sort(U, sigma, V,0); } @@ -801,7 +795,7 @@ inline int singularValueDecomposition(const btMatrix3x3& A, 0 x x 0 0 x */ - else if (fabs(beta_1) <= tol) { + else if (btFabs(beta_1) <= tol) { process<1>(B, U, sigma, V); sort(U, sigma, V,1); } @@ -811,7 +805,7 @@ inline int singularValueDecomposition(const btMatrix3x3& A, 0 0 x 0 0 x */ - else if (fabs(alpha_2) <= tol) { + else if (btFabs(alpha_2) <= tol) { /** Reduce B to x x 0 @@ -832,7 +826,7 @@ inline int singularValueDecomposition(const btMatrix3x3& A, 0 x x 0 0 0 */ - else if (fabs(alpha_3) <= tol) { + else if (btFabs(alpha_3) <= tol) { /** Reduce B to x x + @@ -863,7 +857,7 @@ inline int singularValueDecomposition(const btMatrix3x3& A, 0 x x 0 0 x */ - else if (fabs(alpha_1) <= tol) { + else if (btFabs(alpha_1) <= tol) { /** Reduce B to 0 0 + From 05c25a27ded6502957a46ab5ed17dddd1d2894cb Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 14 Oct 2019 15:21:42 -0700 Subject: [PATCH 24/29] address comment from ldowns --- src/BulletSoftBody/btConjugateGradient.h | 9 ++++----- src/BulletSoftBody/btDeformableBodySolver.cpp | 14 ++++++++++---- 2 files changed, 14 insertions(+), 9 deletions(-) diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h index c1816600e..bd51e584b 100644 --- a/src/BulletSoftBody/btConjugateGradient.h +++ b/src/BulletSoftBody/btConjugateGradient.h @@ -27,12 +27,12 @@ class btConjugateGradient typedef btAlignedObjectArray TVStack; TVStack r,p,z,temp; int max_iterations; - btScalar tolerance; + btScalar tolerance_squared; public: btConjugateGradient(const int max_it_in) : max_iterations(max_it_in) { - tolerance = 1e-5; + tolerance_squared = 1e-5; } virtual ~btConjugateGradient(){} @@ -51,8 +51,7 @@ public: A.precondition(r, z); A.project(z); btScalar r_dot_z = dot(z,r); - btScalar local_tolerance = tolerance; - if (r_dot_z <= local_tolerance) { + if (r_dot_z <= tolerance_squared) { if (verbose) { std::cout << "Iteration = 0" << std::endl; @@ -86,7 +85,7 @@ public: A.precondition(r, z); r_dot_z = r_dot_z_new; r_dot_z_new = dot(r,z); - if (r_dot_z_new < local_tolerance) { + if (r_dot_z_new < tolerance_squared) { if (verbose) { std::cout << "ConjugateGradient iterations " << k << std::endl; diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 676df528d..01a71aa3e 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -18,13 +18,13 @@ #include "btDeformableBodySolver.h" #include "btSoftBodyInternals.h" #include "LinearMath/btQuickprof.h" - +static const int kMaxConjugateGradientIterations = 200; btDeformableBodySolver::btDeformableBodySolver() : m_numNodes(0) -, m_cg(200) +, m_cg(kMaxConjugateGradientIterations) , m_maxNewtonIterations(5) , m_newtonTolerance(1e-4) -, m_lineSearch(true) +, m_lineSearch(false) { m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity); } @@ -101,6 +101,7 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt) m_residual[j].setZero(); } } + updateVelocity(); } } @@ -298,9 +299,14 @@ void btDeformableBodySolver::setupDeformableSolve(bool implicit) { if (implicit) { + if ((psb->m_nodes[j].m_v - m_backupVelocity[counter]).norm() < SIMD_EPSILON) + m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter]; + else + m_dv[counter] = psb->m_nodes[j].m_v - psb->m_nodes[j].m_vn; m_backupVelocity[counter] = psb->m_nodes[j].m_vn; } - m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter]; + else + m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter]; psb->m_nodes[j].m_v = m_backupVelocity[counter]; ++counter; } From 774937bcd66edb455c147c4c7eb77fbfbdcc48b6 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 14 Oct 2019 15:21:58 -0700 Subject: [PATCH 25/29] add option to turn face contact on/off --- .../btDeformableMultiBodyConstraintSolver.cpp | 1 - .../btDeformableMultiBodyDynamicsWorld.cpp | 10 ++-------- src/BulletSoftBody/btSoftBody.cpp | 20 ++++++++++++------- src/BulletSoftBody/btSoftBody.h | 8 ++++++++ 4 files changed, 23 insertions(+), 16 deletions(-) diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index 2567cecab..1e0d53a41 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -23,7 +23,6 @@ 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 = 10; int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; for (int iteration = 0; iteration < maxIterations; iteration++) { diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 73e534d09..9c21bf9b0 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -88,8 +88,6 @@ void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision() void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) { BT_PROFILE("integrateTransforms"); - //m_deformableBodySolver->backupVelocity(); - //positionCorrection(timeStep); // looks like position correction is no longer necessary btMultiBodyDynamicsWorld::integrateTransforms(timeStep); for (int i = 0; i < m_softBodies.size(); ++i) { @@ -116,16 +114,12 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) } psb->interpolateRenderMesh(); } - //m_deformableBodySolver->revertVelocity(); } void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) { - if (!m_implicit) - { - // save v_{n+1}^* velocity after explicit forces - m_deformableBodySolver->backupVelocity(); - } + // save v_{n+1}^* velocity after explicit forces + m_deformableBodySolver->backupVelocity(); // set up constraints among multibodies and between multibodies and deformable bodies setupConstraints(); diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 253de645d..843c60e29 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -117,6 +117,9 @@ void btSoftBody::initDefaults() m_windVelocity = btVector3(0, 0, 0); m_restLengthScale = btScalar(1.0); m_dampingCoefficient = 1; + m_sleepingThreshold = 0.01; + m_useFaceContact = false; + m_collisionFlags = 0; } // @@ -3494,13 +3497,16 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap docollideNode.stamargin = basemargin; m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode); - btSoftColliders::CollideSDF_RDF docollideFace; - docollideFace.psb = this; - docollideFace.m_colObj1Wrap = pcoWrap; - docollideFace.m_rigidBody = prb1; - docollideFace.dynmargin = basemargin + timemargin; - docollideFace.stamargin = basemargin; - m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace); + if (this->m_useFaceContact) + { + btSoftColliders::CollideSDF_RDF docollideFace; + docollideFace.psb = this; + docollideFace.m_colObj1Wrap = pcoWrap; + docollideFace.m_rigidBody = prb1; + docollideFace.dynmargin = basemargin + timemargin; + docollideFace.stamargin = basemargin; + m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace); + } } } break; diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 4f4f6f22b..970290962 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -789,6 +789,9 @@ public: btDbvt m_cdbvt; // Clusters tree tClusterArray m_clusters; // Clusters btScalar m_dampingCoefficient; // Damping Coefficient + btScalar m_sleepingThreshold; + btScalar m_maxSpeedSquared; + bool m_useFaceContact; btAlignedObjectArray m_renderNodesInterpolationWeights; btAlignedObjectArray > m_renderNodesParents; @@ -829,6 +832,11 @@ public: { m_dampingCoefficient = damping_coeff; } + + void setUseFaceContact(bool useFaceContact) + { + m_useFaceContact = false; + } ///@todo: avoid internal softbody shape hack and move collision code to collision library virtual void setCollisionShape(btCollisionShape* collisionShape) From 3ae193ff1533cb58df199edc810e6357d6eb03fe Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Tue, 15 Oct 2019 15:43:26 -0700 Subject: [PATCH 26/29] bool->int --- examples/SharedMemory/PhysicsClientC_API.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 914e47037..085a0384b 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -386,7 +386,7 @@ B3_SHARED_API int b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle return 0; } -B3_SHARED_API int b3LoadSoftBodySetSelfCollision(b3SharedMemoryCommandHandle commandHandle, bool useSelfCollision) +B3_SHARED_API int b3LoadSoftBodySetSelfCollision(b3SharedMemoryCommandHandle commandHandle, int useSelfCollision) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; b3Assert(command->m_type == CMD_LOAD_SOFT_BODY); From 3d622a3bee6d88044eeda7cf874406a4268563d9 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 16 Oct 2019 12:00:02 -0700 Subject: [PATCH 27/29] enable deformable sleeping --- src/BulletSoftBody/btDeformableBodySolver.cpp | 16 ++- .../btDeformableContactProjection.cpp | 15 ++- src/BulletSoftBody/btDeformableGravityForce.h | 4 + .../btDeformableMultiBodyDynamicsWorld.cpp | 34 ++++- .../btDeformableMultiBodyDynamicsWorld.h | 2 + .../btDeformableNeoHookeanForce.h | 12 ++ src/BulletSoftBody/btSoftBody.cpp | 122 ++++++++++++------ src/BulletSoftBody/btSoftBody.h | 7 +- src/BulletSoftBody/btSoftBodyInternals.h | 4 - 9 files changed, 162 insertions(+), 54 deletions(-) diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 01a71aa3e..9af8e3f77 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -200,7 +200,7 @@ void btDeformableBodySolver::updateDv(btScalar scale) void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual) { - m_cg.solve(*m_objective, ddv, residual, false); + m_cg.solve(*m_objective, ddv, residual); } void btDeformableBodySolver::reinitialize(const btAlignedObjectArray& softBodies, btScalar dt) @@ -248,6 +248,11 @@ void btDeformableBodySolver::updateVelocity() for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + psb->m_maxSpeedSquared = 0; + if (!psb->isActive()) + { + continue; + } for (int j = 0; j < psb->m_nodes.size(); ++j) { // set NaN to zero; @@ -256,6 +261,7 @@ void btDeformableBodySolver::updateVelocity() m_dv[counter].setZero(); } psb->m_nodes[j].m_v = m_backupVelocity[counter]+m_dv[counter]; + psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2()); ++counter; } } @@ -267,6 +273,10 @@ void btDeformableBodySolver::updateTempPosition() for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } for (int j = 0; j < psb->m_nodes.size(); ++j) { psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * psb->m_nodes[j].m_v; @@ -295,6 +305,10 @@ void btDeformableBodySolver::setupDeformableSolve(bool implicit) for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } for (int j = 0; j < psb->m_nodes.size(); ++j) { if (implicit) diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index e340a3fd5..cf0a5e896 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -51,6 +51,10 @@ void btDeformableContactProjection::setConstraints() for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } for (int j = 0; j < psb->m_nodes.size(); ++j) { if (psb->m_nodes[j].m_im == 0) @@ -61,10 +65,15 @@ void btDeformableContactProjection::setConstraints() } } - // set Deformable Node vs. Rigid constraint for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + + // set Deformable Node vs. Rigid constraint for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j) { const btSoftBody::DeformableNodeRigidContact& contact = psb->m_nodeRigidContacts[j]; @@ -242,6 +251,10 @@ void btDeformableContactProjection::setProjection() for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } for (int j = 0; j < psb->m_nodes.size(); ++j) { int index = psb->m_nodes[j].index; diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h index abc120252..33e5a8564 100644 --- a/src/BulletSoftBody/btDeformableGravityForce.h +++ b/src/BulletSoftBody/btDeformableGravityForce.h @@ -57,6 +57,10 @@ public: for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } for (int j = 0; j < psb->m_nodes.size(); ++j) { btSoftBody::Node& n = psb->m_nodes[j]; diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 9c21bf9b0..ded9893ce 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -70,18 +70,46 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t ///update vehicle simulation btMultiBodyDynamicsWorld::updateActions(timeStep); - btMultiBodyDynamicsWorld::updateActivationState(timeStep); + updateActivationState(timeStep); // End solver-wise simulation step // /////////////////////////////// } +void btDeformableMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep) +{ + for (int i = 0; i < m_softBodies.size(); i++) + { + btSoftBody* psb = m_softBodies[i]; + psb->updateDeactivation(timeStep); + if (psb->wantsSleeping()) + { + if (psb->getActivationState() == ACTIVE_TAG) + psb->setActivationState(WANTS_DEACTIVATION); + if (psb->getActivationState() == ISLAND_SLEEPING) + { + psb->setZeroVelocity(); + } + } + else + { + if (psb->getActivationState() != DISABLE_DEACTIVATION) + psb->setActivationState(ACTIVE_TAG); + } + } + btMultiBodyDynamicsWorld::updateActivationState(timeStep); +} + + void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision() { m_deformableBodySolver->updateSoftBodies(); for (int i = 0; i < m_softBodies.size(); i++) { - btSoftBody* psb = (btSoftBody*)m_softBodies[i]; - psb->defaultCollisionHandler(psb); + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { + psb->defaultCollisionHandler(psb); + } } } diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index 72cfa4fc5..834c6c8b1 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -62,6 +62,8 @@ protected: void solveConstraints(btScalar timeStep); + void updateActivationState(btScalar timeStep); + public: btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0) : btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration), diff --git a/src/BulletSoftBody/btDeformableNeoHookeanForce.h b/src/BulletSoftBody/btDeformableNeoHookeanForce.h index 26f738fda..b113b4546 100644 --- a/src/BulletSoftBody/btDeformableNeoHookeanForce.h +++ b/src/BulletSoftBody/btDeformableNeoHookeanForce.h @@ -61,6 +61,10 @@ public: for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } for (int j = 0; j < psb->m_tetras.size(); ++j) { btSoftBody::Tetra& tetra = psb->m_tetras[j]; @@ -224,6 +228,10 @@ public: for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } for (int j = 0; j < psb->m_tetras.size(); ++j) { btSoftBody::Tetra& tetra = psb->m_tetras[j]; @@ -262,6 +270,10 @@ public: for (int i = 0; i < m_softBodies.size(); ++i) { btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } for (int j = 0; j < psb->m_tetras.size(); ++j) { btSoftBody::Tetra& tetra = psb->m_tetras[j]; diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 843c60e29..b05b28ca4 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -117,7 +117,7 @@ void btSoftBody::initDefaults() m_windVelocity = btVector3(0, 0, 0); m_restLengthScale = btScalar(1.0); m_dampingCoefficient = 1; - m_sleepingThreshold = 0.01; + m_sleepingThreshold = 0.1; m_useFaceContact = false; m_collisionFlags = 0; } @@ -3601,51 +3601,43 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb) break; case fCollision::VF_DD: { - if (this != psb) + if (psb->isActive() || this->isActive()) { - btSoftColliders::CollideVF_DD docollide; - /* common */ - docollide.mrg = getCollisionShape()->getMargin() + - psb->getCollisionShape()->getMargin(); - /* psb0 nodes vs psb1 faces */ - docollide.psb[0] = this; - docollide.psb[1] = psb; - docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); - /* psb1 nodes vs psb0 faces */ - docollide.psb[0] = psb; - docollide.psb[1] = this; - docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); - } - else - { - if (psb->useSelfCollision()) + if (this != psb) { - btSoftColliders::CollideFF_DD docollide; - docollide.mrg = getCollisionShape()->getMargin() + - psb->getCollisionShape()->getMargin(); - docollide.psb[0] = this; - docollide.psb[1] = psb; - /* psb0 faces vs psb0 faces */ - btDbvntNode* root = copyToDbvnt(this->m_fdbvt.m_root); - calculateNormalCone(root); - this->m_fdbvt.selfCollideT(root,docollide); - delete root; + btSoftColliders::CollideVF_DD docollide; + /* common */ + docollide.mrg = getCollisionShape()->getMargin() + + psb->getCollisionShape()->getMargin(); + /* psb0 nodes vs psb1 faces */ + docollide.psb[0] = this; + docollide.psb[1] = psb; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + /* psb1 nodes vs psb0 faces */ + docollide.psb[0] = psb; + docollide.psb[1] = this; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + } + else + { + if (psb->useSelfCollision()) + { + btSoftColliders::CollideFF_DD docollide; + docollide.mrg = getCollisionShape()->getMargin() + + psb->getCollisionShape()->getMargin(); + docollide.psb[0] = this; + docollide.psb[1] = psb; + /* psb0 faces vs psb0 faces */ + btDbvntNode* root = copyToDbvnt(this->m_fdbvt.m_root); + calculateNormalCone(root); + this->m_fdbvt.selfCollideT(root,docollide); + delete root; + } } - -// btSoftColliders::CollideFF_DD docollide; -// /* common */ -// docollide.mrg = getCollisionShape()->getMargin() + -// psb->getCollisionShape()->getMargin(); -// /* psb0 nodes vs psb1 faces */ -// docollide.psb[0] = this; -// docollide.psb[1] = psb; -// docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_fdbvt.m_root, -// docollide.psb[1]->m_fdbvt.m_root, -// docollide); } } break; @@ -4051,3 +4043,47 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ return btSoftBodyDataName; } + +void btSoftBody::updateDeactivation(btScalar timeStep) +{ + if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION)) + return; + + if (m_maxSpeedSquared < m_sleepingThreshold * m_sleepingThreshold) + { + m_deactivationTime += timeStep; + } + else + { + m_deactivationTime = btScalar(0.); + setActivationState(0); + } +} + + +void btSoftBody::setZeroVelocity() +{ + for (int i = 0; i < m_nodes.size(); ++i) + { + m_nodes[i].m_v.setZero(); + } +} + +bool btSoftBody::wantsSleeping() +{ + if (getActivationState() == DISABLE_DEACTIVATION) + return false; + + //disable deactivation + if (gDisableDeactivation || (gDeactivationTime == btScalar(0.))) + return false; + + if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION)) + return true; + + if (m_deactivationTime > gDeactivationTime) + { + return true; + } + return false; +} diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 970290962..195434c06 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -1017,8 +1017,11 @@ public: /* defaultCollisionHandlers */ void defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap); void defaultCollisionHandler(btSoftBody* psb); - void setSelfCollision(bool useSelfCollision); - bool useSelfCollision(); + void setSelfCollision(bool useSelfCollision); + bool useSelfCollision(); + void updateDeactivation(btScalar timeStep); + void setZeroVelocity(); + bool wantsSleeping(); // // Functionality to deal with new accelerated solvers. diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index afe9c7ea8..6e20b3222 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -1097,8 +1097,6 @@ struct btSoftColliders c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); c.m_c1 = ra; - if (m_rigidBody) - m_rigidBody->activate(); } else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) { @@ -1200,8 +1198,6 @@ struct btSoftColliders // we do not scale the impulse matrix by dt c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); c.m_c1 = ra; - if (m_rigidBody) - m_rigidBody->activate(); } else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) { From 60dfe1fe697c92f95d7f18a3df35ad372a6cac73 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 16 Oct 2019 19:23:01 -0700 Subject: [PATCH 28/29] add support for anchor constraint between deformable and rigid --- .../DeformableDemo/DeformableClothAnchor.cpp | 236 ++++++++++++++++++ .../DeformableDemo/DeformableClothAnchor.h | 19 ++ examples/ExampleBrowser/CMakeLists.txt | 2 + examples/ExampleBrowser/ExampleEntries.cpp | 2 + .../btDeformableContactConstraint.cpp | 121 +++++++++ .../btDeformableContactConstraint.h | 26 ++ .../btDeformableContactProjection.cpp | 29 ++- .../btDeformableContactProjection.h | 2 + .../btDeformableMultiBodyDynamicsWorld.cpp | 7 + src/BulletSoftBody/btSoftBody.cpp | 35 +++ src/BulletSoftBody/btSoftBody.h | 10 +- 11 files changed, 487 insertions(+), 2 deletions(-) create mode 100644 examples/DeformableDemo/DeformableClothAnchor.cpp create mode 100644 examples/DeformableDemo/DeformableClothAnchor.h diff --git a/examples/DeformableDemo/DeformableClothAnchor.cpp b/examples/DeformableDemo/DeformableClothAnchor.cpp new file mode 100644 index 000000000..49fd3184b --- /dev/null +++ b/examples/DeformableDemo/DeformableClothAnchor.cpp @@ -0,0 +1,236 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ +#include "DeformableClothAnchor.h" +///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. +#include "btBulletDynamicsCommon.h" +#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h" +#include "BulletSoftBody/btSoftBody.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" +#include "BulletSoftBody/btDeformableBodySolver.h" +#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include //printf debugging + +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../Utils/b3ResourcePath.h" + +///The DeformableClothAnchor shows contact between deformable objects and rigid objects. +class DeformableClothAnchor : public CommonRigidBodyBase +{ + btAlignedObjectArray m_forces; +public: + DeformableClothAnchor(struct GUIHelperInterface* helper) + : CommonRigidBodyBase(helper) + { + } + virtual ~DeformableClothAnchor() + { + } + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 20; + float pitch = -45; + float yaw = 100; + float targetPos[3] = {0, -3, 0}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + void stepSimulation(float deltaTime) + { + //use a smaller internal timestep, there are stability issues + float internalTimeStep = 1. / 240.f; + m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); + } + + virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const + { + ///just make it a btSoftRigidDynamicsWorld please + ///or we will add type checking + return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; + } + + virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() + { + ///just make it a btSoftRigidDynamicsWorld please + ///or we will add type checking + return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; + } + + virtual void renderScene() + { + CommonRigidBodyBase::renderScene(); + btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); + + for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) + { + btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i]; + //if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) + { + btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); + } + } + } +}; + +void DeformableClothAnchor::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); + + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + m_broadphase = new btDbvtBroadphase(); + btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); + + ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) + btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); + sol->setDeformableSolver(deformableBodySolver); + m_solver = sol; + + m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); + // deformableBodySolver->setWorld(getDeformableDynamicsWorld()); + // m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality + btVector3 gravity = btVector3(0, -10, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; + + // getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + { + ///create a ground + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.))); + + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -50, 0)); + groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.)); + //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: + btScalar mass(0.); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0, 0, 0); + if (isDynamic) + groundShape->calculateLocalInertia(mass, localInertia); + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + body->setFriction(1); + + //add the ground to the dynamics world + m_dynamicsWorld->addRigidBody(body); + } + + // create a piece of cloth + { + const btScalar s = 4; + const btScalar h = 6; + const int r = 9; + btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s), + btVector3(+s, h, -s), + btVector3(-s, h, +s), + btVector3(+s, h, +s), r, r, 4 + 8, true); + psb->getCollisionShape()->setMargin(0.1); + psb->generateBendingConstraints(2); + 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 = 2; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + getDeformableDynamicsWorld()->addSoftBody(psb); + + btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(100,1, true); + 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); + + btTransform startTransform; + startTransform.setIdentity(); + startTransform.setOrigin(btVector3(0, h, -(s + 3.5))); + btRigidBody* body = createRigidBody(2, startTransform, new btBoxShape(btVector3(s, 1, 3))); + psb->appendDeformableAnchor(0, body); + psb->appendDeformableAnchor(r - 1, body); + } + getDeformableDynamicsWorld()->setImplicit(false); + getDeformableDynamicsWorld()->setLineSearch(false); + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void DeformableClothAnchor::exitPhysics() +{ + //cleanup in the reverse order of creation/initialization + + //remove the rigidbodies from the dynamics world and delete them + int i; + for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) + { + btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()) + { + delete body->getMotionState(); + } + m_dynamicsWorld->removeCollisionObject(obj); + delete obj; + } + // delete forces + for (int j = 0; j < m_forces.size(); j++) + { + btDeformableLagrangianForce* force = m_forces[j]; + delete force; + } + m_forces.clear(); + //delete collision shapes + for (int j = 0; j < m_collisionShapes.size(); j++) + { + btCollisionShape* shape = m_collisionShapes[j]; + delete shape; + } + m_collisionShapes.clear(); + + delete m_dynamicsWorld; + + delete m_solver; + + delete m_broadphase; + + delete m_dispatcher; + + delete m_collisionConfiguration; +} + + + +class CommonExampleInterface* DeformableClothAnchorCreateFunc(struct CommonExampleOptions& options) +{ + return new DeformableClothAnchor(options.m_guiHelper); +} + + diff --git a/examples/DeformableDemo/DeformableClothAnchor.h b/examples/DeformableDemo/DeformableClothAnchor.h new file mode 100644 index 000000000..4e8388647 --- /dev/null +++ b/examples/DeformableDemo/DeformableClothAnchor.h @@ -0,0 +1,19 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef DEFORMABLE_CLOTH_ANCHOR_H +#define DEFORMABLE_CLOTH_ANCHOR_H + +class CommonExampleInterface* DeformableClothAnchorCreateFunc(struct CommonExampleOptions& options); + +#endif //DEFORMABLE_CLOTH_ANCHOR_H diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 7c31c9d84..56aaf625b 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -371,6 +371,8 @@ SET(BulletExampleBrowser_SRCS ../DeformableDemo/DeformableRigid.h ../DeformableDemo/VolumetricDeformable.cpp ../DeformableDemo/VolumetricDeformable.h + ../DeformableDemo/DeformableClothAnchor.cpp + ../DeformableDemo/DeformableClothAnchor.h ../MultiBody/MultiDofDemo.cpp ../MultiBody/MultiDofDemo.h ../RigidBody/RigidBodySoftContact.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index e705fd075..887fb2fc5 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -53,6 +53,7 @@ #include "../DeformableDemo/VolumetricDeformable.h" #include "../DeformableDemo/GraspDeformable.h" #include "../DeformableDemo/DeformableContact.h" +#include "../DeformableDemo/DeformableClothAnchor.h" #include "../SharedMemory/PhysicsServerExampleBullet2.h" #include "../SharedMemory/PhysicsServerExample.h" #include "../SharedMemory/PhysicsClientExample.h" @@ -198,6 +199,7 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(1, "Grasp Deformable Cube", "Grasping test", PinchCreateFunc), ExampleEntry(1, "Grasp Deformable with Motor", "Grasping test", GraspDeformableCreateFunc), ExampleEntry(1, "Volumetric Deformable Objects", "Volumetric Deformable test", VolumetricDeformableCreateFunc), + ExampleEntry(1, "Deformable Cloth Anchor", "Deformable Anchor test", DeformableClothAnchorCreateFunc), ExampleEntry(1, "Deformable-MultiBody Contact", "MultiBody and Deformable contact", DeformableMultibodyCreateFunc), // ExampleEntry(1, "MultiBody Baseline", "MultiBody Baseline", MultiBodyBaselineCreateFunc), diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index ce5018f2f..3bb20a6e1 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -15,6 +15,127 @@ #include "btDeformableContactConstraint.h" +/* ================ Deformable Node Anchor =================== */ +btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a) +: m_anchor(&a) +, btDeformableContactConstraint(a.m_cti.m_normal) +{ +} + +btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other) +: m_anchor(other.m_anchor) +, btDeformableContactConstraint(other) +{ +} + +btVector3 btDeformableNodeAnchorConstraint::getVa() const +{ + const btSoftBody::sCti& cti = m_anchor->m_cti; + btVector3 va(0, 0, 0); + if (cti.m_colObj->hasContactResponse()) + { + btRigidBody* rigidCol = 0; + btMultiBodyLinkCollider* multibodyLinkCol = 0; + + // grab the velocity of the rigid body + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_anchor->m_c1)) : btVector3(0, 0, 0); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + // multibody anchor not supported yet + btAssert(false); + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + const btScalar* J_n = &m_anchor->jacobianData_normal.m_jacobians[0]; + const btScalar* J_t1 = &m_anchor->jacobianData_t1.m_jacobians[0]; + const btScalar* J_t2 = &m_anchor->jacobianData_t2.m_jacobians[0]; + const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector(); + const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector(); + // add in the normal component of the va + btScalar vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k]+local_dv[k]) * J_n[k]; + } + va = cti.m_normal * vel; + // add in the tangential components of the va + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k]+local_dv[k]) * J_t1[k]; + } + va += m_anchor->t1 * vel; + vel = 0.0; + for (int k = 0; k < ndof; ++k) + { + vel += (local_v[k]+local_dv[k]) * J_t2[k]; + } + va += m_anchor->t2 * vel; + } + } + } + return va; +} + +btScalar btDeformableNodeAnchorConstraint::solveConstraint() +{ + const btSoftBody::sCti& cti = m_anchor->m_cti; + btVector3 va = getVa(); + btVector3 vb = getVb(); + btVector3 vr = (vb - va); + // + (m_anchor->m_node->m_x - cti.m_colObj->getWorldTransform() * m_anchor->m_local) * 10.0 + const btScalar dn = btDot(vr, cti.m_normal); + // 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_anchor->m_c0 * vr; + // apply impulse to deformable nodes involved and change their velocities + applyImpulse(impulse); + + // apply impulse to the rigid/multibodies involved and change their velocities + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + btRigidBody* rigidCol = 0; + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + if (rigidCol) + { + rigidCol->applyImpulse(impulse, m_anchor->m_c1); + } + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = 0; + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const btScalar* deltaV_normal = &m_anchor->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + // apply normal component of the impulse + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal)); + // apply tangential component of the impulse + const btScalar* deltaV_t1 = &m_anchor->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_anchor->t1)); + const btScalar* deltaV_t2 = &m_anchor->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_anchor->t2)); + } + } + return residualSquare; +} + +btVector3 btDeformableNodeAnchorConstraint::getVb() const +{ + return m_anchor->m_node->m_v; +} + +void btDeformableNodeAnchorConstraint::applyImpulse(const btVector3& impulse) +{ + btVector3 dv = impulse * m_anchor->m_c2; + m_anchor->m_node->m_v -= dv; +} + /* ================ Deformable vs. Rigid =================== */ btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c) : m_contact(&c) diff --git a/src/BulletSoftBody/btDeformableContactConstraint.h b/src/BulletSoftBody/btDeformableContactConstraint.h index f79bc6065..da16c51e5 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.h +++ b/src/BulletSoftBody/btDeformableContactConstraint.h @@ -108,6 +108,32 @@ public: virtual void applyImpulse(const btVector3& impulse){} }; +// +// Anchor Constraint between rigid and deformable node +class btDeformableNodeAnchorConstraint : public btDeformableContactConstraint +{ +public: + const btSoftBody::DeformableNodeRigidAnchor* m_anchor; + + btDeformableNodeAnchorConstraint(){} + btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c); + btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other); + virtual ~btDeformableNodeAnchorConstraint() + { + } + virtual btScalar solveConstraint(); + // object A is the rigid/multi body, and object B is the deformable node/face + virtual btVector3 getVa() const; + // get the velocity of the deformable node in contact + virtual btVector3 getVb() const; + virtual btVector3 getDv(const btSoftBody::Node* n) const + { + return btVector3(0,0,0); + } + virtual void applyImpulse(const btVector3& impulse); +}; + + // // Constraint between rigid/multi body and deformable objects class btDeformableRigidContactConstraint : public btDeformableContactConstraint diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index cf0a5e896..d438d6eec 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -32,6 +32,14 @@ btScalar btDeformableContactProjection::update() } } + // anchor constraints + for (int index = 0; index < m_nodeAnchorConstraints.size(); ++index) + { + btDeformableNodeAnchorConstraint& constraint = *m_nodeAnchorConstraints.getAtIndex(index); + btScalar localResidualSquare = constraint.solveConstraint(); + residualSquare = btMax(residualSquare, localResidualSquare); + } + // face constraints for (int index = 0; index < m_allFaceConstraints.size(); ++index) { @@ -73,6 +81,24 @@ void btDeformableContactProjection::setConstraints() continue; } + // set up deformable anchors + for (int j = 0; j < psb->m_deformableAnchors.size(); ++j) + { + btSoftBody::DeformableNodeRigidAnchor& anchor = psb->m_deformableAnchors[j]; + // skip fixed points + if (anchor.m_node->m_im == 0) + { + continue; + } + + if (m_nodeAnchorConstraints.find(anchor.m_node->index) == NULL) + { + anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local; + btDeformableNodeAnchorConstraint constraint(anchor); + m_nodeAnchorConstraints.insert(anchor.m_node->index, constraint); + } + } + // set Deformable Node vs. Rigid constraint for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j) { @@ -262,7 +288,7 @@ void btDeformableContactProjection::setProjection() bool existStaticConstraint = false; btVector3 averagedNormal(0,0,0); btAlignedObjectArray normals; - if (m_staticConstraints.find(index) != NULL) + if (m_staticConstraints.find(index) != NULL || m_nodeAnchorConstraints.find(index) != NULL) { existStaticConstraint = true; hasConstraint = true; @@ -451,6 +477,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f) void btDeformableContactProjection::reinitialize(bool nodeUpdated) { m_staticConstraints.clear(); + m_nodeAnchorConstraints.clear(); m_nodeRigidConstraints.clear(); m_faceRigidConstraints.clear(); m_deformableConstraints.clear(); diff --git a/src/BulletSoftBody/btDeformableContactProjection.h b/src/BulletSoftBody/btDeformableContactProjection.h index fa5eb662b..90386b0c9 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.h +++ b/src/BulletSoftBody/btDeformableContactProjection.h @@ -36,6 +36,8 @@ public: btHashMap > m_faceRigidConstraints; // map from node index to deformable constraint btHashMap > m_deformableConstraints; + // map from node index to node anchor constraint + btHashMap m_nodeAnchorConstraints; // all constraints involving face btAlignedObjectArray m_allFaceConstraints; diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index ded9893ce..242f84bee 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -140,6 +140,13 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) node.m_q = node.m_x; node.m_vn = node.m_v; } + // enforce anchor constraints + for (int j = 0; j < psb->m_deformableAnchors.size();++j) + { + btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j]; + btSoftBody::Node* n = a.m_node; + n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local; + } psb->interpolateRenderMesh(); } } diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index b05b28ca4..c25f4441b 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -411,6 +411,41 @@ void btSoftBody::appendAnchor(int node, btRigidBody* body, const btVector3& loca m_anchors.push_back(a); } +// +void btSoftBody::appendDeformableAnchor(int node, btRigidBody* body) +{ + DeformableNodeRigidAnchor c; + btSoftBody::Node& n = m_nodes[node]; + const btScalar ima = n.m_im; + const btScalar imb = body->getInvMass(); + btVector3 nrm; + const btCollisionShape* shp = body->getCollisionShape(); + const btTransform& wtr = body->getWorldTransform(); + btScalar dst = + m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(m_nodes[node].m_x), + shp, + nrm, + 0); + + c.m_cti.m_colObj = body; + c.m_cti.m_normal = wtr.getBasis() * nrm; + c.m_cti.m_offset = dst; + c.m_node = &m_nodes[node]; + const btScalar fc = m_cfg.kDF * body->getFriction(); + c.m_c2 = ima; + c.m_c3 = fc; + c.m_c4 = body->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR; + static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); + const btMatrix3x3& iwi = body->getInvInertiaTensorWorld(); + const btVector3 ra = n.m_x - wtr.getOrigin(); + + c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); + c.m_c1 = ra; + c.m_local = body->getWorldTransform().inverse() * m_nodes[node].m_x; + m_deformableAnchors.push_back(c); +} + // void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1) { diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 195434c06..8aca75e1e 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -354,6 +354,12 @@ public: Node* m_node; // Owner node }; + class DeformableNodeRigidAnchor : public DeformableNodeRigidContact + { + public: + btVector3 m_local; // Anchor position in body space + }; + class DeformableFaceRigidContact : public DeformableRigidContact { public: @@ -774,6 +780,7 @@ public: btAlignedObjectArray m_tetraScratches; btAlignedObjectArray m_tetraScratchesTn; tAnchorArray m_anchors; // Anchors + btAlignedObjectArray m_deformableAnchors; tRContactArray m_rcontacts; // Rigid contacts btAlignedObjectArray m_nodeRigidContacts; btAlignedObjectArray m_faceNodeContacts; @@ -897,7 +904,8 @@ public: Material* mat = 0); /* Append anchor */ - void appendAnchor(int node, + void appendDeformableAnchor(int node, btRigidBody* body); + void appendAnchor(int node, btRigidBody* body, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1); void appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1); /* Append linear joint */ From 36f7441790a2d3bc17eda170694bcab199e6b096 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 17 Oct 2019 16:45:28 -0700 Subject: [PATCH 29/29] support anchor constraint between deformable and multibody --- .../DeformableDemo/DeformableMultibody.cpp | 1 - .../DeformableDemo/MultibodyClothAnchor.cpp | 395 ++++++++++++++++++ .../DeformableDemo/MultibodyClothAnchor.h | 19 + examples/ExampleBrowser/CMakeLists.txt | 2 + examples/ExampleBrowser/ExampleEntries.cpp | 4 +- .../btDeformableContactConstraint.cpp | 2 - .../btDeformableMultiBodyDynamicsWorld.cpp | 47 ++- src/BulletSoftBody/btSoftBody.cpp | 57 +++ src/BulletSoftBody/btSoftBody.h | 1 + 9 files changed, 523 insertions(+), 5 deletions(-) create mode 100644 examples/DeformableDemo/MultibodyClothAnchor.cpp create mode 100644 examples/DeformableDemo/MultibodyClothAnchor.h diff --git a/examples/DeformableDemo/DeformableMultibody.cpp b/examples/DeformableDemo/DeformableMultibody.cpp index d7f254960..7b9137cc9 100644 --- a/examples/DeformableDemo/DeformableMultibody.cpp +++ b/examples/DeformableDemo/DeformableMultibody.cpp @@ -145,7 +145,6 @@ void DeformableMultibody::initPhysics() m_dynamicsWorld->addRigidBody(body,1,1+2); } - { bool damping = true; bool gyro = false; diff --git a/examples/DeformableDemo/MultibodyClothAnchor.cpp b/examples/DeformableDemo/MultibodyClothAnchor.cpp new file mode 100644 index 000000000..eeeb4d33c --- /dev/null +++ b/examples/DeformableDemo/MultibodyClothAnchor.cpp @@ -0,0 +1,395 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ +#include "MultibodyClothAnchor.h" +///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. +#include "btBulletDynamicsCommon.h" +#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h" +#include "BulletSoftBody/btSoftBody.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" +#include "BulletSoftBody/btDeformableBodySolver.h" +#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include //printf debugging + +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../Utils/b3ResourcePath.h" + +///The MultibodyClothAnchor shows contact between deformable objects and rigid objects. +class MultibodyClothAnchor : public CommonRigidBodyBase +{ + btAlignedObjectArray m_forces; +public: + MultibodyClothAnchor(struct GUIHelperInterface* helper) + : CommonRigidBodyBase(helper) + { + } + virtual ~MultibodyClothAnchor() + { + } + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 20; + float pitch = -45; + float yaw = 100; + float targetPos[3] = {0, -3, 0}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + void stepSimulation(float deltaTime) + { + //use a smaller internal timestep, there are stability issues + float internalTimeStep = 1. / 240.f; + m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); + } + + virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const + { + ///just make it a btSoftRigidDynamicsWorld please + ///or we will add type checking + return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; + } + + virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() + { + ///just make it a btSoftRigidDynamicsWorld please + ///or we will add type checking + return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; + } + + virtual void renderScene() + { + CommonRigidBodyBase::renderScene(); + btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); + + for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) + { + btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i]; + //if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) + { + btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); + } + } + } + + btMultiBody* createMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool floating = false); + + void addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents); +}; + +void MultibodyClothAnchor::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); + + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + m_broadphase = new btDbvtBroadphase(); + btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); + + ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) + btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); + sol->setDeformableSolver(deformableBodySolver); + m_solver = sol; + + m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); + // deformableBodySolver->setWorld(getDeformableDynamicsWorld()); + // m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality + btVector3 gravity = btVector3(0, -20, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; + + // getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + { + ///create a ground + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.))); + + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -35, 0)); + groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.)); + //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: + btScalar mass(0.); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0, 0, 0); + if (isDynamic) + groundShape->calculateLocalInertia(mass, localInertia); + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + body->setFriction(1); + + //add the ground to the dynamics world + m_dynamicsWorld->addRigidBody(body,1,1+2); + } + + // create a piece of cloth + { + const btScalar s = 4; + const btScalar h = 6; + const int r = 9; + btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s), + btVector3(+s, h, -s), + btVector3(-s, h, +s), + btVector3(+s, h, +s), r, r, 4 + 8, true); + psb->getCollisionShape()->setMargin(0.1); + psb->generateBendingConstraints(2); + 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 = 2; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + getDeformableDynamicsWorld()->addSoftBody(psb); + + btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(30,1, true); + 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); + + bool damping = true; + bool gyro = false; + int numLinks = 5; + bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals + bool canSleep = false; + bool selfCollide = true; + btVector3 linkHalfExtents(1.5, .5, .5); + btVector3 baseHalfExtents(1.5, .5, .5); + + btMultiBody* mbC = createMultiBody(getDeformableDynamicsWorld(), numLinks, btVector3(s+3.5f, h, -s-0.6f), linkHalfExtents, baseHalfExtents, spherical, true); + + mbC->setCanSleep(canSleep); + mbC->setHasSelfCollision(selfCollide); + mbC->setUseGyroTerm(gyro); + // + if (!damping) + { + mbC->setLinearDamping(0.0f); + mbC->setAngularDamping(0.0f); + } + else + { + mbC->setLinearDamping(0.04f); + mbC->setAngularDamping(0.04f); + } + + if (numLinks > 0) + { + btScalar q0 = 0.f * SIMD_PI / 180.f; + if (!spherical) + { + mbC->setJointPosMultiDof(0, &q0); + } + else + { + btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0); + quat0.normalize(); + mbC->setJointPosMultiDof(0, quat0); + } + } + /// + addColliders(mbC, getDeformableDynamicsWorld(), baseHalfExtents, linkHalfExtents); + + // quick hack: advance time to populate the variables in multibody + m_dynamicsWorld->stepSimulation(SIMD_EPSILON, 0); + btAlignedObjectArray scratch_q; + btAlignedObjectArray scratch_m; + mbC->forwardKinematics(scratch_q, scratch_m); + psb->appendDeformableAnchor(0, mbC->getLink(3).m_collider); + psb->appendDeformableAnchor(r - 1, mbC->getLink(0).m_collider); + } + getDeformableDynamicsWorld()->setImplicit(false); + getDeformableDynamicsWorld()->setLineSearch(false); + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void MultibodyClothAnchor::exitPhysics() +{ + //cleanup in the reverse order of creation/initialization + + //remove the rigidbodies from the dynamics world and delete them + int i; + for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) + { + btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()) + { + delete body->getMotionState(); + } + m_dynamicsWorld->removeCollisionObject(obj); + delete obj; + } + // delete forces + for (int j = 0; j < m_forces.size(); j++) + { + btDeformableLagrangianForce* force = m_forces[j]; + delete force; + } + m_forces.clear(); + //delete collision shapes + for (int j = 0; j < m_collisionShapes.size(); j++) + { + btCollisionShape* shape = m_collisionShapes[j]; + delete shape; + } + m_collisionShapes.clear(); + + delete m_dynamicsWorld; + + delete m_solver; + + delete m_broadphase; + + delete m_dispatcher; + + delete m_collisionConfiguration; +} + +btMultiBody* MultibodyClothAnchor::createMultiBody(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool floating) +{ + //init the base + btVector3 baseInertiaDiag(0.f, 0.f, 0.f); + float baseMass = 1.f; + + if (baseMass) + { + btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); + pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag); + delete pTempBox; + } + + bool canSleep = false; + + btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep); + + btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f); + pMultiBody->setBasePos(basePosition); + pMultiBody->setWorldToBaseRot(baseOriQuat); + btVector3 vel(0, 0, 0); + + //init the links + btVector3 hingeJointAxis(0, 1, 0); + float linkMass = 1.f; + btVector3 linkInertiaDiag(0.f, 0.f, 0.f); + + btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2])); + pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag); + delete pTempBox; + + //y-axis assumed up + btVector3 parentComToCurrentCom(-linkHalfExtents[0] * 2.f, 0, 0); //par body's COM to cur body's COM offset + btVector3 currentPivotToCurrentCom(-linkHalfExtents[0], 0, 0); //cur body's COM to cur body's PIV offset + btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset + + ////// + btScalar q0 = 0.f * SIMD_PI / 180.f; + btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0); + quat0.normalize(); + ///// + + for (int i = 0; i < numLinks; ++i) + { + if (!spherical) + pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true); + else + pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true); + } + + pMultiBody->finalizeMultiDof(); + + /// + pWorld->addMultiBody(pMultiBody); + /// + return pMultiBody; +} + +void MultibodyClothAnchor::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents) +{ + btAlignedObjectArray world_to_local; + world_to_local.resize(pMultiBody->getNumLinks() + 1); + + btAlignedObjectArray local_origin; + local_origin.resize(pMultiBody->getNumLinks() + 1); + world_to_local[0] = pMultiBody->getWorldToBaseRot(); + local_origin[0] = pMultiBody->getBasePos(); + + { + btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()}; + + btCollisionShape* box = new btBoxShape(baseHalfExtents); + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1); + col->setCollisionShape(box); + + btTransform tr; + tr.setIdentity(); + tr.setOrigin(local_origin[0]); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + col->setWorldTransform(tr); + pWorld->addCollisionObject(col, 2, 1+2); + col->setFriction(1); + pMultiBody->setBaseCollider(col); + } + + for (int i = 0; i < pMultiBody->getNumLinks(); ++i) + { + const int parent = pMultiBody->getParent(i); + world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1]; + local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i))); + } + + for (int i = 0; i < pMultiBody->getNumLinks(); ++i) + { + btVector3 posr = local_origin[i + 1]; + + btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()}; + + btCollisionShape* box = new btBoxShape(linkHalfExtents); + btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i); + + col->setCollisionShape(box); + btTransform tr; + tr.setIdentity(); + tr.setOrigin(posr); + tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3])); + col->setWorldTransform(tr); + col->setFriction(1); + pWorld->addCollisionObject(col, 2, 1+2); + pMultiBody->getLink(i).m_collider = col; + } +} + +class CommonExampleInterface* MultibodyClothAnchorCreateFunc(struct CommonExampleOptions& options) +{ + return new MultibodyClothAnchor(options.m_guiHelper); +} + + diff --git a/examples/DeformableDemo/MultibodyClothAnchor.h b/examples/DeformableDemo/MultibodyClothAnchor.h new file mode 100644 index 000000000..818e043c1 --- /dev/null +++ b/examples/DeformableDemo/MultibodyClothAnchor.h @@ -0,0 +1,19 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef MULTIBODY_CLOTH_ANCHOR_H +#define MULTIBODY_CLOTH_ANCHOR_H + +class CommonExampleInterface* MultibodyClothAnchorCreateFunc(struct CommonExampleOptions& options); + +#endif //MULTIBODY_CLOTH_ANCHOR_H diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 56aaf625b..56eecf328 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -373,6 +373,8 @@ SET(BulletExampleBrowser_SRCS ../DeformableDemo/VolumetricDeformable.h ../DeformableDemo/DeformableClothAnchor.cpp ../DeformableDemo/DeformableClothAnchor.h + ../DeformableDemo/MultibodyClothAnchor.cpp + ../DeformableDemo/MultibodyClothAnchor.h ../MultiBody/MultiDofDemo.cpp ../MultiBody/MultiDofDemo.h ../RigidBody/RigidBodySoftContact.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 887fb2fc5..191d62e58 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -54,6 +54,7 @@ #include "../DeformableDemo/GraspDeformable.h" #include "../DeformableDemo/DeformableContact.h" #include "../DeformableDemo/DeformableClothAnchor.h" +#include "../DeformableDemo/MultibodyClothAnchor.h" #include "../SharedMemory/PhysicsServerExampleBullet2.h" #include "../SharedMemory/PhysicsServerExample.h" #include "../SharedMemory/PhysicsClientExample.h" @@ -199,7 +200,8 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(1, "Grasp Deformable Cube", "Grasping test", PinchCreateFunc), ExampleEntry(1, "Grasp Deformable with Motor", "Grasping test", GraspDeformableCreateFunc), ExampleEntry(1, "Volumetric Deformable Objects", "Volumetric Deformable test", VolumetricDeformableCreateFunc), - ExampleEntry(1, "Deformable Cloth Anchor", "Deformable Anchor test", DeformableClothAnchorCreateFunc), + ExampleEntry(1, "Rigid Cloth Anchor", "Deformable Rigid body Anchor test", DeformableClothAnchorCreateFunc), + ExampleEntry(1, "Multibody Cloth Anchor", "Deformable Multibody Anchor test", MultibodyClothAnchorCreateFunc), ExampleEntry(1, "Deformable-MultiBody Contact", "MultiBody and Deformable contact", DeformableMultibodyCreateFunc), // ExampleEntry(1, "MultiBody Baseline", "MultiBody Baseline", MultiBodyBaselineCreateFunc), diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index 3bb20a6e1..91a6c7d20 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -45,8 +45,6 @@ btVector3 btDeformableNodeAnchorConstraint::getVa() const } else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) { - // multibody anchor not supported yet - btAssert(false); multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); if (multibodyLinkCol) { diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 242f84bee..9f8e13e20 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -33,7 +33,7 @@ The algorithm also closely resembles the one in http://physbam.stanford.edu/~fed #include "btDeformableMultiBodyDynamicsWorld.h" #include "btDeformableBodySolver.h" #include "LinearMath/btQuickprof.h" - +#include "btSoftBodyInternals.h" void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) { BT_PROFILE("internalSingleStepSimulation"); @@ -146,6 +146,51 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j]; btSoftBody::Node* n = a.m_node; n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local; + + // update multibody anchor info + if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj); + if (multibodyLinkCol) + { + btVector3 nrm; + const btCollisionShape* shp = multibodyLinkCol->getCollisionShape(); + const btTransform& wtr = multibodyLinkCol->getWorldTransform(); + psb->m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(n->m_x), + shp, + nrm, + 0); + a.m_cti.m_normal = wtr.getBasis() * nrm; + btVector3 normal = a.m_cti.m_normal; + btVector3 t1 = generateUnitOrthogonalVector(normal); + btVector3 t2 = btCross(normal, t1); + btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; + findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal); + findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1); + findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, t2); + + btScalar* J_n = &jacobianData_normal.m_jacobians[0]; + btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; + btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; + + btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + + btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), + t1.getX(), t1.getY(), t1.getZ(), + t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + btMatrix3x3 local_impulse_matrix = (Diagonal(n->m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + a.m_c0 = rot.transpose() * local_impulse_matrix * rot; + a.jacobianData_normal = jacobianData_normal; + a.jacobianData_t1 = jacobianData_t1; + a.jacobianData_t2 = jacobianData_t2; + a.t1 = t1; + a.t2 = t2; + } + } } psb->interpolateRenderMesh(); } diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index c25f4441b..c46f7a3b8 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -443,9 +443,66 @@ void btSoftBody::appendDeformableAnchor(int node, btRigidBody* body) c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); c.m_c1 = ra; c.m_local = body->getWorldTransform().inverse() * m_nodes[node].m_x; + c.m_node->m_battach = 1; m_deformableAnchors.push_back(c); } +// +void btSoftBody::appendDeformableAnchor(int node, btMultiBodyLinkCollider* link) +{ + DeformableNodeRigidAnchor c; + btSoftBody::Node& n = m_nodes[node]; + const btScalar ima = n.m_im; + btVector3 nrm; + const btCollisionShape* shp = link->getCollisionShape(); + const btTransform& wtr = link->getWorldTransform(); + btScalar dst = + m_worldInfo->m_sparsesdf.Evaluate( + wtr.invXform(m_nodes[node].m_x), + shp, + nrm, + 0); + c.m_cti.m_colObj = link; + c.m_cti.m_normal = wtr.getBasis() * nrm; + c.m_cti.m_offset = dst; + c.m_node = &m_nodes[node]; + const btScalar fc = m_cfg.kDF * link->getFriction(); + c.m_c2 = ima; + c.m_c3 = fc; + c.m_c4 = link->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR; + btVector3 normal = c.m_cti.m_normal; + btVector3 t1 = generateUnitOrthogonalVector(normal); + btVector3 t2 = btCross(normal, t1); + btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; + findJacobian(link, jacobianData_normal, c.m_node->m_x, normal); + findJacobian(link, jacobianData_t1, c.m_node->m_x, t1); + findJacobian(link, jacobianData_t2, c.m_node->m_x, t2); + + btScalar* J_n = &jacobianData_normal.m_jacobians[0]; + btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; + btScalar* J_t2 = &jacobianData_t2.m_jacobians[0]; + + btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0]; + btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0]; + + btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(), + t1.getX(), t1.getY(), t1.getZ(), + t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame + const int ndof = link->m_multiBody->getNumDofs() + 6; + btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + c.m_c0 = rot.transpose() * local_impulse_matrix * rot; + c.jacobianData_normal = jacobianData_normal; + c.jacobianData_t1 = jacobianData_t1; + c.jacobianData_t2 = jacobianData_t2; + c.t1 = t1; + c.t2 = t2; + const btVector3 ra = n.m_x - wtr.getOrigin(); + c.m_c1 = ra; + c.m_local = link->getWorldTransform().inverse() * m_nodes[node].m_x; + c.m_node->m_battach = 1; + m_deformableAnchors.push_back(c); +} // void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1) { diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 8aca75e1e..51f0dad21 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -905,6 +905,7 @@ public: /* Append anchor */ void appendDeformableAnchor(int node, btRigidBody* body); + void appendDeformableAnchor(int node, btMultiBodyLinkCollider* link); void appendAnchor(int node, btRigidBody* body, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1); void appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1);