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)
This commit is contained in:
Erwin Coumans
2019-11-18 10:22:56 -08:00
committed by Xuchen Han
parent 4ee788e2af
commit e5ed15c3b2
10 changed files with 85 additions and 32 deletions

View File

@@ -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;

View File

@@ -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);

View File

@@ -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;
}
@@ -9437,6 +9437,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;
@@ -12966,30 +12986,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
}
}

View File

@@ -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,
};

View File

@@ -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;
};