diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 0f6f255fb..0284d1d1b 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/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; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index d2037d02e..ff59cef46 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/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); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index ac9db5e10..c1aca27c7 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/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 } } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 4d4324013..ec79d77c5 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/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, }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 2e3d9ae26..feed391a8 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/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; }; diff --git a/examples/pybullet/examples/deformable_ball.py b/examples/pybullet/examples/deformable_ball.py new file mode 100644 index 000000000..8803e2fb2 --- /dev/null +++ b/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) \ No newline at end of file diff --git a/examples/pybullet/examples/deformable_torus.py b/examples/pybullet/examples/deformable_torus.py index ee5c78772..ff02237d6 100644 --- a/examples/pybullet/examples/deformable_torus.py +++ b/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(): diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 0b1ef28cf..260f6db68 100644 --- a/examples/pybullet/pybullet.c +++ b/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); } diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index f16786e16..25ee65450 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/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) diff --git a/src/BulletSoftBody/btSparseSDF.h b/src/BulletSoftBody/btSparseSDF.h index 8258c3eeb..37e657d62 100644 --- a/src/BulletSoftBody/btSparseSDF.h +++ b/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;