fix memory issues in btSparseSDF.h

(hash function on structure with uninitialized padding, and  Reset not called in destructor)
expose sparseSdfVoxelSize in PyBullet.setPhysicsEngineParameter
don't call deformable wireframe drawing in the wrong thread/place (it can cause crashes)

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -895,6 +895,15 @@ B3_SHARED_API int b3PhysicsParamSetDefaultFrictionCFM(b3SharedMemoryCommandHandl
 	return 0;
 }
 
+B3_SHARED_API int b3PhysicsParameterSetSparseSdfVoxelSize(b3SharedMemoryCommandHandle commandHandle, double sparseSdfVoxelSize)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+	b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
+	command->m_updateFlags |= SIM_PARAM_UPDATE_SPARSE_SDF;
+	command->m_physSimParamArgs.m_sparseSdfVoxelSize = sparseSdfVoxelSize;
+	return 0;
+}
+
 B3_SHARED_API b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient)
 {
 	PhysicsClient* cl = (PhysicsClient*)physClient;

examples/SharedMemory/PhysicsClientC_API.h

@@ -357,6 +357,7 @@ extern "C"
 	B3_SHARED_API int b3PhysicsParameterSetConstraintSolverType(b3SharedMemoryCommandHandle commandHandle, int constraintSolverType);
 	B3_SHARED_API int b3PhysicsParameterSetMinimumSolverIslandSize(b3SharedMemoryCommandHandle commandHandle, int minimumSolverIslandSize);
 	B3_SHARED_API int b3PhysicsParamSetSolverAnalytics(b3SharedMemoryCommandHandle commandHandle, int reportSolverAnalytics);
+	B3_SHARED_API int b3PhysicsParameterSetSparseSdfVoxelSize(b3SharedMemoryCommandHandle commandHandle, double sparseSdfVoxelSize);
 	                  
 	B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestPhysicsParamCommand(b3PhysicsClientHandle physClient);
 	B3_SHARED_API int b3GetStatusPhysicsSimulationParameters(b3SharedMemoryStatusHandle statusHandle, struct b3PhysicsSimulationParameters* params);

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -2605,7 +2605,7 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
 btDeformableMultiBodyDynamicsWorld* PhysicsServerCommandProcessor::getDeformableWorld()
 {
 	btDeformableMultiBodyDynamicsWorld* world = 0;
-	if (m_data->m_dynamicsWorld->getWorldType()== BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD)
+	if (m_data->m_dynamicsWorld && m_data->m_dynamicsWorld->getWorldType()== BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD)
 	{
 		world = (btDeformableMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
 	}
@@ -2615,7 +2615,7 @@ btDeformableMultiBodyDynamicsWorld* PhysicsServerCommandProcessor::getDeformable
 btSoftMultiBodyDynamicsWorld* PhysicsServerCommandProcessor::getSoftWorld()
 {
 	btSoftMultiBodyDynamicsWorld* world = 0;
-	if (m_data->m_dynamicsWorld->getWorldType()== BT_SOFT_MULTIBODY_DYNAMICS_WORLD)
+	if (m_data->m_dynamicsWorld && m_data->m_dynamicsWorld->getWorldType()== BT_SOFT_MULTIBODY_DYNAMICS_WORLD)
 	{
 		world = (btSoftMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
 	}
@@ -9435,6 +9435,26 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
 		m_data->m_dynamicsWorld->getSolverInfo().m_frictionCFM = clientCmd.m_physSimParamArgs.m_frictionCFM;
 	}
 
+	if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_SPARSE_SDF)
+	{
+		{
+            btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
+            if (deformWorld)
+            {
+                deformWorld ->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(clientCmd.m_physSimParamArgs.m_sparseSdfVoxelSize);
+                deformWorld ->getWorldInfo().m_sparsesdf.Reset();
+            }
+        }
+        {
+            btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
+            if (softWorld)
+            {
+                softWorld->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(clientCmd.m_physSimParamArgs.m_sparseSdfVoxelSize);
+                softWorld->getWorldInfo().m_sparsesdf.Reset();
+            }
+        }
+	}
+
 	if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD)
 	{
 		m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold = clientCmd.m_physSimParamArgs.m_restitutionVelocityThreshold;
@@ -12964,30 +12984,6 @@ void PhysicsServerCommandProcessor::renderScene(int renderFlags)
 		}
 
 		m_data->m_guiHelper->render(m_data->m_dynamicsWorld);
-#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
-#ifndef SKIP_DEFORMABLE_BODY
-		if (m_data->m_dynamicsWorld)
-		{
-			btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
-			if (deformWorld)
-			{
-				if (deformWorld->getSoftBodyArray().size())
-				{
-					for (int i = 0; i < deformWorld->getSoftBodyArray().size(); i++)
-					{
-						btSoftBody* psb = (btSoftBody*)deformWorld->getSoftBodyArray()[i];
-						{
-							btSoftBodyHelpers::DrawFrame(psb, m_data->m_dynamicsWorld->getDebugDrawer());
-							btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(), deformWorld->getDrawFlags());
-						}
-					}
-					m_data->m_guiHelper->drawDebugDrawerLines();
-					m_data->m_guiHelper->clearLines();
-				}
-			}
-		}
-#endif
-#endif
 	}
 }
 

examples/SharedMemory/SharedMemoryCommands.h

