Merge pull request #2495 from erwincoumans/master

various deformable fixes and reimplement #2494

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,7 +357,8 @@ 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;