add naive implementation of self collision

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 <stdio.h>  //printf debugging
+
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+
+///The DeformableSelfCollision shows deformable self collisions
+class DeformableSelfCollision : public CommonRigidBodyBase
+{
+    btAlignedObjectArray<btDeformableLagrangianForce*> 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);
+}
+
+

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

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

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),

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)
 {

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++)
         {

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");

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;

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:

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;