Merge remote-tracking branch 'origin/master'

README.md

@@ -5,7 +5,20 @@
 
 This is the official C++ source code repository of the Bullet Physics SDK: real-time collision detection and multi-physics simulation for VR, games, visual effects, robotics, machine learning etc.
 
-New in Bullet 2.85: pybullet Python bindings, improved support for robotics and VR. Use pip install pybullet and see [PyBullet Quickstart Guide](https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.2ye70wns7io3).
+## PyBullet ##
+New in Bullet 2.85: pybullet Python bindings, improved support for robotics and VR. Use pip install pybullet and checkout the [PyBullet Quickstart Guide](https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.2ye70wns7io3).
+
+If you use PyBullet in your research, please cite it like this:
+
+```
+@MISC{coumans2019,
+author =   {Erwin Coumans and Yunfei Bai},
+title =    {PyBullet, a Python module for physics simulation for games, robotics and machine learning},
+howpublished = {\url{http://pybullet.org}},
+year = {2016--2019}
+}
+```
+
 
 The Bullet 2 API will stay default and up-to-date while slowly moving to a new API.
 The steps towards a new API is in a nutshell:

data/cloth.obj

@@ -0,0 +1,64 @@
+# Blender v2.79 (sub 0) OBJ File: ''
+# www.blender.org
+mtllib cloth.mtl
+o Plane_Plane.001
+v -1.000000 0.000000 1.000000
+v 1.000000 0.000000 1.000000
+v -1.000000 0.000000 -1.000000
+v 1.000000 0.000000 -1.000000
+v -1.000000 0.000000 0.000000
+v 0.000000 0.000000 1.000000
+v 1.000000 0.000000 0.000000
+v 0.000000 0.000000 -1.000000
+v 0.000000 0.000000 0.000000
+v -1.000000 0.000000 0.500000
+v 0.500000 0.000000 1.000000
+v 1.000000 0.000000 -0.500000
+v -0.500000 0.000000 -1.000000
+v -1.000000 0.000000 -0.500000
+v -0.500000 0.000000 1.000000
+v 1.000000 0.000000 0.500000
+v 0.500000 0.000000 -1.000000
+v 0.000000 0.000000 -0.500000
+v 0.000000 0.000000 0.500000
+v -0.500000 0.000000 0.000000
+v 0.500000 0.000000 0.000000
+v 0.500000 0.000000 0.500000
+v -0.500000 0.000000 0.500000
+v -0.500000 0.000000 -0.500000
+v 0.500000 0.000000 -0.500000
+vn 0.0000 1.0000 0.0000
+usemtl None
+s off
+f 12//1 17//1 25//1
+f 18//1 13//1 24//1
+f 19//1 20//1 23//1
+f 16//1 21//1 22//1
+f 22//1 9//1 19//1
+f 11//1 19//1 6//1
+f 2//1 22//1 11//1
+f 23//1 5//1 10//1
+f 15//1 10//1 1//1
+f 6//1 23//1 15//1
+f 24//1 3//1 14//1
+f 20//1 14//1 5//1
+f 9//1 24//1 20//1
+f 25//1 8//1 18//1
+f 21//1 18//1 9//1
+f 7//1 25//1 21//1
+f 12//1 4//1 17//1
+f 18//1 8//1 13//1
+f 19//1 9//1 20//1
+f 16//1 7//1 21//1
+f 22//1 21//1 9//1
+f 11//1 22//1 19//1
+f 2//1 16//1 22//1
+f 23//1 20//1 5//1
+f 15//1 23//1 10//1
+f 6//1 19//1 23//1
+f 24//1 13//1 3//1
+f 20//1 24//1 14//1
+f 9//1 18//1 24//1
+f 25//1 17//1 8//1
+f 21//1 25//1 18//1
+f 7//1 12//1 25//1

data/cloth_z_up.obj

