Merge pull request #4170 from fuchuyuan/reconstruct

Reconstruct

12 files changed, 525 insertions, 14 deletions

diff --git a/examples/DeformableDemo/LoadDeformed.cpp b/examples/DeformableDemo/LoadDeformed.cpp
new file mode 100644
index 000000000..55c322779
--- /dev/null
+++ b/examples/DeformableDemo/LoadDeformed.cpp
@@ -0,0 +1,261 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ 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.
+ */
+
+#include "LoadDeformed.h"
+///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
+#include "btBulletDynamicsCommon.h"
+#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
+#include "BulletSoftBody/btSoftBody.h"
+#include "BulletSoftBody/btSoftBodyHelpers.h"
+#include "BulletSoftBody/btDeformableBodySolver.h"
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include <stdio.h>  //printf debugging
+
+#include "../CommonInterfaces/CommonDeformableBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+#include "../Utils/b3BulletDefaultFileIO.h"
+
+class LoadDeformed : public CommonDeformableBodyBase
+{
+	int steps;
+	btSoftBody* psb;
+	char filename;
+	int reset_frame;
+	float sim_time;
+
+public:
+	LoadDeformed(struct GUIHelperInterface* helper)
+		: CommonDeformableBodyBase(helper)
+	{
+		steps = 0;
+		psb = nullptr;
+		reset_frame = 0;
+		sim_time = 0;
+	}
+	virtual ~LoadDeformed()
+	{
+	}
+	void initPhysics();
+
+	void exitPhysics();
+
+	void resetCamera()
+	{
+		float dist = 2;
+		float pitch = -45;
+		float yaw = 100;
+		float targetPos[3] = {0, 0, 0};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+
+	void stepSimulation(float deltaTime)
+	{
+		steps++;
+		sim_time += deltaTime;
+		////        int seconds = 1/deltaTime;
+		if (0)
+		{
+			//        if (reset_frame==0 && steps<100){
+			////            printf("steps %d, seconds %d, steps/seconds %d\n", steps,seconds,steps/seconds);
+			char filename[100];
+			sprintf(filename, "%s_%d_%d.txt", "states", reset_frame, steps);
+			btSoftBodyHelpers::writeState(filename, psb);
+		}
+		if (sim_time + reset_frame * 0.05 >= 5) exit(0);
+		float internalTimeStep = 1. / 240.f;
+		//        float internalTimeStep = 0.1f;
+		m_dynamicsWorld->stepSimulation(deltaTime, deltaTime / internalTimeStep, internalTimeStep);
+	}
+
+	void addCloth(const btVector3& origin);
+
+	virtual void renderScene()
+	{
+		CommonDeformableBodyBase::renderScene();
+		btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
+
+		for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
+		{
+			btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i];
+			{
+				btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
+				btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
+			}
+		}
+	}
+};
+
+void LoadDeformed::initPhysics()
+{
+	m_guiHelper->setUpAxis(1);
+
+	///collision configuration contains default setup for memory, collision setup
+	m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
+
+	///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
+	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+	m_broadphase = new btDbvtBroadphase();
+	btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver();
+
+	btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver();
+	sol->setDeformableSolver(deformableBodySolver);
+	m_solver = sol;
+
+	m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
+	btVector3 gravity = btVector3(0, -9.8, 0);
+	m_dynamicsWorld->setGravity(gravity);
+	getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
+	getDeformableDynamicsWorld()->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(0.25);
+
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+	{
+		///create a ground
+		btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(2.5), btScalar(150.)));
+		groundShape->setMargin(0.02);
+		m_collisionShapes.push_back(groundShape);
+
+		btTransform groundTransform;
+		groundTransform.setIdentity();
+		groundTransform.setOrigin(btVector3(0, -3.5, 0));
+		groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0));
+		btScalar mass(0.);
+
+		//rigidbody is dynamic if and only if mass is non zero, otherwise static
+		bool isDynamic = (mass != 0.f);
+
+		btVector3 localInertia(0, 0, 0);
+		if (isDynamic)
+			groundShape->calculateLocalInertia(mass, localInertia);
+
+		//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
+		btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
+		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
+		btRigidBody* body = new btRigidBody(rbInfo);
+		body->setFriction(4);
+
+		//add the ground to the dynamics world
+		m_dynamicsWorld->addRigidBody(body);
+	}
+	addCloth(btVector3(0, 1, 0));
+	getDeformableDynamicsWorld()->setImplicit(false);
+	getDeformableDynamicsWorld()->setLineSearch(false);
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void LoadDeformed::addCloth(const btVector3& origin)
+// create a piece of cloth
+{
+	const btScalar s = 0.6;
+	const btScalar h = 0;
+
+	psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -2 * s),
+										 btVector3(+s, h, -2 * s),
+										 btVector3(-s, h, +2 * s),
+										 btVector3(+s, h, +2 * s),
+										 15, 30,
+										 0, true, 0.0);
+
+	psb->getCollisionShape()->setMargin(0.02);
+	psb->generateBendingConstraints(2);
+	psb->setTotalMass(.5);
+	psb->m_cfg.kKHR = 1;  // collision hardness with kinematic objects
+	psb->m_cfg.kCHR = 1;  // collision hardness with rigid body
+	psb->m_cfg.kDF = 0.1;
+	psb->rotate(btQuaternion(0, SIMD_PI / 2, 0));
+	btTransform clothTransform;
+	clothTransform.setIdentity();
+	clothTransform.setOrigin(btVector3(0, 0.2, 0) + origin);
+	psb->transform(clothTransform);
+
+	b3BulletDefaultFileIO fileio;
+	char absolute_path[1024];
+	char filename[100];
+	sprintf(filename, "/Users/fuchuyuan/Documents/mybullet/build_cmake/examples/ExampleBrowser/states_0_%d.txt", reset_frame);
+	fileio.findResourcePath(filename, absolute_path, 1024);
+	btAlignedObjectArray<btVector3> qs;
+	btAlignedObjectArray<btVector3> vs;
+	btSoftBodyHelpers::loadDeformableState(qs, vs, absolute_path, &fileio);
+	if (reset_frame > 0)
+		psb->updateState(qs, vs);
+	psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+	psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF;
+	psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF;
+	//    psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN;
+	psb->setCollisionFlags(0);
+	psb->setCacheBarycenter(true);
+	getDeformableDynamicsWorld()->addSoftBody(psb);
+	psb->setSelfCollision(false);
+
+	btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2, 0.2, true);
+	psb->setSpringStiffness(4);
+	getDeformableDynamicsWorld()->addForce(psb, mass_spring);
+	m_forces.push_back(mass_spring);
+	btVector3 gravity = btVector3(0, -9.8, 0);
+	btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity);
+	getDeformableDynamicsWorld()->addForce(psb, gravity_force);
+	//    getDeformableDynamicsWorld()->setUseProjection(true);
+	m_forces.push_back(gravity_force);
+}
+
+void LoadDeformed::exitPhysics()
+{
+	//cleanup in the reverse order of creation/initialization
+	removePickingConstraint();
+	//remove the rigidbodies from the dynamics world and delete them
+	int i;
+	for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
+	{
+		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
+		btRigidBody* body = btRigidBody::upcast(obj);
+		if (body && body->getMotionState())
+		{
+			delete body->getMotionState();
+		}
+		m_dynamicsWorld->removeCollisionObject(obj);
+		delete obj;
+	}
+	// delete forces
+	for (int j = 0; j < m_forces.size(); j++)
+	{
+		btDeformableLagrangianForce* force = m_forces[j];
+		delete force;
+	}
+	m_forces.clear();
+
+	//delete collision shapes
+	for (int j = 0; j < m_collisionShapes.size(); j++)
+	{
+		btCollisionShape* shape = m_collisionShapes[j];
+		delete shape;
+	}
+	m_collisionShapes.clear();
+
+	delete m_dynamicsWorld;
+
+	delete m_solver;
+
+	delete m_broadphase;
+
+	delete m_dispatcher;
+
+	delete m_collisionConfiguration;
+}
+
+class CommonExampleInterface* LoadDeformedCreateFunc(struct CommonExampleOptions& options)
+{
+	return new LoadDeformed(options.m_guiHelper);
+}
diff --git a/examples/DeformableDemo/LoadDeformed.h b/examples/DeformableDemo/LoadDeformed.h
new file mode 100644
index 000000000..b2c1a1598
--- /dev/null
+++ b/examples/DeformableDemo/LoadDeformed.h
@@ -0,0 +1,19 @@
+/*
+ Bullet Continuous Collision Detection and Physics Library
+ Copyright (c) 2019 Google Inc. http://bulletphysics.org
+ 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.
+ */
+
+#ifndef _LOAD_DEFORMED_H
+#define _LOAD_DEFORMED_H
+
+class CommonExampleInterface* LoadDeformedCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //_LOAD_DEFORMED_H
diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt
index 86c7a9388..6271ca915 100644
--- a/examples/ExampleBrowser/CMakeLists.txt
+++ b/examples/ExampleBrowser/CMakeLists.txt
@@ -379,6 +379,8 @@ SET(BulletExampleBrowser_SRCS
   ../DeformableDemo/Collide.cpp
   ../DeformableDemo/Collide.h
   ../DeformableDemo/LargeDeformation.cpp
+  ../DeformableDemo/LoadDeformed.h
+  ../DeformableDemo/LoadDeformed.cpp
   ../DeformableDemo/LargeDeformation.h
   ../DeformableDemo/DeformableClothAnchor.cpp
   ../DeformableDemo/DeformableClothAnchor.h
diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp
index 828e98b0c..715a4396d 100644
--- a/examples/ExampleBrowser/ExampleEntries.cpp
+++ b/examples/ExampleBrowser/ExampleEntries.cpp
@@ -55,6 +55,7 @@
 #include "../DeformableDemo/DeformableMultibody.h"
 #include "../DeformableDemo/VolumetricDeformable.h"
 #include "../DeformableDemo/LargeDeformation.h"
+#include "../DeformableDemo/LoadDeformed.h"
 #include "../DeformableDemo/Collide.h"
 #include "../DeformableDemo/GraspDeformable.h"
 #include "../DeformableDemo/DeformableContact.h"
@@ -218,6 +219,7 @@ static ExampleEntry gDefaultExamples[] =
         ExampleEntry(1, "Grasp Deformable with Motor", "Grasping test", GraspDeformableCreateFunc),
         ExampleEntry(1, "Volumetric Deformable Objects", "Volumetric Deformable test", VolumetricDeformableCreateFunc),
 		ExampleEntry(1, "Extreme Deformation", "Recovery from extreme deformation", LargeDeformationCreateFunc),
+        ExampleEntry(1, "Load Deformed", "Reconstruct a deformed object", LoadDeformedCreateFunc),
 		ExampleEntry(1, "Colliding Test", "Volumetric deformable collide with rigid box", CollideCreateFunc),
         ExampleEntry(1, "Rigid Cloth Anchor", "Deformable Rigid body Anchor test", DeformableClothAnchorCreateFunc),
         ExampleEntry(1, "Multibody Cloth Anchor", "Deformable Multibody Anchor test", MultibodyClothAnchorCreateFunc),
diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index 75f48e39d..ec697cdbf 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -1549,6 +1549,22 @@ B3_SHARED_API void b3GetMeshDataSetFlags(b3SharedMemoryCommandHandle commandHand
 	}
 }
 
