fix bug in multibody point contact with deformable

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;
 						}