@@ -0,0 +1,64 @@
+# Blender v2.79 (sub 0) OBJ File: ''
+# www.blender.org
+mtllib cloth_z_up.mtl
+o Plane_Plane.001
+v -1.000000 -1.000000 0.000000
+v 1.000000 -1.000000 0.000000
+v -1.000000 1.000000 -0.000000
+v 1.000000 1.000000 -0.000000
+v -1.000000 0.000000 0.000000
+v -0.000000 -1.000000 0.000000
+v 1.000000 -0.000000 -0.000000
+v 0.000000 1.000000 -0.000000
+v 0.000000 0.000000 0.000000
+v -1.000000 -0.500000 0.000000
+v 0.500000 -1.000000 0.000000
+v 1.000000 0.500000 -0.000000
+v -0.500000 1.000000 -0.000000
+v -1.000000 0.500000 -0.000000
+v -0.500000 -1.000000 0.000000
+v 1.000000 -0.500000 0.000000
+v 0.500000 1.000000 -0.000000
+v 0.000000 0.500000 -0.000000
+v -0.000000 -0.500000 0.000000
+v -0.500000 0.000000 0.000000
+v 0.500000 -0.000000 -0.000000
+v 0.500000 -0.500000 0.000000
+v -0.500000 -0.500000 0.000000
+v -0.500000 0.500000 -0.000000
+v 0.500000 0.500000 -0.000000
+vn 0.0000 0.0000 1.0000
+usemtl None
+s off
+f 12//1 17//1 25//1
+f 18//1 13//1 24//1
+f 19//1 20//1 23//1
+f 16//1 21//1 22//1
+f 22//1 9//1 19//1
+f 11//1 19//1 6//1
+f 2//1 22//1 11//1
+f 23//1 5//1 10//1
+f 15//1 10//1 1//1
+f 6//1 23//1 15//1
+f 24//1 3//1 14//1
+f 20//1 14//1 5//1
+f 9//1 24//1 20//1
+f 25//1 8//1 18//1
+f 21//1 18//1 9//1
+f 7//1 25//1 21//1
+f 12//1 4//1 17//1
+f 18//1 8//1 13//1
+f 19//1 9//1 20//1
+f 16//1 7//1 21//1
+f 22//1 21//1 9//1
+f 11//1 22//1 19//1
+f 2//1 16//1 22//1
+f 23//1 20//1 5//1
+f 15//1 23//1 10//1
+f 6//1 19//1 23//1
+f 24//1 13//1 3//1
+f 20//1 24//1 14//1
+f 9//1 18//1 24//1
+f 25//1 17//1 8//1
+f 21//1 25//1 18//1
+f 7//1 12//1 25//1

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -338,20 +338,20 @@ B3_SHARED_API int b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle
 	return 0;
 }
 
-B3_SHARED_API int b3LoadSoftBodyAddRenderMesh(b3SharedMemoryCommandHandle commandHandle, const char* filename)
+B3_SHARED_API int b3LoadSoftBodyUpdateSimMesh(b3SharedMemoryCommandHandle commandHandle, const char* filename)
 {
     struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
     b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
     int len = strlen(filename);
     if (len < MAX_FILENAME_LENGTH)
     {
-        strcpy(command->m_loadSoftBodyArguments.m_renderFileName, filename);
+        strcpy(command->m_loadSoftBodyArguments.m_simFileName, filename);
     }
     else
     {
-        command->m_loadSoftBodyArguments.m_renderFileName[0] = 0;
+        command->m_loadSoftBodyArguments.m_simFileName[0] = 0;
     }
-    command->m_updateFlags |= LOAD_SOFT_BODY_RENDER_MESH;
+    command->m_updateFlags |= LOAD_SOFT_BODY_SIM_MESH;
     return 0;
 
 }
@@ -3266,6 +3266,30 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetActivationState(b3SharedMemoryCommandHa
 	return 0;
 }
 
+B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateSoftBodyAnchorConstraintCommand(b3PhysicsClientHandle physClient, int softBodyUniqueId, int nodeIndex, int bodyUniqueId, int linkIndex, const double bodyFramePosition[3])
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+	struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+	b3Assert(command);
+	command->m_type = CMD_USER_CONSTRAINT;
+	command->m_updateFlags = USER_CONSTRAINT_ADD_SOFT_BODY_ANCHOR;
+	command->m_userConstraintArguments.m_parentBodyIndex = softBodyUniqueId;
+	command->m_userConstraintArguments.m_parentJointIndex = nodeIndex;
+	command->m_userConstraintArguments.m_childBodyIndex = bodyUniqueId;
+	command->m_userConstraintArguments.m_childJointIndex = linkIndex;
+	command->m_userConstraintArguments.m_childFrame[0] = bodyFramePosition[0];
+	command->m_userConstraintArguments.m_childFrame[1] = bodyFramePosition[1];
+	command->m_userConstraintArguments.m_childFrame[2] = bodyFramePosition[2];
+	command->m_userConstraintArguments.m_childFrame[3] = 0.;
+	command->m_userConstraintArguments.m_childFrame[4] = 0.;
+	command->m_userConstraintArguments.m_childFrame[5] = 0.;
+	command->m_userConstraintArguments.m_childFrame[6] = 1.;
+
+	return (b3SharedMemoryCommandHandle)command;
+}
+
 B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyUniqueId, int parentJointIndex, int childBodyUniqueId, int childJointIndex, struct b3JointInfo* info)
 {
 	PhysicsClient* cl = (PhysicsClient*)physClient;

examples/SharedMemory/PhysicsClientC_API.h

@@ -633,7 +633,7 @@ extern "C"
 	B3_SHARED_API int b3LoadSoftBodySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin);
 	B3_SHARED_API int b3LoadSoftBodySetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX, double startPosY, double startPosZ);
 	B3_SHARED_API int b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW);
-    B3_SHARED_API int b3LoadSoftBodyAddRenderMesh(b3SharedMemoryCommandHandle commandHandle, const char* filename);
+    B3_SHARED_API int b3LoadSoftBodyUpdateSimMesh(b3SharedMemoryCommandHandle commandHandle, const char* filename);
 	B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda);
     B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda);
 	B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda, double NeoHookeanDamping);
