diff --git a/examples/DeformableDemo/DeformableSelfCollision.cpp b/examples/DeformableDemo/DeformableSelfCollision.cpp new file mode 100644 index 000000000..eb963ae05 --- /dev/null +++ b/examples/DeformableDemo/DeformableSelfCollision.cpp @@ -0,0 +1,233 @@ +/* + 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 "DeformableSelfCollision.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 DeformableSelfCollision shows deformable self collisions +class DeformableSelfCollision : public CommonRigidBodyBase +{ + btAlignedObjectArray m_forces; +public: + DeformableSelfCollision(struct GUIHelperInterface* helper) + : CommonRigidBodyBase(helper) + { + } + virtual ~DeformableSelfCollision() + { + } + void initPhysics(); + + void exitPhysics(); + + void resetCamera() + { + float dist = 10; + float pitch = -8; + float yaw = 100; + float targetPos[3] = {0, -10, 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 + { + ///just make it a btSoftRigidDynamicsWorld please + ///or we will add type checking + return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld; + } + + virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() + { + ///just make it a btSoftRigidDynamicsWorld please + ///or we will add type checking + 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 DeformableSelfCollision::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); + // deformableBodySolver->setWorld(getDeformableDynamicsWorld()); + // m_dynamicsWorld->getSolverInfo().m_singleAxisDeformableThreshold = 0.f;//faster but lower quality + btVector3 gravity = btVector3(0, -100, 0); + m_dynamicsWorld->setGravity(gravity); + getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity; + + // getDeformableDynamicsWorld()->before_solver_callbacks.push_back(dynamics); + 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, -35, 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(40); + + //add the ground to the dynamics world + m_dynamicsWorld->addRigidBody(body); + } + + // create a piece of cloth + { + const btScalar s = 2; + const btScalar h = 0; + + btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -4*s), + btVector3(+s, h, -4*s), + btVector3(-s, h, +4*s), + btVector3(+s, h, +4*s), + 10,40, + 0, true, 0.1); + + + psb->getCollisionShape()->setMargin(0.2); + 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 = 0.02; + psb->rotate(btQuaternion(0,SIMD_PI / 2, 0)); + 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,0.2, 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); + } + getDeformableDynamicsWorld()->setImplicit(true); + getDeformableDynamicsWorld()->setLineSearch(false); + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void DeformableSelfCollision::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* DeformableSelfCollisionCreateFunc(struct CommonExampleOptions& options) +{ + return new DeformableSelfCollision(options.m_guiHelper); +} + + diff --git a/examples/DeformableDemo/DeformableSelfCollision.h b/examples/DeformableDemo/DeformableSelfCollision.h new file mode 100644 index 000000000..36359c0d9 --- /dev/null +++ b/examples/DeformableDemo/DeformableSelfCollision.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 DEFORMABLE_SELF_COLLISION_H +#define DEFORMABLE_SELF_COLLISION_H + +class CommonExampleInterface* DeformableSelfCollisionCreateFunc(struct CommonExampleOptions& options); + +#endif //_DEFORMABLE_SELF_COLLISION_H diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index b5314908e..7c31c9d84 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/DeformableSelfCollision.cpp + ../DeformableDemo/DeformableSelfCollision.h ../DeformableDemo/PinchFriction.cpp ../DeformableDemo/PinchFriction.h ../DeformableDemo/ClothFriction.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index dc2fde954..e705fd075 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -47,6 +47,7 @@ #include "../DeformableDemo/DeformableRigid.h" #include "../DeformableDemo/ClothFriction.h" #include "../DeformableDemo/Pinch.h" +#include "../DeformableDemo/DeformableSelfCollision.h" #include "../DeformableDemo/PinchFriction.h" #include "../DeformableDemo/DeformableMultibody.h" #include "../DeformableDemo/VolumetricDeformable.h" @@ -189,6 +190,7 @@ static ExampleEntry gDefaultExamples[] = //ExampleEntry(1, "Spheres & Plane C-API (Bullet3)", "Collision C-API using Bullet 3.x backend", CollisionTutorialBullet2CreateFunc,TUT_SPHERE_PLANE_RTB3), ExampleEntry(0, "Deformabe Body"), + ExampleEntry(1, "Deformable Self Collision", "Deformable Self Collision", DeformableSelfCollisionCreateFunc), 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), diff --git a/src/BulletSoftBody/btDeformableBodySolver.cpp b/src/BulletSoftBody/btDeformableBodySolver.cpp index 0bec738dd..88f885059 100644 --- a/src/BulletSoftBody/btDeformableBodySolver.cpp +++ b/src/BulletSoftBody/btDeformableBodySolver.cpp @@ -22,7 +22,7 @@ btDeformableBodySolver::btDeformableBodySolver() : m_numNodes(0) , m_cg(20) -, m_maxNewtonIterations(3) +, m_maxNewtonIterations(10) , m_newtonTolerance(1e-4) , m_lineSearch(true) { diff --git a/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp b/src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp index 6ae6cb3df..258d08b30 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 = 50; int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; for (int iteration = 0; iteration < maxIterations; iteration++) { diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index 0d5067339..d761d42e9 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -51,6 +51,11 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t ///perform collision detection btMultiBodyDynamicsWorld::performDiscreteCollisionDetection(); + if (m_selfCollision) + { + softBodySelfCollision(); + } + btMultiBodyDynamicsWorld::calculateSimulationIslands(); beforeSolverCallbacks(timeStep); @@ -70,6 +75,15 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t // /////////////////////////////// } +void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision() +{ + for (int i = 0; i < m_softBodies.size(); i++) + { + btSoftBody* psb = (btSoftBody*)m_softBodies[i]; + psb->defaultCollisionHandler(psb); + } +} + void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) { BT_PROFILE("integrateTransforms"); diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h index dc060a8e8..e66e060f2 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h @@ -48,6 +48,7 @@ class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld int m_contact_iterations; bool m_implicit; bool m_lineSearch; + bool m_selfCollision; typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world); btSolverCallback m_solverCallback; @@ -83,6 +84,7 @@ public: m_sbi.m_gravity.setValue(0, -10, 0); m_internalTime = 0.0; m_implicit = true; + m_selfCollision = true; } void setSolverCallback(btSolverCallback cb) @@ -156,6 +158,8 @@ public: void sortConstraints(); + void softBodySelfCollision(); + void setImplicit(bool implicit) { m_implicit = implicit; diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 95f2c4860..eadc56d7c 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -3515,6 +3515,18 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb) docollide.psb[1]->m_fdbvt.m_root, docollide); } + else + { + btSoftColliders::CollideVF_DD docollide; + docollide.mrg = getCollisionShape()->getMargin() + + psb->getCollisionShape()->getMargin(); + /* psb0 nodes vs psb0 faces */ + docollide.psb[0] = this; + docollide.psb[1] = psb; + docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + } } break; default: diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index c9279b278..9f4d2a197 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -1255,6 +1255,12 @@ struct btSoftColliders { btSoftBody::Node* node = (btSoftBody::Node*)lnode->data; btSoftBody::Face* face = (btSoftBody::Face*)lface->data; + for (int i = 0; i < 3; ++i) + { + if (face->m_n[i] == node) + continue; + } + btVector3 o = node->m_x; btVector3 p; btScalar d = SIMD_INFINITY; @@ -1318,7 +1324,8 @@ struct btSoftColliders 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; + btScalar scale = 1; + // enlarge the triangle to catch collision on the edge btVector3 u0 = vc + (v0-vc)*scale; btVector3 u1 = vc + (v1-vc)*scale; btVector3 u2 = vc + (v2-vc)*scale;