@@ -484,6 +484,7 @@ enum EnumSimParamUpdateFlags
 	SIM_PARAM_REPORT_CONSTRAINT_SOLVER_ANALYTICS = 1 << 26,
 	SIM_PARAM_UPDATE_WARM_STARTING_FACTOR = 1 << 27,
 	SIM_PARAM_UPDATE_ARTICULATED_WARM_STARTING_FACTOR = 1 << 28,
+	SIM_PARAM_UPDATE_SPARSE_SDF = 1 << 29,
         
 };
 

examples/SharedMemory/SharedMemoryPublic.h

@@ -7,7 +7,9 @@
 //Please don't replace an existing magic number:
 //instead, only ADD a new one at the top, comment-out previous one
 
-#define SHARED_MEMORY_MAGIC_NUMBER 201909030
+
+#define SHARED_MEMORY_MAGIC_NUMBER 201911180
+//#define SHARED_MEMORY_MAGIC_NUMBER 201909030
 //#define SHARED_MEMORY_MAGIC_NUMBER 201908110
 //#define SHARED_MEMORY_MAGIC_NUMBER 201908050
 //#define SHARED_MEMORY_MAGIC_NUMBER 2019060190
@@ -978,6 +980,7 @@ struct b3PhysicsSimulationParameters
 	int m_constraintSolverType;
 	int m_minimumSolverIslandSize;
 	int m_reportSolverAnalytics;
+	double m_sparseSdfVoxelSize;
 };
 
 

examples/pybullet/examples/deformable_ball.py

@@ -0,0 +1,27 @@
+import pybullet as p
+from time import sleep
+
+physicsClient = p.connect(p.GUI)
+
+p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD)
+
+p.setGravity(0, 0, -10)
+
+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,3,2],useMaximalCoordinates = True)
+
+ballId = p.loadSoftBody("ball.vtk", basePosition = [0,0,-1], scale = 0.5, mass = 0.1, useNeoHookean = 1, NeoHookeanMu = 20, NeoHookeanLambda = 20, NeoHookeanDamping = 0.001, useSelfCollision = 1, frictionCoeff = .5)
+p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
+p.setRealTimeSimulation(1)
+
+#logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "perf.json")
+
+while p.isConnected():
+  
+  p.setGravity(0,0,-10)
+  sleep(1./240.)
+  
+#p.resetSimulation()
+#p.stopStateLogging(logId)

examples/pybullet/examples/deformable_torus.py

@@ -12,6 +12,7 @@ planeId = p.loadURDF("plane.urdf", [0,0,-2])
 boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True)
 
 bunnyId = p.loadSoftBody("torus.vtk", useNeoHookean = 1, NeoHookeanMu = 60, NeoHookeanLambda = 200, NeoHookeanDamping = 0.01, useSelfCollision = 1, frictionCoeff = 0.5)
+p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
 p.setRealTimeSimulation(1)
 
 while p.isConnected():

examples/pybullet/pybullet.c

@@ -1583,6 +1583,7 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
 	int reportSolverAnalytics = -1;
 
 	double warmStartingFactor = -1;
+	double sparseSdfVoxelSize = -1;
 
 	int physicsClientId = 0;
 
@@ -1611,11 +1612,12 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
 							 "minimumSolverIslandSize",
 							 "reportSolverAnalytics",
 							 "warmStartingFactor",
+							 "sparseSdfVoxelSize",
 							 "physicsClientId", NULL};
 
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiidiidi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiidiiddi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
 									 &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &constraintSolverType, &globalCFM, &minimumSolverIslandSize, 
-									&reportSolverAnalytics, &warmStartingFactor, &physicsClientId))
+									&reportSolverAnalytics, &warmStartingFactor, &sparseSdfVoxelSize, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -1740,6 +1742,10 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
 		{
 			b3PhysicsParamSetWarmStartingFactor(command, warmStartingFactor);
 		}
+		if (sparseSdfVoxelSize >= 0)
+		{
+			b3PhysicsParameterSetSparseSdfVoxelSize(command, sparseSdfVoxelSize);
+		}
 		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
 	}
 

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp

@@ -382,6 +382,8 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
 void btDeformableMultiBodyDynamicsWorld::debugDrawWorld()
 {
 
+	btMultiBodyDynamicsWorld::debugDrawWorld();
+
 	for (int i = 0; i < getSoftBodyArray().size(); i++)
 	{
 		btSoftBody* psb = (btSoftBody*)getSoftBodyArray()[i];
@@ -391,7 +393,7 @@ void btDeformableMultiBodyDynamicsWorld::debugDrawWorld()
 		}
 	}
 
-	btMultiBodyDynamicsWorld::debugDrawWorld();
+	
 }
 
 void btDeformableMultiBodyDynamicsWorld::applyRigidBodyGravity(btScalar timeStep)

src/BulletSoftBody/btSparseSDF.h

@@ -77,6 +77,10 @@ struct btSparseSdf
 	int nprobes;
 	int nqueries;
 
+	~btSparseSdf()
+	{
+		Reset();
+	}
 	//
 	// Methods
 	//
@@ -330,15 +334,18 @@ struct btSparseSdf
 	{
 		struct btS
 		{
-			int x, y, z;
+			int x, y, z, w;
 			void* p;
 		};
 
 		btS myset;
+		//memset may be needed in case of additional (uninitialized) padding!
+		//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;