@@ -644,6 +644,8 @@ extern "C"
     B3_SHARED_API int b3LoadSoftBodyUseFaceContact(b3SharedMemoryCommandHandle commandHandle, int useFaceContact);
 	B3_SHARED_API int b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHandle commandHandle, double frictionCoefficient);
 	B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle commandHandle, int useBendingSprings);
+	
+	B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateSoftBodyAnchorConstraintCommand(b3PhysicsClientHandle physClient, int softBodyUniqueId, int nodeIndex, int bodyUniqueId, int linkIndex, const double bodyFramePosition[3]);
 
 	B3_SHARED_API b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient);
 	B3_SHARED_API void b3VREventsSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter);

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -53,7 +53,7 @@
 #include "../Extras/Serialize/BulletFileLoader/btBulletFile.h"
 #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
 #include "LinearMath/TaskScheduler/btThreadSupportInterface.h"
-
+#include "Wavefront/tiny_obj_loader.h"
 #ifndef SKIP_COLLISION_FILTER_PLUGIN
 #include "plugins/collisionFilterPlugin/collisionFilterPlugin.h"
 #endif
@@ -1636,18 +1636,14 @@ struct PhysicsServerCommandProcessorInternalData
 	btDefaultCollisionConfiguration* m_collisionConfiguration;
 
 
-//#ifndef SKIP_DEFORMABLE_BODY
+#ifndef SKIP_DEFORMABLE_BODY
 	
 	btDeformableBodySolver* m_deformablebodySolver;
     btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
-//#else//SKIP_DEFORMABLE_BODY
+#endif
-//#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
 	
 	
-//#else//SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
 	btMultiBodyDynamicsWorld* m_dynamicsWorld;
-//#endif//SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
-//#endif//SKIP_DEFORMABLE_BODY
 
 	int m_constraintSolverType;
 	SharedMemoryDebugDrawer* m_remoteDebugDrawer;
@@ -1713,6 +1709,9 @@ struct PhysicsServerCommandProcessorInternalData
 		  m_dispatcher(0),
 		  m_solver(0),
 		  m_collisionConfiguration(0),
+#ifndef SKIP_DEFORMABLE_BODY
+		  m_deformablebodySolver(0),
+#endif
 		  m_dynamicsWorld(0),
 		  m_deformablebodySolver(0),
 		  m_constraintSolverType(-1),
@@ -2652,23 +2651,25 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld(int flags)
 		bv->setVelocityPrediction(0);
 		m_data->m_broadphase = bv;
 	}
-	
+
-	if (flags & RESET_USE_DEFORMABLE_WORLD)
+    if (flags & RESET_USE_DEFORMABLE_WORLD)
 	{
 #ifndef SKIP_DEFORMABLE_BODY
-		m_data->m_deformablebodySolver = new btDeformableBodySolver();
+        m_data->m_deformablebodySolver = new btDeformableBodySolver();
-		btDeformableMultiBodyConstraintSolver* solver = new btDeformableMultiBodyConstraintSolver; 
+        btDeformableMultiBodyConstraintSolver* solver = new btDeformableMultiBodyConstraintSolver;
-		m_data->m_solver = solver;
+        m_data->m_solver = solver;
-		solver->setDeformableSolver(m_data->m_deformablebodySolver);
+        solver->setDeformableSolver(m_data->m_deformablebodySolver);
-		m_data->m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, solver, m_data->m_collisionConfiguration, m_data->m_deformablebodySolver);
+        m_data->m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, solver, m_data->m_collisionConfiguration, m_data->m_deformablebodySolver);
 #endif
 	}
 
-	if ((0==m_data->m_dynamicsWorld) && (0==(flags&RESET_USE_DISCRETE_DYNAMICS_WORLD)))
+        if ((0==m_data->m_dynamicsWorld) && (0==(flags&RESET_USE_DISCRETE_DYNAMICS_WORLD)))
 	{
+        m_data->m_solver = new btMultiBodyConstraintSolver;
 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
-		m_data->m_solver = new btMultiBodyConstraintSolver;
+        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_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
 	}
 
@@ -5127,24 +5128,24 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
 		{
 			btSoftBody* psb = bodyHandle->m_softBody;
 			int totalBytesPerVertex = sizeof(btVector3);
-            bool separateRenderMesh = (psb->m_renderNodes.size() != 0);
+                        bool separateRenderMesh = (psb->m_renderNodes.size() != 0);
-            int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size();
+                        int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size();
 			int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1;
 			int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex;
 			int verticesCopied = btMin(maxNumVertices, numVerticesRemaining);
 			btVector3* verticesOut = (btVector3*)bufferServerToClient;
 			for (int i = 0; i < verticesCopied; ++i)
 			{
-                if (separateRenderMesh)
+                            if (separateRenderMesh)
-                {
+                            {
-                    const btSoftBody::Node& n = psb->m_renderNodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex];
+                                const btSoftBody::Node& n = psb->m_renderNodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex];
-                    verticesOut[i] = n.m_x;
+                                verticesOut[i] = n.m_x;
-                }
+                            }
-                else
+                            else
-                {
+                            {
-                    const btSoftBody::Node& n = psb->m_nodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex];
+                                const btSoftBody::Node& n = psb->m_nodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex];
-                    verticesOut[i] = n.m_x;
+                                verticesOut[i] = n.m_x;
-                }
+                            }
 			}
 
 			serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_COMPLETED;
