allow multiple reduced deformable object in one sim

6 files changed, 68 insertions, 12 deletions

diff --git a/examples/ReducedDeformableDemo/FreeFall.cpp b/examples/ReducedDeformableDemo/FreeFall.cpp
index 47c98876e..1b4c44469 100644
--- a/examples/ReducedDeformableDemo/FreeFall.cpp
+++ b/examples/ReducedDeformableDemo/FreeFall.cpp
@@ -152,6 +152,8 @@ void FreeFall::initPhysics()
         rsb->getCollisionShape()->setMargin(0.01);
         // rsb->scale(btVector3(1, 1, 0.5));
 
+        rsb->setTotalMass(10);
+
         btTransform init_transform;
         init_transform.setIdentity();
         init_transform.setOrigin(btVector3(0, 4, 0));
@@ -160,8 +162,38 @@ void FreeFall::initPhysics()
 
         rsb->setStiffnessScale(5);
         rsb->setDamping(damping_alpha, damping_beta);
+        // rsb->scale(btVector3(0.5, 0.5, 0.5));
+
+        rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
+        rsb->m_cfg.kCHR = 1; // collision hardness with rigid body
+        rsb->m_cfg.kDF = 0;
+        rsb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+        rsb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
+        rsb->m_sleepingThreshold = 0;
+        btSoftBodyHelpers::generateBoundaryFaces(rsb);
+        
+        // rsb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0));
+        // rsb->setRigidVelocity(btVector3(0, 0, 1));
+        // rsb->setRigidAngularVelocity(btVector3(1, 0, 0));
+    }
+
+    {   
+        btReducedSoftBody* rsb = btReducedSoftBodyHelpers::createReducedTorus(getDeformableDynamicsWorld()->getWorldInfo(), num_modes);
+
+        getDeformableDynamicsWorld()->addSoftBody(rsb);
+        rsb->getCollisionShape()->setMargin(0.01);
+        // rsb->scale(btVector3(1, 1, 0.5));
 
         rsb->setTotalMass(10);
+
+        btTransform init_transform;
+        init_transform.setIdentity();
+        init_transform.setOrigin(btVector3(4, 4, 0));
+        init_transform.setRotation(btQuaternion(SIMD_PI / 2.0, SIMD_PI / 2.0, 0));
+        rsb->transformTo(init_transform);
+
+        rsb->setStiffnessScale(5);
+        rsb->setDamping(damping_alpha, damping_beta);
         // rsb->scale(btVector3(0.5, 0.5, 0.5));
 
         rsb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp
index d8f1b0b14..00a48c194 100644
--- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp
+++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.cpp
@@ -63,6 +63,7 @@ btReducedDeformableRigidContactConstraint::btReducedDeformableRigidContactConstr
 	btScalar dt)
   : m_rsb(rsb), m_dt(dt), btDeformableRigidContactConstraint(c, infoGlobal)
 {
+	m_nodeQueryIndex = 0;
 	m_appliedNormalImpulse = 0;
   m_appliedTangentImpulse = 0;
 	m_rhs = 0;
@@ -326,6 +327,15 @@ btReducedDeformableNodeRigidContactConstraint::btReducedDeformableNodeRigidConta
 	m_contactNormalA = contact.m_cti.m_normal;
   m_contactNormalB = -contact.m_cti.m_normal;
 
+	if (contact.m_node->index < rsb->m_nodes.size())
+	{
+		m_nodeQueryIndex = contact.m_node->index;
+	}
+	else
+	{
+		m_nodeQueryIndex = m_node->index - rsb->m_nodeIndexOffset;
+	}
+
 	if (m_contact->m_cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
 	{
 		m_relPosA = contact.m_c1;
@@ -338,11 +348,11 @@ btReducedDeformableNodeRigidContactConstraint::btReducedDeformableNodeRigidConta
 
 	if (m_collideStatic)		// colliding with static object, only consider reduced deformable body's impulse factor
 	{
-		m_impulseFactor = m_rsb->getImpulseFactor(m_node->index);
+		m_impulseFactor = m_rsb->getImpulseFactor(m_nodeQueryIndex);
 	}
 	else		// colliding with dynamic object, consider both reduced deformable and rigid body's impulse factors
 	{
-		m_impulseFactor = m_rsb->getImpulseFactor(m_node->index) + contact.m_c0;
+		m_impulseFactor = m_rsb->getImpulseFactor(m_nodeQueryIndex) + contact.m_c0;
 	}
 
 	m_normalImpulseFactor = (m_impulseFactor * m_contactNormalA).dot(m_contactNormalA);
@@ -374,9 +384,13 @@ void btReducedDeformableNodeRigidContactConstraint::warmStarting()
 		if (!m_collideMultibody)
 		{
 			m_contactTangent = v_tangent.normalized();
-			// tangent impulse factor
+			m_contactTangent2.setZero();
+			// tangent impulse factor 1
 			m_tangentImpulseFactor = (m_impulseFactor * m_contactTangent).dot(m_contactTangent);
 			m_tangentImpulseFactorInv = btScalar(1) / m_tangentImpulseFactor;
+			// tangent impulse factor 2
+			m_tangentImpulseFactor2 = 0;
+			m_tangentImpulseFactorInv2 = 0;
 		}
 		else	// multibody requires 2 contact directions
 		{
@@ -490,7 +504,7 @@ btVector3 btReducedDeformableNodeRigidContactConstraint::getDeltaVa() const
 btVector3 btReducedDeformableNodeRigidContactConstraint::getDeltaVb() const
 {	
 	// std::cout << "node: " << m_node->index << '\n';
-	return m_rsb->internalComputeNodeDeltaVelocity(m_rsb->getInterpolationWorldTransform(), m_node->index);
+	return m_rsb->internalComputeNodeDeltaVelocity(m_rsb->getInterpolationWorldTransform(), m_nodeQueryIndex);
 }
 
 btVector3 btReducedDeformableNodeRigidContactConstraint::getSplitVb() const
@@ -505,7 +519,7 @@ btVector3 btReducedDeformableNodeRigidContactConstraint::getDv(const btSoftBody:
 
 void btReducedDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse)
 {
-  m_rsb->internalApplyFullSpaceImpulse(impulse, m_relPosB, m_node->index, m_dt);
+  m_rsb->internalApplyFullSpaceImpulse(impulse, m_relPosB, m_nodeQueryIndex, m_dt);
 	// m_rsb->applyFullSpaceImpulse(impulse, m_relPosB, m_node->index, m_dt);
 	// m_rsb->mapToFullVelocity(m_rsb->getInterpolationWorldTransform());
 	// if (!m_collideStatic)
diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h
index 2c719ddd1..fbbd29583 100644
--- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h
+++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedDeformableContactConstraint.h
@@ -44,6 +44,8 @@ class btReducedDeformableRigidContactConstraint : public btDeformableRigidContac
   bool m_collideStatic;     // flag for collision with static object
   bool m_collideMultibody;  // flag for collision with multibody
 
+  int m_nodeQueryIndex;
+
   btReducedSoftBody* m_rsb;
   btSolverBody* m_solverBody;
   btScalar m_dt;
diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp
index be359f903..fcf2f6f8c 100644
--- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp
+++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.cpp
@@ -14,6 +14,7 @@ btReducedSoftBody::btReducedSoftBody(btSoftBodyWorldInfo* worldInfo, int node_co
   m_reducedModel = true;
   m_nReduced = 0;
   m_nFull = 0;
+  m_nodeIndexOffset = 0;
 
   m_transform_lock = false;
   m_ksScale = 1.0;
diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h
index fcc01a6c1..99fcdb568 100644
--- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h
+++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBody.h
@@ -91,6 +91,7 @@ class btReducedSoftBody : public btSoftBody
   TVStack m_x0;					     				 // Rest position
   tDenseArray m_nodalMass;           // Mass on each node
   btAlignedObjectArray<int> m_fixedNodes; // index of the fixed nodes
+  int m_nodeIndexOffset;             // offset of the node index needed for contact solver when there are multiple reduced deformable body in the world.
 
   // contacts
   btAlignedObjectArray<int> m_contactNodesList;
diff --git a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp
index b851ee2af..ee6d924ce 100644
--- a/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp
+++ b/src/BulletSoftBody/BulletReducedSoftBody/btReducedSoftBodySolver.cpp
@@ -62,6 +62,15 @@ void btReducedSoftBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody
     rsb->m_contactNodesList.clear();
   }
 
+  // set node index offsets
+  int sum = 0;
+  for (int i = 0; i < m_softBodies.size(); ++i)
+  {
+    btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
+    rsb->m_nodeIndexOffset = sum;
+    rsb->m_nodeIndexOffset += rsb->m_nodes.size();
+  }
+
 	btDeformableBodySolver::updateSoftBodies();
 }
 
@@ -182,10 +191,6 @@ void btReducedSoftBodySolver::applyTransforms(btScalar timeStep)
     // update the rendering mesh
     rsb->interpolateRenderMesh();
   }
-
-  // static int count = 0;
-  // if (count > 0) exit(100);
-  // count++;
 }
 
 void btReducedSoftBodySolver::setConstraints(const btContactSolverInfo& infoGlobal)
@@ -226,7 +231,7 @@ void btReducedSoftBodySolver::setConstraints(const btContactSolverInfo& infoGlob
 			}
 			btReducedDeformableNodeRigidContactConstraint constraint(rsb, contact, infoGlobal, m_dt);
 			m_nodeRigidConstraints[i].push_back(constraint);
-      rsb->m_contactNodesList.push_back(contact.m_node->index);
+      rsb->m_contactNodesList.push_back(contact.m_node->index - rsb->m_nodeIndexOffset);
 		}
     // std::cout << "contact node list size: " << rsb->m_contactNodesList.size() << "\n";
     // std::cout << "#contact nodes: " << m_nodeRigidConstraints[i].size() << "\n";
@@ -274,10 +279,11 @@ btScalar btReducedSoftBodySolver::solveContactConstraints(btCollisionObject** de
 {
   btScalar residualSquare = 0;
 
-  btAlignedObjectArray<int> m_orderNonContactConstraintPool;
-  btAlignedObjectArray<int> m_orderContactConstraintPool;
   for (int i = 0; i < m_softBodies.size(); ++i)
   {
+    btAlignedObjectArray<int> m_orderNonContactConstraintPool;
+    btAlignedObjectArray<int> m_orderContactConstraintPool;
+
     btReducedSoftBody* rsb = static_cast<btReducedSoftBody*>(m_softBodies[i]);
 
     // shuffle the order of applying constraint