From 5be9f8f3014f19e01d8a871f45ee3e776705479e Mon Sep 17 00:00:00 2001
From: "erwin.coumans" <erwin.coumans@08e121b0-ae19-0410-a57b-3be3395fd4fd>
Date: Mon, 2 Mar 2009 05:13:26 +0000
Subject: [PATCH] basic implementation of self-collision for btSoftBody with
 collision clusters: perform regular soft body versus soft body, but disable
 collision detection between clusters that are connected/share any node
 (vertex).

currently self-collision is always enabled for soft bodies that use clusters collision detectoin. to disable, call 'softbody.m_clusterConnectivity.clear' after 'generateClusters(...) call.
---
 src/BulletSoftBody/btSoftBody.cpp             | 35 ++++++++++++++++
 src/BulletSoftBody/btSoftBody.h               |  2 +
 src/BulletSoftBody/btSoftBodyInternals.h      | 41 +++++++++++++------
 .../btSoftRigidDynamicsWorld.cpp              |  7 ++++
 4 files changed, 73 insertions(+), 12 deletions(-)

diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp
index 8217a87a8..a9c21ed8f 100644
--- a/src/BulletSoftBody/btSoftBody.cpp
+++ b/src/BulletSoftBody/btSoftBody.cpp
@@ -911,6 +911,35 @@ int				btSoftBody::generateClusters(int k,int maxiterations)
 
 		initializeClusters();
 		updateClusters();
+
+		//for self-collision
+		m_clusterConnectivity.resize(m_clusters.size()*m_clusters.size());
+		{
+			for (int c0=0;c0<m_clusters.size();c0++)
+			{
+				m_clusters[c0]->m_clusterIndex=c0;
+				for (int c1=0;c1<m_clusters.size();c1++)
+				{
+					
+					bool connected=false;
+					Cluster* cla = m_clusters[c0];
+					Cluster* clb = m_clusters[c1];
+					for (int i=0;!connected&&i<cla->m_nodes.size();i++)
+					{
+						for (int j=0;j<clb->m_nodes.size();j++)
+						{
+							if (cla->m_nodes[i] == clb->m_nodes[j])
+							{
+								connected=true;
+								break;
+							}
+						}
+					}
+					m_clusterConnectivity[c0+c1*m_clusters.size()]=connected;
+				}
+			}
+		}
+	
 		return(m_clusters.size());
 	}
 	return(0);
@@ -2059,8 +2088,13 @@ void					btSoftBody::updateClusters()
 			}
 		}
 	}
+
+
 }
 
+
+
+
 //
 void					btSoftBody::cleanupClusters()
 {
@@ -2319,6 +2353,7 @@ void				btSoftBody::CJoint::Terminate(btScalar dt)
 //
 void				btSoftBody::applyForces()
 {
+
 	BT_PROFILE("SoftBody applyForces");
 	const btScalar					dt=m_sst.sdt;
 	const btScalar					kLF=m_cfg.kLF;
diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h
index 94d4bdbe7..9fcc4ec3b 100644
--- a/src/BulletSoftBody/btSoftBody.h
+++ b/src/BulletSoftBody/btSoftBody.h
@@ -296,6 +296,7 @@ public:
 		btScalar					m_adamping;	/* Angular damping	*/ 
 		btScalar					m_matching;
 		bool						m_collide;
+		int							m_clusterIndex;
 		Cluster() : m_leaf(0),m_ndamping(0),m_ldamping(0),m_adamping(0),m_matching(0) {}
 	};
 	/* Impulse		*/ 
@@ -603,6 +604,7 @@ public:
 	btDbvt					m_fdbvt;		// Faces tree
 	btDbvt					m_cdbvt;		// Clusters tree
 	tClusterArray			m_clusters;		// Clusters
+	btAlignedObjectArray<bool>m_clusterConnectivity;//cluster connectivity, for self-collision
 
 	//
 	// Api
diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h
index 16c76e3f0..5f0f7d543 100644
--- a/src/BulletSoftBody/btSoftBodyInternals.h
+++ b/src/BulletSoftBody/btSoftBodyInternals.h
@@ -766,21 +766,38 @@ struct btSoftColliders
 		{
 			btSoftBody::Cluster*		cla=(btSoftBody::Cluster*)la->data;
 			btSoftBody::Cluster*		clb=(btSoftBody::Cluster*)lb->data;
-			btSoftClusterCollisionShape	csa(cla);
-			btSoftClusterCollisionShape	csb(clb);
-			btGjkEpaSolver2::sResults	res;		
-			if(btGjkEpaSolver2::SignedDistance(	&csa,btTransform::getIdentity(),
-				&csb,btTransform::getIdentity(),
-				cla->m_com-clb->m_com,res))
+
+
+			bool connected=false;
+			if ((bodies[0]==bodies[1])&&(bodies[0]->m_clusterConnectivity.size()))
 			{
-				btSoftBody::CJoint	joint;
-				if(SolveContact(res,cla,clb,joint))
+				connected = bodies[0]->m_clusterConnectivity[cla->m_clusterIndex+bodies[0]->m_clusters.size()*clb->m_clusterIndex];
+			}
+
+			if (!connected)
+			{
+				btSoftClusterCollisionShape	csa(cla);
+				btSoftClusterCollisionShape	csb(clb);
+				btGjkEpaSolver2::sResults	res;		
+				if(btGjkEpaSolver2::SignedDistance(	&csa,btTransform::getIdentity(),
+					&csb,btTransform::getIdentity(),
+					cla->m_com-clb->m_com,res))
 				{
-					btSoftBody::CJoint*	pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
-					*pj=joint;bodies[0]->m_joints.push_back(pj);
-					pj->m_erp	*=	btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL);
-					pj->m_split	*=	(bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2;
+					btSoftBody::CJoint	joint;
+					if(SolveContact(res,cla,clb,joint))
+					{
+						btSoftBody::CJoint*	pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint();
+						*pj=joint;bodies[0]->m_joints.push_back(pj);
+						pj->m_erp	*=	btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL);
+						pj->m_split	*=	(bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2;
+					}
 				}
+			} else
+			{
+				static int count=0;
+				count++;
+				//printf("count=%d\n",count);
+				
 			}
 		}
 		void		Process(btSoftBody* psa,btSoftBody* psb)
diff --git a/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp b/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp
index 1418746b0..a0069b951 100644
--- a/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp
+++ b/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp
@@ -63,6 +63,13 @@ void	btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep)
 	///solve soft bodies constraints
 	solveSoftBodiesConstraints();
 
+	//self collisions
+	for ( int i=0;i<m_softBodies.size();i++)
+	{
+		btSoftBody*	psb=(btSoftBody*)m_softBodies[i];
+		psb->defaultCollisionHandler(psb);
+	}
+
 	///update soft bodies
 	updateSoftBodies();