@@ -8070,7 +8071,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
 	{
 		collisionMargin = clientCmd.m_loadSoftBodyArguments.m_collisionMargin;
 	}
-
+        
 	{
         btSoftBody* psb = NULL;
         
@@ -8084,13 +8085,22 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
 		b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
 	}
 
-        
 	const std::string& error_message_prefix = "";
-	std::string out_found_filename;
+	std::string out_found_filename, out_found_sim_filename;
-	int out_type(0);
+	int out_type(0), out_sim_type(0);
 
 	bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
-        if (out_type == UrdfGeometry::FILE_OBJ)
+
+        if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SIM_MESH)
+        {
+            bool foundSimMesh = UrdfFindMeshFile(fileIO, pathPrefix, loadSoftBodyArgs.m_simFileName, error_message_prefix, &out_found_sim_filename, &out_sim_type);
+        }
+        else
+        {
+          out_sim_type = out_type;
+          out_found_sim_filename = out_found_filename;
+        }
+        if (out_sim_type == UrdfGeometry::FILE_OBJ)
         {
 	    std::vector<tinyobj::shape_t> shapes;
 	    tinyobj::attrib_t attribute;
@@ -8149,7 +8159,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
 			btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
 			if (deformWorld)
 			{
-				psb = btSoftBodyHelpers::CreateFromVtkFile(deformWorld->getWorldInfo(), out_found_filename.c_str());
+				psb = btSoftBodyHelpers::CreateFromVtkFile(deformWorld->getWorldInfo(), out_found_sim_filename.c_str());
 				btScalar corotated_mu, corotated_lambda;
 				if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE)
 				{
@@ -8186,37 +8196,55 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
 		if (psb != NULL)
 		{
 #ifndef SKIP_DEFORMABLE_BODY
-			btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
+		  	btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
-            bool foundRenderMesh = false;
 			if (deformWorld)
 			{
-                if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_RENDER_MESH)
+            			// load render mesh
-                {
+            			if (out_found_sim_filename != out_found_filename)
-                    int out_render_type(0);
+            			{
-                    std::string out_found_render_filename;
+            			    // load render mesh
-                    char relativeRenderFileName[1024];
+	    			    {
-                    char pathRenderPrefix[1024];
+	    			    	tinyobj::attrib_t attribute;
-                    pathRenderPrefix[0] = 0;
+	    			    	std::vector<tinyobj::shape_t> shapes;
-                    if (fileIO->findResourcePath(loadSoftBodyArgs.m_renderFileName, relativeRenderFileName, 1024))
+	    			    	
-                    {
+	    			    	std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), pathPrefix, m_data->m_pluginManager.getFileIOInterface());
-                        b3FileUtils::extractPath(relativeRenderFileName, pathRenderPrefix, 1024);
-                    }
-                    foundRenderMesh = UrdfFindMeshFile(fileIO, pathRenderPrefix, relativeRenderFileName, error_message_prefix, &out_found_render_filename, &out_render_type);
-                    // load render mesh
-                    if (foundRenderMesh)
-                    {
-                        btSoftBodyHelpers::readRenderMeshFromObj(out_found_render_filename.c_str(), psb);
-                        btSoftBodyHelpers::interpolateBarycentricWeights(psb);
-                    }
-                }
-            
 
-            if (!foundRenderMesh)
+	    			    	for (int s = 0; s < (int)shapes.size(); s++)
-            {
+	    			    	{
-                psb->m_renderNodes.resize(0);
+	    			    		tinyobj::shape_t& shape = shapes[s];
-            }
+	    			    		int faceCount = shape.mesh.indices.size();
-            // This value should really be a exposed parameter 
+	    			    		int vertexCount = attribute.vertices.size()/3;
-            psb->setCollisionQuadrature(5);
+	    			    		for (int v=0;v<vertexCount;v++)
+	    			    		{
+	    			    			btSoftBody::Node n;
+	    			    			n.m_x = btVector3(attribute.vertices[3*v],attribute.vertices[3*v+1],attribute.vertices[3*v+2]);
+	    			    			psb->m_renderNodes.push_back(n);
+	    			    		}
+
+	    			    		for (int f = 0; f < faceCount; f += 3)
+	    			    		{
+	    			    			if (f < 0 && f >= int(shape.mesh.indices.size()))
+	    			    			{
+	    			    				continue;
+	    			    			}
+	    			    			tinyobj::index_t v_0 = shape.mesh.indices[f];
+	    			    			tinyobj::index_t v_1 = shape.mesh.indices[f + 1];
+	    			    			tinyobj::index_t v_2 = shape.mesh.indices[f + 2];
+	    			    			btSoftBody::Face ff;
+	    			    			ff.m_n[0] = &psb->m_renderNodes[v_0.vertex_index];
+	    			    			ff.m_n[1] = &psb->m_renderNodes[v_1.vertex_index];
+	    			    			ff.m_n[2] = &psb->m_renderNodes[v_2.vertex_index];
+	    			    			psb->m_renderFaces.push_back(ff);
+	    			    		}
+	    			    	}
+	    			    	
+	    			    }
+            			    btSoftBodyHelpers::interpolateBarycentricWeights(psb);
+            			}
+            			else
+            			{
+            			    psb->m_renderNodes.resize(0);
+            			}
             btVector3 gravity = m_data->m_dynamicsWorld->getGravity();
             btDeformableLagrangianForce* gravityForce = new btDeformableGravityForce(gravity);
             deformWorld->addForce(psb, gravityForce);
@@ -8563,10 +8591,21 @@ bool PhysicsServerCommandProcessor::processRequestCollisionInfoCommand(const str
 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
 	else if (body && body->m_softBody)
 	{
+		btSoftBody* sb = body->m_softBody;
+		serverCmd.m_sendCollisionInfoArgs.m_numLinks = 0;
+		btVector3 aabbMin, aabbMax;
+		sb->getAabb(aabbMin, aabbMax);
+
+		serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[0] = aabbMin[0];
+		serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[1] = aabbMin[1];
+		serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMin[2] = aabbMin[2];
+
+		serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[0] = aabbMax[0];
+		serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[1] = aabbMax[1];
+		serverCmd.m_sendCollisionInfoArgs.m_rootWorldAABBMax[2] = aabbMax[2];
+
 		SharedMemoryStatus& serverCmd = serverStatusOut;
 		serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_COMPLETED;
-		serverCmd.m_sendCollisionInfoArgs.m_numLinks = 0;
-		setDefaultRootWorldAABB(serverCmd);
 	}
 #endif
 	return hasStatus;
@@ -9441,6 +9480,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
 
 	if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_SPARSE_SDF)
 	{
+#ifndef SKIP_DEFORMABLE_BODY
 		{
             btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
             if (deformWorld)
@@ -9449,6 +9489,8 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
                 deformWorld ->getWorldInfo().m_sparsesdf.Reset();
             }
         }
+#endif
+#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
         {
             btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
             if (softWorld)
@@ -9457,6 +9499,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
                 softWorld->getWorldInfo().m_sparsesdf.Reset();
             }
         }
