Softbody improvements, thanks to Nathanael

Demos/SoftDemo/SoftDemo.cpp

@@ -99,13 +99,32 @@ void SoftDemo::clientMoveAndDisplay()
 
 
 
-	float dt = getDeltaTimeMicroseconds() * 0.000001f;
+	float dt = 1.0/60.;	
-
-	//	printf("dt = %f: ",dt);
-
 
 	if (m_dynamicsWorld)
 	{
+	if(m_drag)
+		{
+		const int				x=m_lastmousepos[0];
+		const int				y=m_lastmousepos[1];
+		const btVector3			rayFrom=m_cameraPosition;
+		const btVector3			rayTo=getRayTo(x,y);
+		const btVector3			rayDir=(rayTo-rayFrom).normalized();
+		const btVector3			N=(m_cameraTargetPosition-m_cameraPosition).normalized();
+		const btScalar			O=dot(m_impact,N);
+		const btScalar			den=dot(N,rayDir);
+		if((den*den)>0)
+			{
+			const btScalar			num=O-dot(N,rayFrom);
+			const btScalar			hit=num/den;
+			if((hit>0)&&(hit<1500))
+				{				
+				m_goal=rayFrom+rayDir*hit;
+				}				
+			}		
+		m_node->m_v+=(m_goal-m_node->m_x)/dt;
+		}
+	
 #define FIXED_STEP
 #ifdef FIXED_STEP
 		m_dynamicsWorld->stepSimulation(dt=1.0f/60.f,0);
@@ -137,6 +156,10 @@ void SoftDemo::clientMoveAndDisplay()
 
 #endif		
 
+		if(m_drag)
+		{
+		m_node->m_v*=0;
+		}
 
 		m_softBodyWorldInfo.m_sparsesdf.GarbageCollect();
 
@@ -670,6 +693,28 @@ static void	Init_Sticks(SoftDemo* pdemo)
 	Ctor_BigBall(pdemo);
 }
 
+//
+// Bending
+//
+static void	Init_Bending(SoftDemo* pdemo)
+{
+	//TRACEDEMO
+	const btScalar	s=4;
+	const btVector3	x[]={	btVector3(-s,0,-s),
+							btVector3(+s,0,-s),
+							btVector3(+s,0,+s),
+							btVector3(-s,0,+s)};
+	const btScalar	m[]={	0,0,0,1};
+	btSoftBody*		psb=new btSoftBody(&pdemo->m_softBodyWorldInfo,4,x,m);
+	psb->appendLink(0,1);
+	psb->appendLink(1,2);
+	psb->appendLink(2,3);
+	psb->appendLink(3,0);
+	psb->appendLink(0,2);
+	
+	pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
+}
+
 //
 // 100kg cloth locked at corners, 10 falling 10kg rb's.
 //
@@ -715,6 +760,7 @@ static void	Init_Bunny(SoftDemo* pdemo)
 	psb->scale(btVector3(6,6,6));
 	psb->setTotalMass(100,true);
 	pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
+	pdemo->m_cutting=true;
 
 }
 
@@ -756,6 +802,7 @@ static void	Init_Torus(SoftDemo* pdemo)
 	psb->scale(btVector3(2,2,2));
 	psb->setTotalMass(50,true);
 	pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
+	pdemo->m_cutting=true;
 
 }
 
@@ -798,155 +845,6 @@ static void	Init_Cutting1(SoftDemo* pdemo)
 	pdemo->m_cutting=true;	
 }
 
-#ifdef BT_SOFTBODY_USE_STL
-//
-// TetraBunny
-//
-static void	Init_TetraBunny(SoftDemo* pdemo)
-{
-	btSoftBody* psb=btSoftBodyHelpers::CreateFromTetGenData(pdemo->m_softBodyWorldInfo,
-															TetraBunny::getElements(),
-															0,
-															TetraBunny::getNodes(),
-															false,true,true);
-	pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
-	psb->rotate(btQuaternion(SIMD_PI/2,0,0));
-	psb->setVolumeMass(150);
-	psb->m_cfg.piterations=2;
-	pdemo->m_cutting=true;	
-}
-
-//
-// TetraCube
-//
-static void	Init_TetraCube(SoftDemo* pdemo)
-{
-	btSoftBody* psb=btSoftBodyHelpers::CreateFromTetGenData(pdemo->m_softBodyWorldInfo,
-															TetraCube::getElements(),
-															0,
-															TetraCube::getNodes(),
-															false,true,true);
-	pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
-	psb->scale(btVector3(4,4,4));
-	psb->translate(btVector3(0,5,0));
-	psb->setVolumeMass(300);
-	psb->setMass(0,0);
-	/*psb->setMass(10,0);
-	psb->setMass(20,0);*/
-	psb->m_cfg.piterations=1;
-	//psb->m_materials[0]->m_kLST=0.05;
-	pdemo->m_cutting=true;	
-}
-#endif //BT_SOFTBODY_USE_STL
-
-//
-// Tetra
-//
-static void	Init_Tetra(SoftDemo* pdemo)
-{
-	//TRACEDEMO
-	#if 0
-		{
-		btVector3	pts[]={	btVector3(-1,-1,-1),
-							btVector3(+1,-1,-1),
-							btVector3(+1,+1,-1),
-							btVector3(-1,+1,-1),
-							btVector3(-1,-1,+1),
-							btVector3(+1,-1,+1),
-							btVector3(+1,+1,+1),
-							btVector3(-1,+1,+1)};
-		btSoftBody*	psb=btSoftBodyHelpers::CreateFromConvexHull(pdemo->m_softBodyWorldInfo,pts,8);
-		btSoftBodyHelpers::ExportAsSMeshFile(psb,"C:/HomeH/Oss/tetgen/Release/cube.smesh");
-		delete psb;
-		/*btSoftBody*	psb=btSoftBodyHelpers::CreateFromTriMesh(	pdemo->m_softBodyWorldInfo,gVerticesBunny,
-																&gIndicesBunny[0][0],
-																BUNNY_NUM_TRIANGLES);
-		psb->scale(btVector3(6,6,6));
-		btSoftBodyHelpers::ExportAsSMeshFile(psb,"C:/HomeH/Oss/tetgen/Release/bunny.smesh");
-		delete psb;*/
-		}
-	btSoftBody* psb=btSoftBodyHelpers::CreateFromTetGenFile(pdemo->m_softBodyWorldInfo,
-												/*"C:/HomeH/Oss/tetgen/Release/bunny.1.ele",
-												"C:/HomeH/Oss/tetgen/Release/bunny.1.face",
-												"C:/HomeH/Oss/tetgen/Release/bunny.1.node",*/
-												"C:/HomeH/Oss/tetgen/Release/cube.1.ele",
-												0/*"C:/HomeH/Oss/tetgen/Release/cube.1.face"*/,
-												"C:/HomeH/Oss/tetgen/Release/cube.1.node",
-												true,true);
-	pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
-	psb->scale(btVector3(4,4,4));
-	/*psb->rotate(btQuaternion(SIMD_PI/4,SIMD_PI/4,0));
-	psb->translate(btVector3(0,10,0));*/
-	psb->setVolumeMass(30);
-	psb->m_cfg.piterations=1;
-	psb->m_cfg.kKHR=1;
-	//psb->addVelocity(btVector3(0,50,0));
-	//psb->m_tetras.clear();
-	ImplicitSphere	fnc;
-	fnc.center	=	btVector3(4,4,4);
-	fnc.radius	=	4;
-	psb->refine(&fnc,0.001,true);
-	//psb->m_tetras.clear();
-	printf("Nodes:  %u\r\n",psb->m_nodes.size());
-	printf("Links:  %u\r\n",psb->m_links.size());
-	printf("Faces:  %u\r\n",psb->m_faces.size());
-	printf("Tetras: %u\r\n",psb->m_tetras.size());
-	#else
-	
-	#if 1
-	const btScalar	s=4;
-	const int		r=32;
-	const btVector3	p[]={	btVector3(+s,0,-s),
-							btVector3(-s,0,-s),
-							btVector3(+s,0,+s),
-							btVector3(-s,0,+s)};
-	btSoftBody*	psb=btSoftBodyHelpers::CreatePatch(pdemo->m_softBodyWorldInfo,p[0],p[1],p[2],p[3],r,r,1+2+4+8,true);
-	pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
-	psb->m_cfg.piterations=2;
-	/*ImplicitSphere	fnc;
-	fnc.center	=	btVector3(0,0,0);
-	fnc.radius	=	1.5;
-	psb->refine(&fnc,0.001,true);*/
-	//psb->m_faces.clear();
-	/*fnc.center	=	btVector3(4,0,4);
-	fnc.radius	=	2;
-	psb->refine(&fnc,0.001,true);*/
-	#else
-	const btScalar	s=4;
-	const btVector3	p[]={	btVector3(+s,-s,0),
-							btVector3(-s,0,0),
-							btVector3(0,0,+s),
-							btVector3(0,+s,0)};
-	btSoftBody*	psb=new btSoftBody(&pdemo->m_softBodyWorldInfo,4,p,0);
-	psb->appendTetra(0,1,2,3);
-	psb->appendLink(0,1,1,btSoftBody::eLType::Structural);
-	psb->appendLink(1,2,1,btSoftBody::eLType::Structural);
-	psb->appendLink(2,0,1,btSoftBody::eLType::Structural);
-	psb->appendLink(0,3,1,btSoftBody::eLType::Structural);
-	psb->appendLink(1,3,1,btSoftBody::eLType::Structural);
-	psb->appendLink(2,3,1,btSoftBody::eLType::Structural);
-	pdemo->getSoftDynamicsWorld()->addSoftBody(psb);	
-	psb->setSolver(btSoftBody::eSolverPresets::Velocities);
-	psb->m_cfg.viterations=1;
-	psb->m_cfg.diterations=1;
-	psb->m_cfg.kDF=0;
-	psb->m_cfg.kLST=0.000001;
-	//psb1->m_cfg.diterations=1;
-	/*btSoftBody*	psb0=btSoftBodyHelpers::CreateRope(	pdemo->m_softBodyWorldInfo,
-													btVector3(0,0,0),btVector3(5,0,0),16,1);
-	pdemo->getSoftDynamicsWorld()->addSoftBody(psb0);
-	
-	btSoftBody*	psb1=btSoftBodyHelpers::CreateRope(	pdemo->m_softBodyWorldInfo,
-													btVector3(0,0,2),btVector3(5,0,2),16,1);
-	psb1->m_cfg.viterations=1;
-	psb1->m_cfg.diterations=1;
-	psb1->setSolver(btSoftBody::eSolverPresets::Velocities);
-	pdemo->getSoftDynamicsWorld()->addSoftBody(psb1);*/
-	#endif
-	#endif
-	pdemo->toggleIdle();
-}
-
 unsigned	current_demo=0;
 
 void	SoftDemo::clientResetScene()
@@ -977,11 +875,6 @@ void	SoftDemo::clientResetScene()
 	void (*demofncs[])(SoftDemo*)=
 	{
 		Init_Cloth,
-		Init_Cutting1,
-#ifdef BT_SOFTBODY_USE_STL
-		Init_TetraBunny,
-		Init_TetraCube,
-#endif //BT_SOFTBODY_USE_STL
 		Init_Pressure,
 		Init_Volume,
 		Init_Ropes,
@@ -998,6 +891,7 @@ void	SoftDemo::clientResetScene()
 		Init_TorusMatch,
 		Init_Bunny,
 		Init_BunnyMatch,
+		Init_Cutting1,
 	};
 	current_demo=current_demo%(sizeof(demofncs)/sizeof(demofncs[0]));
 
@@ -1012,6 +906,7 @@ void	SoftDemo::clientResetScene()
 	m_autocam						=	false;
 	m_raycast						=	false;
 	m_cutting						=	false;
+	m_results.time					=	SIMD_INFINITY;
 	demofncs[current_demo](this);
 }
 
@@ -1157,53 +1052,106 @@ void	SoftDemo::keyboardCallback(unsigned char key, int x, int y)
 	}
 }
 
+//
+void	SoftDemo::mouseMotionFunc(int x,int y)
+{
+if(m_node&&(m_results.time<SIMD_INFINITY))
+	{
+	if(!m_drag)
+		{
+		#define SQ(_x_) (_x_)*(_x_)
+		if((SQ(x-m_lastmousepos[0])+SQ(y-m_lastmousepos[1]))>6)
+			{
+			m_drag=true;
+			}
+		#undef SQ
+		}
+	if(m_drag)
+		{
+		m_lastmousepos[0]	=	x;
+		m_lastmousepos[1]	=	y;		
+		}
+	}
+	else
+	{
+	DemoApplication::mouseMotionFunc(x,y);
+	}
+}
+
 //
 void	SoftDemo::mouseFunc(int button, int state, int x, int y)
 {
-if(m_cutting&&(state==0)&&(button==0))
+if(button==0)
+	{
+	switch(state)
+		{
+		case	0:
+			{
+			m_results.time=SIMD_INFINITY;
+			DemoApplication::mouseFunc(button,state,x,y);
+			if(!m_pickConstraint)
 				{
 				const btVector3			rayFrom=m_cameraPosition;
 				const btVector3			rayTo=getRayTo(x,y);
 				const btVector3			rayDir=(rayTo-rayFrom).normalized();
 				btSoftBodyArray&		sbs=getSoftDynamicsWorld()->getSoftBodyArray();
-	btSoftBody::sRayCast	results;
-	results.time=SIMD_INFINITY;
 				for(int ib=0;ib<sbs.size();++ib)
 					{
 					btSoftBody*				psb=sbs[ib];
 					btSoftBody::sRayCast	res;
-		if(psb->rayCast(rayFrom,rayDir,res,results.time))
+					if(psb->rayCast(rayFrom,rayDir,res,m_results.time))
 						{
-			results=res;
+						m_results=res;
 						}
 					}
-	if(results.time<SIMD_INFINITY)
+				if(m_results.time<SIMD_INFINITY)
 					{				
-		#if 0
+					m_impact			=	rayFrom+rayDir*m_results.time;
-		const btVector3			x=rayFrom+rayDir*results.time;
+					m_drag				=	false;
-		const btSoftBody::Face& f=results.body->m_faces[results.index];
+					m_lastmousepos[0]	=	x;
-		btScalar				bestarea=SIMD_INFINITY;
+					m_lastmousepos[1]	=	y;
-		const btSoftBody::Node*	n[2]={0,0};
+					m_node				=	0;
-		for(int i=2,j=0;j<3;i=j++)
+					switch(m_results.feature)
 						{
-			const btScalar a2=cross(f.m_n[i]->m_x-x,f.m_n[j]->m_x-x).length2();
+						case	btSoftBody::eFeature::Face:
-			if(a2<bestarea)
 							{
-				bestarea=a2;
+							btSoftBody::Face&	f=m_results.body->m_faces[m_results.index];
-				n[0]=f.m_n[i];
+							m_node=f.m_n[0];
-				n[1]=f.m_n[j];
+							for(int i=1;i<3;++i)
+								{
+								if(	(m_node->m_x-m_impact).length2()>
+									(f.m_n[i]->m_x-m_impact).length2())
+									{
+									m_node=f.m_n[i];
 									}
 								}
-		results.body->cutLink(n[0],n[1],0.5);
+							}
-		#endif
+						break;
-		ImplicitSphere	isphere(rayFrom+rayDir*results.time,1);
+						}
-		printf("Mass before: %f\r\n",results.body->getTotalMass());
+					if(m_node) m_goal=m_node->m_x;
-		results.body->refine(&isphere,0.0001,true);
-		printf("Mass after: %f\r\n",results.body->getTotalMass());
 					return;
 					}
 				}
+			}
+		break;
+		case	1:
+		if((!m_drag)&&m_cutting&&(m_results.time<SIMD_INFINITY))
+			{
+			ImplicitSphere	isphere(m_impact,1);
+			printf("Mass before: %f\r\n",m_results.body->getTotalMass());
+			m_results.body->refine(&isphere,0.0001,true);
+			printf("Mass after: %f\r\n",m_results.body->getTotalMass());
+			}
+		m_results.time=SIMD_INFINITY;
+		m_drag=false;
 		DemoApplication::mouseFunc(button,state,x,y);
+		break;
+		}
+	}
+	else
+	{
+	DemoApplication::mouseFunc(button,state,x,y);
+	}
 }
 
 

Demos/SoftDemo/SoftDemo.h

@@ -57,6 +57,12 @@ public:
 	bool								m_raycast;
 	btScalar							m_animtime;
 	btClock								m_clock;
+	int									m_lastmousepos[2];
+	btVector3							m_impact;
+	btSoftBody::sRayCast				m_results;
+	btSoftBody::Node*					m_node;
+	btVector3							m_goal;
+	bool								m_drag;
 
 
 	//keep the collision shapes, for deletion/cleanup
@@ -80,6 +86,10 @@ public:
 
 	void	exitPhysics();
 
+	SoftDemo() : m_drag(false)
+	{
+
+	}
 	virtual ~SoftDemo()
 	{
 		exitPhysics();
@@ -118,6 +128,7 @@ public:
 	void	renderme();
 	void	keyboardCallback(unsigned char key, int x, int y);
 	void	mouseFunc(int button, int state, int x, int y);
+	void	mouseMotionFunc(int x,int y);
 
 };
 

Extras/CDTestFramework/BulletSAPCompleteBoxPruningTest.cpp

@@ -22,10 +22,319 @@ subject to the following restrictions:
 #include "RenderingHelpers.h"
 #include "GLFontRenderer.h"
 #include "btBulletCollisionCommon.h"
-#include "BulletSoftBody/btDbvtBroadphase.h"
+#include "BulletCollision/BroadphaseCollision/btDbvtBroadphase.h"
+#include "Camera.h"
 
 int numParts =2;
 
