From 4fe86d2a1d34b15cb92981da51715657e102aca6 Mon Sep 17 00:00:00 2001
From: yunfeibai <byf1658@gmail.com>
Date: Tue, 11 Oct 2016 16:15:44 -0700
Subject: [PATCH 01/11] Add btSoftMultiBodyDynamicsWorld.

---
 examples/RoboticsLearning/b3RobotSimAPI.cpp   |   2 +
 examples/SoftDemo/SoftDemo.cpp                |  10 +-
 src/BulletDynamics/CMakeLists.txt             |   2 +
 src/BulletDynamics/Dynamics/btDynamicsWorld.h |   3 +-
 .../btSoftMultiBodyDynamicsWorld.cpp          | 367 ++++++++++++++++++
 .../btSoftMultiBodyDynamicsWorld.h            | 108 ++++++
 6 files changed, 487 insertions(+), 5 deletions(-)
 create mode 100644 src/BulletDynamics/Featherstone/btSoftMultiBodyDynamicsWorld.cpp
 create mode 100644 src/BulletDynamics/Featherstone/btSoftMultiBodyDynamicsWorld.h

diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp
index 068bf1687..a0636381b 100644
--- a/examples/RoboticsLearning/b3RobotSimAPI.cpp
+++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp
@@ -961,10 +961,12 @@ void b3RobotSimAPI::renderScene()
 		}
 	}
 
+    
 	if (m_data->m_clientServerDirect)
 	{
 		m_data->m_clientServerDirect->renderScene();
 	}
+    
 	m_data->m_physicsServer.renderScene();
 }
 
diff --git a/examples/SoftDemo/SoftDemo.cpp b/examples/SoftDemo/SoftDemo.cpp
index 9f7f8144a..6b1611825 100644
--- a/examples/SoftDemo/SoftDemo.cpp
+++ b/examples/SoftDemo/SoftDemo.cpp
@@ -1017,7 +1017,7 @@ static void	Init_Bunny(SoftDemo* pdemo)
 	psb->m_cfg.kDF			=	0.5;
 	psb->randomizeConstraints();
 	psb->scale(btVector3(6,6,6));
-	psb->setTotalMass(100,true);	
+	psb->setTotalMass(1,true);	
 	pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 	pdemo->m_cutting=true;
 
@@ -2097,6 +2097,7 @@ void	SoftDemo::initPhysics()
 	motorcontrol.maxtorque = 0;
 
 	btCollisionShape* groundShape = 0;
+    if (0)
 	{
 		int i;
 		int j;
@@ -2151,11 +2152,12 @@ void	SoftDemo::initPhysics()
 		groundShape->setMargin(0.5);
 	}
 
-	m_collisionShapes.push_back(groundShape);
+	//m_collisionShapes.push_back(groundShape);
 
 	btCollisionShape* groundBox = new btBoxShape (btVector3(100,CUBE_HALF_EXTENTS,100));
 	m_collisionShapes.push_back(groundBox);
 
+    /*
 	btCompoundShape* cylinderCompound = new btCompoundShape;
 	btCollisionShape* cylinderShape = new btCylinderShape (btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS));
 	btTransform localTransform;
@@ -2166,7 +2168,7 @@ void	SoftDemo::initPhysics()
 	cylinderCompound->addChildShape(localTransform,cylinderShape);
 
 	m_collisionShapes.push_back(cylinderCompound);
-
+    */
 
 	m_dispatcher=0;
 
@@ -2260,7 +2262,7 @@ void	SoftDemo::initPhysics()
 		newOb->setCollisionShape(m_collisionShapes[0]);
 	} else
 	{
-		newOb->setCollisionShape(m_collisionShapes[1]);
+		newOb->setCollisionShape(m_collisionShapes[0]);
 	}
 
 	m_dynamicsWorld->addCollisionObject(newOb);
diff --git a/src/BulletDynamics/CMakeLists.txt b/src/BulletDynamics/CMakeLists.txt
index 656fc8433..7e31c13d1 100644
--- a/src/BulletDynamics/CMakeLists.txt
+++ b/src/BulletDynamics/CMakeLists.txt
@@ -29,6 +29,7 @@ SET(BulletDynamics_SRCS
 	Featherstone/btMultiBody.cpp
 	Featherstone/btMultiBodyConstraintSolver.cpp
 	Featherstone/btMultiBodyDynamicsWorld.cpp
+	Featherstone/btSoftMultiBodyDynamicsWorld.cpp
 	Featherstone/btMultiBodyJointLimitConstraint.cpp
 	Featherstone/btMultiBodyConstraint.cpp
 	Featherstone/btMultiBodyPoint2Point.cpp
@@ -84,6 +85,7 @@ SET(Featherstone_HDRS
 	Featherstone/btMultiBody.h
 	Featherstone/btMultiBodyConstraintSolver.h
 	Featherstone/btMultiBodyDynamicsWorld.h
+	Featherstone/btSoftMultiBodyDynamicsWorld.h
 	Featherstone/btMultiBodyLink.h
 	Featherstone/btMultiBodyLinkCollider.h
 	Featherstone/btMultiBodySolverConstraint.h
diff --git a/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/src/BulletDynamics/Dynamics/btDynamicsWorld.h
index 35dd1400f..4d65f5489 100644
--- a/src/BulletDynamics/Dynamics/btDynamicsWorld.h
+++ b/src/BulletDynamics/Dynamics/btDynamicsWorld.h
@@ -34,7 +34,8 @@ enum btDynamicsWorldType
 	BT_DISCRETE_DYNAMICS_WORLD=2,
 	BT_CONTINUOUS_DYNAMICS_WORLD=3,
 	BT_SOFT_RIGID_DYNAMICS_WORLD=4,
-	BT_GPU_DYNAMICS_WORLD=5
+	BT_GPU_DYNAMICS_WORLD=5,
+    BT_SOFT_MULTIBODY_DYNAMICS_WORLD=6
 };
 
 ///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc.
diff --git a/src/BulletDynamics/Featherstone/btSoftMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btSoftMultiBodyDynamicsWorld.cpp
new file mode 100644
index 000000000..ffa243ebf
--- /dev/null
+++ b/src/BulletDynamics/Featherstone/btSoftMultiBodyDynamicsWorld.cpp
@@ -0,0 +1,367 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 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.
+*/
+
+
+#include "btSoftMultiBodyDynamicsWorld.h"
+#include "LinearMath/btQuickprof.h"
+
+//softbody & helpers
+#include "BulletSoftBody/btSoftBody.h"
+#include "BulletSoftBody/btSoftBodyHelpers.h"
+#include "BulletSoftBody/btSoftBodySolvers.h"
+#include "BulletSoftBody/btDefaultSoftBodySolver.h"
+#include "LinearMath/btSerializer.h"
+
+
+btSoftMultiBodyDynamicsWorld::btSoftMultiBodyDynamicsWorld(
+	btDispatcher* dispatcher,
+	btBroadphaseInterface* pairCache,
+	btMultiBodyConstraintSolver* constraintSolver,
+	btCollisionConfiguration* collisionConfiguration,
+	btSoftBodySolver *softBodySolver ) : 
+		btMultiBodyDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration),
+        m_softBodySolver( softBodySolver ),
+        m_ownsSolver(false)
+{
+	if( !m_softBodySolver )
+	{
+		void* ptr = btAlignedAlloc(sizeof(btDefaultSoftBodySolver),16);
+		m_softBodySolver = new(ptr) btDefaultSoftBodySolver();
+		m_ownsSolver = true;
+	}
+
+	m_drawFlags			=	fDrawFlags::Std;
+	m_drawNodeTree		=	true;
+	m_drawFaceTree		=	false;
+	m_drawClusterTree	=	false;
+	m_sbi.m_broadphase = pairCache;
+	m_sbi.m_dispatcher = dispatcher;
+	m_sbi.m_sparsesdf.Initialize();
+	m_sbi.m_sparsesdf.Reset();
+
+	m_sbi.air_density		=	(btScalar)1.2;
+	m_sbi.water_density	=	0;
+	m_sbi.water_offset		=	0;
+	m_sbi.water_normal		=	btVector3(0,0,0);
+	m_sbi.m_gravity.setValue(0,-10,0);
+
+	m_sbi.m_sparsesdf.Initialize();
+
+
+}
+
+btSoftMultiBodyDynamicsWorld::~btSoftMultiBodyDynamicsWorld()
+{
+	if (m_ownsSolver)
+	{
+		m_softBodySolver->~btSoftBodySolver();
+		btAlignedFree(m_softBodySolver);
+	}
+}
+
+void	btSoftMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
+{
+	btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep );
+	{
+		BT_PROFILE("predictUnconstraintMotionSoftBody");
+		m_softBodySolver->predictMotion( float(timeStep) );
+	}
+}
+
+void	btSoftMultiBodyDynamicsWorld::internalSingleStepSimulation( btScalar timeStep )
+{
+
+	// Let the solver grab the soft bodies and if necessary optimize for it
+	m_softBodySolver->optimize( getSoftBodyArray() );
+
+	if( !m_softBodySolver->checkInitialized() )
+	{
+		btAssert( "Solver initialization failed\n" );
+	}
+
+	btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep );
+
+	///solve soft bodies constraints
+	solveSoftBodiesConstraints( timeStep );
+
+	//self collisions
+	for ( int i=0;i<m_softBodies.size();i++)
+	{
+		btSoftBody*	psb=(btSoftBody*)m_softBodies[i];
+		psb->defaultCollisionHandler(psb);
+	}
+
+	///update soft bodies
+	m_softBodySolver->updateSoftBodies( );
+	
+	// End solver-wise simulation step
+	// ///////////////////////////////
+
+}
+
+void	btSoftMultiBodyDynamicsWorld::solveSoftBodiesConstraints( btScalar timeStep )
+{
+	BT_PROFILE("solveSoftConstraints");
+
+	if(m_softBodies.size())
+	{
+		btSoftBody::solveClusters(m_softBodies);
+	}
+
+	// Solve constraints solver-wise
+	m_softBodySolver->solveConstraints( timeStep * m_softBodySolver->getTimeScale() );
+
+}
+
+void	btSoftMultiBodyDynamicsWorld::addSoftBody(btSoftBody* body,short int collisionFilterGroup,short int collisionFilterMask)
+{
+	m_softBodies.push_back(body);
+
+	// Set the soft body solver that will deal with this body
+	// to be the world's solver
+	body->setSoftBodySolver( m_softBodySolver );
+
+	btCollisionWorld::addCollisionObject(body,
+		collisionFilterGroup,
+		collisionFilterMask);
+
+}
+
+void	btSoftMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body)
+{
+	m_softBodies.remove(body);
+
+	btCollisionWorld::removeCollisionObject(body);
+}
+
+void	btSoftMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
+{
+	btSoftBody* body = btSoftBody::upcast(collisionObject);
+	if (body)
+		removeSoftBody(body);
+	else
+		btDiscreteDynamicsWorld::removeCollisionObject(collisionObject);
+}
+
+void	btSoftMultiBodyDynamicsWorld::debugDrawWorld()
+{
+	btDiscreteDynamicsWorld::debugDrawWorld();
+
+	if (getDebugDrawer())
+	{
+		int i;
+		for (  i=0;i<this->m_softBodies.size();i++)
+		{
+			btSoftBody*	psb=(btSoftBody*)this->m_softBodies[i];
+			if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
+			{
+				btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer);
+				btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags);
+			}
+			
+			if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+			{
+				if(m_drawNodeTree)		btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer);
+				if(m_drawFaceTree)		btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer);
+				if(m_drawClusterTree)	btSoftBodyHelpers::DrawClusterTree(psb,m_debugDrawer);
+			}
+		}		
+	}	
+}
+
+
+
+
+struct btSoftSingleRayCallback : public btBroadphaseRayCallback
+{
+	btVector3	m_rayFromWorld;
+	btVector3	m_rayToWorld;
+	btTransform	m_rayFromTrans;
+	btTransform	m_rayToTrans;
+	btVector3	m_hitNormal;
+
+	const btSoftMultiBodyDynamicsWorld*	m_world;
+	btCollisionWorld::RayResultCallback&	m_resultCallback;
+
+	btSoftSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btSoftMultiBodyDynamicsWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
+	:m_rayFromWorld(rayFromWorld),
+	m_rayToWorld(rayToWorld),
+	m_world(world),
+	m_resultCallback(resultCallback)
+	{
+		m_rayFromTrans.setIdentity();
+		m_rayFromTrans.setOrigin(m_rayFromWorld);
+		m_rayToTrans.setIdentity();
+		m_rayToTrans.setOrigin(m_rayToWorld);
+
+		btVector3 rayDir = (rayToWorld-rayFromWorld);
+
+		rayDir.normalize ();
+		///what about division by zero? --> just set rayDirection[i] to INF/1e30
+		m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
+		m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
+		m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
+		m_signs[0] = m_rayDirectionInverse[0] < 0.0;
+		m_signs[1] = m_rayDirectionInverse[1] < 0.0;
+		m_signs[2] = m_rayDirectionInverse[2] < 0.0;
+
+		m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld);
+
+	}
+
+	
+
+	virtual bool	process(const btBroadphaseProxy* proxy)
+	{
+		///terminate further ray tests, once the closestHitFraction reached zero
+		if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
+			return false;
+
+		btCollisionObject*	collisionObject = (btCollisionObject*)proxy->m_clientObject;
+
+		//only perform raycast if filterMask matches
+		if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) 
+		{
+			//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
+			//btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+#if 0
+#ifdef RECALCULATE_AABB
+			btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+			collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
+#else
+			//getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
+			const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
+			const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
+#endif
+#endif
+			//btScalar hitLambda = m_resultCallback.m_closestHitFraction;
+			//culling already done by broadphase
+			//if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
+			{
+				m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
+					collisionObject,
+						collisionObject->getCollisionShape(),
+						collisionObject->getWorldTransform(),
+						m_resultCallback);
+			}
+		}
+		return true;
+	}
+};
+
+void	btSoftMultiBodyDynamicsWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
+{
+	BT_PROFILE("rayTest");
+	/// use the broadphase to accelerate the search for objects, based on their aabb
+	/// and for each object with ray-aabb overlap, perform an exact ray test
+	btSoftSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback);
+
+#ifndef USE_BRUTEFORCE_RAYBROADPHASE
+	m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB);
+#else
+	for (int i=0;i<this->getNumCollisionObjects();i++)
+	{
+		rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
+	}	
+#endif //USE_BRUTEFORCE_RAYBROADPHASE
+
+}
+
+
+void	btSoftMultiBodyDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
+					  btCollisionObject* collisionObject,
+					  const btCollisionShape* collisionShape,
+					  const btTransform& colObjWorldTransform,
+					  RayResultCallback& resultCallback)
+{
+	if (collisionShape->isSoftBody()) {
+		btSoftBody* softBody = btSoftBody::upcast(collisionObject);
+		if (softBody) {
+			btSoftBody::sRayCast softResult;
+			if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) 
+			{
+				
+				if (softResult.fraction<= resultCallback.m_closestHitFraction)
+				{
+
+					btCollisionWorld::LocalShapeInfo shapeInfo;
+					shapeInfo.m_shapePart = 0;
+					shapeInfo.m_triangleIndex = softResult.index;
+					// get the normal
+					btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
+					btVector3 normal=-rayDir;
+					normal.normalize();
+
+					if (softResult.feature == btSoftBody::eFeature::Face)
+					{
+						normal = softBody->m_faces[softResult.index].m_normal;
+						if (normal.dot(rayDir) > 0) {
+							// normal always point toward origin of the ray
+							normal = -normal;
+						}
+					}
+	
+					btCollisionWorld::LocalRayResult rayResult
+						(collisionObject,
+						 &shapeInfo,
+						 normal,
+						 softResult.fraction);
+					bool	normalInWorldSpace = true;
+					resultCallback.addSingleResult(rayResult,normalInWorldSpace);
+				}
+			}
+		}
+	} 
+	else {
+		btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,collisionObject,collisionShape,colObjWorldTransform,resultCallback);
+	}
+}
+
+
+void	btSoftMultiBodyDynamicsWorld::serializeSoftBodies(btSerializer* serializer)
+{
+	int i;
+	//serialize all collision objects
+	for (i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		if (colObj->getInternalType() & btCollisionObject::CO_SOFT_BODY)
+		{
+			int len = colObj->calculateSerializeBufferSize();
+			btChunk* chunk = serializer->allocate(len,1);
+			const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
+			serializer->finalizeChunk(chunk,structType,BT_SOFTBODY_CODE,colObj);
+		}
+	}
+
+}
+
+void	btSoftMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
+{
+
+	serializer->startSerialization();
+
+	serializeDynamicsWorldInfo( serializer);
+
+	serializeSoftBodies(serializer);
+
+	serializeRigidBodies(serializer);
+
+	serializeCollisionObjects(serializer);
+
+	serializer->finishSerialization();
+}
+
+
diff --git a/src/BulletDynamics/Featherstone/btSoftMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btSoftMultiBodyDynamicsWorld.h
new file mode 100644
index 000000000..79ff913d6
--- /dev/null
+++ b/src/BulletDynamics/Featherstone/btSoftMultiBodyDynamicsWorld.h
@@ -0,0 +1,108 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 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.
+*/
+
+#ifndef BT_SOFT_MULTIBODY_DYNAMICS_WORLD_H
+#define BT_SOFT_MULTIBODY_DYNAMICS_WORLD_H
+
+#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
+#include "btMultiBodyDynamicsWorld.h"
+#include "BulletSoftBody/btSoftBody.h"
+
+typedef	btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
+
+class btSoftBodySolver;
+
+class btSoftMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld
+{
+
+	btSoftBodyArray	m_softBodies;
+	int				m_drawFlags;
+	bool			m_drawNodeTree;
+	bool			m_drawFaceTree;
+	bool			m_drawClusterTree;
+	btSoftBodyWorldInfo m_sbi;
+	///Solver classes that encapsulate multiple soft bodies for solving
+	btSoftBodySolver *m_softBodySolver;
+	bool			m_ownsSolver;
+
+protected:
+
+	virtual void	predictUnconstraintMotion(btScalar timeStep);
+
+	virtual void	internalSingleStepSimulation( btScalar timeStep);
+
+	void	solveSoftBodiesConstraints( btScalar timeStep );
+
+	void	serializeSoftBodies(btSerializer* serializer);
+
+public:
+
+	btSoftMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btSoftBodySolver *softBodySolver = 0 );
+
+	virtual ~btSoftMultiBodyDynamicsWorld();
+
+	virtual void	debugDrawWorld();
+
+	void	addSoftBody(btSoftBody* body,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter);
+
+	void	removeSoftBody(btSoftBody* body);
+
+	///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btDiscreteDynamicsWorld::removeCollisionObject
+	virtual void	removeCollisionObject(btCollisionObject* collisionObject);
+
+	int		getDrawFlags() const { return(m_drawFlags); }
+	void	setDrawFlags(int f)	{ m_drawFlags=f; }
+
+	btSoftBodyWorldInfo&	getWorldInfo()
+	{
+		return m_sbi;
+	}
+	const btSoftBodyWorldInfo&	getWorldInfo() const
+	{
+		return m_sbi;
+	}
+
+	virtual btDynamicsWorldType	getWorldType() const
+	{
+		return	BT_SOFT_MULTIBODY_DYNAMICS_WORLD;
+	}
+
+	btSoftBodyArray& getSoftBodyArray()
+	{
+		return m_softBodies;
+	}
+
+	const btSoftBodyArray& getSoftBodyArray() const
+	{
+		return m_softBodies;
+	}
+
+
+	virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; 
+
+	/// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest.
+	/// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape.
+	/// This allows more customization.
+	static void	rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
+					  btCollisionObject* collisionObject,
+					  const btCollisionShape* collisionShape,
+					  const btTransform& colObjWorldTransform,
+					  RayResultCallback& resultCallback);
+
+	virtual	void	serialize(btSerializer* serializer);
+
+};
+
+#endif //BT_SOFT_MULTIBODY_DYNAMICS_WORLD_H