+#endif
 	}
 
 	if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD)
@@ -10647,6 +10690,84 @@ bool PhysicsServerCommandProcessor::processCreateUserConstraintCommand(const str
 	SharedMemoryStatus& serverCmd = serverStatusOut;
 	serverCmd.m_type = CMD_USER_CONSTRAINT_FAILED;
 	hasStatus = true;
+	if (clientCmd.m_updateFlags & USER_CONSTRAINT_ADD_SOFT_BODY_ANCHOR)
+	{
+#ifndef SKIP_DEFORMABLE_BODY
+		InternalBodyHandle* sbodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex);
+		if (sbodyHandle)
+		{
+			if (sbodyHandle->m_softBody)
+			{
+				int nodeIndex = clientCmd.m_userConstraintArguments.m_parentJointIndex;
+				if (nodeIndex>=0 && nodeIndex < sbodyHandle->m_softBody->m_nodes.size())
+				{
+					int bodyUniqueId = clientCmd.m_userConstraintArguments.m_childBodyIndex;
+					if (bodyUniqueId<=0)
+					{
+						//fixed anchor (mass = 0)
+						sbodyHandle->m_softBody->setMass(nodeIndex,0.0);
+						int uid = m_data->m_userConstraintUIDGenerator++;
+						serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
+						serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
+					} else
+					{
+						InternalBodyHandle* mbodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
+						if (mbodyHandle && mbodyHandle->m_multiBody)
+						{
+
+							btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
+							if (deformWorld)
+							{
+								int linkIndex = clientCmd.m_userConstraintArguments.m_childJointIndex;
+								if (linkIndex<0)
+								{
+									sbodyHandle->m_softBody->appendDeformableAnchor(nodeIndex, mbodyHandle->m_multiBody->getBaseCollider());
+								} else
+								{
+									if (linkIndex < mbodyHandle->m_multiBody->getNumLinks())
+									{
+										sbodyHandle->m_softBody->appendDeformableAnchor(nodeIndex, mbodyHandle->m_multiBody->getLinkCollider(linkIndex));
+									}
+								}
+							}
+						}
+						if (mbodyHandle && mbodyHandle->m_rigidBody)
+						{
+							btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
+							if (deformWorld)
+							{
+								//todo: expose those values
+								bool disableCollisionBetweenLinkedBodies = true;
+								//btVector3 localPivot(0,0,0);
+								sbodyHandle->m_softBody->appendDeformableAnchor(nodeIndex, mbodyHandle->m_rigidBody);
+							}
+
+#if 1
+							btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
+							if (softWorld)
+							{
+								bool disableCollisionBetweenLinkedBodies = true;
+								btVector3 localPivot(clientCmd.m_userConstraintArguments.m_childFrame[0],
+									clientCmd.m_userConstraintArguments.m_childFrame[1],
+									clientCmd.m_userConstraintArguments.m_childFrame[2]);
+								
+								sbodyHandle->m_softBody->appendAnchor(nodeIndex, mbodyHandle->m_rigidBody, localPivot, disableCollisionBetweenLinkedBodies);
+							}		
+#endif
+						}
+						int uid = m_data->m_userConstraintUIDGenerator++;
+						serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
+						serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
+
+					}
+
+				}
+				
+				
+			}
+		}
+#endif
+	}
 	if (clientCmd.m_updateFlags & USER_CONSTRAINT_REQUEST_STATE)
 	{
 		int constraintUid = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;

examples/SharedMemory/SharedMemoryCommands.h

@@ -505,7 +505,7 @@ enum EnumLoadSoftBodyUpdateFlags
         LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE = 1<<12,
         LOAD_SOFT_BODY_USE_SELF_COLLISION = 1<<13,
     LOAD_SOFT_BODY_USE_FACE_CONTACT = 1<<14,
-    LOAD_SOFT_BODY_RENDER_MESH = 1<<15,
+    LOAD_SOFT_BODY_SIM_MESH = 1<<15,
 };
 
 enum EnumSimParamInternalSimFlags