+bool	enableCulling	=	true;
+bool	cullFarPlane	=	false;
+bool	showCulling		=	false;
+bool	enableOcclusion	=	false;
+bool	showOcclusion	=	true;
+int		visiblecount	=	0;
+
+struct OcclusionBuffer
+{
+struct WriteOCL
+	{
+	static inline bool Process(btScalar& q,btScalar v) { if(q<v) q=v;return(false); }
+	};
+struct QueryOCL
+	{
+	static inline bool Process(btScalar& q,btScalar v) { return(q<=v); }
+	};
+bool							initialized;
+btAlignedObjectArray<btScalar>	buffer;
+int								sizes[2];
+btScalar						scales[2];
+btScalar						offsets[2];
+btScalar						wtrs[16];
+btVector3						eye;
+btVector3						neardist;
+btScalar						ocarea;
+btScalar						qrarea;
+GLuint							texture;
+			OcclusionBuffer()
+	{
+	initialized=false;
+	neardist=btVector3(2,2,2);
+	ocarea=(btScalar)0;
+	qrarea=(btScalar)0;
+	}
+void		setup(int w,int h)
+	{
+	initialized=true;
+	sizes[0]=w;
+	sizes[1]=h;
+	scales[0]=w/2;
+	scales[1]=h/2;
+	offsets[0]=scales[0]+0.5;
+	offsets[1]=scales[1]+0.5;
+	glGenTextures(1,&texture);
+	clear();
+	}
+void		clear()
+	{
+	buffer.resize(0);
+	buffer.resize(sizes[0]*sizes[1],0);
+	}
+void		initialize()
+	{
+	if(!initialized)
+		{
+		setup(128,128);
+		}
+	GLint		v[4];
+	GLdouble	m[16],p[16];
+	glGetIntegerv(GL_VIEWPORT,v);
+	glGetDoublev(GL_MODELVIEW_MATRIX,m);
+	glGetDoublev(GL_PROJECTION_MATRIX,p);
+	for(int i=0;i<16;++i) wtrs[i]=p[i];
+	clear();
+	}
+void		drawBuffer(	btScalar l,btScalar t,
+						btScalar r,btScalar b)
+	{
+	btAlignedObjectArray<GLubyte>	data;
+	data.resize(buffer.size());
+	for(int i=0;i<data.size();++i)
+		{
+		data[i]=int(32/buffer[i])%255;
+		}
+	glBindTexture(GL_TEXTURE_2D,texture);
+	glDisable(GL_BLEND);
+	glEnable(GL_TEXTURE_2D);
+	glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_WRAP_S,GL_REPEAT);
+	glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_WRAP_T,GL_REPEAT);
+	glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_NEAREST);
+	glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_NEAREST);
+	glTexImage2D(GL_TEXTURE_2D,0,GL_LUMINANCE,sizes[0],sizes[1],0,GL_LUMINANCE,GL_UNSIGNED_BYTE,&data[0]);
+	glBegin(GL_QUADS);
+	glColor4ub(255,255,255,255);
+	glTexCoord2f(0,0);glVertex2f(l,t);
+	glTexCoord2f(1,0);glVertex2f(r,t);
+	glTexCoord2f(1,1);glVertex2f(r,b);
+	glTexCoord2f(0,1);glVertex2f(l,b);
+	glEnd();
+	glDisable(GL_TEXTURE_2D);
+	}
+btVector4	transform(const btVector3& x) const
+	{
+	btVector4	t;
+	t[0]	=	x[0]*wtrs[0]+x[1]*wtrs[4]+x[2]*wtrs[8]+wtrs[12];
+	t[1]	=	x[0]*wtrs[1]+x[1]*wtrs[5]+x[2]*wtrs[9]+wtrs[13];
+	t[2]	=	x[0]*wtrs[2]+x[1]*wtrs[6]+x[2]*wtrs[10]+wtrs[14];
+	t[3]	=	x[0]*wtrs[3]+x[1]*wtrs[7]+x[2]*wtrs[11]+wtrs[15];
+	return(t);
+	}
+static bool	project(btVector4* p,int n)
+	{
+	for(int i=0;i<n;++i)
+		{
+		const btScalar	iw=1/p[i][3];
+		p[i][2]=1/p[i][3];
+		p[i][0]*=p[i][2];
+		p[i][1]*=p[i][2];
+		}
+	return(true);
+	}
+template <const int NP>
+static int	clip(const btVector4* pi,btVector4* po)
+	{
+	btScalar	s[NP];
+	int			m=0;
+	for(int i=0;i<NP;++i)
+		{
+		s[i]=pi[i][2]+pi[i][3];
+		if(s[i]<0) m+=1<<i;
+		}
+	if(m==((1<<NP)-1)) return(0);
+	if(m!=0)
+		{
+		int n=0;
+		for(int i=NP-1,j=0;j<NP;i=j++)
+			{
+			const btVector4&	a=pi[i];
+			const btVector4&	b=pi[j];
+			const btScalar		t=s[i]/(a[3]+a[2]-b[3]-b[2]);
+			if((t>0)&&(t<1))
+				{
+				po[n][0]	=	a[0]+(b[0]-a[0])*t;
+				po[n][1]	=	a[1]+(b[1]-a[1])*t;
+				po[n][2]	=	a[2]+(b[2]-a[2])*t;
+				po[n][3]	=	a[3]+(b[3]-a[3])*t;
+				++n;
+				}
+			if(s[j]>0) po[n++]=b;
+			}
+		return(n);
+		}
+	for(int i=0;i<NP;++i) po[i]=pi[i];
+	return(NP);
+	}
+template <typename POLICY>
+inline bool	draw(	const btVector4& a,
+					const btVector4& b,
+					const btVector4& c,
+					const btScalar minarea)
+	{
+	const btScalar		a2=cross(b-a,c-a)[2];
+	if(a2>0)
+		{
+		if(a2<minarea)	return(true);
+		const int		x[]={	(int)(a.x()*scales[0]+offsets[0]),
+								(int)(b.x()*scales[0]+offsets[0]),
+								(int)(c.x()*scales[0]+offsets[0])};
+		const int		y[]={	(int)(a.y()*scales[1]+offsets[1]),
+								(int)(b.y()*scales[1]+offsets[1]),
+								(int)(c.y()*scales[1]+offsets[1])};
+		const btScalar	z[]={	a.z(),b.z(),c.z()};
+		const int		mix=btMax(0,btMin(x[0],btMin(x[1],x[2])));
+		const int		mxx=btMin(sizes[0],1+btMax(x[0],btMax(x[1],x[2])));
+		const int		miy=btMax(0,btMin(y[0],btMin(y[1],y[2])));
+		const int		mxy=btMin(sizes[1],1+btMax(y[0],btMax(y[1],y[2])));
+		const int		width=mxx-mix;
+		const int		height=mxy-miy;
+		if((width*height)>0)
+			{
+			const int		dx[]={	y[0]-y[1],
+									y[1]-y[2],
+									y[2]-y[0]};
+			const int		dy[]={	x[1]-x[0]-dx[0]*width,
+									x[2]-x[1]-dx[1]*width,
+									x[0]-x[2]-dx[2]*width};
+			const int		a=x[2]*y[0]+x[0]*y[1]-x[2]*y[1]-x[0]*y[2]+x[1]*y[2]-x[1]*y[0];
+			const btScalar	ia=1/(btScalar)a;
+			const btScalar	dzx=ia*(y[2]*(z[1]-z[0])+y[1]*(z[0]-z[2])+y[0]*(z[2]-z[1]));
+			const btScalar	dzy=ia*(x[2]*(z[0]-z[1])+x[0]*(z[1]-z[2])+x[1]*(z[2]-z[0]))-(dzx*width);		
+			int				c[]={	miy*x[1]+mix*y[0]-x[1]*y[0]-mix*y[1]+x[0]*y[1]-miy*x[0],
+									miy*x[2]+mix*y[1]-x[2]*y[1]-mix*y[2]+x[1]*y[2]-miy*x[1],
+									miy*x[0]+mix*y[2]-x[0]*y[2]-mix*y[0]+x[2]*y[0]-miy*x[2]};
+			btScalar		v=ia*((z[2]*c[0])+(z[0]*c[1])+(z[1]*c[2]));
+			btScalar*		scan=&buffer[miy*sizes[1]];
+			for(int iy=miy;iy<mxy;++iy)
+				{
+				for(int ix=mix;ix<mxx;++ix)
+					{
+					if((c[0]>=0)&&(c[1]>=0)&&(c[2]>=0))
+						{
+						if(POLICY::Process(scan[ix],v)) return(true);
+						}
+					c[0]+=dx[0];c[1]+=dx[1];c[2]+=dx[2];v+=dzx;
+					}
+				c[0]+=dy[0];c[1]+=dy[1];c[2]+=dy[2];v+=dzy;
+				scan+=sizes[0];
+				}
+			}
+		}
+	return(false);
+	}
+template <const int NP,typename POLICY>
+inline bool	clipDraw(	const btVector4* p,
+						btScalar minarea)
+	{
+	btVector4	o[NP*2];
+	const int	n=clip<NP>(p,o);
+	bool		earlyexit=false;
+	project(o,n);
+	for(int i=2;i<n;++i)
+		{
+		earlyexit|=draw<POLICY>(o[0],o[i-1],o[i],minarea);
+		}
+	return(earlyexit);
+	}
+void		appendOccluder(	const btVector3& a,
+							const btVector3& b,
+							const btVector3& c)
+	{
+	const btVector4	p[]={transform(a),transform(b),transform(c)};
+	clipDraw<3,WriteOCL>(p,ocarea);
+	}
+void		appendOccluder(	const btVector3& a,
+							const btVector3& b,
+							const btVector3& c,
+							const btVector3& d)
+	{	
+	const btVector4	p[]={transform(a),transform(b),transform(c),transform(d)};
+	clipDraw<4,WriteOCL>(p,ocarea);
+	}
+void		appendOccluder( const btVector3& c,
+							const btVector3& e)
+	{
+	const btVector4	x[]={	transform(btVector3(c[0]-e[0],c[1]-e[1],c[2]-e[2])),
+							transform(btVector3(c[0]+e[0],c[1]-e[1],c[2]-e[2])),
+							transform(btVector3(c[0]+e[0],c[1]+e[1],c[2]-e[2])),
+							transform(btVector3(c[0]-e[0],c[1]+e[1],c[2]-e[2])),
+							transform(btVector3(c[0]-e[0],c[1]-e[1],c[2]+e[2])),
+							transform(btVector3(c[0]+e[0],c[1]-e[1],c[2]+e[2])),
+							transform(btVector3(c[0]+e[0],c[1]+e[1],c[2]+e[2])),
+							transform(btVector3(c[0]-e[0],c[1]+e[1],c[2]+e[2]))};
+	static const int	d[]={	1,0,3,2,
+								4,5,6,7,
+								4,7,3,0,
+								6,5,1,2,
+								7,6,2,3,
+								5,4,0,1};
+	for(int i=0;i<(sizeof(d)/sizeof(d[0]));)
+		{
+		const btVector4	p[]={	x[d[i++]],
+								x[d[i++]],
+								x[d[i++]],
+								x[d[i++]]};
+		clipDraw<4,WriteOCL>(p,ocarea);
+		}	
+	}
+inline bool	queryOccluder(	const btVector3& a,
+							const btVector3& b,
+							const btVector3& c)
+	{	
+	const btVector4	p[]={transform(a),transform(b),transform(c)};
+	return(clipDraw<3,QueryOCL>(p,qrarea));
+	}
+inline bool	queryOccluder(	const btVector3& a,
+							const btVector3& b,
+							const btVector3& c,
+							const btVector3& d)
+	{
+	const btVector4	p[]={transform(a),transform(b),transform(c),transform(d)};
+	return(clipDraw<4,QueryOCL>(p,qrarea));
+	}
+inline bool	queryOccluder(	const btVector3& c,
+							const btVector3& e)
+	{
+	const btVector4	x[]={	transform(btVector3(c[0]-e[0],c[1]-e[1],c[2]-e[2])),
+							transform(btVector3(c[0]+e[0],c[1]-e[1],c[2]-e[2])),
+							transform(btVector3(c[0]+e[0],c[1]+e[1],c[2]-e[2])),
+							transform(btVector3(c[0]-e[0],c[1]+e[1],c[2]-e[2])),
+							transform(btVector3(c[0]-e[0],c[1]-e[1],c[2]+e[2])),
+							transform(btVector3(c[0]+e[0],c[1]-e[1],c[2]+e[2])),
+							transform(btVector3(c[0]+e[0],c[1]+e[1],c[2]+e[2])),
+							transform(btVector3(c[0]-e[0],c[1]+e[1],c[2]+e[2]))};
+	for(int i=0;i<8;++i)
+		{
+		if((x[i][2]+x[i][3])<=0) return(true);
+		}
+	static const int	d[]={	1,0,3,2,
+								4,5,6,7,
+								4,7,3,0,
+								6,5,1,2,
+								7,6,2,3,
+								5,4,0,1};
+	for(int i=0;i<(sizeof(d)/sizeof(d[0]));)
+		{
+		const btVector4	p[]={	x[d[i++]],
+								x[d[i++]],
+								x[d[i++]],
+								x[d[i++]]};
+		if(clipDraw<4,QueryOCL>(p,qrarea)) return(true);
+		}
+	return(false);
+	}
+};
+
+OcclusionBuffer		ocb;
+
 BulletSAPCompleteBoxPruningTest::BulletSAPCompleteBoxPruningTest(int numBoxes,int method) :
 	mBar			(null),
 	mNbBoxes		(numBoxes),
@@ -39,7 +348,7 @@ BulletSAPCompleteBoxPruningTest::BulletSAPCompleteBoxPruningTest(int numBoxes,in
 	btVector3 aabbMax(200,200,200);
 
 	int maxNumBoxes = numBoxes;
-
+	m_isdbvt=false;
 	switch (method)
 	{
 	case 1:
@@ -102,6 +411,7 @@ BulletSAPCompleteBoxPruningTest::BulletSAPCompleteBoxPruningTest(int numBoxes,in
 		break;
 	case	7:
 		m_broadphase	=	new btDbvtBroadphase();
+		m_isdbvt		=	true;
 		methodname		=	"btDbvtBroadphase";
 	break;
 	default:
@@ -122,11 +432,13 @@ BulletSAPCompleteBoxPruningTest::~BulletSAPCompleteBoxPruningTest()
 
 void BulletSAPCompleteBoxPruningTest::Init()
 {
+	btClock		clock;
 	m_firstTime = true;	
 	SRand(0);
 
 
 	mBoxes = new AABB[mNbBoxes];
+	mFlags	=	new bool[mNbBoxes];
 	mBoxPtrs = new const AABB*[mNbBoxes];
 	mBoxTime = new float[mNbBoxes];
 	for(udword i=0;i<mNbBoxes;i++)
@@ -146,11 +458,12 @@ void BulletSAPCompleteBoxPruningTest::Init()
 		btVector3	aabbMax(Center.x+Extents.x,Center.y+Extents.y,Center.z+Extents.z);
 		int shapeType =0;
 		void* userPtr = 0;
-		btBroadphaseProxy* proxy = m_broadphase->createProxy(aabbMin,aabbMax,shapeType,(void*)i,1,1,0,0);//m_dispatcher);
+		btBroadphaseProxy* proxy = m_broadphase->createProxy(aabbMin,aabbMax,shapeType,&mBoxes[i],1,1,0,0);//m_dispatcher);
 		m_proxies.push_back( proxy );
 
 		mBoxTime[i] = 2000.0f*UnitRandomFloat();
 	}
+	printf("Initialization of %s with %u boxes: %ums\r\n",methodname,mNbBoxes,clock.getTimeMilliseconds());
 }
 
 void BulletSAPCompleteBoxPruningTest::Release()
@@ -166,6 +479,23 @@ void BulletSAPCompleteBoxPruningTest::Select()
 		mBar = TwNewBar("OPC_CompleteBoxPruning");
 		TwAddVarRW(mBar, "Speed", TW_TYPE_FLOAT, &mSpeed, " min=0.0 max=0.01 step=0.00001");
 		TwAddVarRW(mBar, "Amplitude", TW_TYPE_FLOAT, &mAmplitude, " min=10.0 max=200.0 step=0.1");
+		if(m_isdbvt)
+			{
+			TwAddVarRW(mBar, "Enable culling",TW_TYPE_BOOLCPP,&enableCulling,"");
+			TwAddVarRW(mBar, "Enable occlusion",TW_TYPE_BOOLCPP,&enableOcclusion,"");
+			TwAddVarRW(mBar, "Show culling",TW_TYPE_BOOLCPP,&showCulling,"");			
+			TwAddVarRW(mBar, "Show occlusion",TW_TYPE_BOOLCPP,&showOcclusion,"");
+			TwAddVarRW(mBar, "Cull far plane",TW_TYPE_BOOLCPP,&cullFarPlane,"");
+			TwAddVarRW(mBar, "OC Min area",TW_TYPE_FLOAT,&ocb.ocarea,"min=0.0 max=1.0 step=0.001");
+			TwAddVarRW(mBar, "QR Min area",TW_TYPE_FLOAT,&ocb.qrarea,"min=0.0 max=1.0 step=0.001");
+			TwAddVarRW(mBar, "Dyn lkhd",TW_TYPE_INT32,&((btDbvtBroadphase*)m_broadphase)->m_sets[0].m_lkhd,"min=-1 max=32");
+			TwAddVarRW(mBar, "Fix lkhd",TW_TYPE_INT32,&((btDbvtBroadphase*)m_broadphase)->m_sets[1].m_lkhd,"min=-1 max=32");
+			TwAddVarRW(mBar, "Dyn opt/f(%)",TW_TYPE_INT32,&((btDbvtBroadphase*)m_broadphase)->m_dupdates,"min=0 max=100");
+			TwAddVarRW(mBar, "Fix opt/f(%)",TW_TYPE_INT32,&((btDbvtBroadphase*)m_broadphase)->m_fupdates,"min=0 max=100");
+			TwAddVarRO(mBar, "Dyn leafs",TW_TYPE_INT32,&((btDbvtBroadphase*)m_broadphase)->m_sets[0].m_leafs,"");
+			TwAddVarRO(mBar, "Fix leafs",TW_TYPE_INT32,&((btDbvtBroadphase*)m_broadphase)->m_sets[1].m_leafs,"");
+			TwAddVarRO(mBar, "Visible",TW_TYPE_INT32,&visiblecount,"");			
+			}
 	}
 	printf("SubMethod: %s\r\n",methodname);
 }
@@ -239,8 +569,7 @@ void BulletSAPCompleteBoxPruningTest::PerformTest()
 
 //	printf("%d pairs colliding\r     ", mPairs.GetNbPairs());
 
-	bool* Flags = (bool*)_alloca(sizeof(bool)*mNbBoxes);
+	ZeroMemory(mFlags,sizeof(bool)*mNbBoxes);
-	ZeroMemory(Flags, sizeof(bool)*mNbBoxes);
 
 	btOverlappingPairCache* pairCache = m_broadphase->getOverlappingPairCache();
 	const btBroadphasePair* pairPtr = pairCache->getOverlappingPairArrayPtr();
@@ -249,8 +578,11 @@ void BulletSAPCompleteBoxPruningTest::PerformTest()
 	{
 //		Flags[pairPtr[i].m_pProxy0->getUid()-1] = true;
 //		Flags[pairPtr[i].m_pProxy1->getUid()-1] = true;
-		Flags[int(pairPtr[i].m_pProxy0->m_clientObject)] = true;
+		int	j;
-		Flags[int(pairPtr[i].m_pProxy1->m_clientObject)] = true;
+		j=((AABB*)pairPtr[i].m_pProxy0->m_clientObject)-mBoxes;
+		mFlags[j] = true;
+		j=((AABB*)pairPtr[i].m_pProxy1->m_clientObject)-mBoxes;
+		mFlags[j] = true;
 	}
 	
 
@@ -304,11 +636,13 @@ void BulletSAPCompleteBoxPruningTest::PerformTest()
 		
 
 	glEnable(GL_DEPTH_TEST);
+	//glDisable(GL_DEPTH_TEST);
 
 	glDisable(GL_BLEND);
 
 	// Render boxes
-	OBB CurrentBox;
+	Render();
+	/*OBB CurrentBox;
 	CurrentBox.mRot.Identity();
 	for(udword i=0;i<mNbBoxes;i++)
 	{
@@ -317,7 +651,7 @@ void BulletSAPCompleteBoxPruningTest::PerformTest()
 		mBoxes[i].GetCenter(CurrentBox.mCenter);
 		mBoxes[i].GetExtents(CurrentBox.mExtents);
 		DrawOBB(CurrentBox);
-	}
+	}*/
 
 	char Buffer[4096];
 	sprintf(Buffer, "CompleteBoxPruning: %5.1f us (%d cycles) : %d pairs\n", mProfiler.mMsTime, mProfiler.mCycles, 
@@ -328,6 +662,286 @@ void BulletSAPCompleteBoxPruningTest::PerformTest()
 	GLFontRenderer::print(10.0f, 10.0f, 0.02f, Buffer);
 }
 
