From 0b391798b700f048ebd659be5f2ae3cd375f740d Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 26 Aug 2019 15:43:21 -0700 Subject: [PATCH 01/18] 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 From 7c39052163e308120ff5cf8a0d45ac1fb7ad3670 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 26 Aug 2019 16:01:29 -0700 Subject: [PATCH 02/18] clean up forces --- examples/DeformableDemo/DeformableMultibody.cpp | 11 ++++++----- examples/DeformableDemo/DeformableRigid.cpp | 11 ++++++----- examples/DeformableDemo/GraspDeformable.cpp | 14 ++++++++------ examples/DeformableDemo/Pinch.cpp | 13 +++++++------ examples/DeformableDemo/VolumetricDeformable.cpp | 15 ++++++++------- 5 files changed, 35 insertions(+), 29 deletions(-) diff --git a/examples/DeformableDemo/DeformableMultibody.cpp b/examples/DeformableDemo/DeformableMultibody.cpp index 6a99e08db..5ffaefc36 100644 --- a/examples/DeformableDemo/DeformableMultibody.cpp +++ b/examples/DeformableDemo/DeformableMultibody.cpp @@ -49,7 +49,7 @@ static bool g_floatingBase = true; static float friction = 1.; class DeformableMultibody : public CommonMultiBodyBase { - btAlignedObjectArray forces; + btAlignedObjectArray m_forces; public: DeformableMultibody(struct GUIHelperInterface* helper) : CommonMultiBodyBase(helper) @@ -229,11 +229,11 @@ void DeformableMultibody::initPhysics() btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2, 0.01, false); getDeformableDynamicsWorld()->addForce(psb, mass_spring); - forces.push_back(mass_spring); + m_forces.push_back(mass_spring); btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); getDeformableDynamicsWorld()->addForce(psb, gravity_force); - forces.push_back(gravity_force); + m_forces.push_back(gravity_force); } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); @@ -257,11 +257,12 @@ void DeformableMultibody::exitPhysics() delete obj; } // delete forces - for (int j = 0; j < forces.size(); j++) + for (int j = 0; j < m_forces.size(); j++) { - btDeformableLagrangianForce* force = forces[j]; + btDeformableLagrangianForce* force = m_forces[j]; delete force; } + m_forces.clear(); //delete collision shapes for (int j = 0; j < m_collisionShapes.size(); j++) { diff --git a/examples/DeformableDemo/DeformableRigid.cpp b/examples/DeformableDemo/DeformableRigid.cpp index 576803cc8..3cbfba451 100644 --- a/examples/DeformableDemo/DeformableRigid.cpp +++ b/examples/DeformableDemo/DeformableRigid.cpp @@ -44,7 +44,7 @@ ///Generally it is best to leave the rolling friction coefficient zero (or close to zero). class DeformableRigid : public CommonRigidBodyBase { - btAlignedObjectArray forces; + btAlignedObjectArray m_forces; public: DeformableRigid(struct GUIHelperInterface* helper) : CommonRigidBodyBase(helper) @@ -240,11 +240,11 @@ void DeformableRigid::initPhysics() btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2,0.01, false); getDeformableDynamicsWorld()->addForce(psb, mass_spring); - forces.push_back(mass_spring); + m_forces.push_back(mass_spring); btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); getDeformableDynamicsWorld()->addForce(psb, gravity_force); - forces.push_back(gravity_force); + m_forces.push_back(gravity_force); // add a few rigid bodies Ctor_RbUpStack(1); } @@ -269,11 +269,12 @@ void DeformableRigid::exitPhysics() delete obj; } // delete forces - for (int j = 0; j < forces.size(); j++) + for (int j = 0; j < m_forces.size(); j++) { - btDeformableLagrangianForce* force = forces[j]; + btDeformableLagrangianForce* force = m_forces[j]; delete force; } + m_forces.clear(); //delete collision shapes for (int j = 0; j < m_collisionShapes.size(); j++) { diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index 341f4c737..b5cc66626 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -59,7 +59,7 @@ static bool supportsJointMotor(btMultiBody* mb, int mbLinkIndex) class GraspDeformable : public CommonRigidBodyBase { - btAlignedObjectArray forces; + btAlignedObjectArray m_forces; public: GraspDeformable(struct GUIHelperInterface* helper) : CommonRigidBodyBase(helper) @@ -359,15 +359,15 @@ void GraspDeformable::initPhysics() btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(.5,0.04, true); getDeformableDynamicsWorld()->addForce(psb, mass_spring); - forces.push_back(mass_spring); + m_forces.push_back(mass_spring); btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); getDeformableDynamicsWorld()->addForce(psb, gravity_force); - forces.push_back(gravity_force); + m_forces.push_back(gravity_force); btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(2,10); getDeformableDynamicsWorld()->addForce(psb, neohookean); - forces.push_back(neohookean); + m_forces.push_back(neohookean); } // // create a piece of cloth @@ -440,11 +440,13 @@ void GraspDeformable::exitPhysics() delete obj; } // delete forces - for (int j = 0; j < forces.size(); j++) + for (int j = 0; j < m_forces.size(); j++) { - btDeformableLagrangianForce* force = forces[j]; + btDeformableLagrangianForce* force = m_forces[j]; delete force; } + m_forces.clear(); + //delete collision shapes for (int j = 0; j < m_collisionShapes.size(); j++) { diff --git a/examples/DeformableDemo/Pinch.cpp b/examples/DeformableDemo/Pinch.cpp index 3685869a0..661698a06 100644 --- a/examples/DeformableDemo/Pinch.cpp +++ b/examples/DeformableDemo/Pinch.cpp @@ -56,7 +56,7 @@ struct TetraBunny class Pinch : public CommonRigidBodyBase { - btAlignedObjectArray forces; + btAlignedObjectArray m_forces; public: Pinch(struct GUIHelperInterface* helper) : CommonRigidBodyBase(helper) @@ -339,15 +339,15 @@ void Pinch::initPhysics() btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(1,0.05); getDeformableDynamicsWorld()->addForce(psb, mass_spring); - forces.push_back(mass_spring); + m_forces.push_back(mass_spring); btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); getDeformableDynamicsWorld()->addForce(psb, gravity_force); - forces.push_back(gravity_force); + m_forces.push_back(gravity_force); btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(.2,1); getDeformableDynamicsWorld()->addForce(psb, neohookean); - forces.push_back(neohookean); + m_forces.push_back(neohookean); // add a grippers createGrip(); } @@ -372,11 +372,12 @@ void Pinch::exitPhysics() delete obj; } // delete forces - for (int j = 0; j < forces.size(); j++) + for (int j = 0; j < m_forces.size(); j++) { - btDeformableLagrangianForce* force = forces[j]; + btDeformableLagrangianForce* force = m_forces[j]; delete force; } + m_forces.clear(); //delete collision shapes for (int j = 0; j < m_collisionShapes.size(); j++) { diff --git a/examples/DeformableDemo/VolumetricDeformable.cpp b/examples/DeformableDemo/VolumetricDeformable.cpp index b6657de0c..cff07a175 100644 --- a/examples/DeformableDemo/VolumetricDeformable.cpp +++ b/examples/DeformableDemo/VolumetricDeformable.cpp @@ -51,7 +51,7 @@ struct TetraCube class VolumetricDeformable : public CommonRigidBodyBase { - btAlignedObjectArray forces; + btAlignedObjectArray m_forces; public: VolumetricDeformable(struct GUIHelperInterface* helper) : CommonRigidBodyBase(helper) @@ -238,15 +238,15 @@ void VolumetricDeformable::initPhysics() btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(0,0.03); getDeformableDynamicsWorld()->addForce(psb, mass_spring); - forces.push_back(mass_spring); + m_forces.push_back(mass_spring); btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); getDeformableDynamicsWorld()->addForce(psb, gravity_force); - forces.push_back(gravity_force); + m_forces.push_back(gravity_force); btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(.5,2.5); getDeformableDynamicsWorld()->addForce(psb, neohookean); - forces.push_back(neohookean); + m_forces.push_back(neohookean); } // add a few rigid bodies @@ -273,12 +273,13 @@ void VolumetricDeformable::exitPhysics() delete obj; } // delete forces - for (int j = 0; j < forces.size(); j++) + for (int j = 0; j < m_forces.size(); j++) { - btDeformableLagrangianForce* force = forces[j]; + btDeformableLagrangianForce* force = m_forces[j]; delete force; } - + m_forces.clear(); + //delete collision shapes for (int j = 0; j < m_collisionShapes.size(); j++) { From e73f70efa28ab052e06298ff39005a8d22d78aad Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 26 Aug 2019 16:18:56 -0700 Subject: [PATCH 03/18] swap m_x and m_q in softbody to align with rendering convention --- src/BulletSoftBody/btDeformableBodySolver.cpp | 4 ++-- src/BulletSoftBody/btDeformableCorotatedForce.h | 2 +- src/BulletSoftBody/btDeformableMassSpringForce.h | 6 +++--- .../btDeformableMultiBodyDynamicsWorld.cpp | 2 +- src/BulletSoftBody/btSoftBody.cpp | 6 +++--- src/BulletSoftBody/btSoftBodyInternals.h | 12 ++++++------ 6 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 4489296f0..431a07d28 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -174,7 +174,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d { btSoftBody::Node& n = psb->m_nodes[i]; n.m_q = n.m_x; - n.m_x += n.m_v * dt; + n.m_q += n.m_v * dt; } /* Bounds */ psb->updateBounds(); @@ -184,7 +184,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i) { btSoftBody::Node& n = psb->m_nodes[i]; - vol = btDbvtVolume::FromCR(n.m_x, psb->m_sst.radmrg); + vol = btDbvtVolume::FromCR(n.m_q, psb->m_sst.radmrg); psb->m_ndbvt.update(n.m_leaf, vol, n.m_v * psb->m_sst.velmrg, diff --git a/src/BulletSoftBody/btDeformableCorotatedForce.h b/src/BulletSoftBody/btDeformableCorotatedForce.h index 14896514e..240dbb5d4 100644 --- a/src/BulletSoftBody/btDeformableCorotatedForce.h +++ b/src/BulletSoftBody/btDeformableCorotatedForce.h @@ -92,7 +92,7 @@ public: { // btMatrix3x3 JFinvT = F.adjoint(); btScalar J = F.determinant(); - P = F.adjoint() * (m_lambda * (J-1)); + P = F.adjoint().transpose() * (m_lambda * (J-1)); if (m_mu > SIMD_EPSILON) { btMatrix3x3 R,S; diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h index 14d4e8260..f36e54e41 100644 --- a/src/BulletSoftBody/btDeformableMassSpringForce.h +++ b/src/BulletSoftBody/btDeformableMassSpringForce.h @@ -61,7 +61,7 @@ public: btVector3 scaled_force = scale * m_dampingStiffness * v_diff; if (m_momentum_conserving) { - if ((node2->m_q - node1->m_q).norm() > SIMD_EPSILON) + if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) { btVector3 dir = (node2->m_x - node1->m_x).normalized(); scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir; @@ -91,7 +91,7 @@ public: // elastic force // explicit elastic force - btVector3 dir = (node2->m_q - node1->m_q); + btVector3 dir = (node2->m_x - node1->m_x); btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0); btVector3 scaled_force = scale * m_elasticStiffness * (dir - dir_normalized * r); force[id1] += scaled_force; @@ -118,7 +118,7 @@ public: btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]); if (m_momentum_conserving) { - if ((node2->m_q - node1->m_q).norm() > SIMD_EPSILON) + if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) { btVector3 dir = (node2->m_x - node1->m_x).normalized(); local_scaled_df= scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir; diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 58907fcd3..c6a0417bc 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -166,7 +166,7 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) node.m_v[c] = -clampDeltaV; } } - node.m_x = node.m_q + timeStep * node.m_v; + node.m_x = node.m_x + timeStep * node.m_v; } } m_deformableBodySolver->revertVelocity(); diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index fcfb330c3..c4de6a823 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -2819,9 +2819,9 @@ void btSoftBody::initializeDmInverse() for (int i = 0; i < m_tetras.size(); ++i) { Tetra &t = m_tetras[i]; - btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q; - btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q; - btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q; + btVector3 c1 = t.m_n[1]->m_x - t.m_n[0]->m_x; + btVector3 c2 = t.m_n[2]->m_x - t.m_n[0]->m_x; + btVector3 c3 = t.m_n[3]->m_x - t.m_n[0]->m_x; btMatrix3x3 Dm(c1.getX(), c2.getX(), c3.getX(), c1.getY(), c2.getY(), c3.getY(), c1.getZ(), c2.getZ(), c3.getZ()); diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index a39394e0b..fc4c6b556 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -998,7 +998,7 @@ struct btSoftColliders if (!n.m_battach) { // check for collision at x_{n+1}^* - if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ true)) + if (psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true)) { const btScalar ima = n.m_im; const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; @@ -1006,7 +1006,7 @@ struct btSoftColliders if (ms > 0) { // resolve contact at x_n - psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ false); + psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ false); btSoftBody::sCti& cti = c.m_cti; c.m_node = &n; const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction(); @@ -1019,7 +1019,7 @@ struct btSoftColliders const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform(); static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0); const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; - const btVector3 ra = n.m_q - wtr.getOrigin(); + const btVector3 ra = n.m_x - wtr.getOrigin(); c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra); c.m_c1 = ra; @@ -1035,9 +1035,9 @@ struct btSoftColliders btVector3 t1 = generateUnitOrthogonalVector(normal); btVector3 t2 = btCross(normal, t1); btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2; - findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_q, normal); - findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_q, t1); - findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_q, t2); + findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_x, normal); + findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_x, t1); + findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_x, t2); btScalar* J_n = &jacobianData_normal.m_jacobians[0]; btScalar* J_t1 = &jacobianData_t1.m_jacobians[0]; From c722630fc7a2dc5031fd033cb7ee0afcd29717cb Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Tue, 27 Aug 2019 10:17:00 -0700 Subject: [PATCH 04/18] bug fix in neohookean force --- examples/DeformableDemo/GraspDeformable.cpp | 2 +- src/BulletSoftBody/btDeformableNeoHookeanForce.h | 10 ++++++---- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index b5cc66626..c97aa0d3c 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -357,7 +357,7 @@ void GraspDeformable::initPhysics() // getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); // getDeformableDynamicsWorld()->addForce(psb, new btDeformableCorotatedForce(0,6)); - btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(.5,0.04, true); + btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(.0,0.04, true); getDeformableDynamicsWorld()->addForce(psb, mass_spring); m_forces.push_back(mass_spring); diff --git a/src/BulletSoftBody/btDeformableNeoHookeanForce.h b/src/BulletSoftBody/btDeformableNeoHookeanForce.h index 9bc232e24..2431cd750 100644 --- a/src/BulletSoftBody/btDeformableNeoHookeanForce.h +++ b/src/BulletSoftBody/btDeformableNeoHookeanForce.h @@ -87,18 +87,20 @@ public: btMatrix3x3 C = F.transpose()*F; btScalar J = F.determinant(); btScalar trace = C[0].getX() + C[1].getY() + C[2].getZ(); - P = F * m_mu * ( 1. - 1. / (trace + 1.)) + F.adjoint() * (m_lambda * (J - 1) - 0.75 * m_mu); + P = F * m_mu * ( 1. - 1. / (trace + 1.)) + F.adjoint().transpose() * (m_lambda * (J - 1) - 0.75 * m_mu); } virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) { } - void firstPiolaDifferential(const btMatrix3x3& F, const btMatrix3x3& G, btMatrix3x3& P) + void firstPiolaDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btMatrix3x3& dP) { btScalar J = F.determinant(); - addScaledCofactorMatrixDifferential(F, G, m_lambda*(J-1), P); - P += F.adjoint() * m_lambda * DotProduct(F.adjoint(), G); + addScaledCofactorMatrixDifferential(F, dF, m_lambda*(J-1) - 0.75*m_mu, dP); + dP += F.adjoint().transpose() * m_lambda * DotProduct(F.adjoint().transpose(), dF); + + //todo @xuchenhan: add the dP of the m_mu term. } btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B) From d4a15e016e3c9a7dc80d1758fe1a7067251b78cd Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Tue, 27 Aug 2019 20:54:40 -0700 Subject: [PATCH 05/18] add newton solver --- .../btDeformableBackwardEulerObjective.cpp | 21 +- .../btDeformableBackwardEulerObjective.h | 9 +- src/BulletSoftBody/btDeformableBodySolver.cpp | 95 +++++- src/BulletSoftBody/btDeformableBodySolver.h | 14 +- .../btDeformableCorotatedForce.h | 9 +- src/BulletSoftBody/btDeformableGravityForce.h | 14 +- .../btDeformableLagrangianForce.h | 270 +++++++++++++++++- .../btDeformableMassSpringForce.h | 43 ++- .../btDeformableMultiBodyDynamicsWorld.cpp | 23 +- .../btDeformableMultiBodyDynamicsWorld.h | 9 +- .../btDeformableNeoHookeanForce.h | 113 ++++++-- src/BulletSoftBody/btSoftBody.cpp | 8 +- 12 files changed, 566 insertions(+), 62 deletions(-) diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index 41b1f74c7..2a2ff5c63 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -20,6 +20,7 @@ btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAligned : m_softBodies(softBodies) , projection(m_softBodies, m_dt) , m_backupVelocity(backup_v) +, m_implicit(false) { m_preconditioner = new DefaultPreconditioner(); } @@ -72,7 +73,11 @@ void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) for (int i = 0; i < m_lf.size(); ++i) { // add damping matrix - m_lf[i]->addScaledForceDifferential(-m_dt, x, b); + m_lf[i]->addScaledDampingForceDifferential(-m_dt, x, b); + if (m_implicit) + { + m_lf[i]->addScaledElasticForceDifferential(-m_dt*m_dt, x, b); + } } } @@ -105,14 +110,22 @@ void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero } } -void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual) const +void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &residual) { BT_PROFILE("computeResidual"); // add implicit force for (int i = 0; i < m_lf.size(); ++i) { - m_lf[i]->addScaledImplicitForce(dt, residual); + if (m_implicit) + { + m_lf[i]->addScaledForces(dt, residual); + } + else + { + m_lf[i]->addScaledDampingForce(dt, residual); + } } + projection.project(residual); } btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual) const @@ -122,7 +135,7 @@ btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual { norm_squared += residual[i].length2(); } - return std::sqrt(norm_squared+SIMD_EPSILON); + return std::sqrt(norm_squared); } void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force) diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h index e028564c5..e96738557 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.h +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.h @@ -37,6 +37,8 @@ public: btDeformableContactProjection projection; const TVStack& m_backupVelocity; btAlignedObjectArray m_nodes; + bool m_implicit; + btDeformableBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v); virtual ~btDeformableBackwardEulerObjective(); @@ -44,7 +46,7 @@ public: void initialize(){} // compute the rhs for CG solve, i.e, add the dt scaled implicit force to residual - void computeResidual(btScalar dt, TVStack& residual) const; + void computeResidual(btScalar dt, TVStack& residual); // add explicit force to the velocity void applyExplicitForce(TVStack& force); @@ -117,6 +119,11 @@ public: { return &m_nodes; } + + void setImplicit(bool implicit) + { + m_implicit = implicit; + } }; #endif /* btBackwardEulerObjective_h */ diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 431a07d28..30424e7c3 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -21,6 +21,8 @@ btDeformableBodySolver::btDeformableBodySolver() : m_numNodes(0) , m_cg(50) +, m_maxNewtonIterations(5) +, m_newtonTolerance(1e-10) { m_objective = new btDeformableBackwardEulerObjective(m_softBodySet, m_backupVelocity); } @@ -33,17 +35,70 @@ btDeformableBodySolver::~btDeformableBodySolver() void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt) { BT_PROFILE("solveConstraints"); - m_objective->computeResidual(solverdt, m_residual); - m_objective->applyDynamicFriction(m_residual); - computeStep(m_dv, m_residual); - - updateVelocity(); + if (!m_implicit) + { + m_objective->computeResidual(solverdt, m_residual); + m_objective->applyDynamicFriction(m_residual); + computeStep(m_dv, m_residual); + updateVelocity(); + } + else + { + for (int i = 0; i < m_maxNewtonIterations; ++i) + { + updateState(); + // add the inertia term in the residual + int counter = 0; + for (int k = 0; k < m_softBodySet.size(); ++k) + { + btSoftBody* psb = m_softBodySet[k]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (psb->m_nodes[j].m_im > 0) + { + m_residual[counter] = (-1./psb->m_nodes[j].m_im) * m_dv[counter]; + } + ++counter; + } + } + + m_objective->computeResidual(solverdt, m_residual); + if (m_objective->computeNorm(m_residual) < m_newtonTolerance) + { + break; + } +// m_objective->applyDynamicFriction(m_residual); + computeStep(m_ddv, m_residual); + updateDv(); + for (int j = 0; j < m_numNodes; ++j) + { + m_ddv[j].setZero(); + m_residual[j].setZero(); + } + } + } } -void btDeformableBodySolver::computeStep(TVStack& dv, const TVStack& residual) +void btDeformableBodySolver::updateState() { - btScalar tolerance = std::numeric_limits::epsilon() * 16 * m_objective->computeNorm(residual); - m_cg.solve(*m_objective, dv, residual, tolerance); + updateVelocity(); + updateTempPosition(); + +} + +void btDeformableBodySolver::updateDv() +{ + for (int i = 0; i < m_numNodes; ++i) + { + m_dv[i] += m_ddv[i]; + } +} + +void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual) +{ +// btScalar tolerance = std::numeric_limits::epsilon() * m_objective->computeNorm(residual); + btScalar tolerance = std::numeric_limits::epsilon(); + m_cg.solve(*m_objective, ddv, residual, tolerance); } void btDeformableBodySolver::reinitialize(const btAlignedObjectArray& softBodies, btScalar dt) @@ -54,6 +109,7 @@ void btDeformableBodySolver::reinitialize(const btAlignedObjectArrayreinitialize(nodeUpdated, dt); } @@ -103,6 +161,21 @@ void btDeformableBodySolver::updateVelocity() } } +void btDeformableBodySolver::updateTempPosition() +{ + int counter = 0; + for (int i = 0; i < m_softBodySet.size(); ++i) + { + btSoftBody* psb = m_softBodySet[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * psb->m_nodes[j].m_v; + ++counter; + } + psb->updateDeformation(); + } +} + void btDeformableBodySolver::backupVelocity() { int counter = 0; @@ -209,3 +282,9 @@ void btDeformableBodySolver::updateSoftBodies() } } } + +void btDeformableBodySolver::setImplicit(bool implicit) +{ + m_implicit = implicit; + m_objective->setImplicit(implicit); +} diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index a40d1fb99..a1107de03 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -34,6 +34,7 @@ class btDeformableBodySolver : public btSoftBodySolver protected: int m_numNodes; TVStack m_dv; + TVStack m_ddv; TVStack m_residual; btAlignedObjectArray m_softBodySet; @@ -41,7 +42,9 @@ protected: btScalar m_dt; btScalar m_contact_iterations; btConjugateGradient m_cg; - + bool m_implicit; + int m_maxNewtonIterations; + btScalar m_newtonTolerance; public: btDeformableBackwardEulerObjective* m_objective; @@ -94,7 +97,14 @@ public: virtual void optimize(btAlignedObjectArray &softBodies, bool forceUpdate = false){} virtual bool checkInitialized(){return true;} - + + void setImplicit(bool implicit); + + void updateState(); + + void updateDv(); + + void updateTempPosition(); }; #endif /* btDeformableBodySolver_h */ diff --git a/src/BulletSoftBody/btDeformableCorotatedForce.h b/src/BulletSoftBody/btDeformableCorotatedForce.h index 240dbb5d4..d7f18a1dc 100644 --- a/src/BulletSoftBody/btDeformableCorotatedForce.h +++ b/src/BulletSoftBody/btDeformableCorotatedForce.h @@ -39,8 +39,9 @@ public: { } - virtual void addScaledImplicitForce(btScalar scale, TVStack& force) + virtual void addScaledForces(btScalar scale, TVStack& force) { + addScaledElasticForce(scale, force); } virtual void addScaledExplicitForce(btScalar scale, TVStack& force) @@ -105,7 +106,11 @@ public: } } - virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + } + + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) { } diff --git a/src/BulletSoftBody/btDeformableGravityForce.h b/src/BulletSoftBody/btDeformableGravityForce.h index e238557b5..14d49ec1e 100644 --- a/src/BulletSoftBody/btDeformableGravityForce.h +++ b/src/BulletSoftBody/btDeformableGravityForce.h @@ -28,8 +28,9 @@ public: { } - virtual void addScaledImplicitForce(btScalar scale, TVStack& force) + virtual void addScaledForces(btScalar scale, TVStack& force) { + addScaledGravityForce(scale, force); } virtual void addScaledExplicitForce(btScalar scale, TVStack& force) @@ -37,9 +38,16 @@ public: addScaledGravityForce(scale, force); } - virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + virtual void addScaledDampingForce(btScalar scale, TVStack& force) + { + } + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + } + + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) { - } virtual void addScaledGravityForce(btScalar scale, TVStack& force) diff --git a/src/BulletSoftBody/btDeformableLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h index daea0e86a..3a5f5a5b1 100644 --- a/src/BulletSoftBody/btDeformableLagrangianForce.h +++ b/src/BulletSoftBody/btDeformableLagrangianForce.h @@ -18,6 +18,7 @@ #include "btSoftBody.h" #include +#include enum btDeformableLagrangianForceType { @@ -40,12 +41,21 @@ public: virtual ~btDeformableLagrangianForce(){} - virtual void addScaledImplicitForce(btScalar scale, TVStack& force) = 0; + // add all forces + virtual void addScaledForces(btScalar scale, TVStack& force) = 0; - virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0; + // add damping df + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) = 0; + // add elastic df + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) = 0; + + // add all forces that are explicit in explicit solve virtual void addScaledExplicitForce(btScalar scale, TVStack& force) = 0; + // add all damping forces + virtual void addScaledDampingForce(btScalar scale, TVStack& force) = 0; + virtual btDeformableLagrangianForceType getForceType() = 0; virtual void reinitialize(bool nodeUpdated) @@ -71,5 +81,261 @@ public: { m_nodes = nodes; } + + virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack& dx) + { + btVector3 c1 = dx[id1] - dx[id0]; + btVector3 c2 = dx[id2] - dx[id0]; + btVector3 c3 = dx[id3] - dx[id0]; + btMatrix3x3 dF(c1.getX(), c2.getX(), c3.getX(), + c1.getY(), c2.getY(), c3.getY(), + c1.getZ(), c2.getZ(), c3.getZ()); + return dF; + } + + virtual void testDerivative() + { + for (int i = 0; im_nodes.size(); ++j) + { + psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1)); + } + psb->updateDeformation(); + } + + + TVStack dx; + dx.resize(getNumNodes()); + TVStack dphi_dx; + dphi_dx.resize(dx.size()); + for (int i =0; i < dphi_dx.size();++i) + { + dphi_dx[i].setZero(); + } + addScaledForces(-1, dphi_dx); + + // write down the current position + TVStack x; + x.resize(dx.size()); + int counter = 0; + for (int i = 0; im_nodes.size(); ++j) + { + x[counter] = psb->m_nodes[j].m_q; + counter++; + } + } + counter = 0; + + // populate dx with random vectors + for (int i = 0; i < dx.size(); ++i) + { + dx[i].setX(randomDouble(-1, 1)); + dx[i].setY(randomDouble(-1, 1)); + dx[i].setZ(randomDouble(-1, 1)); + } + + btAlignedObjectArray errors; + double h = 1; + for (int it = 0; it < 10; ++it) + { + for (int i = 0; i < dx.size(); ++i) + { + dx[i] *= 0.5; + } + + // get dphi/dx * dx + double dphi = 0; + for (int i = 0; i < dx.size(); ++i) + { + dphi += dphi_dx[i].dot(dx[i]); + } + + + for (int i = 0; im_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter] + dx[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + double f1 = totalElasticEnergy(); + + for (int i = 0; im_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter] - dx[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + + double f2 = totalElasticEnergy(); + + //restore m_q + for (int i = 0; im_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + double error = f1-f2-2*dphi; + errors.push_back(error); + std::cout << "Iteration = " << it <<", f1 = " << f1 << ", f2 = " << f2 << ", error = " << error << std::endl; + } + for (int i = 1; i < errors.size(); ++i) + { + std::cout << "Iteration = " << i << ", ratio = " << errors[i-1]/errors[i] << std::endl; + } + } + + virtual void testHessian() + { + for (int i = 0; im_nodes.size(); ++j) + { + psb->m_nodes[j].m_q += btVector3(randomDouble(-.1, .1), randomDouble(-.1, .1), randomDouble(-.1, .1)); + } + psb->updateDeformation(); + } + + + TVStack dx; + dx.resize(getNumNodes()); + TVStack df; + df.resize(dx.size()); + TVStack f1; + f1.resize(dx.size()); + TVStack f2; + f2.resize(dx.size()); + + + // write down the current position + TVStack x; + x.resize(dx.size()); + int counter = 0; + for (int i = 0; im_nodes.size(); ++j) + { + x[counter] = psb->m_nodes[j].m_q; + counter++; + } + } + counter = 0; + + // populate dx with random vectors + for (int i = 0; i < dx.size(); ++i) + { + dx[i].setX(randomDouble(-1, 1)); + dx[i].setY(randomDouble(-1, 1)); + dx[i].setZ(randomDouble(-1, 1)); + } + + btAlignedObjectArray errors; + for (int it = 0; it < 10; ++it) + { + for (int i = 0; i < dx.size(); ++i) + { + dx[i] *= 0.5; + } + + // get df + for (int i =0; i < df.size();++i) + { + df[i].setZero(); + f1[i].setZero(); + f2[i].setZero(); + } + + //set df + addScaledElasticForceDifferential(-1, dx, df); + + for (int i = 0; im_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter] + dx[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + + //set f1 + addScaledForces(-1, f1); + + for (int i = 0; im_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter] - dx[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + + //set f2 + addScaledForces(-1, f2); + + //restore m_q + for (int i = 0; im_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = x[counter]; + counter++; + } + psb->updateDeformation(); + } + counter = 0; + double error = 0; + for (int i = 0; i < df.size();++i) + { + btVector3 error_vector = f1[i]-f2[i]-2*df[i]; + error += error_vector.length2(); + } + error = btSqrt(error); + errors.push_back(error); + std::cout << "Iteration = " << it << ", error = " << error << std::endl; + } + for (int i = 1; i < errors.size(); ++i) + { + std::cout << "Iteration = " << i << ", ratio = " << errors[i-1]/errors[i] << std::endl; + } + } + + virtual double totalElasticEnergy() + { + return 0; + } + + double randomDouble(double low, double high) + { + return low + static_cast(rand()) / RAND_MAX * (high - low); + } }; #endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */ diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h index f36e54e41..824f8a485 100644 --- a/src/BulletSoftBody/btDeformableMassSpringForce.h +++ b/src/BulletSoftBody/btDeformableMassSpringForce.h @@ -31,9 +31,10 @@ public: { } - virtual void addScaledImplicitForce(btScalar scale, TVStack& force) + virtual void addScaledForces(btScalar scale, TVStack& force) { addScaledDampingForce(scale, force); + addScaledElasticForce(scale, force); } virtual void addScaledExplicitForce(btScalar scale, TVStack& force) @@ -61,9 +62,9 @@ public: btVector3 scaled_force = scale * m_dampingStiffness * v_diff; if (m_momentum_conserving) { - if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) + if ((node2->m_q - node1->m_q).norm() > SIMD_EPSILON) { - btVector3 dir = (node2->m_x - node1->m_x).normalized(); + btVector3 dir = (node2->m_q - node1->m_q).normalized(); scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir; } } @@ -91,7 +92,7 @@ public: // elastic force // explicit elastic force - btVector3 dir = (node2->m_x - node1->m_x); + btVector3 dir = (node2->m_q - node1->m_q); btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0); btVector3 scaled_force = scale * m_elasticStiffness * (dir - dir_normalized * r); force[id1] += scaled_force; @@ -100,7 +101,7 @@ public: } } - virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) { // implicit damping force differential for (int i = 0; i < m_softBodies.size(); ++i) @@ -118,9 +119,9 @@ public: btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]); if (m_momentum_conserving) { - if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON) + if ((node2->m_q - node1->m_q).norm() > SIMD_EPSILON) { - btVector3 dir = (node2->m_x - node1->m_x).normalized(); + btVector3 dir = (node2->m_q - node1->m_q).normalized(); local_scaled_df= scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir; } } @@ -130,6 +131,34 @@ public: } } + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + // implicit damping force differential + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + btScalar scaled_k_damp = m_dampingStiffness * scale; + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + size_t id1 = node1->index; + size_t id2 = node2->index; + btScalar r = link.m_rl; + + // todo @xuchenhan: find df + btVector3 dir = (node2->m_q - node1->m_q); + btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0); + btVector3 scaled_force = scale * m_elasticStiffness * (dir - dir_normalized * r); + btVector3 local_scaled_df = btVector3(0,0,0); + + df[id1] += local_scaled_df; + df[id2] -= local_scaled_df; + } + } + } + virtual btDeformableLagrangianForceType getForceType() { return BT_MASSSPRING_FORCE; diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index c6a0417bc..7ddecdcd5 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -167,6 +167,7 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) } } node.m_x = node.m_x + timeStep * node.m_v; + node.m_q = node.m_x; } } m_deformableBodySolver->revertVelocity(); @@ -174,12 +175,23 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) { - // save v_{n+1}^* velocity after explicit forces - m_deformableBodySolver->backupVelocity(); + if (!m_implicit) + { + // save v_{n+1}^* velocity after explicit forces + m_deformableBodySolver->backupVelocity(); + } // set up constraints among multibodies and between multibodies and deformable bodies setupConstraints(); solveMultiBodyRelatedConstraints(); + + if (m_implicit) + { + // revert to v_n after collisions are registered + m_deformableBodySolver->revertVelocity(); + // todo @xuchenhan : think about whether contact solve should be done with v_n velocity or v_{n+1}^* velocity. It's using v_n for implicit and v_{n+1}^* for non-implicit now. + } + // At this point, dv is golden for nodes in contact m_deformableBodySolver->solveDeformableConstraints(timeStep); } @@ -273,7 +285,6 @@ void btDeformableMultiBodyDynamicsWorld::solveMultiBodyRelatedConstraints() } } - // todo : may not be necessary for (int i = 0; i < this->m_multiBodies.size(); i++) { btMultiBody* bod = m_multiBodies[i]; @@ -304,7 +315,13 @@ void btDeformableMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar time void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep) { m_internalTime += timeStep; + m_deformableBodySolver->setImplicit(m_implicit); m_deformableBodySolver->reinitialize(m_softBodies, timeStep); + if (m_implicit) + { + // backup v_n velocity + m_deformableBodySolver->backupVelocity(); + } btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo(); dispatchInfo.m_timeStep = timeStep; dispatchInfo.m_stepCount = 0; diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index a91944c55..219cbfb27 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -47,6 +47,7 @@ class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld btSoftBodyWorldInfo m_sbi; btScalar m_internalTime; int m_contact_iterations; + bool m_implicit; typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world); btSolverCallback m_solverCallback; @@ -72,7 +73,7 @@ public: m_sbi.m_broadphase = pairCache; m_sbi.m_dispatcher = dispatcher; m_sbi.m_sparsesdf.Initialize(); - m_sbi.m_sparsesdf.setDefaultVoxelsz(0.025); + m_sbi.m_sparsesdf.setDefaultVoxelsz(0.0025); m_sbi.m_sparsesdf.Reset(); m_sbi.air_density = (btScalar)1.2; @@ -81,6 +82,7 @@ public: m_sbi.water_normal = btVector3(0, 0, 0); m_sbi.m_gravity.setValue(0, -10, 0); m_internalTime = 0.0; + m_implicit = false; } void setSolverCallback(btSolverCallback cb) @@ -153,6 +155,11 @@ public: void solveMultiBodyRelatedConstraints(); void sortConstraints(); + + void setImplicit(bool implicit) + { + m_implicit = implicit; + } }; #endif //BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H diff --git a/src/BulletSoftBody/btDeformableNeoHookeanForce.h b/src/BulletSoftBody/btDeformableNeoHookeanForce.h index 2431cd750..c8787c251 100644 --- a/src/BulletSoftBody/btDeformableNeoHookeanForce.h +++ b/src/BulletSoftBody/btDeformableNeoHookeanForce.h @@ -25,16 +25,16 @@ public: btScalar m_mu, m_lambda; btDeformableNeoHookeanForce(): m_mu(1), m_lambda(1) { - } btDeformableNeoHookeanForce(btScalar mu, btScalar lambda): m_mu(mu), m_lambda(lambda) { } - virtual void addScaledImplicitForce(btScalar scale, TVStack& force) + virtual void addScaledForces(btScalar scale, TVStack& force) { addScaledDampingForce(scale, force); + addScaledElasticForce(scale, force); } virtual void addScaledExplicitForce(btScalar scale, TVStack& force) @@ -46,6 +46,34 @@ public: { } + virtual double totalElasticEnergy() + { + double energy = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + energy += tetra.m_element_measure * elasticEnergyDensity(tetra); + } + } + return energy; + } + + double elasticEnergyDensity(const btSoftBody::Tetra& t) + { + double density = 0; + btMatrix3x3 F = t.m_F; + btMatrix3x3 C = F.transpose()*F; + double J = F.determinant(); + double trace = C[0].getX() + C[1].getY() + C[2].getZ(); + density += m_mu * 0.5 * (trace - 3.); + density += m_lambda * 0.5 * (J - 1. - 0.75 * m_mu / m_lambda)* (J - 1. - 0.75 * m_mu / m_lambda); + density -= m_mu * 0.5 * log(trace+1); + return density; + } + virtual void addScaledElasticForce(btScalar scale, TVStack& force) { int numNodes = getNumNodes(); @@ -72,7 +100,6 @@ public: size_t id3 = node3->index; // elastic force - // explicit elastic force btScalar scale1 = scale * tetra.m_element_measure; force[id0] -= scale1 * force_on_node0; force[id1] -= scale1 * force_on_node123.getColumn(0); @@ -82,25 +109,62 @@ public: } } + virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) + { + } + + virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) + { + int numNodes = getNumNodes(); + btAssert(numNodes <= df.size()) + btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse; + btMatrix3x3 dP; + firstPiolaDifferential(tetra.m_F, dF, dP); + btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + + // elastic force differential + btScalar scale1 = scale * tetra.m_element_measure; + df[id0] -= scale1 * df_on_node0; + df[id1] -= scale1 * df_on_node123.getColumn(0); + df[id2] -= scale1 * df_on_node123.getColumn(1); + df[id3] -= scale1 * df_on_node123.getColumn(2); + } + } + } + void firstPiola(const btMatrix3x3& F, btMatrix3x3& P) { btMatrix3x3 C = F.transpose()*F; btScalar J = F.determinant(); btScalar trace = C[0].getX() + C[1].getY() + C[2].getZ(); - P = F * m_mu * ( 1. - 1. / (trace + 1.)) + F.adjoint().transpose() * (m_lambda * (J - 1) - 0.75 * m_mu); - } - - virtual void addScaledForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) - { + P = F * m_mu * ( 1. - 1. / (trace + 1.)) + F.adjoint().transpose() * (m_lambda * (J - 1.) - 0.75 * m_mu); } void firstPiolaDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btMatrix3x3& dP) { btScalar J = F.determinant(); - addScaledCofactorMatrixDifferential(F, dF, m_lambda*(J-1) - 0.75*m_mu, dP); - dP += F.adjoint().transpose() * m_lambda * DotProduct(F.adjoint().transpose(), dF); + btMatrix3x3 C = F.transpose()*F; + btScalar trace = C[0].getX() + C[1].getY() + C[2].getZ(); + dP = dF * m_mu * ( 1. - 1. / (trace + 1.)) + F * (2*m_mu) * DotProduct(F, dF) * (1./((1.+trace)*(1.+trace))); - //todo @xuchenhan: add the dP of the m_mu term. + addScaledCofactorMatrixDifferential(F, dF, m_lambda*(J-1.) - 0.75*m_mu, dP); + dP += F.adjoint().transpose() * m_lambda * DotProduct(F.adjoint().transpose(), dF); } btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B) @@ -108,25 +172,26 @@ public: btScalar ans = 0; for (int i = 0; i < 3; ++i) { - for (int j = 0; j < 3; ++j) - { - ans += A[i][j] * B[i][j]; - } + ans += A[i].dot(B[i]); +// for (int j = 0; j < 3; ++j) +// { +// ans += A[i][j] * B[i][j]; +// } } return ans; } void addScaledCofactorMatrixDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btScalar scale, btMatrix3x3& M) { - M[0][0] = scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]); - M[1][0] = scale * (dF[2][1] * F[0][2] + F[2][1] * dF[0][2] - dF[0][1] * F[2][2] - F[0][1] * dF[2][2]); - M[2][0] = scale * (dF[0][1] * F[1][2] + F[0][1] * dF[1][2] - dF[1][1] * F[0][2] - F[1][1] * dF[0][2]); - M[0][1] = scale * (dF[2][0] * F[1][2] + F[2][0] * dF[1][2] - dF[1][0] * F[2][2] - F[1][0] * dF[2][2]); - M[1][1] = scale * (dF[0][0] * F[2][2] + F[0][0] * dF[2][2] - dF[2][0] * F[0][2] - F[2][0] * dF[0][2]); - M[2][1] = scale * (dF[1][0] * F[0][2] + F[1][0] * dF[0][2] - dF[0][0] * F[1][2] - F[0][0] * dF[1][2]); - M[0][2] = scale * (dF[1][0] * F[2][1] + F[1][0] * dF[2][1] - dF[2][0] * F[1][1] - F[2][0] * dF[1][1]); - M[1][2] = scale * (dF[2][0] * F[0][1] + F[2][0] * dF[0][1] - dF[0][0] * F[2][1] - F[0][0] * dF[2][1]); - M[2][2] = scale * (dF[0][0] * F[1][1] + F[0][0] * dF[1][1] - dF[1][0] * F[0][1] - F[1][0] * dF[0][1]); + M[0][0] += scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]); + M[1][0] += scale * (dF[2][1] * F[0][2] + F[2][1] * dF[0][2] - dF[0][1] * F[2][2] - F[0][1] * dF[2][2]); + M[2][0] += scale * (dF[0][1] * F[1][2] + F[0][1] * dF[1][2] - dF[1][1] * F[0][2] - F[1][1] * dF[0][2]); + M[0][1] += scale * (dF[2][0] * F[1][2] + F[2][0] * dF[1][2] - dF[1][0] * F[2][2] - F[1][0] * dF[2][2]); + M[1][1] += scale * (dF[0][0] * F[2][2] + F[0][0] * dF[2][2] - dF[2][0] * F[0][2] - F[2][0] * dF[0][2]); + M[2][1] += scale * (dF[1][0] * F[0][2] + F[1][0] * dF[0][2] - dF[0][0] * F[1][2] - F[0][0] * dF[1][2]); + M[0][2] += scale * (dF[1][0] * F[2][1] + F[1][0] * dF[2][1] - dF[2][0] * F[1][1] - F[2][0] * dF[1][1]); + M[1][2] += scale * (dF[2][0] * F[0][1] + F[2][0] * dF[0][1] - dF[0][0] * F[2][1] - F[0][0] * dF[2][1]); + M[2][2] += scale * (dF[0][0] * F[1][1] + F[0][0] * dF[1][1] - dF[1][0] * F[0][1] - F[1][0] * dF[0][1]); } virtual btDeformableLagrangianForceType getForceType() diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index c4de6a823..eb630cbfc 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -2834,12 +2834,10 @@ void btSoftBody::updateDeformation() { for (int i = 0; i < m_tetras.size(); ++i) { - // updateDeformation is called before predictMotion where m_q is sync'd. - // So m_x is the current position of the node. btSoftBody::Tetra& t = m_tetras[i]; - btVector3 c1 = t.m_n[1]->m_x - t.m_n[0]->m_x; - btVector3 c2 = t.m_n[2]->m_x - t.m_n[0]->m_x; - btVector3 c3 = t.m_n[3]->m_x - t.m_n[0]->m_x; + btVector3 c1 = t.m_n[1]->m_q - t.m_n[0]->m_q; + btVector3 c2 = t.m_n[2]->m_q - t.m_n[0]->m_q; + btVector3 c3 = t.m_n[3]->m_q - t.m_n[0]->m_q; btMatrix3x3 Ds(c1.getX(), c2.getX(), c3.getX(), c1.getY(), c2.getY(), c3.getY(), c1.getZ(), c2.getZ(), c3.getZ()); From 5826492020885fd1d87887e260d42376be7e74b8 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Tue, 27 Aug 2019 21:43:33 -0700 Subject: [PATCH 06/18] add elastic force differential for mass spring --- .../btDeformableLagrangianForce.h | 2 - .../btDeformableMassSpringForce.h | 45 ++++++++++++++----- 2 files changed, 35 insertions(+), 12 deletions(-) diff --git a/src/BulletSoftBody/btDeformableLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h index 3a5f5a5b1..0e349e5be 100644 --- a/src/BulletSoftBody/btDeformableLagrangianForce.h +++ b/src/BulletSoftBody/btDeformableLagrangianForce.h @@ -105,7 +105,6 @@ public: psb->updateDeformation(); } - TVStack dx; dx.resize(getNumNodes()); TVStack dphi_dx; @@ -140,7 +139,6 @@ public: } btAlignedObjectArray errors; - double h = 1; for (int it = 0; it < 10; ++it) { for (int i = 0; i < dx.size(); ++i) diff --git a/src/BulletSoftBody/btDeformableMassSpringForce.h b/src/BulletSoftBody/btDeformableMassSpringForce.h index 824f8a485..a85dd547a 100644 --- a/src/BulletSoftBody/btDeformableMassSpringForce.h +++ b/src/BulletSoftBody/btDeformableMassSpringForce.h @@ -91,7 +91,6 @@ public: size_t id2 = node2->index; // elastic force - // explicit elastic force btVector3 dir = (node2->m_q - node1->m_q); btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0); btVector3 scaled_force = scale * m_elasticStiffness * (dir - dir_normalized * r); @@ -106,7 +105,7 @@ public: // implicit damping force differential for (int i = 0; i < m_softBodies.size(); ++i) { - const btSoftBody* psb = m_softBodies[i]; + btSoftBody* psb = m_softBodies[i]; btScalar scaled_k_damp = m_dampingStiffness * scale; for (int j = 0; j < psb->m_links.size(); ++j) { @@ -130,6 +129,26 @@ public: } } } + virtual double totalElasticEnergy() + { + double energy = 0; + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_links.size(); ++j) + { + const btSoftBody::Link& link = psb->m_links[j]; + btSoftBody::Node* node1 = link.m_n[0]; + btSoftBody::Node* node2 = link.m_n[1]; + btScalar r = link.m_rl; + + // elastic force + btVector3 dir = (node2->m_q - node1->m_q); + energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() -r ); + } + } + return energy; + } virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) { @@ -137,7 +156,7 @@ public: for (int i = 0; i < m_softBodies.size(); ++i) { const btSoftBody* psb = m_softBodies[i]; - btScalar scaled_k_damp = m_dampingStiffness * scale; + btScalar scaled_k = m_elasticStiffness * scale; for (int j = 0; j < psb->m_links.size(); ++j) { const btSoftBody::Link& link = psb->m_links[j]; @@ -147,14 +166,20 @@ public: size_t id2 = node2->index; btScalar r = link.m_rl; - // todo @xuchenhan: find df - btVector3 dir = (node2->m_q - node1->m_q); - btVector3 dir_normalized = (dir.norm() > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0); - btVector3 scaled_force = scale * m_elasticStiffness * (dir - dir_normalized * r); - btVector3 local_scaled_df = btVector3(0,0,0); + btVector3 dir = (node1->m_q - node2->m_q); + btScalar dir_norm = dir.norm(); + btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0,0,0); + btVector3 dx_diff = dx[id1] - dx[id2]; + btVector3 scaled_df = btVector3(0,0,0); + if (dir_norm > SIMD_EPSILON) + { + scaled_df -= scaled_k * dir_normalized.dot(dx_diff) * dir_normalized; + scaled_df += scaled_k * dir_normalized.dot(dx_diff) * ((dir_norm-r)/dir_norm) * dir_normalized; + scaled_df -= scaled_k * ((dir_norm-r)/dir_norm) * dx_diff; + } - df[id1] += local_scaled_df; - df[id2] -= local_scaled_df; + df[id1] += scaled_df; + df[id2] -= scaled_df; } } } From 7d1b93cc1766c64cc4a510a3ce29598f6614676a Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 28 Aug 2019 10:01:14 -0700 Subject: [PATCH 07/18] contact solve for newton --- data/single_tet.vtk | 15 ++++++++++ examples/DeformableDemo/DeformableRigid.cpp | 4 +-- examples/DeformableDemo/GraspDeformable.cpp | 24 +++++++-------- .../btDeformableBackwardEulerObjective.cpp | 2 +- src/BulletSoftBody/btDeformableBodySolver.cpp | 19 ++++++++++-- src/BulletSoftBody/btDeformableBodySolver.h | 1 + .../btDeformableMultiBodyDynamicsWorld.cpp | 29 +++++++++---------- .../btDeformableMultiBodyDynamicsWorld.h | 4 +-- src/BulletSoftBody/btSoftBody.h | 3 +- 9 files changed, 63 insertions(+), 38 deletions(-) create mode 100644 data/single_tet.vtk diff --git a/data/single_tet.vtk b/data/single_tet.vtk new file mode 100644 index 000000000..94cf9fe64 --- /dev/null +++ b/data/single_tet.vtk @@ -0,0 +1,15 @@ +# vtk DataFile Version 2.0 +ball_, Created by Gmsh +ASCII +DATASET UNSTRUCTURED_GRID +POINTS 4 double +0 0 0 +1 0 0 +0 1 0 +0 0 1 + +CELLS 1 1 +4 0 1 2 3 + +CELL_TYPES 1 +10 diff --git a/examples/DeformableDemo/DeformableRigid.cpp b/examples/DeformableDemo/DeformableRigid.cpp index 3cbfba451..850f72600 100644 --- a/examples/DeformableDemo/DeformableRigid.cpp +++ b/examples/DeformableDemo/DeformableRigid.cpp @@ -234,11 +234,11 @@ void DeformableRigid::initPhysics() psb->setTotalMass(1); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body - psb->m_cfg.kDF = 1; + psb->m_cfg.kDF = 2; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; getDeformableDynamicsWorld()->addSoftBody(psb); - btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2,0.01, false); + btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2,0.1, true); getDeformableDynamicsWorld()->addForce(psb, mass_spring); m_forces.push_back(mass_spring); diff --git a/examples/DeformableDemo/GraspDeformable.cpp b/examples/DeformableDemo/GraspDeformable.cpp index c97aa0d3c..34127774d 100644 --- a/examples/DeformableDemo/GraspDeformable.cpp +++ b/examples/DeformableDemo/GraspDeformable.cpp @@ -325,7 +325,8 @@ void GraspDeformable::initPhysics() { char relative_path[1024]; // b3FileUtils::findFile("banana.vtk", relative_path, 1024); - b3FileUtils::findFile("ball.vtk", relative_path, 1024); + b3FileUtils::findFile("ball.vtk", relative_path, 1024); +// b3FileUtils::findFile("single_tet.vtk", relative_path, 1024); // b3FileUtils::findFile("tube.vtk", relative_path, 1024); // b3FileUtils::findFile("torus.vtk", relative_path, 1024); // b3FileUtils::findFile("paper_roll.vtk", relative_path, 1024); @@ -343,21 +344,16 @@ void GraspDeformable::initPhysics() psb->scale(btVector3(.25, .25, .25)); // psb->scale(btVector3(.3, .3, .3)); // for tube, torus, boot // psb->scale(btVector3(1, 1, 1)); // for ditto -// psb->translate(btVector3(0, 0, 2)); for boot - psb->getCollisionShape()->setMargin(0.01); + psb->translate(btVector3(.25, 0, 0.4)); + psb->getCollisionShape()->setMargin(0.02); psb->setTotalMass(.1); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body psb->m_cfg.kDF = 2; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; getDeformableDynamicsWorld()->addSoftBody(psb); - psb->getWorldInfo()->m_maxDisplacement = .1f; - // nonlinear damping -// getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(1,0.04, true)); -// getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity)); -// getDeformableDynamicsWorld()->addForce(psb, new btDeformableCorotatedForce(0,6)); - btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(.0,0.04, true); + btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(.0,.04, true); getDeformableDynamicsWorld()->addForce(psb, mass_spring); m_forces.push_back(mass_spring); @@ -365,7 +361,7 @@ void GraspDeformable::initPhysics() getDeformableDynamicsWorld()->addForce(psb, gravity_force); m_forces.push_back(gravity_force); - btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(2,10); + btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(5,10); getDeformableDynamicsWorld()->addForce(psb, neohookean); m_forces.push_back(neohookean); } @@ -415,8 +411,8 @@ void GraspDeformable::initPhysics() { SliderParams slider("Closing velocity", &sGripperClosingTargetVelocity); - slider.m_minVal = -.1; - slider.m_maxVal = .1; + slider.m_minVal = -.3; + slider.m_maxVal = .3; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } @@ -470,8 +466,8 @@ btMultiBody* GraspDeformable::createFeatherstoneMultiBody(btMultiBodyDynamicsWor { //init the base btVector3 baseInertiaDiag(0.f, 0.f, 0.f); - float baseMass = 1.f; - float linkMass = 1.f; + float baseMass = 100.f; + float linkMass = 100.f; int numLinks = 2; if (baseMass) diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index 2a2ff5c63..b66a02aff 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -83,7 +83,7 @@ void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b) void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv) { - // only the velocity of the constrained nodes needs to be updated during CG solve + // only the velocity of the constrained nodes needs to be updated during contact solve for (int i = 0; i < projection.m_constraints.size(); ++i) { int index = projection.m_constraints.getKeyAtIndex(i).getUid1(); diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 30424e7c3..600dbafa4 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -67,7 +67,7 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt) { break; } -// m_objective->applyDynamicFriction(m_residual); + m_objective->applyDynamicFriction(m_residual); computeStep(m_ddv, m_residual); updateDv(); for (int j = 0; j < m_numNodes; ++j) @@ -189,6 +189,20 @@ void btDeformableBodySolver::backupVelocity() } } +void btDeformableBodySolver::backupVn() +{ + int counter = 0; + for (int i = 0; i < m_softBodySet.size(); ++i) + { + btSoftBody* psb = m_softBodySet[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + m_dv[counter] += m_backupVelocity[counter] - psb->m_nodes[j].m_vn; + m_backupVelocity[counter++] = psb->m_nodes[j].m_vn; + } + } +} + void btDeformableBodySolver::revertVelocity() { int counter = 0; @@ -246,8 +260,7 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d for (i = 0, ni = psb->m_nodes.size(); i < ni; ++i) { btSoftBody::Node& n = psb->m_nodes[i]; - n.m_q = n.m_x; - n.m_q += n.m_v * dt; + n.m_q = n.m_x + n.m_v * dt; } /* Bounds */ psb->updateBounds(); diff --git a/src/BulletSoftBody/btDeformableBodySolver.h b/src/BulletSoftBody/btDeformableBodySolver.h index a1107de03..fed15b2b2 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.h +++ b/src/BulletSoftBody/btDeformableBodySolver.h @@ -75,6 +75,7 @@ public: void predictDeformableMotion(btSoftBody* psb, btScalar dt); void backupVelocity(); + void backupVn(); void revertVelocity(); void updateVelocity(); diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 7ddecdcd5..3fff883c9 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -168,6 +168,7 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) } node.m_x = node.m_x + timeStep * node.m_v; node.m_q = node.m_x; + node.m_vn = node.m_v; } } m_deformableBodySolver->revertVelocity(); @@ -175,11 +176,8 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) { - if (!m_implicit) - { - // save v_{n+1}^* velocity after explicit forces - m_deformableBodySolver->backupVelocity(); - } + // save v_{n+1}^* velocity after explicit forces + m_deformableBodySolver->backupVelocity(); // set up constraints among multibodies and between multibodies and deformable bodies setupConstraints(); @@ -187,11 +185,13 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) if (m_implicit) { - // revert to v_n after collisions are registered - m_deformableBodySolver->revertVelocity(); - // todo @xuchenhan : think about whether contact solve should be done with v_n velocity or v_{n+1}^* velocity. It's using v_n for implicit and v_{n+1}^* for non-implicit now. + // at this point dv = v_{n+1} - v_{n+1}^* + // modify dv such that dv = v_{n+1} - v_n + // modify m_backupVelocity so that it stores v_n instead of v_{n+1}^* as needed by Newton + m_deformableBodySolver->backupVn(); } - // At this point, dv is golden for nodes in contact + + // At this point, dv should be golden for nodes in contact m_deformableBodySolver->solveDeformableConstraints(timeStep); } @@ -211,7 +211,6 @@ void btDeformableMultiBodyDynamicsWorld::setupConstraints() // build islands m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld()); - // write the constraint information of each island to the callback, and also setup the constraints in solver } } @@ -317,11 +316,11 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep) m_internalTime += timeStep; m_deformableBodySolver->setImplicit(m_implicit); m_deformableBodySolver->reinitialize(m_softBodies, timeStep); - if (m_implicit) - { - // backup v_n velocity - m_deformableBodySolver->backupVelocity(); - } +// if (m_implicit) +// { +// // todo: backup v_n velocity somewhere else +// m_deformableBodySolver->backupVelocity(); +// } btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo(); dispatchInfo.m_timeStep = timeStep; dispatchInfo.m_stepCount = 0; diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index 219cbfb27..972f05664 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -73,7 +73,7 @@ public: m_sbi.m_broadphase = pairCache; m_sbi.m_dispatcher = dispatcher; m_sbi.m_sparsesdf.Initialize(); - m_sbi.m_sparsesdf.setDefaultVoxelsz(0.0025); + m_sbi.m_sparsesdf.setDefaultVoxelsz(0.025); m_sbi.m_sparsesdf.Reset(); m_sbi.air_density = (btScalar)1.2; @@ -82,7 +82,7 @@ public: m_sbi.water_normal = btVector3(0, 0, 0); m_sbi.m_gravity.setValue(0, -10, 0); m_internalTime = 0.0; - m_implicit = false; + m_implicit = true; } void setSolverCallback(btSolverCallback cb) diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index cd235c29d..5371e172d 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -251,8 +251,9 @@ public: struct Node : Feature { btVector3 m_x; // Position - btVector3 m_q; // Previous step position + btVector3 m_q; // Previous step position/Test position btVector3 m_v; // Velocity + btVector3 m_vn; // Previous step velocity btVector3 m_f; // Force accumulator btVector3 m_n; // Normal btScalar m_im; // 1/mass From 482458c9df8f107242c518b6437764026a3246aa Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 28 Aug 2019 10:37:27 -0700 Subject: [PATCH 08/18] improve deformable objects loading --- .../PhysicsServerCommandProcessor.cpp | 36 +++++++++---------- 1 file changed, 17 insertions(+), 19 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 47a4de27a..e3d1d57c5 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -105,7 +105,6 @@ #include "BulletSoftBody/btDeformableBodySolver.h" #include "BulletSoftBody/btDeformableMultiBodyConstraintSolver.h" #include "../SoftDemo/BunnyMesh.h" -#define SKIP_DEFORMABLE_BODY #else #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #endif @@ -8040,6 +8039,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type); btSoftBody* psb = NULL; + btScalar spring_elastic_stiffness, spring_damping_stiffness; if (out_type == UrdfGeometry::FILE_OBJ) { std::vector shapes; @@ -8064,14 +8064,20 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar psb = btSoftBodyHelpers::CreateFromTriMesh(m_data->m_dynamicsWorld->getWorldInfo(), &vertices[0], &indices[0], numTris); } } +#ifndef SKIP_DEFORMABLE_BODY + 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)); + } +#endif } - else if (out_type = UrdfGeometry::FILE_VTK) + 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; @@ -8085,26 +8091,18 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar neohookean_lambda = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanLambda; m_data->m_dynamicsWorld->addForce(psb, new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda)); } + 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; + m_data->m_dynamicsWorld->addForce(psb, new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true)); + } #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) { From e124c62a705a8e9e8137daeb9e59fb0f54f234ad Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 29 Aug 2019 10:08:34 -0700 Subject: [PATCH 09/18] each node only allows a single contact; initial guess for newton set to x = x_n + dt*v_n --- src/BulletSoftBody/btCGProjection.h | 10 ++++++++++ src/BulletSoftBody/btDeformableBodySolver.cpp | 13 +++++++++++-- .../btDeformableContactProjection.cpp | 13 ++++++++++++- src/BulletSoftBody/btSoftBodyInternals.h | 2 +- 4 files changed, 34 insertions(+), 4 deletions(-) diff --git a/src/BulletSoftBody/btCGProjection.h b/src/BulletSoftBody/btCGProjection.h index d074ca6ed..9055ad5ce 100644 --- a/src/BulletSoftBody/btCGProjection.h +++ b/src/BulletSoftBody/btCGProjection.h @@ -49,6 +49,16 @@ struct DeformableContactConstraint m_static.push_back(false); m_can_be_dynamic.push_back(true); } + + void replace(const btSoftBody::RContact& rcontact) + { + m_contact.clear(); + m_total_normal_dv.clear(); + m_total_tangent_dv.clear(); + m_static.clear(); + m_can_be_dynamic.clear(); + append(rcontact); + } ~DeformableContactConstraint() { diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 600dbafa4..7fc8aa99e 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -96,7 +96,7 @@ void btDeformableBodySolver::updateDv() void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual) { -// btScalar tolerance = std::numeric_limits::epsilon() * m_objective->computeNorm(residual); + //btScalar tolerance = std::numeric_limits::epsilon() * m_objective->computeNorm(residual); btScalar tolerance = std::numeric_limits::epsilon(); m_cg.solve(*m_objective, ddv, residual, tolerance); } @@ -197,7 +197,16 @@ void btDeformableBodySolver::backupVn() btSoftBody* psb = m_softBodySet[i]; for (int j = 0; j < psb->m_nodes.size(); ++j) { - m_dv[counter] += m_backupVelocity[counter] - psb->m_nodes[j].m_vn; + // Here: + // dv = 0 for nodes not in constraints + // dv = v_{n+1} - v_{n+1}^* for nodes in constraints + if (m_objective->projection.m_constraints.find(psb->m_nodes[j].index)!=NULL) + { + m_dv[counter] += m_backupVelocity[counter] - psb->m_nodes[j].m_vn; + } + // Now: + // dv = 0 for nodes not in constraints + // dv = v_{n+1} - v_n for nodes in constraints m_backupVelocity[counter++] = psb->m_nodes[j].m_vn; } } diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 33feb4bc6..ffd91d5df 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -20,6 +20,7 @@ btScalar btDeformableContactProjection::update() { btScalar residualSquare = 0; + btScalar max_impulse = 0; // loop through constraints to set constrained values for (int index = 0; index < m_constraints.size(); ++index) { @@ -128,6 +129,7 @@ btScalar btDeformableContactProjection::update() } } impulse = impulse_normal + impulse_tangent; + max_impulse = btMax(impulse.length2(), max_impulse); // dn is the normal component of velocity diffrerence. Approximates the residual. residualSquare = btMax(residualSquare, dn*dn); @@ -231,7 +233,16 @@ void btDeformableContactProjection::setConstraints() else { DeformableContactConstraint& constraints = *m_constraints[c.m_node->index]; - constraints.append(c); + bool single_contact = true; + if (single_contact) + { + constraints.m_contact[0]->m_cti.m_offset > cti.m_offset; + constraints.replace(c); + } + else + { + constraints.append(c); + } } } } diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index fc4c6b556..8ce114fa8 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -998,7 +998,7 @@ struct btSoftColliders if (!n.m_battach) { // check for collision at x_{n+1}^* - if (psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true)) + if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ true) || psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true)) { const btScalar ima = n.m_im; const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f; From ca3e25d4e2aea554b7926ce885ebb3e1cc52479a Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 29 Aug 2019 10:29:56 -0700 Subject: [PATCH 10/18] add Rayleigh damping for NeoHookean --- .../btDeformableLagrangianForce.h | 11 +++ .../btDeformableNeoHookeanForce.h | 85 +++++++++++++++++-- 2 files changed, 91 insertions(+), 5 deletions(-) diff --git a/src/BulletSoftBody/btDeformableLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h index 0e349e5be..7ce72a7fd 100644 --- a/src/BulletSoftBody/btDeformableLagrangianForce.h +++ b/src/BulletSoftBody/btDeformableLagrangianForce.h @@ -93,6 +93,17 @@ public: return dF; } + virtual btMatrix3x3 DsFromVelocity(const btSoftBody::Node* n0, const btSoftBody::Node* n1, const btSoftBody::Node* n2, const btSoftBody::Node* n3) + { + btVector3 c1 = n1->m_v - n0->m_v; + btVector3 c2 = n2->m_v - n0->m_v; + btVector3 c3 = n3->m_v - n0->m_v; + btMatrix3x3 dF(c1.getX(), c2.getX(), c3.getX(), + c1.getY(), c2.getY(), c3.getY(), + c1.getZ(), c2.getZ(), c3.getZ()); + return dF; + } + virtual void testDerivative() { for (int i = 0; i TVStack; btScalar m_mu, m_lambda; + btScalar m_mu_damp, m_lambda_damp; btDeformableNeoHookeanForce(): m_mu(1), m_lambda(1) { + btScalar damping = 0.005; + m_mu_damp = damping * m_mu; + m_lambda_damp = damping * m_lambda; } - btDeformableNeoHookeanForce(btScalar mu, btScalar lambda): m_mu(mu), m_lambda(lambda) + btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.005): m_mu(mu), m_lambda(lambda) { + m_mu_damp = damping * m_mu; + m_lambda_damp = damping * m_lambda; } virtual void addScaledForces(btScalar scale, TVStack& force) @@ -44,6 +50,37 @@ public: virtual void addScaledDampingForce(btScalar scale, TVStack& force) { + int numNodes = getNumNodes(); + btAssert(numNodes <= force.size()) + btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse; + btMatrix3x3 dP; + firstPiolaDampingDifferential(tetra.m_F, dF, dP); + btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + + // damping force differential + btScalar scale1 = scale * tetra.m_element_measure; + force[id0] -= scale1 * df_on_node0; + force[id1] -= scale1 * df_on_node123.getColumn(0); + force[id2] -= scale1 * df_on_node123.getColumn(1); + force[id3] -= scale1 * df_on_node123.getColumn(2); + } + } } virtual double totalElasticEnergy() @@ -111,6 +148,37 @@ public: virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) { + int numNodes = getNumNodes(); + btAssert(numNodes <= df.size()) + btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btSoftBody::Node* node0 = tetra.m_n[0]; + btSoftBody::Node* node1 = tetra.m_n[1]; + btSoftBody::Node* node2 = tetra.m_n[2]; + btSoftBody::Node* node3 = tetra.m_n[3]; + size_t id0 = node0->index; + size_t id1 = node1->index; + size_t id2 = node2->index; + size_t id3 = node3->index; + btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse; + btMatrix3x3 dP; + firstPiolaDampingDifferential(tetra.m_F, dF, dP); + btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + + // damping force differential + btScalar scale1 = scale * tetra.m_element_measure; + df[id0] -= scale1 * df_on_node0; + df[id1] -= scale1 * df_on_node123.getColumn(0); + df[id2] -= scale1 * df_on_node123.getColumn(1); + df[id3] -= scale1 * df_on_node123.getColumn(2); + } + } } virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) @@ -167,16 +235,23 @@ public: dP += F.adjoint().transpose() * m_lambda * DotProduct(F.adjoint().transpose(), dF); } + void firstPiolaDampingDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btMatrix3x3& dP) + { + btScalar J = F.determinant(); + btMatrix3x3 C = F.transpose()*F; + btScalar trace = C[0].getX() + C[1].getY() + C[2].getZ(); + dP = dF * m_mu_damp * ( 1. - 1. / (trace + 1.)) + F * (2*m_mu_damp) * DotProduct(F, dF) * (1./((1.+trace)*(1.+trace))); + + addScaledCofactorMatrixDifferential(F, dF, m_lambda_damp*(J-1.) - 0.75*m_mu_damp, dP); + dP += F.adjoint().transpose() * m_lambda_damp * DotProduct(F.adjoint().transpose(), dF); + } + btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B) { btScalar ans = 0; for (int i = 0; i < 3; ++i) { ans += A[i].dot(B[i]); -// for (int j = 0; j < 3; ++j) -// { -// ans += A[i][j] * B[i][j]; -// } } return ans; } From f392d8ceb1056bf91c07a8c2a077216bee9678b7 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 29 Aug 2019 10:32:55 -0700 Subject: [PATCH 11/18] clean up memory when exiting --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index e3d1d57c5..ccfd4eab9 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2870,6 +2870,11 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() delete m_data->m_remoteDebugDrawer; m_data->m_remoteDebugDrawer = 0; +#ifndef SKIP_DEFORMABLE_BODY + delete m_data->m_deformablebodySolver; + m_data->m_deformablebodySolver = 0; +#endif + delete m_data->m_solver; m_data->m_solver = 0; @@ -10392,14 +10397,10 @@ 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; From 5330396c70fe671c01162c3e1a1dc2420ba60705 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 29 Aug 2019 11:12:35 -0700 Subject: [PATCH 12/18] enabled mass preconditioner --- src/BulletSoftBody/btConjugateGradient.h | 2 ++ src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp | 3 ++- src/BulletSoftBody/btDeformableBodySolver.cpp | 4 ++-- src/BulletSoftBody/btDeformableContactProjection.cpp | 8 +++++--- src/BulletSoftBody/btPreconditioner.h | 2 ++ 5 files changed, 13 insertions(+), 6 deletions(-) diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h index 6fad0e60a..c20a8ae5d 100644 --- a/src/BulletSoftBody/btConjugateGradient.h +++ b/src/BulletSoftBody/btConjugateGradient.h @@ -104,7 +104,9 @@ public: TVStack c; c.resize(a.size()); for (int i = 0; i < a.size(); ++i) + { c[i] = a[i] - b[i]; + } return c; } diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index b66a02aff..4ca5187c4 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -14,6 +14,7 @@ */ #include "btDeformableBackwardEulerObjective.h" +#include "btPreconditioner.h" #include "LinearMath/btQuickprof.h" btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v) @@ -22,7 +23,7 @@ btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAligned , m_backupVelocity(backup_v) , m_implicit(false) { - m_preconditioner = new DefaultPreconditioner(); + m_preconditioner = new MassPreconditioner(m_softBodies); } btDeformableBackwardEulerObjective::~btDeformableBackwardEulerObjective() diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 7fc8aa99e..a902322ce 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -20,9 +20,9 @@ btDeformableBodySolver::btDeformableBodySolver() : m_numNodes(0) -, m_cg(50) +, m_cg(20) , m_maxNewtonIterations(5) -, m_newtonTolerance(1e-10) +, m_newtonTolerance(1e-4) { m_objective = new btDeformableBackwardEulerObjective(m_softBodySet, m_backupVelocity); } diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index ffd91d5df..404eda494 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -236,12 +236,14 @@ void btDeformableContactProjection::setConstraints() bool single_contact = true; if (single_contact) { - constraints.m_contact[0]->m_cti.m_offset > cti.m_offset; - constraints.replace(c); + if (constraints.m_contact[0]->m_cti.m_offset > cti.m_offset) + { + constraints.replace(c); + } } else { - constraints.append(c); + constraints.append(c); } } } diff --git a/src/BulletSoftBody/btPreconditioner.h b/src/BulletSoftBody/btPreconditioner.h index 32d697a63..d71242038 100644 --- a/src/BulletSoftBody/btPreconditioner.h +++ b/src/BulletSoftBody/btPreconditioner.h @@ -70,7 +70,9 @@ public: btAssert(b.size() == x.size()); btAssert(m_inv_mass.size() == x.size()); for (int i = 0; i < b.size(); ++i) + { b[i] = x[i] * m_inv_mass[i]; + } } }; From 8a08e32f51e108bd2e5bb424216b06efdc763465 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Thu, 29 Aug 2019 11:26:57 -0700 Subject: [PATCH 13/18] update CMakeLists --- src/BulletSoftBody/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/BulletSoftBody/CMakeLists.txt b/src/BulletSoftBody/CMakeLists.txt index 1e9edbd91..b3c32a6c5 100644 --- a/src/BulletSoftBody/CMakeLists.txt +++ b/src/BulletSoftBody/CMakeLists.txt @@ -20,6 +20,7 @@ SET(BulletSoftBody_SRCS btDeformableBackwardEulerObjective.cpp btDeformableBodySolver.cpp + btDeformableMultiBodyConstraintSolver.cpp btDeformableContactProjection.cpp btDeformableMultiBodyDynamicsWorld.cpp @@ -52,6 +53,7 @@ SET(BulletSoftBody_HDRS btDeformableBackwardEulerObjective.h btDeformableBodySolver.h btDeformableMultiBodyConstraintSolver.h + btDeformableMultiBodyConstraintSolver.h btDeformableContactProjection.h btDeformableMultiBodyDynamicsWorld.h From 3d2f945f9c640cbbfd5b14ac33470119365ca855 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Tue, 3 Sep 2019 15:04:12 -0700 Subject: [PATCH 14/18] address PR comments --- .../SharedMemory/PhysicsServerCommandProcessor.cpp | 12 ++++-------- examples/SharedMemory/SharedMemoryCommands.h | 1 - 2 files changed, 4 insertions(+), 9 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index ccfd4eab9..bed051fb1 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -109,6 +109,7 @@ #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #endif +#define SKIP_DEFORMABE_BODY 1 int gInternalSimFlags = 0; bool gResetSimulation = 0; @@ -1622,7 +1623,7 @@ struct PhysicsServerCommandProcessorInternalData btHashedOverlappingPairCache* m_pairCache; btBroadphaseInterface* m_broadphase; btCollisionDispatcher* m_dispatcher; -#ifdef SKIP_DEFORMABLE_BODY +#if defined (SKIP_DEFORMABLE_BODY) || defined(SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD) btMultiBodyConstraintSolver* m_solver; #else btDeformableMultiBodyConstraintSolver* m_solver; @@ -2797,9 +2798,7 @@ 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 @@ -2870,7 +2869,7 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() delete m_data->m_remoteDebugDrawer; m_data->m_remoteDebugDrawer = 0; -#ifndef SKIP_DEFORMABLE_BODY +#if !defined (SKIP_DEFORMABLE_BODY) && !defined(SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD) delete m_data->m_deformablebodySolver; m_data->m_deformablebodySolver = 0; #endif @@ -8108,12 +8107,9 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar if (psb != NULL) { #ifndef SKIP_DEFORMABLE_BODY - btVector3 gravity(0,0,0); + btVector3 gravity = m_data->m_dynamicsWorld->getGravity(); 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; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 58d72ce8d..74e79537a 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -523,7 +523,6 @@ struct LoadSoftBodyArgs double m_springDampingStiffness; double m_corotatedMu; double m_corotatedLambda; - double m_gravity[3]; bool m_useBendingSprings; double m_collisionHardness; double m_frictionCoeff; From 1ded85e62e3a7890760b4a021f299ad49f4594b5 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Tue, 3 Sep 2019 15:42:34 -0700 Subject: [PATCH 15/18] remove extra gravity field --- examples/SharedMemory/PhysicsClientC_API.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 87276b264..2db272fa8 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -372,9 +372,6 @@ B3_SHARED_API int b3LoadSoftBodyAddGravityForce(b3SharedMemoryCommandHandle comm { 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; } From 1965f4695944f6e22c5eea747b8d4d5c420e35ca Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Tue, 3 Sep 2019 15:54:32 -0700 Subject: [PATCH 16/18] update CMakeLists --- src/BulletSoftBody/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/src/BulletSoftBody/CMakeLists.txt b/src/BulletSoftBody/CMakeLists.txt index b3c32a6c5..06059c77b 100644 --- a/src/BulletSoftBody/CMakeLists.txt +++ b/src/BulletSoftBody/CMakeLists.txt @@ -53,7 +53,6 @@ SET(BulletSoftBody_HDRS btDeformableBackwardEulerObjective.h btDeformableBodySolver.h btDeformableMultiBodyConstraintSolver.h - btDeformableMultiBodyConstraintSolver.h btDeformableContactProjection.h btDeformableMultiBodyDynamicsWorld.h From d5afccf3f10655beb96c776a33ea6282521d0548 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Tue, 3 Sep 2019 16:25:16 -0700 Subject: [PATCH 17/18] update CMakeLists --- src/BulletSoftBody/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/src/BulletSoftBody/CMakeLists.txt b/src/BulletSoftBody/CMakeLists.txt index 06059c77b..8c1e9c022 100644 --- a/src/BulletSoftBody/CMakeLists.txt +++ b/src/BulletSoftBody/CMakeLists.txt @@ -16,7 +16,6 @@ SET(BulletSoftBody_SRCS btSoftMultiBodyDynamicsWorld.cpp btSoftSoftCollisionAlgorithm.cpp btDefaultSoftBodySolver.cpp - btDeformableMultiBodyConstraintSolver.cpp btDeformableBackwardEulerObjective.cpp btDeformableBodySolver.cpp From 899bf9afd124a3520e98436635bde0c867333e03 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 4 Sep 2019 18:55:16 -0700 Subject: [PATCH 18/18] update setup.py --- setup.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/setup.py b/setup.py index b2e46b109..88d3e47c1 100644 --- a/setup.py +++ b/setup.py @@ -247,6 +247,11 @@ sources = ["examples/pybullet/pybullet.c"]\ +["src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp"]\ +["src/BulletSoftBody/btSoftMultiBodyDynamicsWorld.cpp"]\ +["src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp"]\ ++["src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp"]\ ++["src/BulletSoftBody/btDeformableBodySolver.cpp"]\ ++["src/BulletSoftBody/btDeformableContactProjection.cpp"]\ ++["src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp"]\ ++["src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp"]\ +["src/BulletInverseDynamics/IDMath.cpp"]\ +["src/BulletInverseDynamics/MultiBodyTree.cpp"]\ +["src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp"]\