From 379f2ac933be556b867ac86e28b580a10fed68f4 Mon Sep 17 00:00:00 2001
From: yunfeibai <byf1658@gmail.com>
Date: Wed, 12 Oct 2016 11:51:04 -0700
Subject: [PATCH 02/11] Load bunny to to the world in the grasp demo.

---
 data/gripper/wsg50_one_motor_gripper_new.sdf  |  2 +-
 .../RoboticsLearning/GripperGraspExample.cpp  | 10 ++--
 .../RoboticsLearning/KukaGraspExample.cpp     |  2 +-
 .../RoboticsLearning/R2D2GraspExample.cpp     |  2 +-
 examples/RoboticsLearning/b3RobotSimAPI.cpp   |  2 +-
 .../PhysicsServerCommandProcessor.cpp         | 48 +++++++++++++++++--
 examples/SoftDemo/SoftDemo.cpp                |  8 ++--
 7 files changed, 59 insertions(+), 15 deletions(-)

diff --git a/data/gripper/wsg50_one_motor_gripper_new.sdf b/data/gripper/wsg50_one_motor_gripper_new.sdf
index a3a11f3b2..1555a5c09 100644
--- a/data/gripper/wsg50_one_motor_gripper_new.sdf
+++ b/data/gripper/wsg50_one_motor_gripper_new.sdf
@@ -2,7 +2,7 @@
 <sdf version='1.6'>
   <world name='default'>
   <model name='wsg50_with_gripper'>
-    <pose frame=''>0 0 0.26 3.14 0 0</pose>
+    <pose frame=''>0 0 0.6 3.14 0 0</pose>
 	
     <link name='world'>
 	</link>
diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp
index b46706e65..4d53f6a8c 100644
--- a/examples/RoboticsLearning/GripperGraspExample.cpp
+++ b/examples/RoboticsLearning/GripperGraspExample.cpp
@@ -232,6 +232,7 @@ public:
                 slider.m_maxVal=1;
                 m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
             }
+            if (0)
             {
                 b3RobotSimLoadFileArgs args("");
                 b3RobotSimLoadFileResults results;
@@ -241,7 +242,7 @@ public:
                 args.m_useMultiBody = true;
                 m_robotSim.loadFile(args, results);
             }
-            
+            if (1)
             {
                 b3RobotSimLoadFileArgs args("");
                 args.m_fileName = "gripper/wsg50_one_motor_gripper_new.sdf";
@@ -329,6 +330,7 @@ public:
             m_robotSim.createJoint(1, 0, 1, 3, &sliderJoint1);
             m_robotSim.createJoint(1, 0, 1, 6, &sliderJoint2);
             */
+            /*
             b3JointInfo revoluteJoint1;
             revoluteJoint1.m_parentFrame[0] = -0.055;
             revoluteJoint1.m_parentFrame[1] = 0;
@@ -369,6 +371,7 @@ public:
             revoluteJoint2.m_jointType = ePoint2PointType;
             m_robotSim.createJoint(1, 2, 1, 4, &revoluteJoint1);
             m_robotSim.createJoint(1, 3, 1, 6, &revoluteJoint2);
+            */
         }
 		
     }
@@ -398,6 +401,7 @@ public:
             }
         }
         
+        /*
         if ((m_options & eONE_MOTOR_GRASP)!=0)
         {
             int fingerJointIndices[2]={0,1};
@@ -413,9 +417,9 @@ public:
                 m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
             }
         }
+        */
         
-        
-		m_robotSim.stepSimulation();
+		//m_robotSim.stepSimulation();
     }
     virtual void	renderScene()
     {
diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp
index a07d6f643..48e3b7476 100644
--- a/examples/RoboticsLearning/KukaGraspExample.cpp
+++ b/examples/RoboticsLearning/KukaGraspExample.cpp
@@ -256,7 +256,7 @@ public:
         }
         
         
-		m_robotSim.stepSimulation();
+		//m_robotSim.stepSimulation();
     }
     virtual void	renderScene()
     {
diff --git a/examples/RoboticsLearning/R2D2GraspExample.cpp b/examples/RoboticsLearning/R2D2GraspExample.cpp
index e0c9cadd1..2b1c1a8f8 100644
--- a/examples/RoboticsLearning/R2D2GraspExample.cpp
+++ b/examples/RoboticsLearning/R2D2GraspExample.cpp
@@ -179,7 +179,7 @@ public:
     }
     virtual void	stepSimulation(float deltaTime)
 	{
-		m_robotSim.stepSimulation();
+		//m_robotSim.stepSimulation();
     }
     virtual void	renderScene()
     {
diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp
index a0636381b..590bc0fa7 100644
--- a/examples/RoboticsLearning/b3RobotSimAPI.cpp
+++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp
@@ -967,7 +967,7 @@ void b3RobotSimAPI::renderScene()
 		m_data->m_clientServerDirect->renderScene();
 	}
     
-	m_data->m_physicsServer.renderScene();
+	//m_data->m_physicsServer.renderScene();
 }
 
 void b3RobotSimAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian)
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 5ba9e9490..282567bf2 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -5,7 +5,7 @@
 #include "../Importers/ImportURDFDemo/URDF2Bullet.h"
 #include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp"
 #include "TinyRendererVisualShapeConverter.h"
-#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+#include "BulletDynamics/Featherstone/btSoftMultiBodyDynamicsWorld.h"
 #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
 #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
@@ -26,6 +26,10 @@
 #include "Bullet3Common/b3Logging.h"
 #include "../CommonInterfaces/CommonGUIHelperInterface.h"
 #include "SharedMemoryCommands.h"
+#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
+#include "BulletSoftBody/btSoftBodySolvers.h"
+#include "BulletSoftBody/btSoftBodyHelpers.h"
+#include "../SoftDemo/BunnyMesh.h"
 
 //@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
 btVector3 gLastPickPos(0, 0, 0);
@@ -421,8 +425,9 @@ struct PhysicsServerCommandProcessorInternalData
 	btBroadphaseInterface*	m_broadphase;
 	btCollisionDispatcher*	m_dispatcher;
 	btMultiBodyConstraintSolver*	m_solver;
+    btSoftBodySolver* m_softbodySolver;
 	btDefaultCollisionConfiguration* m_collisionConfiguration;
-	btMultiBodyDynamicsWorld* m_dynamicsWorld;
+	btSoftMultiBodyDynamicsWorld* m_dynamicsWorld;
 	SharedMemoryDebugDrawer*		m_remoteDebugDrawer;
 	
 	btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
@@ -596,8 +601,9 @@ PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor()
 void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 {
 	///collision configuration contains default setup for memory, collision setup
-	m_data->m_collisionConfiguration = new btDefaultCollisionConfiguration();
+	//m_data->m_collisionConfiguration = new btDefaultCollisionConfiguration();
 	//m_collisionConfiguration->setConvexConvexMultipointIterations();
+    m_data->m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
 
 	///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
 	m_data->m_dispatcher = new	btCollisionDispatcher(m_data->m_collisionConfiguration);
@@ -605,8 +611,10 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 	m_data->m_broadphase = new btDbvtBroadphase();
 
 	m_data->m_solver = new btMultiBodyConstraintSolver;
+    
+    m_data->m_softbodySolver = 0;
 
-	m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
+	m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration, m_data->m_softbodySolver);
 
 	//Workaround: in a VR application, where we avoid synchronizaing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
 	m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(8192);
@@ -616,6 +624,29 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 
 	m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
 	m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08;
+    
+    btSoftBodyWorldInfo	softBodyWorldInfo;
+    softBodyWorldInfo.air_density		=	(btScalar)1.2;
+    softBodyWorldInfo.water_density	=	0;
+    softBodyWorldInfo.water_offset		=	0;
+    softBodyWorldInfo.water_normal		=	btVector3(0,0,0);
+    softBodyWorldInfo.m_gravity.setValue(0,0,0);
+    
+    btSoftBody*	psb=btSoftBodyHelpers::CreateFromTriMesh(softBodyWorldInfo,gVerticesBunny,
+                                                         &gIndicesBunny[0][0],
+                                                         BUNNY_NUM_TRIANGLES);
+    btSoftBody::Material*	pm=psb->appendMaterial();
+    pm->m_kLST				=	0.5;
+    pm->m_flags				-=	btSoftBody::fMaterial::DebugDraw;
+    psb->generateBendingConstraints(2,pm);
+    psb->m_cfg.piterations	=	2;
+    psb->m_cfg.kDF			=	0.5;
+    psb->randomizeConstraints();
+    psb->rotate(btQuaternion(0.70711,0,0,0.70711));
+    psb->translate(btVector3(0,0,1.0));
+    psb->scale(btVector3(0.1,0.1,0.1));
+    psb->setTotalMass(1,true);	
+    m_data->m_dynamicsWorld->addSoftBody(psb);
 
 }
 