+//
+static void	DrawVolume(const btDbvt::Volume& volume,const btVector3& color)
+{
+const btVector3	mins=volume.Mins();
+const btVector3	maxs=volume.Maxs();
+glColor3f(color.x(),color.y(),color.z());
+glVertex3f(mins.x(),mins.y(),mins.z());
+glVertex3f(maxs.x(),mins.y(),mins.z());
+
+glVertex3f(maxs.x(),mins.y(),mins.z());
+glVertex3f(maxs.x(),maxs.y(),mins.z());
+
+glVertex3f(maxs.x(),maxs.y(),mins.z());
+glVertex3f(mins.x(),maxs.y(),mins.z());
+
+glVertex3f(mins.x(),maxs.y(),mins.z());
+glVertex3f(mins.x(),mins.y(),mins.z());
+
+glVertex3f(mins.x(),mins.y(),maxs.z());
+glVertex3f(maxs.x(),mins.y(),maxs.z());
+
+glVertex3f(maxs.x(),mins.y(),maxs.z());
+glVertex3f(maxs.x(),maxs.y(),maxs.z());
+
+glVertex3f(maxs.x(),maxs.y(),maxs.z());
+glVertex3f(mins.x(),maxs.y(),maxs.z());
+
+glVertex3f(mins.x(),maxs.y(),maxs.z());
+glVertex3f(mins.x(),mins.y(),maxs.z());
+
+glVertex3f(mins.x(),mins.y(),mins.z());
+glVertex3f(mins.x(),mins.y(),maxs.z());
+
+glVertex3f(maxs.x(),mins.y(),mins.z());
+glVertex3f(maxs.x(),mins.y(),maxs.z());
+
+glVertex3f(maxs.x(),maxs.y(),mins.z());
+glVertex3f(maxs.x(),maxs.y(),maxs.z());
+
+glVertex3f(mins.x(),maxs.y(),mins.z());
+glVertex3f(mins.x(),maxs.y(),maxs.z());
+}
+
+//
+void BulletSAPCompleteBoxPruningTest::RenderAll()
+{
+OBB CurrentBox;
+CurrentBox.mRot.Identity();
+for(udword i=0;i<mNbBoxes;i++)
+	{
+	if(mFlags[i])	glColor3f(1.0f, 0.0f, 0.0f);
+	else			glColor3f(0.0f, 1.0f, 0.0f);
+	mBoxes[i].GetCenter(CurrentBox.mCenter);
+	mBoxes[i].GetExtents(CurrentBox.mExtents);
+	DrawOBB(CurrentBox);
+	}
+}
+
+//
+void BulletSAPCompleteBoxPruningTest::Render()
+{
+visiblecount=mNbBoxes;
+if((!m_isdbvt)||(!enableCulling))
+	{	
+	RenderAll();
+	}
+	else
+	{
+	btDbvtBroadphase*	pbp=(btDbvtBroadphase*)m_broadphase;
+	const int			margin=0;
+	const int			lc=margin;
+	const int			rc=glutGet(GLUT_WINDOW_WIDTH)-(1+margin);
+	const int			tc=margin;
+	const int			bc=glutGet(GLUT_WINDOW_HEIGHT)-(1+margin);
+	const btVector3		c00(ComputeWorldRay(lc,tc).x,
+							ComputeWorldRay(lc,tc).y,
+							ComputeWorldRay(lc,tc).z);
+	const btVector3		c10(ComputeWorldRay(rc,tc).x,
+							ComputeWorldRay(rc,tc).y,
+							ComputeWorldRay(rc,tc).z);
+	const btVector3		c01(ComputeWorldRay(lc,bc).x,
+							ComputeWorldRay(lc,bc).y,
+							ComputeWorldRay(lc,bc).z);
+	const btVector3		c11(ComputeWorldRay(rc,bc).x,
+							ComputeWorldRay(rc,bc).y,
+							ComputeWorldRay(rc,bc).z);
+	
+	const btVector3	eye(GetCameraPos().x,GetCameraPos().y,GetCameraPos().z);
+	const btVector3	dir(GetCameraDir().x,GetCameraDir().y,GetCameraDir().z);
+	const btVector3	x00=eye+c00*100;
+	const btVector3	x10=eye+c10*100;
+	const btVector3	x01=eye+c01*100;
+	const btVector3	x11=eye+c11*100;
+	
+	ocb.initialize();
+	ocb.eye=eye;
+	
+	btVector3	planes_n[5];
+	btScalar	planes_o[5];
+	static const btScalar	farplane=200;
+	static const int		nplanes=sizeof(planes_n)/sizeof(planes_n[0]);
+	const int				acplanes=cullFarPlane?5:4;
+	planes_n[0]	=	cross(c01,c00).normalized();
+	planes_n[1]	=	cross(c10,c11).normalized();
+	planes_n[2]	=	cross(c00,c10).normalized();
+	planes_n[3]	=	cross(c11,c01).normalized();
+	planes_n[4]	=	-dir;
+	planes_o[4]	=	-dot(eye+dir*farplane,planes_n[4]);
+	for(int i=0;i<4;++i) planes_o[i]=-dot(eye,planes_n[i]);
+	
+	struct	SceneRenderer : btDbvt::ICollide
+		{
+		int									drawn;
+		BulletSAPCompleteBoxPruningTest*	self;
+		OBB									box;
+		OcclusionBuffer*					ocb;
+				SceneRenderer()
+			{
+			drawn=0;
+			box.mRot.Identity();
+			}
+		bool	Descent(const btDbvt::Node* node)
+			{
+			return(ocb->queryOccluder(node->volume.Center(),node->volume.Extent()));
+			}
+		void	Process(const btDbvt::Node* leaf)
+			{	
+			btBroadphaseProxy*	proxy=(btBroadphaseProxy*)leaf->data;
+			int					i=((AABB*)proxy->m_clientObject)-self->mBoxes;
+			if(self->mFlags[i])	glColor3f(1.0f, 0.0f, 0.0f);
+			else				glColor3f(0.0f, 1.0f, 0.0f);
+			self->mBoxes[i].GetCenter(box.mCenter);
+			self->mBoxes[i].GetExtents(box.mExtents);
+			DrawOBB(box);drawn++;
+			if(ocb)
+				{
+				ocb->appendOccluder(btVector3(box.mCenter.x,box.mCenter.y,box.mCenter.z),
+									btVector3(box.mExtents.x,box.mExtents.y,box.mExtents.z));
+				}
+			}
+		}	srenderer;
+	srenderer.self=this;
+	srenderer.ocb=0;
+	if(enableOcclusion)
+		{
+		srenderer.ocb=&ocb;
+		btDbvt::collideOCL(pbp->m_sets[1].m_root,planes_n,planes_o,dir,acplanes,srenderer);
+		btDbvt::collideOCL(pbp->m_sets[0].m_root,planes_n,planes_o,dir,acplanes,srenderer);		
+		}
+		else
+		{
+		btDbvt::collideKDOP(pbp->m_sets[1].m_root,planes_n,planes_o,acplanes,srenderer);
+		btDbvt::collideKDOP(pbp->m_sets[0].m_root,planes_n,planes_o,acplanes,srenderer);		
+		}
+	visiblecount=srenderer.drawn;
+	if(showOcclusion&&enableOcclusion)
+		{
+		const btScalar		ratio=((float)glutGet(GLUT_WINDOW_HEIGHT))/((float)glutGet(GLUT_WINDOW_WIDTH));	
+		glMatrixMode(GL_PROJECTION);
+		glLoadIdentity();
+		glOrtho(-1,1,-1,1,-1,1);
+		glMatrixMode(GL_MODELVIEW);
+		glLoadIdentity();
+		const float	mm[]={	1,0,0,0,
+							0,1,0,0,
+							0,0,0,1,
+							0,0,0,1};
+		glMultMatrixf(mm);
+		glDisable(GL_DEPTH_TEST);	
+		glDisable(GL_LIGHTING);
+		const float size=0.6f;
+		const float	orgx=0.3f;
+		const float	orgy=0.25f;
+		const float	left=orgx;
+		const float	right=orgx+size;
+		const float	top=orgy+size;
+		const float	bottom=orgy;
+		ocb.drawBuffer(left,bottom,right,top);
+		}
+	if(showCulling)
+		{
+		const btScalar		ratio=((float)glutGet(GLUT_WINDOW_HEIGHT))/((float)glutGet(GLUT_WINDOW_WIDTH));	
+		static const float	scale=0.004;
+		
+		glMatrixMode(GL_PROJECTION);
+		glLoadIdentity();
+		glOrtho(-1,1,-1*ratio,1*ratio,-1,1);
+		glMatrixMode(GL_MODELVIEW);
+		glLoadIdentity();
+		const float	mm[]={	1,0,0,0,
+							0,0,1,0,
+							0,1,0,0,
+							0,0,0,1};
+		glMultMatrixf(mm);
+		glScalef(scale,scale,scale);
+		
+		glDisable(GL_DEPTH_TEST);	
+		glDisable(GL_LIGHTING);	
+		
+		glBegin(GL_LINES);
+		glColor4f(1,1,1,1);	
+		
+		struct	DebugRenderer : btDbvt::ICollide
+		{
+		OcclusionBuffer*	ocb;
+		int					sid;
+		bool	AllLeafs(const btDbvt::Node* node)
+			{
+			Process(node);
+			return(false);
+			}
+		bool	Descent(const btDbvt::Node* node)
+			{
+			return(ocb->queryOccluder(node->volume.Center(),node->volume.Extent()));
+			}
+		void	Process(const btDbvt::Node* node)
+			{
+			if(ocb)
+				{
+				ocb->appendOccluder(node->volume.Center(),node->volume.Extent());
+				}
+			if(sid>=0)
+				{
+				const float f=sid/1023.;
+				DrawVolume(node->volume,btVector3(1,f,f));
+				sid=(sid+1)%1024;
+				}
+				else
+				{
+				if(node->isinternal())
+					DrawVolume(node->volume,btVector3(0,1,0));
+					else
+					DrawVolume(node->volume,btVector3(1,0,1));
+				}
+			}
+		}	drenderer;
+		if(enableOcclusion)
+			{
+			drenderer.ocb=&ocb;
+			drenderer.sid=0;
+			ocb.clear();
+			btDbvt::collideOCL(pbp->m_sets[1].m_root,planes_n,planes_o,dir,acplanes,drenderer);
+			btDbvt::collideOCL(pbp->m_sets[0].m_root,planes_n,planes_o,dir,acplanes,drenderer);			
+			}
+			else
+			{
+			drenderer.ocb=0;
+			drenderer.sid=-1;
+			btDbvt::collideKDOP(pbp->m_sets[1].m_root,planes_n,planes_o,acplanes,drenderer);
+			btDbvt::collideKDOP(pbp->m_sets[0].m_root,planes_n,planes_o,acplanes,drenderer);			
+			}
+		glEnd();
+		
+		glBegin(GL_LINES);
+		glColor4f(1,1,1,1);
+		glVertex3f(eye.x(),eye.y(),eye.z());
+		glVertex3f(x00.x(),x00.y(),x00.z());
+		glVertex3f(eye.x(),eye.y(),eye.z());
+		glVertex3f(x10.x(),x10.y(),x10.z());
+		glVertex3f(eye.x(),eye.y(),eye.z());
+		glVertex3f(x01.x(),x01.y(),x01.z());
+		glVertex3f(eye.x(),eye.y(),eye.z());
+		glVertex3f(x11.x(),x11.y(),x11.z());
+		
+		glVertex3f(x00.x(),x00.y(),x00.z());
+		glVertex3f(x10.x(),x10.y(),x10.z());
+		
+		glVertex3f(x10.x(),x10.y(),x10.z());
+		glVertex3f(x11.x(),x11.y(),x11.z());
+		
+		glVertex3f(x11.x(),x11.y(),x11.z());
+		glVertex3f(x01.x(),x01.y(),x01.z());
+		
+		glVertex3f(x01.x(),x01.y(),x01.z());
+		glVertex3f(x00.x(),x00.y(),x00.z());	
+		glEnd();
+		}
+	}
+}
+
 void BulletSAPCompleteBoxPruningTest::KeyboardCallback(unsigned char key, int x, int y)
 {
 }

Extras/CDTestFramework/CDTestFramework.cpp

@@ -21,17 +21,12 @@ subject to the following restrictions:
 #include "CapsuleMeshQuery.h"
 #include "CompleteBoxPruning.h"
 #include "BulletSAPCompleteBoxPruningTest.h"
-#include "DbvtTest.h"
 #include "BipartiteBoxPruning.h"
 #include "RenderingHelpers.h"
 #include "Terrain.h"
 #include "Camera.h"
 #include "GLFontRenderer.h"
 
-///#define NUM_SAP_BOXES 16384
-//#define NUM_SAP_BOXES 8192
-//#define NUM_SAP_BOXES 4096
-
 #define NUM_SAP_BOXES 8192
 
 int		percentUpdate	=	10;
@@ -64,9 +59,9 @@ enum TestIndex
 //	TEST_OBB_MESH_QUERY,
 //	TEST_CAPSULE_MESH_QUERY,
 //	TEST_COMPLETE_BOX_PRUNING=0,
-	TEST_COMPLETE_BOX_PRUNING_8192,
+ //	TEST_COMPLETE_BOX_PRUNING_8192,
 //	TEST_BULLET_SAP_1024,
-	TEST_BULLET_SAP_8192,
+ //	TEST_BULLET_SAP_8192,
 //	TEST_BULLET_SAP_SORTEDPAIRS_8192,
  	TEST_BULLET_MULTISAP_8192,
 //	TEST_BIPARTITE_BOX_PRUNING,
@@ -291,9 +286,9 @@ int main(int argc, char** argv)
 //			{TEST_OBB_MESH_QUERY, "OBB-mesh query"},
 //			{TEST_CAPSULE_MESH_QUERY, "Capsule-mesh query"},
 //			{TEST_COMPLETE_BOX_PRUNING, "OPCODE SAP 1024"},
-			{TEST_COMPLETE_BOX_PRUNING_8192, "OPCODE SAP 8192"},
+	//		{TEST_COMPLETE_BOX_PRUNING_8192, "OPCODE SAP 8192"},
 //			{TEST_BULLET_SAP_1024, "Bullet SAP HASHPAIR 1024"},
-			{TEST_BULLET_SAP_8192, "Bullet SAP HASHPAIR 8192"},
+	//		{TEST_BULLET_SAP_8192, "Bullet SAP HASHPAIR 8192"},
 //			{TEST_BULLET_SAP_SORTEDPAIRS_8192, "Bullet SAP SORTEDPAIR 8192"},
 			{TEST_BULLET_MULTISAP_8192, "Bullet MultiSAP 8192"},
 //			{TEST_BIPARTITE_BOX_PRUNING, "Bipartite box pruning"},
@@ -310,10 +305,10 @@ int main(int argc, char** argv)
 //	gCollisionTests[TEST_OBB_MESH_QUERY]	= new OBBMeshQuery;
 //	gCollisionTests[TEST_CAPSULE_MESH_QUERY]	= new CapsuleMeshQuery;
 //	gCollisionTests[TEST_COMPLETE_BOX_PRUNING]	= new CompleteBoxPruningTest(NUM_SAP_BOXES);
-	gCollisionTests[TEST_COMPLETE_BOX_PRUNING_8192]	= new CompleteBoxPruningTest(NUM_SAP_BOXES);
+ //	gCollisionTests[TEST_COMPLETE_BOX_PRUNING_8192]	= new CompleteBoxPruningTest(NUM_SAP_BOXES);
 //	gCollisionTests[TEST_COMPLETE_BOX_PRUNING_8192]	= new CompleteBoxPruningTest(NUM_SAP_BOXES);
 //	gCollisionTests[TEST_BULLET_SAP_1024]	= new BulletSAPCompleteBoxPruningTest(NUM_SAP_BOXES,1);
-	gCollisionTests[TEST_BULLET_SAP_8192]	= new BulletSAPCompleteBoxPruningTest(NUM_SAP_BOXES,1);
+ //	gCollisionTests[TEST_BULLET_SAP_8192]	= new BulletSAPCompleteBoxPruningTest(NUM_SAP_BOXES,1);
 //	gCollisionTests[TEST_BULLET_SAP_SORTEDPAIRS_8192]	= new BulletSAPCompleteBoxPruningTest(NUM_SAP_BOXES,3);
  	gCollisionTests[TEST_BULLET_MULTISAP_8192]	= new BulletSAPCompleteBoxPruningTest(NUM_SAP_BOXES,6);
 //	gCollisionTests[TEST_BIPARTITE_BOX_PRUNING]	= new BipartiteBoxPruningTest;

src/BulletCollision/BroadphaseCollision/btDbvt.cpp

@@ -18,6 +18,14 @@ subject to the following restrictions:
 
 //
 typedef btAlignedObjectArray<btDbvt::Node*>			tNodeArray;
+typedef btAlignedObjectArray<const btDbvt::Node*>	tConstNodeArray;
+
+//
+struct btDbvtNodeEnumerator : btDbvt::ICollide
+{
+tConstNodeArray	nodes;
+void Process(const btDbvt::Node* n) { nodes.push_back(n); }
+};
 
 //
 static inline int				indexof(const btDbvt::Node* node)
@@ -46,7 +54,7 @@ static inline btScalar			size(const btDbvt::Volume& a)
 static inline void				deletenode(	btDbvt* pdbvt,
 											btDbvt::Node* node)
 {
-	delete pdbvt->m_free;
+btAlignedFree(pdbvt->m_free);
 pdbvt->m_free=node;
 }
 	
@@ -59,8 +67,7 @@ static inline void				recursedeletenode(	btDbvt* pdbvt,
 	recursedeletenode(pdbvt,node->childs[0]);
 	recursedeletenode(pdbvt,node->childs[1]);
 	}
-	if(node==pdbvt->m_root) 
+if(node==pdbvt->m_root) pdbvt->m_root=0;
-		pdbvt->m_root=0;
 deletenode(pdbvt,node);
 }
 
@@ -74,7 +81,7 @@ static inline btDbvt::Node*		createnode(	btDbvt* pdbvt,
 if(pdbvt->m_free)
 	{ node=pdbvt->m_free;pdbvt->m_free=0; }
 	else
-	{ node=new btDbvt::Node(); }
+	{ node=new(btAlignedAlloc(sizeof(btDbvt::Node),16)) btDbvt::Node(); }
 node->parent	=	parent;
 node->volume	=	volume;
 node->data		=	data;
@@ -112,10 +119,10 @@ static inline void				insertleaf(	btDbvt* pdbvt,
 		node->childs[0]				=	root;root->parent=node;
 		node->childs[1]				=	leaf;leaf->parent=node;
 		do	{
-				if(prev->volume.Contain(node->volume))
+			if(!prev->volume.Contain(node->volume))
-					break;
-				else
 				Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume);
+				else
+				break;
 			node=prev;
 			} while(0!=(prev=node->parent));
 		}
