diff --git a/CMakeLists.txt b/CMakeLists.txt index e4cdcc313..00e7b1a10 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -470,6 +470,7 @@ IF (BUILD_UNIT_TESTS) ENDIF() set (BULLET_CONFIG_CMAKE_PATH lib${LIB_SUFFIX}/cmake/bullet ) +list (APPEND BULLET_DEFINITIONS ${BULLET_DOUBLE_DEF}) list (APPEND BULLET_LIBRARIES LinearMath) list (APPEND BULLET_LIBRARIES Bullet3Common) list (APPEND BULLET_LIBRARIES BulletInverseDynamics) diff --git a/examples/CommonInterfaces/CommonDeformableBodyBase.h b/examples/CommonInterfaces/CommonDeformableBodyBase.h index 2a5e3b687..90dbf8f06 100644 --- a/examples/CommonInterfaces/CommonDeformableBodyBase.h +++ b/examples/CommonInterfaces/CommonDeformableBodyBase.h @@ -21,11 +21,13 @@ struct CommonDeformableBodyBase : public CommonMultiBodyBase btAlignedObjectArray m_forces; btSoftBody* m_pickedSoftBody; btDeformableMousePickingForce* m_mouseForce; - btScalar m_maxPickingForce; + btScalar m_pickingForceElasticStiffness, m_pickingForceDampingStiffness, m_maxPickingForce; CommonDeformableBodyBase(GUIHelperInterface* helper) : CommonMultiBodyBase(helper), m_pickedSoftBody(0), m_mouseForce(0), + m_pickingForceElasticStiffness(100), + m_pickingForceDampingStiffness(0.0), m_maxPickingForce(0.3) { } @@ -115,7 +117,7 @@ struct CommonDeformableBodyBase : public CommonMultiBodyBase m_pickedSoftBody = psb; psb->setActivationState(DISABLE_DEACTIVATION); const btSoftBody::Face& f = psb->m_faces[face_id]; - btDeformableMousePickingForce* mouse_force = new btDeformableMousePickingForce(100, 0, f, m_hitPos, m_maxPickingForce); + btDeformableMousePickingForce* mouse_force = new btDeformableMousePickingForce(m_pickingForceElasticStiffness, m_pickingForceDampingStiffness, f, m_hitPos, m_maxPickingForce); m_mouseForce = mouse_force; getDeformableDynamicsWorld()->addForce(psb, mouse_force); } diff --git a/examples/DeformableDemo/Collide.cpp b/examples/DeformableDemo/Collide.cpp new file mode 100644 index 000000000..01590ce7a --- /dev/null +++ b/examples/DeformableDemo/Collide.cpp @@ -0,0 +1,273 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#include "Collide.h" +///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. +#include "btBulletDynamicsCommon.h" +#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h" +#include "BulletSoftBody/btSoftBody.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" +#include "BulletSoftBody/btDeformableBodySolver.h" +#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "../CommonInterfaces/CommonParameterInterface.h" +#include //printf debugging + +#include "../CommonInterfaces/CommonDeformableBodyBase.h" +#include "../Utils/b3ResourcePath.h" + +///The Collide shows the contact between volumetric deformable objects and rigid objects. +static btScalar E = 50; +static btScalar nu = 0.3; +static btScalar damping_alpha = 0.1; +static btScalar damping_beta = 0.01; +static btScalar COLLIDING_VELOCITY = 15; + +struct TetraCube +{ +#include "../SoftDemo/cube.inl" +}; + +class Collide : public CommonDeformableBodyBase +{ + btDeformableLinearElasticityForce* m_linearElasticity; + +public: + Collide(struct GUIHelperInterface* helper) + : CommonDeformableBodyBase(helper) + { + m_linearElasticity = 0; + } + virtual ~Collide() + { + } + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 20; + float pitch = 0; + float yaw = 90; + float targetPos[3] = {0, 3, 0}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + void Ctor_RbUpStack() + { + float mass = 0.5; + btCollisionShape* shape = new btBoxShape(btVector3(2, 2, 2)); + btTransform startTransform; + startTransform.setIdentity(); + startTransform.setOrigin(btVector3(0,-2,0)); + btRigidBody* rb = createRigidBody(mass, startTransform, shape); + rb->setLinearVelocity(btVector3(0,+COLLIDING_VELOCITY, 0)); + } + + void stepSimulation(float deltaTime) + { + m_linearElasticity->setPoissonRatio(nu); + m_linearElasticity->setYoungsModulus(E); + m_linearElasticity->setDamping(damping_alpha, damping_beta); + float internalTimeStep = 1. / 60.f; + m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep); + } + + virtual void renderScene() + { + CommonDeformableBodyBase::renderScene(); + btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); + + for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) + { + btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i]; + { + btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); + } + } + } +}; + +void Collide::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); + + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + m_broadphase = new btDbvtBroadphase(); + btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); + + btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); + sol->setDeformableSolver(deformableBodySolver); + m_solver = sol; + + m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); + btVector3 gravity = btVector3(0, 0, 0); + m_dynamicsWorld->setGravity(gravity); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + // create volumetric soft body + { + btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(), + TetraCube::getElements(), + 0, + TetraCube::getNodes(), + false, true, true); + getDeformableDynamicsWorld()->addSoftBody(psb); + psb->scale(btVector3(2, 2, 2)); + psb->translate(btVector3(0, 7, 0)); + psb->getCollisionShape()->setMargin(0.1); + psb->setTotalMass(0.5); + psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb->m_cfg.kCHR = 1; // collision hardness with rigid body + psb->m_cfg.kDF = 0; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN; + psb->m_sleepingThreshold = 0; + btSoftBodyHelpers::generateBoundaryFaces(psb); + + psb->setVelocity(btVector3(0, -COLLIDING_VELOCITY, 0)); + + btDeformableLinearElasticityForce* linearElasticity = new btDeformableLinearElasticityForce(100,100,0.01); + m_linearElasticity = linearElasticity; + getDeformableDynamicsWorld()->addForce(psb, linearElasticity); + m_forces.push_back(linearElasticity); + } + getDeformableDynamicsWorld()->setImplicit(true); + getDeformableDynamicsWorld()->setLineSearch(false); + getDeformableDynamicsWorld()->setUseProjection(true); + getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3; + getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200); + getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3; + getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = true; + getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100; + // add a few rigid bodies + Ctor_RbUpStack(); + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); + +// { +// SliderParams slider("Young's Modulus", &E); +// slider.m_minVal = 0; +// slider.m_maxVal = 2000; +// if (m_guiHelper->getParameterInterface()) +// m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); +// } +// { +// SliderParams slider("Poisson Ratio", &nu); +// slider.m_minVal = 0.05; +// slider.m_maxVal = 0.49; +// if (m_guiHelper->getParameterInterface()) +// m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); +// } +// { +// SliderParams slider("Mass Damping", &damping_alpha); +// slider.m_minVal = 0.001; +// slider.m_maxVal = 0.01; +// if (m_guiHelper->getParameterInterface()) +// m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); +// } +// { +// SliderParams slider("Stiffness Damping", &damping_beta); +// slider.m_minVal = 0.001; +// slider.m_maxVal = 0.01; +// if (m_guiHelper->getParameterInterface()) +// m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); +// } + { + SliderParams slider("Young's Modulus", &E); + slider.m_minVal = 0; + slider.m_maxVal = 2000; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + { + SliderParams slider("Poisson Ratio", &nu); + slider.m_minVal = 0.05; + slider.m_maxVal = 0.49; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + { + SliderParams slider("Mass Damping", &damping_alpha); + slider.m_minVal = 0; + slider.m_maxVal = 1; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + { + SliderParams slider("Stiffness Damping", &damping_beta); + slider.m_minVal = 0; + slider.m_maxVal = 0.1; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } +} + +void Collide::exitPhysics() +{ + //cleanup in the reverse order of creation/initialization + removePickingConstraint(); + //remove the rigidbodies from the dynamics world and delete them + int i; + for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) + { + btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()) + { + delete body->getMotionState(); + } + m_dynamicsWorld->removeCollisionObject(obj); + delete obj; + } + // delete forces + for (int j = 0; j < m_forces.size(); j++) + { + btDeformableLagrangianForce* force = m_forces[j]; + delete force; + } + m_forces.clear(); + + //delete collision shapes + for (int j = 0; j < m_collisionShapes.size(); j++) + { + btCollisionShape* shape = m_collisionShapes[j]; + delete shape; + } + m_collisionShapes.clear(); + + delete m_dynamicsWorld; + + delete m_solver; + + delete m_broadphase; + + delete m_dispatcher; + + delete m_collisionConfiguration; +} + + + +class CommonExampleInterface* CollideCreateFunc(struct CommonExampleOptions& options) +{ + return new Collide(options.m_guiHelper); +} + + diff --git a/examples/DeformableDemo/Collide.h b/examples/DeformableDemo/Collide.h new file mode 100644 index 000000000..97a432e5c --- /dev/null +++ b/examples/DeformableDemo/Collide.h @@ -0,0 +1,19 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef _COLLIDE_H +#define _COLLIDE_H + +class CommonExampleInterface* CollideCreateFunc(struct CommonExampleOptions& options); + +#endif //_COLLIDE_H diff --git a/examples/DeformableDemo/DeformableSelfCollision.cpp b/examples/DeformableDemo/DeformableSelfCollision.cpp index 576f8f488..99180ea0c 100644 --- a/examples/DeformableDemo/DeformableSelfCollision.cpp +++ b/examples/DeformableDemo/DeformableSelfCollision.cpp @@ -31,6 +31,7 @@ public: DeformableSelfCollision(struct GUIHelperInterface* helper) : CommonDeformableBodyBase(helper) { + m_maxPickingForce = 0.004; } virtual ~DeformableSelfCollision() { @@ -93,7 +94,7 @@ void DeformableSelfCollision::initPhysics() { ///create a ground btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(2.5), btScalar(150.))); - groundShape->setMargin(0.0001); + groundShape->setMargin(0.02); m_collisionShapes.push_back(groundShape); btTransform groundTransform; @@ -152,6 +153,7 @@ void DeformableSelfCollision::addCloth(btVector3 origin) clothTransform.setOrigin(btVector3(0,0.2,0)+origin); psb->transform(clothTransform); psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN; psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; getDeformableDynamicsWorld()->addSoftBody(psb); @@ -164,6 +166,7 @@ void DeformableSelfCollision::addCloth(btVector3 origin) btVector3 gravity = btVector3(0, -9.8, 0); btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); getDeformableDynamicsWorld()->addForce(psb, gravity_force); + getDeformableDynamicsWorld()->setUseProjection(true); m_forces.push_back(gravity_force); } diff --git a/examples/DeformableDemo/LargeDeformation.cpp b/examples/DeformableDemo/LargeDeformation.cpp new file mode 100644 index 000000000..f1a6a29f5 --- /dev/null +++ b/examples/DeformableDemo/LargeDeformation.cpp @@ -0,0 +1,262 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#include "LargeDeformation.h" +///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files. +#include "btBulletDynamicsCommon.h" +#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h" +#include "BulletSoftBody/btSoftBody.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" +#include "BulletSoftBody/btDeformableBodySolver.h" +#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "../CommonInterfaces/CommonParameterInterface.h" +#include //printf debugging + +#include "../CommonInterfaces/CommonDeformableBodyBase.h" +#include "../Utils/b3ResourcePath.h" + +///The LargeDeformation shows the contact between volumetric deformable objects and rigid objects. +static btScalar E = 50; +static btScalar nu = 0.3; +static btScalar damping_alpha = 0.1; +static btScalar damping_beta = 0.01; + +struct TetraCube +{ +#include "../SoftDemo/cube.inl" +}; + +class LargeDeformation : public CommonDeformableBodyBase +{ + btDeformableLinearElasticityForce* m_linearElasticity; + +public: + LargeDeformation(struct GUIHelperInterface* helper) + : CommonDeformableBodyBase(helper) + { + m_linearElasticity = 0; + } + virtual ~LargeDeformation() + { + } + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 20; + float pitch = -45; + float yaw = 100; + float targetPos[3] = {0, 3, 0}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + void stepSimulation(float deltaTime) + { + m_linearElasticity->setPoissonRatio(nu); + m_linearElasticity->setYoungsModulus(E); + m_linearElasticity->setDamping(damping_alpha, damping_beta); + float internalTimeStep = 1. / 60.f; + m_dynamicsWorld->stepSimulation(deltaTime, 1, internalTimeStep); + } + + virtual void renderScene() + { + CommonDeformableBodyBase::renderScene(); + btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); + + for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) + { + btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i]; + { + btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags()); + } + } + } +}; + +void LargeDeformation::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); + + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + m_broadphase = new btDbvtBroadphase(); + btDeformableBodySolver* deformableBodySolver = new btDeformableBodySolver(); + + btDeformableMultiBodyConstraintSolver* sol = new btDeformableMultiBodyConstraintSolver(); + sol->setDeformableSolver(deformableBodySolver); + m_solver = sol; + + m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + // create volumetric soft body + { + btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(), + TetraCube::getElements(), + 0, + TetraCube::getNodes(), + false, true, true); + getDeformableDynamicsWorld()->addSoftBody(psb); + psb->scale(btVector3(2, 2, 2)); + psb->translate(btVector3(0, 5, 0)); + psb->getCollisionShape()->setMargin(0.1); + psb->setTotalMass(0.5); + psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb->m_cfg.kCHR = 1; // collision hardness with rigid body + psb->m_cfg.kDF = 0.5; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN; + psb->m_sleepingThreshold = 0; + btSoftBodyHelpers::generateBoundaryFaces(psb); + for (int i = 0; i < psb->m_nodes.size(); ++i) + { + for (int j = 0; j < 3; ++j) + psb->m_nodes[i].m_x[j] = ((double) 2*rand() / (RAND_MAX))-1.0; + psb->m_nodes[i].m_x[1]+=8; + } + + btDeformableLinearElasticityForce* linearElasticity = new btDeformableLinearElasticityForce(100,100,0.01); + m_linearElasticity = linearElasticity; + getDeformableDynamicsWorld()->addForce(psb, linearElasticity); + m_forces.push_back(linearElasticity); + } + getDeformableDynamicsWorld()->setImplicit(true); + getDeformableDynamicsWorld()->setLineSearch(false); + getDeformableDynamicsWorld()->setUseProjection(true); + getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.1; + getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(20); + getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3; + getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = true; + getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100; + // add a few rigid bodies + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); + + { + SliderParams slider("Young's Modulus", &E); + slider.m_minVal = 0; + slider.m_maxVal = 2000; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + { + SliderParams slider("Poisson Ratio", &nu); + slider.m_minVal = 0.05; + slider.m_maxVal = 0.49; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + { + SliderParams slider("Mass Damping", &damping_alpha); + slider.m_minVal = 0; + slider.m_maxVal = 1; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + { + SliderParams slider("Stiffness Damping", &damping_beta); + slider.m_minVal = 0; + slider.m_maxVal = 0.1; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } +// { +// SliderParams slider("Young's Modulus", &E); +// slider.m_minVal = 0; +// slider.m_maxVal = 200; +// if (m_guiHelper->getParameterInterface()) +// m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); +// } +// { +// SliderParams slider("Poisson Ratio", &nu); +// slider.m_minVal = 0.05; +// slider.m_maxVal = 0.40; +// if (m_guiHelper->getParameterInterface()) +// m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); +// } +// { +// SliderParams slider("Mass Damping", &damping_alpha); +// slider.m_minVal = 0.001; +// slider.m_maxVal = 0.01; +// if (m_guiHelper->getParameterInterface()) +// m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); +// } +// { +// SliderParams slider("Stiffness Damping", &damping_beta); +// slider.m_minVal = 0.001; +// slider.m_maxVal = 0.01; +// if (m_guiHelper->getParameterInterface()) +// m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); +// } +} + +void LargeDeformation::exitPhysics() +{ + //cleanup in the reverse order of creation/initialization + removePickingConstraint(); + //remove the rigidbodies from the dynamics world and delete them + int i; + for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) + { + btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i]; + btRigidBody* body = btRigidBody::upcast(obj); + if (body && body->getMotionState()) + { + delete body->getMotionState(); + } + m_dynamicsWorld->removeCollisionObject(obj); + delete obj; + } + // delete forces + for (int j = 0; j < m_forces.size(); j++) + { + btDeformableLagrangianForce* force = m_forces[j]; + delete force; + } + m_forces.clear(); + + //delete collision shapes + for (int j = 0; j < m_collisionShapes.size(); j++) + { + btCollisionShape* shape = m_collisionShapes[j]; + delete shape; + } + m_collisionShapes.clear(); + + delete m_dynamicsWorld; + + delete m_solver; + + delete m_broadphase; + + delete m_dispatcher; + + delete m_collisionConfiguration; +} + + + +class CommonExampleInterface* LargeDeformationCreateFunc(struct CommonExampleOptions& options) +{ + return new LargeDeformation(options.m_guiHelper); +} + + diff --git a/examples/DeformableDemo/LargeDeformation.h b/examples/DeformableDemo/LargeDeformation.h new file mode 100644 index 000000000..bb68082f4 --- /dev/null +++ b/examples/DeformableDemo/LargeDeformation.h @@ -0,0 +1,19 @@ +/* + Bullet Continuous Collision Detection and Physics Library + Copyright (c) 2019 Google Inc. http://bulletphysics.org + This software is provided 'as-is', without any express or implied warranty. + In no event will the authors be held liable for any damages arising from the use of this software. + Permission is granted to anyone to use this software for any purpose, + including commercial applications, and to alter it and redistribute it freely, + subject to the following restrictions: + 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. + 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. + 3. This notice may not be removed or altered from any source distribution. + */ + +#ifndef _LARGE_DEFORMATION_H +#define _LARGE_DEFORMATION_H + +class CommonExampleInterface* LargeDeformationCreateFunc(struct CommonExampleOptions& options); + +#endif //_LARGE_DEFORMATION_H diff --git a/examples/DeformableDemo/VolumetricDeformable.cpp b/examples/DeformableDemo/VolumetricDeformable.cpp index a60bb10d8..bfc51a156 100644 --- a/examples/DeformableDemo/VolumetricDeformable.cpp +++ b/examples/DeformableDemo/VolumetricDeformable.cpp @@ -27,9 +27,10 @@ #include "../Utils/b3ResourcePath.h" ///The VolumetricDeformable shows the contact between volumetric deformable objects and rigid objects. -static btScalar E = 25; +static btScalar E = 50; static btScalar nu = 0.3; -static btScalar damping = 0.01; +static btScalar damping_alpha = 0.1; +static btScalar damping_beta = 0.01; struct TetraCube { @@ -38,13 +39,16 @@ struct TetraCube class VolumetricDeformable : public CommonDeformableBodyBase { - btDeformableNeoHookeanForce* m_neohookean; + btDeformableLinearElasticityForce* m_linearElasticity; public: VolumetricDeformable(struct GUIHelperInterface* helper) : CommonDeformableBodyBase(helper) { - m_neohookean = 0; + m_linearElasticity = 0; + m_pickingForceElasticStiffness = 100; + m_pickingForceDampingStiffness = 0; + m_maxPickingForce = 1e10; // allow large picking force with implicit scheme. } virtual ~VolumetricDeformable() { @@ -64,12 +68,12 @@ public: void stepSimulation(float deltaTime) { - m_neohookean->setPoissonRatio(nu); - m_neohookean->setYoungsModulus(E); - m_neohookean->setDamping(damping); + m_linearElasticity->setPoissonRatio(nu); + m_linearElasticity->setYoungsModulus(E); + m_linearElasticity->setDamping(damping_alpha, damping_beta); //use a smaller internal timestep, there are stability issues - float internalTimeStep = 1. / 480.f; - m_dynamicsWorld->stepSimulation(deltaTime, 8, internalTimeStep); + float internalTimeStep = 1. / 240; + m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); } void createStaticBox(const btVector3& halfEdge, const btVector3& translation) @@ -100,7 +104,7 @@ public: void Ctor_RbUpStack(int count) { - float mass = 0.2; + float mass = 2; btCompoundShape* cylinderCompound = new btCompoundShape; btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(2, .5, .5)); @@ -187,7 +191,7 @@ void VolumetricDeformable::initPhysics() btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); btRigidBody* body = new btRigidBody(rbInfo); - body->setFriction(0.5); + body->setFriction(1); //add the ground to the dynamics world m_dynamicsWorld->addRigidBody(body); @@ -208,29 +212,32 @@ void VolumetricDeformable::initPhysics() getDeformableDynamicsWorld()->addSoftBody(psb); psb->scale(btVector3(2, 2, 2)); psb->translate(btVector3(0, 5, 0)); - psb->getCollisionShape()->setMargin(0.025); + psb->getCollisionShape()->setMargin(0.1); psb->setTotalMass(0.5); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body - psb->m_cfg.kDF = 0.5; + psb->m_cfg.kDF = 2; psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; - psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; - psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDF; + psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_RDN; psb->m_sleepingThreshold = 0; btSoftBodyHelpers::generateBoundaryFaces(psb); - btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); getDeformableDynamicsWorld()->addForce(psb, gravity_force); m_forces.push_back(gravity_force); - btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(500,2000,0.01); - m_neohookean = neohookean; - getDeformableDynamicsWorld()->addForce(psb, neohookean); - m_forces.push_back(neohookean); - + btDeformableLinearElasticityForce* linearElasticity = new btDeformableLinearElasticityForce(100,100,0.01); + m_linearElasticity = linearElasticity; + getDeformableDynamicsWorld()->addForce(psb, linearElasticity); + m_forces.push_back(linearElasticity); } - getDeformableDynamicsWorld()->setImplicit(false); + getDeformableDynamicsWorld()->setImplicit(true); getDeformableDynamicsWorld()->setLineSearch(false); + getDeformableDynamicsWorld()->setUseProjection(true); + getDeformableDynamicsWorld()->getSolverInfo().m_deformable_erp = 0.3; + getDeformableDynamicsWorld()->getSolverInfo().m_deformable_maxErrorReduction = btScalar(200); + getDeformableDynamicsWorld()->getSolverInfo().m_leastSquaresResidualThreshold = 1e-3; + getDeformableDynamicsWorld()->getSolverInfo().m_splitImpulse = true; + getDeformableDynamicsWorld()->getSolverInfo().m_numIterations = 100; // add a few rigid bodies Ctor_RbUpStack(4); m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); @@ -238,24 +245,31 @@ void VolumetricDeformable::initPhysics() { SliderParams slider("Young's Modulus", &E); slider.m_minVal = 0; - slider.m_maxVal = 50; + slider.m_maxVal = 2000; if (m_guiHelper->getParameterInterface()) m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { SliderParams slider("Poisson Ratio", &nu); slider.m_minVal = 0.05; - slider.m_maxVal = 0.40; + slider.m_maxVal = 0.49; if (m_guiHelper->getParameterInterface()) m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { - SliderParams slider("Damping", &damping); - slider.m_minVal = 0.01; - slider.m_maxVal = 0.02; + SliderParams slider("Mass Damping", &damping_alpha); + slider.m_minVal = 0; + slider.m_maxVal = 1; if (m_guiHelper->getParameterInterface()) m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } + { + SliderParams slider("Stiffness Damping", &damping_beta); + slider.m_minVal = 0; + slider.m_maxVal = 0.1; + if (m_guiHelper->getParameterInterface()) + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } } void VolumetricDeformable::exitPhysics() diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index fc34707fc..ca06b7d11 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -373,6 +373,10 @@ SET(BulletExampleBrowser_SRCS ../DeformableDemo/SplitImpulse.h ../DeformableDemo/VolumetricDeformable.cpp ../DeformableDemo/VolumetricDeformable.h + ../DeformableDemo/Collide.cpp + ../DeformableDemo/Collide.h + ../DeformableDemo/LargeDeformation.cpp + ../DeformableDemo/LargeDeformation.h ../DeformableDemo/DeformableClothAnchor.cpp ../DeformableDemo/DeformableClothAnchor.h ../DeformableDemo/MultibodyClothAnchor.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index e59641d18..127ba5172 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -52,6 +52,8 @@ #include "../DeformableDemo/PinchFriction.h" #include "../DeformableDemo/DeformableMultibody.h" #include "../DeformableDemo/VolumetricDeformable.h" +#include "../DeformableDemo/LargeDeformation.h" +#include "../DeformableDemo/Collide.h" #include "../DeformableDemo/GraspDeformable.h" #include "../DeformableDemo/DeformableContact.h" #include "../DeformableDemo/DeformableClothAnchor.h" @@ -202,6 +204,8 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(1, "Grasp Deformable Cube", "Grasping test", PinchCreateFunc), ExampleEntry(1, "Grasp Deformable with Motor", "Grasping test", GraspDeformableCreateFunc), ExampleEntry(1, "Volumetric Deformable Objects", "Volumetric Deformable test", VolumetricDeformableCreateFunc), + ExampleEntry(1, "Extreme Deformation", "Recovery from extreme deformation", LargeDeformationCreateFunc), + ExampleEntry(1, "Colliding Test", "Volumetric deformable collide with rigid box", CollideCreateFunc), ExampleEntry(1, "Rigid Cloth Anchor", "Deformable Rigid body Anchor test", DeformableClothAnchorCreateFunc), ExampleEntry(1, "Multibody Cloth Anchor", "Deformable Multibody Anchor test", MultibodyClothAnchorCreateFunc), ExampleEntry(1, "Deformable-MultiBody Contact", "MultiBody and Deformable contact", DeformableMultibodyCreateFunc), diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py index 309de425c..6cdbb7e0c 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py @@ -10,16 +10,28 @@ import pybullet_data import pybullet as p1 import random +from enum import Enum + +class InitializationStrategy(Enum): + """Set how the environment is initialized.""" + START = 0 + RANDOM = 1 # random state initialization (RSI) + class PyBulletDeepMimicEnv(Env): - def __init__(self, arg_parser=None, enable_draw=False, pybullet_client=None): + def __init__(self, arg_parser=None, enable_draw=False, pybullet_client=None, + time_step=1./240, + init_strategy=InitializationStrategy.RANDOM): super().__init__(arg_parser, enable_draw) self._num_agents = 1 self._pybullet_client = pybullet_client self._isInitialized = False self._useStablePD = True self._arg_parser = arg_parser + self.timeStep = time_step + self._init_strategy = init_strategy + print("Initialization strategy: {:s}".format(init_strategy)) self.reset() def reset(self): @@ -52,7 +64,7 @@ class PyBulletDeepMimicEnv(Env): motionPath = pybullet_data.getDataPath() + "/" + motion_file[0] #motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt" self._mocapData.Load(motionPath) - timeStep = 1. / 240. + timeStep = self.timeStep useFixedBase = False self._humanoid = humanoid_stable_pd.HumanoidStablePD(self._pybullet_client, self._mocapData, timeStep, useFixedBase, self._arg_parser) @@ -77,9 +89,14 @@ class PyBulletDeepMimicEnv(Env): time.sleep(timeStep) #print("numframes = ", self._humanoid._mocap_data.NumFrames()) #startTime = random.randint(0,self._humanoid._mocap_data.NumFrames()-2) - rnrange = 1000 - rn = random.randint(0, rnrange) - startTime = float(rn) / rnrange * self._humanoid.getCycleTime() + + if self._init_strategy == InitializationStrategy.RANDOM: + rnrange = 1000 + rn = random.randint(0, rnrange) + startTime = float(rn) / rnrange * self._humanoid.getCycleTime() + elif self._init_strategy == InitializationStrategy.START: + startTime = 0 + self.t = startTime self._humanoid.setSimTime(startTime) @@ -263,6 +280,7 @@ class PyBulletDeepMimicEnv(Env): #print("pybullet_deep_mimic_env:update timeStep=",timeStep," t=",self.t) self._pybullet_client.setTimeStep(timeStep) self._humanoid._timeStep = timeStep + self.timeStep = timeStep for i in range(1): self.t += timeStep @@ -314,7 +332,7 @@ class PyBulletDeepMimicEnv(Env): def check_valid_episode(self): #could check if limbs exceed velocity threshold - return true + return True def getKeyboardEvents(self): return self._pybullet_client.getKeyboardEvents() diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/deep_mimic_env.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/deep_mimic_env.py index 4a829db75..ec060dd95 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/deep_mimic_env.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/gym_env/deep_mimic_env.py @@ -19,18 +19,29 @@ import pybullet as p2 import pybullet_data from pybullet_utils import bullet_client as bc from pkg_resources import parse_version -from pybullet_envs.deep_mimic.env.pybullet_deep_mimic_env import PyBulletDeepMimicEnv +from pybullet_envs.deep_mimic.env.pybullet_deep_mimic_env import PyBulletDeepMimicEnv, InitializationStrategy from pybullet_utils.arg_parser import ArgParser from pybullet_utils.logger import Logger +from typing import Optional + logger = logging.getLogger(__name__) class HumanoidDeepBulletEnv(gym.Env): + """Base Gym environment for DeepMimic.""" metadata = {'render.modes': ['human', 'rgb_array'], 'video.frames_per_second': 50} - def __init__(self, renders=False, arg_file=''): - + def __init__(self, renders=False, arg_file='', test_mode=False, + time_step=1./240, + rescale_actions=True, + rescale_observations=True): + """ + Args: + test_mode (bool): in test mode, the `reset()` method will always set the mocap clip time + to 0. + time_step (float): physics time step. + """ self._arg_parser = ArgParser() Logger.print2("===========================================================") succ = False @@ -39,15 +50,33 @@ class HumanoidDeepBulletEnv(gym.Env): succ = self._arg_parser.load_file(path) Logger.print2(arg_file) assert succ, Logger.print2('Failed to load args from: ' + arg_file) - - self._p = None - self._time_step = 1./240. - self._internal_env = None + + self._p: Optional[BulletClient] = None + self._time_step = time_step + self._internal_env: Optional[PyBulletDeepMimicEnv] = None self._renders = renders self._discrete_actions = False - self._arg_file=arg_file - self._render_height = 200 - self._render_width = 320 + self._arg_file = arg_file + self._render_height = 400 + self._render_width = 640 + self._rescale_actions = rescale_actions + self._rescale_observations = rescale_observations + self.agent_id = -1 + + self._numSteps = None + self.test_mode = test_mode + if self.test_mode: + print("Environment running in TEST mode") + + self.reset() + + # Query the policy at 30Hz + self.policy_query_30 = True + if self.policy_query_30: + self._policy_step = 1./30 + else: + self._policy_step = 1./240 + self._num_env_steps = int(self._policy_step / self._time_step) self.theta_threshold_radians = 12 * 2 * math.pi / 360 self.x_threshold = 0.4 #2.4 @@ -94,8 +123,6 @@ class HumanoidDeepBulletEnv(gym.Env): self.viewer = None self._configure() - # Query the policy at 30Hz - self.policy_query_30 = True def _configure(self, display=None): self.display = display @@ -105,42 +132,73 @@ class HumanoidDeepBulletEnv(gym.Env): return [seed] def step(self, action): - - #apply control action - agent_id = -1 + agent_id = self.agent_id + + if self._rescale_actions: + # Rescale the action + mean = -self._action_offset + std = 1./self._action_scale + action = action * std + mean + + # Record reward + reward = self._internal_env.calc_reward(agent_id) + + # Apply control action self._internal_env.set_action(agent_id, action) - - #step sim - self._internal_env.update(self._time_step) - - if self.policy_query_30: - # Step simulation until a new action is required - while not self._internal_env.need_new_action(agent_id): - self._p.stepSimulation() - self._internal_env.t += self._time_step - # print("t:", self._internal_env.t) - # print("next update time:", self._internal_env.needs_update_time) - # print("Performing update") - - #record state + + start_time = self._internal_env.t + + # step sim + for i in range(self._num_env_steps): + self._internal_env.update(self._time_step) + + elapsed_time = self._internal_env.t - start_time + + self._numSteps += 1 + + # Record state self.state = self._internal_env.record_state(agent_id) - #record reward - reward = self._internal_env.calc_reward(agent_id) - - #record done + if self._rescale_observations: + state = np.array(self.state) + mean = -self._state_offset + std = 1./self._state_scale + state = (state - mean) / (std + 1e-8) + + # Record done done = self._internal_env.is_episode_end() - return np.array(self.state), reward, done, {} + info = {} + return state, reward, done, info def reset(self): - # print("-----------reset simulation---------------") - if self._internal_env==None: - self._internal_env = PyBulletDeepMimicEnv(self._arg_parser, self._renders) + # use the initialization strategy + if self._internal_env is None: + if self.test_mode: + init_strat = InitializationStrategy.START + else: + init_strat = InitializationStrategy.RANDOM + self._internal_env = PyBulletDeepMimicEnv(self._arg_parser, self._renders, + time_step=self._time_step, + init_strategy=init_strat) + self._internal_env.reset() self._p = self._internal_env._pybullet_client - agent_id = -1 #unused here - state = self._internal_env.record_state(agent_id) + agent_id = self.agent_id # unused here + self._state_offset = self._internal_env.build_state_offset(self.agent_id) + self._state_scale = self._internal_env.build_state_scale(self.agent_id) + self._action_offset = self._internal_env.build_action_offset(self.agent_id) + self._action_scale = self._internal_env.build_action_scale(self.agent_id) + self._numSteps = 0 + # Record state + self.state = self._internal_env.record_state(agent_id) + + # return state as ndarray + state = np.array(self.state) + if self._rescale_observations: + mean = -self._state_offset + std = 1./self._state_scale + state = (state - mean) / (std + 1e-8) return state def render(self, mode='human', close=False): @@ -148,8 +206,15 @@ class HumanoidDeepBulletEnv(gym.Env): self._renders = True if mode != "rgb_array": return np.array([]) - base_pos=[0,0,0] - self._cam_dist = 2 + human = self._internal_env._humanoid + base_pos, orn = self._p.getBasePositionAndOrientation(human._sim_model) + base_pos = np.asarray(base_pos) + # track the position + base_pos[1] += 0.3 + rpy = self._p.getEulerFromQuaternion(orn) # rpy, in radians + rpy = 180 / np.pi * np.asarray(rpy) # convert rpy in degrees + + self._cam_dist = 3 self._cam_pitch = 0.3 self._cam_yaw = 0 if (not self._p == None): @@ -159,10 +224,9 @@ class HumanoidDeepBulletEnv(gym.Env): yaw=self._cam_yaw, pitch=self._cam_pitch, roll=0, - upAxisIndex=2) + upAxisIndex=1) proj_matrix = self._p.computeProjectionMatrixFOV(fov=60, - aspect=float(self._render_width) / - self._render_height, + aspect=float(self._render_width) / self._render_height, nearVal=0.1, farVal=100.0) (_, _, px, _, _) = self._p.getCameraImage( @@ -171,6 +235,12 @@ class HumanoidDeepBulletEnv(gym.Env): renderer=self._p.ER_BULLET_HARDWARE_OPENGL, viewMatrix=view_matrix, projectionMatrix=proj_matrix) + # self._p.resetDebugVisualizerCamera( + # cameraDistance=2 * self._cam_dist, + # cameraYaw=self._cam_yaw, + # cameraPitch=self._cam_pitch, + # cameraTargetPosition=base_pos + # ) else: px = np.array([[[255,255,255,255]]*self._render_width]*self._render_height, dtype=np.uint8) rgb_array = np.array(px, dtype=np.uint8) diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 6968a3310..3316403a8 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -48,6 +48,7 @@ struct btContactSolverInfoData btScalar m_erp2; //error reduction for contact constraints btScalar m_deformable_erp; //error reduction for deformable constraints btScalar m_deformable_cfm; //constraint force mixing for deformable constraints + btScalar m_deformable_maxErrorReduction; // maxErrorReduction for deformable contact btScalar m_globalCfm; //constraint force mixing for contacts and non-contacts btScalar m_frictionERP; //error reduction for friction constraints btScalar m_frictionCFM; //constraint force mixing for friction constraints @@ -86,6 +87,7 @@ struct btContactSolverInfo : public btContactSolverInfoData m_erp2 = btScalar(0.2); m_deformable_erp = btScalar(0.06); m_deformable_cfm = btScalar(0.01); + m_deformable_maxErrorReduction = btScalar(0.1); m_globalCfm = btScalar(0.); m_frictionERP = btScalar(0.2); //positional friction 'anchors' are disabled by default m_frictionCFM = btScalar(0.); diff --git a/src/BulletSoftBody/btConjugateGradient.h b/src/BulletSoftBody/btConjugateGradient.h index 83b5ab084..bcd5e6b51 100644 --- a/src/BulletSoftBody/btConjugateGradient.h +++ b/src/BulletSoftBody/btConjugateGradient.h @@ -37,6 +37,12 @@ public: BT_PROFILE("CGSolve"); btAssert(x.size() == b.size()); reinitialize(b); + temp = b; + A.project(temp); + p = temp; + A.precondition(p, z); + btScalar d0 = this->dot(z, temp); + d0 = btMin(btScalar(1), d0); // r = b - A * x --with assigned dof zeroed out A.multiply(x, temp); r = this->sub(b, temp); @@ -45,7 +51,7 @@ public: A.precondition(r, z); A.project(z); btScalar r_dot_z = this->dot(z, r); - if (r_dot_z <= Base::m_tolerance) + if (r_dot_z <= Base::m_tolerance * d0) { if (verbose) { @@ -61,7 +67,7 @@ public: // temp = A*p A.multiply(p, temp); A.project(temp); - if (this->dot(p, temp) < SIMD_EPSILON) + if (this->dot(p, temp) < 0) { if (verbose) std::cout << "Encountered negative direction in CG!" << std::endl; @@ -81,11 +87,11 @@ public: A.precondition(r, z); r_dot_z = r_dot_z_new; r_dot_z_new = this->dot(r, z); - if (r_dot_z_new < Base::m_tolerance) + if (r_dot_z_new < Base::m_tolerance * d0) { if (verbose) { - std::cout << "ConjugateGradient iterations " << k << std::endl; + std::cout << "ConjugateGradient iterations " << k << " residual = " << r_dot_z_new << std::endl; } return k; } @@ -95,7 +101,7 @@ public: } if (verbose) { - std::cout << "ConjugateGradient max iterations reached " << Base::m_maxIterations << std::endl; + std::cout << "ConjugateGradient max iterations reached " << Base::m_maxIterations << " error = " << r_dot_z_new << std::endl; } return Base::m_maxIterations; } diff --git a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp index c2243d9d5..e3b8a462b 100644 --- a/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp +++ b/src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp @@ -46,6 +46,17 @@ void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar { m_lf[i]->reinitialize(nodeUpdated); } + btMatrix3x3 I; + I.setIdentity(); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (psb->m_nodes[j].m_im > 0) + psb->m_nodes[j].m_effectiveMass = I * (1.0 / psb->m_nodes[j].m_im); + } + } m_projection.reinitialize(nodeUpdated); // m_preconditioner->reinitialize(nodeUpdated); } @@ -133,10 +144,23 @@ void btDeformableBackwardEulerObjective::applyForce(TVStack& force, bool setZero counter += psb->m_nodes.size(); continue; } - for (int j = 0; j < psb->m_nodes.size(); ++j) + if (m_implicit) { - btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im; - psb->m_nodes[j].m_v += one_over_mass * force[counter++]; + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + if (psb->m_nodes[j].m_im != 0) + { + psb->m_nodes[j].m_v += psb->m_nodes[j].m_effectiveMass_inv * force[counter++]; + } + } + } + else + { + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btScalar one_over_mass = (psb->m_nodes[j].m_im == 0) ? 0 : psb->m_nodes[j].m_im; + psb->m_nodes[j].m_v += one_over_mass * force[counter++]; + } } } if (setZero) @@ -190,10 +214,56 @@ void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force) { m_softBodies[i]->advanceDeformation(); } - - for (int i = 0; i < m_lf.size(); ++i) + if (m_implicit) { - m_lf[i]->addScaledExplicitForce(m_dt, force); + // apply forces except gravity force + btVector3 gravity; + for (int i = 0; i < m_lf.size(); ++i) + { + if (m_lf[i]->getForceType() == BT_GRAVITY_FORCE) + { + gravity = static_cast(m_lf[i])->m_gravity; + } + else + { + m_lf[i]->addScaledForces(m_dt, force); + } + } + for (int i = 0; i < m_lf.size(); ++i) + { + m_lf[i]->addScaledHessian(m_dt); + } + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + // add gravity explicitly + psb->m_nodes[j].m_v += m_dt * psb->m_gravityFactor * gravity; + } + } + } + } + else + { + for (int i = 0; i < m_lf.size(); ++i) + { + m_lf[i]->addScaledExplicitForce(m_dt, force); + } + } + // calculate inverse mass matrix for all nodes + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_effectiveMass_inv = psb->m_nodes[j].m_effectiveMass.inverse(); + } + } } applyForce(force, true); } diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 3e8b8afe7..4916848fe 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -20,7 +20,7 @@ #include "LinearMath/btQuickprof.h" static const int kMaxConjugateGradientIterations = 300; btDeformableBodySolver::btDeformableBodySolver() - : m_numNodes(0), m_cg(kMaxConjugateGradientIterations), m_cr(kMaxConjugateGradientIterations), m_maxNewtonIterations(5), m_newtonTolerance(1e-4), m_lineSearch(false), m_useProjection(false) + : m_numNodes(0), m_cg(kMaxConjugateGradientIterations), m_cr(kMaxConjugateGradientIterations), m_maxNewtonIterations(1), m_newtonTolerance(1e-4), m_lineSearch(false), m_useProjection(false) { m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity); } @@ -276,7 +276,14 @@ void btDeformableBodySolver::updateVelocity() { m_dv[counter].setZero(); } - psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter] - psb->m_nodes[j].m_splitv; + if (m_implicit) + { + psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter]; + } + else + { + psb->m_nodes[j].m_v = m_backupVelocity[counter] + m_dv[counter] - psb->m_nodes[j].m_splitv; + } psb->m_maxSpeedSquared = btMax(psb->m_maxSpeedSquared, psb->m_nodes[j].m_v.length2()); ++counter; } @@ -296,7 +303,7 @@ void btDeformableBodySolver::updateTempPosition() } 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; + psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + m_dt * (psb->m_nodes[j].m_v + psb->m_nodes[j].m_splitv); ++counter; } psb->updateDeformation(); @@ -331,8 +338,9 @@ void btDeformableBodySolver::setupDeformableSolve(bool implicit) { if (implicit) { - if ((psb->m_nodes[j].m_v - m_backupVelocity[counter]).norm() < SIMD_EPSILON) - m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter]; + // setting the initial guess for newton, need m_dv = v_{n+1} - v_n for dofs that are in constraint. + if (psb->m_nodes[j].m_v == m_backupVelocity[counter]) + m_dv[counter].setZero(); else m_dv[counter] = psb->m_nodes[j].m_v - psb->m_nodes[j].m_vn; m_backupVelocity[counter] = psb->m_nodes[j].m_vn; @@ -376,6 +384,20 @@ bool btDeformableBodySolver::updateNodes() void btDeformableBodySolver::predictMotion(btScalar solverdt) { // apply explicit forces to velocity + if (m_implicit) + { + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (psb->isActive()) + { + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + psb->m_nodes[j].m_v * solverdt; + } + } + } + } m_objective->applyExplicitForce(m_residual); for (int i = 0; i < m_softBodies.size(); ++i) { @@ -424,12 +446,19 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d // apply drag n.m_v *= (1 - psb->m_cfg.drag); // scale velocity back - if (n.m_v.norm() > max_v) + if (m_implicit) { - n.m_v.safeNormalize(); - n.m_v *= max_v; + n.m_q = n.m_x; + } + else + { + if (n.m_v.norm() > max_v) + { + n.m_v.safeNormalize(); + n.m_v *= max_v; + } + n.m_q = n.m_x + n.m_v * dt; } - n.m_q = n.m_x + n.m_v * dt; n.m_splitv.setZero(); n.m_constrained = false; } diff --git a/src/BulletSoftBody/btDeformableContactConstraint.cpp b/src/BulletSoftBody/btDeformableContactConstraint.cpp index c2211840d..09398d79a 100644 --- a/src/BulletSoftBody/btDeformableContactConstraint.cpp +++ b/src/BulletSoftBody/btDeformableContactConstraint.cpp @@ -278,19 +278,19 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv btVector3 impulse_tangent = impulse - impulse_normal; if (dn > 0) { - m_binding = false; return 0; } m_binding = true; btScalar residualSquare = dn * dn; btVector3 old_total_tangent_dv = m_total_tangent_dv; - // m_c2 is the inverse mass of the deformable node/face - m_total_normal_dv -= impulse_normal * m_contact->m_c2; - m_total_tangent_dv -= impulse_tangent * m_contact->m_c2; + // m_c5 is the inverse mass of the deformable node/face + m_total_normal_dv -= m_contact->m_c5 * impulse_normal; + m_total_tangent_dv -= m_contact->m_c5 * impulse_tangent; if (m_total_normal_dv.dot(cti.m_normal) < 0) { // separating in the normal direction + m_binding = false; m_static = false; impulse_tangent.setZero(); } @@ -309,7 +309,8 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv { m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.safeNorm() * m_contact->m_c3; } - impulse_tangent = -btScalar(1) / m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv); + // impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv); + impulse_tangent = m_contact->m_c5.inverse() * (old_total_tangent_dv - m_total_tangent_dv); } else { @@ -354,7 +355,7 @@ btScalar btDeformableRigidContactConstraint::solveConstraint(const btContactSolv btScalar btDeformableRigidContactConstraint::solveSplitImpulse(const btContactSolverInfo& infoGlobal) { - btScalar MAX_PENETRATION_CORRECTION = 0.1; + btScalar MAX_PENETRATION_CORRECTION = infoGlobal.m_deformable_maxErrorReduction; const btSoftBody::sCti& cti = m_contact->m_cti; btVector3 vb = getSplitVb(); btVector3 va = getSplitVa(); @@ -435,14 +436,14 @@ btVector3 btDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse) { const btSoftBody::DeformableNodeRigidContact* contact = getContact(); - btVector3 dv = impulse * contact->m_c2; + btVector3 dv = contact->m_c5 * impulse; contact->m_node->m_v -= dv; } void btDeformableNodeRigidContactConstraint::applySplitImpulse(const btVector3& impulse) { const btSoftBody::DeformableNodeRigidContact* contact = getContact(); - btVector3 dv = impulse * contact->m_c2; + btVector3 dv = contact->m_c5 * impulse; contact->m_node->m_splitv -= dv; } diff --git a/src/BulletSoftBody/btDeformableContactProjection.cpp b/src/BulletSoftBody/btDeformableContactProjection.cpp index f05871c3d..584b9d6ec 100644 --- a/src/BulletSoftBody/btDeformableContactProjection.cpp +++ b/src/BulletSoftBody/btDeformableContactProjection.cpp @@ -70,6 +70,12 @@ btScalar btDeformableContactProjection::solveSplitImpulse(btCollisionObject** de { continue; } + for (int k = 0; k < m_nodeRigidConstraints[j].size(); ++k) + { + btDeformableNodeRigidContactConstraint& constraint = m_nodeRigidConstraints[j][k]; + btScalar localResidualSquare = constraint.solveSplitImpulse(infoGlobal); + residualSquare = btMax(residualSquare, localResidualSquare); + } for (int k = 0; k < m_faceRigidConstraints[j].size(); ++k) { btDeformableFaceRigidContactConstraint& constraint = m_faceRigidConstraints[j][k]; @@ -140,15 +146,6 @@ void btDeformableContactProjection::setConstraints(const btContactSolverInfo& in } btDeformableFaceRigidContactConstraint constraint(contact, infoGlobal, m_useStrainLimiting); m_faceRigidConstraints[i].push_back(constraint); - // btVector3 va = constraint.getVa(); - // btVector3 vb = constraint.getVb(); - // const btVector3 vr = vb - va; - // const btSoftBody::sCti& cti = contact.m_cti; - // const btScalar dn = btDot(vr, cti.m_normal); - // if (dn < SIMD_EPSILON) - // { - // m_faceRigidConstraints[i].push_back(constraint); - // } } } } @@ -175,7 +172,6 @@ void btDeformableContactProjection::project(TVStack& x) if (free_dir.safeNorm() < SIMD_EPSILON) { x[i] -= x[i].dot(dir0) * dir0; - x[i] -= x[i].dot(dir1) * dir1; } else { @@ -256,33 +252,36 @@ void btDeformableContactProjection::setProjection() { int index = m_nodeRigidConstraints[i][j].m_node->index; m_nodeRigidConstraints[i][j].m_node->m_constrained = true; - if (m_nodeRigidConstraints[i][j].m_static) + if (m_nodeRigidConstraints[i][j].m_binding) { - if (m_projectionsDict.find(index) == NULL) + if (m_nodeRigidConstraints[i][j].m_static) { - m_projectionsDict.insert(index, units); - } - else - { - btAlignedObjectArray& projections = *m_projectionsDict[index]; - for (int k = 0; k < 3; ++k) + if (m_projectionsDict.find(index) == NULL) { - projections.push_back(units[k]); + m_projectionsDict.insert(index, units); + } + else + { + btAlignedObjectArray& projections = *m_projectionsDict[index]; + for (int k = 0; k < 3; ++k) + { + projections.push_back(units[k]); + } } } - } - else - { - if (m_projectionsDict.find(index) == NULL) - { - btAlignedObjectArray projections; - projections.push_back(m_nodeRigidConstraints[i][j].m_normal); - m_projectionsDict.insert(index, projections); - } else { - btAlignedObjectArray& projections = *m_projectionsDict[index]; - projections.push_back(m_nodeRigidConstraints[i][j].m_normal); + if (m_projectionsDict.find(index) == NULL) + { + btAlignedObjectArray projections; + projections.push_back(m_nodeRigidConstraints[i][j].m_normal); + m_projectionsDict.insert(index, projections); + } + else + { + btAlignedObjectArray& projections = *m_projectionsDict[index]; + projections.push_back(m_nodeRigidConstraints[i][j].m_normal); + } } } } @@ -309,9 +308,9 @@ void btDeformableContactProjection::setProjection() else { btAlignedObjectArray& projections = *m_projectionsDict[index]; - for (int k = 0; k < 3; ++k) + for (int l = 0; l < 3; ++l) { - projections.push_back(units[k]); + projections.push_back(units[l]); } } } diff --git a/src/BulletSoftBody/btDeformableLagrangianForce.h b/src/BulletSoftBody/btDeformableLagrangianForce.h index 8cfd4c782..d58d825d1 100644 --- a/src/BulletSoftBody/btDeformableLagrangianForce.h +++ b/src/BulletSoftBody/btDeformableLagrangianForce.h @@ -66,6 +66,8 @@ public: // add all damping forces virtual void addScaledDampingForce(btScalar scale, TVStack& force) = 0; + virtual void addScaledHessian(btScalar scale) {} + virtual btDeformableLagrangianForceType getForceType() = 0; virtual void reinitialize(bool nodeUpdated) diff --git a/src/BulletSoftBody/btDeformableLinearElasticityForce.h b/src/BulletSoftBody/btDeformableLinearElasticityForce.h index 0f2c1d577..971192050 100644 --- a/src/BulletSoftBody/btDeformableLinearElasticityForce.h +++ b/src/BulletSoftBody/btDeformableLinearElasticityForce.h @@ -18,23 +18,64 @@ #include "btDeformableLagrangianForce.h" #include "LinearMath/btQuickprof.h" +#include "btSoftBodyInternals.h" +#define TETRA_FLAT_THRESHOLD 0.01 class btDeformableLinearElasticityForce : public btDeformableLagrangianForce { public: typedef btAlignedObjectArray TVStack; btScalar m_mu, m_lambda; - btScalar m_mu_damp, m_lambda_damp; - btDeformableLinearElasticityForce() : m_mu(1), m_lambda(1) + btScalar m_E, m_nu; // Young's modulus and Poisson ratio + btScalar m_damping_alpha, m_damping_beta; + btDeformableLinearElasticityForce() : m_mu(1), m_lambda(1), m_damping_alpha(0.01), m_damping_beta(0.01) { - btScalar damping = 0.05; - m_mu_damp = damping * m_mu; - m_lambda_damp = damping * m_lambda; + updateYoungsModulusAndPoissonRatio(); } - btDeformableLinearElasticityForce(btScalar mu, btScalar lambda, btScalar damping = 0.05) : m_mu(mu), m_lambda(lambda) + btDeformableLinearElasticityForce(btScalar mu, btScalar lambda, btScalar damping_alpha = 0.01, btScalar damping_beta = 0.01) : m_mu(mu), m_lambda(lambda), m_damping_alpha(damping_alpha), m_damping_beta(damping_beta) { - m_mu_damp = damping * m_mu; - m_lambda_damp = damping * m_lambda; + updateYoungsModulusAndPoissonRatio(); + } + + void updateYoungsModulusAndPoissonRatio() + { + // conversion from Lame Parameters to Young's modulus and Poisson ratio + // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters + m_E = m_mu * (3 * m_lambda + 2 * m_mu) / (m_lambda + m_mu); + m_nu = m_lambda * 0.5 / (m_mu + m_lambda); + } + + void updateLameParameters() + { + // conversion from Young's modulus and Poisson ratio to Lame Parameters + // https://en.wikipedia.org/wiki/Lam%C3%A9_parameters + m_mu = m_E * 0.5 / (1 + m_nu); + m_lambda = m_E * m_nu / ((1 + m_nu) * (1 - 2 * m_nu)); + } + + void setYoungsModulus(btScalar E) + { + m_E = E; + updateLameParameters(); + } + + void setPoissonRatio(btScalar nu) + { + m_nu = nu; + updateLameParameters(); + } + + void setDamping(btScalar damping_alpha, btScalar damping_beta) + { + m_damping_alpha = damping_alpha; + m_damping_beta = damping_beta; + } + + void setLameParameters(btScalar mu, btScalar lambda) + { + m_mu = mu; + m_lambda = lambda; + updateYoungsModulusAndPoissonRatio(); } virtual void addScaledForces(btScalar scale, TVStack& force) @@ -51,8 +92,10 @@ public: // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search virtual void addScaledDampingForce(btScalar scale, TVStack& force) { - if (m_mu_damp == 0 && m_lambda_damp == 0) + if (m_damping_alpha == 0 && m_damping_beta == 0) return; + btScalar mu_damp = m_damping_beta * m_mu; + btScalar lambda_damp = m_damping_beta * m_lambda; int numNodes = getNumNodes(); btAssert(numNodes <= force.size()); btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); @@ -65,6 +108,7 @@ public: } for (int j = 0; j < psb->m_tetras.size(); ++j) { + bool close_to_flat = (psb->m_tetraScratches[j].m_J < TETRA_FLAT_THRESHOLD); btSoftBody::Tetra& tetra = psb->m_tetras[j]; btSoftBody::Node* node0 = tetra.m_n[0]; btSoftBody::Node* node1 = tetra.m_n[1]; @@ -75,13 +119,19 @@ public: size_t id2 = node2->index; size_t id3 = node3->index; btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse; + if (!close_to_flat) + { + dF = psb->m_tetraScratches[j].m_corotation.transpose() * dF; + } btMatrix3x3 I; I.setIdentity(); - btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0] + dF[1][1] + dF[2][2]) * m_lambda_damp; - // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); - btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose() * grad_N_hat_1st_col); + btMatrix3x3 dP = (dF + dF.transpose()) * mu_damp + I * ((dF[0][0] + dF[1][1] + dF[2][2]) * lambda_damp); btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); - + if (!close_to_flat) + { + df_on_node123 = psb->m_tetraScratches[j].m_corotation * df_on_node123; + } + btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; // damping force differential btScalar scale1 = scale * tetra.m_element_measure; force[id0] -= scale1 * df_on_node0; @@ -89,6 +139,15 @@ public: force[id2] -= scale1 * df_on_node123.getColumn(1); force[id3] -= scale1 * df_on_node123.getColumn(2); } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + size_t id = node.index; + if (node.m_im > 0) + { + force[id] -= scale * node.m_v / node.m_im * m_damping_alpha; + } + } } } @@ -201,7 +260,7 @@ public: } #endif // btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); - btMatrix3x3 force_on_node123 = P * tetra.m_Dm_inverse.transpose(); + btMatrix3x3 force_on_node123 = psb->m_tetraScratches[j].m_corotation * P * tetra.m_Dm_inverse.transpose(); btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col; btSoftBody::Node* node0 = tetra.m_n[0]; @@ -223,11 +282,15 @@ public: } } + virtual void buildDampingForceDifferentialDiagonal(btScalar scale, TVStack& diagA) {} + // The damping matrix is calculated using the time n state as described in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df) { - if (m_mu_damp == 0 && m_lambda_damp == 0) + if (m_damping_alpha == 0 && m_damping_beta == 0) return; + btScalar mu_damp = m_damping_beta * m_mu; + btScalar lambda_damp = m_damping_beta * m_lambda; int numNodes = getNumNodes(); btAssert(numNodes <= df.size()); btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); @@ -240,6 +303,7 @@ public: } for (int j = 0; j < psb->m_tetras.size(); ++j) { + bool close_to_flat = (psb->m_tetraScratches[j].m_J < TETRA_FLAT_THRESHOLD); btSoftBody::Tetra& tetra = psb->m_tetras[j]; btSoftBody::Node* node0 = tetra.m_n[0]; btSoftBody::Node* node1 = tetra.m_n[1]; @@ -250,12 +314,18 @@ public: size_t id2 = node2->index; size_t id3 = node3->index; btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse; + if (!close_to_flat) + { + dF = psb->m_tetraScratches[j].m_corotation.transpose() * dF; + } btMatrix3x3 I; I.setIdentity(); - btMatrix3x3 dP = (dF + dF.transpose()) * m_mu_damp + I * (dF[0][0] + dF[1][1] + dF[2][2]) * m_lambda_damp; - // firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP); - // btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col); + btMatrix3x3 dP = (dF + dF.transpose()) * mu_damp + I * ((dF[0][0] + dF[1][1] + dF[2][2]) * lambda_damp); btMatrix3x3 df_on_node123 = dP * tetra.m_Dm_inverse.transpose(); + if (!close_to_flat) + { + df_on_node123 = psb->m_tetraScratches[j].m_corotation * df_on_node123; + } btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; // damping force differential @@ -265,6 +335,15 @@ public: df[id2] -= scale1 * df_on_node123.getColumn(1); df[id3] -= scale1 * df_on_node123.getColumn(2); } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + const btSoftBody::Node& node = psb->m_nodes[j]; + size_t id = node.index; + if (node.m_im > 0) + { + df[id] -= scale * dv[id] / node.m_im * m_damping_alpha; + } + } } } @@ -291,11 +370,11 @@ public: 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 dF = psb->m_tetraScratches[j].m_corotation.transpose() * Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse; btMatrix3x3 dP; firstPiolaDifferential(psb->m_tetraScratches[j], 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(); + btMatrix3x3 df_on_node123 = psb->m_tetraScratches[j].m_corotation * dP * tetra.m_Dm_inverse.transpose(); btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col; // elastic force differential @@ -310,7 +389,9 @@ public: void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P) { - btMatrix3x3 epsilon = (s.m_F + s.m_F.transpose()) * 0.5 - btMatrix3x3::getIdentity(); + btMatrix3x3 corotated_F = s.m_corotation.transpose() * s.m_F; + + btMatrix3x3 epsilon = (corotated_F + corotated_F.transpose()) * 0.5 - btMatrix3x3::getIdentity(); btScalar trace = epsilon[0][0] + epsilon[1][1] + epsilon[2][2]; P = epsilon * btScalar(2) * m_mu + btMatrix3x3::getIdentity() * m_lambda * trace; } @@ -327,8 +408,50 @@ public: // This function calculates the dP = dQ/dF * dF void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF, btMatrix3x3& dP) { + btScalar mu_damp = m_damping_beta * m_mu; + btScalar lambda_damp = m_damping_beta * m_lambda; btScalar trace = (dF[0][0] + dF[1][1] + dF[2][2]); - dP = (dF + dF.transpose()) * m_mu_damp + btMatrix3x3::getIdentity() * m_lambda_damp * trace; + dP = (dF + dF.transpose()) * mu_damp + btMatrix3x3::getIdentity() * lambda_damp * trace; + } + + virtual void addScaledHessian(btScalar scale) + { + btVector3 grad_N_hat_1st_col = btVector3(-1, -1, -1); + for (int i = 0; i < m_softBodies.size(); ++i) + { + btSoftBody* psb = m_softBodies[i]; + if (!psb->isActive()) + { + continue; + } + for (int j = 0; j < psb->m_tetras.size(); ++j) + { + btSoftBody::Tetra& tetra = psb->m_tetras[j]; + btMatrix3x3 P; + firstPiola(psb->m_tetraScratches[j], P); // make sure scratch is evaluated at x_n + dt * vn + btMatrix3x3 force_on_node123 = psb->m_tetraScratches[j].m_corotation * P * tetra.m_Dm_inverse.transpose(); + btVector3 force_on_node0 = force_on_node123 * grad_N_hat_1st_col; + 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]; + btScalar scale1 = scale * (scale + m_damping_beta) * tetra.m_element_measure; // stiff and stiffness-damping terms; + node0->m_effectiveMass += OuterProduct(force_on_node0, force_on_node0) * scale1; + node1->m_effectiveMass += OuterProduct(force_on_node123.getColumn(0), force_on_node123.getColumn(0)) * scale1; + node2->m_effectiveMass += OuterProduct(force_on_node123.getColumn(1), force_on_node123.getColumn(1)) * scale1; + node3->m_effectiveMass += OuterProduct(force_on_node123.getColumn(2), force_on_node123.getColumn(2)) * scale1; + } + for (int j = 0; j < psb->m_nodes.size(); ++j) + { + btSoftBody::Node& node = psb->m_nodes[j]; + if (node.m_im > 0) + { + btMatrix3x3 I; + I.setIdentity(); + node.m_effectiveMass += I * (scale * (1.0 / node.m_im) * m_damping_alpha); + } + } + } } virtual btDeformableLagrangianForceType getForceType() diff --git a/src/BulletSoftBody/btDeformableMousePickingForce.h b/src/BulletSoftBody/btDeformableMousePickingForce.h index 392bf5ac2..d218d9621 100644 --- a/src/BulletSoftBody/btDeformableMousePickingForce.h +++ b/src/BulletSoftBody/btDeformableMousePickingForce.h @@ -128,7 +128,24 @@ public: virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df) { - //TODO + btScalar scaled_stiffness = scale * m_elasticStiffness; + for (int i = 0; i < 3; ++i) + { + btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos); + btScalar dir_norm = dir.norm(); + btVector3 dir_normalized = (dir_norm > SIMD_EPSILON) ? dir.normalized() : btVector3(0, 0, 0); + int id = m_face.m_n[i]->index; + btVector3 dx_diff = dx[id]; + btScalar r = 0; // rest length is 0 for picking spring + btVector3 scaled_df = btVector3(0, 0, 0); + if (dir_norm > SIMD_EPSILON) + { + scaled_df -= scaled_stiffness * dir_normalized.dot(dx_diff) * dir_normalized; + scaled_df += scaled_stiffness * dir_normalized.dot(dx_diff) * ((dir_norm - r) / dir_norm) * dir_normalized; + scaled_df -= scaled_stiffness * ((dir_norm - r) / dir_norm) * dx_diff; + } + df[id] += scaled_df; + } } void setMousePos(const btVector3& p) diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index c30f5bb19..32bae3aa6 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -162,6 +162,11 @@ public: m_lineSearch = lineSearch; } + void setUseProjection(bool useProjection) + { + m_useProjection = useProjection; + } + void applyRepulsionForce(btScalar timeStep); void performGeometricCollisions(btScalar timeStep); diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index c40a3fd08..427c14954 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -900,6 +900,7 @@ void btSoftBody::setVelocity(const btVector3& velocity) if (n.m_im > 0) { n.m_v = velocity; + n.m_vn = velocity; } } } @@ -2772,6 +2773,7 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr shp, nrm, margin); + if (!predict) { cti.m_colObj = colObjWrap->getCollisionObject(); @@ -3462,11 +3464,46 @@ void btSoftBody::initializeDmInverse() c1.getZ(), c2.getZ(), c3.getZ()); t.m_element_measure = Dm.determinant() * unit_simplex_measure; t.m_Dm_inverse = Dm.inverse(); + + // calculate the first three columns of P^{-1} + btVector3 a = t.m_n[0]->m_x; + btVector3 b = t.m_n[1]->m_x; + btVector3 c = t.m_n[2]->m_x; + btVector3 d = t.m_n[3]->m_x; + + btScalar det = 1 / (a[0] * b[1] * c[2] - a[0] * b[1] * d[2] - a[0] * b[2] * c[1] + a[0] * b[2] * d[1] + a[0] * c[1] * d[2] - a[0] * c[2] * d[1] + a[1] * (-b[0] * c[2] + b[0] * d[2] + b[2] * c[0] - b[2] * d[0] - c[0] * d[2] + c[2] * d[0]) + a[2] * (b[0] * c[1] - b[0] * d[1] + b[1] * (d[0] - c[0]) + c[0] * d[1] - c[1] * d[0]) - b[0] * c[1] * d[2] + b[0] * c[2] * d[1] + b[1] * c[0] * d[2] - b[1] * c[2] * d[0] - b[2] * c[0] * d[1] + b[2] * c[1] * d[0]); + + btScalar P11 = -b[2] * c[1] + d[2] * c[1] + b[1] * c[2] + b[2] * d[1] - c[2] * d[1] - b[1] * d[2]; + btScalar P12 = b[2] * c[0] - d[2] * c[0] - b[0] * c[2] - b[2] * d[0] + c[2] * d[0] + b[0] * d[2]; + btScalar P13 = -b[1] * c[0] + d[1] * c[0] + b[0] * c[1] + b[1] * d[0] - c[1] * d[0] - b[0] * d[1]; + btScalar P21 = a[2] * c[1] - d[2] * c[1] - a[1] * c[2] - a[2] * d[1] + c[2] * d[1] + a[1] * d[2]; + btScalar P22 = -a[2] * c[0] + d[2] * c[0] + a[0] * c[2] + a[2] * d[0] - c[2] * d[0] - a[0] * d[2]; + btScalar P23 = a[1] * c[0] - d[1] * c[0] - a[0] * c[1] - a[1] * d[0] + c[1] * d[0] + a[0] * d[1]; + btScalar P31 = -a[2] * b[1] + d[2] * b[1] + a[1] * b[2] + a[2] * d[1] - b[2] * d[1] - a[1] * d[2]; + btScalar P32 = a[2] * b[0] - d[2] * b[0] - a[0] * b[2] - a[2] * d[0] + b[2] * d[0] + a[0] * d[2]; + btScalar P33 = -a[1] * b[0] + d[1] * b[0] + a[0] * b[1] + a[1] * d[0] - b[1] * d[0] - a[0] * d[1]; + btScalar P41 = a[2] * b[1] - c[2] * b[1] - a[1] * b[2] - a[2] * c[1] + b[2] * c[1] + a[1] * c[2]; + btScalar P42 = -a[2] * b[0] + c[2] * b[0] + a[0] * b[2] + a[2] * c[0] - b[2] * c[0] - a[0] * c[2]; + btScalar P43 = a[1] * b[0] - c[1] * b[0] - a[0] * b[1] - a[1] * c[0] + b[1] * c[0] + a[0] * c[1]; + + btVector4 p1(P11 * det, P21 * det, P31 * det, P41 * det); + btVector4 p2(P12 * det, P22 * det, P32 * det, P42 * det); + btVector4 p3(P13 * det, P23 * det, P33 * det, P43 * det); + + t.m_P_inv[0] = p1; + t.m_P_inv[1] = p2; + t.m_P_inv[2] = p3; } } +static btScalar Dot4(const btVector4& a, const btVector4& b) +{ + return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3]; +} + void btSoftBody::updateDeformation() { + btQuaternion q; for (int i = 0; i < m_tetras.size(); ++i) { btSoftBody::Tetra& t = m_tetras[i]; @@ -3484,6 +3521,21 @@ void btSoftBody::updateDeformation() btMatrix3x3 C = t.m_F.transpose() * t.m_F; s.m_trace = C[0].getX() + C[1].getY() + C[2].getZ(); s.m_cofF = t.m_F.adjoint().transpose(); + + btVector3 a = t.m_n[0]->m_q; + btVector3 b = t.m_n[1]->m_q; + btVector3 c = t.m_n[2]->m_q; + btVector3 d = t.m_n[3]->m_q; + btVector4 q1(a[0], b[0], c[0], d[0]); + btVector4 q2(a[1], b[1], c[1], d[1]); + btVector4 q3(a[2], b[2], c[2], d[2]); + btMatrix3x3 B(Dot4(q1, t.m_P_inv[0]), Dot4(q1, t.m_P_inv[1]), Dot4(q1, t.m_P_inv[2]), + Dot4(q2, t.m_P_inv[0]), Dot4(q2, t.m_P_inv[1]), Dot4(q2, t.m_P_inv[2]), + Dot4(q3, t.m_P_inv[0]), Dot4(q3, t.m_P_inv[1]), Dot4(q3, t.m_P_inv[2])); + q.setRotation(btVector3(0, 0, 1), 0); + B.extractRotation(q, 0.01); // precision of the rotation is not very important for visual correctness. + btMatrix3x3 Q(q); + s.m_corotation = Q; } } diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index 885e2368b..efe3f5f3c 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -272,7 +272,9 @@ public: int m_constrained; // depth of penetration int m_battach : 1; // Attached int index; - btVector3 m_splitv; // velocity associated with split impulse + btVector3 m_splitv; // velocity associated with split impulse + btMatrix3x3 m_effectiveMass; // effective mass in contact + btMatrix3x3 m_effectiveMass_inv; // inverse of effective mass }; /* Link */ ATTRIBUTE_ALIGNED16(struct) @@ -311,15 +313,17 @@ public: btMatrix3x3 m_Dm_inverse; // rest Dm^-1 btMatrix3x3 m_F; btScalar m_element_measure; + btVector4 m_P_inv[3]; // first three columns of P_inv matrix }; /* TetraScratch */ struct TetraScratch { - btMatrix3x3 m_F; // deformation gradient F - btScalar m_trace; // trace of F^T * F - btScalar m_J; // det(F) - btMatrix3x3 m_cofF; // cofactor of F + btMatrix3x3 m_F; // deformation gradient F + btScalar m_trace; // trace of F^T * F + btScalar m_J; // det(F) + btMatrix3x3 m_cofF; // cofactor of F + btMatrix3x3 m_corotation; // corotatio of the tetra }; /* RContact */ @@ -350,6 +354,7 @@ public: btScalar m_c2; // inverse mass of node/face btScalar m_c3; // Friction btScalar m_c4; // Hardness + btMatrix3x3 m_c5; // inverse effective mass // jacobians and unit impulse responses for multibody btMultiBodyJacobianData jacobianData_normal; @@ -1323,7 +1328,7 @@ public: int face_penetration = 0, node_penetration = node->m_constrained; for (int i = 0; i < 3; ++i) face_penetration |= face->m_n[i]->m_constrained; - btScalar I_tilde = .5 * I / (1.0 + w.length2()); + btScalar I_tilde = 2.0 * I / (1.0 + w.length2()); // double the impulse if node or face is constrained. if (face_penetration > 0 || node_penetration > 0) @@ -1349,10 +1354,10 @@ public: btScalar vt_new = btMax(btScalar(1) - mu * delta_vn / (vt_norm + SIMD_EPSILON), btScalar(0)) * vt_norm; I = 0.5 * mass * (vt_norm - vt_new); vt.safeNormalize(); - I_tilde = .5 * I / (1.0 + w.length2()); + I_tilde = 2.0 * I / (1.0 + w.length2()); // double the impulse if node or face is constrained. - // if (face_penetration > 0 || node_penetration > 0) - // I_tilde *= 2.0; + if (face_penetration > 0 || node_penetration > 0) + I_tilde *= 2.0; if (face_penetration <= 0) { for (int j = 0; j < 3; ++j) diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 15e6ec430..8d13b39bd 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -1010,6 +1010,20 @@ static inline btMatrix3x3 ImpulseMatrix(btScalar dt, return (Diagonal(1 / dt) * Add(Diagonal(ima), MassMatrix(imb, iwi, r)).inverse()); } +// +static inline btMatrix3x3 ImpulseMatrix(btScalar dt, + const btMatrix3x3& effective_mass_inv, + btScalar imb, + const btMatrix3x3& iwi, + const btVector3& r) +{ + return (Diagonal(1 / dt) * Add(effective_mass_inv, MassMatrix(imb, iwi, r)).inverse()); + // btMatrix3x3 iimb = MassMatrix(imb, iwi, r); + // if (iimb.determinant() == 0) + // return effective_mass_inv.inverse(); + // return effective_mass_inv.inverse() * Add(effective_mass_inv.inverse(), iimb.inverse()).inverse() * iimb.inverse(); +} + // static inline btMatrix3x3 ImpulseMatrix(btScalar ima, const btMatrix3x3& iia, const btVector3& ra, btScalar imb, const btMatrix3x3& iib, const btVector3& rb) @@ -1132,9 +1146,7 @@ static inline bool lineIntersectsTriangle(const btVector3& rayStart, const btVec if (dir_norm < SIMD_EPSILON) return false; dir.normalize(); - btScalar t; - bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t); if (ret) @@ -1682,7 +1694,9 @@ struct btSoftColliders const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic; const btVector3 ra = n.m_x - wtr.getOrigin(); - c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); + c.m_c0 = ImpulseMatrix(1, n.m_effectiveMass_inv, imb, iwi, ra); + c.m_c5 = n.m_effectiveMass_inv; + // c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra); c.m_c1 = ra; } else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) @@ -1710,7 +1724,7 @@ struct btSoftColliders t1.getX(), t1.getY(), t1.getZ(), t2.getX(), t2.getY(), t2.getZ()); // world frame to local frame const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - btMatrix3x3 local_impulse_matrix = (Diagonal(n.m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); + btMatrix3x3 local_impulse_matrix = (n.m_effectiveMass_inv + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse(); c.m_c0 = rot.transpose() * local_impulse_matrix * rot; c.jacobianData_normal = jacobianData_normal; c.jacobianData_t1 = jacobianData_t1; @@ -1776,6 +1790,7 @@ struct btSoftColliders c.m_c2 = ima; c.m_c3 = fc; c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR; + c.m_c5 = Diagonal(ima); if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();