+B3_SHARED_API void b3GetMeshDataSimulationMesh(b3SharedMemoryCommandHandle commandHandle)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+	b3Assert(command);
+	b3Assert(command->m_type == CMD_REQUEST_MESH_DATA);
+	command->m_updateFlags |= B3_MESH_DATA_SIMULATION_MESH;
+}
+
+B3_SHARED_API void b3MeshDataSimulationMeshVelocity(b3SharedMemoryCommandHandle commandHandle)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+	b3Assert(command);
+	b3Assert(command->m_type == CMD_REQUEST_MESH_DATA || command->m_type == CMD_RESET_MESH_DATA);
+	command->m_updateFlags |= B3_MESH_DATA_SIMULATION_MESH_VELOCITY;
+}
+
 B3_SHARED_API void b3GetMeshData(b3PhysicsClientHandle physClient, struct b3MeshData* meshData)
 {
 	PhysicsClient* cl = (PhysicsClient*)physClient;
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index c14feb82f..fb0ac3d7e 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -532,6 +532,8 @@ extern "C"
 	B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveCollisionShapeCommand(b3PhysicsClientHandle physClient, int collisionShapeId);
 
 	B3_SHARED_API b3SharedMemoryCommandHandle b3GetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex);
+	B3_SHARED_API void b3GetMeshDataSimulationMesh(b3SharedMemoryCommandHandle commandHandle);
+	B3_SHARED_API void b3MeshDataSimulationMeshVelocity(b3SharedMemoryCommandHandle commandHandle);
 	B3_SHARED_API void b3GetMeshDataSetCollisionShapeIndex(b3SharedMemoryCommandHandle commandHandle, int shapeIndex);
 	B3_SHARED_API void b3GetMeshDataSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
 	
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index faad9e634..ebbfbdc80 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -5551,10 +5551,23 @@ bool PhysicsServerCommandProcessor::processResetMeshDataCommand(const struct Sha
 			int numVertices = psb->m_nodes.size();
 			if (clientCmd.m_resetMeshDataArgs.m_numVertices == numVertices)
 			{
-				for (int i = 0; i < numVertices; ++i)
+				if (clientCmd.m_updateFlags & B3_MESH_DATA_SIMULATION_MESH_VELOCITY)
 				{
-					btSoftBody::Node& n = psb->m_nodes[i];
-					n.m_x.setValue(vertexUpload[i*3+0], vertexUpload[i*3+1],vertexUpload[i*3+2]);
+					for (int i = 0; i < numVertices; ++i)
+					{
+						btSoftBody::Node& n = psb->m_nodes[i];
+						n.m_v.setValue(vertexUpload[i * 3 + 0], vertexUpload[i * 3 + 1], vertexUpload[i * 3 + 2]);
+						n.m_vn.setValue(vertexUpload[i * 3 + 0], vertexUpload[i * 3 + 1], vertexUpload[i * 3 + 2]);
+					}
+				}
+				else
+				{
+					for (int i = 0; i < numVertices; ++i)
+					{
+						btSoftBody::Node& n = psb->m_nodes[i];
+						n.m_x.setValue(vertexUpload[i * 3 + 0], vertexUpload[i * 3 + 1], vertexUpload[i * 3 + 2]);
+						n.m_q.setValue(vertexUpload[i * 3 + 0], vertexUpload[i * 3 + 1], vertexUpload[i * 3 + 2]);
+					}
 				}
 				serverStatusOut.m_type = CMD_RESET_MESH_DATA_COMPLETED;
 			}
@@ -5641,10 +5654,12 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
 			}
 
 			bool separateRenderMesh = false;