@@ -279,7 +286,7 @@ static btDbvt::Node*			topdown(btDbvt* pdbvt,
 			{
 			if((splitcount[i][0]>0)&&(splitcount[i][1]>0))
 				{
-					const int	midp=static_cast<int>(btFabs(static_cast<btScalar>(splitcount[i][0]-splitcount[i][1])));
+				const int	midp=abs(splitcount[i][0]-splitcount[i][1]);
 				if(midp<bestmidp)
 					{
 					bestaxis=i;
@@ -349,8 +356,9 @@ btDbvt::btDbvt()
 {
 m_root	=	0;
 m_free	=	0;
-	m_lkhd	=	2;
+m_lkhd	=	-1;
 m_leafs	=	0;
+m_opath	=	0;
 }
 
 //
@@ -363,7 +371,7 @@ btDbvt::~btDbvt()
 void			btDbvt::clear()
 {
 if(m_root)	recursedeletenode(this,m_root);
-	delete m_free;
+btAlignedFree(m_free);
 m_free=0;
 }
 
@@ -392,6 +400,25 @@ void			btDbvt::optimizeTopDown(int bu_treshold)
 	}
 }
 
+//
+void			btDbvt::optimizeIncremental(int passes)
+{
+if(m_root&&(passes>0))
+	{
+	do	{
+		Node*		node=m_root;
+		unsigned	bit=0;
+		while(node->isinternal())
+			{
+			node=node->childs[(m_opath>>bit)&1];
+			bit=(bit+1)&(sizeof(unsigned)*8-1);
+			}
+		update(node);
+		++m_opath;
+		} while(--passes);
+	}
+}
+
 //
 btDbvt::Node*	btDbvt::insert(const Volume& volume,void* data)
 {
@@ -406,11 +433,14 @@ void			btDbvt::update(Node* leaf,int lookahead)
 {
 Node*	root=removeleaf(this,leaf);
 if(root)
+	{
+	if(lookahead>=0)
 		{
 		for(int i=0;(i<lookahead)&&root->parent;++i)
 			{
 			root=root->parent;
 			}
+		} else root=m_root;
 	}
 insertleaf(this,root,leaf);
 }
@@ -420,11 +450,14 @@ void			btDbvt::update(Node* leaf,const Volume& volume)
 {
 Node*	root=removeleaf(this,leaf);
 if(root)
+	{
+	if(m_lkhd>=0)
 		{
 		for(int i=0;(i<m_lkhd)&&root->parent;++i)
 			{
 			root=root->parent;
 			}
+		} else root=m_root;
 	}
 leaf->volume=volume;
 insertleaf(this,root,leaf);
@@ -467,28 +500,26 @@ void			btDbvt::remove(Node* leaf)
 }
 
 //
-void			btDbvt::collide(btDbvt* tree,
+void			btDbvt::write(IWriter* iwriter) const
-								ICollide* icollide) const
 {
-	collideGeneric(tree,icollide);
+btDbvtNodeEnumerator	nodes;
-}
+nodes.nodes.reserve(m_leafs*2);
-
+enumNodes(m_root,nodes);
-//
+iwriter->Prepare(m_root,nodes.nodes.size());
-void			btDbvt::collide(btDbvt::Node* node,
+for(int i=0;i<nodes.nodes.size();++i)
-								ICollide* icollide) const
 	{
-	collideGeneric(node,icollide);
+	const Node* n=nodes.nodes[i];
-}
+	int			p=-1;
-
+	if(n->parent) p=nodes.nodes.findLinearSearch(n->parent);
-//
+	if(n->isinternal())
-void			btDbvt::collide(const Volume& volume,
-								ICollide* icollide) const
 		{
-	collideGeneric(volume,icollide);
+		const int	c0=nodes.nodes.findLinearSearch(n->childs[0]);
+		const int	c1=nodes.nodes.findLinearSearch(n->childs[1]);
+		iwriter->WriteNode(n,i,p,c0,c1);
 		}
-
+		else
-//
-void			btDbvt::collide(ICollide* icollide) const
 		{
-	collideGeneric(icollide);
+		iwriter->WriteLeaf(n,i,p);
+		}	
+	}
 }

src/BulletCollision/BroadphaseCollision/btDbvt.h

@@ -20,6 +20,26 @@ subject to the following restrictions:
 #include "LinearMath/btAlignedObjectArray.h"
 #include "LinearMath/btVector3.h"
 
+//
+// Compile time configuration
+//
+
+#ifdef WIN32
+#define	DBVT_USE_TEMPLATE
+#endif
+
+#ifdef DBVT_USE_TEMPLATE
+#define	DBVT_VIRTUAL
+#define DBVT_PREFIX		template <typename T>
+#define DBVT_IPOLICY	T& policy
+#define DBVT_CHECKTYPE	static const ICollide&	typechecker=*(T*)0;
+#else
+#define	DBVT_VIRTUAL	virtual
+#define DBVT_PREFIX
+#define DBVT_IPOLICY	ICollide& policy
+#define DBVT_CHECKTYPE
+#endif
+
 //
 // Defaults volumes
 //
@@ -40,10 +60,16 @@ struct	btDbvtAabbMm
 inline void					Expand(const btVector3 e);
 inline void					SignedExpand(const btVector3 e);
 inline bool					Contain(const btDbvtAabbMm& a) const;
+inline int					Classify(const btVector3& n,btScalar o,int s) const;
+inline btScalar				ProjectMinimum(const btVector3& v,unsigned signs) const;
 inline friend bool			Intersect(	const btDbvtAabbMm& a,
 										const btDbvtAabbMm& b);
 inline friend bool			Intersect(	const btDbvtAabbMm& a,
 										const btVector3& b);
+inline friend bool			Intersect(	const btDbvtAabbMm& a,
+										const btVector3& org,
+										const btVector3& invdir,
+										const unsigned* signs);
 inline friend btScalar		Proximity(	const btDbvtAabbMm& a,
 										const btDbvtAabbMm& b);
 inline friend void			Merge(	const btDbvtAabbMm& a,
@@ -75,30 +101,47 @@ struct	btDbvt
 				};
 		};
 	/* Stack element	*/ 
-	struct	sStkElm
+	struct	sStkNN
 		{
 		const Node*	a;
 		const Node*	b;
-		sStkElm(const Node* na,const Node* nb) : a(na),b(nb) {}
+		sStkNN(const Node* na,const Node* nb) : a(na),b(nb) {}
 		};
-
+	struct	sStkNP
-	// Interfaces
+		{
+		const Node*	node;
+		unsigned	mask;
+		sStkNP(const Node* n,unsigned m) : node(n),mask(m) {}
+		};
+	struct	sStkNPS
+		{
+		const Node*	node;
+		unsigned	mask;
+		btScalar	value;
+		sStkNPS(const Node* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {}
+		};
+	// Policies/Interfaces
 			
 	/* ICollide	*/ 
 	struct	ICollide
 		{		
-		virtual ~ICollide()
+		DBVT_VIRTUAL void	Process(const Node*,const Node*)		{}
+		DBVT_VIRTUAL void	Process(const Node*)					{}
+		DBVT_VIRTUAL bool	Descent(const Node*)					{ return(true); }
+		DBVT_VIRTUAL bool	AllLeafs(const Node*)					{ return(true); }
+		};
+	/* IWriter	*/ 
+	struct	IWriter
 		{
-		}
+		virtual void		Prepare(const Node* root,int numnodes)=0;
-		virtual void	Process(const Node*,const Node*)=0;
+		virtual void		WriteNode(const Node*,int index,int parent,int child0,int child1)=0;
-		virtual void	Process(const Node*)=0;
+		virtual void		WriteLeaf(const Node*,int index,int parent)=0;
-		virtual bool	Descent(const Node*)=0;
 		};
 		
 	// Constants
 	enum	{
-		TREETREE_STACKSIZE		=	128,
+			SIMPLE_STACKSIZE	=	64,
-		VOLUMETREE_STACKSIZE	=	64
+			DOUBLE_STACKSIZE	=	SIMPLE_STACKSIZE*2,
 			};
 		
 	// Fields
@@ -106,6 +149,7 @@ struct	btDbvt
 	Node*			m_free;
 	int				m_lkhd;
 	int				m_leafs;
+	unsigned		m_opath;
 	// Methods
 					btDbvt();
 					~btDbvt();
@@ -113,32 +157,51 @@ struct	btDbvt
 	bool			empty() const { return(0==m_root); }
 	void			optimizeBottomUp();
 	void			optimizeTopDown(int bu_treshold=128);
+	void			optimizeIncremental(int passes);
 	Node*			insert(const Volume& box,void* data);
-	void			update(Node* leaf,int lookahead=1);
+	void			update(Node* leaf,int lookahead=-1);
 	void			update(Node* leaf,const Volume& volume);
 	bool			update(Node* leaf,Volume volume,const btVector3& velocity,btScalar margin);
 	bool			update(Node* leaf,Volume volume,const btVector3& velocity);
 	bool			update(Node* leaf,Volume volume,btScalar margin);
 	void			remove(Node* leaf);
-	void			collide(btDbvt* tree,
+	void			write(IWriter* iwriter) const;
-		ICollide* icollide) const;
+	// DBVT_IPOLICY must support ICollide policy/interface
-	void			collide(btDbvt::Node* node,
+	DBVT_PREFIX
-		ICollide* icollide) const;
+	static void		enumNodes(	const Node* root,
-	void			collide(const Volume& volume,
+								DBVT_IPOLICY);
-		ICollide* icollide) const;
+	DBVT_PREFIX
-	void			collide(const btVector3& org,
+	static void		enumLeafs(	const Node* root,
-		const btVector3& dir,
+								DBVT_IPOLICY);
-		ICollide* icollide) const;
+	DBVT_PREFIX
-	void			collide(ICollide* icollide) const;
+	static void		collideTT(	const Node* root0,
-	// Generics : T must implement ICollide
+								const Node* root1,
-
+								DBVT_IPOLICY);
-	void			collideGeneric(	btDbvt* tree,ICollide* policy) const;
+	DBVT_PREFIX
-
+	static void		collideTV(	const Node* root,
-	void			collideGeneric(	btDbvt::Node* node,ICollide*  policy) const;
+								const Volume& volume,
-
+								DBVT_IPOLICY);
-	void			collideGeneric(const Volume& volume,ICollide*  policy) const;
+	DBVT_PREFIX
-
+	static void		collideRAY(	const Node* root,
-	void			collideGeneric(ICollide*  policy) const;
+								const btVector3& origin,
+								const btVector3& direction,
+								DBVT_IPOLICY);
+	DBVT_PREFIX
+	static void		collideKDOP(const Node* root,
+								const btVector3* normals,
+								const btScalar* offsets,
+								int count,
+								DBVT_IPOLICY);
+	DBVT_PREFIX
+	static void		collideOCL(	const Node* root,
+								const btVector3* normals,
+								const btScalar* offsets,
+								const btVector3& sortaxis,
+								int count,								
+								DBVT_IPOLICY);
+	DBVT_PREFIX
+	static void		collideTU(	const Node* root,
+								DBVT_IPOLICY);
 	//
 	private:
 	btDbvt(const btDbvt&)	{}
@@ -221,6 +284,44 @@ inline bool				btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const
 		(mx.z()>=a.mx.z()));
 }
 
+//
+inline int				btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) const
+{
+btVector3			pi,px;
+switch(s)
+	{
+	case	(0+0+0):	px=btVector3(mi.x(),mi.y(),mi.z());
+						pi=btVector3(mx.x(),mx.y(),mx.z());break;
+	case	(1+0+0):	px=btVector3(mx.x(),mi.y(),mi.z());
+						pi=btVector3(mi.x(),mx.y(),mx.z());break;
+	case	(0+2+0):	px=btVector3(mi.x(),mx.y(),mi.z());
+						pi=btVector3(mx.x(),mi.y(),mx.z());break;
+	case	(1+2+0):	px=btVector3(mx.x(),mx.y(),mi.z());
+						pi=btVector3(mi.x(),mi.y(),mx.z());break;
+	case	(0+0+4):	px=btVector3(mi.x(),mi.y(),mx.z());
+						pi=btVector3(mx.x(),mx.y(),mi.z());break;
+	case	(1+0+4):	px=btVector3(mx.x(),mi.y(),mx.z());
+						pi=btVector3(mi.x(),mx.y(),mi.z());break;
+	case	(0+2+4):	px=btVector3(mi.x(),mx.y(),mx.z());
+						pi=btVector3(mx.x(),mi.y(),mi.z());break;
+	case	(1+2+4):	px=btVector3(mx.x(),mx.y(),mx.z());
+						pi=btVector3(mi.x(),mi.y(),mi.z());break;
+	}
+if((dot(n,px)+o)<0)		return(-1);
+if((dot(n,pi)+o)>=0)	return(+1);
+return(0);
+}
+
+//
+inline btScalar			btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const
+{
+const btVector3*	b[]={&mx,&mi};
+const btVector3		p(	b[(signs>>0)&1]->x(),
+						b[(signs>>1)&1]->y(),
+						b[(signs>>2)&1]->z());
+return(dot(p,v));
+}
+	
 //
 inline bool				Intersect(	const btDbvtAabbMm& a,
 									const btDbvtAabbMm& b)
@@ -259,6 +360,28 @@ inline bool				Intersect(	const btDbvtAabbMm& a,
 		(b.z()<=a.mx.z()));
 }
 
+//
+inline bool				Intersect(	const btDbvtAabbMm& a,
+									const btVector3& org,
+									const btVector3& invdir,
+									const unsigned* signs)
+{
+const btVector3*	bounds[2]={&a.mi,&a.mx};
+btScalar			txmin=(bounds[  signs[0]]->x()-org[0])*invdir[0];
+btScalar			txmax=(bounds[1-signs[0]]->x()-org[0])*invdir[0];
+const btScalar		tymin=(bounds[  signs[1]]->y()-org[1])*invdir[1];
+const btScalar		tymax=(bounds[1-signs[1]]->y()-org[1])*invdir[1];
+if((txmin>tymax)||(tymin>txmax)) return(false);
+if(tymin>txmin) txmin=tymin;
+if(tymax<txmax) txmax=tymax;
+const btScalar		tzmin=(bounds[  signs[2]]->z()-org[2])*invdir[2];
+const btScalar		tzmax=(bounds[1-signs[2]]->z()-org[2])*invdir[2];
+if((txmin>tzmax)||(tzmin>txmax)) return(false);
+if(tzmin>txmin) txmin=tzmin;
+if(tzmax<txmax) txmax=tzmax;
+return(txmax>0);
+}
+	
 //
 inline btScalar			Proximity(	const btDbvtAabbMm& a,
 									const btDbvtAabbMm& b)
@@ -290,27 +413,62 @@ inline bool				NotEqual(	const btDbvtAabbMm& a,
 }
 
 //
-// Generic's
+// Inline's
 //
 
 //
-inline void			btDbvt::collideGeneric(	btDbvt::Node* node,ICollide*  policy) const
+DBVT_PREFIX
+inline void		btDbvt::enumNodes(	const Node* root,
+									DBVT_IPOLICY)
 {
-	if(m_root&&node)
+DBVT_CHECKTYPE
+policy.Process(root);
+if(root->isinternal())
 	{
-		btAlignedObjectArray<sStkElm>	stack;
+	enumNodes(root->childs[0],policy);
-		stack.reserve(TREETREE_STACKSIZE);
+	enumNodes(root->childs[1],policy);
-		stack.push_back(sStkElm(m_root,node));
+	}
+}
+
+//
+DBVT_PREFIX
+inline void		btDbvt::enumLeafs(	const Node* root,
+									DBVT_IPOLICY)
+{
+DBVT_CHECKTYPE
+if(root->isinternal())
+	{
+	enumLeafs(root->childs[0],policy);
+	enumLeafs(root->childs[1],policy);
+	}
+	else
+	{
+	policy.Process(root);
+	}
+}
+
+//
+DBVT_PREFIX
+inline void		btDbvt::collideTT(	const Node* root0,
+									const Node* root1,
+									DBVT_IPOLICY)
+{
+DBVT_CHECKTYPE
+if(root0&&root1)
+	{
+	btAlignedObjectArray<sStkNN>	stack;
+	stack.reserve(DOUBLE_STACKSIZE);
+	stack.push_back(sStkNN(root0,root1));
 	do	{
-			sStkElm	p=stack[stack.size()-1];
+		sStkNN	p=stack[stack.size()-1];
 		stack.pop_back();
 		if(p.a==p.b)
 			{
 			if(p.a->isinternal())
 				{
-					stack.push_back(sStkElm(p.a->childs[0],p.a->childs[0]));
+				stack.push_back(sStkNN(p.a->childs[0],p.a->childs[0]));
-					stack.push_back(sStkElm(p.a->childs[1],p.a->childs[1]));
+				stack.push_back(sStkNN(p.a->childs[1],p.a->childs[1]));
-					stack.push_back(sStkElm(p.a->childs[0],p.a->childs[1]));
+				stack.push_back(sStkNN(p.a->childs[0],p.a->childs[1]));
 				}
 			}
 		else if(Intersect(p.a->volume,p.b->volume))
@@ -319,27 +477,27 @@ inline void			btDbvt::collideGeneric(	btDbvt::Node* node,ICollide*  policy) cons
 				{
 				if(p.b->isinternal())
 					{
-						stack.push_back(sStkElm(p.a->childs[0],p.b->childs[0]));
+					stack.push_back(sStkNN(p.a->childs[0],p.b->childs[0]));
-						stack.push_back(sStkElm(p.a->childs[1],p.b->childs[0]));
+					stack.push_back(sStkNN(p.a->childs[1],p.b->childs[0]));
-						stack.push_back(sStkElm(p.a->childs[0],p.b->childs[1]));
+					stack.push_back(sStkNN(p.a->childs[0],p.b->childs[1]));
-						stack.push_back(sStkElm(p.a->childs[1],p.b->childs[1]));
+					stack.push_back(sStkNN(p.a->childs[1],p.b->childs[1]));
 					}
 					else
 					{
-						stack.push_back(sStkElm(p.a->childs[0],p.b));
+					stack.push_back(sStkNN(p.a->childs[0],p.b));
-						stack.push_back(sStkElm(p.a->childs[1],p.b));
+					stack.push_back(sStkNN(p.a->childs[1],p.b));
 					}
 				}
 				else
 				{
 				if(p.b->isinternal())
 					{
-						stack.push_back(sStkElm(p.a,p.b->childs[0]));
+					stack.push_back(sStkNN(p.a,p.b->childs[0]));
-						stack.push_back(sStkElm(p.a,p.b->childs[1]));
+					stack.push_back(sStkNN(p.a,p.b->childs[1]));
 					}
 					else
 					{
-						policy->Process(p.a,p.b);
+					policy.Process(p.a,p.b);
 					}
 				}
 			}
@@ -348,19 +506,17 @@ inline void			btDbvt::collideGeneric(	btDbvt::Node* node,ICollide*  policy) cons
 }
 
 //
-inline void			btDbvt::collideGeneric(	btDbvt* tree,ICollide* policy) const
+DBVT_PREFIX
+inline void		btDbvt::collideTV(	const Node* root,
+									const Volume& volume,
+									DBVT_IPOLICY)
 {
-	collideGeneric(tree->m_root,policy);
+DBVT_CHECKTYPE
-}
+if(root)
-
-//
-inline void			btDbvt::collideGeneric(const Volume& volume,ICollide* policy) const
-{
-	if(m_root)
 	{
 	btAlignedObjectArray<const Node*>	stack;
-		stack.reserve(VOLUMETREE_STACKSIZE);
+	stack.reserve(SIMPLE_STACKSIZE);
-		stack.push_back(m_root);
+	stack.push_back(root);
 	do	{
 		const Node*	n=stack[stack.size()-1];
 		stack.pop_back();
@@ -373,7 +529,7 @@ inline void			btDbvt::collideGeneric(const Volume& volume,ICollide* policy) cons
 				}
 				else
 				{
-					policy->Process(n);
+				policy.Process(n);
 				}
 			}
 		} while(stack.size()>0);
@@ -381,26 +537,206 @@ inline void			btDbvt::collideGeneric(const Volume& volume,ICollide* policy) cons
 }
 
 //