@@ -2766,6 +2797,15 @@ void PhysicsServerCommandProcessor::renderScene()
 		m_data->m_guiHelper->render(m_data->m_dynamicsWorld);
 	}
 
+    for (  int i=0;i<m_data->m_dynamicsWorld->getSoftBodyArray().size();i++)
+    {
+        btSoftBody*	psb=(btSoftBody*)m_data->m_dynamicsWorld->getSoftBodyArray()[i];
+        if (m_data->m_dynamicsWorld->getDebugDrawer() && !(m_data->m_dynamicsWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
+        {
+            btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer());
+            btSoftBodyHelpers::Draw(psb,m_data->m_dynamicsWorld->getDebugDrawer(),m_data->m_dynamicsWorld->getDrawFlags());
+        }
+    }
 }
 
 void    PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags)
diff --git a/examples/SoftDemo/SoftDemo.cpp b/examples/SoftDemo/SoftDemo.cpp
index 6b1611825..286633464 100644
--- a/examples/SoftDemo/SoftDemo.cpp
+++ b/examples/SoftDemo/SoftDemo.cpp
@@ -1019,7 +1019,7 @@ static void	Init_Bunny(SoftDemo* pdemo)
 	psb->scale(btVector3(6,6,6));
 	psb->setTotalMass(1,true);	
 	pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
-	pdemo->m_cutting=true;
+	//pdemo->m_cutting=true;
 
 }
 
@@ -2214,7 +2214,7 @@ void	SoftDemo::initPhysics()
 	if( g_softBodyOutput )
 		delete g_softBodyOutput;
 
-	if (1)
+	if (0)
 	{
 		g_openCLSIMDSolver = new btOpenCLSoftBodySolverSIMDAware( g_cqCommandQue, g_cxMainContext);
 	//	g_openCLSIMDSolver = new btOpenCLSoftBodySolver( g_cqCommandQue, g_cxMainContext);
@@ -2223,7 +2223,7 @@ void	SoftDemo::initPhysics()
 
 
 
-	softBodySolver = g_openCLSIMDSolver;
+	//softBodySolver = g_openCLSIMDSolver;
 	g_softBodyOutput = new btSoftBodySolverOutputCLtoCPU;
 #endif //USE_AMD_OPENCL
 
@@ -2289,7 +2289,7 @@ void	SoftDemo::initPhysics()
 
 	m_autocam						=	false;
 	m_raycast						=	false;
-	m_cutting						=	false;
+	//m_cutting						=	false;
 	m_results.fraction				=	1.f;
 	
 	demofncs[current_demo](this);

From 880ee097fa1fcade24bd9c0c59822f5f0b881a5f Mon Sep 17 00:00:00 2001
From: yunfeibai <byf1658@gmail.com>
Date: Wed, 12 Oct 2016 15:34:45 -0700
Subject: [PATCH 03/11] Simulate bunny in grasp demo and set collision margin.

---
 data/gripper/wsg50_one_motor_gripper_new.sdf  |  2 +-
 .../RoboticsLearning/GripperGraspExample.cpp  |  4 +--
 .../PhysicsServerCommandProcessor.cpp         | 35 ++++++++++++-------
 3 files changed, 25 insertions(+), 16 deletions(-)

diff --git a/data/gripper/wsg50_one_motor_gripper_new.sdf b/data/gripper/wsg50_one_motor_gripper_new.sdf
index 1555a5c09..5bb80d6a6 100644
--- a/data/gripper/wsg50_one_motor_gripper_new.sdf
+++ b/data/gripper/wsg50_one_motor_gripper_new.sdf
@@ -2,7 +2,7 @@
 <sdf version='1.6'>
   <world name='default'>
   <model name='wsg50_with_gripper'>
-    <pose frame=''>0 0 0.6 3.14 0 0</pose>
+    <pose frame=''>0 0 0.7 3.14 0 0</pose>
 	
     <link name='world'>
 	</link>
diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp
index 4d53f6a8c..ea349fa11 100644
--- a/examples/RoboticsLearning/GripperGraspExample.cpp
+++ b/examples/RoboticsLearning/GripperGraspExample.cpp
@@ -401,7 +401,6 @@ public:
             }
         }
         
-        /*
         if ((m_options & eONE_MOTOR_GRASP)!=0)
         {
             int fingerJointIndices[2]={0,1};
@@ -417,9 +416,8 @@ public:
                 m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
             }
         }
-        */
         
-		//m_robotSim.stepSimulation();
+		m_robotSim.stepSimulation();
     }
     virtual void	renderScene()
     {
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 282567bf2..29dcf5337 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -429,6 +429,7 @@ struct PhysicsServerCommandProcessorInternalData
 	btDefaultCollisionConfiguration* m_collisionConfiguration;
 	btSoftMultiBodyDynamicsWorld* m_dynamicsWorld;
 	SharedMemoryDebugDrawer*		m_remoteDebugDrawer;
+    btSoftBodyWorldInfo	m_softBodyWorldInfo;
 	
 	btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
 
@@ -607,8 +608,14 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 
 	///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
 	m_data->m_dispatcher = new	btCollisionDispatcher(m_data->m_collisionConfiguration);
+    
+    m_data->m_softBodyWorldInfo.m_dispatcher = m_data->m_dispatcher;
 
-	m_data->m_broadphase = new btDbvtBroadphase();
+	//m_data->m_broadphase = new btDbvtBroadphase();
+    btVector3 worldAabbMin(-1000,-1000,-1000);
+    btVector3 worldAabbMax(1000,1000,1000);
+    int maxProxies = 32766;
+    m_data->m_broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies);
 
 	m_data->m_solver = new btMultiBodyConstraintSolver;
     
@@ -622,30 +629,34 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 	m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer();
 
 
-	m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
+	m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
 	m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08;
     
-    btSoftBodyWorldInfo	softBodyWorldInfo;
-    softBodyWorldInfo.air_density		=	(btScalar)1.2;
-    softBodyWorldInfo.water_density	=	0;
-    softBodyWorldInfo.water_offset		=	0;
-    softBodyWorldInfo.water_normal		=	btVector3(0,0,0);
-    softBodyWorldInfo.m_gravity.setValue(0,0,0);
+    m_data->m_softBodyWorldInfo.air_density		=	(btScalar)1.2;
+    m_data->m_softBodyWorldInfo.water_density	=	0;
+    m_data->m_softBodyWorldInfo.water_offset		=	0;
+    m_data->m_softBodyWorldInfo.water_normal		=	btVector3(0,0,0);
+    m_data->m_softBodyWorldInfo.m_gravity.setValue(0,0,-10);
+
+    m_data->m_softBodyWorldInfo.m_broadphase = m_data->m_broadphase;
+    m_data->m_softBodyWorldInfo.m_sparsesdf.Initialize();
     
-    btSoftBody*	psb=btSoftBodyHelpers::CreateFromTriMesh(softBodyWorldInfo,gVerticesBunny,
+    btSoftBody*	psb=btSoftBodyHelpers::CreateFromTriMesh(m_data->m_softBodyWorldInfo,gVerticesBunny,
                                                          &gIndicesBunny[0][0],
                                                          BUNNY_NUM_TRIANGLES);
     btSoftBody::Material*	pm=psb->appendMaterial();
-    pm->m_kLST				=	0.5;
+    pm->m_kLST				=	1.0
+    ;
     pm->m_flags				-=	btSoftBody::fMaterial::DebugDraw;
     psb->generateBendingConstraints(2,pm);
     psb->m_cfg.piterations	=	2;
     psb->m_cfg.kDF			=	0.5;
     psb->randomizeConstraints();
     psb->rotate(btQuaternion(0.70711,0,0,0.70711));
-    psb->translate(btVector3(0,0,1.0));
+    psb->translate(btVector3(0,0,3.0));
     psb->scale(btVector3(0.1,0.1,0.1));
-    psb->setTotalMass(1,true);	
+    psb->setTotalMass(1,true);
+    psb->getCollisionShape()->setMargin(0.01);
     m_data->m_dynamicsWorld->addSoftBody(psb);
 
 }

From b07df4d504299a6bf45eae277d1a9deb3af60e7b Mon Sep 17 00:00:00 2001
From: yunfeibai <byf1658@gmail.com>
Date: Mon, 17 Oct 2016 13:01:04 -0700
Subject: [PATCH 04/11] Load bunny through shared memory API and RobotSimAPI.
 Create grasp bunny example.

---
 examples/ExampleBrowser/ExampleEntries.cpp    |   1 +
 .../RoboticsLearning/GripperGraspExample.cpp  | 167 +++++++++++++-----
 .../RoboticsLearning/GripperGraspExample.h    |   1 +
 .../RoboticsLearning/KukaGraspExample.cpp     |   2 +-
 examples/RoboticsLearning/b3RobotSimAPI.cpp   |   6 +
 examples/RoboticsLearning/b3RobotSimAPI.h     |   2 +
 examples/SharedMemory/PhysicsClientC_API.cpp  |  14 ++
 examples/SharedMemory/PhysicsClientC_API.h    |   2 +
 .../PhysicsServerCommandProcessor.cpp         | 101 +++++------
 examples/SharedMemory/SharedMemoryPublic.h    |   1 +
 10 files changed, 196 insertions(+), 101 deletions(-)

diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp
index 0608f0bd1..b371ca660 100644
--- a/examples/ExampleBrowser/ExampleEntries.cpp
+++ b/examples/ExampleBrowser/ExampleEntries.cpp
@@ -267,6 +267,7 @@ static ExampleEntry gDefaultExamples[]=
     ExampleEntry(1,"Gripper Grasp","Grasp experiment with a gripper to improve contact model", GripperGraspExampleCreateFunc,eGRIPPER_GRASP),
     ExampleEntry(1,"Two Point Grasp","Grasp experiment with two point contact to test rolling friction", GripperGraspExampleCreateFunc, eTWO_POINT_GRASP),
 	ExampleEntry(1,"One Motor Gripper Grasp","Grasp experiment with a gripper with one motor to test slider constraint for closed loop structure", GripperGraspExampleCreateFunc, eONE_MOTOR_GRASP),
+    ExampleEntry(1,"Grasp Soft Body","Grasp soft body experiment", GripperGraspExampleCreateFunc, eGRASP_SOFT_BODY),
 
 
 #ifdef ENABLE_LUA
diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp
index ea349fa11..f96dd7c33 100644
--- a/examples/RoboticsLearning/GripperGraspExample.cpp
+++ b/examples/RoboticsLearning/GripperGraspExample.cpp
@@ -217,7 +217,6 @@ public:
         
         if ((m_options & eONE_MOTOR_GRASP)!=0)
         {
-            
             {
                 SliderParams slider("Vertical velocity",&sGripperVerticalVelocity);
                 slider.m_minVal=-2;
@@ -232,7 +231,7 @@ public:
                 slider.m_maxVal=1;
                 m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
             }
-            if (0)
+            if (1)
             {
                 b3RobotSimLoadFileArgs args("");
                 b3RobotSimLoadFileResults results;
@@ -288,49 +287,6 @@ public:
             }
             m_robotSim.setGravity(b3MakeVector3(0,0,-10));
             
-            /*
-            b3JointInfo sliderJoint1;
-            sliderJoint1.m_parentFrame[0] = 0;
-            sliderJoint1.m_parentFrame[1] = 0;
-            sliderJoint1.m_parentFrame[2] = 0.06;
-            sliderJoint1.m_parentFrame[3] = 0;
-            sliderJoint1.m_parentFrame[4] = 0;
-            sliderJoint1.m_parentFrame[5] = 0;
-            sliderJoint1.m_parentFrame[6] = 1.0;
-            sliderJoint1.m_childFrame[0] = 0;
-            sliderJoint1.m_childFrame[1] = 0;
-            sliderJoint1.m_childFrame[2] = 0;
-            sliderJoint1.m_childFrame[3] = 0;
-            sliderJoint1.m_childFrame[4] = 0;
-            sliderJoint1.m_childFrame[5] = 0;
-            sliderJoint1.m_childFrame[6] = 1.0;
-            sliderJoint1.m_jointAxis[0] = 1.0;
-            sliderJoint1.m_jointAxis[1] = 0.0;
-            sliderJoint1.m_jointAxis[2] = 0.0;
-            sliderJoint1.m_jointType = ePrismaticType;
-            b3JointInfo sliderJoint2;
-            sliderJoint2.m_parentFrame[0] = 0;
-            sliderJoint2.m_parentFrame[1] = 0;
-            sliderJoint2.m_parentFrame[2] = 0.06;
-            sliderJoint2.m_parentFrame[3] = 0;
-            sliderJoint2.m_parentFrame[4] = 0;
-            sliderJoint2.m_parentFrame[5] = 0;
-            sliderJoint2.m_parentFrame[6] = 1.0;
-            sliderJoint2.m_childFrame[0] = 0;
-            sliderJoint2.m_childFrame[1] = 0;
-            sliderJoint2.m_childFrame[2] = 0;
-            sliderJoint2.m_childFrame[3] = 0;
-            sliderJoint2.m_childFrame[4] = 0;
-            sliderJoint2.m_childFrame[5] = 1.0;
-            sliderJoint2.m_childFrame[6] = 0;
-            sliderJoint2.m_jointAxis[0] = 1.0;
-            sliderJoint2.m_jointAxis[1] = 0.0;
-            sliderJoint2.m_jointAxis[2] = 0.0;
-            sliderJoint2.m_jointType = ePrismaticType;
-            m_robotSim.createJoint(1, 0, 1, 3, &sliderJoint1);
-            m_robotSim.createJoint(1, 0, 1, 6, &sliderJoint2);
-            */
-            /*
             b3JointInfo revoluteJoint1;
             revoluteJoint1.m_parentFrame[0] = -0.055;
             revoluteJoint1.m_parentFrame[1] = 0;
@@ -371,9 +327,112 @@ public:
             revoluteJoint2.m_jointType = ePoint2PointType;
             m_robotSim.createJoint(1, 2, 1, 4, &revoluteJoint1);
             m_robotSim.createJoint(1, 3, 1, 6, &revoluteJoint2);
-            */
         }
 		
