diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 9677197ca..5a5d3ad95 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -923,6 +923,19 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand2(b3Shared return (b3SharedMemoryCommandHandle)command; } +B3_SHARED_API int b3InitResetSimulationSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command); + btAssert(command->m_type == CMD_RESET_SIMULATION); + if (command->m_type == CMD_RESET_SIMULATION) + { + command->m_updateFlags = flags; + } + return 0; +} + + B3_SHARED_API b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode) { return b3JointControlCommandInit2(physClient, 0, controlMode); diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 4e0e0eb63..49e5848f2 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -373,6 +373,7 @@ 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); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 96766b861..c2f73668d 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -92,6 +92,11 @@ #include "../TinyAudio/b3SoundEngine.h" #endif + +#ifdef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD +#define SKIP_DEFORMABLE_BODY 1 +#endif + #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD #include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" #include "BulletSoftBody/btSoftBodySolvers.h" @@ -101,11 +106,17 @@ #include "BulletSoftBody/btDeformableBodySolver.h" #include "BulletSoftBody/btDeformableMultiBodyConstraintSolver.h" #include "../SoftDemo/BunnyMesh.h" -#else -#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" -#endif +#endif//SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + +#ifndef SKIP_DEFORMABLE_BODY +#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h" +#include "BulletSoftBody/btDeformableBodySolver.h" +#include "BulletSoftBody/btDeformableMultiBodyConstraintSolver.h" +#endif//SKIP_DEFORMABLE_BODY + +#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" + -#define SKIP_DEFORMABLE_BODY 1 int gInternalSimFlags = 0; bool gResetSimulation = 0; @@ -1619,26 +1630,24 @@ struct PhysicsServerCommandProcessorInternalData btHashedOverlappingPairCache* m_pairCache; btBroadphaseInterface* m_broadphase; btCollisionDispatcher* m_dispatcher; -#if defined(SKIP_DEFORMABLE_BODY) || defined(SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD) + btMultiBodyConstraintSolver* m_solver; -#else - btDeformableMultiBodyConstraintSolver* m_solver; -#endif btDefaultCollisionConfiguration* m_collisionConfiguration; -#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD -#ifndef SKIP_DEFORMABLE_BODY - btDeformableMultiBodyDynamicsWorld* m_dynamicsWorld; + +//#ifndef SKIP_DEFORMABLE_BODY + btDeformableBodySolver* m_deformablebodySolver; btAlignedObjectArray m_lf; -#else - btSoftMultiBodyDynamicsWorld* m_dynamicsWorld; - btSoftBodySolver* m_softbodySolver; -#endif -#else +//#else//SKIP_DEFORMABLE_BODY +//#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + + +//#else//SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD btMultiBodyDynamicsWorld* m_dynamicsWorld; -#endif +//#endif//SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD +//#endif//SKIP_DEFORMABLE_BODY int m_constraintSolverType; SharedMemoryDebugDrawer* m_remoteDebugDrawer; @@ -1705,6 +1714,7 @@ struct PhysicsServerCommandProcessorInternalData m_solver(0), m_collisionConfiguration(0), m_dynamicsWorld(0), + m_deformablebodySolver(0), m_constraintSolverType(-1), m_remoteDebugDrawer(0), m_stateLoggersUniqueId(0), @@ -2592,7 +2602,27 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface } }; -void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() +btDeformableMultiBodyDynamicsWorld* PhysicsServerCommandProcessor::getDeformableWorld() +{ + btDeformableMultiBodyDynamicsWorld* world = 0; + if (m_data->m_dynamicsWorld->getWorldType()== BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD) + { + world = (btDeformableMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; + } + return world; +} + +btSoftMultiBodyDynamicsWorld* PhysicsServerCommandProcessor::getSoftWorld() +{ + btSoftMultiBodyDynamicsWorld* world = 0; + if (m_data->m_dynamicsWorld->getWorldType()== BT_SOFT_MULTIBODY_DYNAMICS_WORLD) + { + world = (btSoftMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; + } + return world; +} + +void PhysicsServerCommandProcessor::createEmptyDynamicsWorld(int flags) { m_data->m_constraintSolverType = eConstraintSolverLCP_SI; ///collision configuration contains default setup for memory, collision setup @@ -2613,28 +2643,41 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() m_data->m_pairCache->setOverlapFilterCallback(m_data->m_broadphaseCollisionFilterCallback); //int maxProxies = 32768; - //m_data->m_broadphase = new btSimpleBroadphase(maxProxies, m_data->m_pairCache); - btDbvtBroadphase* bv = new btDbvtBroadphase(m_data->m_pairCache); - bv->setVelocityPrediction(0); - m_data->m_broadphase = bv; - -#ifdef SKIP_DEFORMABLE_BODY - m_data->m_solver = new btMultiBodyConstraintSolver; -#endif - -#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + if (flags&RESET_USE_SIMPLE_BROADPHASE) + { + m_data->m_broadphase = new btSimpleBroadphase(65536, m_data->m_pairCache); + } else + { + btDbvtBroadphase* bv = new btDbvtBroadphase(m_data->m_pairCache); + bv->setVelocityPrediction(0); + m_data->m_broadphase = bv; + } + + if (flags & RESET_USE_DEFORMABLE_WORLD) + { #ifndef SKIP_DEFORMABLE_BODY - m_data->m_deformablebodySolver = new btDeformableBodySolver(); - m_data->m_solver = new btDeformableMultiBodyConstraintSolver; - m_data->m_solver->setDeformableSolver(m_data->m_deformablebodySolver); - m_data->m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration, m_data->m_deformablebodySolver); -#else - m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); -#endif -#else - m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); + m_data->m_deformablebodySolver = new btDeformableBodySolver(); + btDeformableMultiBodyConstraintSolver* solver = new btDeformableMultiBodyConstraintSolver; + m_data->m_solver = solver; + 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); #endif + } + if ((0==m_data->m_dynamicsWorld) && (0==(flags&RESET_USE_DISCRETE_DYNAMICS_WORLD))) + { +#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); +#endif + } + + if (0==m_data->m_dynamicsWorld) + { + m_data->m_solver = new btMultiBodyConstraintSolver; + m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); + } + //Workaround: in a VR application, where we avoid synchronizing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(128 * 1024); @@ -2800,13 +2843,35 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() m_data->m_lf.clear(); #endif #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD - for (i = m_data->m_dynamicsWorld->getSoftBodyArray().size() - 1; i >= 0; i--) { - btSoftBody* sb = m_data->m_dynamicsWorld->getSoftBodyArray()[i]; - m_data->m_dynamicsWorld->removeSoftBody(sb); - delete sb; + btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); + if (softWorld) + { + for (i =softWorld->getSoftBodyArray().size() - 1; i >= 0; i--) + { + btSoftBody* sb =softWorld->getSoftBodyArray()[i]; + softWorld->removeSoftBody(sb); + delete sb; + } + } + } +#endif//SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + +#ifndef SKIP_DEFORMABLE_BODY + { + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld) + { + for (i =deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--) + { + btSoftBody* sb =deformWorld->getSoftBodyArray()[i]; + deformWorld->removeSoftBody(sb); + delete sb; + } + } } #endif + } for (int i = 0; i < constraints.size(); i++) @@ -8019,12 +8084,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024); } - // add _sim.vtk postfix - char relativeSimFileName[1024]; - char sim_file_ending[9] = "_sim.vtk"; - strncpy(relativeSimFileName, relativeFileName, strlen(relativeFileName)); - strncpy(relativeSimFileName + strlen(relativeFileName)-4, sim_file_ending, sizeof(sim_file_ending)); - + const std::string& error_message_prefix = ""; std::string out_found_filename; std::string out_found_sim_filename; @@ -8035,7 +8095,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); if (out_type == UrdfGeometry::FILE_OBJ) { - bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeSimFileName, error_message_prefix, &out_found_sim_filename, &out_sim_type); + foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_sim_filename, &out_sim_type); render_mesh_is_sim_mesh = !foundFile; } @@ -8066,59 +8126,83 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar int numTris = shape.mesh.indices.size() / 3; if (numTris > 0) { - psb = btSoftBodyHelpers::CreateFromTriMesh(m_data->m_dynamicsWorld->getWorldInfo(), &vertices[0], &indices[0], numTris); + { + btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); + if (softWorld) + { + psb = btSoftBodyHelpers::CreateFromTriMesh(softWorld->getWorldInfo(), &vertices[0], &indices[0], numTris); + } + } + { + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld) + { + psb = btSoftBodyHelpers::CreateFromTriMesh(deformWorld->getWorldInfo(), &vertices[0], &indices[0], numTris); + } + } } } #ifndef SKIP_DEFORMABLE_BODY btScalar spring_elastic_stiffness, spring_damping_stiffness; if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE) { - spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness; - spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness; - btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, false); - m_data->m_dynamicsWorld->addForce(psb, springForce); - m_data->m_lf.push_back(springForce); + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld) + { + spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness; + spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness; + btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, false); + deformWorld->addForce(psb, springForce); + m_data->m_lf.push_back(springForce); + } } #endif } else if (out_sim_type == UrdfGeometry::FILE_VTK) { #ifndef SKIP_DEFORMABLE_BODY - psb = btSoftBodyHelpers::CreateFromVtkFile(m_data->m_dynamicsWorld->getWorldInfo(), out_found_sim_filename.c_str()); - btScalar corotated_mu, corotated_lambda; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE) - { - corotated_mu = clientCmd.m_loadSoftBodyArguments.m_corotatedMu; - corotated_lambda = clientCmd.m_loadSoftBodyArguments.m_corotatedLambda; - btDeformableLagrangianForce* corotatedForce = new btDeformableCorotatedForce(corotated_mu, corotated_lambda); - m_data->m_dynamicsWorld->addForce(psb, corotatedForce); - m_data->m_lf.push_back(corotatedForce); - } - btScalar neohookean_mu, neohookean_lambda, neohookean_damping; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE) - { - neohookean_mu = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanMu; - neohookean_lambda = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanLambda; - neohookean_damping = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanDamping; - btDeformableLagrangianForce* neohookeanForce = new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda, neohookean_damping); - m_data->m_dynamicsWorld->addForce(psb, neohookeanForce); - m_data->m_lf.push_back(neohookeanForce); - } - btScalar spring_elastic_stiffness, spring_damping_stiffness; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE) - { - spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness; - spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness; - btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true); - m_data->m_dynamicsWorld->addForce(psb, springForce); - m_data->m_lf.push_back(springForce); - } + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld) + { + psb = btSoftBodyHelpers::CreateFromVtkFile(deformWorld->getWorldInfo(), out_found_sim_filename.c_str()); + btScalar corotated_mu, corotated_lambda; + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE) + { + corotated_mu = clientCmd.m_loadSoftBodyArguments.m_corotatedMu; + corotated_lambda = clientCmd.m_loadSoftBodyArguments.m_corotatedLambda; + btDeformableLagrangianForce* corotatedForce = new btDeformableCorotatedForce(corotated_mu, corotated_lambda); + deformWorld->addForce(psb, corotatedForce); + m_data->m_lf.push_back(corotatedForce); + } + btScalar neohookean_mu, neohookean_lambda, neohookean_damping; + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE) + { + neohookean_mu = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanMu; + neohookean_lambda = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanLambda; + neohookean_damping = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanDamping; + btDeformableLagrangianForce* neohookeanForce = new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda, neohookean_damping); + deformWorld->addForce(psb, neohookeanForce); + m_data->m_lf.push_back(neohookeanForce); + } + btScalar spring_elastic_stiffness, spring_damping_stiffness; + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE) + { + spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness; + spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness; + btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true); + deformWorld->addForce(psb, springForce); + m_data->m_lf.push_back(springForce); + } + } #endif } if (psb != NULL) { #ifndef SKIP_DEFORMABLE_BODY + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld) + { if (!render_mesh_is_sim_mesh) { // load render mesh @@ -8133,7 +8217,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar psb->setCollisionQuadrature(5); btVector3 gravity = m_data->m_dynamicsWorld->getGravity(); btDeformableLagrangianForce* gravityForce = new btDeformableGravityForce(gravity); - m_data->m_dynamicsWorld->addForce(psb, gravityForce); + deformWorld->addForce(psb, gravityForce); m_data->m_lf.push_back(gravityForce); btScalar collision_hardness = 1; psb->m_cfg.kKHR = collision_hardness; @@ -8171,25 +8255,44 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar use_self_collision = loadSoftBodyArgs.m_useSelfCollision; } psb->setSelfCollision(use_self_collision); -#else - btSoftBody::Material* pm = psb->appendMaterial(); - pm->m_kLST = 0.5; - pm->m_flags -= btSoftBody::fMaterial::DebugDraw; - psb->generateBendingConstraints(2, pm); - psb->m_cfg.piterations = 20; - psb->m_cfg.kDF = 0.5; - //turn on softbody vs softbody collision - psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS; - psb->randomizeConstraints(); - psb->setTotalMass(mass, true); -#endif + } +#endif//SKIP_DEFORMABLE_BODY +#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + + btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); + if (softWorld) + { + btSoftBody::Material* pm = psb->appendMaterial(); + pm->m_kLST = 0.5; + pm->m_flags -= btSoftBody::fMaterial::DebugDraw; + psb->generateBendingConstraints(2, pm); + psb->m_cfg.piterations = 20; + psb->m_cfg.kDF = 0.5; + //turn on softbody vs softbody collision + psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS; + psb->randomizeConstraints(); + psb->setTotalMass(mass, true); + } +#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD psb->scale(btVector3(scale, scale, scale)); psb->rotate(initialOrn); psb->translate(initialPos); psb->getCollisionShape()->setMargin(collisionMargin); psb->getCollisionShape()->setUserPointer(psb); - m_data->m_dynamicsWorld->addSoftBody(psb); +#ifndef SKIP_DEFORMABLE_BODY + if (deformWorld) + { + deformWorld->addSoftBody(psb); + } else +#endif//SKIP_DEFORMABLE_BODY + { + btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); + if (softWorld) + { + softWorld->addSoftBody(psb); + } + } m_data->m_guiHelper->createCollisionShapeGraphicsObject(psb->getCollisionShape()); m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); int bodyUniqueId = m_data->m_bodyHandles.allocHandle(); @@ -9190,7 +9293,17 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st clientCmd.m_physSimParamArgs.m_gravityAcceleration[2]); this->m_data->m_dynamicsWorld->setGravity(grav); #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD - m_data->m_dynamicsWorld->getWorldInfo().m_gravity = grav; + btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); + if (softWorld) + { + softWorld->getWorldInfo().m_gravity = grav; + } + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld) + { + deformWorld->getWorldInfo().m_gravity = grav; + } + #endif if (m_data->m_verboseOutput) @@ -9549,7 +9662,8 @@ bool PhysicsServerCommandProcessor::processResetSimulationCommand(const struct S bool hasStatus = true; BT_PROFILE("CMD_RESET_SIMULATION"); m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL, 0); - resetSimulation(); + + resetSimulation(clientCmd.m_updateFlags); m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL, 1); SharedMemoryStatus& serverCmd = serverStatusOut; @@ -10416,7 +10530,17 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared m_data->m_pluginManager.getRenderInterface()->removeVisualShape(psb->getBroadphaseHandle()->getUid()); } serverCmd.m_removeObjectArgs.m_bodyUniqueIds[serverCmd.m_removeObjectArgs.m_numBodies++] = bodyUniqueId; - m_data->m_dynamicsWorld->removeSoftBody(psb); + btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); + if (softWorld) + { + softWorld->removeSoftBody(psb); + } + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld) + { + deformWorld->removeSoftBody(psb); + } + int graphicsInstance = psb->getUserIndex2(); m_data->m_guiHelper->removeGraphicsInstance(graphicsInstance); delete psb; @@ -12846,18 +12970,22 @@ void PhysicsServerCommandProcessor::renderScene(int renderFlags) #ifndef SKIP_DEFORMABLE_BODY if (m_data->m_dynamicsWorld) { - if (m_data->m_dynamicsWorld->getSoftBodyArray().size()) + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld) { - for (int i = 0; i < m_data->m_dynamicsWorld->getSoftBodyArray().size(); i++) + if (deformWorld->getSoftBodyArray().size()) { - btSoftBody* psb = (btSoftBody*)m_data->m_dynamicsWorld->getSoftBodyArray()[i]; + for (int i = 0; i < deformWorld->getSoftBodyArray().size(); i++) { - btSoftBodyHelpers::DrawFrame(psb, m_data->m_dynamicsWorld->getDebugDrawer()); - btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(), m_data->m_dynamicsWorld->getDrawFlags()); + 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(); } - m_data->m_guiHelper->drawDebugDrawerLines(); - m_data->m_guiHelper->clearLines(); } } #endif @@ -12875,15 +13003,36 @@ void PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags) m_data->m_dynamicsWorld->debugDrawWorld(); #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD - for (int i = 0; i < m_data->m_dynamicsWorld->getSoftBodyArray().size(); i++) { - btSoftBody* psb = (btSoftBody*)m_data->m_dynamicsWorld->getSoftBodyArray()[i]; - if (m_data->m_dynamicsWorld->getDebugDrawer() && !(m_data->m_dynamicsWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld) + { + for (int i = 0; i < deformWorld->getSoftBodyArray().size(); i++) { - //btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer()); - btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(), m_data->m_dynamicsWorld->getDrawFlags()); + btSoftBody* psb = (btSoftBody*)deformWorld->getSoftBodyArray()[i]; + if (m_data->m_dynamicsWorld->getDebugDrawer() && !(m_data->m_dynamicsWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) + { + //btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(),deformWorld->getDrawFlags()); + } } } + } + { + btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); + if (softWorld) + { + for (int i = 0; i < softWorld->getSoftBodyArray().size(); i++) + { + btSoftBody* psb = (btSoftBody*)softWorld->getSoftBodyArray()[i]; + if (m_data->m_dynamicsWorld->getDebugDrawer() && !(m_data->m_dynamicsWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) + { + //btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(),softWorld->getDrawFlags()); + } + } + } + } #endif } } @@ -13272,7 +13421,7 @@ void PhysicsServerCommandProcessor::addBodyChangedNotifications() } } -void PhysicsServerCommandProcessor::resetSimulation() +void PhysicsServerCommandProcessor::resetSimulation(int flags) { //clean up all data m_data->m_remoteSyncTransformTime = m_data->m_remoteSyncTransformInterval; @@ -13283,7 +13432,21 @@ void PhysicsServerCommandProcessor::resetSimulation() #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD if (m_data && m_data->m_dynamicsWorld) { - m_data->m_dynamicsWorld->getWorldInfo().m_sparsesdf.Reset(); + { + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld) + { + deformWorld ->getWorldInfo().m_sparsesdf.Reset(); + } + } + { + btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); + if (softWorld) + { + softWorld->getWorldInfo().m_sparsesdf.Reset(); + } + } + } #endif if (m_data && m_data->m_guiHelper) @@ -13314,7 +13477,7 @@ void PhysicsServerCommandProcessor::resetSimulation() removePickingConstraint(); deleteDynamicsWorld(); - createEmptyDynamicsWorld(); + createEmptyDynamicsWorld(flags); m_data->m_bodyHandles.exitHandles(); m_data->m_bodyHandles.initHandles(); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index d36b1a713..78bc9da83 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h @@ -17,9 +17,12 @@ class PhysicsServerCommandProcessor : public CommandProcessorInterface { struct PhysicsServerCommandProcessorInternalData* m_data; - void resetSimulation(); + void resetSimulation(int flags=0); void createThreadPool(); + class btDeformableMultiBodyDynamicsWorld* getDeformableWorld(); + class btSoftMultiBodyDynamicsWorld* getSoftWorld(); + protected: bool processStateLoggingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processRequestCameraImageCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); @@ -114,7 +117,7 @@ public: void createJointMotors(class btMultiBody* body); - virtual void createEmptyDynamicsWorld(); + virtual void createEmptyDynamicsWorld(int flags=0); virtual void deleteDynamicsWorld(); virtual bool connect() diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 3db75d397..2e3d9ae26 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -563,6 +563,13 @@ enum b3NotificationType SOFTBODY_CHANGED = 9, }; +enum b3ResetSimulationFlags +{ + RESET_USE_DEFORMABLE_WORLD=1, + RESET_USE_DISCRETE_DYNAMICS_WORLD=2, + RESET_USE_SIMPLE_BROADPHASE=4, +}; + struct b3BodyNotificationArgs { int m_bodyUniqueId; diff --git a/examples/pybullet/examples/load_soft_body.py b/examples/pybullet/examples/load_soft_body.py index 1d1052c88..3c55ca2e6 100644 --- a/examples/pybullet/examples/load_soft_body.py +++ b/examples/pybullet/examples/load_soft_body.py @@ -1,24 +1,24 @@ import pybullet as p from time import sleep -physicsClient = p.connect(p.DIRECT) - +physicsClient = p.connect(p.GUI) p.setGravity(0, 0, -10) -planeId = p.loadURDF("plane.urdf") -boxId = p.loadURDF("cube.urdf", useMaximalCoordinates = True) -bunnyId = p.loadSoftBody("bunny.obj") +planeId = p.loadURDF("plane.urdf", [0,0,-2]) +boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True) +bunnyId = p.loadSoftBody("bunny.obj")#.obj")#.vtk") + #meshData = p.getMeshData(bunnyId) #print("meshData=",meshData) -p.loadURDF("cube_small.urdf", [1, 0, 1]) +#p.loadURDF("cube_small.urdf", [1, 0, 1]) useRealTimeSimulation = 1 if (useRealTimeSimulation): p.setRealTimeSimulation(1) print(p.getDynamicsInfo(planeId, -1)) -print(p.getDynamicsInfo(bunnyId, 0)) +#print(p.getDynamicsInfo(bunnyId, 0)) print(p.getDynamicsInfo(boxId, -1)) - +p.changeDynamics(boxId,-1,mass=10) while p.isConnected(): p.setGravity(0, 0, -10) if (useRealTimeSimulation): diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 2627ecc4c..6e673bc1b 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -2050,10 +2050,11 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* static PyObject* pybullet_resetSimulation(PyObject* self, PyObject* args, PyObject* keywds) { int physicsClientId = 0; - static char* kwlist[] = {"physicsClientId", NULL}; + int flags = 0; + static char* kwlist[] = {"flags", "physicsClientId", NULL}; b3PhysicsClientHandle sm = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|ii", kwlist, &flags, &physicsClientId)) { return NULL; } @@ -2065,9 +2066,12 @@ static PyObject* pybullet_resetSimulation(PyObject* self, PyObject* args, PyObje } { + b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; + commandHandle = b3InitResetSimulationCommand(sm); + b3InitResetSimulationSetFlags(commandHandle, flags); statusHandle = b3SubmitClientCommandAndWaitStatus( - sm, b3InitResetSimulationCommand(sm)); + sm, commandHandle); } Py_INCREF(Py_None); return Py_None; @@ -11721,7 +11725,7 @@ static PyMethodDef SpamMethods[] = { {"resetSimulation", (PyCFunction)pybullet_resetSimulation, METH_VARARGS | METH_KEYWORDS, "resetSimulation(physicsClientId=0)\n" "Reset the simulation: remove all objects and start from an empty world."}, - + {"stepSimulation", (PyCFunction)pybullet_stepSimulation, METH_VARARGS | METH_KEYWORDS, "stepSimulation(physicsClientId=0)\n" "Step the simulation using forward dynamics."}, @@ -12376,6 +12380,10 @@ initpybullet(void) //PyModule_AddIntConstant(m, "CONSTRAINT_SOLVER_LCP_NNCF",eConstraintSolverLCP_NNCG); //PyModule_AddIntConstant(m, "CONSTRAINT_SOLVER_LCP_BLOCK",eConstraintSolverLCP_BLOCK_PGS); + PyModule_AddIntConstant(m, "RESET_USE_DEFORMABLE_WORLD", RESET_USE_DEFORMABLE_WORLD); + PyModule_AddIntConstant(m, "RESET_USE_DISCRETE_DYNAMICS_WORLD", RESET_USE_DISCRETE_DYNAMICS_WORLD); + PyModule_AddIntConstant(m, "RESET_USE_SIMPLE_BROADPHASE", RESET_USE_SIMPLE_BROADPHASE); + PyModule_AddIntConstant(m, "VR_BUTTON_IS_DOWN", eButtonIsDown); PyModule_AddIntConstant(m, "VR_BUTTON_WAS_TRIGGERED", eButtonTriggered); PyModule_AddIntConstant(m, "VR_BUTTON_WAS_RELEASED", eButtonReleased); diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index 3aafe23d2..c9e4af382 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -1509,28 +1509,31 @@ void btSoftBodyHelpers::readRenderMeshFromObj(const char* file, btSoftBody* psb) while (std::getline(fs, line)) { std::stringstream ss(line); - if (line[0] == 'v' && line[1] != 't' && line[1] != 'n') - { - ss.ignore(); - for (size_t i = 0; i < 3; i++) - ss >> pos[i]; - btSoftBody::Node n; - n.m_x = pos; - psb->m_renderNodes.push_back(n); - } - else if (line[0] == 'f') - { - ss.ignore(); - int id0, id1, id2; - ss >> id0; - ss >> id1; - ss >> id2; - btSoftBody::Face f; - f.m_n[0] = &psb->m_renderNodes[id0-1]; - f.m_n[1] = &psb->m_renderNodes[id1-1]; - f.m_n[2] = &psb->m_renderNodes[id2-1]; - psb->m_renderFaces.push_back(f); - } + if (line.length()>1) + { + if (line[0] == 'v' && line[1] != 't' && line[1] != 'n') + { + ss.ignore(); + for (size_t i = 0; i < 3; i++) + ss >> pos[i]; + btSoftBody::Node n; + n.m_x = pos; + psb->m_renderNodes.push_back(n); + } + else if (line[0] == 'f') + { + ss.ignore(); + int id0, id1, id2; + ss >> id0; + ss >> id1; + ss >> id2; + btSoftBody::Face f; + f.m_n[0] = &psb->m_renderNodes[id0-1]; + f.m_n[1] = &psb->m_renderNodes[id1-1]; + f.m_n[2] = &psb->m_renderNodes[id2-1]; + psb->m_renderFaces.push_back(f); + } + } } fs.close(); }