-
+DBVT_PREFIX
-inline void			btDbvt::collideGeneric(ICollide* policy) const
+inline void		btDbvt::collideRAY(	const Node* root,
+									const btVector3& origin,
+									const btVector3& direction,
+									DBVT_IPOLICY)
 {
-	if(m_root)
+DBVT_CHECKTYPE
+if(root)
+	{
+	const btVector3	normal=direction.normalized();
+	const btVector3	invdir(	1/normal.x(),
+							1/normal.y(),
+							1/normal.z());
+	const unsigned	signs[]={	direction.x()<0,
+								direction.y()<0,
+								direction.z()<0};
+	btAlignedObjectArray<const Node*>	stack;
+	stack.reserve(SIMPLE_STACKSIZE);
+	stack.push_back(root);
+	do	{
+		const Node*	node=stack[stack.size()-1];
+		stack.pop_back();
+		if(Intersect(node->volume,origin,invdir,signs))
+			{
+			if(node->isinternal())
+				{
+				stack.push_back(node->childs[0]);
+				stack.push_back(node->childs[1]);
+				}
+				else
+				{
+				policy.Process(node);
+				}
+			}
+		} while(stack.size());
+	}
+}
+
+//
+DBVT_PREFIX
+inline void		btDbvt::collideKDOP(const Node* root,
+									const btVector3* normals,
+									const btScalar* offsets,
+									int count,
+									DBVT_IPOLICY)
+{
+DBVT_CHECKTYPE
+if(root)
+	{
+	const int						inside=(1<<count)-1;
+	btAlignedObjectArray<sStkNP>	stack;
+	int								signs[sizeof(unsigned)*8];
+	btAssert(count<(sizeof(signs)/sizeof(signs[0])));
+	for(int i=0;i<count;++i)
+		{
+		signs[i]=	((normals[i].x()>=0)?1:0)+
+					((normals[i].y()>=0)?2:0)+
+					((normals[i].z()>=0)?4:0);
+		}
+	stack.reserve(SIMPLE_STACKSIZE);
+	stack.push_back(sStkNP(root,0));
+	do	{
+		sStkNP	se=stack[stack.size()-1];
+		bool	out=false;
+		stack.pop_back();
+		for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
+			{
+			if(0==(se.mask&j))
+				{
+				const int	side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
+				switch(side)
+					{
+					case	-1:	out=true;break;
+					case	+1:	se.mask|=j;break;
+					}
+				}
+			}
+		if(!out)
+			{
+			if((se.mask!=inside)&&(se.node->isinternal()))
+				{
+				stack.push_back(sStkNP(se.node->childs[0],se.mask));
+				stack.push_back(sStkNP(se.node->childs[1],se.mask));
+				}
+				else
+				{
+				if(policy.AllLeafs(se.node)) enumLeafs(se.node,policy);
+				}
+			}
+		} while(stack.size());
+	}
+}
+
+//
+DBVT_PREFIX
+inline void		btDbvt::collideOCL(	const Node* root,
+									const btVector3* normals,
+									const btScalar* offsets,
+									const btVector3& sortaxis,
+									int count,
+									DBVT_IPOLICY)
+{
+DBVT_CHECKTYPE
+if(root)
+	{
+	const unsigned					srtsgns=(sortaxis[0]>=0?1:0)+
+											(sortaxis[1]>=0?2:0)+
+											(sortaxis[2]>=0?4:0);
+	const int						inside=(1<<count)-1;
+	btAlignedObjectArray<sStkNPS>	stack;
+	int								signs[sizeof(unsigned)*8];
+	btAssert(count<(sizeof(signs)/sizeof(signs[0])));
+	for(int i=0;i<count;++i)
+		{
+		signs[i]=	((normals[i].x()>=0)?1:0)+
+					((normals[i].y()>=0)?2:0)+
+					((normals[i].z()>=0)?4:0);
+		}
+	stack.reserve(SIMPLE_STACKSIZE);
+	stack.push_back(sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns)));
+	do	{
+		sStkNPS	se=stack[stack.size()-1];
+		stack.pop_back();
+		if(se.mask!=inside)
+			{
+			bool	out=false;
+			for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
+				{
+				if(0==(se.mask&j))
+					{
+					const int	side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
+					switch(side)
+						{
+						case	-1:	out=true;break;
+						case	+1:	se.mask|=j;break;
+						}
+					}
+				}
+			if(out) continue;
+			}
+		if(policy.Descent(se.node))
+			{
+			if(se.node->isinternal())
+				{
+				for(int i=0;i<2;++i)
+					{
+					const Node* n=se.node->childs[i];
+					int			j=stack.size();
+					sStkNPS		ne(n,se.mask,n->volume.ProjectMinimum(sortaxis,srtsgns));
+					stack.push_back(ne);
+					while((j>0)&&(ne.value>stack[j-1].value))
+						{
+						btSwap(stack[j],stack[j-1]);--j;
+						}
+					}
+				}
+				else
+				{
+				policy.Process(se.node);
+				}
+			}
+		} while(stack.size());
+	}
+}
+
+//
+DBVT_PREFIX
+inline void		btDbvt::collideTU(	const Node* root,
+									DBVT_IPOLICY)
+{
+DBVT_CHECKTYPE
+if(root)
 	{
 	btAlignedObjectArray<const Node*>	stack;
-		stack.reserve(VOLUMETREE_STACKSIZE);
+	stack.reserve(SIMPLE_STACKSIZE);
-		stack.push_back(m_root);
+	stack.push_back(root);
 	do	{
 		const Node*	n=stack[stack.size()-1];
 		stack.pop_back();
-			if(policy->Descent(n))
+		if(policy.Descent(n))
 			{
 			if(n->isinternal())
 				{ stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); }
 				else
-				{ policy->Process(n); }
+				{ policy.Process(n); }
 			}
 		} while(stack.size()>0);
 	}
 }
 
+//
+// PP Cleanup
+//
+
+#ifdef DBVT_USE_TEMPLATE
+	#undef DBVT_USE_TEMPLATE
+#endif
+#undef DBVT_VIRTUAL
+#undef DBVT_PREFIX
+#undef DBVT_IPOLICY
+#undef DBVT_CHECKTYPE
+
 #endif

src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp

@@ -1,19 +1,3 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
-
-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.
-*/
-///btDbvtBroadphase implementation by Nathanael Presson
-
 #include "btDbvtBroadphase.h"
 
 //
@@ -47,19 +31,6 @@ struct	ProfileScope
 // Helpers
 //
 
-//
-static inline int	hash(unsigned int i,unsigned int j)
-{
-	int key=static_cast<int>(((unsigned int)i)|(((unsigned int)j)<<16));
-	key+=~(key<<15);
-	key^= (key>>10);
-	key+= (key<<3);
-	key^= (key>>6);
-	key+=~(key<<11);
-	key^= (key>>16);
-	return(key);
-}
-
 //
 template <typename T>
 static inline void	listappend(T* item,T*& list)
@@ -91,28 +62,24 @@ static inline int	listcount(T* root)
 template <typename T>
 static inline void	clear(T& value)
 {
-	static const T zerodummy;
+static const struct ZeroDummy : T {} zerodummy;
 value=zerodummy;
 }
 
 //
-// Collider
+// Colliders
 //
-struct	btDbvtBroadphaseCollider : btDbvt::ICollide
+
+/* Leaf collider	*/ 
+struct	btDbvtLeafCollider : btDbvt::ICollide
 {
 btDbvtBroadphase*	pbp;
-	int					pid;
+btDbvtProxy*		ppx;
-	btDbvtBroadphaseCollider(btDbvtBroadphase* p,int id) : pbp(p),pid(id) {}
+		btDbvtLeafCollider(btDbvtBroadphase* p,btDbvtProxy* px) : pbp(p),ppx(px) {}
-
+void	Process(const btDbvt::Node* na)
-	virtual void	Process(const btDbvt::Node* /*na*/)
 	{
-	}
+	const btDbvt::Node*	nb=ppx->leaf;
-	virtual bool	Descent(const btDbvt::Node*)
+	if(nb!=na)
-	{
-		return false;
-	}
-
-	virtual void	Process(const btDbvt::Node* na,const btDbvt::Node* nb)
 		{
 		btDbvtProxy*		pa=(btDbvtProxy*)na->data;
 		btDbvtProxy*		pb=(btDbvtProxy*)nb->data;
@@ -120,9 +87,28 @@ struct	btDbvtBroadphaseCollider : btDbvt::ICollide
 		if(Intersect(pa->aabb,pb->aabb))
 		#endif
 			{
-			btBroadphasePair*	pp=pbp->m_paircache->addOverlappingPair(pa,pb);
+			if(pa>pb) btSwap(pa,pb);
-			if(pp) 
+			pbp->m_paircache->addOverlappingPair(pa,pb);
-				pp->m_userInfo=*(void**)&pid;
+			}
+		}
+	}
+};
+
+/* Tree collider	*/ 
+struct	btDbvtTreeCollider : btDbvt::ICollide
+{
+btDbvtBroadphase*	pbp;
+		btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {}
+void	Process(const btDbvt::Node* na,const btDbvt::Node* nb)
+	{
+	btDbvtProxy*	pa=(btDbvtProxy*)na->data;
+	btDbvtProxy*	pb=(btDbvtProxy*)nb->data;
+	#if DBVT_BP_DISCRETPAIRS
+	if(Intersect(pa->aabb,pb->aabb))
+	#endif
+		{
+		if(pa>pb) btSwap(pa,pb);
+		pbp->m_paircache->addOverlappingPair(pa,pb);
 		}
 	}
 };
@@ -132,17 +118,15 @@ struct	btDbvtBroadphaseCollider : btDbvt::ICollide
 //
 
 //
-btDbvtBroadphase::btDbvtBroadphase()
+btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache)
 {
-	m_invalidPair		=	0;
+m_releasepaircache	=	(paircache!=0)?false:true;
-	m_fcursor			=	0;
-	m_dcursor			=	0;
 m_stageCurrent		=	0;
 m_fupdates			=	1;
-	m_dupdates			=	0;
+m_dupdates			=	1;
-	//m_paircache			=	new btSortedOverlappingPairCache();
+m_paircache			=	paircache?
-	m_paircache			=	new btHashedOverlappingPairCache();
+							paircache	:
-	
+							new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache();
 m_gid				=	0;
 m_pid				=	0;
 for(int i=0;i<=STAGECOUNT;++i)
@@ -157,7 +141,7 @@ btDbvtBroadphase::btDbvtBroadphase()
 //
 btDbvtBroadphase::~btDbvtBroadphase()
 {
-	delete m_paircache;
+if(m_releasepaircache) btAlignedFree(m_paircache);
 }
 
 //
@@ -170,7 +154,9 @@ btBroadphaseProxy*				btDbvtBroadphase::createProxy(	const btVector3& aabbMin,
 																btDispatcher* /*dispatcher*/,
 																void* /*multiSapProxy*/)
 {
-	btDbvtProxy*	proxy=new btDbvtProxy(userPtr,collisionFilterGroup,collisionFilterMask);
+btDbvtProxy*	proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy(	userPtr,
+																				collisionFilterGroup,
+																				collisionFilterMask);
 proxy->aabb			=	btDbvtAabbMm::FromMM(aabbMin,aabbMax);
 proxy->leaf			=	m_sets[0].insert(proxy->aabb,proxy);
 proxy->stage		=	m_stageCurrent;
@@ -190,7 +176,7 @@ void							btDbvtBroadphase::destroyProxy(	btBroadphaseProxy* absproxy,
 	m_sets[0].remove(proxy->leaf);
 listremove(proxy,m_stageRoots[proxy->stage]);
 m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher);
-	delete proxy;
+btAlignedFree(proxy);
 }
 
 //
@@ -205,12 +191,22 @@ void							btDbvtBroadphase::setAabb(		btBroadphaseProxy* absproxy,
 	{/* fixed -> dynamic set	*/ 
 	m_sets[1].remove(proxy->leaf);
 	proxy->leaf=m_sets[0].insert(aabb,proxy);
-		m_fcursor=0;
 	}
 	else
 	{/* dynamic set				*/ 
+	if(Intersect(proxy->leaf->volume,aabb))
+		{/* Moving				*/ 
 		const btVector3	delta=(aabbMin+aabbMax)/2-proxy->aabb.Center();
+		#ifdef DBVT_BP_MARGIN
 		m_sets[0].update(proxy->leaf,aabb,delta*PREDICTED_FRAMES,DBVT_BP_MARGIN);
+		#else
+		m_sets[0].update(proxy->leaf,aabb,delta*PREDICTED_FRAMES);
+		#endif
+		}
+		else
+		{/* Teleporting			*/ 
+		m_sets[0].update(proxy->leaf,aabb);		
+		}	
 	}
 listremove(proxy,m_stageRoots[proxy->stage]);
 proxy->aabb		=	aabb;
@@ -226,6 +222,8 @@ void							btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 if(0==(m_pid%DBVT_BP_PROFILING_RATE))
 	{	
 	printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leafs,m_sets[0].m_leafs,m_paircache->getNumOverlappingPairs());
+	printf("mode:    %s\r\n",m_mode==MODE_FULL?"full":"incremental");
+	printf("cleanup: %s\r\n",m_cleanupmode==CLEANUP_FULL?"full":"incremental");
 	unsigned int	total=m_profiling.m_total;
 	if(total<=0) total=1;
 	printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE);
@@ -236,6 +234,7 @@ void							btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 							m_profiling.m_fdcollide+
 							m_profiling.m_cleanup;
 	printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE);
+	printf("job counts: %u%%\r\n",(m_profiling.m_jobcount*100)/((m_sets[0].m_leafs+m_sets[1].m_leafs)*DBVT_BP_PROFILING_RATE));
 	clear(m_profiling);
 	m_clock.reset();
 	}
@@ -246,164 +245,58 @@ void							btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 void							btDbvtBroadphase::collide(btDispatcher* dispatcher)
 {
 SPC(m_profiling.m_total);
-	/* refine dynamic		*/ 
+/* optimize				*/ 
-	if(m_stageRoots[m_stageCurrent]&&(m_dupdates>0))
+m_sets[0].optimizeIncremental(1+(m_sets[0].m_leafs*m_dupdates)/100);
-	{
+m_sets[1].optimizeIncremental(1+(m_sets[1].m_leafs*m_fupdates)/100);
-		const int	count=1+(m_sets[0].m_leafs*m_dupdates)/100;
-		for(int i=0;i<count;++i)
-		{
-			if(!m_dcursor) 
-				m_dcursor=m_stageRoots[m_stageCurrent];
-			m_sets[0].update(m_dcursor->leaf);
-			m_dcursor=m_dcursor->links[1];
-		}
-	}
 /* dynamic -> fixed set	*/ 
 m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT;
 btDbvtProxy*	current=m_stageRoots[m_stageCurrent];
 if(current)
 	{
-		btDbvtBroadphaseCollider	collider(this,m_pid);
+	btDbvtTreeCollider	collider(this);
 	do	{
 		btDbvtProxy*	next=current->links[1];
-			if(m_dcursor==current) m_dcursor=0;
 		listremove(current,m_stageRoots[current->stage]);
 		listappend(current,m_stageRoots[STAGECOUNT]);
-			m_sets[1].collideGeneric(current->leaf,&collider);
+		btDbvt::collideTT(m_sets[1].m_root,current->leaf,collider);
 		m_sets[0].remove(current->leaf);
 		current->leaf	=	m_sets[1].insert(current->aabb,current);
 		current->stage	=	STAGECOUNT;	
 		current			=	next;
 		} while(current);
 	}
-	/* refine fixed			*/ 
-	if(m_stageRoots[STAGECOUNT]&&(m_fupdates>0))
-	{
-		const int	count=1+(m_sets[1].m_leafs*m_fupdates)/100;
-		for(int i=0;i<count;++i)
-		{
-			if(!m_fcursor) m_fcursor=m_stageRoots[STAGECOUNT];
-			m_sets[1].update(m_fcursor->leaf);
-			m_fcursor=m_fcursor->links[1];		
-		}
-	}
 /* collide dynamics		*/ 
-	btDbvtBroadphaseCollider	collider(this,m_pid);
+	{
+	btDbvtTreeCollider	collider(this);
 		{
 		SPC(m_profiling.m_fdcollide);
-		m_sets[0].collideGeneric(&m_sets[1],&collider);
+		btDbvt::collideTT(m_sets[0].m_root,m_sets[1].m_root,collider);
 		}
 		{
 		SPC(m_profiling.m_ddcollide);
-		m_sets[0].collideGeneric(&m_sets[0],&collider);
+		btDbvt::collideTT(m_sets[0].m_root,m_sets[0].m_root,collider);
+		}
 	}
 /* clean up				*/ 
-	///m_paircache->processAllOverlappingPairs(0,dispatcher);
 	{
 	SPC(m_profiling.m_cleanup);
-		if (!m_paircache->hasDeferredRemoval())
-		{
 	btBroadphasePairArray&	pairs=m_paircache->getOverlappingPairArray();
+	if(pairs.size()>0)
+		{
 		for(int i=0,ni=pairs.size();i<ni;++i)
 			{
 			btBroadphasePair&	p=pairs[i];
-				if(m_pid!=(*(int*)&p.m_userInfo))
-				{
 			btDbvtProxy*	pa=(btDbvtProxy*)p.m_pProxy0;
 			btDbvtProxy*	pb=(btDbvtProxy*)p.m_pProxy1;
 			if(!Intersect(pa->aabb,pb->aabb))
 				{
+				if(pa>pb) btSwap(pa,pb);
 				m_paircache->removeOverlappingPair(pa,pb,dispatcher);
 				--ni;--i;
 				}
 			}
 		}
-		} else
-		{
-			if ( m_paircache->hasDeferredRemoval())
-			{
-				
-				btBroadphasePairArray&	overlappingPairArray = m_paircache->getOverlappingPairArray();
-
-				//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
-				overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
-
-				overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
-				m_invalidPair = 0;
-
-
-				btBroadphasePair previousPair;
-				previousPair.m_pProxy0 = 0;
-				previousPair.m_pProxy1 = 0;
-				previousPair.m_algorithm = 0;
-				
-				
-				int i;
-				for (i=0;i<overlappingPairArray.size();i++)
-				{
-				
-					btBroadphasePair& pair = overlappingPairArray[i];
-
-					bool isDuplicate = (pair == previousPair);
-
-					previousPair = pair;
-
-					bool needsRemoval = false;
-
-					if (!isDuplicate)
-					{
-						btDbvtProxy*	pa=(btDbvtProxy*)pair.m_pProxy0;
-						btDbvtProxy*	pb=(btDbvtProxy*)pair.m_pProxy1;
-
-						bool hasOverlap = Intersect(pa->aabb,pb->aabb);
-
-						if (hasOverlap)
-						{
-							needsRemoval = false;//callback->processOverlap(pair);
-						} else
-						{
-							needsRemoval = true;
 	}
-					} else
-					{
-						//remove duplicate
-						needsRemoval = true;
-						//should have no algorithm
-						btAssert(!pair.m_algorithm);
-					}
-					
-					if (needsRemoval)
-					{
-						m_paircache->cleanOverlappingPair(pair,dispatcher);
-
-				//		m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
-				//		m_overlappingPairArray.pop_back();
-						pair.m_pProxy0 = 0;
-						pair.m_pProxy1 = 0;
-						m_invalidPair++;
-						
-					} 
-					
-				}
-
-			///if you don't like to skip the invalid pairs in the array, execute following code:
-			#define CLEAN_INVALID_PAIRS 1
-			#ifdef CLEAN_INVALID_PAIRS
-
-				//perform a sort, to sort 'invalid' pairs to the end
-				overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
-
-				overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
-				m_invalidPair = 0;
-			#endif//CLEAN_INVALID_PAIRS
-
-			}
-		}
-
-
-	}
-
-
 ++m_pid;
 }
 

src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h

@@ -13,30 +13,22 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 ///btDbvtBroadphase implementation by Nathanael Presson
-
 #ifndef BT_DBVT_BROADPHASE_H
 #define BT_DBVT_BROADPHASE_H
 
-#include "btDbvt.h"
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
-#include "LinearMath/btPoint3.h"
-#include "LinearMath/btVector3.h"
 #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
-#include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
-#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
-#include "BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h"
-#include "BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h"
 
 //
 // Compile time config
 //
 
-//#define	DBVT_BP_PROFILE			1
+#define	DBVT_BP_PROFILE			0
-
+#define DBVT_BP_DISCRETPAIRS	1
-#define DBVT_BP_DISCRETPAIRS	0
 #define DBVT_BP_MARGIN			(btScalar)0.05
 
 #if DBVT_BP_PROFILE
-#define	DBVT_BP_PROFILING_RATE	50
+	#define	DBVT_BP_PROFILING_RATE	256
 	#include "LinearMath/btQuickprof.h"
 #endif
 
@@ -58,6 +50,8 @@ struct btDbvtProxy : btBroadphaseProxy
 	}
 };
 
