diff --git a/examples/DeformableDemo/ClothFriction.cpp b/examples/DeformableDemo/ClothFriction.cpp new file mode 100644 index 000000000..6626560c6 --- /dev/null +++ b/examples/DeformableDemo/ClothFriction.cpp @@ -0,0 +1,251 @@ +/* + 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 "ClothFriction.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 ClothFriction 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). +class ClothFriction : public CommonRigidBodyBase +{ + btAlignedObjectArray m_forces; +public: + ClothFriction(struct GUIHelperInterface* helper) + : CommonRigidBodyBase(helper) + { + } + virtual ~ClothFriction() + { + } + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 12; + float pitch = -50; + float yaw = 120; + float targetPos[3] = {0, -3, 0}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + } + + void stepSimulation(float deltaTime) + { + float internalTimeStep = 1. / 240.f; + m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep); + } + + 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 ClothFriction::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(); + + ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) + 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; + + 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, -32, 0)); + groundTransform.setRotation(btQuaternion(btVector3(1, 0, 0), SIMD_PI * 0.1)); + //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(3); + + //add the ground to the dynamics world + m_dynamicsWorld->addRigidBody(body); + } + + // create a piece of cloth + { + btScalar s = 4; + btScalar h = 0; + + btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s), + btVector3(+s, h, -s), + btVector3(-s, h, +s), + btVector3(+s, h, +s), + 20,20, + 0, true); + + psb->getCollisionShape()->setMargin(0.05); + psb->generateBendingConstraints(2); + 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 = 3; + psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; + getDeformableDynamicsWorld()->addSoftBody(psb); + + btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(10,1, true); + getDeformableDynamicsWorld()->addForce(psb, mass_spring); + m_forces.push_back(mass_spring); + + btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); + getDeformableDynamicsWorld()->addForce(psb, gravity_force); + m_forces.push_back(gravity_force); + + + h = 2; + s = 2; + btSoftBody* psb2 = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s), + btVector3(+s, h, -s), + btVector3(-s, h, +s), + btVector3(+s, h, +s), + 10,10, + 0, true); + psb2->getCollisionShape()->setMargin(0.05); + psb2->generateBendingConstraints(2); + 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 = 20; + psb2->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb2->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; + psb->translate(btVector3(0,0,0)); + getDeformableDynamicsWorld()->addSoftBody(psb2); + + btDeformableMassSpringForce* mass_spring2 = new btDeformableMassSpringForce(10,1, true); + getDeformableDynamicsWorld()->addForce(psb2, mass_spring2); + m_forces.push_back(mass_spring2); + + btDeformableGravityForce* gravity_force2 = new btDeformableGravityForce(gravity); + getDeformableDynamicsWorld()->addForce(psb2, gravity_force2); + m_forces.push_back(gravity_force2); + } + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void ClothFriction::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* ClothFrictionCreateFunc(struct CommonExampleOptions& options) +{ + return new ClothFriction(options.m_guiHelper); +} + + diff --git a/examples/DeformableDemo/ClothFriction.h b/examples/DeformableDemo/ClothFriction.h new file mode 100644 index 000000000..228ec659c --- /dev/null +++ b/examples/DeformableDemo/ClothFriction.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 CLOTH_FRICTION_H +#define CLOTH_FRICTION_H + +class CommonExampleInterface* ClothFrictionCreateFunc(struct CommonExampleOptions& options); + +#endif //CLOTH_FRICTION_H diff --git a/examples/DeformableDemo/PinchFriction.cpp b/examples/DeformableDemo/PinchFriction.cpp index 06f8c2d5f..4aa2ba0d1 100644 --- a/examples/DeformableDemo/PinchFriction.cpp +++ b/examples/DeformableDemo/PinchFriction.cpp @@ -95,8 +95,8 @@ public: virtual void renderScene() { CommonRigidBodyBase::renderScene(); - btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); - +// btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld(); +// // for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++) // { // btSoftBody* psb = (btSoftBody*)deformableWorld->getSoftBodyArray()[i]; @@ -115,7 +115,7 @@ void dynamics2(btScalar time, btDeformableMultiBodyDynamicsWorld* world) return; btRigidBody* rb0 = rbs[0]; btScalar pressTime = 0.9; - btScalar liftTime = 2.5; + btScalar liftTime = 10; btScalar shiftTime = 3.5; btScalar holdTime = 4.5*1000; btScalar dropTime = 5.3*1000; @@ -126,9 +126,9 @@ void dynamics2(btScalar time, btDeformableMultiBodyDynamicsWorld* world) 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 PinchFrictionVelocityLeft = btVector3(0,0,-1); + btVector3 PinchFrictionVelocityRight = btVector3(0,0,1); + btVector3 liftVelocity = btVector3(0,1,0); btVector3 shiftVelocity = btVector3(0,0,0); btVector3 holdVelocity = btVector3(0,0,0); btVector3 openVelocityLeft = btVector3(0,0,4); @@ -209,8 +209,8 @@ void dynamics2(btScalar time, btDeformableMultiBodyDynamicsWorld* world) rb1->setAngularVelocity(btVector3(0,0,0)); rb1->setLinearVelocity(velocity); - rb0->setFriction(20); - rb1->setFriction(20); + rb0->setFriction(200); + rb1->setFriction(200); } void PinchFriction::initPhysics() @@ -260,7 +260,7 @@ void PinchFriction::initPhysics() btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform); btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia); btRigidBody* body = new btRigidBody(rbInfo); - body->setFriction(0.5); + body->setFriction(0); //add the ground to the dynamics world m_dynamicsWorld->addRigidBody(body); @@ -274,13 +274,13 @@ void PinchFriction::initPhysics() 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->scale(btVector3(2, 2, 1)); + psb->translate(btVector3(0, 2.1, 2.2)); + psb->getCollisionShape()->setMargin(0.03); psb->setTotalMass(1); psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects psb->m_cfg.kCHR = 1; // collision hardness with rigid body - psb->m_cfg.kDF = 2; + psb->m_cfg.kDF = 20; btSoftBodyHelpers::generateBoundaryFaces(psb); psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; @@ -290,7 +290,7 @@ void PinchFriction::initPhysics() getDeformableDynamicsWorld()->addForce(psb, gravity_force); m_forces.push_back(gravity_force); - btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(20,40); + btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(4,8,.1); getDeformableDynamicsWorld()->addForce(psb, neohookean); m_forces.push_back(neohookean); } @@ -303,13 +303,13 @@ void PinchFriction::initPhysics() 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->scale(btVector3(2, 2, 1)); + psb2->translate(btVector3(0, 2.1, -2.2)); + psb2->getCollisionShape()->setMargin(0.03); 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.kDF = 20; psb2->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; psb2->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; btSoftBodyHelpers::generateBoundaryFaces(psb2); @@ -319,11 +319,40 @@ void PinchFriction::initPhysics() getDeformableDynamicsWorld()->addForce(psb2, gravity_force); m_forces.push_back(gravity_force); - btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(20,40); + btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(4,8,.1); getDeformableDynamicsWorld()->addForce(psb2, neohookean); m_forces.push_back(neohookean); } + // create a third soft block + { + btSoftBody* psb3 = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(), + TetraCube::getElements(), + 0, + TetraCube::getNodes(), + false, true, true); + + psb3->scale(btVector3(2, 2, 1)); + psb3->translate(btVector3(0, 2.1, 0)); + psb3->getCollisionShape()->setMargin(0.03); + psb3->setTotalMass(1); + psb3->m_cfg.kKHR = 1; // collision hardness with kinematic objects + psb3->m_cfg.kCHR = 1; // collision hardness with rigid body + psb3->m_cfg.kDF = 20; + psb3->m_cfg.collisions = btSoftBody::fCollision::SDF_RD; + psb3->m_cfg.collisions |= btSoftBody::fCollision::VF_DD; + btSoftBodyHelpers::generateBoundaryFaces(psb3); + getDeformableDynamicsWorld()->addSoftBody(psb3); + + btDeformableGravityForce* gravity_force = new btDeformableGravityForce(gravity); + getDeformableDynamicsWorld()->addForce(psb3, gravity_force); + m_forces.push_back(gravity_force); + + btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(4,8,.1); + getDeformableDynamicsWorld()->addForce(psb3, neohookean); + m_forces.push_back(neohookean); + } + // add a pair of grippers createGrip(); m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 0456f4f26..b5314908e 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -361,6 +361,8 @@ SET(BulletExampleBrowser_SRCS ../DeformableDemo/Pinch.h ../DeformableDemo/PinchFriction.cpp ../DeformableDemo/PinchFriction.h + ../DeformableDemo/ClothFriction.cpp + ../DeformableDemo/ClothFriction.h ../DeformableDemo/DeformableMultibody.cpp ../DeformableDemo/DeformableMultibody.h ../DeformableDemo/DeformableRigid.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index c72931891..dc2fde954 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -45,6 +45,7 @@ #include "../DynamicControlDemo/MotorDemo.h" #include "../RollingFrictionDemo/RollingFrictionDemo.h" #include "../DeformableDemo/DeformableRigid.h" +#include "../DeformableDemo/ClothFriction.h" #include "../DeformableDemo/Pinch.h" #include "../DeformableDemo/PinchFriction.h" #include "../DeformableDemo/DeformableMultibody.h" @@ -189,6 +190,7 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(0, "Deformabe Body"), ExampleEntry(1, "Deformable-Deformable Contact", "Deformable contact", DeformableContactCreateFunc), + ExampleEntry(1, "Cloth Friction", "Cloth friction contact", ClothFrictionCreateFunc), 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), diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index fac85fbc1..7ee4d4bea 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -24,7 +24,7 @@ btDeformableBodySolver::btDeformableBodySolver() , m_cg(20) , m_maxNewtonIterations(3) , m_newtonTolerance(1e-4) -, m_lineSearch(true) +, m_lineSearch(false) { m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity); } diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index f201a4d24..6ae6cb3df 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp @@ -23,7 +23,7 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration ///this is a special step to resolve penetrations (just for contacts) solveGroupCacheFriendlySplitImpulseIterations(bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer); -// m_maxOverrideNumSolverIterations = 150; + m_maxOverrideNumSolverIterations = 150; int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; for (int iteration = 0; iteration < maxIterations; iteration++) { diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index 1db6ab8ff..c9279b278 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -1314,7 +1314,15 @@ struct btSoftColliders return; btVector3 rayEnd = dir.normalized() * (l + 2*mrg); // register an intersection if the line segment formed by the trajectory of the node in the timestep intersects the face - bool intersect = lineIntersectsTriangle(btVector3(0,0,0), rayEnd, face->m_n[0]->m_x-o, face->m_n[1]->m_x-o, face->m_n[2]->m_x-o, p, normal); + btVector3 v0 = face->m_n[0]->m_x; + btVector3 v1 = face->m_n[1]->m_x; + btVector3 v2 = face->m_n[2]->m_x; + btVector3 vc = (v0+v1+v2)/3.; + btScalar scale = 2; + btVector3 u0 = vc + (v0-vc)*scale; + btVector3 u1 = vc + (v1-vc)*scale; + btVector3 u2 = vc + (v2-vc)*scale; + bool intersect = lineIntersectsTriangle(btVector3(0,0,0), rayEnd, u0-o, u1-o, u2-o, p, normal); if (intersect) {