add Thomas Jacobsen / position based dynamics cloth constraint solver

Demos3/CpuDemos/deformable/CpuSoftBodyDemo.cpp

@@ -15,6 +15,7 @@
 #include "CpuSoftClothDemoInternalData.h"
 
 #include "ExplicitEuler.h"
+#include "PositionBasedDynamics.h"
 
 static b3KeyboardCallback oldCallback = 0;
 extern bool gReset;
@@ -22,8 +23,9 @@ extern bool gReset;
 float clothWidth = 4;
 float clothHeight= 4;
 
-int width = 64;
+int width = 32;//64;
-int height = 64;
+int height = 32;//64;
+
 int numPoints = width*height;
 float clothMass = 100.f;
 
@@ -114,7 +116,7 @@ struct GraphicsVertex
 void CpuSoftClothDemo::renderScene()
 {
 	//wireframe
-	bool wireframe=true;
+	bool wireframe=false;
 	if (wireframe)
 	{
 		m_instancingRenderer->init();
@@ -157,8 +159,18 @@ void CpuSoftClothDemo::clientMoveAndDisplay()
 		//write positions
 		int vertexStride  =sizeof(GraphicsVertex);//9 * sizeof(float);
 
-		ExplicitEuler::solveConstraints(m_clothData, (char*)m_data->m_clothVertices, vertexStride, deltaTime);
+		int method = 1;
-		
+		switch (method)
+		{
+		case 0:
+			ExplicitEuler::solveConstraints(m_clothData, (char*)m_data->m_clothVertices, vertexStride, deltaTime);
+			break;
+		case 1:
+			PositionBasedDynamics::solveConstraints(m_clothData, (char*)m_data->m_clothVertices, vertexStride, deltaTime);
+			break;
+		default:
+			b3Error("unknown method for CpuSoftClothDemo::solveConstraints");
+		};
 		
 		//read positions
 
@@ -260,8 +272,8 @@ void	CpuSoftClothDemo::setupScene(const ConstructionInfo& ci)
 			float posX = (x/((float)(width-1)))*(clothWidth);
 			float posZ = ((y-height/2.f)/((float)(height-1)))*(clothHeight);
 
-			cpu_buffer[y*width+x].pos[0]      = posX;
+			cpu_buffer[y*width+x].pos[0]      = 0;
-			cpu_buffer[y*width+x].pos[1]      = coord;
+			cpu_buffer[y*width+x].pos[1]      = -posX;
 			cpu_buffer[y*width+x].pos[2]      = posZ; 
 			cpu_buffer[y*width+x].pos[3]      = 0.f;
 

Demos3/CpuDemos/deformable/ExplicitEuler.cpp

@@ -6,7 +6,7 @@
 
 
 
-void ExplicitEuler::computeForces(struct CpuSoftClothDemoInternalData* clothData, char* vertexPositions, int vertexStride, float dt)
+void ExplicitEuler::computeGravityForces(struct CpuSoftClothDemoInternalData* clothData, char* vertexPositions, int vertexStride, float dt)
 {
 	
 	B3_PROFILE("computeForces");
@@ -24,11 +24,11 @@ void ExplicitEuler::computeForces(struct CpuSoftClothDemoInternalData* clothData
 		}
 	}
 
+}
 	
+void ExplicitEuler::computeSpringForces(struct CpuSoftClothDemoInternalData* clothData, char* vertexPositions, int vertexStride, float dt)
+{
 	
-
-
-
 	//add spring forces
 	for(int i=0;i<clothData->m_springs.size();i++) 
 	{
@@ -68,9 +68,7 @@ void ExplicitEuler::computeForces(struct CpuSoftClothDemoInternalData* clothData
 			}
 		}
 	}
-	
 }