+typedef btAlignedObjectArray<btDbvtProxy*>	btDbvtProxyArray;
+
 //
 // btDbvtBroadphase
 //
@@ -73,16 +67,13 @@ struct	btDbvtBroadphase : btBroadphaseInterface
 /* Fields		*/ 
 btDbvt					m_sets[2];					// Dbvt sets
 btDbvtProxy*			m_stageRoots[STAGECOUNT+1];	// Stages list
-	int						m_stageCurrent;				// Current stage
 btOverlappingPairCache*	m_paircache;				// Pair cache
-	btDbvtProxy*			m_fcursor;					// Current fixed cursor
+int						m_stageCurrent;				// Current stage
-	btDbvtProxy*			m_dcursor;					// Current dynamic cursor
 int						m_fupdates;					// % of fixed updates per frame
 int						m_dupdates;					// % of dynamic updates per frame
 int						m_pid;						// Parse id
 int						m_gid;						// Gen id
-	int						m_invalidPair;
+bool					m_releasepaircache;			// Release pair cache on delete
-
 #if DBVT_BP_PROFILE
 btClock					m_clock;
 struct	{
@@ -90,10 +81,11 @@ struct	btDbvtBroadphase : btBroadphaseInterface
 		unsigned long		m_ddcollide;
 		unsigned long		m_fdcollide;
 		unsigned long		m_cleanup;
+		unsigned long		m_jobcount;
 		}				m_profiling;
 #endif
 /* Methods		*/ 
-	btDbvtBroadphase();
+btDbvtBroadphase(btOverlappingPairCache* paircache=0);
 ~btDbvtBroadphase();
 void							collide(btDispatcher* dispatcher);
 /* btBroadphaseInterface Implementation	*/ 

src/BulletSoftBody/btSoftBody.cpp

@@ -15,13 +15,7 @@ subject to the following restrictions:
 ///btSoftBody implementation by Nathanael Presson
 
 #include "btSoftBody.h"
-#if	1
-#include <stdio.h>
-#define	DOTRACE
-#endif
-#include <string.h>
 #include "LinearMath/btQuickprof.h"
-#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
 
@@ -136,17 +130,6 @@ void	btSoftBodyCollisionShape::processAllTriangles(btTriangleCallback* /*callbac
 //
 
 //
-#ifdef DOTRACE
-static inline void			Trace(const btMatrix3x3& m,const char* name)
-{
-	printf("%s[0]: %.2f,\t%.2f,\t%.2f\r\n",name,m[0].x(),m[0].y(),m[0].z());
-	printf("%s[1]: %.2f,\t%.2f,\t%.2f\r\n",name,m[1].x(),m[1].y(),m[1].z());
-	printf("%s[2]: %.2f,\t%.2f,\t%.2f\r\n",name,m[2].x(),m[2].y(),m[2].z());
-	printf("\r\n");
-}
-#else
-static inline void			Trace(const btMatrix3x3&,const char*) {}
-#endif
 
 //
 template <typename T>
@@ -199,6 +182,10 @@ template <typename T>
 static inline T				Sign(const T& x)
 { return((T)(x<0?-1:+1)); }
 //
+template <typename T>
+static inline bool			SameSign(const T& x,const T& y)
+{ return((x*y)>0); }
+//
 static inline btMatrix3x3	ScaleAlongAxis(const btVector3& a,btScalar s)
 {
 	const btScalar	xx=a.x()*a.x();
@@ -456,17 +443,6 @@ static void					PointersToIndices(btSoftBody* psb)
 			psb->m_faces[i].m_leaf->data=*(void**)&i;
 			}
 	}
-	for(int i=0,ni=psb->m_tetras.size();i<ni;++i)
-	{
-		psb->m_tetras[i].m_n[0]=PTR2IDX(psb->m_tetras[i].m_n[0],base);
-		psb->m_tetras[i].m_n[1]=PTR2IDX(psb->m_tetras[i].m_n[1],base);
-		psb->m_tetras[i].m_n[2]=PTR2IDX(psb->m_tetras[i].m_n[2],base);
-		psb->m_tetras[i].m_n[3]=PTR2IDX(psb->m_tetras[i].m_n[3],base);
-		if(psb->m_tetras[i].m_leaf)
-			{
-			psb->m_tetras[i].m_leaf->data=*(void**)&i;
-			}
-	}
 	for(int i=0,ni=psb->m_anchors.size();i<ni;++i)
 		{
 		psb->m_anchors[i].m_node=PTR2IDX(psb->m_anchors[i].m_node,base);
@@ -509,17 +485,6 @@ static void					IndicesToPointers(btSoftBody* psb,const int* map=0)
 			psb->m_faces[i].m_leaf->data=&psb->m_faces[i];
 			}
 	}
-	for(int i=0,ni=psb->m_tetras.size();i<ni;++i)
-	{
-		psb->m_tetras[i].m_n[0]=IDX2PTR(psb->m_tetras[i].m_n[0],base);
-		psb->m_tetras[i].m_n[1]=IDX2PTR(psb->m_tetras[i].m_n[1],base);
-		psb->m_tetras[i].m_n[2]=IDX2PTR(psb->m_tetras[i].m_n[2],base);
-		psb->m_tetras[i].m_n[3]=IDX2PTR(psb->m_tetras[i].m_n[3],base);
-		if(psb->m_tetras[i].m_leaf)
-			{
-			psb->m_tetras[i].m_leaf->data=&psb->m_tetras[i];
-			}
-	}
 	for(int i=0,ni=psb->m_anchors.size();i<ni;++i)
 		{
 		psb->m_anchors[i].m_node=IDX2PTR(psb->m_anchors[i].m_node,base);
@@ -605,11 +570,10 @@ static inline btScalar		RayTriangle(const btVector3& org,
 // Private implementation
 //
 
-struct	RayCaster : public btDbvt::ICollide
+struct	RayCaster : btDbvt::ICollide
 	{
 	btVector3			o;
 	btVector3			d;
-	btVector3			nd;
 	btScalar			mint;
 	btSoftBody::Face*	face;
 	int					tests;
@@ -617,18 +581,11 @@ struct	RayCaster : public btDbvt::ICollide
 		{
 		o		=	org;
 		d		=	dir;
-		nd		=	dir.normalized();	
 		mint	=	mxt;
 		face	=	0;
 		tests	=	0;
 		}
-
+	void	Process(const btDbvt::Node* leaf)
-	virtual void	Process(const btDbvt::Node* /*a*/,const btDbvt::Node* /*b*/)
-	{
-
-	}
-
-	virtual void	Process(const btDbvt::Node* leaf)
 		{
 		btSoftBody::Face&	f=*(btSoftBody::Face*)leaf->data;
 		const btScalar		t=RayTriangle(	o,d,
@@ -643,13 +600,6 @@ struct	RayCaster : public btDbvt::ICollide
 			}
 		++tests;
 		}
-	virtual bool	Descent(const btDbvt::Node* node)
-		{
-		const btVector3	ctr=node->volume.Center()-o;		
-		const btScalar	sqr=node->volume.Lengths().length2()/4;
-		const btScalar	prj=dot(ctr,nd);
-		return((ctr-(nd*prj)).length2()<=sqr);
-		}
 	};
 
 
@@ -665,38 +615,6 @@ static int		RaycastInternal(const btSoftBody* psb,
 	int	cnt=0;
 	if(bcountonly||psb->m_fdbvt.empty())
 		{/* Full search	*/ 
-		if(!psb->m_faces.size())
-			{/* Tetras		*/ 
-			for(int i=0,ni=psb->m_tetras.size();i<ni;++i)
-				{
-				const btSoftBody::Tetra&		t=psb->m_tetras[i];
-				const btSoftBody::Node* const*	n=t.m_n;
-				const int						f[]={	0,1,2,
-														0,1,3,
-														1,2,3,
-														2,0,3};
-				for(int j=0;j<12;j+=3)
-					{
-					const btScalar	t=RayTriangle(	org,dir,
-													n[f[j+0]]->m_x,
-													n[f[j+1]]->m_x,
-													n[f[j+2]]->m_x,
-													mint);
-					if(t>0)
-						{
-						++cnt;
-						if(!bcountonly)
-							{
-							feature=btSoftBody::eFeature::Tetra;
-							index=-1;
-							mint=t;
-							}
-						}
-					}
-				}
-			}
-			else
-			{/* Faces		*/ 
 		for(int i=0,ni=psb->m_faces.size();i<ni;++i)
 			{
 			const btSoftBody::Face&	f=psb->m_faces[i];
@@ -717,11 +635,10 @@ static int		RaycastInternal(const btSoftBody* psb,
 				}
 			}
 		}
-		}
 		else
 		{/* Use dbvt	*/ 
 		RayCaster	collider(org,dir,mint);
-		psb->m_fdbvt.collideGeneric(&collider);
+		btDbvt::collideRAY(psb->m_fdbvt.m_root,org,dir,collider);
 		if(collider.face)
 			{
 			mint=collider.mint;
@@ -742,7 +659,6 @@ for(int i=0;i<psb->m_faces.size();++i)
 	btSoftBody::Face&	f=psb->m_faces[i];
 	f.m_leaf=psb->m_fdbvt.insert(VolumeOf(f,0),&f);
 	}
-psb->m_fdbvt.optimizeTopDown();
 }
 
 //
@@ -910,15 +826,6 @@ static void			UpdateConstants(btSoftBody* psb)
 		btSoftBody::Face&	f=psb->m_faces[i];
 		f.m_ra	=	AreaOf(f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x);
 	}
-	/* Tetras		*/ 
-	for(int i=0,ni=psb->m_tetras.size();i<ni;++i)
-	{
-		btSoftBody::Tetra&		t=psb->m_tetras[i];
-		btSoftBody::Material&	m=*t.m_material;
-		btSoftBody::Node**		n=t.m_n;
-		t.m_rv	=	VolumeOf(n[0]->m_x,n[1]->m_x,n[2]->m_x,n[3]->m_x);
-		t.m_c1	=	(4*m.m_kVST)/(n[0]->m_im+n[1]->m_im+n[2]->m_im+n[3]->m_im);
-	}
 	/* Area's		*/ 
 	btAlignedObjectArray<int>	counts;
 	counts.resize(psb->m_nodes.size(),0);
@@ -1081,9 +988,9 @@ static void			ApplyForces(btSoftBody* psb,btScalar dt)
 }
 
 //
-static void			PSolve_Anchors(btSoftBody* psb)
+static void			PSolve_Anchors(btSoftBody* psb,btScalar kst)
 {
-	const btScalar	kAHR=psb->m_cfg.kAHR;
+	const btScalar	kAHR=psb->m_cfg.kAHR*kst;
 	const btScalar	dt=psb->m_sst.sdt;
 	for(int i=0,ni=psb->m_anchors.size();i<ni;++i)
 	{
@@ -1101,9 +1008,10 @@ static void			PSolve_Anchors(btSoftBody* psb)
 }
 
 //
-static void			PSolve_RContacts(btSoftBody* psb)
+static void			PSolve_RContacts(btSoftBody* psb,btScalar kst)
 {
 	const btScalar	dt=psb->m_sst.sdt;
+	const btScalar	mrg=psb->getCollisionShape()->getMargin();
 	for(int i=0,ni=psb->m_rcontacts.size();i<ni;++i)
 	{
 		const btSoftBody::RContact&	c=psb->m_rcontacts[i];
@@ -1114,9 +1022,9 @@ static void			PSolve_RContacts(btSoftBody* psb)
 		const btScalar		dn=dot(vr,cti.m_normal);		
 		if(dn<=SIMD_EPSILON)
 		{
-			const btScalar		dp=dot(c.m_node->m_x,cti.m_normal)+cti.m_offset;
+			const btScalar		dp=btMin(dot(c.m_node->m_x,cti.m_normal)+cti.m_offset,mrg);
 			const btVector3		fv=vr-cti.m_normal*dn;
-			const btVector3		impulse=c.m_c0*(vr-fv*c.m_c3+cti.m_normal*(dp*c.m_c4));
+			const btVector3		impulse=c.m_c0*((vr-fv*c.m_c3+cti.m_normal*(dp*c.m_c4))*kst);
 			c.m_node->m_x-=impulse*c.m_c2;
 			c.m_cti.m_body->applyImpulse(impulse,c.m_c1);
 		}
@@ -1124,7 +1032,7 @@ static void			PSolve_RContacts(btSoftBody* psb)
 }
 
 //
-static void			PSolve_SContacts(btSoftBody* psb)
+static void			PSolve_SContacts(btSoftBody* psb,btScalar)
 {
 for(int i=0,ni=psb->m_scontacts.size();i<ni;++i)
 	{
@@ -1156,7 +1064,7 @@ for(int i=0,ni=psb->m_scontacts.size();i<ni;++i)
 }
 
 //
-static void			PSolve_Links(btSoftBody* psb)
+static void			PSolve_Links(btSoftBody* psb,btScalar kst)
 {
 	for(int i=0,ni=psb->m_links.size();i<ni;++i)
 	{			
@@ -1167,7 +1075,7 @@ static void			PSolve_Links(btSoftBody* psb)
 			btSoftBody::Node&	b=*l.m_n[1];
 			const btVector3		del=b.m_x-a.m_x;
 			const btScalar		len=del.length2();
-			const btScalar		k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)));
+			const btScalar		k=((l.m_c1-len)/(l.m_c0*(l.m_c1+len)))*kst;
 			a.m_x-=del*(k*a.m_im);
 			b.m_x+=del*(k*b.m_im);
 		}
@@ -1175,68 +1083,26 @@ static void			PSolve_Links(btSoftBody* psb)
 }
 
 //
-static void			PSolve_Tetras(btSoftBody* psb)
+static void			VSolve_Links(btSoftBody* psb,btScalar kst)
-{
-for(int i=0,ni=psb->m_tetras.size();i<ni;++i)
-	{			
-	btSoftBody::Tetra&	t=psb->m_tetras[i];
-	btSoftBody::Node**	n=t.m_n;
-	const btVector3		g0=cross(n[3]->m_x-n[1]->m_x,n[2]->m_x-n[1]->m_x);
-	const btVector3		g1=cross(n[3]->m_x-n[2]->m_x,n[0]->m_x-n[2]->m_x);
-	const btVector3		g2=cross(n[3]->m_x-n[0]->m_x,n[1]->m_x-n[0]->m_x);
-	const btVector3		g3=cross(n[1]->m_x-n[0]->m_x,n[2]->m_x-n[0]->m_x);
-	const btScalar		v1=VolumeOf(n[0]->m_x,n[1]->m_x,n[2]->m_x,n[3]->m_x);
-	const btScalar		k=(t.m_rv-v1)/(g0.length2()+g1.length2()+g2.length2()+g3.length2());
-	const btScalar		j=k*t.m_c1;
-	n[0]->m_x	+=	g0*j*n[0]->m_im;
-	n[1]->m_x	+=	g1*j*n[1]->m_im;
-	n[2]->m_x	+=	g2*j*n[2]->m_im;
-	n[3]->m_x	+=	g3*j*n[3]->m_im;
-	}
-}
-
-//
-static void			VSolve_Links(btSoftBody* psb)
 {
 for(int i=0,ni=psb->m_links.size();i<ni;++i)
 	{			
 	btSoftBody::Link&	l=psb->m_links[i];
 	btSoftBody::Node**	n=l.m_n;
-	const btScalar		j=-dot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2;
+	const btScalar		j=-dot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst;
 	n[0]->m_v+=	l.m_c3*(j*n[0]->m_im);
 	n[1]->m_v-=	l.m_c3*(j*n[1]->m_im);
 	}
 }
 
 //