+        if ((m_options & eGRASP_SOFT_BODY)!=0)
+        {
+            {
+                SliderParams slider("Vertical velocity",&sGripperVerticalVelocity);
+                slider.m_minVal=-2;
+                slider.m_maxVal=2;
+                m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+            }
+            
+            {
+                SliderParams slider("Closing velocity",&sGripperClosingTargetVelocity
+                                    );
+                slider.m_minVal=-1;
+                slider.m_maxVal=1;
+                m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+            }
+            if (1)
+            {
+                b3RobotSimLoadFileArgs args("");
+                args.m_fileName = "gripper/wsg50_one_motor_gripper_new.sdf";
+                args.m_fileType = B3_SDF_FILE;
+                args.m_useMultiBody = true;
+                b3RobotSimLoadFileResults results;
+                
+                if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1)
+                {
+                    
+                    m_gripperIndex = results.m_uniqueObjectIds[0];
+                    int numJoints = m_robotSim.getNumJoints(m_gripperIndex);
+                    b3Printf("numJoints = %d",numJoints);
+                    
+                    for (int i=0;i<numJoints;i++)
+                    {
+                        b3JointInfo jointInfo;
+                        m_robotSim.getJointInfo(m_gripperIndex,i,&jointInfo);
+                        b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
+                    }
+                    
+                    for (int i=0;i<8;i++)
+                    {
+                        b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
+                        controlArgs.m_maxTorqueValue = 0.0;
+                        m_robotSim.setJointMotorControl(m_gripperIndex,i,controlArgs);
+                    }
+                    
+                }
+            }
+            
+            if (1)
+            {
+                b3RobotSimLoadFileArgs args("");
+                args.m_fileName = "plane.urdf";
+                args.m_startPosition.setValue(0,0,0);
+                args.m_startOrientation.setEulerZYX(0,0,0);
+                args.m_forceOverrideFixedBase = true;
+                args.m_useMultiBody = true;
+                b3RobotSimLoadFileResults results;
+                m_robotSim.loadFile(args,results);
+                
+            }
+            m_robotSim.setGravity(b3MakeVector3(0,0,-10));
+            m_robotSim.loadBunny();
+            
+            b3JointInfo revoluteJoint1;
+            revoluteJoint1.m_parentFrame[0] = -0.055;
+            revoluteJoint1.m_parentFrame[1] = 0;
+            revoluteJoint1.m_parentFrame[2] = 0.02;
+            revoluteJoint1.m_parentFrame[3] = 0;
+            revoluteJoint1.m_parentFrame[4] = 0;
+            revoluteJoint1.m_parentFrame[5] = 0;
+            revoluteJoint1.m_parentFrame[6] = 1.0;
+            revoluteJoint1.m_childFrame[0] = 0;
+            revoluteJoint1.m_childFrame[1] = 0;
+            revoluteJoint1.m_childFrame[2] = 0;
+            revoluteJoint1.m_childFrame[3] = 0;
+            revoluteJoint1.m_childFrame[4] = 0;
+            revoluteJoint1.m_childFrame[5] = 0;
+            revoluteJoint1.m_childFrame[6] = 1.0;
+            revoluteJoint1.m_jointAxis[0] = 1.0;
+            revoluteJoint1.m_jointAxis[1] = 0.0;
+            revoluteJoint1.m_jointAxis[2] = 0.0;
+            revoluteJoint1.m_jointType = ePoint2PointType;
+            b3JointInfo revoluteJoint2;
+            revoluteJoint2.m_parentFrame[0] = 0.055;
+            revoluteJoint2.m_parentFrame[1] = 0;
+            revoluteJoint2.m_parentFrame[2] = 0.02;
+            revoluteJoint2.m_parentFrame[3] = 0;
+            revoluteJoint2.m_parentFrame[4] = 0;
+            revoluteJoint2.m_parentFrame[5] = 0;
+            revoluteJoint2.m_parentFrame[6] = 1.0;
+            revoluteJoint2.m_childFrame[0] = 0;
+            revoluteJoint2.m_childFrame[1] = 0;
+            revoluteJoint2.m_childFrame[2] = 0;
+            revoluteJoint2.m_childFrame[3] = 0;
+            revoluteJoint2.m_childFrame[4] = 0;
+            revoluteJoint2.m_childFrame[5] = 0;
+            revoluteJoint2.m_childFrame[6] = 1.0;
+            revoluteJoint2.m_jointAxis[0] = 1.0;
+            revoluteJoint2.m_jointAxis[1] = 0.0;
+            revoluteJoint2.m_jointAxis[2] = 0.0;
+            revoluteJoint2.m_jointType = ePoint2PointType;
+            m_robotSim.createJoint(0, 2, 0, 4, &revoluteJoint1);
+            m_robotSim.createJoint(0, 3, 0, 6, &revoluteJoint2);
+        }
     }
     virtual void    exitPhysics()
     {
@@ -402,6 +461,22 @@ public:
         }
         
         if ((m_options & eONE_MOTOR_GRASP)!=0)
+        {
+            int fingerJointIndices[2]={0,1};
+            double fingerTargetVelocities[2]={sGripperVerticalVelocity,sGripperClosingTargetVelocity
+            };
+            double maxTorqueValues[2]={50.0,50.0};
+            for (int i=0;i<2;i++)
+            {
+                b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
+                controlArgs.m_targetVelocity = fingerTargetVelocities[i];
+                controlArgs.m_maxTorqueValue = maxTorqueValues[i];
+                controlArgs.m_kd = 1.;
+                m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
+            }
+        }
+        
+        if ((m_options & eGRASP_SOFT_BODY)!=0)
         {
             int fingerJointIndices[2]={0,1};
             double fingerTargetVelocities[2]={sGripperVerticalVelocity,sGripperClosingTargetVelocity
diff --git a/examples/RoboticsLearning/GripperGraspExample.h b/examples/RoboticsLearning/GripperGraspExample.h
index 37177823f..6a9747ec5 100644
--- a/examples/RoboticsLearning/GripperGraspExample.h
+++ b/examples/RoboticsLearning/GripperGraspExample.h
@@ -21,6 +21,7 @@ enum GripperGraspExampleOptions
     eGRIPPER_GRASP=1,
     eTWO_POINT_GRASP=2,
     eONE_MOTOR_GRASP=4,
+    eGRASP_SOFT_BODY=8,
 };
 
 class CommonExampleInterface*    GripperGraspExampleCreateFunc(struct CommonExampleOptions& options);
diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp
index 48e3b7476..a07d6f643 100644
--- a/examples/RoboticsLearning/KukaGraspExample.cpp
+++ b/examples/RoboticsLearning/KukaGraspExample.cpp
@@ -256,7 +256,7 @@ public:
         }
         
         
-		//m_robotSim.stepSimulation();
+		m_robotSim.stepSimulation();
     }
     virtual void	renderScene()
     {
diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp
index 590bc0fa7..d2ea24714 100644
--- a/examples/RoboticsLearning/b3RobotSimAPI.cpp
+++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp
@@ -998,4 +998,10 @@ void b3RobotSimAPI::getLinkState(int bodyUniqueId, int linkIndex, double* worldP
         worldOrientation[2] = linkState.m_worldOrientation[2];
         worldOrientation[3] = linkState.m_worldOrientation[3];
     }
+}
+
+void b3RobotSimAPI::loadBunny()
+{
+    b3SharedMemoryCommandHandle command = b3LoadBunnyCommandInit(m_data->m_physicsClient);
+    b3SubmitClientCommand(m_data->m_physicsClient, command);
 }
\ No newline at end of file
diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h
index 798b892e0..882817384 100644
--- a/examples/RoboticsLearning/b3RobotSimAPI.h
+++ b/examples/RoboticsLearning/b3RobotSimAPI.h
@@ -164,6 +164,8 @@ public:
     void getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian);
     
     void getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition, double* worldOrientation);
+    
+    void loadBunny();
 };
 
 #endif //B3_ROBOT_SIM_API_H
diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index 0e75054cc..30be95fc2 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -55,6 +55,20 @@ b3SharedMemoryCommandHandle	b3LoadUrdfCommandInit(b3PhysicsClientHandle physClie
 	return (b3SharedMemoryCommandHandle) command;
 }
 
+b3SharedMemoryCommandHandle	b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient)
+{
+    PhysicsClient* cl = (PhysicsClient* ) physClient;
+    b3Assert(cl);
+    b3Assert(cl->canSubmitCommand());
+    
+    struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+    b3Assert(command);
+    command->m_type = CMD_LOAD_BUNNY;
+    command->m_updateFlags = 0;
+    
+    return (b3SharedMemoryCommandHandle) command;
+}
+
 int	b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody)
 {
     struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index b44b9a452..fb4a3c29a 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -214,6 +214,8 @@ b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle phys
 b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient);
 void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags);
 void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags);
+    
+b3SharedMemoryCommandHandle	b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient);
 
 #ifdef __cplusplus
 }
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 29dcf5337..da9f95785 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -601,64 +601,28 @@ PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor()
 
 void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 {
-	///collision configuration contains default setup for memory, collision setup
-	//m_data->m_collisionConfiguration = new btDefaultCollisionConfiguration();
-	//m_collisionConfiguration->setConvexConvexMultipointIterations();
+    ///collision configuration contains default setup for memory, collision setup
+    //m_data->m_collisionConfiguration = new btDefaultCollisionConfiguration();
+    //m_collisionConfiguration->setConvexConvexMultipointIterations();
     m_data->m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
-
-	///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
-	m_data->m_dispatcher = new	btCollisionDispatcher(m_data->m_collisionConfiguration);
+    ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
+    m_data->m_dispatcher = new	btCollisionDispatcher(m_data->m_collisionConfiguration);
     
-    m_data->m_softBodyWorldInfo.m_dispatcher = m_data->m_dispatcher;
-
-	//m_data->m_broadphase = new btDbvtBroadphase();
-    btVector3 worldAabbMin(-1000,-1000,-1000);
-    btVector3 worldAabbMax(1000,1000,1000);
-    int maxProxies = 32766;
-    m_data->m_broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies);
-
-	m_data->m_solver = new btMultiBodyConstraintSolver;
+    m_data->m_broadphase = new btDbvtBroadphase();
     
-    m_data->m_softbodySolver = 0;
-
-	m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration, m_data->m_softbodySolver);
-
-	//Workaround: in a VR application, where we avoid synchronizaing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
-	m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(8192);
-
-	m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer();
-
-
-	m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
-	m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08;
+    m_data->m_solver = new btMultiBodyConstraintSolver;
     
-    m_data->m_softBodyWorldInfo.air_density		=	(btScalar)1.2;
-    m_data->m_softBodyWorldInfo.water_density	=	0;
-    m_data->m_softBodyWorldInfo.water_offset		=	0;
-    m_data->m_softBodyWorldInfo.water_normal		=	btVector3(0,0,0);
-    m_data->m_softBodyWorldInfo.m_gravity.setValue(0,0,-10);
-
-    m_data->m_softBodyWorldInfo.m_broadphase = m_data->m_broadphase;
-    m_data->m_softBodyWorldInfo.m_sparsesdf.Initialize();
+    m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
+    
+    //Workaround: in a VR application, where we avoid synchronizaing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
+    m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(8192);
+    
+    m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer();
+    
+    
+    m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
+    m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08;
     
-    btSoftBody*	psb=btSoftBodyHelpers::CreateFromTriMesh(m_data->m_softBodyWorldInfo,gVerticesBunny,
-                                                         &gIndicesBunny[0][0],
-                                                         BUNNY_NUM_TRIANGLES);
-    btSoftBody::Material*	pm=psb->appendMaterial();
-    pm->m_kLST				=	1.0
-    ;
-    pm->m_flags				-=	btSoftBody::fMaterial::DebugDraw;
-    psb->generateBendingConstraints(2,pm);
-    psb->m_cfg.piterations	=	2;
-    psb->m_cfg.kDF			=	0.5;
-    psb->randomizeConstraints();
-    psb->rotate(btQuaternion(0.70711,0,0,0.70711));
-    psb->translate(btVector3(0,0,3.0));
-    psb->scale(btVector3(0.1,0.1,0.1));
-    psb->setTotalMass(1,true);
-    psb->getCollisionShape()->setMargin(0.01);
-    m_data->m_dynamicsWorld->addSoftBody(psb);
-
 }
 
 void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies()
@@ -1464,6 +1428,35 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 
 
 
+                    break;
+                }
+                case CMD_LOAD_BUNNY:
+                {
+                    m_data->m_softBodyWorldInfo.air_density		=	(btScalar)1.2;
+                    m_data->m_softBodyWorldInfo.water_density	=	0;
+                    m_data->m_softBodyWorldInfo.water_offset	=	0;
+                    m_data->m_softBodyWorldInfo.water_normal	=	btVector3(0,0,0);
+                    m_data->m_softBodyWorldInfo.m_gravity.setValue(0,0,-10);
+                    m_data->m_softBodyWorldInfo.m_broadphase = m_data->m_broadphase;
+                    m_data->m_softBodyWorldInfo.m_sparsesdf.Initialize();
+                    
+                    btSoftBody*	psb=btSoftBodyHelpers::CreateFromTriMesh(m_data->m_softBodyWorldInfo,gVerticesBunny,                                                       &gIndicesBunny[0][0],                                                         BUNNY_NUM_TRIANGLES);
+                    
+                    btSoftBody::Material*	pm=psb->appendMaterial();
+                    pm->m_kLST				=	1.0;
+                    pm->m_flags				-=	btSoftBody::fMaterial::DebugDraw;
+                    psb->generateBendingConstraints(2,pm);
+                    psb->m_cfg.piterations	=	2;
+                    psb->m_cfg.kDF			=	0.5;
+                    psb->randomizeConstraints();
+                    psb->rotate(btQuaternion(0.70711,0,0,0.70711));
+                    psb->translate(btVector3(0,0,3.0));
+                    psb->scale(btVector3(0.1,0.1,0.1));
+                    psb->setTotalMass(1,true);
+                    psb->getCollisionShape()->setMargin(0.01);
+                    
+                    m_data->m_dynamicsWorld->addSoftBody(psb);
+
                     break;
                 }
                 case CMD_CREATE_SENSOR:
@@ -2813,7 +2806,7 @@ void PhysicsServerCommandProcessor::renderScene()
         btSoftBody*	psb=(btSoftBody*)m_data->m_dynamicsWorld->getSoftBodyArray()[i];
         if (m_data->m_dynamicsWorld->getDebugDrawer() && !(m_data->m_dynamicsWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
         {
-            btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer());
+            //btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer());
             btSoftBodyHelpers::Draw(psb,m_data->m_dynamicsWorld->getDebugDrawer(),m_data->m_dynamicsWorld->getDrawFlags());
         }
     }
diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h
index 1490fae68..fdae85d29 100644
--- a/examples/SharedMemory/SharedMemoryPublic.h
+++ b/examples/SharedMemory/SharedMemoryPublic.h
@@ -7,6 +7,7 @@ enum EnumSharedMemoryClientCommand
 {
     CMD_LOAD_SDF,
 	CMD_LOAD_URDF,
+    CMD_LOAD_BUNNY,
         CMD_SEND_BULLET_DATA_STREAM,
         CMD_CREATE_BOX_COLLISION_SHAPE,
 //      CMD_DELETE_BOX_COLLISION_SHAPE,

From 14fc8ae8c2cba32a2b4cc47043a5ab1caf7bfc80 Mon Sep 17 00:00:00 2001
From: yunfeibai <byf1658@gmail.com>
Date: Mon, 17 Oct 2016 13:19:34 -0700
Subject: [PATCH 05/11] Restore original demo settings.

---
 .../RoboticsLearning/GripperGraspExample.cpp    |  2 --
 examples/RoboticsLearning/R2D2GraspExample.cpp  |  2 +-
 examples/RoboticsLearning/b3RobotSimAPI.cpp     |  2 +-
 examples/SoftDemo/SoftDemo.cpp                  | 17 +++++++----------
 4 files changed, 9 insertions(+), 14 deletions(-)

diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp
index f96dd7c33..37e6c06a0 100644
--- a/examples/RoboticsLearning/GripperGraspExample.cpp
+++ b/examples/RoboticsLearning/GripperGraspExample.cpp
@@ -231,7 +231,6 @@ public:
                 slider.m_maxVal=1;
                 m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
             }
-            if (1)
             {
                 b3RobotSimLoadFileArgs args("");
                 b3RobotSimLoadFileResults results;
@@ -241,7 +240,6 @@ public:
                 args.m_useMultiBody = true;
                 m_robotSim.loadFile(args, results);
             }
-            if (1)
             {
                 b3RobotSimLoadFileArgs args("");
                 args.m_fileName = "gripper/wsg50_one_motor_gripper_new.sdf";
diff --git a/examples/RoboticsLearning/R2D2GraspExample.cpp b/examples/RoboticsLearning/R2D2GraspExample.cpp
index 2b1c1a8f8..e0c9cadd1 100644
--- a/examples/RoboticsLearning/R2D2GraspExample.cpp
+++ b/examples/RoboticsLearning/R2D2GraspExample.cpp
@@ -179,7 +179,7 @@ public:
     }
     virtual void	stepSimulation(float deltaTime)
 	{
-		//m_robotSim.stepSimulation();
+		m_robotSim.stepSimulation();
     }
     virtual void	renderScene()
     {
diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp
index d2ea24714..40527e481 100644
--- a/examples/RoboticsLearning/b3RobotSimAPI.cpp
+++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp
@@ -967,7 +967,7 @@ void b3RobotSimAPI::renderScene()
 		m_data->m_clientServerDirect->renderScene();
 	}
     
-	//m_data->m_physicsServer.renderScene();
+	m_data->m_physicsServer.renderScene();
 }
 
 void b3RobotSimAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian)
diff --git a/examples/SoftDemo/SoftDemo.cpp b/examples/SoftDemo/SoftDemo.cpp
index 286633464..7a046f9c4 100644
--- a/examples/SoftDemo/SoftDemo.cpp
+++ b/examples/SoftDemo/SoftDemo.cpp
@@ -1017,9 +1017,9 @@ static void	Init_Bunny(SoftDemo* pdemo)
 	psb->m_cfg.kDF			=	0.5;
 	psb->randomizeConstraints();
 	psb->scale(btVector3(6,6,6));
-	psb->setTotalMass(1,true);	
+	psb->setTotalMass(100,true);
 	pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
-	//pdemo->m_cutting=true;
+	pdemo->m_cutting=true;
 
 }
 
@@ -2097,7 +2097,6 @@ void	SoftDemo::initPhysics()
 	motorcontrol.maxtorque = 0;
 
 	btCollisionShape* groundShape = 0;
-    if (0)
 	{
 		int i;
 		int j;
@@ -2152,12 +2151,11 @@ void	SoftDemo::initPhysics()
 		groundShape->setMargin(0.5);
 	}
 
-	//m_collisionShapes.push_back(groundShape);
+	m_collisionShapes.push_back(groundShape);
 
 	btCollisionShape* groundBox = new btBoxShape (btVector3(100,CUBE_HALF_EXTENTS,100));
 	m_collisionShapes.push_back(groundBox);
 
-    /*
 	btCompoundShape* cylinderCompound = new btCompoundShape;
 	btCollisionShape* cylinderShape = new btCylinderShape (btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS));
 	btTransform localTransform;
@@ -2168,7 +2166,6 @@ void	SoftDemo::initPhysics()
 	cylinderCompound->addChildShape(localTransform,cylinderShape);
 
 	m_collisionShapes.push_back(cylinderCompound);
-    */
 
 	m_dispatcher=0;
 
@@ -2214,7 +2211,7 @@ void	SoftDemo::initPhysics()
 	if( g_softBodyOutput )
 		delete g_softBodyOutput;
 
-	if (0)
+	if (1)
 	{
 		g_openCLSIMDSolver = new btOpenCLSoftBodySolverSIMDAware( g_cqCommandQue, g_cxMainContext);
 	//	g_openCLSIMDSolver = new btOpenCLSoftBodySolver( g_cqCommandQue, g_cxMainContext);
@@ -2223,7 +2220,7 @@ void	SoftDemo::initPhysics()
 
 
 
-	//softBodySolver = g_openCLSIMDSolver;
+	softBodySolver = g_openCLSIMDSolver;
 	g_softBodyOutput = new btSoftBodySolverOutputCLtoCPU;
 #endif //USE_AMD_OPENCL
 
@@ -2262,7 +2259,7 @@ void	SoftDemo::initPhysics()
 		newOb->setCollisionShape(m_collisionShapes[0]);
 	} else
 	{
-		newOb->setCollisionShape(m_collisionShapes[0]);
+		newOb->setCollisionShape(m_collisionShapes[1]);
 	}
 
 	m_dynamicsWorld->addCollisionObject(newOb);
@@ -2289,7 +2286,7 @@ void	SoftDemo::initPhysics()
 
 	m_autocam						=	false;
 	m_raycast						=	false;
-	//m_cutting						=	false;
+	m_cutting						=	false;
 	m_results.fraction				=	1.f;
 	
 	demofncs[current_demo](this);

From fddccca48265b3791550d75fa97255c8dde73849 Mon Sep 17 00:00:00 2001
From: yunfeibai <byf1658@gmail.com>
Date: Mon, 17 Oct 2016 14:46:47 -0700
Subject: [PATCH 06/11] Fix error in integration test.

---
 test/SharedMemory/CMakeLists.txt | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/test/SharedMemory/CMakeLists.txt b/test/SharedMemory/CMakeLists.txt
index 496ca5076..491e54186 100644
--- a/test/SharedMemory/CMakeLists.txt
+++ b/test/SharedMemory/CMakeLists.txt
@@ -13,7 +13,7 @@ ADD_DEFINITIONS(-DPHYSICS_LOOP_BACK -DPHYSICS_SERVER_DIRECT -DENABLE_GTEST -D_VA
 
 
 LINK_LIBRARIES(
- BulletInverseDynamicsUtils BulletInverseDynamics BulletFileLoader BulletWorldImporter Bullet3Common BulletDynamics BulletCollision LinearMath gtest BussIK
+ BulletInverseDynamicsUtils BulletInverseDynamics BulletFileLoader BulletWorldImporter Bullet3Common BulletDynamics BulletCollision LinearMath gtest BussIK BulletSoftBody
 )
 
 IF (NOT WIN32)

From 6f0a404b1ea68565d092abb3df04a7f85e9dba02 Mon Sep 17 00:00:00 2001
From: yunfeibai <byf1658@gmail.com>
Date: Mon, 17 Oct 2016 16:20:15 -0700
Subject: [PATCH 07/11] More fix for integration test.

---
 examples/BasicDemo/CMakeLists.txt           | 4 ++--
 examples/HelloWorld/CMakeLists.txt          | 2 +-
 test/BulletDynamics/pendulum/CMakeLists.txt | 2 +-
 3 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/examples/BasicDemo/CMakeLists.txt b/examples/BasicDemo/CMakeLists.txt
index f01a03a51..3d86a54e3 100644
--- a/examples/BasicDemo/CMakeLists.txt
+++ b/examples/BasicDemo/CMakeLists.txt
@@ -5,7 +5,7 @@ ${BULLET_PHYSICS_SOURCE_DIR}/src
 )
 
 LINK_LIBRARIES(
- BulletDynamics BulletCollision LinearMath
+ BulletDynamics BulletCollision LinearMath BulletSoftBody
 )
 
 IF (WIN32)
@@ -60,7 +60,7 @@ SET(AppBasicExampleGui_SRCS
 ADD_DEFINITIONS(-DB3_USE_STANDALONE_EXAMPLE)
 
 LINK_LIBRARIES(
-        BulletDynamics BulletCollision LinearMath OpenGLWindow Bullet3Common ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
+        BulletDynamics BulletCollision BulletSoftBody LinearMath OpenGLWindow Bullet3Common ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
 )
 
 #some code to support OpenGL and Glew cross platform
diff --git a/examples/HelloWorld/CMakeLists.txt b/examples/HelloWorld/CMakeLists.txt
index f61fbe676..39a19b0d1 100644
--- a/examples/HelloWorld/CMakeLists.txt
+++ b/examples/HelloWorld/CMakeLists.txt
@@ -5,7 +5,7 @@ ${BULLET_PHYSICS_SOURCE_DIR}/src
 )
 
 LINK_LIBRARIES(
- BulletDynamics BulletCollision LinearMath
+ BulletDynamics BulletCollision LinearMath BulletSoftBody
 )
 
 IF (WIN32)
diff --git a/test/BulletDynamics/pendulum/CMakeLists.txt b/test/BulletDynamics/pendulum/CMakeLists.txt
index 570dc0715..323aba414 100644
--- a/test/BulletDynamics/pendulum/CMakeLists.txt
+++ b/test/BulletDynamics/pendulum/CMakeLists.txt
@@ -10,7 +10,7 @@ ADD_DEFINITIONS(-DUSE_GTEST)
 ADD_DEFINITIONS(-D_VARIADIC_MAX=10)
 
 LINK_LIBRARIES(
- BulletDynamics BulletCollision LinearMath gtest
+ BulletDynamics BulletCollision LinearMath gtest BulletSoftBody
 )
 
 IF (NOT WIN32)

From 02393a1eddff522f029a250d6313088c1b421e7d Mon Sep 17 00:00:00 2001
From: yunfeibai <byf1658@gmail.com>
Date: Mon, 17 Oct 2016 16:45:28 -0700
Subject: [PATCH 08/11] More fix

---
 test/InverseDynamics/CMakeLists.txt | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/test/InverseDynamics/CMakeLists.txt b/test/InverseDynamics/CMakeLists.txt