-	
 void ExplicitEuler::integrateExplicitEuler(struct CpuSoftClothDemoInternalData* clothData, char* vertexPositions, int vertexStride,float deltaTime)
 {
 	B3_PROFILE("integrateEuler");
@@ -100,7 +98,8 @@ void ExplicitEuler::integrateExplicitEuler(struct CpuSoftClothDemoInternalData*
 
 void ExplicitEuler::solveConstraints(struct CpuSoftClothDemoInternalData* data, char* vertexPositions, int vertexStride,float deltaTime)
 {
-	computeForces(data,vertexPositions,vertexStride,deltaTime);
+	computeGravityForces(data,vertexPositions,vertexStride,deltaTime);
+	computeSpringForces(data,vertexPositions,vertexStride,deltaTime);
 
 	integrateExplicitEuler(data,vertexPositions,vertexStride,deltaTime);
 }

Demos3/CpuDemos/deformable/ExplicitEuler.h

@@ -4,11 +4,12 @@
 
 struct ExplicitEuler
 {
-	static void computeForces(struct CpuSoftClothDemoInternalData* data, char* vtx, int vertexStride, float dt);
+	static void computeGravityForces(struct CpuSoftClothDemoInternalData* clothData, char* vtx, int vertexStride, float dt);
-	
+	static void	computeSpringForces(struct CpuSoftClothDemoInternalData* clothData, char* vertexPositions, int vertexStride, float dt);
-	static void integrateExplicitEuler(struct CpuSoftClothDemoInternalData* data, char* vtx, int vertexStride,float dt);
 
-	static void solveConstraints(struct CpuSoftClothDemoInternalData* data, char* vtx, int vertexStride,float dt);
+	static void integrateExplicitEuler(struct CpuSoftClothDemoInternalData* clothData, char* vtx, int vertexStride,float dt);
+
+	static void solveConstraints(struct CpuSoftClothDemoInternalData* clothData, char* vtx, int vertexStride,float dt);
 	
 };
 

Demos3/CpuDemos/deformable/PositionBasedDynamics.cpp

@@ -0,0 +1,54 @@
+
+#include "PositionBasedDynamics.h"
+#include "CpuSoftClothDemoInternalData.h"
+#include "ExplicitEuler.h"
+
+void PositionBasedDynamics::solveLinks(struct CpuSoftClothDemoInternalData* clothData, char* vertexPositions, int vertexStride,float dt)
+{
+	float kst = 1.f;
+	float kLST = 1.f;
+
+	
+	for(int i=0; i<clothData->m_springs.size();i++)
+	{			
+
+		ClothSpring&	link=clothData->m_springs[i];
+
+		//if(l.m_c0>0)
+		{
+
+			float invMassA = clothData->m_particleMasses[link.m_particleIndexA]? 1.f/clothData->m_particleMasses[link.m_particleIndexA] : 0.f;
+			float invMassB = clothData->m_particleMasses[link.m_particleIndexB]? 1.f/clothData->m_particleMasses[link.m_particleIndexB] : 0.f;
+
+			float			m_c0  = (invMassA+invMassB)*kLST;
+			float			m_c1  = link.m_restLength*link.m_restLength;
+						
+			b3Vector3& posA = (b3Vector3&)vertexPositions[link.m_particleIndexA*vertexStride];
+			b3Vector3& posB = (b3Vector3&)vertexPositions[link.m_particleIndexB*vertexStride];
+
+			const b3Vector3	del=posB-posA;
+			const float len=del.length2();
+			if (m_c1+len > B3_EPSILON)
+			{
+				const float k=((m_c1-len)/(m_c0*(m_c1+len)))*kst;
+				posA-=del*(k*invMassA);
+				posB+=del*(k*invMassB);
+			}
+		}
+	}
+
+}
+
+void PositionBasedDynamics::solveConstraints(struct CpuSoftClothDemoInternalData* clothData, char* vtx, int vertexStride,float dt)
+{
+	B3_PROFILE("computeGravityForces");
+	ExplicitEuler::computeGravityForces(clothData,vtx,vertexStride,dt);
+	ExplicitEuler::integrateExplicitEuler(clothData,vtx,vertexStride,dt);
+
+	int numIter=10;
+	for (int i=0;i<numIter;i++)
+	{
+		solveLinks(clothData,vtx,vertexStride,dt);
+	}
+
+};

Demos3/CpuDemos/deformable/PositionBasedDynamics.h

@@ -0,0 +1,10 @@
+#ifndef POSITION_BASED_DYNAMICS_H
+#define POSITION_BASED_DYNAMICS_H
+
+struct PositionBasedDynamics
+{
+	static void solveLinks(struct CpuSoftClothDemoInternalData* clothData, char* vtx, int vertexStride,float dt);
+	static void solveConstraints(struct CpuSoftClothDemoInternalData* data, char* vtx, int vertexStride,float dt);
+};
+
+#endif //POSITION_BASED_DYNAMICS_H