-static void			VSolve_Tetras(btSoftBody* psb)
+static void (* const VSolvers[])(btSoftBody*,btScalar)=	{
-{
-for(int i=0,ni=psb->m_tetras.size();i<ni;++i)
-	{			
-	btSoftBody::Tetra&	t=psb->m_tetras[i];		
-	btSoftBody::Node**	n=t.m_n;
-	const btScalar		r=	dot(t.m_c0[0],n[0]->m_v)+
-							dot(t.m_c0[1],n[1]->m_v)+
-							dot(t.m_c0[2],n[2]->m_v)+
-							dot(t.m_c0[3],n[3]->m_v);
-	const btScalar		j=r*t.m_c2;
-	n[0]->m_v	+=	t.m_c0[0]*(j*n[0]->m_im);
-	n[1]->m_v	+=	t.m_c0[1]*(j*n[1]->m_im);
-	n[2]->m_v	+=	t.m_c0[2]*(j*n[2]->m_im);
-	n[3]->m_v	+=	t.m_c0[3]*(j*n[3]->m_im);
-	}
-}
-
-//
-static void (* const VSolvers[])(btSoftBody*)=	{
 														VSolve_Links,
-												VSolve_Tetras,
 														};
 												
 //
-static void (* const PSolvers[])(btSoftBody*)=	{
+static void (* const PSolvers[])(btSoftBody*,btScalar)=	{
 														PSolve_Links,
-												PSolve_Tetras,
 														PSolve_Anchors,
 														PSolve_RContacts,
 														PSolve_SContacts,
@@ -1262,7 +1128,7 @@ btSoftBody::btSoftBody(btSoftBody::btSoftBodyWorldInfo*	worldInfo,int node_count
 	m_cfg.kDF			=	(btScalar)0.2;
 	m_cfg.kMT			=	0;
 	m_cfg.kCHR			=	(btScalar)1.0;
-	m_cfg.kKHR			=	(btScalar)0.5;
+	m_cfg.kKHR			=	(btScalar)0.1;
 	m_cfg.kSHR			=	(btScalar)1.0;
 	m_cfg.kAHR			=	(btScalar)0.7;
 	m_cfg.maxvolume		=	(btScalar)1;
@@ -1308,7 +1174,6 @@ btSoftBody::btSoftBody(btSoftBody::btSoftBodyWorldInfo*	worldInfo,int node_count
 		n.m_leaf	=	m_ndbvt.insert(btDbvt::Volume::FromCR(n.m_x,margin),&n);
 		n.m_material=	pm;
 	}
-	m_ndbvt.optimizeTopDown();
 	UpdateBounds(this);	
 
 
@@ -1319,7 +1184,7 @@ btSoftBody::~btSoftBody()
 {
 	//for now, delete the internal shape
 	delete m_collisionShape;	
-	for(int i=0;i<m_materials.size();++i) delete m_materials[i];
+	for(int i=0;i<m_materials.size();++i) btAlignedFree(m_materials[i]);
 }
 
 //
@@ -1368,7 +1233,7 @@ bool			btSoftBody::checkFace(int node0,int node1,int node2) const
 //
 btSoftBody::Material*		btSoftBody::appendMaterial()
 {
-Material*	pm=new Material();
+Material*	pm=new(btAlignedAlloc(sizeof(Material),16)) Material();
 if(m_materials.size()>0)
 	*pm=*m_materials[0];
 	else
@@ -1431,18 +1296,6 @@ appendNote(text,o,btVector4(w,w,w,0),	feature->m_n[0],
 										feature->m_n[2]);
 }
 
-//
-void			btSoftBody::appendNote(	const char* text,
-										const btVector3& o,
-										Tetra* feature)
-{
-static const btScalar	w=1/(btScalar)4;
-appendNote(text,o,btVector4(w,w,w,w),	feature->m_n[0],
-										feature->m_n[1],
-										feature->m_n[2],
-										feature->m_n[3]);
-}
-
 //
 void			btSoftBody::appendNode(	const btVector3& x,btScalar m)
 {
@@ -1525,34 +1378,6 @@ void			btSoftBody::appendFace(int node0,int node1,int node2,Material* mat)
 	m_bUpdateRtCst=true;
 }
 
-//
-void			btSoftBody::appendTetra(int model,Material* mat)
-{
-Tetra	t;
-if(model>=0)
-	t=m_tetras[model];
-	else
-	{ ZeroInitialize(t);t.m_material=mat?mat:m_materials[0]; }
-m_tetras.push_back(t);
-}
-
-//
-void			btSoftBody::appendTetra(int node0,
-										int node1,
-										int node2,
-										int node3,
-										Material* mat)
-{
-	appendTetra(-1,mat);
-	Tetra&	t=m_tetras[m_tetras.size()-1];
-	t.m_n[0]	=	&m_nodes[node0];
-	t.m_n[1]	=	&m_nodes[node1];
-	t.m_n[2]	=	&m_nodes[node2];
-	t.m_n[3]	=	&m_nodes[node3];
-	t.m_rv		=	VolumeOf(t.m_n[0]->m_x,t.m_n[1]->m_x,t.m_n[2]->m_x,t.m_n[3]->m_x);
-	m_bUpdateRtCst=true;
-}
-
 //
 void			btSoftBody::appendAnchor(int node,btRigidBody* body)
 {
@@ -1660,49 +1485,6 @@ void			btSoftBody::setTotalDensity(btScalar density)
 	setTotalMass(getVolume()*density,true);
 }
 
-//
-void			btSoftBody::setVolumeMass(btScalar mass)
-{
-btAlignedObjectArray<btScalar>	ranks;
-ranks.resize(m_nodes.size(),0);
-for(int i=0;i<m_nodes.size();++i)
-	{
-	m_nodes[i].m_im=0;
-	}
-for(int i=0;i<m_tetras.size();++i)
-	{
-	const Tetra& t=m_tetras[i];
-	for(int j=0;j<4;++j)
-		{
-		t.m_n[j]->m_im+=btFabs(t.m_rv);
-		ranks[int(t.m_n[j]-&m_nodes[0])]+=1;
-		}
-	}
-for(int i=0;i<m_nodes.size();++i)
-	{
-	if(m_nodes[i].m_im>0)
-		{
-		m_nodes[i].m_im=ranks[i]/m_nodes[i].m_im;
-		}
-	}
-setTotalMass(mass,false);
-}
-
-//
-void			btSoftBody::setVolumeDensity(btScalar density)
-{
-btScalar	volume=0;
-for(int i=0;i<m_tetras.size();++i)
-	{
-	const Tetra& t=m_tetras[i];
-	for(int j=0;j<4;++j)
-		{
-		volume+=btFabs(t.m_rv);
-		}
-	}
-setVolumeMass(volume*density/6);
-}
-
 //
 void			btSoftBody::transform(const btTransform& trs)
 {
@@ -1715,7 +1497,6 @@ void			btSoftBody::transform(const btTransform& trs)
 		n.m_n=trs.getBasis()*n.m_n;
 		m_ndbvt.update(n.m_leaf,btDbvt::Volume::FromCR(n.m_x,margin));
 	}
-	m_ndbvt.optimizeTopDown();
 	UpdateNormals(this);
 	UpdateBounds(this);
 	UpdateConstants(this);
@@ -1750,7 +1531,6 @@ void			btSoftBody::scale(const btVector3& scl)
 		n.m_q*=scl;
 		m_ndbvt.update(n.m_leaf,btDbvt::Volume::FromCR(n.m_x,margin));
 	}
-	m_ndbvt.optimizeTopDown();
 	UpdateNormals(this);
 	UpdateBounds(this);
 	UpdateConstants(this);
@@ -1870,6 +1650,7 @@ int				btSoftBody::generateBendingConstraints(int distance,Material* mat)
 				if(adj[IDX(i,j)]==(unsigned)distance)
 				{
 					appendLink(i,j,mat);
+					m_links[m_links.size()-1].m_bbending=1;
 					++nlinks;
 				}
 			}
@@ -1880,24 +1661,20 @@ int				btSoftBody::generateBendingConstraints(int distance,Material* mat)
 	return(0);
 }
 
-//
-int				btSoftBody::generateTetrahedralConstraints()
-{
-/// not implemented
-return(0);
-}
-
 //
 void			btSoftBody::randomizeConstraints()
 {
+unsigned long	seed=243703;
+#define NEXTRAND (seed=(1664525L*seed+1013904223L)&0xffffffff)
 	for(int i=0,ni=m_links.size();i<ni;++i)
 	{
-		btSwap(m_links[i],m_links[std::rand()%ni]);
+		btSwap(m_links[i],m_links[NEXTRAND%ni]);
 	}
 	for(int i=0,ni=m_faces.size();i<ni;++i)
 	{
-		btSwap(m_faces[i],m_faces[std::rand()%ni]);
+		btSwap(m_faces[i],m_faces[NEXTRAND%ni]);
 	}
+#undef NEXTRAND
 }
 
 //
@@ -1907,6 +1684,19 @@ const Node*			nbase(&m_nodes[0]);
 int					ncount(m_nodes.size());
 btSymMatrix<int>	edges(ncount,-2);
 int					newnodes=0;
+/* Filter out		*/ 
+for(int i=0;i<m_links.size();++i)
+	{
+	Link&	l=m_links[i];
+	if(l.m_bbending)
+		{
+		if(!SameSign(ifn->Eval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x)))
+			{
+			btSwap(m_links[i],m_links[m_links.size()-1]);
+			m_links.pop_back();--i;
+			}
+		}	
+	}
 /* Fill edges		*/ 
 for(int i=0;i<m_links.size();++i)
 	{
@@ -1920,16 +1710,6 @@ for(int i=0;i<m_faces.size();++i)
 	edges(int(f.m_n[1]-nbase),int(f.m_n[2]-nbase))=-1;
 	edges(int(f.m_n[2]-nbase),int(f.m_n[0]-nbase))=-1;
 	}
-for(int i=0;i<m_tetras.size();++i)
-	{
-	Tetra&	t=m_tetras[i];
-	edges(int(t.m_n[0]-nbase),int(t.m_n[1]-nbase))=-1;
-	edges(int(t.m_n[1]-nbase),int(t.m_n[2]-nbase))=-1;
-	edges(int(t.m_n[2]-nbase),int(t.m_n[0]-nbase))=-1;
-	edges(int(t.m_n[0]-nbase),int(t.m_n[3]-nbase))=-1;
-	edges(int(t.m_n[1]-nbase),int(t.m_n[3]-nbase))=-1;
-	edges(int(t.m_n[2]-nbase),int(t.m_n[3]-nbase))=-1;
-	}
 /* Intersect		*/ 
 for(int i=0;i<ncount;++i)
 	{
@@ -2027,60 +1807,6 @@ for(int i=0;i<m_faces.size();++i)
 			}
 		}
 	}
-/* Refine tetras	*/ 
-static const int	edg[]={0,1,1,2,2,0,0,3,1,3,2,3};
-static const int	apx[]={2,3,0,3,1,3,1,2,2,0,0,1};
-for(int i=0;i<m_tetras.size();++i)
-	{
-	const Tetra&	feat=m_tetras[i];
-	const int		idx[]={	int(feat.m_n[0]-nbase),
-							int(feat.m_n[1]-nbase),
-							int(feat.m_n[2]-nbase),
-							int(feat.m_n[3]-nbase)};	
-	for(int j=0;j<(sizeof(edg)/sizeof(edg[0]));j+=2)
-		{
-		const int	ia=idx[edg[j]];
-		const int	ib=idx[edg[j+1]];
-		if((ia<ncount)&&(ib<ncount))
-			{
-			const int ni=edges(ia,ib);
-			const int xa=idx[apx[j]];
-			const int xb=idx[apx[j+1]];
-			if(ni!=-1)
-				{
-				appendTetra(i);			
-				Tetra*			pft[]={	&m_tetras[i],
-										&m_tetras[m_tetras.size()-1]};
-				const btScalar	sig=Sign(pft[0]->m_rv);
-				pft[0]->m_n[0]=&m_nodes[ia];
-				pft[0]->m_n[1]=&m_nodes[xb];
-				pft[0]->m_n[2]=&m_nodes[xa];
-				pft[0]->m_n[3]=&m_nodes[ni];
-				if(Sign(VolumeOf(	pft[0]->m_n[0]->m_x,
-									pft[0]->m_n[1]->m_x,
-									pft[0]->m_n[2]->m_x,
-									pft[0]->m_n[3]->m_x))!=sig)
-					{
-					btSwap(pft[0]->m_n[0],pft[0]->m_n[1]);
-					}
-				pft[1]->m_n[0]=&m_nodes[ib];
-				pft[1]->m_n[1]=&m_nodes[xa];
-				pft[1]->m_n[2]=&m_nodes[xb];
-				pft[1]->m_n[3]=&m_nodes[ni];
-				if(Sign(VolumeOf(	pft[1]->m_n[0]->m_x,
-									pft[1]->m_n[1]->m_x,
-									pft[1]->m_n[2]->m_x,
-									pft[1]->m_n[3]->m_x))!=sig)
-					{
-					btSwap(pft[1]->m_n[0],pft[1]->m_n[1]);
-					}
-				appendLink(ni,xa,pft[0]->m_material,true);
-				appendLink(ni,xb,pft[0]->m_material,true);
-				--i;break;
-				}
-			}
-		}
-	}
 /* Cut				*/ 
 if(cut)
 	{	
@@ -2145,22 +1871,6 @@ if(cut)
 				}
 			}
 		}
-	/* Tetras		*/ 
-	for(int i=0,ni=m_tetras.size();i<ni;++i)
-		{
-		Node**			n=	m_tetras[i].m_n;
-		if(	(ifn->Eval(n[0]->m_x)<accurary)&&
-			(ifn->Eval(n[1]->m_x)<accurary)&&
-			(ifn->Eval(n[2]->m_x)<accurary)&&
-			(ifn->Eval(n[3]->m_x)<accurary))
-			{
-			for(int j=0;j<4;++j)
-				{
-				int cn=cnodes[int(n[j]-nbase)];
-				if(cn) n[j]=&m_nodes[cn];
-				}
-			}		
-		}
 	/* Clean orphans	*/ 
 	int							nnodes=m_nodes.size();
 	btAlignedObjectArray<int>	ranks;
@@ -2174,10 +1884,6 @@ if(cut)
 		{
 		for(int j=0;j<3;++j) ranks[int(m_faces[i].m_n[j]-nbase)]++;
 		}
-	for(int i=0,ni=m_tetras.size();i<ni;++i)
-		{
-		for(int j=0;j<4;++j) ranks[int(m_tetras[i].m_n[j]-nbase)]++;
-		}
 	for(int i=0;i<m_links.size();++i)
 		{
 		const int	id[]={	int(m_links[i].m_n[0]-nbase),
@@ -2318,18 +2024,15 @@ switch(preset)
 	m_cfg.m_psequence.push_back(ePSolver::Anchors);
 	m_cfg.m_psequence.push_back(ePSolver::RContacts);
 	m_cfg.m_psequence.push_back(ePSolver::SContacts);
-	m_cfg.m_psequence.push_back(ePSolver::Volume);
 	m_cfg.m_psequence.push_back(ePSolver::Linear);	
 	break;	
 	case	eSolverPresets::Velocities:
-	m_cfg.m_vsequence.push_back(eVSolver::Volume);
 	m_cfg.m_vsequence.push_back(eVSolver::Linear);
 	
 	m_cfg.m_psequence.push_back(ePSolver::Anchors);
 	m_cfg.m_psequence.push_back(ePSolver::RContacts);
 	m_cfg.m_psequence.push_back(ePSolver::SContacts);
 	
-	m_cfg.m_dsequence.push_back(ePSolver::Volume);
 	m_cfg.m_dsequence.push_back(ePSolver::Linear);
 	break;
 	}
@@ -2389,53 +2092,6 @@ void			btSoftBody::predictMotion(btScalar dt)
 		}
 	/* Pose					*/ 
 	UpdatePose(this);
-	/* Clear contacts	*/ 
-	m_rcontacts.resize(0);
-	m_scontacts.resize(0);
-}
-
-//
-void			btSoftBody::solveConstraints()
-{
-/* Solve velocities		*/ 
-if(m_cfg.viterations>0)
-	{
-	/* Prepare			*/ 
-	for(int i=0,ni=m_links.size();i<ni;++i)
-		{
-		Link&	l=m_links[i];
-		l.m_c3		=	l.m_n[1]->m_x-l.m_n[0]->m_x;
-		l.m_c2		=	1/(l.m_c3.length2()*l.m_c0);
-		}
-	for(int i=0,ni=m_tetras.size();i<ni;++i)
-		{
-		Tetra&	t=m_tetras[i];
-		Node**	n=t.m_n;
-		t.m_c0[0]	=	cross(n[3]->m_x-n[1]->m_x,n[2]->m_x-n[1]->m_x);
-		t.m_c0[1]	=	cross(n[3]->m_x-n[2]->m_x,n[0]->m_x-n[2]->m_x);
-		t.m_c0[2]	=	cross(n[3]->m_x-n[0]->m_x,n[1]->m_x-n[0]->m_x);
-		t.m_c0[3]	=	cross(n[1]->m_x-n[0]->m_x,n[2]->m_x-n[0]->m_x);
-		const btScalar	den=t.m_c0[0].length2()+
-							t.m_c0[1].length2()+
-							t.m_c0[2].length2()+
-							t.m_c0[3].length2();
-		t.m_c2		=	-t.m_c1/den;
-		}
-	/* Solve			*/ 
-	for(int isolve=0;isolve<m_cfg.viterations;++isolve)
-		{
-		for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq)
-			{
-			VSolvers[m_cfg.m_vsequence[iseq]](this);
-			}
-		}
-	/* Update			*/ 
-	for(int i=0,ni=m_nodes.size();i<ni;++i)
-		{
-		Node&	n=m_nodes[i];
-		n.m_x	=	n.m_q+n.m_v*m_sst.sdt;
-		}
-	}
 	/* Match				*/ 
 	if(m_pose.m_bframe&&(m_cfg.kMT>0))
 		{
@@ -2449,6 +2105,24 @@ if(m_pose.m_bframe&&(m_cfg.kMT>0))
 				}
 			}
 		}
+	/* Clear contacts		*/ 
+	m_rcontacts.resize(0);
+	m_scontacts.resize(0);
+	/* Optimize dbvt's		*/ 
+	m_ndbvt.optimizeIncremental(1);
+	m_fdbvt.optimizeIncremental(1);
+}
+
+//
+void			btSoftBody::solveConstraints()
+{
+/* Prepare links		*/ 
+for(int i=0,ni=m_links.size();i<ni;++i)
+	{
+	Link&	l=m_links[i];
+	l.m_c3		=	l.m_n[1]->m_x-l.m_n[0]->m_x;
+	l.m_c2		=	1/(l.m_c3.length2()*l.m_c0);
+	}
 /* Prepare anchors		*/ 
 for(int i=0,ni=m_anchors.size();i<ni;++i)
 	{
@@ -2463,6 +2137,24 @@ for(int i=0,ni=m_anchors.size();i<ni;++i)
 	a.m_c2	=	m_sst.sdt*a.m_node->m_im;
 	a.m_body->activate();
 	}
+/* Solve velocities		*/ 
+if(m_cfg.viterations>0)
+	{	
+	/* Solve			*/ 
+	for(int isolve=0;isolve<m_cfg.viterations;++isolve)
+		{
+		for(int iseq=0;iseq<m_cfg.m_vsequence.size();++iseq)
+			{
+			VSolvers[m_cfg.m_vsequence[iseq]](this,1);
+			}
+		}
+	/* Update			*/ 
+	for(int i=0,ni=m_nodes.size();i<ni;++i)
+		{
+		Node&	n=m_nodes[i];
+		n.m_x	=	n.m_q+n.m_v*m_sst.sdt;
+		}
+	}
 /* Solve positions		*/ 
 if(m_cfg.piterations>0)
 	{
@@ -2470,7 +2162,7 @@ if(m_cfg.piterations>0)
 		{
 		for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
 			{
-			PSolvers[m_cfg.m_psequence[iseq]](this);
+			PSolvers[m_cfg.m_psequence[iseq]](this,1);
 			}
 		}
 	const btScalar	vc=m_sst.isdt*(1-m_cfg.kDP);
@@ -2494,7 +2186,7 @@ if(m_cfg.diterations>0)
 		{
 		for(int iseq=0;iseq<m_cfg.m_dsequence.size();++iseq)
 			{
-			PSolvers[m_cfg.m_dsequence[iseq]](this);
+			PSolvers[m_cfg.m_dsequence[iseq]](this,1);
 			}
 		}
 	for(int i=0,ni=m_nodes.size();i<ni;++i)
@@ -2512,7 +2204,7 @@ for(int isolve=0;isolve<iterations;++isolve)
 	{
 	for(int iseq=0;iseq<m_cfg.m_psequence.size();++iseq)
 		{
-		PSolvers[m_cfg.m_psequence[iseq]](this);
+		PSolvers[m_cfg.m_psequence[iseq]](this,1);
 		}
 	}
 }
@@ -2539,17 +2231,7 @@ switch(m_cfg.collisions&fCollision::RVSmask)
 		{
 		struct	DoCollide : btDbvt::ICollide
 			{
-				
+			void		Process(const btDbvt::Node* leaf)
-				virtual void		Process(const btDbvt::Node* /*a*/,const btDbvt::Node* /*b*/)
-				{
-
-				}
-
-			virtual bool	Descent(const btDbvt::Node*)
-			{
-				return false;
-			}
-			virtual void		Process(const btDbvt::Node* leaf)
 				{
 				Node*	node=(Node*)leaf->data;
 				DoNode(*node);
@@ -2608,7 +2290,7 @@ switch(m_cfg.collisions&fCollision::RVSmask)
 		docollide.prb		=	prb;
 		docollide.dynmargin	=	basemargin+timemargin;
 		docollide.stamargin	=	basemargin;
-		m_ndbvt.collide(volume,&docollide);
+		btDbvt::collideTV(m_ndbvt.m_root,volume,docollide);
 		}
 	break;
 	}
@@ -2624,15 +2306,6 @@ switch(cf&fCollision::SVSmask)
 		{
 		struct	DoCollide : btDbvt::ICollide
 			{
-			virtual bool	Descent(const btDbvt::Node*)
-			{
-				return false;
-			}
-			virtual void		Process(const btDbvt::Node* /*leaf*/)
-			{
-			
-			}
-
 			void		Process(const btDbvt::Node* lnode,
 								const btDbvt::Node* lface)
 				{
@@ -2683,13 +2356,15 @@ switch(cf&fCollision::SVSmask)
 		/* psb0 nodes vs psb1 faces	*/ 
 		docollide.psb[0]=this;
 		docollide.psb[1]=psb;
-		docollide.psb[0]->m_ndbvt.collide(	&docollide.psb[1]->m_fdbvt,
+		btDbvt::collideTT(	docollide.psb[0]->m_ndbvt.m_root,
-											&docollide);
+							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.collide(	&docollide.psb[1]->m_fdbvt,
+		btDbvt::collideTT(	docollide.psb[0]->m_ndbvt.m_root,
-											&docollide);
+							docollide.psb[1]->m_fdbvt.m_root,
+							docollide);
 		}
 	break;
 	}

src/BulletSoftBody/btSoftBody.h

@@ -45,29 +45,31 @@ public:
 		V_TwoSided,	///Vertex normals are fliped to match velocity	
 		V_OneSided,	///Vertex normals are taken as it is	
 		F_TwoSided,	///Face normals are fliped to match velocity
-		F_OneSided	///Face normals are taken as it is
+		F_OneSided,	///Face normals are taken as it is
+		END
 	};};
 	
 	///eVSolver : velocities solvers
 	struct	eVSolver { enum _ {
 		Linear,		///Linear solver
-		Volume		///Volume solver
+		END
 	};};
 	
 	///ePSolver : positions solvers
 	struct	ePSolver { enum _ {
 		Linear,		///Linear solver
-		Volume,		///Volume solver
 		Anchors,	///Anchor solver
 		RContacts,	///Rigid contacts solver
-		SContacts	///Soft contacts solver
+		SContacts,	///Soft contacts solver
+		END
 	};};
 	
 	///eSolverPresets
 	struct	eSolverPresets { enum _ {
 		Positions,
 		Velocities,
-		Default	=	Positions
+		Default	=	Positions,
+		END
 	};};
 	
 	///eFeature
@@ -76,7 +78,7 @@ public:
 		Node,
 		Link,
 		Face,
-		Tetra
+		END
 	};};
 	
 	typedef btAlignedObjectArray<eVSolver::_>	tVSolverArray;