@@ -536,7 +536,7 @@ struct LoadSoftBodyArgs
         double m_NeoHookeanLambda;
         double m_NeoHookeanDamping;
     int m_useFaceContact;
-    char m_renderFileName[MAX_FILENAME_LENGTH];
+    char m_simFileName[MAX_FILENAME_LENGTH];
 };
 
 struct b3LoadSoftBodyResultArgs
@@ -802,6 +802,7 @@ enum EnumUserConstraintFlags
 	USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET = 512,
 	USER_CONSTRAINT_CHANGE_ERP = 1024,
 	USER_CONSTRAINT_REQUEST_STATE = 2048,
+	USER_CONSTRAINT_ADD_SOFT_BODY_ANCHOR = 4096,
 };
 
 enum EnumBodyChangeFlags

examples/pybullet/examples/deformable_anchor.py

@@ -0,0 +1,29 @@
+import pybullet as p
+from time import sleep
+
+physicsClient = p.connect(p.GUI)
+p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
+
+gravZ=-10
+p.setGravity(0, 0, gravZ)
+
+planeOrn = [0,0,0,1]#p.getQuaternionFromEuler([0.3,0,0])
+planeId = p.loadURDF("plane.urdf", [0,0,-2],planeOrn)
+
+boxId = p.loadURDF("cube.urdf", [0,1,2],useMaximalCoordinates = True)
+
+clothId = p.loadSoftBody("cloth_z_up.obj", basePosition = [0,0,2], scale = 0.5, mass = 1., useNeoHookean = 0, useBendingSprings=1,useMassSpring=1, springElasticStiffness=40, springDampingStiffness=.1, useSelfCollision = 0, frictionCoeff = .5, useFaceContact=1)
+
+
+p.createSoftBodyAnchor(clothId  ,0,-1,-1)
+p.createSoftBodyAnchor(clothId ,1,-1,-1)
+p.createSoftBodyAnchor(clothId ,3,boxId,-1, [0.5,-0.5,0])
+p.createSoftBodyAnchor(clothId ,2,boxId,-1, [-0.5,-0.5,0])
+p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
+p.setRealTimeSimulation(1)
+
+
+while p.isConnected():
+  p.setGravity(0,0,gravZ)
+  sleep(1./240.)
+  

examples/pybullet/examples/deformable_ball.py

@@ -24,4 +24,4 @@ while p.isConnected():
   sleep(1./240.)
   
 #p.resetSimulation()
-#p.stopStateLogging(logId)
+#p.stopStateLogging(logId)

examples/pybullet/examples/test_inertia.py

@@ -0,0 +1,28 @@
+import pybullet as p
+from time import sleep
+
+physicsClient = p.connect(p.GUI)
+
+useDeformable = True
+if useDeformable:
+	p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
+
+gravZ=-10
+p.setGravity(0, 0, gravZ)
+
+planeOrn = [0,0,0,1]#p.getQuaternionFromEuler([0.3,0,0])
+planeId = p.loadURDF("plane.urdf", [0,0,-2],planeOrn)
+
+boxId = p.loadURDF("cube.urdf", [0,1,2],useMaximalCoordinates = True)
+
+clothId = p.loadSoftBody("bunny.obj", basePosition = [0,0,2], scale = 0.5, mass = 1., useNeoHookean = 0, useBendingSprings=1, useMassSpring=1, springElasticStiffness=100, springDampingStiffness=.001, useSelfCollision = 0, frictionCoeff = .5, useFaceContact=1)
+
+p.setTimeStep(0.0005)
+
+p.setRealTimeSimulation(1)
+
+
+while p.isConnected():
+  p.setGravity(0,0,gravZ)
+  sleep(1./240.)
+  

examples/pybullet/pybullet.c

@@ -1963,6 +1963,10 @@ static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject* keyw
 }
 
 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
