From 01d62b12bba7b517d53e2d9be9053716ec127bf4 Mon Sep 17 00:00:00 2001
From: erwincoumans <erwin.coumans@gmail.com>
Date: Fri, 6 Sep 2013 23:18:14 -0700
Subject: [PATCH] adjust velocity after solving PBD springs/links

---
 .../CpuDemos/deformable/CpuSoftBodyDemo.cpp   |  9 +++----
 .../deformable/PositionBasedDynamics.cpp      | 26 ++++++++++++++++---
 2 files changed, 25 insertions(+), 10 deletions(-)

diff --git a/Demos3/CpuDemos/deformable/CpuSoftBodyDemo.cpp b/Demos3/CpuDemos/deformable/CpuSoftBodyDemo.cpp
index 6afa9acae..208c9131a 100644
--- a/Demos3/CpuDemos/deformable/CpuSoftBodyDemo.cpp
+++ b/Demos3/CpuDemos/deformable/CpuSoftBodyDemo.cpp
@@ -23,8 +23,8 @@ extern bool gReset;
 float clothWidth = 4;
 float clothHeight= 4;
 
-int width = 32;//64;
-int height = 32;//64;
+int width = 64;
+int height = 64;
 
 int numPoints = width*height;
 float clothMass = 100.f;
@@ -153,7 +153,7 @@ void CpuSoftClothDemo::clientMoveAndDisplay()
 	if (m_data->m_clothShapeIndex>=0 && m_data->m_clothVertices)
 	{
 		
-		float deltaTime = 1./1000.;//1./60.f;
+		float deltaTime = 1./100.;//1./60.f;
 		//float deltaTime = 1./60.f;
 		
 		//write positions
@@ -266,9 +266,6 @@ void	CpuSoftClothDemo::setupScene(const ConstructionInfo& ci)
 	{
 		for(int x = 0; x < width; x++)
 		{
-			double coord = b3Sin(x/5.0)*0.01+1;
-			//coord = sin(y/);
-
 			float posX = (x/((float)(width-1)))*(clothWidth);
 			float posZ = ((y-height/2.f)/((float)(height-1)))*(clothHeight);
 
diff --git a/Demos3/CpuDemos/deformable/PositionBasedDynamics.cpp b/Demos3/CpuDemos/deformable/PositionBasedDynamics.cpp
index 9edc11dca..84dea8eb6 100644
--- a/Demos3/CpuDemos/deformable/PositionBasedDynamics.cpp
+++ b/Demos3/CpuDemos/deformable/PositionBasedDynamics.cpp
@@ -6,15 +6,22 @@
 void PositionBasedDynamics::solveLinks(struct CpuSoftClothDemoInternalData* clothData, char* vertexPositions, int vertexStride,float dt)
 {
 	float kst = 1.f;
-	float kLST = 1.f;
+	float kLST = 1.f;//0.8f;
+	float damping = 0.999;
+	int numPoints = clothData->m_particleMasses.size();
 
+	b3AlignedObjectArray<b3Vector3> startPositions;
+	
+	startPositions.resize(numPoints);
+	for (int i=0;i<numPoints;i++)
+	{
+		startPositions[i] = (b3Vector3&)vertexPositions[i*vertexStride];
+	}
 	
 	for(int i=0; i<clothData->m_springs.size();i++)
 	{			
-
 		ClothSpring&	link=clothData->m_springs[i];
-
-		//if(l.m_c0>0)
+		//if(l.m_c0>0)//avoid links between two particles with mass==0
 		{
 
 			float invMassA = clothData->m_particleMasses[link.m_particleIndexA]? 1.f/clothData->m_particleMasses[link.m_particleIndexA] : 0.f;
@@ -37,6 +44,17 @@ void PositionBasedDynamics::solveLinks(struct CpuSoftClothDemoInternalData* clot
 		}
 	}
 
+	
+
+	//adjust velocity
+	for (int i=0;i<numPoints;i++)
+	{
+		b3Vector3 endPosition = (b3Vector3&)vertexPositions[i*vertexStride];
+		b3Vector3 diff = endPosition-startPositions[i];
+
+		clothData->m_velocities[i]+=(diff/dt);
+		clothData->m_velocities[i]*=damping;
+	}
 }
 
 void PositionBasedDynamics::solveConstraints(struct CpuSoftClothDemoInternalData* clothData, char* vtx, int vertexStride,float dt)