@@ -94,14 +96,16 @@ public:
 		SVSmask	=	0x00f0,	///Rigid versus soft mask		
 		VF_SS	=	0x0010,	///Vertex vs face soft vs soft handling
 		/* presets	*/ 
-		Default	=	SDF_RS
+		Default	=	SDF_RS,
+		END
 	};};
 	
 	///fMaterial
 	struct fMaterial { enum _ {
 		DebugDraw	=	0x0001,	/// Enable debug draw
 		/* presets	*/ 
-		Default		=	DebugDraw
+		Default		=	DebugDraw,
+		END
 	};};
 		
 	//
@@ -120,7 +124,6 @@ public:
 	/* ImplicitFn	*/ 
 	struct	ImplicitFn
 	{
-		virtual ~ImplicitFn() {}
 		virtual btScalar	Eval(const btVector3& x)=0;
 	};
 	
@@ -163,16 +166,13 @@ public:
 	struct	Element
 	{
 		void*			m_tag;			// User data
-		Element()
+		Element() : m_tag(0) {}
-		{
-			m_tag=0;
-		}
 	};
 	/* Material		*/ 
 	struct	Material : Element
 	{
 		btScalar				m_kLST;			// Linear stiffness coefficient [0,1]
-		btScalar				m_kAST;			// Area stiffness coefficient [0,1]
+		btScalar				m_kAST;			// Area/Angular stiffness coefficient [0,1]
 		btScalar				m_kVST;			// Volume stiffness coefficient [0,1]
 		int						m_flags;		// Flags
 	};
@@ -200,6 +200,7 @@ public:
 	{
 		Node*					m_n[2];			// Node pointers
 		btScalar				m_rl;			// Rest length		
+		int						m_bbending:1;	// Bending link
 		btScalar				m_c0;			// (ima+imb)*kLST
 		btScalar				m_c1;			// rl^2
 		btScalar				m_c2;			// |gradient|^2/c0
@@ -213,16 +214,6 @@ public:
 		btScalar				m_ra;			// Rest area
 		btDbvt::Node*			m_leaf;			// Leaf data
 	};
-	/* Tetra		*/ 
-	struct	Tetra : Feature
-	{
-		Node*					m_n[4];			// Node pointers		
-		btScalar				m_rv;			// Rest volume
-		btDbvt::Node*			m_leaf;			// Leaf data
-		btVector3				m_c0[4];		// gradients
-		btScalar				m_c1;			// (4*kVST)/(im0+im1+im2+im3)
-		btScalar				m_c2;			// m_c1/sum(|g0..3|^2)
-	};
 	/* RContact		*/ 
 	struct	RContact
 	{
@@ -322,7 +313,6 @@ public:
 	typedef btAlignedObjectArray<btDbvt::Node*>	tLeafArray;
 	typedef btAlignedObjectArray<Link>			tLinkArray;
 	typedef btAlignedObjectArray<Face>			tFaceArray;
-	typedef btAlignedObjectArray<Tetra>			tTetraArray;
 	typedef btAlignedObjectArray<Anchor>		tAnchorArray;
 	typedef btAlignedObjectArray<RContact>		tRContactArray;
 	typedef btAlignedObjectArray<SContact>		tSContactArray;
@@ -342,7 +332,6 @@ public:
 	tNodeArray				m_nodes;		// Nodes
 	tLinkArray				m_links;		// Links
 	tFaceArray				m_faces;		// Faces
-	tTetraArray				m_tetras;		// Tetras
 	tAnchorArray			m_anchors;		// Anchors
 	tRContactArray			m_rcontacts;	// Rigid contacts
 	tSContactArray			m_scontacts;	// Soft contacts
@@ -365,8 +354,8 @@ public:
 	/* Check for existing link												*/ 
 	bool				checkLink(	int node0,
 		int node1) const;
-	bool				checkLink(	const btSoftBody::Node* node0,
+	bool				checkLink(	const Node* node0,
-		const btSoftBody::Node* node1) const;
+									const Node* node1) const;
 	/* Check for existring face												*/ 
 	bool				checkFace(	int node0,
 									int node1,
@@ -390,9 +379,6 @@ public:
 	void				appendNote(	const char* text,
 									const btVector3& o,
 									Face* feature);
-	void				appendNote(	const char* text,
-									const btVector3& o,
-									Tetra* feature);
 	/* Append node															*/ 
 	void				appendNode(	const btVector3& x,btScalar m);
 	/* Append link															*/ 
@@ -401,8 +387,8 @@ public:
 									int node1,
 									Material* mat=0,
 									bool bcheckexist=false);
-	void				appendLink(	btSoftBody::Node* node0,
+	void				appendLink(	Node* node0,
-									btSoftBody::Node* node1,
+									Node* node1,
 									Material* mat=0,
 									bool bcheckexist=false);
 	/* Append face															*/ 
@@ -411,13 +397,6 @@ public:
 									int node1,
 									int node2,
 									Material* mat=0);
-	/* Append tetrahedron													*/ 
-	void				appendTetra(int model=-1,Material* mat=0);
-	void				appendTetra(int node0,
-									int node1,
-									int node2,
-									int node3,
-									Material* mat=0);
 	/* Append anchor														*/ 
 	void				appendAnchor(	int node,
 										btRigidBody* body);
@@ -443,10 +422,6 @@ public:
 										bool fromfaces=false);
 	/* Set total density													*/ 
 	void				setTotalDensity(btScalar density);
-	/* Set volume mass (using tetrahedrons)									*/ 
-	void				setVolumeMass(		btScalar mass);
-	/* Set volume density (using tetrahedrons)								*/ 
-	void				setVolumeDensity(	btScalar density);
 	/* Transform															*/ 
 	void				transform(		const btTransform& trs);
 	/* Translate															*/ 
@@ -463,8 +438,6 @@ public:
 	/* Generate bending constraints based on distance in the adjency graph	*/ 
 	int					generateBendingConstraints(	int distance,
 													Material* mat=0);
-	/* Generate tetrahedral constraints										*/ 
-	int					generateTetrahedralConstraints();
 	/* Randomize constraints to reduce solver bias							*/ 
 	void				randomizeConstraints();
 	/* Refine																*/ 

src/BulletSoftBody/btSoftBodyHelpers.cpp

@@ -15,7 +15,6 @@ subject to the following restrictions:
 ///btSoftBodyHelpers.cpp by Nathanael Presson
 
 #include "btSoftBody.h"
-#include "BulletCollision/BroadphaseCollision/btDbvt.h"
 #include <stdio.h>
 #include <string.h>
 #include "btSoftBodyHelpers.h"
@@ -92,7 +91,7 @@ static inline btScalar		tetravolume(const btVector3& x0,
 }
 
 //
-/*
+#if 0
 static btVector3		stresscolor(btScalar stress)
 	{
 	static const btVector3	spectrum[]=	{	btVector3(1,0,1),
@@ -109,7 +108,8 @@ static btVector3		stresscolor(btScalar stress)
 	const btScalar			frc=stress-sel;
 	return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc);
 	}
-*/
+#endif
+
 //
 void			btSoftBodyHelpers::Draw(	btSoftBody* psb,
 					 btIDebugDraw* idraw,
@@ -219,24 +219,6 @@ void			btSoftBodyHelpers::Draw(	btSoftBody* psb,
 				col,alp);*/
 		}	
 	}
-	/* Tetras	*/ 
-	if(0!=(drawflags&fDrawFlags::Tetras))
-	{
-		const btScalar	scl=(btScalar)0.8;
-		const btScalar	alp=(btScalar)1;
-		const btVector3	col((btScalar)0.7,(btScalar)0.7,(btScalar)0.7);
-		for(int i=0;i<psb->m_tetras.size();++i)
-		{
-			const btSoftBody::Tetra&	t=psb->m_tetras[i];
-			if(0==(t.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;
-			const btVector3				x[]={t.m_n[0]->m_x,t.m_n[1]->m_x,t.m_n[2]->m_x,t.m_n[3]->m_x};
-			const btVector3				c=(x[0]+x[1]+x[2]+x[3])/4;
-			idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[2]-c)*scl+c,col,alp);
-			idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
-			idraw->drawTriangle((x[1]-c)*scl+c,(x[2]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
-			idraw->drawTriangle((x[2]-c)*scl+c,(x[0]-c)*scl+c,(x[3]-c)*scl+c,col,alp);
-		}	
-	}
 	/* Notes	*/ 
 	if(0!=(drawflags&fDrawFlags::Notes))
 	{
@@ -253,7 +235,6 @@ void			btSoftBodyHelpers::Draw(	btSoftBody* psb,
 	}
 }
 
-#if 0
 //
 void			btSoftBodyHelpers::DrawInfos(		btSoftBody* psb,
 						  btIDebugDraw* idraw,
@@ -279,7 +260,6 @@ void			btSoftBodyHelpers::DrawInfos(		btSoftBody* psb,
 		if(text[0]) idraw->draw3dText(n.m_x,text);
 	}
 }
-#endif
 
 //
 void			btSoftBodyHelpers::DrawNodeTree(	btSoftBody* psb,
@@ -504,18 +484,18 @@ btSoftBody*		btSoftBodyHelpers::CreateFromTriMesh(btSoftBody::btSoftBodyWorldInf
 btSoftBody*		btSoftBodyHelpers::CreateFromConvexHull(btSoftBody::btSoftBodyWorldInfo& worldInfo,	const btVector3* vertices,
 									 int nvertices)
 {
-	HullDesc		hdsc(QF_TRIANGLES,static_cast<unsigned int>(nvertices),vertices);
+	HullDesc		hdsc(QF_TRIANGLES,nvertices,vertices);
 	HullResult		hres;
 	HullLibrary		hlib;/*??*/ 
-	hdsc.mMaxVertices=static_cast<unsigned int>(nvertices);
+	hdsc.mMaxVertices=nvertices;
 	hlib.CreateConvexHull(hdsc,hres);
 	btSoftBody*		psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices,
 		&hres.m_OutputVertices[0],0);
 	for(int i=0;i<(int)hres.mNumFaces;++i)
 	{
-		const int idx[]={	static_cast<int>(hres.m_Indices[i*3+0]),
+		const int idx[]={	hres.m_Indices[i*3+0],
-			static_cast<int>(hres.m_Indices[i*3+1]),
+			hres.m_Indices[i*3+1],
-			static_cast<int>(hres.m_Indices[i*3+2])};
+			hres.m_Indices[i*3+2]};
 		if(idx[0]<idx[1]) psb->appendLink(	idx[0],idx[1]);
 		if(idx[1]<idx[2]) psb->appendLink(	idx[1],idx[2]);
 		if(idx[2]<idx[0]) psb->appendLink(	idx[2],idx[0]);
@@ -525,155 +505,3 @@ btSoftBody*		btSoftBodyHelpers::CreateFromConvexHull(btSoftBody::btSoftBodyWorld
 	psb->randomizeConstraints();
 	return(psb);
 }
-
-#if BT_SOFTBODY_USE_STL
-
-#include <string>
-#include <sstream>
-#include <fstream>
-#include <iostream>
-#include <stdio.h>
-
-//
-void		btSoftBodyHelpers::ExportAsSMeshFile(btSoftBody* psb,
-												 const char* filename)
-{
-	std::ofstream	output(filename);
-	output << psb->m_nodes.size() << " " << 3 << " " << 0 << " " << 0 << "\n";
-	for(int i=0;i<psb->m_nodes.size();++i)
-	{
-		const btSoftBody::Node& n=psb->m_nodes[i];
-		output	<< i << " "
-			<< n.m_x.x() << " "
-			<< n.m_x.y() << " "
-			<< n.m_x.z() << "\n";
-	}
-	output << psb->m_faces.size() << " " << 1 << "\n";
-	for(int i=0;i<psb->m_faces.size();++i)
-	{
-		const btSoftBody::Node*	b=&psb->m_nodes[0];
-		const btSoftBody::Face&	f=psb->m_faces[i];
-		output	<< 3 << " "
-			<< int(f.m_n[0]-b) << " "
-			<< int(f.m_n[1]-b) << " "
-			<< int(f.m_n[2]-b) << " "
-			<< 1 << "\n";
-	}
-	output << 0 << "\n";
-	output << 0 << "\n";
-	output.close();
-}
-
-/* Create from TetGen .ele, .face, .node files							*/ 
-btSoftBody*	btSoftBodyHelpers::CreateFromTetGenFile(btSoftBody::btSoftBodyWorldInfo& worldInfo,
-													const char* ele,
-													const char* face,
-													const char* node,
-													bool bfacelinks,
-													bool btetralinks,
-													bool bfacesfromtetras)
-{
-	std::ifstream	efile(ele?ele:"");
-	std::ifstream	ffile(face?face:"");
-	std::ifstream	nfile(node);
-	std::string		edata;
-	std::string		fdata;
-	std::string		ndata;
-	if(efile.good()) while(!efile.eof()) edata+=efile.get();
-	if(ffile.good()) while(!ffile.eof()) fdata+=ffile.get();
-	if(nfile.good()) while(!nfile.eof()) ndata+=nfile.get();
-	efile.close();
-	ffile.close();
-	nfile.close();
-	return(CreateFromTetGenData(worldInfo,edata.c_str(),fdata.c_str(),ndata.c_str(),
-		bfacelinks,btetralinks,bfacesfromtetras));
-}
-
-/* Create from TetGen .ele, .face, .node data							*/ 
-btSoftBody*	btSoftBodyHelpers::CreateFromTetGenData(btSoftBody::btSoftBodyWorldInfo& worldInfo,
-													const char* ele,
-													const char* face,
-													const char* node,
-													bool bfacelinks,
-													bool btetralinks,
-													bool bfacesfromtetras)
-{
-	std::istringstream				se(ele?ele:"");
-	std::istringstream				sf(face?face:"");
-	std::istringstream				sn(node?node:"");
-	btAlignedObjectArray<btVector3>	pos;
-	int								nnode=0;
-	int								ndims=0;
-	int								nattrb=0;
-	int								hasbounds=0;
-	sn>>nnode;sn>>ndims;sn>>nattrb;sn>>hasbounds;
-	pos.resize(nnode);
-	for(int i=0;i<pos.size();++i)
-	{
-		int			index=0;
-		int			bound=0;
-		btScalar	x,y,z,a;
-		sn>>index;
-		sn>>x;sn>>y;sn>>z;
-		for(int j=0;j<nattrb;++j) sn>>a;
-		if(hasbounds) sn>>bound;
-		pos[index].setX(x);
-		pos[index].setY(y);
-		pos[index].setZ(z);
-	}
-	btSoftBody*						psb=new btSoftBody(&worldInfo,nnode,&pos[0],0);
-	if(face&&face[0])
-	{
-		int								nface=0;
-		sf>>nface;sf>>hasbounds;
-		for(int i=0;i<nface;++i)
-		{
-			int			index=0;
-			int			bound=0;
-			int			ni[3];
-			sf>>index;
-			sf>>ni[0];sf>>ni[1];sf>>ni[2];
-			sf>>bound;
-			psb->appendFace(ni[0],ni[1],ni[2]);	
-			if(btetralinks)
-			{
-				psb->appendLink(ni[0],ni[1],0,true);
-				psb->appendLink(ni[1],ni[2],0,true);
-				psb->appendLink(ni[2],ni[0],0,true);
-			}
-		}
-	}
-	if(ele&&ele[0])
-	{
-		int								ntetra=0;
-		int								ncorner=0;
-		int								neattrb=0;
-		se>>ntetra;se>>ncorner;se>>neattrb;
-		for(int i=0;i<ntetra;++i)
-		{
-			int			index=0;
-			int			ni[4],a;
-			se>>index;
-			se>>ni[0];se>>ni[1];se>>ni[2];se>>ni[3];
-			for(int j=0;j<neattrb;++j) se>>a;
-			psb->appendTetra(ni[0],ni[1],ni[2],ni[3]);
-			if(btetralinks)
-			{
-				psb->appendLink(ni[0],ni[1],0,true);
-				psb->appendLink(ni[1],ni[2],0,true);
-				psb->appendLink(ni[2],ni[0],0,true);
-				psb->appendLink(ni[0],ni[3],0,true);
-				psb->appendLink(ni[1],ni[3],0,true);
-				psb->appendLink(ni[2],ni[3],0,true);
-			}
-		}
-	}
-	printf("Nodes:  %u\r\n",psb->m_nodes.size());
-	printf("Links:  %u\r\n",psb->m_links.size());
-	printf("Faces:  %u\r\n",psb->m_faces.size());
-	printf("Tetras: %u\r\n",psb->m_tetras.size());
-	return(psb);
-}
-
-#endif
-

src/BulletSoftBody/btSoftBodyHelpers.h

@@ -18,9 +18,6 @@ subject to the following restrictions:
 
 #include "btSoftBody.h"
 
-//Can't enable this, Bullet doesn't use STL
-//#define	BT_SOFTBODY_USE_STL	1
-
 //
 // Helpers
 //
@@ -37,7 +34,7 @@ struct	fDrawFlags { enum _ {
 	Notes		=	0x0080,
 	/* presets	*/ 
 	Std			=	Links+Faces+Tetras+Anchors+Notes,
-	StdTetra	=	Std-Faces+Tetras
+	StdTetra	=	Std-Faces+Tetras,
 };};
 
 struct	btSoftBodyHelpers
@@ -46,14 +43,12 @@ struct	btSoftBodyHelpers
 	static void				Draw(		btSoftBody* psb,
 										btIDebugDraw* idraw,
 										int drawflags=fDrawFlags::Std);
-#if 0
 	/* Draw body infos														*/ 
 	static	void			DrawInfos(	btSoftBody* psb,
 										btIDebugDraw* idraw,
 										bool masses,
 										bool areas,
 										bool stress);
-#endif
 	/* Draw node tree														*/ 
 	static void				DrawNodeTree(	btSoftBody* psb,
 											btIDebugDraw* idraw,
@@ -97,27 +92,6 @@ struct	btSoftBodyHelpers
 	static	btSoftBody*		CreateFromConvexHull(	btSoftBody::btSoftBodyWorldInfo& worldInfo,
 													const btVector3* vertices,
 													int nvertices);
-	#if BT_SOFTBODY_USE_STL
-	/* Export TetGen compatible .smesh file									*/ 
-	static void				ExportAsSMeshFile(	btSoftBody* psb,
-												const char* filename);	
-	/* Create from TetGen .ele, .face, .node files							*/ 
-	static btSoftBody*		CreateFromTetGenFile(	btSoftBody::btSoftBodyWorldInfo& worldInfo,
-													const char* ele,
-													const char* face,
-													const char* node,
-													bool bfacelinks,
-													bool btetralinks,
-													bool bfacesfromtetras);
-	/* Create from TetGen .ele, .face, .node data							*/ 
-	static btSoftBody*		CreateFromTetGenData(	btSoftBody::btSoftBodyWorldInfo& worldInfo,
-													const char* ele,
-													const char* face,
-													const char* node,
-													bool bfacelinks,
-													bool btetralinks,
-													bool bfacesfromtetras);
-	#endif
 };
 
 #endif //SOFT_BODY_HELPERS_H