+
+
+
+
 // Load a softbody from an obj file
 static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* keywds)
 {
@@ -2072,6 +2076,56 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject*
 	}
 	return PyLong_FromLong(bodyUniqueId);
 }
+
+static PyObject* pybullet_createSoftBodyAnchor(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	b3SharedMemoryCommandHandle commandHandle;
+	int softBodyUniqueId = -1;
+	int nodeIndex  = -1;
+	int bodyUniqueId = -1;
+	int linkIndex = -1;
+	PyObject* bodyFramePositionObj = 0;
+	double bodyFramePosition[3] = {0, 0, 0};
+	struct b3JointInfo jointInfo;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	int physicsClientId = 0;
+	b3PhysicsClientHandle sm = 0;
+	static char* kwlist[] = {"softBodyBodyUniqueId", "nodeIndex",
+							 "bodyUniqueId", "linkIndex", "bodyFramePosition",
+							 "physicsClientId",
+							 NULL};
+
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|iiOi", kwlist, &softBodyUniqueId, &nodeIndex,
+									 &bodyUniqueId, &linkIndex,&bodyFramePositionObj,&physicsClientId))
+	{
+		return NULL;
+	}
+
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0)
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+
+	pybullet_internalSetVectord(bodyFramePositionObj, bodyFramePosition);
+
+	commandHandle = b3InitCreateSoftBodyAnchorConstraintCommand(sm, softBodyUniqueId, nodeIndex, bodyUniqueId, linkIndex, bodyFramePosition);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+	statusType = b3GetStatusType(statusHandle);
+	if (statusType == CMD_USER_CONSTRAINT_COMPLETED)
+	{
+		int userConstraintUid = b3GetStatusUserConstraintUniqueId(statusHandle);
+		PyObject* ob = PyLong_FromLong(userConstraintUid);
+		return ob;
+	}
+
+	PyErr_SetString(SpamError, "createSoftBodyAnchor failed.");
+	return NULL;
+}
+
+
 #endif
 
 // Reset the simulation to remove all loaded objects
@@ -11798,6 +11852,10 @@ static PyMethodDef SpamMethods[] = {
 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
 	{"loadSoftBody", (PyCFunction)pybullet_loadSoftBody, METH_VARARGS | METH_KEYWORDS,
 	 "Load a softbody from an obj file."},
+
+	{"createSoftBodyAnchor", (PyCFunction)pybullet_createSoftBodyAnchor, METH_VARARGS | METH_KEYWORDS,
+	 "Create an anchor (attachment) between a soft body and a rigid or multi body."},
+
 #endif
 	{"loadBullet", (PyCFunction)pybullet_loadBullet, METH_VARARGS | METH_KEYWORDS,
 	 "Load a world from a .bullet file."},

src/BulletSoftBody/btSoftBody.cpp

@@ -2567,7 +2567,8 @@ void btSoftBody::updateNormals()
 		btSoftBody::Face& f = m_faces[i];
 		const btVector3 n = btCross(f.m_n[1]->m_x - f.m_n[0]->m_x,
 									f.m_n[2]->m_x - f.m_n[0]->m_x);
-		f.m_normal = n.normalized();
+		f.m_normal = n;
+		f.m_normal.safeNormalize();
 		f.m_n[0]->m_n += n;
 		f.m_n[1]->m_n += n;
 		f.m_n[2]->m_n += n;
@@ -3377,7 +3378,10 @@ void btSoftBody::interpolateRenderMesh()
         n.m_x.setZero();
         for (int j = 0; j < 4; ++j)
         {
-            n.m_x += m_renderNodesParents[i][j]->m_x * m_renderNodesInterpolationWeights[i][j];
+			if (m_renderNodesParents[i].size())
+			{
+				n.m_x += m_renderNodesParents[i][j]->m_x * m_renderNodesInterpolationWeights[i][j];
+			}
         }
     }
 }
@@ -3966,7 +3970,7 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ
 			for (int j = 0; j < 4; j++)
 			{
 				m_tetras[i].m_c0[j].serializeFloat(memPtr->m_c0[j]);
-				memPtr->m_nodeIndices[j] = m_tetras[j].m_n[j] ? m_tetras[j].m_n[j] - &m_nodes[0] : -1;
+				memPtr->m_nodeIndices[j] = m_tetras[i].m_n[j] ? m_tetras[i].m_n[j] - &m_nodes[0] : -1;
 			}
 			memPtr->m_c1 = m_tetras[i].m_c1;
 			memPtr->m_c2 = m_tetras[i].m_c2;

src/BulletSoftBody/btSoftBodyHelpers.cpp

@@ -1500,44 +1500,6 @@ void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector
     bary = btVector4(va6*v6, vb6*v6, vc6*v6, vd6*v6);
 }
 