-			if ((flags & B3_MESH_DATA_SIMULATION_MESH) == 0)
+			if ((clientCmd.m_updateFlags & B3_MESH_DATA_SIMULATION_MESH) == 0)
 			{
 				separateRenderMesh = (psb->m_renderNodes.size() != 0);
 			}
+            bool requestVelocity = clientCmd.m_updateFlags & B3_MESH_DATA_SIMULATION_MESH_VELOCITY;
+            
 			int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size();
 			int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1;
 			int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex;
@@ -5653,14 +5668,22 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
 			{
 				if (separateRenderMesh)
 				{
-					
 					const btSoftBody::RenderNode& n = psb->m_renderNodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex];
+                    if(requestVelocity){
+                        b3Warning("Request mesh velocity not implemented for Render Mesh.");
+                        return hasStatus;
+                    }
 					verticesOut[i].setValue(n.m_x.x(), n.m_x.y(), n.m_x.z());
 				}
 				else
 				{
 					const btSoftBody::Node& n = psb->m_nodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex];
-					verticesOut[i].setValue(n.m_x.x(), n.m_x.y(), n.m_x.z());
+                    if(!requestVelocity){
+                        verticesOut[i].setValue(n.m_x.x(), n.m_x.y(), n.m_x.z());
+                    }
+                    else{
+                        verticesOut[i].setValue(n.m_v.x(), n.m_v.y(), n.m_v.z());
+                    }
 				}
 			}
 			sizeInBytes = verticesCopied * sizeof(btVector3);
diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h
index aab74a3ea..7541ddb5a 100644
--- a/examples/SharedMemory/SharedMemoryPublic.h
+++ b/examples/SharedMemory/SharedMemoryPublic.h
@@ -461,12 +461,12 @@ struct b3MeshVertex
 	double x, y, z, w;
 };
 
-
 enum eMeshDataFlags
 {
-	B3_MESH_DATA_SIMULATION_MESH=1,
-	B3_MESH_DATA_SIMULATION_INDICES,
-	B3_MESH_DATA_GRAPHICS_INDICES,
+	B3_MESH_DATA_SIMULATION_MESH = 1,
+	B3_MESH_DATA_SIMULATION_INDICES = 2,
+	B3_MESH_DATA_GRAPHICS_INDICES = 4,
+	B3_MESH_DATA_SIMULATION_MESH_VELOCITY = 8,
 };
 
 enum eMeshDataEnum
diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp
index 7b7a3ab3c..933f3edc5 100644
--- a/src/BulletSoftBody/btSoftBody.cpp
+++ b/src/BulletSoftBody/btSoftBody.cpp
@@ -1486,6 +1486,21 @@ void btSoftBody::randomizeConstraints()
 #undef NEXTRAND
 }
 
+void btSoftBody::updateState(const btAlignedObjectArray<btVector3>& q, const btAlignedObjectArray<btVector3>& v)
+{
+	int node_count = m_nodes.size();
+	btAssert(node_count == q.size());
+	btAssert(node_count == v.size());
+	for (int i = 0; i < node_count; i++)
+	{
+		Node& n = m_nodes[i];
+		n.m_x = q[i];
+		n.m_q = q[i];
+		n.m_v = v[i];
+		n.m_vn = v[i];
+	}
+}
+
 //
 void btSoftBody::releaseCluster(int index)
 {
@@ -2824,7 +2839,7 @@ bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colO
 	btScalar dst;
 	btGjkEpaSolver2::sResults results;
 
-//	#define USE_QUADRATURE 1
+	//	#define USE_QUADRATURE 1
 
 	// use collision quadrature point
 #ifdef USE_QUADRATURE
diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h
index 89842d70d..f91640acb 100644
--- a/src/BulletSoftBody/btSoftBody.h
+++ b/src/BulletSoftBody/btSoftBody.h
@@ -298,7 +298,7 @@ public:
 	};
 	struct RenderFace
 	{
-		RenderNode* m_n[3];          // Node pointers
+		RenderNode* m_n[3];  // Node pointers
 	};
 
 	/* Face			*/
