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/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/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..850f72600 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) @@ -234,17 +234,17 @@ 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); - 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 6b01bc66d..34127774d 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) @@ -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() @@ -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,31 +344,26 @@ 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(.5,0.04, true); + btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(.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); + btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(5,10); getDeformableDynamicsWorld()->addForce(psb, neohookean); - forces.push_back(neohookean); + m_forces.push_back(neohookean); } // // create a piece of cloth @@ -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); } @@ -440,11 +436,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++) { @@ -468,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/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++) { 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..2db272fa8 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -338,6 +338,71 @@ 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_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 +1438,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..bed051fb1 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -101,11 +101,15 @@ #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" #else #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #endif +#define SKIP_DEFORMABE_BODY 1 int gInternalSimFlags = 0; bool gResetSimulation = 0; @@ -1619,12 +1623,22 @@ struct PhysicsServerCommandProcessorInternalData btHashedOverlappingPairCache* m_pairCache; btBroadphaseInterface* m_broadphase; btCollisionDispatcher* m_dispatcher; +#if defined (SKIP_DEFORMABLE_BODY) || defined(SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD) btMultiBodyConstraintSolver* m_solver; +#else + btDeformableMultiBodyConstraintSolver* m_solver; +#endif + btDefaultCollisionConfiguration* m_collisionConfiguration; #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD +#ifndef SKIP_DEFORMABLE_BODY + btDeformableMultiBodyDynamicsWorld* m_dynamicsWorld; + 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 @@ -2846,6 +2869,11 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() delete m_data->m_remoteDebugDrawer; m_data->m_remoteDebugDrawer = 0; +#if !defined (SKIP_DEFORMABLE_BODY) && !defined(SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD) + delete m_data->m_deformablebodySolver; + m_data->m_deformablebodySolver = 0; +#endif + delete m_data->m_solver; m_data->m_solver = 0; @@ -4728,8 +4756,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 +4936,7 @@ bool PhysicsServerCommandProcessor::processCreateCollisionShapeCommand(const str } break; } + default: { } @@ -8014,26 +8042,109 @@ 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; + btScalar spring_elastic_stiffness, spring_damping_stiffness; + 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); + } + } +#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) + { +#ifndef SKIP_DEFORMABLE_BODY + psb = btSoftBodyHelpers::CreateFromVtkFile(m_data->m_dynamicsWorld->getWorldInfo(), out_found_filename.c_str()); + btScalar corotated_mu, corotated_lambda; + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE) + { + corotated_mu = clientCmd.m_loadSoftBodyArguments.m_corotatedMu; + corotated_lambda = clientCmd.m_loadSoftBodyArguments.m_corotatedLambda; + 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)); + } + 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 + btVector3 gravity = m_data->m_dynamicsWorld->getGravity(); + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_GRAVITY_FORCE) + { + 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 +8154,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 +8220,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 +9244,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st } }; +#ifdef SKIP_DEFORMABLE_BODY if (newSolver) { delete oldSolver; @@ -9142,6 +9253,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st m_data->m_solver = newSolver; printf("switched solver\n"); } +#endif } } @@ -12020,7 +12132,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 +12542,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 +12828,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..74e79537a 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,15 @@ 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; + bool m_useBendingSprings; + double m_collisionHardness; + double m_frictionCoeff; + double m_NeoHookeanMu; + double m_NeoHookeanLambda; }; struct b3LoadSoftBodyResultArgs 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"]\ diff --git a/src/BulletSoftBody/CMakeLists.txt b/src/BulletSoftBody/CMakeLists.txt index 1e9edbd91..8c1e9c022 100644 --- a/src/BulletSoftBody/CMakeLists.txt +++ b/src/BulletSoftBody/CMakeLists.txt @@ -16,10 +16,10 @@ SET(BulletSoftBody_SRCS btSoftMultiBodyDynamicsWorld.cpp btSoftSoftCollisionAlgorithm.cpp btDefaultSoftBodySolver.cpp - btDeformableMultiBodyConstraintSolver.cpp btDeformableBackwardEulerObjective.cpp btDeformableBodySolver.cpp + btDeformableMultiBodyConstraintSolver.cpp btDeformableContactProjection.cpp btDeformableMultiBodyDynamicsWorld.cpp 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/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 41b1f74c7..4ca5187c4 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -14,14 +14,16 @@ */ #include "btDeformableBackwardEulerObjective.h" +#include "btPreconditioner.h" #include "LinearMath/btQuickprof.h" btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray& softBodies, const TVStack& backup_v) : m_softBodies(softBodies) , projection(m_softBodies, m_dt) , m_backupVelocity(backup_v) +, m_implicit(false) { - m_preconditioner = new DefaultPreconditioner(); + m_preconditioner = new MassPreconditioner(m_softBodies); } btDeformableBackwardEulerObjective::~btDeformableBackwardEulerObjective() @@ -72,13 +74,17 @@ 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); + } } } 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(); @@ -105,14 +111,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 +136,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 4489296f0..a902322ce 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -20,7 +20,9 @@ btDeformableBodySolver::btDeformableBodySolver() : m_numNodes(0) -, m_cg(50) +, m_cg(20) +, m_maxNewtonIterations(5) +, m_newtonTolerance(1e-4) { 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; @@ -116,6 +189,29 @@ 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) + { + // 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; + } + } +} + void btDeformableBodySolver::revertVelocity() { int counter = 0; @@ -173,8 +269,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_x += n.m_v * dt; + n.m_q = n.m_x + n.m_v * dt; } /* Bounds */ psb->updateBounds(); @@ -184,7 +279,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, @@ -209,3 +304,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..fed15b2b2 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; @@ -72,6 +75,7 @@ public: void predictDeformableMotion(btSoftBody* psb, btScalar dt); void backupVelocity(); + void backupVn(); void revertVelocity(); void updateVelocity(); @@ -94,7 +98,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/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index 33feb4bc6..404eda494 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,18 @@ void btDeformableContactProjection::setConstraints() else { DeformableContactConstraint& constraints = *m_constraints[c.m_node->index]; - constraints.append(c); + bool single_contact = true; + if (single_contact) + { + if (constraints.m_contact[0]->m_cti.m_offset > cti.m_offset) + { + constraints.replace(c); + } + } + else + { + constraints.append(c); + } } } } diff --git a/src/BulletSoftBody/btDeformableCorotatedForce.h b/src/BulletSoftBody/btDeformableCorotatedForce.h index 14896514e..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) @@ -92,7 +93,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; @@ -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..7ce72a7fd 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,270 @@ 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 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; 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; + 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 14d4e8260..a85dd547a 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) @@ -63,7 +64,7 @@ public: { 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; } } @@ -90,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); @@ -100,12 +100,12 @@ 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) { - 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) { @@ -120,7 +120,7 @@ public: { 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; } } @@ -129,6 +129,60 @@ 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) + { + // implicit damping force differential + for (int i = 0; i < m_softBodies.size(); ++i) + { + const btSoftBody* psb = m_softBodies[i]; + btScalar scaled_k = m_elasticStiffness * 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; + + 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] += scaled_df; + df[id2] -= scaled_df; + } + } + } virtual btDeformableLagrangianForceType getForceType() { diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 58907fcd3..3fff883c9 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -166,7 +166,9 @@ 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; + node.m_q = node.m_x; + node.m_vn = node.m_v; } } m_deformableBodySolver->revertVelocity(); @@ -180,6 +182,16 @@ void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep) // set up constraints among multibodies and between multibodies and deformable bodies setupConstraints(); solveMultiBodyRelatedConstraints(); + + if (m_implicit) + { + // 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 should be golden for nodes in contact m_deformableBodySolver->solveDeformableConstraints(timeStep); } @@ -199,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 } } @@ -273,7 +284,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 +314,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) +// { +// // 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 a91944c55..972f05664 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; @@ -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 = true; } 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 9bc232e24..88763e8a1 100644 --- a/src/BulletSoftBody/btDeformableNeoHookeanForce.h +++ b/src/BulletSoftBody/btDeformableNeoHookeanForce.h @@ -23,18 +23,24 @@ class btDeformableNeoHookeanForce : public btDeformableLagrangianForce public: typedef btAlignedObjectArray 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 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) @@ -44,6 +50,65 @@ 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() + { + 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) @@ -72,7 +137,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,23 +146,104 @@ 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) + { + 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() * (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); + 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))); + + addScaledCofactorMatrixDifferential(F, dF, m_lambda*(J-1.) - 0.75*m_mu, dP); + 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) @@ -106,25 +251,22 @@ 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]); } 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/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]; + } } }; diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index fcfb330c3..eb630cbfc 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()); @@ -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()); 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 diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index a39394e0b..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_x, 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; @@ -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];