From 0b391798b700f048ebd659be5f2ae3cd375f740d Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 26 Aug 2019 15:43:21 -0700 Subject: [PATCH] hook deformable world into the physics server --- .../CommonGUIHelperInterface.h | 3 +- examples/DeformableDemo/GraspDeformable.cpp | 2 +- .../ImportURDFDemo/UrdfFindMeshFile.h | 6 +- .../Importers/ImportURDFDemo/UrdfParser.h | 1 + examples/SharedMemory/PhysicsClientC_API.cpp | 69 ++++++- examples/SharedMemory/PhysicsClientC_API.h | 7 + .../PhysicsServerCommandProcessor.cpp | 185 +++++++++++++++--- .../SharedMemory/PhysicsServerExample.cpp | 20 +- examples/SharedMemory/SharedMemoryCommands.h | 27 ++- 9 files changed, 277 insertions(+), 43 deletions(-) diff --git a/examples/CommonInterfaces/CommonGUIHelperInterface.h b/examples/CommonInterfaces/CommonGUIHelperInterface.h index c604410a8..323fe103a 100644 --- a/examples/CommonInterfaces/CommonGUIHelperInterface.h +++ b/examples/CommonInterfaces/CommonGUIHelperInterface.h @@ -1,6 +1,5 @@ #ifndef GUI_HELPER_INTERFACE_H #define GUI_HELPER_INTERFACE_H - class btRigidBody; class btVector3; class btCollisionObject; @@ -118,6 +117,8 @@ struct GUIHelperInterface //empty name stops dumping video virtual void dumpFramesToVideo(const char* mp4FileName){}; + virtual void drawDebugDrawerLines(){} + virtual void clearLines(){} }; ///the DummyGUIHelper does nothing, so we can test the examples without GUI/graphics (in 'console mode') diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index 6b01bc66d..341f4c737 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -128,7 +128,7 @@ public: //use a smaller internal timestep, there are stability issues float internalTimeStep = 1. / 250.f; - m_dynamicsWorld->stepSimulation(deltaTime, 100, internalTimeStep); + m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); } void createGrip() diff --git a/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h b/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h index 2903e7cf2..147d8c911 100644 --- a/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h +++ b/examples/Importers/ImportURDFDemo/UrdfFindMeshFile.h @@ -45,6 +45,10 @@ static bool UrdfFindMeshFile( { *out_type = UrdfGeometry::FILE_CDF; } + else if (ext == ".vtk") + { + *out_type = UrdfGeometry::FILE_VTK; + } else { b3Warning("%s: invalid mesh filename extension '%s'\n", error_message_prefix.c_str(), ext.c_str()); @@ -115,4 +119,4 @@ static bool UrdfFindMeshFile( } } -#endif //URDF_FIND_MESH_FILE_H \ No newline at end of file +#endif //URDF_FIND_MESH_FILE_H diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 77c2fabf3..48463044d 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -82,6 +82,7 @@ struct UrdfGeometry FILE_OBJ = 3, FILE_CDF = 4, MEMORY_VERTICES = 5, + FILE_VTK = 6, }; int m_meshFileType; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index cac6b161f..87276b264 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -338,6 +338,74 @@ B3_SHARED_API int b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle return 0; } +B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command->m_type == CMD_LOAD_SOFT_BODY); + command->m_loadSoftBodyArguments.m_corotatedMu = corotatedMu; + command->m_loadSoftBodyArguments.m_corotatedLambda = corotatedLambda; + command->m_updateFlags |= LOAD_SOFT_BODY_ADD_COROTATED_FORCE; + return 0; +} + +B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command->m_type == CMD_LOAD_SOFT_BODY); + command->m_loadSoftBodyArguments.m_NeoHookeanMu = NeoHookeanMu; + command->m_loadSoftBodyArguments.m_NeoHookeanLambda = NeoHookeanLambda; + command->m_updateFlags |= LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE; + return 0; +} + +B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command->m_type == CMD_LOAD_SOFT_BODY); + command->m_loadSoftBodyArguments.m_springElasticStiffness = springElasticStiffness; + command->m_loadSoftBodyArguments.m_springDampingStiffness = springDampingStiffness; + command->m_updateFlags |= LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE; + return 0; +} + +B3_SHARED_API int b3LoadSoftBodyAddGravityForce(b3SharedMemoryCommandHandle commandHandle, double gravityX, double gravityY, double gravityZ) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command->m_type == CMD_LOAD_SOFT_BODY); + command->m_loadSoftBodyArguments.m_gravity[0] = gravityX; + command->m_loadSoftBodyArguments.m_gravity[1] = gravityY; + command->m_loadSoftBodyArguments.m_gravity[2] = gravityZ; + command->m_updateFlags |= LOAD_SOFT_BODY_ADD_GRAVITY_FORCE; + return 0; +} + +B3_SHARED_API int b3LoadSoftBodySetCollsionHardness(b3SharedMemoryCommandHandle commandHandle, double collisionHardness) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command->m_type == CMD_LOAD_SOFT_BODY); + command->m_loadSoftBodyArguments.m_collisionHardness = collisionHardness; + command->m_updateFlags |= LOAD_SOFT_BODY_SET_COLLISION_HARDNESS; + return 0; +} + +B3_SHARED_API int b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHandle commandHandle, double frictionCoefficient) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command->m_type == CMD_LOAD_SOFT_BODY); + command->m_loadSoftBodyArguments.m_frictionCoeff = frictionCoefficient; + command->m_updateFlags |= LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT; + return 0; +} + +B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle commandHandle, int useBendingSprings) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command->m_type == CMD_LOAD_SOFT_BODY); + command->m_loadSoftBodyArguments.m_useBendingSprings = useBendingSprings; + command->m_updateFlags |= LOAD_SOFT_BODY_ADD_BENDING_SPRINGS; + return 0; +} + B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName) { PhysicsClient* cl = (PhysicsClient*)physClient; @@ -1373,7 +1441,6 @@ B3_SHARED_API int b3CreateCollisionShapeAddHeightfield2(b3PhysicsClientHandle ph command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numHeightfieldRows = numHeightfieldRows; command->m_createUserShapeArgs.m_shapes[shapeIndex].m_numHeightfieldColumns = numHeightfieldColumns; command->m_createUserShapeArgs.m_shapes[shapeIndex].m_replaceHeightfieldIndex = replaceHeightfieldIndex; - cl->uploadBulletFileToSharedMemory((const char*)heightfieldData, numHeightfieldRows*numHeightfieldColumns* sizeof(float)); command->m_createUserShapeArgs.m_numUserShapes++; return shapeIndex; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 9a488562f..886e7a6ea 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -631,6 +631,13 @@ extern "C" B3_SHARED_API int b3LoadSoftBodySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin); B3_SHARED_API int b3LoadSoftBodySetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX, double startPosY, double startPosZ); B3_SHARED_API int b3LoadSoftBodySetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX, double startOrnY, double startOrnZ, double startOrnW); + B3_SHARED_API int b3LoadSoftBodyAddCorotatedForce(b3SharedMemoryCommandHandle commandHandle, double corotatedMu, double corotatedLambda); + B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda); + B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness); + B3_SHARED_API int b3LoadSoftBodyAddGravityForce(b3SharedMemoryCommandHandle commandHandle, double gravityX, double gravityY, double gravityZ); + B3_SHARED_API int b3LoadSoftBodySetCollsionHardness(b3SharedMemoryCommandHandle commandHandle, double collisionHardness); + B3_SHARED_API int b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHandle commandHandle, double frictionCoefficient); + B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle commandHandle, int useBendingSprings); B3_SHARED_API b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient); B3_SHARED_API void b3VREventsSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 6b8981674..47a4de27a 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -101,7 +101,11 @@ #include "BulletSoftBody/btSoftBodySolvers.h" #include "BulletSoftBody/btSoftBodyHelpers.h" #include "BulletSoftBody/btSoftMultiBodyDynamicsWorld.h" +#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h" +#include "BulletSoftBody/btDeformableBodySolver.h" +#include "BulletSoftBody/btDeformableMultiBodyConstraintSolver.h" #include "../SoftDemo/BunnyMesh.h" +#define SKIP_DEFORMABLE_BODY #else #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #endif @@ -1619,12 +1623,22 @@ struct PhysicsServerCommandProcessorInternalData btHashedOverlappingPairCache* m_pairCache; btBroadphaseInterface* m_broadphase; btCollisionDispatcher* m_dispatcher; +#ifdef SKIP_DEFORMABLE_BODY 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; + btDeformableBodySolver* m_deformablebodySolver; +#else btSoftMultiBodyDynamicsWorld* m_dynamicsWorld; btSoftBodySolver* m_softbodySolver; +#endif #else btMultiBodyDynamicsWorld* m_dynamicsWorld; #endif @@ -2607,10 +2621,19 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() 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 +#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); #endif @@ -2775,7 +2798,9 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() for (i = m_data->m_dynamicsWorld->getSoftBodyArray().size() - 1; i >= 0; i--) { btSoftBody* sb = m_data->m_dynamicsWorld->getSoftBodyArray()[i]; +#ifdef SKIP_DEFORMABLE_BODY m_data->m_dynamicsWorld->removeSoftBody(sb); +#endif delete sb; } #endif @@ -4728,8 +4753,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str break; } - - case GEOM_MESH: + case GEOM_MESH: { btVector3 meshScale(clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[0], clientCmd.m_createUserShapeArgs.m_shapes[i].m_meshScale[1], @@ -4909,6 +4933,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str } break; } + default: { } @@ -8014,26 +8039,113 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar int out_type; bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); - std::vector shapes; - tinyobj::attrib_t attribute; - std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), "", fileIO); - if (!shapes.empty()) - { - const tinyobj::shape_t& shape = shapes[0]; - btAlignedObjectArray vertices; - btAlignedObjectArray indices; - for (int i = 0; i < attribute.vertices.size(); i++) - { - vertices.push_back(attribute.vertices[i]); - } - for (int i = 0; i < shape.mesh.indices.size(); i++) - { - indices.push_back(shape.mesh.indices[i].vertex_index); - } - int numTris = shape.mesh.indices.size() / 3; - if (numTris > 0) - { - btSoftBody* psb = btSoftBodyHelpers::CreateFromTriMesh(m_data->m_dynamicsWorld->getWorldInfo(), &vertices[0], &indices[0], numTris); + btSoftBody* psb = NULL; + if (out_type == UrdfGeometry::FILE_OBJ) + { + std::vector shapes; + tinyobj::attrib_t attribute; + std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), "", fileIO); + if (!shapes.empty()) + { + const tinyobj::shape_t& shape = shapes[0]; + btAlignedObjectArray vertices; + btAlignedObjectArray indices; + for (int i = 0; i < attribute.vertices.size(); i++) + { + vertices.push_back(attribute.vertices[i]); + } + for (int i = 0; i < shape.mesh.indices.size(); i++) + { + indices.push_back(shape.mesh.indices[i].vertex_index); + } + int numTris = shape.mesh.indices.size() / 3; + if (numTris > 0) + { + psb = btSoftBodyHelpers::CreateFromTriMesh(m_data->m_dynamicsWorld->getWorldInfo(), &vertices[0], &indices[0], numTris); + } + } + } + else if (out_type = UrdfGeometry::FILE_VTK) + { +#ifndef SKIP_DEFORMABLE_BODY + psb = btSoftBodyHelpers::CreateFromVtkFile(m_data->m_dynamicsWorld->getWorldInfo(), out_found_filename.c_str()); + btScalar corotated_mu, corotated_lambda; + btScalar spring_elastic_stiffness, spring_damping_stiffness; + btScalar gravity_constant; + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE) + { + corotated_mu = clientCmd.m_loadSoftBodyArguments.m_corotatedMu; + corotated_lambda = clientCmd.m_loadSoftBodyArguments.m_corotatedLambda; + m_data->m_dynamicsWorld->addForce(psb, new btDeformableCorotatedForce(corotated_mu, corotated_lambda)); + } + btScalar neohookean_mu, neohookean_lambda; + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE) + { + neohookean_mu = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanMu; + neohookean_lambda = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanLambda; + m_data->m_dynamicsWorld->addForce(psb, new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda)); + } +#endif + } + if (psb != NULL) + { +#ifndef SKIP_DEFORMABLE_BODY + btScalar corotated_mu, corotated_lambda; + btScalar spring_elastic_stiffness, spring_damping_stiffness; + btScalar gravity_constant; + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE) + { + corotated_mu = clientCmd.m_loadSoftBodyArguments.m_corotatedMu; + corotated_lambda = clientCmd.m_loadSoftBodyArguments.m_corotatedLambda; + m_data->m_dynamicsWorld->addForce(psb, new btDeformableCorotatedForce(corotated_mu, corotated_lambda)); + } + 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; + m_data->m_dynamicsWorld->addForce(psb, new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, false)); + } + btVector3 gravity(0,0,0); + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_GRAVITY_FORCE) + { + gravity[0] = loadSoftBodyArgs.m_gravity[0]; + gravity[1] = loadSoftBodyArgs.m_gravity[1]; + gravity[2] = loadSoftBodyArgs.m_gravity[2]; + m_data->m_dynamicsWorld->addForce(psb, new btDeformableGravityForce(gravity)); + } + btScalar collision_hardness = 1; + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_COLLISION_HARDNESS) + { + collision_hardness = loadSoftBodyArgs.m_collisionHardness; + } + psb->m_cfg.kKHR = collision_hardness; + psb->m_cfg.kCHR = collision_hardness; + + btScalar friction_coeff = 0; + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT) + { + friction_coeff = loadSoftBodyArgs.m_frictionCoeff; + } + psb->m_cfg.kDF = friction_coeff; + + bool use_bending_spring = true; + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS) + { + use_bending_spring = loadSoftBodyArgs.m_useBendingSprings; + } + btSoftBody::Material* pm = psb->appendMaterial(); + pm->m_flags -= btSoftBody::fMaterial::DebugDraw; + if (use_bending_spring) + { + psb->generateBendingConstraints(2,pm); + } + + // turn on the collision flag for deformable + // collision + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->setCollisionFlags(0); + psb->setTotalMass(mass); +#else btSoftBody::Material* pm = psb->appendMaterial(); pm->m_kLST = 0.5; pm->m_flags -= btSoftBody::fMaterial::DebugDraw; @@ -8043,11 +8155,12 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar //turn on softbody vs softbody collision psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS; psb->randomizeConstraints(); + psb->setTotalMass(mass, true); +#endif psb->scale(btVector3(scale, scale, scale)); psb->rotate(initialOrn); psb->translate(initialPos); - psb->setTotalMass(mass, true); psb->getCollisionShape()->setMargin(collisionMargin); psb->getCollisionShape()->setUserPointer(psb); m_data->m_dynamicsWorld->addSoftBody(psb); @@ -8108,10 +8221,8 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar notification.m_notificationType = BODY_ADDED; notification.m_bodyArgs.m_bodyUniqueId = bodyUniqueId; m_data->m_pluginManager.addNotification(notification); - } } } - #endif return hasStatus; } @@ -9134,6 +9245,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st } }; +#ifdef SKIP_DEFORMABLE_BODY if (newSolver) { delete oldSolver; @@ -9142,6 +9254,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st m_data->m_solver = newSolver; printf("switched solver\n"); } +#endif } } @@ -10281,10 +10394,14 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared btSoftBody* psb = bodyHandle->m_softBody; if (m_data->m_pluginManager.getRenderInterface()) { +#ifdef SKIP_DEFORMABLE_BODY m_data->m_pluginManager.getRenderInterface()->removeVisualShape(psb->getBroadphaseHandle()->getUid()); +#endif } serverCmd.m_removeObjectArgs.m_bodyUniqueIds[serverCmd.m_removeObjectArgs.m_numBodies++] = bodyUniqueId; +#ifdef SKIP_DEFORMABLE_BODY m_data->m_dynamicsWorld->removeSoftBody(psb); +#endif int graphicsInstance = psb->getUserIndex2(); m_data->m_guiHelper->removeGraphicsInstance(graphicsInstance); delete psb; @@ -12020,7 +12137,7 @@ bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct Share bool PhysicsServerCommandProcessor::processSaveStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { BT_PROFILE("CMD_SAVE_STATE"); - bool hasStatus = true; + bool hasStatus = true; SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_SAVE_STATE_FAILED; @@ -12430,7 +12547,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } case CMD_LOAD_SOFT_BODY: { - hasStatus = processLoadSoftBodyCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + hasStatus = processLoadSoftBodyCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } case CMD_CREATE_SENSOR: @@ -12716,6 +12833,20 @@ 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 + for (int i = 0; i < m_data->m_dynamicsWorld->getSoftBodyArray().size(); i++) + { + btSoftBody* psb = (btSoftBody*)m_data->m_dynamicsWorld->getSoftBodyArray()[i]; + { + btSoftBodyHelpers::DrawFrame(psb, m_data->m_dynamicsWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(), m_data->m_dynamicsWorld->getDrawFlags()); + } + } + m_data->m_guiHelper->drawDebugDrawerLines(); + m_data->m_guiHelper->clearLines(); +#endif +#endif } } diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index f3e670c0d..0c0640884 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -546,7 +546,7 @@ MultithreadedDebugDrawer : public btIDebugDraw btHashMap m_hashedLines; public: - void drawDebugDrawerLines() + virtual void drawDebugDrawerLines() { if (m_hashedLines.size()) { @@ -628,7 +628,7 @@ public: return m_debugMode; } - virtual void clearLines() + virtual void clearLines() override { m_hashedLines.clear(); m_sortedIndices.clear(); @@ -650,13 +650,21 @@ class MultiThreadedOpenGLGuiHelper : public GUIHelperInterface public: MultithreadedDebugDrawer* m_debugDraw; - void drawDebugDrawerLines() + virtual void drawDebugDrawerLines() { if (m_debugDraw) { m_debugDraw->drawDebugDrawerLines(); } } + virtual void clearLines() + { + if (m_debugDraw) + { + m_debugDraw->clearLines(); + } + } + GUIHelperInterface* m_childGuiHelper; btHashMap m_cachedTextureIds; @@ -847,10 +855,8 @@ public: delete m_debugDraw; m_debugDraw = 0; } - - m_debugDraw = new MultithreadedDebugDrawer(this); - - rbWorld->setDebugDrawer(m_debugDraw); + m_debugDraw = new MultithreadedDebugDrawer(this); + rbWorld->setDebugDrawer(m_debugDraw); //m_childGuiHelper->createPhysicsDebugDrawer(rbWorld); } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 4ff925a59..58d72ce8d 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -489,11 +489,18 @@ enum EnumSimParamUpdateFlags enum EnumLoadSoftBodyUpdateFlags { LOAD_SOFT_BODY_FILE_NAME = 1, - LOAD_SOFT_BODY_UPDATE_SCALE = 2, - LOAD_SOFT_BODY_UPDATE_MASS = 4, - LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN = 8, - LOAD_SOFT_BODY_INITIAL_POSITION = 16, - LOAD_SOFT_BODY_INITIAL_ORIENTATION = 32 + LOAD_SOFT_BODY_UPDATE_SCALE = 1<<1, + LOAD_SOFT_BODY_UPDATE_MASS = 1<<2, + LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN = 1<<3, + LOAD_SOFT_BODY_INITIAL_POSITION = 1<<4, + LOAD_SOFT_BODY_INITIAL_ORIENTATION = 1<<5, + LOAD_SOFT_BODY_ADD_COROTATED_FORCE = 1<<6, + LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE = 1<<7, + LOAD_SOFT_BODY_ADD_GRAVITY_FORCE = 1<<8, + LOAD_SOFT_BODY_SET_COLLISION_HARDNESS = 1<<9, + LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT = 1<<10, + LOAD_SOFT_BODY_ADD_BENDING_SPRINGS = 1<<11, + LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE = 1<<12, }; enum EnumSimParamInternalSimFlags @@ -512,6 +519,16 @@ struct LoadSoftBodyArgs double m_collisionMargin; double m_initialPosition[3]; double m_initialOrientation[4]; + double m_springElasticStiffness; + double m_springDampingStiffness; + double m_corotatedMu; + double m_corotatedLambda; + double m_gravity[3]; + bool m_useBendingSprings; + double m_collisionHardness; + double m_frictionCoeff; + double m_NeoHookeanMu; + double m_NeoHookeanLambda; }; struct b3LoadSoftBodyResultArgs