@@ -787,7 +787,7 @@ public:
 	typedef btAlignedObjectArray<Cluster*> tClusterArray;
 	typedef btAlignedObjectArray<Note> tNoteArray;
 	typedef btAlignedObjectArray<Node> tNodeArray;
-	typedef btAlignedObjectArray< RenderNode> tRenderNodeArray;
+	typedef btAlignedObjectArray<RenderNode> tRenderNodeArray;
 	typedef btAlignedObjectArray<btDbvtNode*> tLeafArray;
 	typedef btAlignedObjectArray<Link> tLinkArray;
 	typedef btAlignedObjectArray<Face> tFaceArray;
@@ -815,7 +815,7 @@ public:
 	tRenderNodeArray m_renderNodes;    // Render Nodes
 	tLinkArray m_links;                // Links
 	tFaceArray m_faces;                // Faces
-	tRenderFaceArray m_renderFaces;          // Faces
+	tRenderFaceArray m_renderFaces;    // Faces
 	tTetraArray m_tetras;              // Tetras
 	btAlignedObjectArray<TetraScratch> m_tetraScratches;
 	btAlignedObjectArray<TetraScratch> m_tetraScratchesTn;
@@ -1056,6 +1056,9 @@ public:
 								   Material* mat = 0);
 	/* Randomize constraints to reduce solver bias							*/
 	void randomizeConstraints();