-void btSoftBodyHelpers::readRenderMeshFromObj(const char* file, btSoftBody* psb)
-{
-    std::ifstream fs;
-    fs.open(file);
-    std::string line;
-    btVector3 pos;
-    while (std::getline(fs, line))
-    {
-        std::stringstream ss(line);
-		if (line.length()>1)
-		{
-			if (line[0] == 'v' && line[1] != 't' && line[1] != 'n')
-			{
-				ss.ignore();
-				for (size_t i = 0; i < 3; i++)
-					ss >> pos[i];
-				btSoftBody::Node n;
-				n.m_x = pos;
-				psb->m_renderNodes.push_back(n);
-			}
-			else if (line[0] == 'f')
-			{
-				ss.ignore();
-				int id0, id1, id2;
-				ss >> id0;
-				ss >> id1;
-				ss >> id2;
-				btSoftBody::Face f;
-				f.m_n[0] = &psb->m_renderNodes[id0-1];
-				f.m_n[1] = &psb->m_renderNodes[id1-1];
-				f.m_n[2] = &psb->m_renderNodes[id2-1];
-				psb->m_renderFaces.push_back(f);
-			}
-		}
-    }
-    fs.close();
-}
-
 // Iterate through all render nodes to find the simulation tetrahedron that contains the render node and record the barycentric weights
 // If the node is not inside any tetrahedron, assign it to the tetrahedron in which the node has the least negative barycentric weight
 void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb)

src/BulletSoftBody/btSoftBodyHelpers.h

@@ -148,8 +148,6 @@ struct btSoftBodyHelpers
     
     static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary);
     
-    static void readRenderMeshFromObj(const char* file, btSoftBody* psb);
-
     static void interpolateBarycentricWeights(btSoftBody* psb);
     
     static void generateBoundaryFaces(btSoftBody* psb);

src/BulletSoftBody/btSparseSDF.h

@@ -20,27 +20,38 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
 
-// Modified Paul Hsieh hash
+// Fast Hash
-template <const int DWORDLEN>
+
-unsigned int HsiehHash(const void* pdata)
+#if !defined (get16bits)
-{
+#define get16bits(d) ((((unsigned int)(((const unsigned char *)(d))[1])) << 8)\
-	const unsigned short* data = (const unsigned short*)pdata;
++(unsigned int)(((const unsigned char *)(d))[0]) )
-	unsigned hash = DWORDLEN << 2, tmp;
+#endif
-	for (int i = 0; i < DWORDLEN; ++i)
+//
-	{
+// super hash function by Paul Hsieh
-		hash += data[0];
+//
-		tmp = (data[1] << 11) ^ hash;
+inline unsigned int HsiehHash (const char * data, int len) {
-		hash = (hash << 16) ^ tmp;
+  unsigned int hash = len, tmp;
-		data += 2;
+  len>>=2;
-		hash += hash >> 11;
+
-	}
+    /* Main loop */
-	hash ^= hash << 3;
+    for (;len > 0; len--) {
-	hash += hash >> 5;
+        hash  += get16bits (data);
-	hash ^= hash << 4;
+        tmp    = (get16bits (data+2) << 11) ^ hash;
-	hash += hash >> 17;
+        hash   = (hash << 16) ^ tmp;
-	hash ^= hash << 25;
+        data  += 2*sizeof (unsigned short);
-	hash += hash >> 6;
+        hash  += hash >> 11;
-	return (hash);
+    }
+
+
+    /* Force "avalanching" of final 127 bits */
+    hash ^= hash << 3;
+    hash += hash >> 5;
+    hash ^= hash << 4;
+    hash += hash >> 17;
+    hash ^= hash << 25;
+    hash += hash >> 6;
+
+    return hash;
 }
 
 template <const int CELLSIZE>
@@ -209,6 +220,9 @@ struct btSparseSdf
 			}
 			else
 			{
+				// printf("c->hash/c[0][1][2]=%d,%d,%d,%d\n", c->hash, c->c[0], c->c[1],c->c[2]);
+                        //printf("h,ixb,iyb,izb=%d,%d,%d,%d\n", h,ix.b, iy.b, iz.b);
+
 				c = c->next;
 			}
 		}
@@ -260,7 +274,7 @@ struct btSparseSdf
 						 Lerp(gy[2], gy[3], ix.f), iz.f));
 		normal.setZ(Lerp(Lerp(gz[0], gz[1], ix.f),
 						 Lerp(gz[2], gz[3], ix.f), iy.f));
-		normal = normal.normalized();
+		normal.safeNormalize();
 #else
 		normal = btVector3(d[1] - d[0], d[3] - d[0], d[4] - d[0]).normalized();
 #endif
@@ -340,16 +354,16 @@ struct btSparseSdf
 
 		btS myset;
 		//memset may be needed in case of additional (uninitialized) padding!
-		//memset(myset, 0, sizeof(btS));
+		//memset(&myset, 0, sizeof(btS));
 
 		myset.x = x;
 		myset.y = y;
 		myset.z = z;
 		myset.w = 0;
 		myset.p = (void*)shape;
-		const void* ptr = &myset;
+		const char* ptr = (const char*)&myset;
 
-		unsigned int result = HsiehHash<sizeof(btS) / 4>(ptr);
+		unsigned int result = HsiehHash(ptr, sizeof(btS) );
 
 		return result;
 	}