index e31ea6e12..488184fd3 100644
--- a/test/InverseDynamics/CMakeLists.txt
+++ b/test/InverseDynamics/CMakeLists.txt
@@ -13,7 +13,7 @@ INCLUDE_DIRECTORIES(
 ADD_DEFINITIONS(-D_VARIADIC_MAX=10)
 
 LINK_LIBRARIES(
- BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest
+ BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest BulletSoftBody
 )
 IF (NOT WIN32)
 	LINK_LIBRARIES(		pthread	)
@@ -44,7 +44,7 @@ INCLUDE_DIRECTORIES(
 ADD_DEFINITIONS(-D_VARIADIC_MAX=10)
 
 LINK_LIBRARIES(
-BulletDynamics BulletCollision  BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest
+BulletDynamics BulletCollision  BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest BulletSoftBody
 )
 IF (NOT WIN32)
         LINK_LIBRARIES(         pthread )
@@ -110,7 +110,7 @@ INCLUDE_DIRECTORIES(
 ADD_DEFINITIONS(-D_VARIADIC_MAX=10)
 
 LINK_LIBRARIES(
- BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest
+ BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest BulletSoftBody
 )
 IF (NOT WIN32)
         LINK_LIBRARIES(         pthread )

From e112b152815bb01620fdbb14b900a17ce79b7edb Mon Sep 17 00:00:00 2001
From: yunfeibai <byf1658@gmail.com>
Date: Mon, 17 Oct 2016 23:40:38 -0700
Subject: [PATCH 09/11] Move btSoftMultiBodyDynamicsWorld to BulletSoftBody.

---
 examples/BasicDemo/CMakeLists.txt             |   4 +-
 examples/HelloWorld/CMakeLists.txt            |   2 +-
 .../PhysicsServerCommandProcessor.cpp         |   2 +-
 src/BulletDynamics/CMakeLists.txt             |   2 -
 .../btSoftMultiBodyDynamicsWorld.cpp          | 367 ------------------
 .../btSoftMultiBodyDynamicsWorld.h            | 108 ------
 src/BulletSoftBody/CMakeLists.txt             |   2 +
 test/BulletDynamics/pendulum/CMakeLists.txt   |   2 +-
 test/InverseDynamics/CMakeLists.txt           |   6 +-
 9 files changed, 10 insertions(+), 485 deletions(-)
 delete mode 100644 src/BulletDynamics/Featherstone/btSoftMultiBodyDynamicsWorld.cpp
 delete mode 100644 src/BulletDynamics/Featherstone/btSoftMultiBodyDynamicsWorld.h

diff --git a/examples/BasicDemo/CMakeLists.txt b/examples/BasicDemo/CMakeLists.txt
index 3d86a54e3..f01a03a51 100644
--- a/examples/BasicDemo/CMakeLists.txt
+++ b/examples/BasicDemo/CMakeLists.txt
@@ -5,7 +5,7 @@ ${BULLET_PHYSICS_SOURCE_DIR}/src
 )
 
 LINK_LIBRARIES(
- BulletDynamics BulletCollision LinearMath BulletSoftBody
+ BulletDynamics BulletCollision LinearMath
 )
 
 IF (WIN32)
@@ -60,7 +60,7 @@ SET(AppBasicExampleGui_SRCS
 ADD_DEFINITIONS(-DB3_USE_STANDALONE_EXAMPLE)
 
 LINK_LIBRARIES(
-        BulletDynamics BulletCollision BulletSoftBody LinearMath OpenGLWindow Bullet3Common ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
+        BulletDynamics BulletCollision LinearMath OpenGLWindow Bullet3Common ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
 )
 
 #some code to support OpenGL and Glew cross platform
diff --git a/examples/HelloWorld/CMakeLists.txt b/examples/HelloWorld/CMakeLists.txt
index 39a19b0d1..f61fbe676 100644
--- a/examples/HelloWorld/CMakeLists.txt
+++ b/examples/HelloWorld/CMakeLists.txt
@@ -5,7 +5,7 @@ ${BULLET_PHYSICS_SOURCE_DIR}/src
 )
 
 LINK_LIBRARIES(
- BulletDynamics BulletCollision LinearMath BulletSoftBody
+ BulletDynamics BulletCollision LinearMath
 )
 
 IF (WIN32)
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 3b0d96a54..a46613c59 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -5,7 +5,6 @@
 #include "../Importers/ImportURDFDemo/URDF2Bullet.h"
 #include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp"
 #include "TinyRendererVisualShapeConverter.h"
-#include "BulletDynamics/Featherstone/btSoftMultiBodyDynamicsWorld.h"
 #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
 #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
@@ -29,6 +28,7 @@
 #include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
 #include "BulletSoftBody/btSoftBodySolvers.h"
 #include "BulletSoftBody/btSoftBodyHelpers.h"
+#include "BulletSoftBody/btSoftMultiBodyDynamicsWorld.h"
 #include "../SoftDemo/BunnyMesh.h"
 
 
diff --git a/src/BulletDynamics/CMakeLists.txt b/src/BulletDynamics/CMakeLists.txt
index 7e31c13d1..656fc8433 100644
--- a/src/BulletDynamics/CMakeLists.txt
+++ b/src/BulletDynamics/CMakeLists.txt
@@ -29,7 +29,6 @@ SET(BulletDynamics_SRCS
 	Featherstone/btMultiBody.cpp
 	Featherstone/btMultiBodyConstraintSolver.cpp
 	Featherstone/btMultiBodyDynamicsWorld.cpp
-	Featherstone/btSoftMultiBodyDynamicsWorld.cpp
 	Featherstone/btMultiBodyJointLimitConstraint.cpp
 	Featherstone/btMultiBodyConstraint.cpp
 	Featherstone/btMultiBodyPoint2Point.cpp
@@ -85,7 +84,6 @@ SET(Featherstone_HDRS
 	Featherstone/btMultiBody.h
 	Featherstone/btMultiBodyConstraintSolver.h
 	Featherstone/btMultiBodyDynamicsWorld.h
-	Featherstone/btSoftMultiBodyDynamicsWorld.h
 	Featherstone/btMultiBodyLink.h
 	Featherstone/btMultiBodyLinkCollider.h
 	Featherstone/btMultiBodySolverConstraint.h
diff --git a/src/BulletDynamics/Featherstone/btSoftMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btSoftMultiBodyDynamicsWorld.cpp
deleted file mode 100644
index ffa243ebf..000000000
--- a/src/BulletDynamics/Featherstone/btSoftMultiBodyDynamicsWorld.cpp
+++ /dev/null
@@ -1,367 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 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.
-*/
-
-
-#include "btSoftMultiBodyDynamicsWorld.h"
-#include "LinearMath/btQuickprof.h"
-
-//softbody & helpers
-#include "BulletSoftBody/btSoftBody.h"
-#include "BulletSoftBody/btSoftBodyHelpers.h"
-#include "BulletSoftBody/btSoftBodySolvers.h"
-#include "BulletSoftBody/btDefaultSoftBodySolver.h"
-#include "LinearMath/btSerializer.h"
-
-
-btSoftMultiBodyDynamicsWorld::btSoftMultiBodyDynamicsWorld(
-	btDispatcher* dispatcher,
-	btBroadphaseInterface* pairCache,
-	btMultiBodyConstraintSolver* constraintSolver,
-	btCollisionConfiguration* collisionConfiguration,
-	btSoftBodySolver *softBodySolver ) : 
-		btMultiBodyDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration),
-        m_softBodySolver( softBodySolver ),
-        m_ownsSolver(false)
-{
-	if( !m_softBodySolver )
-	{
-		void* ptr = btAlignedAlloc(sizeof(btDefaultSoftBodySolver),16);
-		m_softBodySolver = new(ptr) btDefaultSoftBodySolver();
-		m_ownsSolver = true;
-	}
-
-	m_drawFlags			=	fDrawFlags::Std;
-	m_drawNodeTree		=	true;
-	m_drawFaceTree		=	false;
-	m_drawClusterTree	=	false;
-	m_sbi.m_broadphase = pairCache;
-	m_sbi.m_dispatcher = dispatcher;
-	m_sbi.m_sparsesdf.Initialize();
-	m_sbi.m_sparsesdf.Reset();
-
-	m_sbi.air_density		=	(btScalar)1.2;
-	m_sbi.water_density	=	0;
-	m_sbi.water_offset		=	0;
-	m_sbi.water_normal		=	btVector3(0,0,0);
-	m_sbi.m_gravity.setValue(0,-10,0);
-
-	m_sbi.m_sparsesdf.Initialize();
-
-
-}
-
-btSoftMultiBodyDynamicsWorld::~btSoftMultiBodyDynamicsWorld()
-{
-	if (m_ownsSolver)
-	{
-		m_softBodySolver->~btSoftBodySolver();
-		btAlignedFree(m_softBodySolver);
-	}
-}
-
-void	btSoftMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
-{
-	btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep );
-	{
-		BT_PROFILE("predictUnconstraintMotionSoftBody");
-		m_softBodySolver->predictMotion( float(timeStep) );
-	}
-}
-
-void	btSoftMultiBodyDynamicsWorld::internalSingleStepSimulation( btScalar timeStep )
-{
-
-	// Let the solver grab the soft bodies and if necessary optimize for it
-	m_softBodySolver->optimize( getSoftBodyArray() );
-
-	if( !m_softBodySolver->checkInitialized() )
-	{
-		btAssert( "Solver initialization failed\n" );
-	}
-
-	btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep );
-
-	///solve soft bodies constraints
-	solveSoftBodiesConstraints( timeStep );
-
-	//self collisions
-	for ( int i=0;i<m_softBodies.size();i++)
-	{
-		btSoftBody*	psb=(btSoftBody*)m_softBodies[i];
-		psb->defaultCollisionHandler(psb);
-	}
-
-	///update soft bodies
-	m_softBodySolver->updateSoftBodies( );
-	
-	// End solver-wise simulation step
-	// ///////////////////////////////
-
-}
-
-void	btSoftMultiBodyDynamicsWorld::solveSoftBodiesConstraints( btScalar timeStep )
-{
-	BT_PROFILE("solveSoftConstraints");
-
-	if(m_softBodies.size())
-	{
-		btSoftBody::solveClusters(m_softBodies);
-	}
-
-	// Solve constraints solver-wise
-	m_softBodySolver->solveConstraints( timeStep * m_softBodySolver->getTimeScale() );
-
-}
-
-void	btSoftMultiBodyDynamicsWorld::addSoftBody(btSoftBody* body,short int collisionFilterGroup,short int collisionFilterMask)
-{
-	m_softBodies.push_back(body);
-
-	// Set the soft body solver that will deal with this body
-	// to be the world's solver
-	body->setSoftBodySolver( m_softBodySolver );
-
-	btCollisionWorld::addCollisionObject(body,
-		collisionFilterGroup,
-		collisionFilterMask);
-
-}
-
-void	btSoftMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body)
-{
-	m_softBodies.remove(body);
-
-	btCollisionWorld::removeCollisionObject(body);
-}
-
-void	btSoftMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
-{
-	btSoftBody* body = btSoftBody::upcast(collisionObject);
-	if (body)
-		removeSoftBody(body);
-	else
-		btDiscreteDynamicsWorld::removeCollisionObject(collisionObject);
-}
-
-void	btSoftMultiBodyDynamicsWorld::debugDrawWorld()
-{
-	btDiscreteDynamicsWorld::debugDrawWorld();
-
-	if (getDebugDrawer())
-	{
-		int i;
-		for (  i=0;i<this->m_softBodies.size();i++)
-		{
-			btSoftBody*	psb=(btSoftBody*)this->m_softBodies[i];
-			if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
-			{
-				btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer);
-				btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags);
-			}
-			
-			if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
-			{
-				if(m_drawNodeTree)		btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer);
-				if(m_drawFaceTree)		btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer);
-				if(m_drawClusterTree)	btSoftBodyHelpers::DrawClusterTree(psb,m_debugDrawer);
-			}
-		}		
-	}	
-}
-
-
-
-
-struct btSoftSingleRayCallback : public btBroadphaseRayCallback
-{
-	btVector3	m_rayFromWorld;
-	btVector3	m_rayToWorld;
-	btTransform	m_rayFromTrans;
-	btTransform	m_rayToTrans;
-	btVector3	m_hitNormal;
-
-	const btSoftMultiBodyDynamicsWorld*	m_world;
-	btCollisionWorld::RayResultCallback&	m_resultCallback;
-
-	btSoftSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btSoftMultiBodyDynamicsWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
-	:m_rayFromWorld(rayFromWorld),
-	m_rayToWorld(rayToWorld),
-	m_world(world),
-	m_resultCallback(resultCallback)
-	{
-		m_rayFromTrans.setIdentity();
-		m_rayFromTrans.setOrigin(m_rayFromWorld);
-		m_rayToTrans.setIdentity();
-		m_rayToTrans.setOrigin(m_rayToWorld);
-
-		btVector3 rayDir = (rayToWorld-rayFromWorld);
-
-		rayDir.normalize ();
-		///what about division by zero? --> just set rayDirection[i] to INF/1e30
-		m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
-		m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
-		m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
-		m_signs[0] = m_rayDirectionInverse[0] < 0.0;
-		m_signs[1] = m_rayDirectionInverse[1] < 0.0;
-		m_signs[2] = m_rayDirectionInverse[2] < 0.0;
-
-		m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld);
-
-	}
-
-	
-
-	virtual bool	process(const btBroadphaseProxy* proxy)
-	{
-		///terminate further ray tests, once the closestHitFraction reached zero
-		if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
-			return false;
-
-		btCollisionObject*	collisionObject = (btCollisionObject*)proxy->m_clientObject;
-
-		//only perform raycast if filterMask matches
-		if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) 
-		{
-			//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
-			//btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
-#if 0
-#ifdef RECALCULATE_AABB
-			btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
-			collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
-#else
-			//getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
-			const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
-			const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
-#endif
-#endif
-			//btScalar hitLambda = m_resultCallback.m_closestHitFraction;
-			//culling already done by broadphase
-			//if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
-			{
-				m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
-					collisionObject,
-						collisionObject->getCollisionShape(),
-						collisionObject->getWorldTransform(),
-						m_resultCallback);
-			}
-		}
-		return true;
-	}
-};
-
-void	btSoftMultiBodyDynamicsWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
-{
-	BT_PROFILE("rayTest");
-	/// use the broadphase to accelerate the search for objects, based on their aabb
-	/// and for each object with ray-aabb overlap, perform an exact ray test
-	btSoftSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback);
-
-#ifndef USE_BRUTEFORCE_RAYBROADPHASE
-	m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB);
-#else
-	for (int i=0;i<this->getNumCollisionObjects();i++)
-	{
-		rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
-	}	
-#endif //USE_BRUTEFORCE_RAYBROADPHASE
-
-}
-
-
-void	btSoftMultiBodyDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
-					  btCollisionObject* collisionObject,
-					  const btCollisionShape* collisionShape,
-					  const btTransform& colObjWorldTransform,
-					  RayResultCallback& resultCallback)
-{
-	if (collisionShape->isSoftBody()) {
-		btSoftBody* softBody = btSoftBody::upcast(collisionObject);
-		if (softBody) {
-			btSoftBody::sRayCast softResult;
-			if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) 
-			{
-				
-				if (softResult.fraction<= resultCallback.m_closestHitFraction)
-				{
-
-					btCollisionWorld::LocalShapeInfo shapeInfo;
-					shapeInfo.m_shapePart = 0;
-					shapeInfo.m_triangleIndex = softResult.index;
-					// get the normal
-					btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
-					btVector3 normal=-rayDir;
-					normal.normalize();
-
-					if (softResult.feature == btSoftBody::eFeature::Face)
-					{
-						normal = softBody->m_faces[softResult.index].m_normal;
-						if (normal.dot(rayDir) > 0) {
-							// normal always point toward origin of the ray
-							normal = -normal;
-						}
-					}
-	
-					btCollisionWorld::LocalRayResult rayResult
-						(collisionObject,
-						 &shapeInfo,
-						 normal,
-						 softResult.fraction);
-					bool	normalInWorldSpace = true;
-					resultCallback.addSingleResult(rayResult,normalInWorldSpace);
-				}
-			}
-		}
-	} 
-	else {
-		btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,collisionObject,collisionShape,colObjWorldTransform,resultCallback);
-	}
-}
-
-
-void	btSoftMultiBodyDynamicsWorld::serializeSoftBodies(btSerializer* serializer)
-{
-	int i;
-	//serialize all collision objects
-	for (i=0;i<m_collisionObjects.size();i++)
-	{
-		btCollisionObject* colObj = m_collisionObjects[i];
-		if (colObj->getInternalType() & btCollisionObject::CO_SOFT_BODY)
-		{
-			int len = colObj->calculateSerializeBufferSize();
-			btChunk* chunk = serializer->allocate(len,1);
-			const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
-			serializer->finalizeChunk(chunk,structType,BT_SOFTBODY_CODE,colObj);
-		}
-	}
-
-}
-
-void	btSoftMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
-{
-
-	serializer->startSerialization();
-
-	serializeDynamicsWorldInfo( serializer);
-
-	serializeSoftBodies(serializer);
-
-	serializeRigidBodies(serializer);
-
-	serializeCollisionObjects(serializer);
-
-	serializer->finishSerialization();
-}
-
-
diff --git a/src/BulletDynamics/Featherstone/btSoftMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btSoftMultiBodyDynamicsWorld.h
deleted file mode 100644
index 79ff913d6..000000000
--- a/src/BulletDynamics/Featherstone/btSoftMultiBodyDynamicsWorld.h
+++ /dev/null
@@ -1,108 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 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.
-*/
-
-#ifndef BT_SOFT_MULTIBODY_DYNAMICS_WORLD_H
-#define BT_SOFT_MULTIBODY_DYNAMICS_WORLD_H
-
-#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
-#include "btMultiBodyDynamicsWorld.h"
-#include "BulletSoftBody/btSoftBody.h"
-
-typedef	btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
-
-class btSoftBodySolver;
-
-class btSoftMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld
-{
-
-	btSoftBodyArray	m_softBodies;
-	int				m_drawFlags;
-	bool			m_drawNodeTree;
-	bool			m_drawFaceTree;
-	bool			m_drawClusterTree;
-	btSoftBodyWorldInfo m_sbi;
-	///Solver classes that encapsulate multiple soft bodies for solving
-	btSoftBodySolver *m_softBodySolver;
-	bool			m_ownsSolver;
-
-protected:
-
-	virtual void	predictUnconstraintMotion(btScalar timeStep);
-
-	virtual void	internalSingleStepSimulation( btScalar timeStep);
-
-	void	solveSoftBodiesConstraints( btScalar timeStep );
-
-	void	serializeSoftBodies(btSerializer* serializer);
-
-public:
-
-	btSoftMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btSoftBodySolver *softBodySolver = 0 );
-
-	virtual ~btSoftMultiBodyDynamicsWorld();
-
-	virtual void	debugDrawWorld();
-
-	void	addSoftBody(btSoftBody* body,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter);
-
-	void	removeSoftBody(btSoftBody* body);
-
-	///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btDiscreteDynamicsWorld::removeCollisionObject
-	virtual void	removeCollisionObject(btCollisionObject* collisionObject);
-
-	int		getDrawFlags() const { return(m_drawFlags); }
-	void	setDrawFlags(int f)	{ m_drawFlags=f; }
-
-	btSoftBodyWorldInfo&	getWorldInfo()
-	{
-		return m_sbi;
-	}
-	const btSoftBodyWorldInfo&	getWorldInfo() const
-	{
-		return m_sbi;
-	}
-
-	virtual btDynamicsWorldType	getWorldType() const
-	{
-		return	BT_SOFT_MULTIBODY_DYNAMICS_WORLD;
-	}
-
-	btSoftBodyArray& getSoftBodyArray()
-	{
-		return m_softBodies;
-	}
-
-	const btSoftBodyArray& getSoftBodyArray() const
-	{
-		return m_softBodies;
-	}
-
-
-	virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; 
-
-	/// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest.
-	/// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape.
-	/// This allows more customization.
-	static void	rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
-					  btCollisionObject* collisionObject,
-					  const btCollisionShape* collisionShape,
-					  const btTransform& colObjWorldTransform,
-					  RayResultCallback& resultCallback);
-
-	virtual	void	serialize(btSerializer* serializer);
-
-};
-
-#endif //BT_SOFT_MULTIBODY_DYNAMICS_WORLD_H
diff --git a/src/BulletSoftBody/CMakeLists.txt b/src/BulletSoftBody/CMakeLists.txt
index 1538eb037..d43df1c67 100644
--- a/src/BulletSoftBody/CMakeLists.txt
+++ b/src/BulletSoftBody/CMakeLists.txt
@@ -13,6 +13,7 @@ SET(BulletSoftBody_SRCS
 	btSoftBodyRigidBodyCollisionConfiguration.cpp
 	btSoftRigidCollisionAlgorithm.cpp
 	btSoftRigidDynamicsWorld.cpp
+	btSoftMultiBodyDynamicsWorld.cpp
 	btSoftSoftCollisionAlgorithm.cpp
 	btDefaultSoftBodySolver.cpp
 
@@ -26,6 +27,7 @@ SET(BulletSoftBody_HDRS
 	btSoftBodyRigidBodyCollisionConfiguration.h
 	btSoftRigidCollisionAlgorithm.h
 	btSoftRigidDynamicsWorld.h
+	btSoftMultiBodyDynamicsWorld.h
 	btSoftSoftCollisionAlgorithm.h
 	btSparseSDF.h
 
diff --git a/test/BulletDynamics/pendulum/CMakeLists.txt b/test/BulletDynamics/pendulum/CMakeLists.txt
index 323aba414..570dc0715 100644
--- a/test/BulletDynamics/pendulum/CMakeLists.txt
+++ b/test/BulletDynamics/pendulum/CMakeLists.txt
@@ -10,7 +10,7 @@ ADD_DEFINITIONS(-DUSE_GTEST)
 ADD_DEFINITIONS(-D_VARIADIC_MAX=10)
 
 LINK_LIBRARIES(
- BulletDynamics BulletCollision LinearMath gtest BulletSoftBody
+ BulletDynamics BulletCollision LinearMath gtest
 )
 
 IF (NOT WIN32)
diff --git a/test/InverseDynamics/CMakeLists.txt b/test/InverseDynamics/CMakeLists.txt
index 488184fd3..217655b09 100644
--- a/test/InverseDynamics/CMakeLists.txt
+++ b/test/InverseDynamics/CMakeLists.txt
@@ -13,7 +13,7 @@ INCLUDE_DIRECTORIES(
 ADD_DEFINITIONS(-D_VARIADIC_MAX=10)
 
 LINK_LIBRARIES(
- BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest BulletSoftBody
+ BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest 
 )
 IF (NOT WIN32)
 	LINK_LIBRARIES(		pthread	)
@@ -44,7 +44,7 @@ INCLUDE_DIRECTORIES(
 ADD_DEFINITIONS(-D_VARIADIC_MAX=10)
 
 LINK_LIBRARIES(
-BulletDynamics BulletCollision  BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest BulletSoftBody
+BulletDynamics BulletCollision  BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest 
 )
 IF (NOT WIN32)
         LINK_LIBRARIES(         pthread )
@@ -110,7 +110,7 @@ INCLUDE_DIRECTORIES(
 ADD_DEFINITIONS(-D_VARIADIC_MAX=10)
 
 LINK_LIBRARIES(
- BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest BulletSoftBody
+ BulletInverseDynamicsUtils BulletInverseDynamics Bullet3Common LinearMath gtest 
 )
 IF (NOT WIN32)
         LINK_LIBRARIES(         pthread )

From 3dc56d7e922c8057a7edc9974e556c195bd77ff3 Mon Sep 17 00:00:00 2001
From: yunfeibai <byf1658@gmail.com>
Date: Mon, 17 Oct 2016 23:46:56 -0700
Subject: [PATCH 10/11] Add files.

---
 .../btSoftMultiBodyDynamicsWorld.cpp          | 367 ++++++++++++++++++
 .../btSoftMultiBodyDynamicsWorld.h            | 108 ++++++
 2 files changed, 475 insertions(+)
 create mode 100644 src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp
 create mode 100644 src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h

diff --git a/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp
new file mode 100644
index 000000000..ffa243ebf
--- /dev/null
+++ b/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp
@@ -0,0 +1,367 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 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.
+*/
+
+
+#include "btSoftMultiBodyDynamicsWorld.h"
+#include "LinearMath/btQuickprof.h"
+
+//softbody & helpers
+#include "BulletSoftBody/btSoftBody.h"
+#include "BulletSoftBody/btSoftBodyHelpers.h"
+#include "BulletSoftBody/btSoftBodySolvers.h"
+#include "BulletSoftBody/btDefaultSoftBodySolver.h"
+#include "LinearMath/btSerializer.h"
+
+
+btSoftMultiBodyDynamicsWorld::btSoftMultiBodyDynamicsWorld(
+	btDispatcher* dispatcher,
+	btBroadphaseInterface* pairCache,
+	btMultiBodyConstraintSolver* constraintSolver,
+	btCollisionConfiguration* collisionConfiguration,
+	btSoftBodySolver *softBodySolver ) : 
+		btMultiBodyDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration),
+        m_softBodySolver( softBodySolver ),
+        m_ownsSolver(false)
+{
+	if( !m_softBodySolver )
+	{
+		void* ptr = btAlignedAlloc(sizeof(btDefaultSoftBodySolver),16);
+		m_softBodySolver = new(ptr) btDefaultSoftBodySolver();
+		m_ownsSolver = true;
+	}
+
+	m_drawFlags			=	fDrawFlags::Std;
+	m_drawNodeTree		=	true;
+	m_drawFaceTree		=	false;
+	m_drawClusterTree	=	false;
+	m_sbi.m_broadphase = pairCache;
+	m_sbi.m_dispatcher = dispatcher;
+	m_sbi.m_sparsesdf.Initialize();
+	m_sbi.m_sparsesdf.Reset();
+
+	m_sbi.air_density		=	(btScalar)1.2;
+	m_sbi.water_density	=	0;
+	m_sbi.water_offset		=	0;
+	m_sbi.water_normal		=	btVector3(0,0,0);
+	m_sbi.m_gravity.setValue(0,-10,0);
+
+	m_sbi.m_sparsesdf.Initialize();
+
+
+}
+
+btSoftMultiBodyDynamicsWorld::~btSoftMultiBodyDynamicsWorld()
+{
+	if (m_ownsSolver)
+	{
+		m_softBodySolver->~btSoftBodySolver();
+		btAlignedFree(m_softBodySolver);
+	}
+}
+
+void	btSoftMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
+{
+	btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep );
+	{
+		BT_PROFILE("predictUnconstraintMotionSoftBody");
+		m_softBodySolver->predictMotion( float(timeStep) );
+	}
+}
+
+void	btSoftMultiBodyDynamicsWorld::internalSingleStepSimulation( btScalar timeStep )
+{
+
+	// Let the solver grab the soft bodies and if necessary optimize for it
+	m_softBodySolver->optimize( getSoftBodyArray() );
+
+	if( !m_softBodySolver->checkInitialized() )
+	{
+		btAssert( "Solver initialization failed\n" );
+	}
+
+	btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep );
+
+	///solve soft bodies constraints
+	solveSoftBodiesConstraints( timeStep );
+
+	//self collisions
+	for ( int i=0;i<m_softBodies.size();i++)
+	{
+		btSoftBody*	psb=(btSoftBody*)m_softBodies[i];
+		psb->defaultCollisionHandler(psb);
+	}
+
+	///update soft bodies
+	m_softBodySolver->updateSoftBodies( );
+	
+	// End solver-wise simulation step
+	// ///////////////////////////////
+
+}
+
+void	btSoftMultiBodyDynamicsWorld::solveSoftBodiesConstraints( btScalar timeStep )
+{
+	BT_PROFILE("solveSoftConstraints");
+
+	if(m_softBodies.size())
+	{
+		btSoftBody::solveClusters(m_softBodies);
+	}
+
+	// Solve constraints solver-wise
+	m_softBodySolver->solveConstraints( timeStep * m_softBodySolver->getTimeScale() );
+
+}
+
+void	btSoftMultiBodyDynamicsWorld::addSoftBody(btSoftBody* body,short int collisionFilterGroup,short int collisionFilterMask)
+{
+	m_softBodies.push_back(body);
+
+	// Set the soft body solver that will deal with this body
+	// to be the world's solver
+	body->setSoftBodySolver( m_softBodySolver );
+
+	btCollisionWorld::addCollisionObject(body,
+		collisionFilterGroup,
+		collisionFilterMask);
+
+}
+
+void	btSoftMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body)
+{
+	m_softBodies.remove(body);
+
+	btCollisionWorld::removeCollisionObject(body);
+}
+
+void	btSoftMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
+{
+	btSoftBody* body = btSoftBody::upcast(collisionObject);
+	if (body)
+		removeSoftBody(body);
+	else
+		btDiscreteDynamicsWorld::removeCollisionObject(collisionObject);
+}
+
+void	btSoftMultiBodyDynamicsWorld::debugDrawWorld()
+{
+	btDiscreteDynamicsWorld::debugDrawWorld();
+
+	if (getDebugDrawer())
+	{
+		int i;
+		for (  i=0;i<this->m_softBodies.size();i++)
+		{
+			btSoftBody*	psb=(btSoftBody*)this->m_softBodies[i];
+			if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
+			{
+				btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer);
+				btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags);
+			}
+			
+			if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
+			{
+				if(m_drawNodeTree)		btSoftBodyHelpers::DrawNodeTree(psb,m_debugDrawer);
+				if(m_drawFaceTree)		btSoftBodyHelpers::DrawFaceTree(psb,m_debugDrawer);
+				if(m_drawClusterTree)	btSoftBodyHelpers::DrawClusterTree(psb,m_debugDrawer);
+			}
+		}		
+	}	
+}
+
+
+
+
+struct btSoftSingleRayCallback : public btBroadphaseRayCallback
+{
+	btVector3	m_rayFromWorld;
+	btVector3	m_rayToWorld;
+	btTransform	m_rayFromTrans;
+	btTransform	m_rayToTrans;
+	btVector3	m_hitNormal;
+
+	const btSoftMultiBodyDynamicsWorld*	m_world;
+	btCollisionWorld::RayResultCallback&	m_resultCallback;
+
+	btSoftSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btSoftMultiBodyDynamicsWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
+	:m_rayFromWorld(rayFromWorld),
+	m_rayToWorld(rayToWorld),
+	m_world(world),
+	m_resultCallback(resultCallback)
+	{
+		m_rayFromTrans.setIdentity();
+		m_rayFromTrans.setOrigin(m_rayFromWorld);
+		m_rayToTrans.setIdentity();
+		m_rayToTrans.setOrigin(m_rayToWorld);
+
+		btVector3 rayDir = (rayToWorld-rayFromWorld);
+
+		rayDir.normalize ();
+		///what about division by zero? --> just set rayDirection[i] to INF/1e30
+		m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
+		m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
+		m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
+		m_signs[0] = m_rayDirectionInverse[0] < 0.0;
+		m_signs[1] = m_rayDirectionInverse[1] < 0.0;
+		m_signs[2] = m_rayDirectionInverse[2] < 0.0;
+
+		m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld);
+
+	}
+
+	
+
+	virtual bool	process(const btBroadphaseProxy* proxy)
+	{
+		///terminate further ray tests, once the closestHitFraction reached zero
+		if (m_resultCallback.m_closestHitFraction == btScalar(0.f))
+			return false;
+
+		btCollisionObject*	collisionObject = (btCollisionObject*)proxy->m_clientObject;
+
+		//only perform raycast if filterMask matches
+		if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) 
+		{
+			//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
+			//btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+#if 0
+#ifdef RECALCULATE_AABB
+			btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
+			collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);
+#else
+			//getBroadphase()->getAabb(collisionObject->getBroadphaseHandle(),collisionObjectAabbMin,collisionObjectAabbMax);
+			const btVector3& collisionObjectAabbMin = collisionObject->getBroadphaseHandle()->m_aabbMin;
+			const btVector3& collisionObjectAabbMax = collisionObject->getBroadphaseHandle()->m_aabbMax;
+#endif
+#endif
+			//btScalar hitLambda = m_resultCallback.m_closestHitFraction;
+			//culling already done by broadphase
+			//if (btRayAabb(m_rayFromWorld,m_rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,m_hitNormal))
+			{
+				m_world->rayTestSingle(m_rayFromTrans,m_rayToTrans,
+					collisionObject,
+						collisionObject->getCollisionShape(),
+						collisionObject->getWorldTransform(),
+						m_resultCallback);
+			}
+		}
+		return true;
+	}
+};
+
+void	btSoftMultiBodyDynamicsWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
+{
+	BT_PROFILE("rayTest");
+	/// use the broadphase to accelerate the search for objects, based on their aabb
+	/// and for each object with ray-aabb overlap, perform an exact ray test
+	btSoftSingleRayCallback rayCB(rayFromWorld,rayToWorld,this,resultCallback);
+
+#ifndef USE_BRUTEFORCE_RAYBROADPHASE
+	m_broadphasePairCache->rayTest(rayFromWorld,rayToWorld,rayCB);
+#else
+	for (int i=0;i<this->getNumCollisionObjects();i++)
+	{
+		rayCB.process(m_collisionObjects[i]->getBroadphaseHandle());
+	}	
+#endif //USE_BRUTEFORCE_RAYBROADPHASE
+
+}
+
+
+void	btSoftMultiBodyDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
+					  btCollisionObject* collisionObject,
+					  const btCollisionShape* collisionShape,
+					  const btTransform& colObjWorldTransform,
+					  RayResultCallback& resultCallback)
+{
+	if (collisionShape->isSoftBody()) {
+		btSoftBody* softBody = btSoftBody::upcast(collisionObject);
+		if (softBody) {
+			btSoftBody::sRayCast softResult;
+			if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) 
+			{
+				
+				if (softResult.fraction<= resultCallback.m_closestHitFraction)
+				{
+
+					btCollisionWorld::LocalShapeInfo shapeInfo;
+					shapeInfo.m_shapePart = 0;
+					shapeInfo.m_triangleIndex = softResult.index;
+					// get the normal
+					btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
+					btVector3 normal=-rayDir;
+					normal.normalize();
+
+					if (softResult.feature == btSoftBody::eFeature::Face)
+					{
+						normal = softBody->m_faces[softResult.index].m_normal;
+						if (normal.dot(rayDir) > 0) {
+							// normal always point toward origin of the ray
+							normal = -normal;
+						}
+					}
+	
+					btCollisionWorld::LocalRayResult rayResult
+						(collisionObject,
+						 &shapeInfo,
+						 normal,
+						 softResult.fraction);
+					bool	normalInWorldSpace = true;
+					resultCallback.addSingleResult(rayResult,normalInWorldSpace);
+				}
+			}
+		}
+	} 
+	else {
+		btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,collisionObject,collisionShape,colObjWorldTransform,resultCallback);
+	}
+}
+
+
+void	btSoftMultiBodyDynamicsWorld::serializeSoftBodies(btSerializer* serializer)
+{
+	int i;
+	//serialize all collision objects
+	for (i=0;i<m_collisionObjects.size();i++)
+	{
+		btCollisionObject* colObj = m_collisionObjects[i];
+		if (colObj->getInternalType() & btCollisionObject::CO_SOFT_BODY)
+		{
+			int len = colObj->calculateSerializeBufferSize();
+			btChunk* chunk = serializer->allocate(len,1);
+			const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);
+			serializer->finalizeChunk(chunk,structType,BT_SOFTBODY_CODE,colObj);
+		}
+	}
+
+}
+
+void	btSoftMultiBodyDynamicsWorld::serialize(btSerializer* serializer)
+{
+
+	serializer->startSerialization();
+
+	serializeDynamicsWorldInfo( serializer);
+
+	serializeSoftBodies(serializer);
+
+	serializeRigidBodies(serializer);
+
+	serializeCollisionObjects(serializer);
+
+	serializer->finishSerialization();
+}
+
+
diff --git a/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h
new file mode 100644
index 000000000..2d0423a44
--- /dev/null
+++ b/src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.h
@@ -0,0 +1,108 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 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.
+*/
+
+#ifndef BT_SOFT_MULTIBODY_DYNAMICS_WORLD_H
+#define BT_SOFT_MULTIBODY_DYNAMICS_WORLD_H
+
+#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+#include "BulletSoftBody/btSoftBody.h"
+
+typedef	btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
+
+class btSoftBodySolver;
+
+class btSoftMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld
+{
+
+	btSoftBodyArray	m_softBodies;
+	int				m_drawFlags;
+	bool			m_drawNodeTree;
+	bool			m_drawFaceTree;
+	bool			m_drawClusterTree;
+	btSoftBodyWorldInfo m_sbi;
+	///Solver classes that encapsulate multiple soft bodies for solving
+	btSoftBodySolver *m_softBodySolver;
+	bool			m_ownsSolver;
+
+protected:
+
+	virtual void	predictUnconstraintMotion(btScalar timeStep);
+
+	virtual void	internalSingleStepSimulation( btScalar timeStep);
+
+	void	solveSoftBodiesConstraints( btScalar timeStep );
+
+	void	serializeSoftBodies(btSerializer* serializer);
+
+public:
+
+	btSoftMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btSoftBodySolver *softBodySolver = 0 );
+
+	virtual ~btSoftMultiBodyDynamicsWorld();
+
+	virtual void	debugDrawWorld();
+
+	void	addSoftBody(btSoftBody* body,short int collisionFilterGroup=btBroadphaseProxy::DefaultFilter,short int collisionFilterMask=btBroadphaseProxy::AllFilter);
+
+	void	removeSoftBody(btSoftBody* body);
+
+	///removeCollisionObject will first check if it is a rigid body, if so call removeRigidBody otherwise call btDiscreteDynamicsWorld::removeCollisionObject
+	virtual void	removeCollisionObject(btCollisionObject* collisionObject);
+
+	int		getDrawFlags() const { return(m_drawFlags); }
+	void	setDrawFlags(int f)	{ m_drawFlags=f; }
+
+	btSoftBodyWorldInfo&	getWorldInfo()
+	{
+		return m_sbi;
+	}
+	const btSoftBodyWorldInfo&	getWorldInfo() const
+	{
+		return m_sbi;
+	}
+
+	virtual btDynamicsWorldType	getWorldType() const
+	{
+		return	BT_SOFT_MULTIBODY_DYNAMICS_WORLD;
+	}
+
+	btSoftBodyArray& getSoftBodyArray()
+	{
+		return m_softBodies;
+	}
+
+	const btSoftBodyArray& getSoftBodyArray() const
+	{
+		return m_softBodies;
+	}
+
+
+	virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; 
+
+	/// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest.
+	/// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape.
+	/// This allows more customization.
+	static void	rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
+					  btCollisionObject* collisionObject,
+					  const btCollisionShape* collisionShape,
+					  const btTransform& colObjWorldTransform,
+					  RayResultCallback& resultCallback);
+
+	virtual	void	serialize(btSerializer* serializer);
+
+};
+
+#endif //BT_SOFT_MULTIBODY_DYNAMICS_WORLD_H

