add Thomas Jacobsen / position based dynamics cloth constraint solver

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