From 086638d16e03cce058fc4249939617c7bfbe60bc Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Thu, 6 Aug 2020 20:54:15 -0700 Subject: fix bug in multibody point contact with deformable --- examples/DeformableDemo/GraspDeformable.cpp | 25 ++++++++++++++++++---- .../btDeformableContactProjection.cpp | 7 +++++- src/BulletSoftBody/btSoftBodyInternals.h | 2 +- 3 files changed, 28 insertions(+), 6 deletions(-) diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index 1d692a60b..285f7c8c8 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -205,9 +205,9 @@ void GraspDeformable::initPhysics() getDeformableDynamicsWorld()->getSolverInfo().m_deformable_cfm = 0; getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 150; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); - m_maxPickingForce = 0.001; + m_maxPickingForce =100; // build a gripper - if(1) + if(0) { bool damping = true; bool gyro = false; @@ -257,9 +257,10 @@ void GraspDeformable::initPhysics() } //create a ground + if(0) { btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(10.), btScalar(5.), btScalar(10.))); - groundShape->setMargin(0.003); + groundShape->setMargin(0.001); m_collisionShapes.push_back(groundShape); btTransform groundTransform; @@ -285,6 +286,22 @@ void GraspDeformable::initPhysics() //add the ground to the dynamics world m_dynamicsWorld->addRigidBody(body,1,1+2); } + + if(1) + { + bool gyro = false; + bool canSleep = false; + bool selfCollide = true; + btVector3 linkHalfExtents(10, 5, 10); + btVector3 baseHalfExtents(10, 5, 10); + btVector3 basepos(0, -5, 0); + btMultiBody* mbC = createFeatherstoneMultiBody(getDeformableDynamicsWorld(), basepos, linkHalfExtents, baseHalfExtents, false); + + mbC->setCanSleep(canSleep); + mbC->setHasSelfCollision(selfCollide); + mbC->setUseGyroTerm(gyro); + addColliders(mbC, getDeformableDynamicsWorld(), baseHalfExtents, linkHalfExtents); + } // create a soft block if (0) @@ -453,7 +470,7 @@ void GraspDeformable::initPhysics() } // psb->scale(btVector3(1, 1, 1)); psb->rotate(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.5)); -// psb->translate(tr); + psb->translate(btVector3(0,1,0)); psb->getCollisionShape()->setMargin(0.005); psb->getCollisionShape()->setUserPointer(psb); diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 1fd67daf5..6394eb9a2 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -494,6 +494,9 @@ void btDeformableContactProjection::setLagrangeMultiplier() lm.m_dirs[2] = btVector3(0, 0, 1); m_lagrangeMultipliers.push_back(lm); } + + + if( m_faceRigidConstraints[i].size()==0){ for (int j = 0; j < m_nodeRigidConstraints[i].size(); ++j) { if (!m_nodeRigidConstraints[i][j].m_binding) @@ -519,7 +522,9 @@ void btDeformableContactProjection::setLagrangeMultiplier() lm.m_dirs[0] = m_nodeRigidConstraints[i][j].m_normal; } m_lagrangeMultipliers.push_back(lm); - } + } + } + for (int j = 0; j < m_faceRigidConstraints[i].size(); ++j) { if (!m_faceRigidConstraints[i][j].m_binding) diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index e626a8b75..81fd9bbca 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -1686,6 +1686,7 @@ struct btSoftColliders c.m_c2 = ima; c.m_c3 = fc; c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; + c.m_c5 = n.m_effectiveMass_inv; if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { @@ -1695,7 +1696,6 @@ struct btSoftColliders const btVector3 ra = n.m_x - wtr.getOrigin(); c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra); - c.m_c5 = n.m_effectiveMass_inv; // c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); c.m_c1 = ra; } -- cgit v1.2.1