From 7630bf62d56efe6119d834ab006da8253426c68f Mon Sep 17 00:00:00 2001
From: yunfeibai <byf1658@gmail.com>
Date: Tue, 18 Oct 2016 17:38:43 -0700
Subject: [PATCH 11/11] Add USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD option.

---
 CMakeLists.txt                                |  5 +++
 .../PhysicsServerCommandProcessor.cpp         | 32 +++++++++++++++----
 test/SharedMemory/CMakeLists.txt              |  5 ++-
 3 files changed, 35 insertions(+), 7 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 41a4ba98d..acc474da3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -27,6 +27,7 @@ SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -D_DEBUG")
 OPTION(USE_DOUBLE_PRECISION "Use double precision"	OFF)
 OPTION(USE_GRAPHICAL_BENCHMARK "Use Graphical Benchmark" ON)
 OPTION(BUILD_SHARED_LIBS "Use shared libraries" OFF)
+OPTION(USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD "Use btSoftMultiBodyDynamicsWorld" OFF)	
 
 OPTION(USE_MSVC_INCREMENTAL_LINKING "Use MSVC Incremental Linking" OFF)
 OPTION(USE_CUSTOM_VECTOR_MATH "Use custom vectormath library" OFF)
@@ -146,6 +147,10 @@ ADD_DEFINITIONS( -DBT_USE_DOUBLE_PRECISION)
 SET( BULLET_DOUBLE_DEF "-DBT_USE_DOUBLE_PRECISION")
 ENDIF (USE_DOUBLE_PRECISION)
 
