Merge pull request #2499 from xhan0619/fix-render-mesh

Fix render mesh

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -338,6 +338,24 @@ B3_SHARED_API int b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle
 	return 0;
 }
 
+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_simFileName, filename);
+    }
+    else
+    {
+        command->m_loadSoftBodyArguments.m_simFileName[0] = 0;
+    }
+    command->m_updateFlags |= LOAD_SOFT_BODY_SIM_MESH;
+    return 0;
+
+}
+
 B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;

examples/SharedMemory/PhysicsClientC_API.h

@@ -375,7 +375,6 @@ extern "C"
 	B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient);
 	B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand2(b3SharedMemoryCommandHandle commandHandle);
 	B3_SHARED_API int b3InitResetSimulationSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
-
 	///Load a robot from a URDF file. Status type will CMD_URDF_LOADING_COMPLETED.
 	///Access the robot from the unique body index, through b3GetStatusBodyIndex(statusHandle);
 	B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName);
@@ -634,7 +633,9 @@ 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 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);
 	B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness);
 	B3_SHARED_API int b3LoadSoftBodyAddGravityForce(b3SharedMemoryCommandHandle commandHandle, double gravityX, double gravityY, double gravityZ);

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -1709,7 +1709,6 @@ struct PhysicsServerCommandProcessorInternalData
 		  m_dispatcher(0),
 		  m_solver(0),
 		  m_collisionConfiguration(0),
-		  
 #ifndef SKIP_DEFORMABLE_BODY
 		  m_deformablebodySolver(0),
 #endif
@@ -2652,22 +2651,24 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld(int flags)
 		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
 	}
 
@@ -5126,24 +5127,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;
@@ -8083,27 +8084,21 @@ 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;
-	std::string out_found_sim_filename;
 	int out_type(0), out_sim_type(0);
 
-        bool render_mesh_is_sim_mesh = true;
-
 	bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
-        if (out_type == UrdfGeometry::FILE_OBJ)
-        {
-		    foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_sim_filename, &out_sim_type);
-            render_mesh_is_sim_mesh = !foundFile;
-        }
 
-        if (render_mesh_is_sim_mesh)
+        if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SIM_MESH)
         {
-            out_sim_type = out_type;
+            bool foundSimMesh = UrdfFindMeshFile(fileIO, pathPrefix, loadSoftBodyArgs.m_simFileName, error_message_prefix, &out_found_sim_filename, &out_sim_type);
-            out_found_sim_filename = out_found_filename;
+        }
+        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;
@@ -8192,6 +8187,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
 					deformWorld->addForce(psb, springForce);
 					m_data->m_lf.push_back(springForce);
 				}
+                
 			}
 #endif
 		}
@@ -8199,58 +8195,55 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
 		if (psb != NULL)
 		{
 #ifndef SKIP_DEFORMABLE_BODY
-			btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
+		  	btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
 			if (deformWorld)
 			{
-            if (!render_mesh_is_sim_mesh)
+            			// load render mesh
-            {
+            			if (out_found_sim_filename != out_found_filename)
-                // load render mesh
+            			{
+            			    // load render mesh
+	    			    {
+	    			    	tinyobj::attrib_t attribute;
+	    			    	std::vector<tinyobj::shape_t> shapes;
 	    			    	
+	    			    	std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), pathPrefix, m_data->m_pluginManager.getFileIOInterface());
 
-				{
+	    			    	for (int s = 0; s < (int)shapes.size(); s++)
+	    			    	{
+	    			    		tinyobj::shape_t& shape = shapes[s];
+	    			    		int faceCount = shape.mesh.indices.size();
+	    			    		int vertexCount = attribute.vertices.size()/3;
+	    			    		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);
+	    			    		}
 
-					tinyobj::attrib_t attribute;
+	    			    		for (int f = 0; f < faceCount; f += 3)
-					std::vector<tinyobj::shape_t> shapes;
+	    			    		{
+	    			    			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);
+	    			    		}
+	    			    	}
 	    			    	
-					std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), pathPrefix, m_data->m_pluginManager.getFileIOInterface());
+	    			    }
-
+            			    btSoftBodyHelpers::interpolateBarycentricWeights(psb);
-					for (int s = 0; s < (int)shapes.size(); s++)
+            			}
-					{
+            			else
-						tinyobj::shape_t& shape = shapes[s];
+            			{
-						int faceCount = shape.mesh.indices.size();
+            			    psb->m_renderNodes.resize(0);
-						int vertexCount = attribute.vertices.size()/3;
+            			}
-						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);

examples/SharedMemory/SharedMemoryCommands.h

@@ -505,6 +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_SIM_MESH = 1<<15,
 };
 
 enum EnumSimParamInternalSimFlags
@@ -535,6 +536,7 @@ struct LoadSoftBodyArgs
         double m_NeoHookeanLambda;
         double m_NeoHookeanDamping;
     int m_useFaceContact;
+    char m_simFileName[MAX_FILENAME_LENGTH];
 };
 
 struct b3LoadSoftBodyResultArgs

examples/pybullet/examples/deformable_anchor.py

@@ -2,8 +2,8 @@ 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)
 

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.)
+