+
+	void updateState(const btAlignedObjectArray<btVector3>& qs, const btAlignedObjectArray<btVector3>& vs);
+
 	/* Release clusters														*/
 	void releaseCluster(int index);
 	void releaseClusters();
diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp
index e464c4668..35eb22ec1 100644
--- a/src/BulletSoftBody/btSoftBodyHelpers.cpp
+++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp
@@ -18,6 +18,7 @@ subject to the following restrictions:
 #include <stdio.h>
 #include <string>
 #include <iostream>
+#include <iomanip> 
 #include <sstream>
 #include <string.h>
 #include <algorithm>
@@ -1487,6 +1488,168 @@ void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb)
 	fs.close();
 }
 
+static inline bool isSpace(const char c)
+{
+	return (c == ' ') || (c == '\t');
+}
+static inline bool isNewLine(const char c)
+{
+	return (c == '\r') || (c == '\n') || (c == '\0');
+}
+static inline float parseFloat(const char*& token)
+{
+	token += strspn(token, " \t");
+	float f = (float)atof(token);
+	token += strcspn(token, " \t\r");
+	return f;
+}
+static inline void parseFloat3(
+	float& x, float& y, float& z,
+	const char*& token)
+{
+	x = parseFloat(token);
+	y = parseFloat(token);
+	z = parseFloat(token);
+}
+
+void btSoftBodyHelpers::writeState(const char* file, const btSoftBody* psb)
+{
+	std::ofstream fs;
+	fs.open(file);
+	btAssert(fs);
+	fs << std::scientific << std::setprecision(16);
+
+	// Only write out for trimesh, directly write out all the nodes and faces.xs
+	for (int i = 0; i < psb->m_nodes.size(); ++i)
+	{
+		fs << "q";
+		for (int d = 0; d < 3; d++)
+		{
+			fs << " " << psb->m_nodes[i].m_q[d];
+		}
+		fs << "\n";
+	}
+
+	for (int i = 0; i < psb->m_nodes.size(); ++i)
+	{
+		fs << "v";
+		for (int d = 0; d < 3; d++)
+		{
+			fs << " " << psb->m_nodes[i].m_v[d];
+		}
+		fs << "\n";
+	}
+	fs.close();
+}
+
+std::string btSoftBodyHelpers::loadDeformableState(btAlignedObjectArray<btVector3>& qs, btAlignedObjectArray<btVector3>& vs, const char* filename, CommonFileIOInterface* fileIO)
+{
+	{
+		qs.clear();
+		vs.clear();
+		std::string tmp = filename;
+		std::stringstream err;
+#ifdef USE_STREAM
+		std::ifstream ifs(filename);
+		if (!ifs)
+		{
+			err << "Cannot open file [" << filename << "]" << std::endl;
+			return err.str();
+		}
+#else
+		int fileHandle = fileIO->fileOpen(filename, "r");
+		if (fileHandle < 0)
+		{
+			err << "Cannot open file [" << filename << "]" << std::endl;
+			return err.str();
+		}
+#endif
+
+		std::string name;
+
+		int maxchars = 8192;              // Alloc enough size.
+		std::vector<char> buf(maxchars);  // Alloc enough size.
+		std::string linebuf;
+		linebuf.reserve(maxchars);
+
+#ifdef USE_STREAM
+		while (ifs.peek() != -1)
+#else
+		char* line = 0;
+		do
+#endif
+		{
+			linebuf.resize(0);
+#ifdef USE_STREAM
+			safeGetline(ifs, linebuf);
+#else
+			char tmpBuf[1024];
+			line = fileIO->readLine(fileHandle, tmpBuf, 1024);
+			if (line)
+			{
+				linebuf = line;
+			}
+#endif
+			// Trim newline '\r\n' or '\r'
+			if (linebuf.size() > 0)
+			{
+				if (linebuf[linebuf.size() - 1] == '\n') linebuf.erase(linebuf.size() - 1);
+			}
+			if (linebuf.size() > 0)
+			{
+				if (linebuf[linebuf.size() - 1] == '\n') linebuf.erase(linebuf.size() - 1);
+			}
+
+			// Skip if empty line.
+			if (linebuf.empty())
+			{
+				continue;
+			}
+
+			// Skip leading space.
+			const char* token = linebuf.c_str();
+			token += strspn(token, " \t");
+
+			btAssert(token);
+			if (token[0] == '\0') continue;  // empty line
+
+			if (token[0] == '#') continue;  // comment line
+
+			// q
+			if (token[0] == 'q' && isSpace((token[1])))
+			{
+				token += 2;
+				float x, y, z;
+				parseFloat3(x, y, z, token);
+				qs.push_back(btVector3(x, y, z));
+				continue;
+			}
+
+			// v
+			if (token[0] == 'v' && isSpace((token[1])))
+			{
+				token += 3;
+				float x, y, z;
+				parseFloat3(x, y, z, token);
+				vs.push_back(btVector3(x, y, z));
+				continue;
+			}
+
+			// Ignore unknown command.
+		}
+#ifndef USE_STREAM
+		while (line)
+			;
+#endif
+
+		if (fileHandle >= 0)
+		{
+			fileIO->fileClose(fileHandle);
+		}
+		return err.str();
+	}
+}
+
 void btSoftBodyHelpers::duplicateFaces(const char* filename, const btSoftBody* psb)
 {
 	std::ifstream fs_read;
diff --git a/src/BulletSoftBody/btSoftBodyHelpers.h b/src/BulletSoftBody/btSoftBodyHelpers.h
index b292d90d1..d1fdf0450 100644
--- a/src/BulletSoftBody/btSoftBodyHelpers.h
+++ b/src/BulletSoftBody/btSoftBodyHelpers.h
@@ -17,6 +17,7 @@ subject to the following restrictions:
 #define BT_SOFT_BODY_HELPERS_H
 
 #include "btSoftBody.h"
+#include "../../examples/CommonInterfaces/CommonFileIOInterface.h"
 #include <fstream>
 #include <string>
 //
@@ -146,6 +147,10 @@ struct btSoftBodyHelpers
 
 	static void writeObj(const char* file, const btSoftBody* psb);
 
+	static void writeState(const char* file, const btSoftBody* psb);
+
+	static std::string loadDeformableState(btAlignedObjectArray<btVector3>& qs, btAlignedObjectArray<btVector3>& vs, const char* filename, CommonFileIOInterface* fileIO);
+
 	static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary);
 
 	static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& p, btVector4& bary);