diff --git a/examples/DeformableDemo/PinchFriction.cpp b/examples/DeformableDemo/PinchFriction.cpp new file mode 100644 index 000000000..06f8c2d5f --- /dev/null +++ b/examples/DeformableDemo/PinchFriction.cpp @@ -0,0 +1,382 @@ +/* + 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 "PinchFriction.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 //printf debugging + +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "../Utils/b3ResourcePath.h" + +///The PinchFriction shows the use of rolling friction. +///Spheres will come to a rest on a sloped plane using a constraint. Damping cannot achieve the same. +///Generally it is best to leave the rolling friction coefficient zero (or close to zero). + +struct TetraCube +{ +#include "../SoftDemo/cube.inl" +}; + +class PinchFriction : public CommonRigidBodyBase +{ + btAlignedObjectArray m_forces; +public: + PinchFriction(struct GUIHelperInterface* helper) + : CommonRigidBodyBase(helper) + { + } + virtual ~PinchFriction() + { + } + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 25; + float pitch = -30; + float yaw = 100; + float targetPos[3] = {0, -0, 0}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + void stepSimulation(float deltaTime) + { + //use a smaller internal timestep, there are stability issues + float internalTimeStep = 1. / 240.f; + m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); + } + + void createGrip() + { + int count = 2; + float mass = 1e6; + btCollisionShape* shape[] = { + new btBoxShape(btVector3(3, 3, 0.5)), + }; + static const int nshapes = sizeof(shape) / sizeof(shape[0]); + for (int i = 0; i < count; ++i) + { + btTransform startTransform; + startTransform.setIdentity(); + startTransform.setOrigin(btVector3(10, 0, 0)); + startTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.)); + createRigidBody(mass, startTransform, shape[i % nshapes]); + } + } + + virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const + { + return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; + } + + virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() + { + return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; + } + + virtual void renderScene() + { + CommonRigidBodyBase::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 dynamics2(btScalar time, btDeformableMultiBodyDynamicsWorld* world) +{ + btAlignedObjectArray& rbs = world->getNonStaticRigidBodies(); + if (rbs.size()<2) + return; + btRigidBody* rb0 = rbs[0]; + btScalar pressTime = 0.9; + btScalar liftTime = 2.5; + btScalar shiftTime = 3.5; + btScalar holdTime = 4.5*1000; + btScalar dropTime = 5.3*1000; + btTransform rbTransform; + rbTransform.setIdentity(); + btVector3 translation; + btVector3 velocity; + + btVector3 initialTranslationLeft = btVector3(0.5,3,4); + btVector3 initialTranslationRight = btVector3(0.5,3,-4); + btVector3 PinchFrictionVelocityLeft = btVector3(0,0,-.5); + btVector3 PinchFrictionVelocityRight = btVector3(0,0,.5); + btVector3 liftVelocity = btVector3(0,5,0); + btVector3 shiftVelocity = btVector3(0,0,0); + btVector3 holdVelocity = btVector3(0,0,0); + btVector3 openVelocityLeft = btVector3(0,0,4); + btVector3 openVelocityRight = btVector3(0,0,-4); + + if (time < pressTime) + { + velocity = PinchFrictionVelocityLeft; + translation = initialTranslationLeft + PinchFrictionVelocityLeft * time; + } + else if (time < liftTime) + { + velocity = liftVelocity; + translation = initialTranslationLeft + PinchFrictionVelocityLeft * pressTime + liftVelocity * (time - pressTime); + + } + else if (time < shiftTime) + { + velocity = shiftVelocity; + translation = initialTranslationLeft + PinchFrictionVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime); + } + else if (time < holdTime) + { + velocity = btVector3(0,0,0); + translation = initialTranslationLeft + PinchFrictionVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime); + } + else if (time < dropTime) + { + velocity = openVelocityLeft; + translation = initialTranslationLeft + PinchFrictionVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (time - holdTime); + } + else + { + velocity = holdVelocity; + translation = initialTranslationLeft + PinchFrictionVelocityLeft * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityLeft * (dropTime - holdTime); + } + rbTransform.setOrigin(translation); + rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0)); + rb0->setCenterOfMassTransform(rbTransform); + rb0->setAngularVelocity(btVector3(0,0,0)); + rb0->setLinearVelocity(velocity); + + btRigidBody* rb1 = rbs[1]; + if (time < pressTime) + { + velocity = PinchFrictionVelocityRight; + translation = initialTranslationRight + PinchFrictionVelocityRight * time; + } + else if (time < liftTime) + { + velocity = liftVelocity; + translation = initialTranslationRight + PinchFrictionVelocityRight * pressTime + liftVelocity * (time - pressTime); + + } + else if (time < shiftTime) + { + velocity = shiftVelocity; + translation = initialTranslationRight + PinchFrictionVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (time - liftTime); + } + else if (time < holdTime) + { + velocity = btVector3(0,0,0); + translation = initialTranslationRight + PinchFrictionVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (time - shiftTime); + } + else if (time < dropTime) + { + velocity = openVelocityRight; + translation = initialTranslationRight + PinchFrictionVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (time - holdTime); + } + else + { + velocity = holdVelocity; + translation = initialTranslationRight + PinchFrictionVelocityRight * pressTime + liftVelocity * (liftTime-pressTime) + shiftVelocity * (shiftTime - liftTime) + holdVelocity * (holdTime - shiftTime)+ openVelocityRight * (dropTime - holdTime); + } + rbTransform.setOrigin(translation); + rbTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0)); + rb1->setCenterOfMassTransform(rbTransform); + rb1->setAngularVelocity(btVector3(0,0,0)); + rb1->setLinearVelocity(velocity); + + rb0->setFriction(20); + rb1->setFriction(20); +} + +void PinchFriction::initPhysics() +{ + m_guiHelper->setUpAxis(1); + + m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); + + 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, -10, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; + + getDeformableDynamicsWorld()->setSolverCallback(dynamics2); + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + //create a ground + { + btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.))); + + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, -25, 0)); + groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0)); + //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here: + btScalar mass(0.); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0, 0, 0); + if (isDynamic) + groundShape->calculateLocalInertia(mass, localInertia); + + //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); + btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); + btRigidBody* body = new btRigidBody(rbInfo); + body->setFriction(0.5); + + //add the ground to the dynamics world + m_dynamicsWorld->addRigidBody(body); + } + + // create a soft block + { + btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(), + TetraCube::getElements(), + 0, + TetraCube::getNodes(), + false, true, true); + + psb->scale(btVector3(1.5, 1.5, 1.5)); + psb->translate(btVector3(0, 4, 1.6)); + psb->getCollisionShape()->setMargin(0.15); + 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; + btSoftBodyHelpers::generateBoundaryFaces(psb); + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; + getDeformableDynamicsWorld()->addSoftBody(psb); + + btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); + getDeformableDynamicsWorld()->addForce(psb, gravity_force); + m_forces.push_back(gravity_force); + + btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(20,40); + getDeformableDynamicsWorld()->addForce(psb, neohookean); + m_forces.push_back(neohookean); + } + + // create a second soft block + { + btSoftBody* psb2 = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(), + TetraCube::getElements(), + 0, + TetraCube::getNodes(), + false, true, true); + + psb2->scale(btVector3(1.5, 1.5, 1.5)); + psb2->translate(btVector3(0, 4, -1.6)); + psb2->getCollisionShape()->setMargin(0.15); + psb2->setTotalMass(1); + psb2->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb2->m_cfg.kCHR = 1; // collision hardness with rigid body + psb2->m_cfg.kDF = 2; + psb2->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb2->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; + btSoftBodyHelpers::generateBoundaryFaces(psb2); + getDeformableDynamicsWorld()->addSoftBody(psb2); + + btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); + getDeformableDynamicsWorld()->addForce(psb2, gravity_force); + m_forces.push_back(gravity_force); + + btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(20,40); + getDeformableDynamicsWorld()->addForce(psb2, neohookean); + m_forces.push_back(neohookean); + } + + // add a pair of grippers + createGrip(); + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void PinchFriction::exitPhysics() +{ + //cleanup in the reverse order of creation/initialization + + //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* PinchFrictionCreateFunc(struct CommonExampleOptions& options) +{ + return new PinchFriction(options.m_guiHelper); +} + + diff --git a/examples/DeformableDemo/PinchFriction.h b/examples/DeformableDemo/PinchFriction.h new file mode 100644 index 000000000..b397ba342 --- /dev/null +++ b/examples/DeformableDemo/PinchFriction.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 _PINCH_FRICTION_H +#define _PINCH_FRICTION_H + +class CommonExampleInterface* PinchFrictionCreateFunc(struct CommonExampleOptions& options); + +#endif //_PINCH_FRICTION_H diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index d3e4b3f8c..0456f4f26 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -359,6 +359,8 @@ SET(BulletExampleBrowser_SRCS ../DeformableDemo/GraspDeformable.h ../DeformableDemo/Pinch.cpp ../DeformableDemo/Pinch.h + ../DeformableDemo/PinchFriction.cpp + ../DeformableDemo/PinchFriction.h ../DeformableDemo/DeformableMultibody.cpp ../DeformableDemo/DeformableMultibody.h ../DeformableDemo/DeformableRigid.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index ee04ece90..c72931891 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -46,6 +46,7 @@ #include "../RollingFrictionDemo/RollingFrictionDemo.h" #include "../DeformableDemo/DeformableRigid.h" #include "../DeformableDemo/Pinch.h" +#include "../DeformableDemo/PinchFriction.h" #include "../DeformableDemo/DeformableMultibody.h" #include "../DeformableDemo/VolumetricDeformable.h" #include "../DeformableDemo/GraspDeformable.h" @@ -188,6 +189,7 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(0, "Deformabe Body"), ExampleEntry(1, "Deformable-Deformable Contact", "Deformable contact", DeformableContactCreateFunc), + ExampleEntry(1, "Deformable-Deformable Friction Contact", "Deformable friction contact", PinchFrictionCreateFunc), ExampleEntry(1, "Deformable-RigidBody Contact", "Deformable test", DeformableRigidCreateFunc), ExampleEntry(1, "Grasp Deformable Cube", "Grasping test", PinchCreateFunc), ExampleEntry(1, "Grasp Deformable with Motor", "Grasping test", GraspDeformableCreateFunc), diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index b9059f0c6..36f0b1218 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -13,8 +13,8 @@ 3. This notice may not be removed or altered from any source distribution. */ -#ifndef BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H -#define BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H +#ifndef BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H +#define BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H #include "btSoftMultiBodyDynamicsWorld.h" #include "btDeformableLagrangianForce.h" @@ -36,7 +36,6 @@ typedef btAlignedObjectArray btSoftBodyArray; class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld { typedef btAlignedObjectArray TVStack; -// using TVStack = btAlignedObjectArray; ///Solver classes that encapsulate multiple deformable bodies for solving btDeformableBodySolver* m_deformableBodySolver; btSoftBodyArray m_softBodies; @@ -162,4 +161,4 @@ public: } }; -#endif //BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H +#endif //BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index 0c0bc4f64..c28b28848 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -1387,6 +1387,76 @@ btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, return psb; } +void btSoftBodyHelpers::generateBoundaryFaces(btSoftBody* psb) +{ + int counter = 0; + for (int i = 0; i < psb->m_nodes.size(); ++i) + { + psb->m_nodes[i].index = counter++; + } + typedef btAlignedObjectArray Index; + btAlignedObjectArray indices; + indices.resize(psb->m_tetras.size()); + for (int i = 0; i < indices.size(); ++i) + { + Index index; + index.push_back(psb->m_tetras[i].m_n[0]->index); + index.push_back(psb->m_tetras[i].m_n[1]->index); + index.push_back(psb->m_tetras[i].m_n[2]->index); + index.push_back(psb->m_tetras[i].m_n[3]->index); + indices[i] = index; + } + + std::map, std::vector > dict; + for (int i = 0; i < indices.size(); ++i) + { + for (int j = 0; j < 4; ++j) + { + std::vector f; + if (j == 0) + { + f.push_back(indices[i][1]); + f.push_back(indices[i][0]); + f.push_back(indices[i][2]); + } + if (j == 1) + { + f.push_back(indices[i][3]); + f.push_back(indices[i][0]); + f.push_back(indices[i][1]); + } + if (j == 2) + { + f.push_back(indices[i][3]); + f.push_back(indices[i][1]); + f.push_back(indices[i][2]); + } + if (j == 3) + { + f.push_back(indices[i][2]); + f.push_back(indices[i][0]); + f.push_back(indices[i][3]); + } + std::vector f_sorted = f; + std::sort(f_sorted.begin(), f_sorted.end()); + if (dict.find(f_sorted) != dict.end()) + { + dict.erase(f_sorted); + } + else + { + dict.insert(std::make_pair(f_sorted, f)); + } + } + } + + for (auto it = dict.begin(); it != dict.end(); ++it) + { + std::vector f = it->second; + psb->appendFace(f[0], f[1], f[2]); + } +} + void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb) { std::ofstream fs; diff --git a/src/BulletSoftBody/btSoftBodyHelpers.h b/src/BulletSoftBody/btSoftBodyHelpers.h index f189e7353..d5d7c3648 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.h +++ b/src/BulletSoftBody/btSoftBodyHelpers.h @@ -151,6 +151,9 @@ struct btSoftBodyHelpers static void readRenderMeshFromObj(const char* file, btSoftBody* psb); static void interpolateBarycentricWeights(btSoftBody* psb); + + static void generateBoundaryFaces(btSoftBody* psb); + static void duplicateFaces(const char* filename, const btSoftBody* psb); /// Sort the list of links to move link calculations that are dependent upon earlier /// ones as far as possible away from the calculation of those values