+IF (USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
+ADD_DEFINITIONS(-DUSE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
+ENDIF (USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
+
 IF(USE_GRAPHICAL_BENCHMARK)
 ADD_DEFINITIONS( -DUSE_GRAPHICAL_BENCHMARK)
 ENDIF (USE_GRAPHICAL_BENCHMARK)
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index a46613c59..4c86279ac 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -25,11 +25,16 @@
 #include "Bullet3Common/b3Logging.h"
 #include "../CommonInterfaces/CommonGUIHelperInterface.h"
 #include "SharedMemoryCommands.h"
+
+#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
 #include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
 #include "BulletSoftBody/btSoftBodySolvers.h"
 #include "BulletSoftBody/btSoftBodyHelpers.h"
 #include "BulletSoftBody/btSoftMultiBodyDynamicsWorld.h"
 #include "../SoftDemo/BunnyMesh.h"
+#else
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+#endif
 
 
 //@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
@@ -434,12 +439,18 @@ struct PhysicsServerCommandProcessorInternalData
 	btBroadphaseInterface*	m_broadphase;
 	btCollisionDispatcher*	m_dispatcher;
 	btMultiBodyConstraintSolver*	m_solver;
-    btSoftBodySolver* m_softbodySolver;
 	btDefaultCollisionConfiguration* m_collisionConfiguration;
+    
+#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
 	btSoftMultiBodyDynamicsWorld* m_dynamicsWorld;
-	SharedMemoryDebugDrawer*		m_remoteDebugDrawer;
+    btSoftBodySolver* m_softbodySolver;
     btSoftBodyWorldInfo	m_softBodyWorldInfo;
-	
+#else
+    btMultiBodyDynamicsWorld* m_dynamicsWorld;
+#endif
+    
+	SharedMemoryDebugDrawer*		m_remoteDebugDrawer;
+    
 	btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
 
 	btAlignedObjectArray<int> m_sdfRecentLoadedBodies;
@@ -615,9 +626,12 @@ PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor()
 void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 {
     ///collision configuration contains default setup for memory, collision setup
-    //m_data->m_collisionConfiguration = new btDefaultCollisionConfiguration();
     //m_collisionConfiguration->setConvexConvexMultipointIterations();
+#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
     m_data->m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
+#else
+    m_data->m_collisionConfiguration = new btDefaultCollisionConfiguration();
+#endif
     ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
     m_data->m_dispatcher = new	btCollisionDispatcher(m_data->m_collisionConfiguration);
     
@@ -625,7 +639,11 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
     
     m_data->m_solver = new btMultiBodyConstraintSolver;
     
+#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
     m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
+#else
+    m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
+#endif
     
     //Workaround: in a VR application, where we avoid synchronizaing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
     m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(8192);
@@ -1609,6 +1627,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                 }
                 case CMD_LOAD_BUNNY:
                 {
+#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
                     m_data->m_softBodyWorldInfo.air_density		=	(btScalar)1.2;
                     m_data->m_softBodyWorldInfo.water_density	=	0;
                     m_data->m_softBodyWorldInfo.water_offset	=	0;
@@ -1633,7 +1652,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                     psb->getCollisionShape()->setMargin(0.01);
                     
                     m_data->m_dynamicsWorld->addSoftBody(psb);
-
+#endif
                     break;
                 }
                 case CMD_CREATE_SENSOR:
@@ -2979,7 +2998,7 @@ void PhysicsServerCommandProcessor::renderScene()
 		m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld);
 		m_data->m_guiHelper->render(m_data->m_dynamicsWorld);
 	}
-
+#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
     for (  int i=0;i<m_data->m_dynamicsWorld->getSoftBodyArray().size();i++)
     {
         btSoftBody*	psb=(btSoftBody*)m_data->m_dynamicsWorld->getSoftBodyArray()[i];
@@ -2989,6 +3008,7 @@ void PhysicsServerCommandProcessor::renderScene()
             btSoftBodyHelpers::Draw(psb,m_data->m_dynamicsWorld->getDebugDrawer(),m_data->m_dynamicsWorld->getDrawFlags());
         }
     }
+#endif
 }
 
 void    PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags)
diff --git a/test/SharedMemory/CMakeLists.txt b/test/SharedMemory/CMakeLists.txt
index 491e54186..8ece789cc 100644
--- a/test/SharedMemory/CMakeLists.txt
+++ b/test/SharedMemory/CMakeLists.txt
@@ -13,8 +13,11 @@ ADD_DEFINITIONS(-DPHYSICS_LOOP_BACK -DPHYSICS_SERVER_DIRECT -DENABLE_GTEST -D_VA
 
 
 LINK_LIBRARIES(
- BulletInverseDynamicsUtils BulletInverseDynamics BulletFileLoader BulletWorldImporter Bullet3Common BulletDynamics BulletCollision LinearMath gtest BussIK BulletSoftBody
+ BulletInverseDynamicsUtils BulletInverseDynamics BulletFileLoader BulletWorldImporter Bullet3Common BulletDynamics BulletCollision LinearMath gtest BussIK
 )
+IF (USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
+	LINK_LIBRARIES(BulletSoftBody)
+ENDIF()
 
 IF (NOT WIN32)
 	LINK_LIBRARIES(		pthread	)