Merge pull request #2426 from xhan0619/faceContact

Face contact

examples/DeformableDemo/ClothFriction.cpp

@@ -0,0 +1,250 @@
+/*
+ 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 <stdio.h>  //printf debugging
+
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+
+///The ClothFriction shows the use of deformable friction.
+class ClothFriction : public CommonRigidBodyBase
+{
+    btAlignedObjectArray<btDeformableLagrangianForce*> 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),
+                                                         10,10,
+                                                         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),
+                                                          5,5,
+                                                          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);
+    }
+    getDeformableDynamicsWorld()->setImplicit(false);
+    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);
+}
+
+

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

examples/DeformableDemo/DeformableContact.cpp

@@ -0,0 +1,252 @@
+/*
+ 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 "DeformableContact.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 DeformableContact shows the contact between deformable objects
+
+class DeformableContact : public CommonRigidBodyBase
+{
+    btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
+public:
+    DeformableContact(struct GUIHelperInterface* helper)
+    : CommonRigidBodyBase(helper)
+    {
+    }
+    virtual ~DeformableContact()
+    {
+    }
+    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 DeformableContact::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.));
+        //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(2);
+        
+        //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,
+                                                         1 + 2 + 4 + 8, true);
+        
+        psb->getCollisionShape()->setMargin(0.1);
+        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;
+        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.1);
+        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 = 0.1;
+        psb2->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+        psb2->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
+        psb->translate(btVector3(3.5,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);
+    }
+    getDeformableDynamicsWorld()->setImplicit(true);
+    getDeformableDynamicsWorld()->setLineSearch(false);
+    m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void DeformableContact::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* DeformableContactCreateFunc(struct CommonExampleOptions& options)
+{
+    return new DeformableContact(options.m_guiHelper);
+}
+
+

examples/DeformableDemo/DeformableContact.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_CONTACT_H
+#define DEFORMABLE_CONTACT_H
+
+class CommonExampleInterface* DeformableContactCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //_DEFORMABLE_CONTACT_H

examples/DeformableDemo/DeformableMultibody.cpp

@@ -11,20 +11,6 @@
  3. This notice may not be removed or altered from any source distribution.
  */
 
-///create 125 (5x5x5) dynamic object
-#define ARRAY_SIZE_X 5
-#define ARRAY_SIZE_Y 5
-#define ARRAY_SIZE_Z 5
-
-//maximum number of objects (and allow user to shoot additional boxes)
-#define MAX_PROXIES (ARRAY_SIZE_X * ARRAY_SIZE_Y * ARRAY_SIZE_Z + 1024)
-
-///scaling of the objects (0.1 = 20 centimeter boxes )
-#define SCALING 1.
-#define START_POS_X -5
-#define START_POS_Y -5
-#define START_POS_Z -3
-
 #include "DeformableMultibody.h"
 ///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
 #include "btBulletDynamicsCommon.h"
@@ -235,7 +221,7 @@ void DeformableMultibody::initPhysics()
         getDeformableDynamicsWorld()->addForce(psb, gravity_force);
         m_forces.push_back(gravity_force);
     }
-
+    getDeformableDynamicsWorld()->setImplicit(false);
 	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
 }
 
@@ -360,26 +346,23 @@ void DeformableMultibody::addColliders_testMultiDof(btMultiBody* pMultiBody, btM
     local_origin[0] = pMultiBody->getBasePos();
     
     {
-        //    float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
+
         btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
         
-        if (1)
+        btCollisionShape* box = new btBoxShape(baseHalfExtents);
-        {
+        btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
-            btCollisionShape* box = new btBoxShape(baseHalfExtents);
+        col->setCollisionShape(box);
-            btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
+        
-            col->setCollisionShape(box);
+        btTransform tr;
-            
+        tr.setIdentity();
-            btTransform tr;
+        tr.setOrigin(local_origin[0]);
-            tr.setIdentity();
+        tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
-            tr.setOrigin(local_origin[0]);
+        col->setWorldTransform(tr);
-            tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+        
-            col->setWorldTransform(tr);
+        pWorld->addCollisionObject(col, 2, 1 + 2);
-            
+        
-            pWorld->addCollisionObject(col, 2, 1 + 2);
+        col->setFriction(friction);
-            
+        pMultiBody->setBaseCollider(col);
-            col->setFriction(friction);
-            pMultiBody->setBaseCollider(col);
-        }
     }
     
     for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
@@ -392,7 +375,6 @@ void DeformableMultibody::addColliders_testMultiDof(btMultiBody* pMultiBody, btM
     for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
     {
         btVector3 posr = local_origin[i + 1];
-        //    float pos[4]={posr.x(),posr.y(),posr.z(),1};
         
         btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()};
         

examples/DeformableDemo/DeformableRigid.cpp

@@ -10,21 +10,6 @@
  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.
  */
-
-///create 125 (5x5x5) dynamic object
-#define ARRAY_SIZE_X 5
-#define ARRAY_SIZE_Y 5
-#define ARRAY_SIZE_Z 5
-
-//maximum number of objects (and allow user to shoot additional boxes)
-#define MAX_PROXIES (ARRAY_SIZE_X * ARRAY_SIZE_Y * ARRAY_SIZE_Z + 1024)
-
-///scaling of the objects (0.1 = 20 centimeter boxes )
-#define SCALING 1.
-#define START_POS_X -5
-#define START_POS_Y -5
-#define START_POS_Z -3
-
 #include "DeformableRigid.h"
 ///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
 #include "btBulletDynamicsCommon.h"
@@ -39,9 +24,7 @@
 #include "../CommonInterfaces/CommonRigidBodyBase.h"
 #include "../Utils/b3ResourcePath.h"
 
-///The DeformableRigid shows the use of rolling friction.
+///The DeformableRigid shows contact between deformable objects and rigid objects.
-///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 DeformableRigid : public CommonRigidBodyBase
 {
     btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
@@ -238,7 +221,7 @@ void DeformableRigid::initPhysics()
         psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
         getDeformableDynamicsWorld()->addSoftBody(psb);
         
-        btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(2,0.1, true);
+        btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(30,1, true);
         getDeformableDynamicsWorld()->addForce(psb, mass_spring);
         m_forces.push_back(mass_spring);
         
@@ -248,6 +231,8 @@ void DeformableRigid::initPhysics()
         // add a few rigid bodies
         Ctor_RbUpStack(1);
     }
+    getDeformableDynamicsWorld()->setImplicit(true);
+    getDeformableDynamicsWorld()->setLineSearch(false);
 	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
 }
 

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.01);
+
+        
+        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.2;
+        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/DeformableDemo/GraspDeformable.cpp

@@ -34,9 +34,7 @@
 #include "../CommonInterfaces/CommonFileIOInterface.h"
 #include "Bullet3Common/b3FileUtils.h"
 
-///The GraspDeformable shows the use of rolling friction.
+///The GraspDeformable shows grasping a volumetric deformable objects with multibody gripper with moter constraints.
-///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).
 static btScalar sGripperVerticalVelocity = 0.f;
 static btScalar sGripperClosingTargetVelocity = 0.f;
 static float friction = 1.;
@@ -103,11 +101,6 @@ public:
                     btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr;
                     if (motor)
                     {
-//                        if (dofIndex == 10 || dofIndex == 11)
-//                        {
-//                            motor->setVelocityTarget(fingerTargetVelocities[1], 1);
-//                            motor->setMaxAppliedImpulse(1);
-//                        }
                         if (dofIndex == 6)
                         {
                             motor->setVelocityTarget(-fingerTargetVelocities[1], 1);
@@ -118,7 +111,6 @@ public:
                             motor->setVelocityTarget(fingerTargetVelocities[1], 1);
                             motor->setMaxAppliedImpulse(2);
                         }
-//                        motor->setRhsClamp(SIMD_INFINITY);
                         motor->setMaxAppliedImpulse(1);
                     }
                 }
@@ -127,7 +119,7 @@ public:
         }
         
         //use a smaller internal timestep, there are stability issues
-        float internalTimeStep = 1. / 250.f;
+        float internalTimeStep = 1. / 240.f;
         m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
     }
     
@@ -192,54 +184,11 @@ void GraspDeformable::initPhysics()
 	m_solver = sol;
 
 	m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
-//    deformableBodySolver->setWorld(getDeformableDynamicsWorld());
     btVector3 gravity = btVector3(0, -9.81, 0);
 	m_dynamicsWorld->setGravity(gravity);
     getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
 	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
 
-//    // load a gripper
-//    {
-//        btTransform rootTrans;
-//        rootTrans.setIdentity();
-//        BulletURDFImporter u2b(m_guiHelper,0,0,50,0);
-//        bool forceFixedBase = false;
-//        bool loadOk = u2b.loadSDF("gripper/wsg50_one_motor_gripper_new.sdf", forceFixedBase);
-//        if (loadOk)
-//        {
-//            for (int m = 0; m < u2b.getNumModels(); m++)
-//            {
-//                u2b.activateModel(m);
-//
-//                btMultiBody* mb = 0;
-//
-//                MyMultiBodyCreator creation(m_guiHelper);
-//
-//                u2b.getRootTransformInWorld(rootTrans);
-//                ConvertURDF2Bullet(u2b, creation, rootTrans, getDeformableDynamicsWorld(), true, u2b.getPathPrefix(), CUF_USE_SDF+CUF_RESERVED);
-//                mb = creation.getBulletMultiBody();
-//
-//                int numLinks = mb->getNumLinks();
-//                for (int i = 0; i < numLinks; i++)
-//                {
-//                    int mbLinkIndex = i;
-//                    float maxMotorImpulse = 1.f;
-//
-//                    if (supportsJointMotor(mb, mbLinkIndex))
-//                    {
-//                        int dof = 0;
-//                        btScalar desiredVelocity = 0.f;
-//                        btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb, mbLinkIndex, dof, desiredVelocity, maxMotorImpulse);
-//                        motor->setPositionTarget(0, 0);
-//                        motor->setVelocityTarget(0, 1);
-//                        mb->getLink(mbLinkIndex).m_userPtr = motor;
-//                        getDeformableDynamicsWorld()->addMultiBodyConstraint(motor);
-//                        motor->finalizeMultiDof();
-//                    }
-//                }
-//            }
-//        }
-//    }
     // build a gripper
     {
         bool damping = true;
@@ -273,7 +222,6 @@ void GraspDeformable::initPhysics()
             }
         }
         
-        
         if (!damping)
         {
             mbC->setLinearDamping(0.0f);
@@ -287,10 +235,9 @@ void GraspDeformable::initPhysics()
         btScalar q0 = 0.f * SIMD_PI / 180.f;
         if (numLinks > 0)
             mbC->setJointPosMultiDof(0, &q0);
-        ///
         addColliders(mbC, getDeformableDynamicsWorld(), baseHalfExtents, linkHalfExtents);
-
     }
+    
     //create a ground
     {
         btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(150.), btScalar(25.), btScalar(150.)));
@@ -325,12 +272,13 @@ void GraspDeformable::initPhysics()
     {
         char relative_path[1024];
 //        b3FileUtils::findFile("banana.vtk", relative_path, 1024);
-        b3FileUtils::findFile("ball.vtk", relative_path, 1024);
+//        b3FileUtils::findFile("ball.vtk", relative_path, 1024);
+//        b3FileUtils::findFile("paper_collision.vtk", relative_path, 1024);
 //        b3FileUtils::findFile("single_tet.vtk", relative_path, 1024);
-//         b3FileUtils::findFile("tube.vtk", relative_path, 1024);
+        b3FileUtils::findFile("tube.vtk", relative_path, 1024);
-//         b3FileUtils::findFile("torus.vtk", relative_path, 1024);
+//        b3FileUtils::findFile("torus.vtk", relative_path, 1024);
-//         b3FileUtils::findFile("paper_roll.vtk", relative_path, 1024);
+//        b3FileUtils::findFile("paper_roll.vtk", relative_path, 1024);
-//         b3FileUtils::findFile("bread.vtk", relative_path, 1024);
+//        b3FileUtils::findFile("bread.vtk", relative_path, 1024);
 //        b3FileUtils::findFile("ditto.vtk", relative_path, 1024);
 //        b3FileUtils::findFile("boot.vtk", relative_path, 1024);
 //        btSoftBody* psb = btSoftBodyHelpers::CreateFromTetGenData(getDeformableDynamicsWorld()->getWorldInfo(),
@@ -341,27 +289,24 @@ void GraspDeformable::initPhysics()
         btSoftBody* psb = btSoftBodyHelpers::CreateFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), relative_path);
 
 //        psb->scale(btVector3(30, 30, 30)); // for banana
-        psb->scale(btVector3(.25, .25, .25));
+//        psb->scale(btVector3(.2, .2, .2));
-//        psb->scale(btVector3(.3, .3, .3));  // for tube, torus, boot
+//        psb->scale(btVector3(2, 2, 2));
-//        psb->scale(btVector3(1, 1, 1));  // for ditto
+        psb->scale(btVector3(.3, .3, .3));  // for tube, torus, boot
-        psb->translate(btVector3(.25, 0, 0.4)); 
+//        psb->scale(btVector3(.1, .1, .1));  // for ditto
-        psb->getCollisionShape()->setMargin(0.02);
+//        psb->translate(btVector3(.25, 2, 0.4));
-        psb->setTotalMass(.1);
+        psb->getCollisionShape()->setMargin(0.01);
+        psb->setTotalMass(.01);
         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;
         psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
         getDeformableDynamicsWorld()->addSoftBody(psb);
-        
+
-        btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(.0,.04, 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);
-        
+
-        btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(5,10);
+        btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(10,20, 0.01);
         getDeformableDynamicsWorld()->addForce(psb, neohookean);
         m_forces.push_back(neohookean);
     }
@@ -369,7 +314,7 @@ void GraspDeformable::initPhysics()
 //    // create a piece of cloth
 //    {
 //        bool onGround = false;
-//        const btScalar s = 4;
+//        const btScalar s = .4;
 //        btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, 0, -s),
 //                                                         btVector3(+s, 0, -s),
 //                                                         btVector3(-s, 0, +s),
@@ -386,17 +331,17 @@ void GraspDeformable::initPhysics()
 //                                                 2,2,
 //                                                 0, true);
 //
-//        psb->getCollisionShape()->setMargin(0.1);
+//        psb->getCollisionShape()->setMargin(0.02);
 //        psb->generateBendingConstraints(2);
-//        psb->setTotalMass(1);
+//        psb->setTotalMass(.01);
-//        psb->setSpringStiffness(2);
+//        psb->setSpringStiffness(5);
-//        psb->setDampingCoefficient(0.03);
+//        psb->setDampingCoefficient(0.05);
 //        psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
 //        psb->m_cfg.kCHR = 1; // collision hardness with rigid body
 //        psb->m_cfg.kDF = 1;
 //        psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
 //        getDeformableDynamicsWorld()->addSoftBody(psb);
-//        getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce());
+//        getDeformableDynamicsWorld()->addForce(psb, new btDeformableMassSpringForce(.2,0.02, true));
 //        getDeformableDynamicsWorld()->addForce(psb, new btDeformableGravityForce(gravity));
 //    }
     

examples/DeformableDemo/Pinch.cpp

@@ -11,20 +11,6 @@
  3. This notice may not be removed or altered from any source distribution.
  */
 
-///create 125 (5x5x5) dynamic object
-#define ARRAY_SIZE_X 5
-#define ARRAY_SIZE_Y 5
-#define ARRAY_SIZE_Z 5
-
-//maximum number of objects (and allow user to shoot additional boxes)
-#define MAX_PROXIES (ARRAY_SIZE_X * ARRAY_SIZE_Y * ARRAY_SIZE_Z + 1024)
-
-///scaling of the objects (0.1 = 20 centimeter boxes )
-#define SCALING 1.
-#define START_POS_X -5
-#define START_POS_Y -5
-#define START_POS_Z -3
-
 #include "Pinch.h"
 ///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
 #include "btBulletDynamicsCommon.h"
@@ -39,21 +25,13 @@
 #include "../CommonInterfaces/CommonRigidBodyBase.h"
 #include "../Utils/b3ResourcePath.h"
 
-///The Pinch shows the use of rolling friction.
+///The Pinch shows the frictional contact between kinematic rigid objects with deformable objects
-///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"
 };
 
-struct TetraBunny
-{
-#include "../SoftDemo/bunny.inl"
-};
-
-
 class Pinch : public CommonRigidBodyBase
 {
     btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
@@ -351,6 +329,7 @@ void Pinch::initPhysics()
         // add a grippers
         createGrip();
     }
+    getDeformableDynamicsWorld()->setImplicit(false);
 	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
 }
 

examples/DeformableDemo/PinchFriction.cpp

@@ -0,0 +1,409 @@
+/*
+ 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 <stdio.h>  //printf debugging
+
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../Utils/b3ResourcePath.h"
+
+///The PinchFriction shows the frictional contacts among volumetric deformable objects
+
+struct TetraCube
+{
+#include "../SoftDemo/cube.inl"
+};
+
+class PinchFriction : public CommonRigidBodyBase
+{
+    btAlignedObjectArray<btDeformableLagrangianForce*> 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<btRigidBody*>& rbs = world->getNonStaticRigidBodies();
+    if (rbs.size()<2)
+        return;
+    btRigidBody* rb0 = rbs[0];
+    btScalar pressTime = 0.9;
+    btScalar liftTime = 10;
+    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,-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);
+    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(200);
+    rb1->setFriction(200);
+}
+
+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);
+        
+        //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(2, 2, 1));
+        psb->translate(btVector3(0, 2.1, 2.2));
+        psb->getCollisionShape()->setMargin(0.1);
+        psb->setTotalMass(.6);
+        psb->m_cfg.kKHR = 1; // collision hardness with kinematic objects
+        psb->m_cfg.kCHR = 1; // collision hardness with rigid body
+        psb->m_cfg.kDF = 20;
+        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(4,8,.1);
+        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(2, 2, 1));
+        psb2->translate(btVector3(0, 2.1, -2.2));
+        psb2->getCollisionShape()->setMargin(0.1);
+        psb2->setTotalMass(.6);
+        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;
+        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(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.1);
+        psb3->setTotalMass(.6);
+        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);
+}
+
+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);
+}
+
+

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

examples/DeformableDemo/VolumetricDeformable.cpp

@@ -11,20 +11,6 @@
  3. This notice may not be removed or altered from any source distribution.
  */
 
-///create 125 (5x5x5) dynamic object
-#define ARRAY_SIZE_X 5
-#define ARRAY_SIZE_Y 5
-#define ARRAY_SIZE_Z 5
-
-//maximum number of objects (and allow user to shoot additional boxes)
-#define MAX_PROXIES (ARRAY_SIZE_X * ARRAY_SIZE_Y * ARRAY_SIZE_Z + 1024)
-
-///scaling of the objects (0.1 = 20 centimeter boxes )
-#define SCALING 1.
-#define START_POS_X -5
-#define START_POS_Y -5
-#define START_POS_Z -3
-
 #include "VolumetricDeformable.h"
 ///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
 #include "btBulletDynamicsCommon.h"
@@ -39,10 +25,7 @@
 #include "../CommonInterfaces/CommonRigidBodyBase.h"
 #include "../Utils/b3ResourcePath.h"
 
-///The VolumetricDeformable shows the use of rolling friction.
+///The VolumetricDeformable shows the contact between volumetric deformable objects and rigid objects.
-///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
 {
@@ -108,7 +91,7 @@ public:
     
     void Ctor_RbUpStack(int count)
     {
-        float mass = 0.02;
+        float mass = 1;
         
         btCompoundShape* cylinderCompound = new btCompoundShape;
         btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(2, .5, .5));
@@ -183,7 +166,7 @@ void VolumetricDeformable::initPhysics()
 	m_solver = sol;
 
 	m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration, deformableBodySolver);
-    btVector3 gravity = btVector3(0, -10, 0);
+    btVector3 gravity = btVector3(0, -100, 0);
 	m_dynamicsWorld->setGravity(gravity);
     getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
 	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
@@ -236,19 +219,17 @@ void VolumetricDeformable::initPhysics()
         psb->m_cfg.kDF = 0.5;
         psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
         
-        btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(0,0.03);
-        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);
         
-        btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(.5,2.5);
+        btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(30,100,0.02);
         getDeformableDynamicsWorld()->addForce(psb, neohookean);
         m_forces.push_back(neohookean);
         
     }
+    getDeformableDynamicsWorld()->setImplicit(true);
+    getDeformableDynamicsWorld()->setLineSearch(false);
     // add a few rigid bodies
     Ctor_RbUpStack(4); 
     

examples/ExampleBrowser/CMakeLists.txt

@@ -353,10 +353,18 @@ SET(BulletExampleBrowser_SRCS
  ../MultiBody/MultiBodyConstraintFeedback.cpp
   ../SoftDemo/SoftDemo.cpp
   ../SoftDemo/SoftDemo.h
+  ../DeformableDemo/DeformableContact.cpp
+  ../DeformableDemo/DeformableContact.h
   ../DeformableDemo/GraspDeformable.cpp
   ../DeformableDemo/GraspDeformable.h
   ../DeformableDemo/Pinch.cpp
   ../DeformableDemo/Pinch.h
+  ../DeformableDemo/DeformableSelfCollision.cpp
+  ../DeformableDemo/DeformableSelfCollision.h
+  ../DeformableDemo/PinchFriction.cpp
+  ../DeformableDemo/PinchFriction.h
+  ../DeformableDemo/ClothFriction.cpp
+  ../DeformableDemo/ClothFriction.h
   ../DeformableDemo/DeformableMultibody.cpp
   ../DeformableDemo/DeformableMultibody.h
   ../DeformableDemo/DeformableRigid.cpp

examples/ExampleBrowser/ExampleEntries.cpp

@@ -45,10 +45,14 @@
 #include "../DynamicControlDemo/MotorDemo.h"
 #include "../RollingFrictionDemo/RollingFrictionDemo.h"
 #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"
 #include "../DeformableDemo/GraspDeformable.h"
+#include "../DeformableDemo/DeformableContact.h"
 #include "../SharedMemory/PhysicsServerExampleBullet2.h"
 #include "../SharedMemory/PhysicsServerExample.h"
 #include "../SharedMemory/PhysicsClientExample.h"
@@ -186,6 +190,10 @@ 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),
         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),

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -376,7 +376,7 @@ B3_SHARED_API int b3LoadSoftBodyAddGravityForce(b3SharedMemoryCommandHandle comm
 	return 0;
 }
 
-B3_SHARED_API int b3LoadSoftBodySetCollsionHardness(b3SharedMemoryCommandHandle commandHandle, double collisionHardness)
+B3_SHARED_API int b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle commandHandle, double collisionHardness)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
 	b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);

examples/SharedMemory/PhysicsClientC_API.h

@@ -635,7 +635,7 @@ extern "C"
 	B3_SHARED_API int b3LoadSoftBodyAddNeoHookeanForce(b3SharedMemoryCommandHandle commandHandle, double NeoHookeanMu, double NeoHookeanLambda);
 	B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness);
 	B3_SHARED_API int b3LoadSoftBodyAddGravityForce(b3SharedMemoryCommandHandle commandHandle, double gravityX, double gravityY, double gravityZ);
-	B3_SHARED_API int b3LoadSoftBodySetCollsionHardness(b3SharedMemoryCommandHandle commandHandle, double collisionHardness);
+	B3_SHARED_API int b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle commandHandle, double collisionHardness);
 	B3_SHARED_API int b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHandle commandHandle, double frictionCoefficient);
 	B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle commandHandle, int useBendingSprings);
 

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -5055,15 +5055,24 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S
 		{
 			btSoftBody* psb = bodyHandle->m_softBody;
 			int totalBytesPerVertex = sizeof(btVector3);
-			int numVertices = psb->m_nodes.size();
+            bool separateRenderMesh = (psb->m_renderNodes.size() != 0);
+            int numVertices = separateRenderMesh ? psb->m_renderNodes.size() : psb->m_nodes.size();
 			int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1;
 			int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex;
 			int verticesCopied = btMin(maxNumVertices, numVerticesRemaining);
 			btVector3* verticesOut = (btVector3*)bufferServerToClient;
 			for (int i = 0; i < verticesCopied; ++i)
 			{
-				const btSoftBody::Node& n = psb->m_nodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex];
+                if (separateRenderMesh)
-				verticesOut[i] = n.m_x;
+                {
+                    const btSoftBody::Node& n = psb->m_renderNodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex];
+                    verticesOut[i] = n.m_x;
+                }
+                else
+                {
+                    const btSoftBody::Node& n = psb->m_nodes[i + clientCmd.m_requestMeshDataArgs.m_startingVertex];
+                    verticesOut[i] = n.m_x;
+                }
 			}
 
 			serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_COMPLETED;
@@ -7989,200 +7998,230 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
 	}
 
 	{
-		CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface());
+        btSoftBody* psb = NULL;
-		char relativeFileName[1024];
+        
-		char pathPrefix[1024];
-		pathPrefix[0] = 0;
-		if (fileIO->findResourcePath(loadSoftBodyArgs.m_fileName, relativeFileName, 1024))
-		{
-			b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
-		}
-		const std::string& error_message_prefix = "";
-		std::string out_found_filename;
-		int out_type=0;
 
-		bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
+	CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface());
-		btSoftBody* psb = NULL;
+	char relativeFileName[1024];
-		if (foundFile)
+	char pathPrefix[1024];
-		{
+	pathPrefix[0] = 0;
-		btScalar spring_elastic_stiffness, spring_damping_stiffness;
+	if (fileIO->findResourcePath(loadSoftBodyArgs.m_fileName, relativeFileName, 1024))
-		if (out_type == UrdfGeometry::FILE_OBJ)
+	{
-		{
+		b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
-			std::vector<tinyobj::shape_t> shapes;
+	}
-			tinyobj::attrib_t attribute;
+
-			std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), "", fileIO);
+        // add _sim.vtk postfix
-			if (!shapes.empty())
+        char relativeSimFileName[1024];
-			{
+        char sim_file_ending[9] = "_sim.vtk";
-				const tinyobj::shape_t& shape = shapes[0];
+        strncpy(relativeSimFileName, relativeFileName, strlen(relativeFileName));
-				btAlignedObjectArray<btScalar> vertices;
+        strncpy(relativeSimFileName + strlen(relativeFileName)-4, sim_file_ending, sizeof(sim_file_ending));
-				btAlignedObjectArray<int> indices;
+
-				for (int i = 0; i < attribute.vertices.size(); i++)
+	const std::string& error_message_prefix = "";
-				{
+	std::string out_found_filename;
-					vertices.push_back(attribute.vertices[i]);
+	std::string out_found_sim_filename;
-				}
+	int out_type, out_sim_type;
-				for (int i = 0; i < shape.mesh.indices.size(); i++)
+
-				{
+        bool render_mesh_is_sim_mesh = true;
-					indices.push_back(shape.mesh.indices[i].vertex_index);
+
-				}
+	bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
-				int numTris = shape.mesh.indices.size() / 3;
+        if (out_type == UrdfGeometry::FILE_OBJ)
-				if (numTris > 0)
+        {
-				{
+		    bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeSimFileName, error_message_prefix, &out_found_sim_filename, &out_sim_type);
-					psb = btSoftBodyHelpers::CreateFromTriMesh(m_data->m_dynamicsWorld->getWorldInfo(), &vertices[0], &indices[0], numTris);
+            render_mesh_is_sim_mesh = !foundFile;
-				}
+        }
-			}
+
+        if (render_mesh_is_sim_mesh)
+        {
+            out_sim_type = out_type;
+            out_found_sim_filename = out_found_filename;
+        }
+        
+        if (out_sim_type == UrdfGeometry::FILE_OBJ)
+        {
+	    std::vector<tinyobj::shape_t> shapes;
+	    tinyobj::attrib_t attribute;
+	    std::string err = tinyobj::LoadObj(attribute, shapes, out_found_sim_filename.c_str(), "", fileIO);
+	    if (!shapes.empty())
+	    {
+            const tinyobj::shape_t& shape = shapes[0];
+            btAlignedObjectArray<btScalar> vertices;
+	    	btAlignedObjectArray<int> indices;
+	    	for (int i = 0; i < attribute.vertices.size(); i++)
+	    	{
+	    		vertices.push_back(attribute.vertices[i]);
+	    	}
+	    	for (int i = 0; i < shape.mesh.indices.size(); i++)
+	    	{
+	    		indices.push_back(shape.mesh.indices[i].vertex_index);
+	    	}
+	    	int numTris = shape.mesh.indices.size() / 3;
+	    	if (numTris > 0)
+            {
+                psb = btSoftBodyHelpers::CreateFromTriMesh(m_data->m_dynamicsWorld->getWorldInfo(), &vertices[0], &indices[0], numTris);
+            }
+        }
 #ifndef SKIP_DEFORMABLE_BODY
-			if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE)
+        btScalar spring_elastic_stiffness, spring_damping_stiffness;
-			{
+	    if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE)
-				spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness;
+	    {
-				spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness;
+            spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness;
-				m_data->m_dynamicsWorld->addForce(psb, new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, false));
+            spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness;
-			}
+            m_data->m_dynamicsWorld->addForce(psb, new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, false));
+	    }
+#endif
+        }
+        else if (out_sim_type == UrdfGeometry::FILE_VTK)
+        {
+#ifndef SKIP_DEFORMABLE_BODY
+            psb = btSoftBodyHelpers::CreateFromVtkFile(m_data->m_dynamicsWorld->getWorldInfo(), out_found_sim_filename.c_str());
+            btScalar corotated_mu, corotated_lambda;
+            if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE)
+            {
+                    corotated_mu = clientCmd.m_loadSoftBodyArguments.m_corotatedMu;
+                    corotated_lambda = clientCmd.m_loadSoftBodyArguments.m_corotatedLambda;
+                    m_data->m_dynamicsWorld->addForce(psb, new btDeformableCorotatedForce(corotated_mu, corotated_lambda));
+            }
+            btScalar neohookean_mu, neohookean_lambda;
+            if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE)
+            {
+                    neohookean_mu = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanMu;
+                    neohookean_lambda = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanLambda;
+                    m_data->m_dynamicsWorld->addForce(psb, new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda));
+            }
+            btScalar spring_elastic_stiffness, spring_damping_stiffness;
+            if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE)
+            {
+                    spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness;
+                    spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness;
+                    m_data->m_dynamicsWorld->addForce(psb, new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true));
+            }
 #endif
 		}
-		else if (out_type == UrdfGeometry::FILE_VTK)
+		
-		{
-#ifndef SKIP_DEFORMABLE_BODY
-			psb = btSoftBodyHelpers::CreateFromVtkFile(m_data->m_dynamicsWorld->getWorldInfo(), out_found_filename.c_str());
-			btScalar corotated_mu, corotated_lambda;
-			if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE)
-			{
-				corotated_mu = clientCmd.m_loadSoftBodyArguments.m_corotatedMu;
-				corotated_lambda = clientCmd.m_loadSoftBodyArguments.m_corotatedLambda;
-				m_data->m_dynamicsWorld->addForce(psb, new btDeformableCorotatedForce(corotated_mu, corotated_lambda));
-			}
-			btScalar neohookean_mu, neohookean_lambda;
-			if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE)
-			{
-				neohookean_mu = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanMu;
-				neohookean_lambda = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanLambda;
-				m_data->m_dynamicsWorld->addForce(psb, new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda));
-			}
-			btScalar spring_elastic_stiffness, spring_damping_stiffness;
-			if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE)
-			{
-				spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness;
-				spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness;
-				m_data->m_dynamicsWorld->addForce(psb, new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true));
-			}
-#endif
-		}
-		}
 		if (psb != NULL)
 		{
 #ifndef SKIP_DEFORMABLE_BODY
-			btVector3 gravity = m_data->m_dynamicsWorld->getGravity();
+            if (!render_mesh_is_sim_mesh)
-			if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_GRAVITY_FORCE)
+            {
-			{
+                // load render mesh
-				m_data->m_dynamicsWorld->addForce(psb, new btDeformableGravityForce(gravity));
+                btSoftBodyHelpers::readRenderMeshFromObj(out_found_filename.c_str(), psb);
-			}
+                btSoftBodyHelpers::interpolateBarycentricWeights(psb);
-			btScalar collision_hardness = 1;
+            }
-			if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_COLLISION_HARDNESS)
+            else
-			{
+            {
-				collision_hardness = loadSoftBodyArgs.m_collisionHardness;
+                psb->m_renderNodes.resize(0);
-			}
+            }
-			psb->m_cfg.kKHR = collision_hardness;
+            btVector3 gravity = m_data->m_dynamicsWorld->getGravity();
-			psb->m_cfg.kCHR = collision_hardness;
+            if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_GRAVITY_FORCE)
+            {
+                    m_data->m_dynamicsWorld->addForce(psb, new btDeformableGravityForce(gravity));
+            }
+            btScalar collision_hardness = 1;
+            psb->m_cfg.kKHR = collision_hardness;
+            psb->m_cfg.kCHR = collision_hardness;
 
-			btScalar friction_coeff = 0;
+            btScalar friction_coeff = 0;
-			if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT)
+            if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT)
-			{
+            {
-				friction_coeff = loadSoftBodyArgs.m_frictionCoeff;
+                    friction_coeff = loadSoftBodyArgs.m_frictionCoeff;
-			}
+            }
-			psb->m_cfg.kDF = friction_coeff;
+            psb->m_cfg.kDF = friction_coeff;
+            
+            bool use_bending_spring = true;
+            if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS)
+            {
+                    use_bending_spring = loadSoftBodyArgs.m_useBendingSprings;
+            }
+            btSoftBody::Material* pm = psb->appendMaterial();
+            pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
+            if (use_bending_spring)
+            {
+                    psb->generateBendingConstraints(2,pm);
+            }
 
-			bool use_bending_spring = true;
+            // turn on the collision flag for deformable
-			if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS)
+            // collision between deformable and rigid
-			{
+            psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
-				use_bending_spring = loadSoftBodyArgs.m_useBendingSprings;
+            // collion between deformable and deformable and self-collision
-			}
+            psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
-			btSoftBody::Material* pm = psb->appendMaterial();
+            psb->setCollisionFlags(0);
-			pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
+	    psb->setTotalMass(mass);
-			if (use_bending_spring)
-			{
-				psb->generateBendingConstraints(2, pm);
-			}
-
-			// turn on the collision flag for deformable
-			// collision
-			psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
-			psb->setCollisionFlags(0);
-			psb->setTotalMass(mass);
 #else
-			btSoftBody::Material* pm = psb->appendMaterial();
+	    btSoftBody::Material* pm = psb->appendMaterial();
-			pm->m_kLST = 0.5;
+	    pm->m_kLST = 0.5;
-			pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
+	    pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
-			psb->generateBendingConstraints(2, pm);
+	    psb->generateBendingConstraints(2, pm);
-			psb->m_cfg.piterations = 20;
+	    psb->m_cfg.piterations = 20;
-			psb->m_cfg.kDF = 0.5;
+	    psb->m_cfg.kDF = 0.5;
-			//turn on softbody vs softbody collision
+	    //turn on softbody vs softbody collision
-			psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS;
+	    psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS;
-			psb->randomizeConstraints();
+	    psb->randomizeConstraints();
-			psb->setTotalMass(mass, true);
+	    psb->setTotalMass(mass, true);
 #endif
-			psb->scale(btVector3(scale, scale, scale));
+	    psb->scale(btVector3(scale, scale, scale));
-			psb->rotate(initialOrn);
+	    psb->rotate(initialOrn);
-			psb->translate(initialPos);
+	    psb->translate(initialPos);
 
-			psb->getCollisionShape()->setMargin(collisionMargin);
+	    psb->getCollisionShape()->setMargin(collisionMargin);
-			psb->getCollisionShape()->setUserPointer(psb);
+	    psb->getCollisionShape()->setUserPointer(psb);
-			m_data->m_dynamicsWorld->addSoftBody(psb);
+	    m_data->m_dynamicsWorld->addSoftBody(psb);
-			m_data->m_guiHelper->createCollisionShapeGraphicsObject(psb->getCollisionShape());
+	    m_data->m_guiHelper->createCollisionShapeGraphicsObject(psb->getCollisionShape());
-			m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
+	    m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
-			int bodyUniqueId = m_data->m_bodyHandles.allocHandle();
+	    int bodyUniqueId = m_data->m_bodyHandles.allocHandle();
-			InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
+	    InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
-			bodyHandle->m_softBody = psb;
+	    bodyHandle->m_softBody = psb;
 
-			b3VisualShapeData visualShape;
+	    b3VisualShapeData visualShape;
 
-			visualShape.m_objectUniqueId = bodyUniqueId;
+	    visualShape.m_objectUniqueId = bodyUniqueId;
-			visualShape.m_linkIndex = -1;
+	    visualShape.m_linkIndex = -1;
-			visualShape.m_visualGeometryType = URDF_GEOM_MESH;
+	    visualShape.m_visualGeometryType = URDF_GEOM_MESH;
-			//dimensions just contains the scale
+	    //dimensions just contains the scale
-			visualShape.m_dimensions[0] = scale;
+	    visualShape.m_dimensions[0] = scale;
-			visualShape.m_dimensions[1] = scale;
+	    visualShape.m_dimensions[1] = scale;
-			visualShape.m_dimensions[2] = scale;
+	    visualShape.m_dimensions[2] = scale;
-			//filename
+	    //filename
-			strncpy(visualShape.m_meshAssetFileName, relativeFileName, VISUAL_SHAPE_MAX_PATH_LEN);
+	    strncpy(visualShape.m_meshAssetFileName, relativeFileName, VISUAL_SHAPE_MAX_PATH_LEN);
-			visualShape.m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN - 1] = 0;
+	    visualShape.m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN - 1] = 0;
-			//position and orientation
+	    //position and orientation
-			visualShape.m_localVisualFrame[0] = initialPos[0];
+	    visualShape.m_localVisualFrame[0] = initialPos[0];
-			visualShape.m_localVisualFrame[1] = initialPos[1];
+	    visualShape.m_localVisualFrame[1] = initialPos[1];
-			visualShape.m_localVisualFrame[2] = initialPos[2];
+	    visualShape.m_localVisualFrame[2] = initialPos[2];
-			visualShape.m_localVisualFrame[3] = initialOrn[0];
+	    visualShape.m_localVisualFrame[3] = initialOrn[0];
-			visualShape.m_localVisualFrame[4] = initialOrn[1];
+	    visualShape.m_localVisualFrame[4] = initialOrn[1];
-			visualShape.m_localVisualFrame[5] = initialOrn[2];
+	    visualShape.m_localVisualFrame[5] = initialOrn[2];
-			visualShape.m_localVisualFrame[6] = initialOrn[3];
+	    visualShape.m_localVisualFrame[6] = initialOrn[3];
-			//color and ids to be set by the renderer
+	    //color and ids to be set by the renderer
-			visualShape.m_rgbaColor[0] = 0;
+	    visualShape.m_rgbaColor[0] = 0;
-			visualShape.m_rgbaColor[1] = 0;
+	    visualShape.m_rgbaColor[1] = 0;
-			visualShape.m_rgbaColor[2] = 0;
+	    visualShape.m_rgbaColor[2] = 0;
-			visualShape.m_rgbaColor[3] = 1;
+	    visualShape.m_rgbaColor[3] = 1;
-			visualShape.m_tinyRendererTextureId = -1;
+	    visualShape.m_tinyRendererTextureId = -1;
-			visualShape.m_textureUniqueId = -1;
+	    visualShape.m_textureUniqueId  =-1;
-			visualShape.m_openglTextureId = -1;
+	    visualShape.m_openglTextureId = -1;
 
-			m_data->m_pluginManager.getRenderInterface()->addVisualShape(&visualShape, fileIO);
+	    m_data->m_pluginManager.getRenderInterface()->addVisualShape(&visualShape, fileIO);
 
-			serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId;
+	    serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId;
-			serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_COMPLETED;
+	    serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_COMPLETED;
 
-			int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
+	    int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
-			serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
+	    serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
 
 #ifdef ENABLE_LINK_MAPPER
-			if (m_data->m_urdfLinkNameMapper.size())
+            if (m_data->m_urdfLinkNameMapper.size())
-			{
+            {
-				serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size() - 1)->m_memSerializer->getCurrentBufferSize();
+              serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size() - 1)->m_memSerializer->getCurrentBufferSize();
-			}
+            }
 #endif
-			serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
+            serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
-			InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
+            InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
-			strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());
+            strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());
 
-			b3Notification notification;
+		    b3Notification notification;
-			notification.m_notificationType = BODY_ADDED;
+		    notification.m_notificationType = BODY_ADDED;
-			notification.m_bodyArgs.m_bodyUniqueId = bodyUniqueId;
+		    notification.m_bodyArgs.m_bodyUniqueId = bodyUniqueId;
-			m_data->m_pluginManager.addNotification(notification);
+		    m_data->m_pluginManager.addNotification(notification);
 		}
 	}
 #endif
@@ -8879,8 +8918,8 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
 	{
 		SharedMemoryStatus& serverCmd = serverStatusOut;
 		serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED;
-		serverCmd.m_dynamicsInfo.m_bodyType = BT_MULTI_BODY;
+                serverCmd.m_dynamicsInfo.m_bodyType = BT_MULTI_BODY;
-
+                
 		btMultiBody* mb = body->m_multiBody;
 		if (linkIndex == -1)
 		{
@@ -8994,7 +9033,7 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
 	{
 		SharedMemoryStatus& serverCmd = serverStatusOut;
 		serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED;
-		serverCmd.m_dynamicsInfo.m_bodyType = BT_RIGID_BODY;
+                serverCmd.m_dynamicsInfo.m_bodyType = BT_RIGID_BODY;
 
 		btRigidBody* rb = body->m_rigidBody;
 		serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = rb->getFriction();
@@ -9005,8 +9044,7 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S
 		serverCmd.m_dynamicsInfo.m_mass = rb->getMass();
 	}
 #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
-	else if (body && body->m_softBody)
+	else if (body && body->m_softBody){
-	{
 		SharedMemoryStatus& serverCmd = serverStatusOut;
 		serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED;
 		serverCmd.m_dynamicsInfo.m_bodyType = BT_SOFT_BODY;

examples/pybullet/examples/load_soft_body.py

@@ -5,6 +5,7 @@ physicsClient = p.connect(p.DIRECT)
 
 p.setGravity(0, 0, -10)
 planeId = p.loadURDF("plane.urdf")
+boxId = p.loadURDF("cube.urdf", useMaximalCoordinates = True)
 bunnyId = p.loadSoftBody("bunny.obj")
 #meshData = p.getMeshData(bunnyId)
 #print("meshData=",meshData)
@@ -14,6 +15,10 @@ useRealTimeSimulation = 1
 if (useRealTimeSimulation):
   p.setRealTimeSimulation(1)
 
+print(p.getDynamicsInfo(planeId, -1))
+print(p.getDynamicsInfo(bunnyId, 0))
+print(p.getDynamicsInfo(boxId, -1))
+
 while p.isConnected():
   p.setGravity(0, 0, -10)
   if (useRealTimeSimulation):

setup.py

@@ -250,6 +250,7 @@ sources = ["examples/pybullet/pybullet.c"]\
 +["src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp"]\
 +["src/BulletSoftBody/btDeformableBodySolver.cpp"]\
 +["src/BulletSoftBody/btDeformableContactProjection.cpp"]\
++["src/BulletSoftBody/btDeformableContactConstraint.cpp"]\
 +["src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp"]\
 +["src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp"]\
 +["src/BulletInverseDynamics/IDMath.cpp"]\

src/BulletCollision/BroadphaseCollision/btDbvt.h

@@ -21,7 +21,6 @@ subject to the following restrictions:
 #include "LinearMath/btVector3.h"
 #include "LinearMath/btTransform.h"
 #include "LinearMath/btAabbUtil2.h"
-
 //
 // Compile time configuration
 //
@@ -131,6 +130,8 @@ subject to the following restrictions:
 /* btDbvtAabbMm			*/
 struct btDbvtAabbMm
 {
+    DBVT_INLINE btDbvtAabbMm(){}
+    DBVT_INLINE btDbvtAabbMm(const btDbvtAabbMm& other): mi(other.mi), mx(other.mx){}
 	DBVT_INLINE btVector3 Center() const { return ((mi + mx) / 2); }
 	DBVT_INLINE btVector3 Lengths() const { return (mx - mi); }
 	DBVT_INLINE btVector3 Extents() const { return ((mx - mi) / 2); }
@@ -190,6 +191,36 @@ struct btDbvtNode
 	};
 };
 
+/* btDbv(normal)tNode                */
+struct btDbvntNode
+{
+    btDbvtVolume volume;
+    btVector3 normal;
+    btScalar angle;
+    DBVT_INLINE bool isleaf() const { return (childs[1] == 0); }
+    DBVT_INLINE bool isinternal() const { return (!isleaf()); }
+    btDbvntNode* childs[2];
+    void* data;
+
+    btDbvntNode(const btDbvtNode* n)
+    : volume(n->volume)
+    , angle(0)
+    , normal(0,0,0)
+    , data(n->data)
+    {
+        childs[0] = 0;
+        childs[1] = 0;
+    }
+    
+    ~btDbvntNode()
+    {
+        if (childs[0])
+            delete childs[0];
+        if (childs[1])
+            delete childs[1];
+    }
+};
+
 typedef btAlignedObjectArray<const btDbvtNode*> btNodeStack;
 
 ///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree).
@@ -225,6 +256,14 @@ struct btDbvt
 		btDbvtNode* parent;
 		sStkCLN(const btDbvtNode* n, btDbvtNode* p) : node(n), parent(p) {}
 	};
+    
+    struct sStknNN
+    {
+        const btDbvntNode* a;
+        const btDbvntNode* b;
+        sStknNN() {}
+        sStknNN(const btDbvntNode* na, const btDbvntNode* nb) : a(na), b(nb) {}
+    };
 	// Policies/Interfaces
 
 	/* ICollide	*/
@@ -234,6 +273,7 @@ struct btDbvt
 		DBVT_VIRTUAL void Process(const btDbvtNode*, const btDbvtNode*) {}
 		DBVT_VIRTUAL void Process(const btDbvtNode*) {}
 		DBVT_VIRTUAL void Process(const btDbvtNode* n, btScalar) { Process(n); }
+        DBVT_VIRTUAL void Process(const btDbvntNode*, const btDbvntNode*) {}
 		DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return (true); }
 		DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return (true); }
 	};
@@ -306,6 +346,9 @@ struct btDbvt
 	void collideTT(const btDbvtNode* root0,
 				   const btDbvtNode* root1,
 				   DBVT_IPOLICY);
+    DBVT_PREFIX
+    void selfCollideT(const btDbvntNode* root,
+                   DBVT_IPOLICY);
 
 	DBVT_PREFIX
 	void collideTTpersistentStack(const btDbvtNode* root0,
@@ -837,6 +880,71 @@ inline void btDbvt::collideTT(const btDbvtNode* root0,
 	}
 }
 
+//
+DBVT_PREFIX
+inline void btDbvt::selfCollideT(const btDbvntNode* root,
+                              DBVT_IPOLICY)
+{
+    DBVT_CHECKTYPE
+    if (root)
+    {
+        int depth = 1;
+        int treshold = DOUBLE_STACKSIZE - 4;
+        btAlignedObjectArray<sStknNN> stkStack;
+        stkStack.resize(DOUBLE_STACKSIZE);
+        stkStack[0] = sStknNN(root, root);
+        do
+        {
+            sStknNN p = stkStack[--depth];
+            if (depth > treshold)
+            {
+                stkStack.resize(stkStack.size() * 2);
+                treshold = stkStack.size() - 4;
+            }
+            if (p.a == p.b)
+            {
+                if (p.a->isinternal() && p.a->angle > SIMD_PI)
+                {
+                    stkStack[depth++] = sStknNN(p.a->childs[0], p.a->childs[0]);
+                    stkStack[depth++] = sStknNN(p.a->childs[1], p.a->childs[1]);
+                    stkStack[depth++] = sStknNN(p.a->childs[0], p.a->childs[1]);
+                }
+            }
+            else if (Intersect(p.a->volume, p.b->volume))
+            {
+                if (p.a->isinternal())
+                {
+                    if (p.b->isinternal())
+                    {
+                        stkStack[depth++] = sStknNN(p.a->childs[0], p.b->childs[0]);
+                        stkStack[depth++] = sStknNN(p.a->childs[1], p.b->childs[0]);
+                        stkStack[depth++] = sStknNN(p.a->childs[0], p.b->childs[1]);
+                        stkStack[depth++] = sStknNN(p.a->childs[1], p.b->childs[1]);
+                    }
+                    else
+                    {
+                        stkStack[depth++] = sStknNN(p.a->childs[0], p.b);
+                        stkStack[depth++] = sStknNN(p.a->childs[1], p.b);
+                    }
+                }
+                else
+                {
+                    if (p.b->isinternal())
+                    {
+                        stkStack[depth++] = sStknNN(p.a, p.b->childs[0]);
+                        stkStack[depth++] = sStknNN(p.a, p.b->childs[1]);
+                    }
+                    else
+                    {
+                        policy.Process(p.a, p.b);
+                    }
+                }
+            }
+        } while (depth);
+    }
+}
+
+
 DBVT_PREFIX
 inline void btDbvt::collideTTpersistentStack(const btDbvtNode* root0,
 											 const btDbvtNode* root1,

src/BulletSoftBody/CMakeLists.txt

@@ -22,6 +22,7 @@ SET(BulletSoftBody_SRCS
 	btDeformableMultiBodyConstraintSolver.cpp
 	btDeformableContactProjection.cpp
 	btDeformableMultiBodyDynamicsWorld.cpp
+	btDeformableContactConstraint.cpp
 
 )
 
@@ -54,6 +55,7 @@ SET(BulletSoftBody_HDRS
 	btDeformableMultiBodyConstraintSolver.h
 	btDeformableContactProjection.h
 	btDeformableMultiBodyDynamicsWorld.h
+	btDeformableContactConstraint.h
 
 	btSoftBodySolverVertexBuffer.h
 )

src/BulletSoftBody/btCGProjection.h

@@ -20,8 +20,6 @@
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
 
-//class btDeformableMultiBodyDynamicsWorld;
-
 struct DeformableContactConstraint
 {
     const btSoftBody::Node* m_node;
@@ -73,6 +71,8 @@ public:
     typedef btAlignedObjectArray<btAlignedObjectArray<btScalar> > TArrayStack;
     btAlignedObjectArray<btSoftBody *>& m_softBodies;
     const btScalar& m_dt;
+    // map from node indices to node pointers
+    const btAlignedObjectArray<btSoftBody::Node*>* m_nodes;
     
     btCGProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
     : m_softBodies(softBodies)
@@ -95,6 +95,11 @@ public:
     virtual void reinitialize(bool nodeUpdated)
     {
     }
+    
+    virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes)
+    {
+        m_nodes = nodes;
+    }
 };
 
 

src/BulletSoftBody/btConjugateGradient.h

@@ -17,6 +17,7 @@
 #define BT_CONJUGATE_GRADIENT_H
 #include <iostream>
 #include <cmath>
+#include <limits>
 #include <LinearMath/btAlignedObjectArray.h>
 #include <LinearMath/btVector3.h>
 #include "LinearMath/btQuickprof.h"
@@ -26,16 +27,18 @@ class btConjugateGradient
     typedef btAlignedObjectArray<btVector3> TVStack;
     TVStack r,p,z,temp;
     int max_iterations;
+    btScalar tolerance;
 public:
     btConjugateGradient(const int max_it_in)
     : max_iterations(max_it_in)
     {
+       tolerance = 1024 * std::numeric_limits<btScalar>::epsilon();
     }
     
     virtual ~btConjugateGradient(){}
     
     // return the number of iterations taken
-    int solve(MatrixX& A, TVStack& x, const TVStack& b, btScalar tolerance, bool verbose = false)
+    int solve(MatrixX& A, TVStack& x, const TVStack& b, bool verbose = false)
     {
         BT_PROFILE("CGSolve");
         btAssert(x.size() == b.size());
@@ -48,7 +51,8 @@ public:
         A.precondition(r, z);
         A.project(z);
         btScalar r_dot_z = dot(z,r);
-        if (r_dot_z < tolerance) {
+        btScalar local_tolerance = tolerance;
+        if (std::sqrt(r_dot_z) <= local_tolerance) {
             if (verbose)
             {
                 std::cout << "Iteration = 0" << std::endl;
@@ -58,11 +62,21 @@ public:
         }
         p = z;
         btScalar r_dot_z_new = r_dot_z;
-        for (int k = 1; k < max_iterations; k++) {
+        for (int k = 1; k <= max_iterations; k++) {
             // temp = A*p
             A.multiply(p, temp);
             A.project(temp);
             // alpha = r^T * z / (p^T * A * p)
+            if (dot(p,temp) < SIMD_EPSILON)
+            {
+                if (verbose)
+                    std::cout << "Encountered negative direction in CG!" << std::endl;
+                if (k == 1)
+                {
+                    x = b;
+                }
+              return k;
+            }
             btScalar alpha = r_dot_z_new / dot(p, temp);
             //  x += alpha * p;
             multAndAddTo(alpha, p, x);
@@ -72,14 +86,15 @@ public:
             A.precondition(r, z);
             r_dot_z = r_dot_z_new;
             r_dot_z_new = dot(r,z);
-            if (r_dot_z_new < tolerance) {
+            if (std::sqrt(r_dot_z_new) < local_tolerance) {
                 if (verbose)
                 {
                     std::cout << "ConjugateGradient iterations " << k << std::endl;
                 }
                 return k;
             }
-            btScalar beta = r_dot_z_new/ r_dot_z;
+
+            btScalar beta = r_dot_z_new/r_dot_z;
             p = multAndAdd(beta, p, z);
         }
         if (verbose)

src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp

@@ -19,7 +19,7 @@
 
 btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAlignedObjectArray<btSoftBody *>& softBodies, const TVStack& backup_v)
 : m_softBodies(softBodies)
-, projection(m_softBodies, m_dt)
+, m_projection(softBodies)
 , m_backupVelocity(backup_v)
 , m_implicit(false)
 {
@@ -46,7 +46,7 @@ void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated, btScalar
     {
         m_lf[i]->reinitialize(nodeUpdated);
     }
-    projection.reinitialize(nodeUpdated);
+    m_projection.reinitialize(nodeUpdated);
     m_preconditioner->reinitialize(nodeUpdated);
 }
 
@@ -84,11 +84,14 @@ void btDeformableBackwardEulerObjective::multiply(const TVStack& x, TVStack& b)
 
 void btDeformableBackwardEulerObjective::updateVelocity(const TVStack& dv)
 {
-    // only the velocity of the constrained nodes needs to be updated during contact solve
+    for (int i = 0; i < m_softBodies.size(); ++i)
-    for (int i = 0; i < projection.m_constraints.size(); ++i)
     {
-        int index = projection.m_constraints.getKeyAtIndex(i).getUid1();
+        btSoftBody* psb = m_softBodies[i];
-        m_nodes[index]->m_v = m_backupVelocity[index] + dv[index];
+        for (int j = 0; j < psb->m_nodes.size(); ++j)
+        {
+            btSoftBody::Node& node = psb->m_nodes[j];
+            node.m_v = m_backupVelocity[node.index] + dv[node.index];
+        }
     }
 }
 
@@ -126,24 +129,34 @@ void btDeformableBackwardEulerObjective::computeResidual(btScalar dt, TVStack &r
             m_lf[i]->addScaledDampingForce(dt, residual);
         }
     }
-    projection.project(residual);
+    m_projection.project(residual);
 }
 
 btScalar btDeformableBackwardEulerObjective::computeNorm(const TVStack& residual) const
 {
-    btScalar norm_squared = 0;
+    btScalar mag = 0;
     for (int i = 0; i < residual.size(); ++i)
     {
-        norm_squared += residual[i].length2();
+        mag += residual[i].length2();
     }
-    return std::sqrt(norm_squared);
+    return std::sqrt(mag);
+}
+
+btScalar btDeformableBackwardEulerObjective::totalEnergy(btScalar dt)
+{
+    btScalar e = 0;
+    for (int i = 0; i < m_lf.size(); ++i)
+    {
+        e += m_lf[i]->totalEnergy(dt);
+    }
+    return e;
 }
 
 void btDeformableBackwardEulerObjective::applyExplicitForce(TVStack& force)
 {
     for (int i = 0; i < m_softBodies.size(); ++i)
     {
-        m_softBodies[i]->updateDeformation();
+        m_softBodies[i]->advanceDeformation();
     }
     
     for (int i = 0; i < m_lf.size(); ++i)
@@ -170,10 +183,10 @@ void btDeformableBackwardEulerObjective::initialGuess(TVStack& dv, const TVStack
 //set constraints as projections
 void btDeformableBackwardEulerObjective::setConstraints()
 {
-    projection.setConstraints();
+    m_projection.setConstraints();
 }
 
 void btDeformableBackwardEulerObjective::applyDynamicFriction(TVStack& r)
 {
-     projection.applyDynamicFriction(r);
+     m_projection.applyDynamicFriction(r);
 }

src/BulletSoftBody/btDeformableBackwardEulerObjective.h

@@ -34,7 +34,7 @@ public:
     btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
     btAlignedObjectArray<btSoftBody *>& m_softBodies;
     Preconditioner* m_preconditioner;
-    btDeformableContactProjection projection;
+    btDeformableContactProjection m_projection;
     const TVStack& m_backupVelocity;
     btAlignedObjectArray<btSoftBody::Node* > m_nodes;
     bool m_implicit;
@@ -71,13 +71,7 @@ public:
     
     void setDt(btScalar dt);
     
-    // enforce constraints in CG solve
+    // add friction force to residual
-    void enforceConstraint(TVStack& x)
-    {
-        BT_PROFILE("enforceConstraint");
-        projection.enforceConstraint(x);
-    }
-    
     void applyDynamicFriction(TVStack& r);
     
     // add dv to velocity
@@ -90,7 +84,7 @@ public:
     void project(TVStack& r)
     {
         BT_PROFILE("project");
-        projection.project(r);
+        m_projection.project(r);
     }
     
     // perform precondition M^(-1) x = b
@@ -99,18 +93,25 @@ public:
         m_preconditioner->operator()(x,b);
     }
 
+    // reindex all the vertices 
     virtual void updateId()
     {
-        size_t id = 0;
+        size_t node_id = 0;
+        size_t face_id = 0;
         m_nodes.clear();
         for (int i = 0; i < m_softBodies.size(); ++i)
         {
             btSoftBody* psb = m_softBodies[i];
             for (int j = 0; j < psb->m_nodes.size(); ++j)
             {
-                psb->m_nodes[j].index = id;
+                psb->m_nodes[j].index = node_id;
                 m_nodes.push_back(&psb->m_nodes[j]);
-                ++id;
+                ++node_id;
+            }
+            for (int j = 0; j < psb->m_faces.size(); ++j)
+            {
+                psb->m_faces[j].m_index = face_id;
+                ++face_id;
             }
         }
     }
@@ -124,6 +125,9 @@ public:
     {
         m_implicit = implicit;
     }
+
+    // Calculate the total potential energy in the system
+    btScalar totalEnergy(btScalar dt);
 };
 
 #endif /* btBackwardEulerObjective_h */

src/BulletSoftBody/btDeformableBodySolver.cpp

@@ -16,6 +16,7 @@
 #include <stdio.h>
 #include <limits>
 #include "btDeformableBodySolver.h"
+#include "btSoftBodyInternals.h"
 #include "LinearMath/btQuickprof.h"
 
 btDeformableBodySolver::btDeformableBodySolver()
@@ -23,8 +24,9 @@ btDeformableBodySolver::btDeformableBodySolver()
 , m_cg(20)
 , m_maxNewtonIterations(5)
 , m_newtonTolerance(1e-4)
+, m_lineSearch(true)
 {
-    m_objective = new btDeformableBackwardEulerObjective(m_softBodySet, m_backupVelocity);
+    m_objective = new btDeformableBackwardEulerObjective(m_softBodies, m_backupVelocity);
 }
 
 btDeformableBodySolver::~btDeformableBodySolver()
@@ -49,9 +51,9 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
             updateState();
             // add the inertia term in the residual
             int counter = 0;
-            for (int k = 0; k < m_softBodySet.size(); ++k)
+            for (int k = 0; k < m_softBodies.size(); ++k)
             {
-                btSoftBody* psb = m_softBodySet[k];
+                btSoftBody* psb = m_softBodies[k];
                 for (int j = 0; j < psb->m_nodes.size(); ++j)
                 {
                     if (psb->m_nodes[j].m_im > 0)
@@ -63,13 +65,36 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
             }
             
             m_objective->computeResidual(solverdt, m_residual);
-            if (m_objective->computeNorm(m_residual) < m_newtonTolerance)
+            if (m_objective->computeNorm(m_residual) < m_newtonTolerance && i > 0)
             {
                 break;
             }
+            // todo xuchenhan@: this really only needs to be calculated once
             m_objective->applyDynamicFriction(m_residual);
-            computeStep(m_ddv, m_residual);
+            if (m_lineSearch)
-            updateDv();
+            {
+                btScalar inner_product = computeDescentStep(m_ddv,m_residual);
+                btScalar alpha = 0.01, beta = 0.5; // Boyd & Vandenberghe suggested alpha between 0.01 and 0.3, beta between 0.1 to 0.8
+                btScalar scale = 2;
+                btScalar f0 = m_objective->totalEnergy(solverdt)+kineticEnergy(), f1, f2;
+                backupDv();
+                do {
+                    scale *= beta;
+                    if (scale < 1e-8) {
+                        return;
+                    }
+                    updateEnergy(scale);
+                    f1 = m_objective->totalEnergy(solverdt)+kineticEnergy();
+                    f2 = f0 - alpha * scale * inner_product;
+                } while (!(f1 < f2+SIMD_EPSILON)); // if anything here is nan then the search continues
+                revertDv();
+                updateDv(scale);
+            }
+            else
+            {
+                computeStep(m_ddv, m_residual);
+                updateDv();
+            }
             for (int j = 0; j < m_numNodes; ++j)
             {
                 m_ddv[j].setZero();
@@ -79,31 +104,107 @@ void btDeformableBodySolver::solveDeformableConstraints(btScalar solverdt)
     }
 }
 
+btScalar btDeformableBodySolver::kineticEnergy()
+{
+    btScalar ke = 0;
+    for (int i = 0; i < m_softBodies.size();++i)
+    {
+        btSoftBody* psb = m_softBodies[i];
+        for (int j = 0; j < psb->m_nodes.size();++j)
+        {
+            btSoftBody::Node& node = psb->m_nodes[j];
+            if (node.m_im > 0)
+            {
+                ke += m_dv[node.index].length2() * 0.5 / node.m_im;
+            }
+        }
+    }
+    return ke;
+}
+
+void btDeformableBodySolver::backupDv()
+{
+    m_backup_dv.resize(m_dv.size());
+    for (int i = 0; i<m_backup_dv.size(); ++i)
+    {
+        m_backup_dv[i] = m_dv[i];
+    }
+}
+
+void btDeformableBodySolver::revertDv()
+{
+    for (int i = 0; i<m_backup_dv.size(); ++i)
+    {
+        m_dv[i] = m_backup_dv[i];
+    }
+}
+
+void btDeformableBodySolver::updateEnergy(btScalar scale)
+{
+    for (int i = 0; i<m_dv.size(); ++i)
+    {
+        m_dv[i] = m_backup_dv[i] + scale * m_ddv[i];
+    }
+    updateState();
+}
+
+
+btScalar btDeformableBodySolver::computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose)
+{
+    m_cg.solve(*m_objective, ddv, residual, false);
+    btScalar inner_product = m_cg.dot(residual, m_ddv);
+    btScalar res_norm = m_objective->computeNorm(residual);
+    btScalar tol = 1e-5 * res_norm * m_objective->computeNorm(m_ddv);
+    if (inner_product < -tol)
+    {
+        if (verbose)
+        {
+            std::cout << "Looking backwards!" << std::endl;
+        }
+        for (int i = 0; i < m_ddv.size();++i)
+        {
+            m_ddv[i] = -m_ddv[i];
+        }
+        inner_product = -inner_product;
+    }
+    else if (std::abs(inner_product) < tol)
+    {
+        if (verbose)
+        {
+            std::cout << "Gradient Descent!" << std::endl;
+        }
+        btScalar scale = m_objective->computeNorm(m_ddv) / res_norm;
+        for (int i = 0; i < m_ddv.size();++i)
+        {
+            m_ddv[i] = scale * residual[i];
+        }
+        inner_product = scale * res_norm * res_norm;
+    }
+    return inner_product;
+}
+
 void btDeformableBodySolver::updateState()
 {
     updateVelocity();
     updateTempPosition();
-    
 }
 
-void btDeformableBodySolver::updateDv()
+void btDeformableBodySolver::updateDv(btScalar scale)
 {
     for (int i = 0; i < m_numNodes; ++i)
     {
-        m_dv[i] += m_ddv[i];
+        m_dv[i] += scale * m_ddv[i];
     }
 }
 
 void btDeformableBodySolver::computeStep(TVStack& ddv, const TVStack& residual)
 {
-    //btScalar tolerance = std::numeric_limits<btScalar>::epsilon() * m_objective->computeNorm(residual);
+    m_cg.solve(*m_objective, ddv, residual, false);
-    btScalar tolerance = std::numeric_limits<btScalar>::epsilon();
-    m_cg.solve(*m_objective, ddv, residual, tolerance);
 }
 
 void btDeformableBodySolver::reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt)
 {
-    m_softBodySet.copyFromArray(softBodies);
+    m_softBodies.copyFromArray(softBodies);
     bool nodeUpdated = updateNodes();
     
     if (nodeUpdated)
@@ -134,10 +235,8 @@ void btDeformableBodySolver::setConstraints()
 
 btScalar btDeformableBodySolver::solveContactConstraints()
 {
-    BT_PROFILE("setConstraint");
+    BT_PROFILE("solveContactConstraints");
-    btScalar maxSquaredResidual = m_objective->projection.update();
+    btScalar maxSquaredResidual = m_objective->m_projection.update();
-    m_objective->enforceConstraint(m_dv);
-    m_objective->updateVelocity(m_dv);
     return maxSquaredResidual;
 }
 
@@ -145,9 +244,9 @@ btScalar btDeformableBodySolver::solveContactConstraints()
 void btDeformableBodySolver::updateVelocity()
 {
     int counter = 0;
-    for (int i = 0; i < m_softBodySet.size(); ++i)
+    for (int i = 0; i < m_softBodies.size(); ++i)
     {
-        btSoftBody* psb = m_softBodySet[i];
+        btSoftBody* psb = m_softBodies[i];
         for (int j = 0; j < psb->m_nodes.size(); ++j)
         {
             // set NaN to zero;
@@ -164,9 +263,9 @@ void btDeformableBodySolver::updateVelocity()
 void btDeformableBodySolver::updateTempPosition()
 {
     int counter = 0;
-    for (int i = 0; i < m_softBodySet.size(); ++i)
+    for (int i = 0; i < m_softBodies.size(); ++i)
     {
-        btSoftBody* psb = m_softBodySet[i];
+        btSoftBody* psb = m_softBodies[i];
         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;
@@ -179,9 +278,9 @@ void btDeformableBodySolver::updateTempPosition()
 void btDeformableBodySolver::backupVelocity()
 {
     int counter = 0;
-    for (int i = 0; i < m_softBodySet.size(); ++i)
+    for (int i = 0; i < m_softBodies.size(); ++i)
     {
-        btSoftBody* psb = m_softBodySet[i];
+        btSoftBody* psb = m_softBodies[i];
         for (int j = 0; j < psb->m_nodes.size(); ++j)
         {
             m_backupVelocity[counter++] = psb->m_nodes[j].m_v;
@@ -189,25 +288,20 @@ void btDeformableBodySolver::backupVelocity()
     }
 }
 
-void btDeformableBodySolver::backupVn()
+void btDeformableBodySolver::setupDeformableSolve(bool implicit)
 {
     int counter = 0;
-    for (int i = 0; i < m_softBodySet.size(); ++i)
+    for (int i = 0; i < m_softBodies.size(); ++i)
     {
-        btSoftBody* psb = m_softBodySet[i];
+        btSoftBody* psb = m_softBodies[i];
         for (int j = 0; j < psb->m_nodes.size(); ++j)
         {
-            // Here:
+            if (implicit)
-            // dv = 0 for nodes not in constraints
-            // dv = v_{n+1} - v_{n+1}^* for nodes in constraints
-            if (m_objective->projection.m_constraints.find(psb->m_nodes[j].index)!=NULL)
             {
-                m_dv[counter] += m_backupVelocity[counter] - psb->m_nodes[j].m_vn;
+                m_backupVelocity[counter] = psb->m_nodes[j].m_vn;
             }
-            // Now:
+            m_dv[counter] = psb->m_nodes[j].m_v - m_backupVelocity[counter];
-            // dv = 0 for nodes not in constraints
+            ++counter;
-            // dv = v_{n+1} - v_n for nodes in constraints
-            m_backupVelocity[counter++] = psb->m_nodes[j].m_vn;
         }
     }
 }
@@ -215,9 +309,9 @@ void btDeformableBodySolver::backupVn()
 void btDeformableBodySolver::revertVelocity()
 {
     int counter = 0;
-    for (int i = 0; i < m_softBodySet.size(); ++i)
+    for (int i = 0; i < m_softBodies.size(); ++i)
     {
-        btSoftBody* psb = m_softBodySet[i];
+        btSoftBody* psb = m_softBodies[i];
         for (int j = 0; j < psb->m_nodes.size(); ++j)
         {
             psb->m_nodes[j].m_v = m_backupVelocity[counter++];
@@ -228,8 +322,8 @@ void btDeformableBodySolver::revertVelocity()
 bool btDeformableBodySolver::updateNodes()
 {
     int numNodes = 0;
-    for (int i = 0; i < m_softBodySet.size(); ++i)
+    for (int i = 0; i < m_softBodies.size(); ++i)
-        numNodes += m_softBodySet[i]->m_nodes.size();
+        numNodes += m_softBodies[i]->m_nodes.size();
     if (numNodes != m_numNodes)
     {
         m_numNodes = numNodes;
@@ -241,9 +335,9 @@ bool btDeformableBodySolver::updateNodes()
 
 void btDeformableBodySolver::predictMotion(btScalar solverdt)
 {
-    for (int i = 0; i < m_softBodySet.size(); ++i)
+    for (int i = 0; i < m_softBodies.size(); ++i)
     {
-        btSoftBody *psb = m_softBodySet[i];
+        btSoftBody *psb = m_softBodies[i];
         
         if (psb->isActive())
         {
@@ -259,6 +353,18 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
 {
     int i, ni;
     
+    /* Update                */
+    if (psb->m_bUpdateRtCst)
+    {
+        psb->m_bUpdateRtCst = false;
+        psb->updateConstants();
+        psb->m_fdbvt.clear();
+        if (psb->m_cfg.collisions & btSoftBody::fCollision::SDF_RD)
+        {
+            psb->initializeFaceTree();
+        }
+    }
+    
     /* Prepare                */
     psb->m_sst.sdt = dt * psb->m_cfg.timescale;
     psb->m_sst.isdt = 1 / psb->m_sst.sdt;
@@ -286,21 +392,41 @@ void btDeformableBodySolver::predictDeformableMotion(btSoftBody* psb, btScalar d
                        psb->m_sst.updmrg);
     }
 
+    if (!psb->m_fdbvt.empty())
+    {
+        for (int i = 0; i < psb->m_faces.size(); ++i)
+        {
+            btSoftBody::Face& f = psb->m_faces[i];
+            const btVector3 v = (f.m_n[0]->m_v +
+                                 f.m_n[1]->m_v +
+                                 f.m_n[2]->m_v) /
+            3;
+            vol = VolumeOf(f, psb->m_sst.radmrg);
+            psb->m_fdbvt.update(f.m_leaf,
+                           vol,
+                           v * psb->m_sst.velmrg,
+                           psb->m_sst.updmrg);
+        }
+    }
     /* Clear contacts        */
-    psb->m_rcontacts.resize(0);
+    psb->m_nodeRigidContacts.resize(0);
-    psb->m_scontacts.resize(0);
+    psb->m_faceRigidContacts.resize(0);
+    psb->m_faceNodeContacts.resize(0);
     /* Optimize dbvt's        */
     psb->m_ndbvt.optimizeIncremental(1);
+    psb->m_fdbvt.optimizeIncremental(1);
 }
 
+
 void btDeformableBodySolver::updateSoftBodies()
 {
-    for (int i = 0; i < m_softBodySet.size(); i++)
+    BT_PROFILE("updateSoftBodies");
+    for (int i = 0; i < m_softBodies.size(); i++)
     {
-        btSoftBody *psb = (btSoftBody *)m_softBodySet[i];
+        btSoftBody *psb = (btSoftBody *)m_softBodies[i];
         if (psb->isActive())
         {
-            psb->updateNormals(); // normal is updated here
+            psb->updateNormals();
         }
     }
 }
@@ -310,3 +436,8 @@ void btDeformableBodySolver::setImplicit(bool implicit)
     m_implicit = implicit;
     m_objective->setImplicit(implicit);
 }
+
+void btDeformableBodySolver::setLineSearch(bool lineSearch)
+{
+    m_lineSearch = lineSearch;
+}

src/BulletSoftBody/btDeformableBodySolver.h

@@ -29,24 +29,24 @@ class btDeformableMultiBodyDynamicsWorld;
 
 class btDeformableBodySolver : public btSoftBodySolver
 {
-//    using TVStack = btAlignedObjectArray<btVector3>;
     typedef btAlignedObjectArray<btVector3> TVStack;
 protected:
-    int m_numNodes;
+    int m_numNodes;                 // total number of deformable body nodes
-    TVStack m_dv;
+    TVStack m_dv;                   // v_{n+1} - v_n
-    TVStack m_ddv;
+    TVStack m_backup_dv;            // backed up dv
-    TVStack m_residual;
+    TVStack m_ddv;                  // incremental dv
-    btAlignedObjectArray<btSoftBody *> m_softBodySet;
+    TVStack m_residual;             // rhs of the linear solve
-   
+    btAlignedObjectArray<btSoftBody *> m_softBodies;  // all deformable bodies
-    btAlignedObjectArray<btVector3> m_backupVelocity;
+    TVStack m_backupVelocity;       // backed up v, equals v_n for implicit, equals v_{n+1}^* for explicit
-    btScalar m_dt;
+    btScalar m_dt;                  // dt
-    btScalar m_contact_iterations;
+    btConjugateGradient<btDeformableBackwardEulerObjective> m_cg;  // CG solver
-    btConjugateGradient<btDeformableBackwardEulerObjective> m_cg;
+    bool m_implicit;                // use implicit scheme if true, explicit scheme if false
-    bool m_implicit;
+    int m_maxNewtonIterations;      // max number of newton iterations
-    int m_maxNewtonIterations;
+    btScalar m_newtonTolerance;     // stop newton iterations if f(x) < m_newtonTolerance
-    btScalar m_newtonTolerance;
+    bool m_lineSearch;              // If true, use newton's method with line search under implicit scheme
     
 public:
+    // handles data related to objective function
     btDeformableBackwardEulerObjective* m_objective;
     
     btDeformableBodySolver();
@@ -58,54 +58,101 @@ public:
         return DEFORMABLE_SOLVER;
     }
 
+    // update soft body normals
     virtual void updateSoftBodies();
-
-    virtual void copyBackToSoftBodies(bool bMove = true) {}
     
+    // solve the momentum equation
     virtual void solveDeformableConstraints(btScalar solverdt);
     
+    // solve the contact between deformable and rigid as well as among deformables
     btScalar solveContactConstraints();
-    
+
-    virtual void solveConstraints(btScalar dt){}
+    // resize/clear data structures
-    
     void reinitialize(const btAlignedObjectArray<btSoftBody *>& softBodies, btScalar dt);
     
+    // set up contact constraints
     void setConstraints();
     
+    // add in elastic forces and gravity to obtain v_{n+1}^* and calls predictDeformableMotion
+    virtual void predictMotion(btScalar solverdt);
+    
+    // move to temporary position x_{n+1}^* = x_n + dt * v_{n+1}^*
+    // x_{n+1}^* is stored in m_q
     void predictDeformableMotion(btSoftBody* psb, btScalar dt);
     
+    // save the current velocity to m_backupVelocity
     void backupVelocity();
-    void backupVn();
+    
+    // set m_dv and m_backupVelocity to desired value to prepare for momentum solve
+    void setupDeformableSolve(bool implicit);
+    
+    // set the current velocity to that backed up in m_backupVelocity
     void revertVelocity();
+    
+    // set velocity to m_dv + m_backupVelocity
     void updateVelocity();
     
+    // update the node count
     bool updateNodes();
     
-    void computeStep(TVStack& dv, const TVStack& residual);
+    // calculate the change in dv resulting from the momentum solve
-                     
+    void computeStep(TVStack& ddv, const TVStack& residual);
-    virtual void predictMotion(btScalar solverdt);
+    
+    // calculate the change in dv resulting from the momentum solve when line search is turned on
+    btScalar computeDescentStep(TVStack& ddv, const TVStack& residual, bool verbose=false);
 
     virtual void copySoftBodyToVertexBuffer(const btSoftBody *const softBody, btVertexBufferDescriptor *vertexBuffer) {}
 
+    // process collision between deformable and rigid
     virtual void processCollision(btSoftBody * softBody, const btCollisionObjectWrapper * collisionObjectWrap)
     {
         softBody->defaultCollisionHandler(collisionObjectWrap);
     }
-
+    
+    // process collision between deformable and deformable
     virtual void processCollision(btSoftBody * softBody, btSoftBody * otherSoftBody) {
         softBody->defaultCollisionHandler(otherSoftBody);
     }
-    virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false){}
+
-    
+    // If true, implicit time stepping scheme is used.
-    virtual bool checkInitialized(){return true;}
+    // Otherwise, explicit time stepping scheme is used
-    
     void setImplicit(bool implicit);
     
+    // If true, newton's method with line search is used when implicit time stepping scheme is turned on
+    void setLineSearch(bool lineSearch);
+    
+    // set temporary position x^* = x_n + dt * v
+    // update the deformation gradient at position x^*
     void updateState();
     
-    void updateDv();
+    // set dv = dv + scale * ddv
+    void updateDv(btScalar scale = 1);
     
+    // set temporary position x^* = x_n + dt * v^*
     void updateTempPosition();
+    
+    // save the current dv to m_backup_dv;
+    void backupDv();
+    
+    // set dv to the backed-up value
+    void revertDv();
+    
+    // set dv = dv + scale * ddv
+    // set v^* = v_n + dv
+    // set temporary position x^* = x_n + dt * v^*
+    // update the deformation gradient at position x^*
+    void updateEnergy(btScalar scale);
+    
+    // calculates the appropriately scaled kinetic energy in the system, which is
+    // 1/2 * dv^T * M * dv
+    // used in line search
+    btScalar kineticEnergy();
+    
+    // unused functions
+    virtual void optimize(btAlignedObjectArray<btSoftBody *> &softBodies, bool forceUpdate = false){}
+    virtual void solveConstraints(btScalar dt){}
+    virtual bool checkInitialized(){return true;}
+    virtual void copyBackToSoftBodies(bool bMove = true) {}
 };
 
 #endif /* btDeformableBodySolver_h */

src/BulletSoftBody/btDeformableContactConstraint.cpp

@@ -0,0 +1,378 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ 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 "btDeformableContactConstraint.h"
+
+/* ================   Deformable vs. Rigid   =================== */
+btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c)
+: m_contact(&c)
+, btDeformableContactConstraint(c.m_cti.m_normal)
+{
+    m_total_normal_dv.setZero();
+    m_total_tangent_dv.setZero();
+}
+
+btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other)
+: m_contact(other.m_contact)
+, btDeformableContactConstraint(other)
+{
+    m_total_normal_dv = other.m_total_normal_dv;
+    m_total_tangent_dv = other.m_total_tangent_dv;
+}
+
+
+btVector3 btDeformableRigidContactConstraint::getVa() const
+{
+    const btSoftBody::sCti& cti = m_contact->m_cti;
+    btVector3 va(0, 0, 0);
+    if (cti.m_colObj->hasContactResponse())
+    {
+        btRigidBody* rigidCol = 0;
+        btMultiBodyLinkCollider* multibodyLinkCol = 0;
+        
+        // grab the velocity of the rigid body
+        if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+        {
+            rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+            va = rigidCol ? (rigidCol->getVelocityInLocalPoint(m_contact->m_c1)) : btVector3(0, 0, 0);
+        }
+        else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+        {
+            multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+            if (multibodyLinkCol)
+            {
+                const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+                const btScalar* J_n = &m_contact->jacobianData_normal.m_jacobians[0];
+                const btScalar* J_t1 = &m_contact->jacobianData_t1.m_jacobians[0];
+                const btScalar* J_t2 = &m_contact->jacobianData_t2.m_jacobians[0];
+                const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
+                const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
+                // add in the normal component of the va
+                btScalar vel = 0.0;
+                for (int k = 0; k < ndof; ++k)
+                {
+                    vel += (local_v[k]+local_dv[k]) * J_n[k];
+                }
+                va = cti.m_normal * vel;
+                // add in the tangential components of the va
+                vel = 0.0;
+                for (int k = 0; k < ndof; ++k)
+                {
+                    vel += (local_v[k]+local_dv[k]) * J_t1[k];
+                }
+                va += m_contact->t1 * vel;
+                vel = 0.0;
+                for (int k = 0; k < ndof; ++k)
+                {
+                    vel += (local_v[k]+local_dv[k]) * J_t2[k];
+                }
+                va += m_contact->t2 * vel;
+            }
+        }
+    }
+    return va;
+}
+
+btScalar btDeformableRigidContactConstraint::solveConstraint()
+{
+    const btSoftBody::sCti& cti = m_contact->m_cti;
+    btVector3 va = getVa();
+    btVector3 vb = getVb();
+    btVector3 vr = vb - va;
+    const btScalar dn = btDot(vr, cti.m_normal);
+    // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
+    btScalar residualSquare = dn*dn;
+    btVector3 impulse = m_contact->m_c0 * vr;
+    const btVector3 impulse_normal = m_contact->m_c0 * (cti.m_normal * dn);
+    btVector3 impulse_tangent = impulse - impulse_normal;
+    
+    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;
+    
+    if (m_total_normal_dv.dot(cti.m_normal) < 0)
+    {
+        // separating in the normal direction
+        m_static = false;
+        m_total_tangent_dv = btVector3(0,0,0);
+        impulse_tangent.setZero();
+    }
+    else
+    {
+        if (m_total_normal_dv.norm() * m_contact->m_c3 < m_total_tangent_dv.norm())
+        {
+            // dynamic friction
+            // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations.
+            m_static = false;
+            if (m_total_tangent_dv.norm() < SIMD_EPSILON)
+            {
+                m_total_tangent_dv = btVector3(0,0,0);
+            }
+            else
+            {
+                m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.norm() * m_contact->m_c3;
+            }
+            impulse_tangent = -btScalar(1)/m_contact->m_c2 * (m_total_tangent_dv - old_total_tangent_dv);
+        }
+        else
+        {
+            // static friction
+            m_static = true;
+        }
+    }
+    impulse = impulse_normal + impulse_tangent;
+    // apply impulse to deformable nodes involved and change their velocities
+    applyImpulse(impulse);
+    
+    // apply impulse to the rigid/multibodies involved and change their velocities
+    if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+    {
+        btRigidBody* rigidCol = 0;
+        rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+        if (rigidCol)
+        {
+            rigidCol->applyImpulse(impulse, m_contact->m_c1);
+        }
+    }
+    else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+    {
+        btMultiBodyLinkCollider* multibodyLinkCol = 0;
+        multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+        if (multibodyLinkCol)
+        {
+            const btScalar* deltaV_normal = &m_contact->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+            // apply normal component of the impulse
+            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
+            if (impulse_tangent.norm() > SIMD_EPSILON)
+            {
+                // apply tangential component of the impulse
+                const btScalar* deltaV_t1 = &m_contact->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
+                multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_contact->t1));
+                const btScalar* deltaV_t2 = &m_contact->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
+                multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_contact->t2));
+            }
+        }
+    }
+    return residualSquare;
+}
+
+/* ================   Node vs. Rigid   =================== */
+btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact)
+    : m_node(contact.m_node)
+    , btDeformableRigidContactConstraint(contact)
+    {
+    }
+
+btDeformableNodeRigidContactConstraint::btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other)
+: m_node(other.m_node)
+, btDeformableRigidContactConstraint(other)
+{
+}
+
+btVector3 btDeformableNodeRigidContactConstraint::getVb() const
+{
+    return m_node->m_v;
+}
+
+
+btVector3 btDeformableNodeRigidContactConstraint::getDv(const btSoftBody::Node* node) const
+{
+    return m_total_normal_dv + m_total_tangent_dv;
+}
+
+void btDeformableNodeRigidContactConstraint::applyImpulse(const btVector3& impulse)
+{
+    const btSoftBody::DeformableNodeRigidContact* contact = getContact();
+    btVector3 dv = impulse * contact->m_c2;
+    contact->m_node->m_v -= dv;
+}
+
+/* ================   Face vs. Rigid   =================== */
+btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact)
+: m_face(contact.m_face)
+, btDeformableRigidContactConstraint(contact)
+{
+}
+
+btDeformableFaceRigidContactConstraint::btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other)
+: m_face(other.m_face)
+, btDeformableRigidContactConstraint(other)
+{
+}
+
+btVector3 btDeformableFaceRigidContactConstraint::getVb() const
+{
+    const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+    btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2];
+    return vb;
+}
+
+
+btVector3 btDeformableFaceRigidContactConstraint::getDv(const btSoftBody::Node* node) const
+{
+    btVector3 face_dv = m_total_normal_dv + m_total_tangent_dv;
+    const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+    if (m_face->m_n[0] == node)
+    {
+        return face_dv * contact->m_weights[0];
+    }
+    if (m_face->m_n[1] == node)
+    {
+        return face_dv * contact->m_weights[1];
+    }
+    btAssert(node == m_face->m_n[2]);
+    return face_dv * contact->m_weights[2];
+}
+
+void btDeformableFaceRigidContactConstraint::applyImpulse(const btVector3& impulse)
+{
+    const btSoftBody::DeformableFaceRigidContact* contact = getContact();
+    btVector3 dv = impulse * contact->m_c2;
+    btSoftBody::Face* face = contact->m_face;
+    if (face->m_n[0]->m_im > 0)
+        face->m_n[0]->m_v -= dv * contact->m_weights[0];
+    if (face->m_n[1]->m_im > 0)
+        face->m_n[1]->m_v -= dv * contact->m_weights[1];
+    if (face->m_n[2]->m_im > 0)
+        face->m_n[2]->m_v -= dv * contact->m_weights[2];
+}
+
+/* ================   Face vs. Node   =================== */
+btDeformableFaceNodeContactConstraint::btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact)
+: m_node(contact.m_node)
+, m_face(contact.m_face)
+, m_contact(&contact)
+, btDeformableContactConstraint(contact.m_normal)
+{
+    m_total_normal_dv.setZero();
+    m_total_tangent_dv.setZero();
+}
+
+btVector3 btDeformableFaceNodeContactConstraint::getVa() const
+{
+    return m_node->m_v;
+}
+
+btVector3 btDeformableFaceNodeContactConstraint::getVb() const
+{
+    const btSoftBody::DeformableFaceNodeContact* contact = getContact();
+    btVector3 vb = m_face->m_n[0]->m_v * contact->m_bary[0] + m_face->m_n[1]->m_v * contact->m_bary[1] + m_face->m_n[2]->m_v * contact->m_bary[2];
+    return vb;
+}
+
+btVector3 btDeformableFaceNodeContactConstraint::getDv(const btSoftBody::Node* n) const
+{
+    btVector3 dv = m_total_normal_dv + m_total_tangent_dv;
+    if (n == m_node)
+        return dv;
+    const btSoftBody::DeformableFaceNodeContact* contact = getContact();
+    if (m_face->m_n[0] == n)
+    {
+        return dv * contact->m_weights[0];
+    }
+    if (m_face->m_n[1] == n)
+    {
+        return dv * contact->m_weights[1];
+    }
+    btAssert(n == m_face->m_n[2]);
+    return dv * contact->m_weights[2];
+}
+
+btScalar btDeformableFaceNodeContactConstraint::solveConstraint()
+{
+    btVector3 va = getVa();
+    btVector3 vb = getVb();
+    btVector3 vr = vb - va;
+    const btScalar dn = btDot(vr, m_contact->m_normal);
+    // dn is the normal component of velocity diffrerence. Approximates the residual. // todo xuchenhan@: this prob needs to be scaled by dt
+    btScalar residualSquare = dn*dn;
+    btVector3 impulse = m_contact->m_c0 * vr;
+    const btVector3 impulse_normal = m_contact->m_c0 * (m_contact->m_normal * dn);
+    btVector3 impulse_tangent = impulse - impulse_normal;
+    
+    btVector3 old_total_tangent_dv = m_total_tangent_dv;
+    // m_c2 is the inverse mass of the deformable node/face
+    if (m_node->m_im > 0)
+    {
+        m_total_normal_dv -= impulse_normal * m_node->m_im;
+        m_total_tangent_dv -= impulse_tangent * m_node->m_im;
+    }
+    else
+    {
+        m_total_normal_dv -= impulse_normal * m_contact->m_imf;
+        m_total_tangent_dv -= impulse_tangent *  m_contact->m_imf;
+    }
+    
+    if (m_total_normal_dv.dot(m_contact->m_normal) < 0)
+    {
+        // separating in the normal direction
+        m_static = false;
+        m_total_tangent_dv = btVector3(0,0,0);
+        impulse_tangent.setZero();
+    }
+    else
+    {
+        if (m_total_normal_dv.norm() * m_contact->m_friction < m_total_tangent_dv.norm())
+        {
+            // dynamic friction
+            // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations.
+            m_static = false;
+            if (m_total_tangent_dv.norm() < SIMD_EPSILON)
+            {
+                m_total_tangent_dv = btVector3(0,0,0);
+            }
+            else
+            {
+                m_total_tangent_dv = m_total_tangent_dv.normalized() * m_total_normal_dv.norm() * m_contact->m_friction;
+            }
+            impulse_tangent = -btScalar(1)/m_node->m_im * (m_total_tangent_dv - old_total_tangent_dv);
+        }
+        else
+        {
+            // static friction
+            m_static = true;
+        }
+    }
+    impulse = impulse_normal + impulse_tangent;
+    // apply impulse to deformable nodes involved and change their velocities
+    applyImpulse(impulse);
+    return residualSquare;
+}
+
+void btDeformableFaceNodeContactConstraint::applyImpulse(const btVector3& impulse)
+{
+    const btSoftBody::DeformableFaceNodeContact* contact = getContact();
+    btVector3 dva = impulse * contact->m_node->m_im;
+    btVector3 dvb = impulse * contact->m_imf;
+    if (contact->m_node->m_im > 0)
+    {
+        contact->m_node->m_v += dva;
+    }
+    
+    btSoftBody::Face* face = contact->m_face;
+    if (face->m_n[0]->m_im > 0)
+    {
+        face->m_n[0]->m_v -= dvb * contact->m_weights[0];
+    }
+    if (face->m_n[1]->m_im > 0)
+    {
+        face->m_n[1]->m_v -= dvb * contact->m_weights[1];
+    }
+    if (face->m_n[2]->m_im > 0)
+    {
+        face->m_n[2]->m_v -= dvb * contact->m_weights[2];
+    }
+}

src/BulletSoftBody/btDeformableContactConstraint.h

@@ -0,0 +1,229 @@
+/*
+ Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
+ 
+ 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 BT_DEFORMABLE_CONTACT_CONSTRAINT_H
+#define BT_DEFORMABLE_CONTACT_CONSTRAINT_H
+#include "btSoftBody.h"
+
+// btDeformableContactConstraint is an abstract class specifying the method that each type of contact constraint needs to implement
+class btDeformableContactConstraint
+{
+public:
+    // True if the friction is static
+    // False if the friction is dynamic
+    bool m_static;
+    
+    // normal of the contact
+    btVector3 m_normal;
+    
+    btDeformableContactConstraint(const btVector3& normal): m_static(false), m_normal(normal)
+    {
+    }
+    
+    btDeformableContactConstraint(bool isStatic, const btVector3& normal): m_static(isStatic), m_normal(normal)
+    {
+    }
+    
+    btDeformableContactConstraint(const btDeformableContactConstraint& other)
+    : m_static(other.m_static)
+    , m_normal(other.m_normal)
+    {
+        
+    }
+    btDeformableContactConstraint(){}
+    
+    virtual ~btDeformableContactConstraint(){}
+    
+    // solve the constraint with inelastic impulse and return the error, which is the square of normal component of velocity diffrerence
+    // the constraint is solved by calculating the impulse between object A and B in the contact and apply the impulse to both objects involved in the contact
+    virtual btScalar solveConstraint() = 0;
+    
+    // get the velocity of the object A in the contact
+    virtual btVector3 getVa() const = 0;
+    
+    // get the velocity of the object B in the contact
+    virtual btVector3 getVb() const = 0;
+    
+    // get the velocity change of the soft body node in the constraint
+    virtual btVector3 getDv(const btSoftBody::Node*) const = 0;
+    
+    // apply impulse to the soft body node and/or face involved
+    virtual void applyImpulse(const btVector3& impulse) = 0;
+};
+
+//
+// Constraint that a certain node in the deformable objects cannot move
+class btDeformableStaticConstraint : public btDeformableContactConstraint
+{
+public:
+    const btSoftBody::Node* m_node;
+    
+    btDeformableStaticConstraint(){}
+    
+    btDeformableStaticConstraint(const btSoftBody::Node* node): m_node(node), btDeformableContactConstraint(false, btVector3(0,0,0))
+    {
+    }
+    
+    btDeformableStaticConstraint(const btDeformableStaticConstraint& other)
+    : m_node(other.m_node)
+    , btDeformableContactConstraint(other)
+    {
+        
+    }
+    
+    virtual ~btDeformableStaticConstraint(){}
+    
+    virtual btScalar solveConstraint()
+    {
+        return 0;
+    }
+    
+    virtual btVector3 getVa() const
+    {
+        return btVector3(0,0,0);
+    }
+    
+    virtual btVector3 getVb() const
+    {
+        return btVector3(0,0,0);
+    }
+    
+    virtual btVector3 getDv(const btSoftBody::Node* n) const
+    {
+        return btVector3(0,0,0);
+    }
+    
+    virtual void applyImpulse(const btVector3& impulse){}
+};
+
+//
+// Constraint between rigid/multi body and deformable objects
+class btDeformableRigidContactConstraint : public btDeformableContactConstraint
+{
+public:
+    btVector3 m_total_normal_dv;
+    btVector3 m_total_tangent_dv;
+    const btSoftBody::DeformableRigidContact* m_contact;
+    
+    btDeformableRigidContactConstraint(){}
+    btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c);
+    btDeformableRigidContactConstraint(const btDeformableRigidContactConstraint& other);
+    virtual ~btDeformableRigidContactConstraint()
+    {
+    }
+    
+    // object A is the rigid/multi body, and object B is the deformable node/face
+    virtual btVector3 getVa() const;
+    
+    virtual btScalar solveConstraint();
+};
+
+//
+// Constraint between rigid/multi body and deformable objects nodes
+class btDeformableNodeRigidContactConstraint : public btDeformableRigidContactConstraint
+{
+public:
+    // the deformable node in contact
+    const btSoftBody::Node* m_node;
+    
+    btDeformableNodeRigidContactConstraint(){}
+    btDeformableNodeRigidContactConstraint(const btSoftBody::DeformableNodeRigidContact& contact);
+    btDeformableNodeRigidContactConstraint(const btDeformableNodeRigidContactConstraint& other);
+    
+    virtual ~btDeformableNodeRigidContactConstraint()
+    {
+    }
+    
+    // get the velocity of the deformable node in contact
+    virtual btVector3 getVb() const;
+    
+    // get the velocity change of the input soft body node in the constraint
+    virtual btVector3 getDv(const btSoftBody::Node*) const;
+    
+    // cast the contact to the desired type
+    const btSoftBody::DeformableNodeRigidContact* getContact() const
+    {
+        return static_cast<const btSoftBody::DeformableNodeRigidContact*>(m_contact);
+    }
+    
+    virtual void applyImpulse(const btVector3& impulse);
+};
+
+//
+// Constraint between rigid/multi body and deformable objects faces
+class btDeformableFaceRigidContactConstraint : public btDeformableRigidContactConstraint
+{
+public:
+    const btSoftBody::Face* m_face;
+    btDeformableFaceRigidContactConstraint(){}
+    btDeformableFaceRigidContactConstraint(const btSoftBody::DeformableFaceRigidContact& contact);
+    btDeformableFaceRigidContactConstraint(const btDeformableFaceRigidContactConstraint& other);
+    
+    virtual ~btDeformableFaceRigidContactConstraint()
+    {
+    }
+    
+    // get the velocity of the deformable face at the contact point
+    virtual btVector3 getVb() const;
+    
+    // get the velocity change of the input soft body node in the constraint
+    virtual btVector3 getDv(const btSoftBody::Node*) const;
+    
+    // cast the contact to the desired type
+    const btSoftBody::DeformableFaceRigidContact* getContact() const
+    {
+        return static_cast<const btSoftBody::DeformableFaceRigidContact*>(m_contact);
+    }
+    
+    virtual void applyImpulse(const btVector3& impulse);
+};
+
+//
+// Constraint between  deformable objects faces and deformable objects nodes
+class btDeformableFaceNodeContactConstraint : public btDeformableContactConstraint
+{
+public:
+    btSoftBody::Node* m_node;
+    btSoftBody::Face* m_face;
+    const btSoftBody::DeformableFaceNodeContact* m_contact;
+    btVector3 m_total_normal_dv;
+    btVector3 m_total_tangent_dv;
+    
+    btDeformableFaceNodeContactConstraint(){}
+    
+    btDeformableFaceNodeContactConstraint(const btSoftBody::DeformableFaceNodeContact& contact);
+    
+    virtual ~btDeformableFaceNodeContactConstraint(){}
+    
+    virtual btScalar solveConstraint();
+    
+    // get the velocity of the object A in the contact
+    virtual btVector3 getVa() const;
+    
+    // get the velocity of the object B in the contact
+    virtual btVector3 getVb() const;
+    
+    // get the velocity change of the input soft body node in the constraint
+    virtual btVector3 getDv(const btSoftBody::Node*) const;
+    
+    // cast the contact to the desired type
+    const btSoftBody::DeformableFaceNodeContact* getContact() const
+    {
+        return static_cast<const btSoftBody::DeformableFaceNodeContact*>(m_contact);
+    }
+    
+    virtual void applyImpulse(const btVector3& impulse);
+};
+#endif /* BT_DEFORMABLE_CONTACT_CONSTRAINT_H */

src/BulletSoftBody/btDeformableContactProjection.cpp

@@ -20,148 +20,30 @@
 btScalar btDeformableContactProjection::update()
 {
     btScalar residualSquare = 0;
-    btScalar max_impulse = 0;
+
-    // loop through constraints to set constrained values
+    // node constraints
-    for (int index = 0; index < m_constraints.size(); ++index)
+    for (int index = 0; index < m_nodeRigidConstraints.size(); ++index)
     {
-        DeformableContactConstraint& constraint = *m_constraints.getAtIndex(index);
+        btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraints = *m_nodeRigidConstraints.getAtIndex(index);
-        const btSoftBody::Node* node = constraint.m_node;
+        for (int i = 0; i < constraints.size(); ++i)
-        for (int j = 0; j < constraint.m_contact.size(); ++j)
         {
-            if (constraint.m_contact[j] == NULL)
+            btScalar localResidualSquare = constraints[i].solveConstraint();
-            {
+            residualSquare = btMax(residualSquare, localResidualSquare);
-                // nothing needs to be done for dirichelet constraints
-                continue;
-            }
-            const btSoftBody::RContact* c = constraint.m_contact[j];
-            const btSoftBody::sCti& cti = c->m_cti;
-            
-            if (cti.m_colObj->hasContactResponse())
-            {
-                btVector3 va(0, 0, 0);
-                btRigidBody* rigidCol = 0;
-                btMultiBodyLinkCollider* multibodyLinkCol = 0;
-                const btScalar* deltaV_normal;
-                
-                // grab the velocity of the rigid body
-                if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
-                {
-                    rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
-                    va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)) * m_dt : btVector3(0, 0, 0);
-                }
-                else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
-                {
-                    multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
-                    if (multibodyLinkCol)
-                    {
-                        const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
-                        const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0];
-                        const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0];
-                        const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0];
-                        const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
-                        const btScalar* local_dv = multibodyLinkCol->m_multiBody->getDeltaVelocityVector();
-                        deltaV_normal = &c->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
-                        // add in the normal component of the va
-                        btScalar vel = 0.0;
-                        for (int k = 0; k < ndof; ++k)
-                        {
-                            vel += (local_v[k]+local_dv[k]) * J_n[k];
-                        }
-                        va = cti.m_normal * vel * m_dt;
-                        // add in the tangential components of the va
-                        vel = 0.0;
-                        for (int k = 0; k < ndof; ++k)
-                        {
-                            vel += (local_v[k]+local_dv[k]) * J_t1[k];
-                        }
-                        va += c->t1 * vel * m_dt;
-                        vel = 0.0;
-                        for (int k = 0; k < ndof; ++k)
-                        {
-                            vel += (local_v[k]+local_dv[k]) * J_t2[k];
-                        }
-                        va += c->t2 * vel * m_dt;
-                    }
-                }
-                
-                const btVector3 vb = c->m_node->m_v * m_dt;
-                const btVector3 vr = vb - va;
-                const btScalar dn = btDot(vr, cti.m_normal);
-                btVector3 impulse = c->m_c0 * vr;
-                const btVector3 impulse_normal = c->m_c0 * (cti.m_normal * dn);
-                btVector3 impulse_tangent = impulse - impulse_normal;
-                
-                btVector3 old_total_tangent_dv = constraint.m_total_tangent_dv[j];
-                constraint.m_total_normal_dv[j] -= impulse_normal * node->m_im;
-                constraint.m_total_tangent_dv[j] -= impulse_tangent * node->m_im;
-                
-                if (constraint.m_total_normal_dv[j].dot(cti.m_normal) < 0)
-                {
-                    // separating in the normal direction
-                    constraint.m_static[j] = false;
-                    constraint.m_can_be_dynamic[j] = false;
-                    constraint.m_total_tangent_dv[j] = btVector3(0,0,0);
-                    impulse_tangent.setZero();
-                }
-                else
-                {
-                    if (constraint.m_can_be_dynamic[j] && constraint.m_total_normal_dv[j].norm() * c->m_c3 < constraint.m_total_tangent_dv[j].norm())
-                    {
-                        // dynamic friction
-                        // with dynamic friction, the impulse are still applied to the two objects colliding, however, it does not pose a constraint in the cg solve, hence the change to dv merely serves to update velocity in the contact iterations.
-                        constraint.m_static[j] = false;
-                        constraint.m_can_be_dynamic[j] = true;
-                        if (constraint.m_total_tangent_dv[j].norm() < SIMD_EPSILON)
-                        {
-                            constraint.m_total_tangent_dv[j] = btVector3(0,0,0);
-                        }
-                        else
-                        {
-                            constraint.m_total_tangent_dv[j] = constraint.m_total_tangent_dv[j].normalized() * constraint.m_total_normal_dv[j].norm() * c->m_c3;
-                        }
-                        impulse_tangent = -btScalar(1)/node->m_im * (constraint.m_total_tangent_dv[j] - old_total_tangent_dv);
-                    }
-                    else
-                    {
-                        // static friction
-                        constraint.m_static[j] = true;
-                        constraint.m_can_be_dynamic[j] = false;
-                    }
-                }
-                impulse = impulse_normal + impulse_tangent;
-                max_impulse = btMax(impulse.length2(), max_impulse);
-                
-                // dn is the normal component of velocity diffrerence. Approximates the residual.
-                residualSquare = btMax(residualSquare, dn*dn);
-                if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
-                {
-                    if (rigidCol)
-                    {
-                        rigidCol->applyImpulse(impulse, c->m_c1);
-                    }
-                }
-                else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
-                {
-                    if (multibodyLinkCol)
-                    {
-                        // apply normal component of the impulse
-                        multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
-                        if (impulse_tangent.norm() > SIMD_EPSILON)
-                        {
-                            // apply tangential component of the impulse
-                            const btScalar* deltaV_t1 = &c->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
-                            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(c->t1));
-                            const btScalar* deltaV_t2 = &c->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
-                            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(c->t2));
-                        }
-                    }
-                }
-            }
         }
     }
+    
+    // face constraints
+    for (int index = 0; index < m_allFaceConstraints.size(); ++index)
+    {
+        btDeformableContactConstraint* constraint = m_allFaceConstraints[index];
+        btScalar localResidualSquare = constraint->solveConstraint();
+        residualSquare = btMax(residualSquare, localResidualSquare);
+    }
+    
     return residualSquare;
 }
 
+
 void btDeformableContactProjection::setConstraints()
 {
     BT_PROFILE("setConstraints");
@@ -173,96 +55,146 @@ void btDeformableContactProjection::setConstraints()
         {
             if (psb->m_nodes[j].m_im == 0)
             {
-                m_constraints.insert(psb->m_nodes[j].index, DeformableContactConstraint());
+                btDeformableStaticConstraint static_constraint(&psb->m_nodes[j]);
+                m_staticConstraints.insert(psb->m_nodes[j].index, static_constraint);
             }
         }
     }
+    
+    // set Deformable Node vs. Rigid constraint
     for (int i = 0; i < m_softBodies.size(); ++i)
     {
         btSoftBody* psb = m_softBodies[i];
-        btMultiBodyJacobianData jacobianData_normal;
+        for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j)
-        btMultiBodyJacobianData jacobianData_complementary;
-        for (int j = 0; j < psb->m_rcontacts.size(); ++j)
         {
-            const btSoftBody::RContact& c = psb->m_rcontacts[j];
+            const btSoftBody::DeformableNodeRigidContact& contact = psb->m_nodeRigidContacts[j];
-            // skip anchor points
+            // skip fixed points
-            if (c.m_node->m_im == 0)
+            if (contact.m_node->m_im == 0)
             {
                 continue;
             }
-            
+            btDeformableNodeRigidContactConstraint constraint(contact);
-            const btSoftBody::sCti& cti = c.m_cti;
+            btVector3 va = constraint.getVa();
-            if (cti.m_colObj->hasContactResponse())
+            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)
             {
-                btVector3 va(0, 0, 0);
+                if (m_nodeRigidConstraints.find(contact.m_node->index) == NULL)
-                btRigidBody* rigidCol = 0;
-                btMultiBodyLinkCollider* multibodyLinkCol = 0;
-                
-                // grab the velocity of the rigid body
-                if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
                 {
-                    rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
+                    btAlignedObjectArray<btDeformableNodeRigidContactConstraint> constraintsList;
-                    va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c.m_c1)) * m_dt : btVector3(0, 0, 0);
+                    constraintsList.push_back(constraint);
+                    m_nodeRigidConstraints.insert(contact.m_node->index, constraintsList);
                 }
-                else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+                else
                 {
-                    multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+                    btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints[contact.m_node->index];
-                    if (multibodyLinkCol)
+                    constraintsList.push_back(constraint);
-                    {
-                        btScalar vel = 0.0;
-                        const btScalar* jac = &c.jacobianData_normal.m_jacobians[0];
-                        const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
-                        for (int j = 0; j < ndof; ++j)
-                        {
-                            vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j];
-                        }
-                        va = cti.m_normal * vel * m_dt;
-                    }
                 }
-                
+            }
-                const btVector3 vb = c.m_node->m_v * m_dt;
+        }
-                const btVector3 vr = vb - va;
+        
-                const btScalar dn = btDot(vr, cti.m_normal);
+         // set Deformable Face vs. Rigid constraint
-                if (dn < SIMD_EPSILON)
+        for (int j = 0; j < psb->m_faceRigidContacts.size(); ++j)
+        {
+            const btSoftBody::DeformableFaceRigidContact& contact = psb->m_faceRigidContacts[j];
+            // skip fixed faces
+            if (contact.m_c2 == 0)
+            {
+                continue;
+            }
+            btDeformableFaceRigidContactConstraint* constraint = new btDeformableFaceRigidContactConstraint(contact);
+            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_allFaceConstraints.push_back(constraint);
+                // add face constraints to each of the nodes
+                for (int k = 0; k < 3; ++k)
                 {
-                    
+                    btSoftBody::Node* node = contact.m_face->m_n[k];
-                    if (m_constraints.find(c.m_node->index) == NULL)
+                    // static node does not need to own face/rigid constraint
+                    if (node->m_im != 0)
                     {
-                        m_constraints.insert(c.m_node->index, DeformableContactConstraint(c));
+                        if (m_faceRigidConstraints.find(node->index) == NULL)
-                    }
-                    else
-                    {
-                        DeformableContactConstraint& constraints = *m_constraints[c.m_node->index];
-                        bool single_contact = true;
-                        if (single_contact)
                         {
-                            if (constraints.m_contact[0]->m_cti.m_offset > cti.m_offset)
+                            btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> constraintsList;
-                            {
+                            constraintsList.push_back(constraint);
-                                constraints.replace(c);
+                            m_faceRigidConstraints.insert(node->index, constraintsList);
-                            }
                         }
                         else
                         {
-                            constraints.append(c);
+                            btAlignedObjectArray<btDeformableFaceRigidContactConstraint*>& constraintsList = *m_faceRigidConstraints[node->index];
+                            constraintsList.push_back(constraint);
                         }
                     }
                 }
             }
+            else
+            {
+                delete constraint;
+            }
         }
-    }
+    
-}
+        // set Deformable Face vs. Deformable Node constraint
-
+        for (int j = 0; j < psb->m_faceNodeContacts.size(); ++j)
-void btDeformableContactProjection::enforceConstraint(TVStack& x)
-{
-    for (int index = 0; index < m_constraints.size(); ++index)
-    {
-        const DeformableContactConstraint& constraints = *m_constraints.getAtIndex(index);
-        size_t i = m_constraints.getKeyAtIndex(index).getUid1();
-        x[i].setZero();
-        for (int j = 0; j < constraints.m_total_normal_dv.size(); ++j)
         {
-            x[i] += constraints.m_total_normal_dv[j];
+            const btSoftBody::DeformableFaceNodeContact& contact = psb->m_faceNodeContacts[j];
-            x[i] += constraints.m_total_tangent_dv[j];
+            
+            btDeformableFaceNodeContactConstraint* constraint = new btDeformableFaceNodeContactConstraint(contact);
+            btVector3 va = constraint->getVa();
+            btVector3 vb = constraint->getVb();
+            const btVector3 vr = vb - va;
+            const btScalar dn = btDot(vr, contact.m_normal);
+            if (dn > -SIMD_EPSILON)
+            {
+                btSoftBody::Node* node = contact.m_node;
+                btSoftBody::Face* face = contact.m_face;
+                m_allFaceConstraints.push_back(constraint);
+                if (node->m_im != 0)
+                {
+                    if (m_deformableConstraints.find(node->index) == NULL)
+                    {
+                        btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> constraintsList;
+                        constraintsList.push_back(constraint);
+                        m_deformableConstraints.insert(node->index, constraintsList);
+                    }
+                    else
+                    {
+                        btAlignedObjectArray<btDeformableFaceNodeContactConstraint*>& constraintsList = *m_deformableConstraints[node->index];
+                        constraintsList.push_back(constraint);
+                    }
+                }
+                    
+                // add face constraints to each of the nodes
+                for (int k = 0; k < 3; ++k)
+                {
+                    btSoftBody::Node* node = face->m_n[k];
+                    // static node does not need to own face/rigid constraint
+                    if (node->m_im != 0)
+                    {
+                        if (m_deformableConstraints.find(node->index) == NULL)
+                        {
+                            btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> constraintsList;
+                            constraintsList.push_back(constraint);
+                            m_deformableConstraints.insert(node->index, constraintsList);
+                        }
+                        else
+                        {
+                            btAlignedObjectArray<btDeformableFaceNodeContactConstraint*>& constraintsList = *m_deformableConstraints[node->index];
+                            constraintsList.push_back(constraint);
+                        }
+                    }
+                }
+            }
+            else
+            {
+                delete constraint;
+            }
         }
     }
 }
@@ -270,35 +202,20 @@ void btDeformableContactProjection::enforceConstraint(TVStack& x)
 void btDeformableContactProjection::project(TVStack& x)
 {
     const int dim = 3;
-    for (int index = 0; index < m_constraints.size(); ++index)
+    for (int index = 0; index < m_projectionsDict.size(); ++index)
     {
-        const DeformableContactConstraint& constraints = *m_constraints.getAtIndex(index);
+        btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict.getAtIndex(index);
-        size_t i = m_constraints.getKeyAtIndex(index).getUid1();
+        size_t i = m_projectionsDict.getKeyAtIndex(index).getUid1();
-        if (constraints.m_contact[0] == NULL)
+        if (projectionDirs.size() >= dim)
         {
             // static node
             x[i].setZero();
             continue;
         }
-        bool has_static = false;
+        else if (projectionDirs.size() == 2)
-        for (int j = 0; j < constraints.m_static.size(); ++j)
         {
-            has_static = has_static || constraints.m_static[j];
+            btVector3 dir0 = projectionDirs[0];
-        }
+            btVector3 dir1 = projectionDirs[1];
-        // static friction => fully constrained
-        if (has_static)
-        {
-            x[i].setZero();
-        }
-        else if (constraints.m_total_normal_dv.size() >= dim)
-        {
-            x[i].setZero();
-        }
-        else if (constraints.m_total_normal_dv.size() == 2)
-        {
-            
-            btVector3 dir0 = (constraints.m_total_normal_dv[0].norm() > SIMD_EPSILON) ? constraints.m_total_normal_dv[0].normalized() : btVector3(0,0,0);
-            btVector3 dir1 = (constraints.m_total_normal_dv[1].norm() > SIMD_EPSILON) ? constraints.m_total_normal_dv[1].normalized() : btVector3(0,0,0);
             btVector3 free_dir = btCross(dir0, dir1);
             if (free_dir.norm() < SIMD_EPSILON)
             {
@@ -313,39 +230,206 @@ void btDeformableContactProjection::project(TVStack& x)
         }
         else
         {
-            btAssert(constraints.m_total_normal_dv.size() == 1);
+            btAssert(projectionDirs.size() == 1);
-            btVector3 dir0 = (constraints.m_total_normal_dv[0].norm() > SIMD_EPSILON) ? constraints.m_total_normal_dv[0].normalized() : btVector3(0,0,0);
+            btVector3 dir0 = projectionDirs[0];
             x[i] -= x[i].dot(dir0) * dir0;
         }
     }
 }
 
+void btDeformableContactProjection::setProjection()
+{
+    for (int i = 0; i < m_softBodies.size(); ++i)
+    {
+        btSoftBody* psb = m_softBodies[i];
+        for (int j = 0; j < psb->m_nodes.size(); ++j)
+        {
+            int index = psb->m_nodes[j].index;
+            bool hasConstraint = false;
+            bool existStaticConstraint = false;
+            btVector3 averagedNormal(0,0,0);
+            btAlignedObjectArray<btVector3> normals;
+            if (m_staticConstraints.find(index) != NULL)
+            {
+                existStaticConstraint = true;
+                hasConstraint = true;
+            }
+            
+            // accumulate normals from Deformable Node vs. Rigid constraints
+            if (!existStaticConstraint && m_nodeRigidConstraints.find(index) != NULL)
+            {
+                hasConstraint = true;
+                btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints[index];
+                for (int k = 0; k < constraintsList.size(); ++k)
+                {
+                    if (constraintsList[k].m_static)
+                    {
+                        existStaticConstraint = true;
+                        break;
+                    }
+                    const btVector3& local_normal = constraintsList[k].m_normal;
+                    normals.push_back(local_normal);
+                    averagedNormal += local_normal;
+                }
+            }
+        
+            // accumulate normals from Deformable Face vs. Rigid constraints
+            if (!existStaticConstraint && m_faceRigidConstraints.find(index) != NULL)
+            {
+                hasConstraint = true;
+                btAlignedObjectArray<btDeformableFaceRigidContactConstraint*>& constraintsList = *m_faceRigidConstraints[index];
+                for (int k = 0; k < constraintsList.size(); ++k)
+                {
+                    if (constraintsList[k]->m_static)
+                    {
+                        existStaticConstraint = true;
+                        break;
+                    }
+                    const btVector3& local_normal = constraintsList[k]->m_normal;
+                    normals.push_back(local_normal);
+                    averagedNormal += local_normal;
+                }
+            }
+            
+            // accumulate normals from Deformable Node vs. Deformable Face constraints
+            if (!existStaticConstraint && m_deformableConstraints.find(index) != NULL)
+            {
+                hasConstraint = true;
+                btAlignedObjectArray<btDeformableFaceNodeContactConstraint*>& constraintsList = *m_deformableConstraints[index];
+                for (int k = 0; k < constraintsList.size(); ++k)
+                {
+                    if (constraintsList[k]->m_static)
+                    {
+                        existStaticConstraint = true;
+                        break;
+                    }
+                    const btVector3& local_normal = constraintsList[k]->m_normal;
+                    normals.push_back(local_normal);
+                    averagedNormal += local_normal;
+                }
+            }
+            
+            
+            // build projections
+            if (!hasConstraint)
+            {
+                continue;
+            }
+            btAlignedObjectArray<btVector3> projections;
+            if (existStaticConstraint)
+            {
+                projections.push_back(btVector3(1,0,0));
+                projections.push_back(btVector3(0,1,0));
+                projections.push_back(btVector3(0,0,1));
+            }
+            else
+            {
+                bool averageExists = (averagedNormal.length2() > SIMD_EPSILON);
+                averagedNormal = averageExists ? averagedNormal.normalized() : btVector3(0,0,0);
+                if (averageExists)
+                {
+                    projections.push_back(averagedNormal);
+                }
+                for (int k = 0; k < normals.size(); ++k)
+                {
+                    const btVector3& local_normal = normals[k];
+                    // add another projection direction if it deviates from the average by more than about 15 degrees
+                    if (!averageExists || btAngle(averagedNormal, local_normal) > 0.25)
+                    {
+                        projections.push_back(local_normal);
+                    }
+                }
+            }
+            m_projectionsDict.insert(index, projections);
+        }
+    }
+}
+
+
 void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
 {
-    for (int index = 0; index < m_constraints.size(); ++index)
+    // loop over constraints
+    for (int i = 0; i < f.size(); ++i)
     {
-        const DeformableContactConstraint& constraint = *m_constraints.getAtIndex(index);
+        if (m_projectionsDict.find(i) != NULL)
-        const btSoftBody::Node* node = constraint.m_node;
-        if (node == NULL)
-            continue;
-        size_t i = m_constraints.getKeyAtIndex(index).getUid1();
-        bool has_static_constraint = false;
-        
-        // apply dynamic friction force (scaled by dt) if the node does not have static friction constraint
-        for (int j = 0; j < constraint.m_static.size(); ++j)
         {
-            if (constraint.m_static[j])
+            // doesn't need to add friction force for fully constrained vertices
+            btAlignedObjectArray<btVector3>& projectionDirs = *m_projectionsDict[i];
+            if (projectionDirs.size() >= 3)
             {
-                has_static_constraint = true;
+                continue;
-                break;
             }
         }
-        for (int j = 0; j < constraint.m_total_tangent_dv.size(); ++j)
+        
+        // add friction contribution from Face vs. Node
+        if (m_nodeRigidConstraints.find(i) != NULL)
         {
-            btVector3 friction_force =  constraint.m_total_tangent_dv[j] * (1./node->m_im);
+            btAlignedObjectArray<btDeformableNodeRigidContactConstraint>& constraintsList = *m_nodeRigidConstraints[i];
-            if (!has_static_constraint)
+            for (int j = 0; j < constraintsList.size(); ++j)
             {
-                f[i] += friction_force;
+                const btDeformableNodeRigidContactConstraint& constraint = constraintsList[j];
+                btSoftBody::Node* node = constraint.getContact()->m_node;
+                
+                // it's ok to add the friction force generated by the entire impulse here because the normal component of the residual will be projected out anyway.
+                f[i] += constraint.getDv(node)* (1./node->m_im);
+            }
+        }
+
+        // add friction contribution from Face vs. Rigid
+        if (m_faceRigidConstraints.find(i) != NULL)
+        {
+            btAlignedObjectArray<btDeformableFaceRigidContactConstraint*>& constraintsList = *m_faceRigidConstraints[i];
+            for (int j = 0; j < constraintsList.size(); ++j)
+            {
+                const btDeformableFaceRigidContactConstraint* constraint = constraintsList[j];
+                btSoftBody::Face* face = constraint->getContact()->m_face;
+                
+                // it's ok to add the friction force generated by the entire impulse here because the normal component of the residual will be projected out anyway.
+                for (int k = 0; k < 3; ++k)
+                {
+                    if (face->m_n[k]->index == i)
+                    {
+                        if (face->m_n[k]->m_im != 0)
+                        {
+                            f[i] += constraint->getDv(face->m_n[k])* (1./face->m_n[k]->m_im);
+                        }
+                        break;
+                    }
+                }
+            }
+        }
+        
+        if (m_deformableConstraints.find(i) != NULL)
+        {
+            btAlignedObjectArray<btDeformableFaceNodeContactConstraint*>& constraintsList = *m_deformableConstraints[i];
+            for (int j = 0; j < constraintsList.size(); ++j)
+            {
+                const btDeformableFaceNodeContactConstraint* constraint = constraintsList[j];
+                btSoftBody::Face* face = constraint->getContact()->m_face;
+                btSoftBody::Node* node = constraint->getContact()->m_node;
+                
+                // it's ok to add the friction force generated by the entire impulse here because the normal component of the residual will be projected out anyway.
+                if (node->index == i)
+                {
+                    if (node->m_im != 0)
+                    {
+                        f[i] += constraint->getDv(node)*(1./node->m_im);
+                    }
+                }
+                else
+                {
+                    for (int k = 0; k < 3; ++k)
+                    {
+                        if (face->m_n[k]->index == i)
+                        {
+                            if (face->m_n[k]->m_im != 0)
+                            {
+                                f[i] += constraint->getDv(face->m_n[k])* (1./face->m_n[k]->m_im);
+                            }
+                            break;
+                        }
+                    }
+                }
             }
         }
     }
@@ -353,8 +437,16 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
 
 void btDeformableContactProjection::reinitialize(bool nodeUpdated)
 {
-    btCGProjection::reinitialize(nodeUpdated);
+    m_staticConstraints.clear();
-    m_constraints.clear();
+    m_nodeRigidConstraints.clear();
+    m_faceRigidConstraints.clear();
+    m_deformableConstraints.clear();
+    m_projectionsDict.clear();
+    for (int i = 0; i < m_allFaceConstraints.size(); ++i)
+    {
+        delete m_allFaceConstraints[i];
+    }
+    m_allFaceConstraints.clear();
 }
 
 

src/BulletSoftBody/btDeformableContactProjection.h

@@ -19,15 +19,32 @@
 #include "btSoftBody.h"
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
+#include "btDeformableContactConstraint.h"
 #include "LinearMath/btHashMap.h"
-class btDeformableContactProjection : public btCGProjection
+#include <vector>
+class btDeformableContactProjection
 {
 public:
-    // map from node index to constraints
+    typedef btAlignedObjectArray<btVector3> TVStack;
-    btHashMap<btHashInt, DeformableContactConstraint> m_constraints;
+    btAlignedObjectArray<btSoftBody *>& m_softBodies;
     
-    btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies, const btScalar& dt)
+    // map from node index to static constraint
-    : btCGProjection(softBodies, dt)
+    btHashMap<btHashInt, btDeformableStaticConstraint> m_staticConstraints;
+    // map from node index to node rigid constraint
+    btHashMap<btHashInt, btAlignedObjectArray<btDeformableNodeRigidContactConstraint> > m_nodeRigidConstraints;
+    // map from node index to face rigid constraint
+    btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> > m_faceRigidConstraints;
+    // map from node index to deformable constraint
+    btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> > m_deformableConstraints;
+    
+    // all constraints involving face
+    btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints;
+    
+    // map from node index to projection directions
+    btHashMap<btHashInt, btAlignedObjectArray<btVector3> > m_projectionsDict;
+    
+    btDeformableContactProjection(btAlignedObjectArray<btSoftBody *>& softBodies)
+    : m_softBodies(softBodies)
     {
     }
     
@@ -35,19 +52,21 @@ public:
     {
     }
     
-    // apply the constraints to the rhs
+    // apply the constraints to the rhs of the linear solve
     virtual void project(TVStack& x);
-    // add to friction
-    virtual void applyDynamicFriction(TVStack& f);
     
-    // apply constraints to x in Ax=b
+    // add friction force to the rhs of the linear solve
-    virtual void enforceConstraint(TVStack& x);
+    virtual void applyDynamicFriction(TVStack& f);
     
     // update the constraints
     virtual btScalar update();
     
+    // Add constraints to m_constraints. In addition, the constraints that each vertex own are recorded in m_constraintsDict.
     virtual void setConstraints();
     
+    // Set up projections for each vertex by adding the projection direction to
+    virtual void setProjection();
+    
     virtual void reinitialize(bool nodeUpdated);
 };
 #endif /* btDeformableContactProjection_h */

src/BulletSoftBody/btDeformableCorotatedForce.h

@@ -19,7 +19,7 @@
 #include "btDeformableLagrangianForce.h"
 #include "LinearMath/btPolarDecomposition.h"
 
-static inline int PolarDecompose(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s)
+static inline int PolarDecomposition(const btMatrix3x3& m, btMatrix3x3& q, btMatrix3x3& s)
 {
     static const btPolarDecomposition polar;
     return polar.decompose(m, q, s);
@@ -100,7 +100,7 @@ public:
             if (J < 1024 * SIMD_EPSILON)
                 R.setIdentity();
             else
-                PolarDecompose(F, R, S); // this QR is not robust, consider using implicit shift svd
+                PolarDecomposition(F, R, S); // this QR is not robust, consider using implicit shift svd
             /*https://fuchuyuan.github.io/research/svd/paper.pdf*/
             P += (F-R) * 2 * m_mu;
         }

src/BulletSoftBody/btDeformableGravityForce.h

@@ -72,6 +72,25 @@ public:
     {
         return BT_GRAVITY_FORCE;
     }
+
+    // the gravitational potential energy
+    virtual double totalEnergy(btScalar dt)
+    {
+        double e = 0;
+        for (int i = 0; i<m_softBodies.size();++i)
+        {
+            btSoftBody* psb = m_softBodies[i];
+            for (int j = 0; j < psb->m_nodes.size(); ++j)
+            {
+                const btSoftBody::Node& node = psb->m_nodes[j];
+                if (node.m_im > 0)
+                {
+                    e -= m_gravity.dot(node.m_q)/node.m_im;
+                }
+            }
+        }
+        return e;
+    }
     
     
 };

src/BulletSoftBody/btDeformableLagrangianForce.h

@@ -28,6 +28,11 @@ enum btDeformableLagrangianForceType
     BT_NEOHOOKEAN_FORCE = 4
 };
 
+static inline double randomDouble(double low, double high)
+{
+    return low + static_cast<double>(rand()) / RAND_MAX * (high - low);
+}
+
 class btDeformableLagrangianForce
 {
 public:
@@ -62,6 +67,7 @@ public:
     {
     }
     
+    // get number of nodes that have the force
     virtual int getNumNodes()
     {
         int numNodes = 0;
@@ -72,6 +78,7 @@ public:
         return numNodes;
     }
     
+    // add a soft body to be affected by the particular lagrangian force
     virtual void addSoftBody(btSoftBody* psb)
     {
         m_softBodies.push_back(psb);
@@ -82,28 +89,25 @@ public:
         m_nodes = nodes;
     }
     
+     // Calculate the incremental deformable generated from the input dx
     virtual btMatrix3x3 Ds(int id0, int id1, int id2, int id3, const TVStack& dx)
     {
         btVector3 c1 = dx[id1] - dx[id0];
         btVector3 c2 = dx[id2] - dx[id0];
         btVector3 c3 = dx[id3] - dx[id0];
-        btMatrix3x3 dF(c1.getX(), c2.getX(), c3.getX(),
+        return btMatrix3x3(c1,c2,c3).transpose();
-                       c1.getY(), c2.getY(), c3.getY(),
-                       c1.getZ(), c2.getZ(), c3.getZ());
-        return dF;
     }
     
+    // Calculate the incremental deformable generated from the current velocity
     virtual btMatrix3x3 DsFromVelocity(const btSoftBody::Node* n0, const btSoftBody::Node* n1, const btSoftBody::Node* n2, const btSoftBody::Node* n3)
     {
         btVector3 c1 = n1->m_v - n0->m_v;
         btVector3 c2 = n2->m_v - n0->m_v;
         btVector3 c3 = n3->m_v - n0->m_v;
-        btMatrix3x3 dF(c1.getX(), c2.getX(), c3.getX(),
+        return btMatrix3x3(c1,c2,c3).transpose();
-                       c1.getY(), c2.getY(), c3.getY(),
-                       c1.getZ(), c2.getZ(), c3.getZ());
-        return dF;
     }
     
+    // test for addScaledElasticForce function
     virtual void testDerivative()
     {
         for (int i = 0; i<m_softBodies.size();++i)
@@ -176,7 +180,7 @@ public:
                 psb->updateDeformation();
             }
             counter = 0;
-            double f1 = totalElasticEnergy();
+            double f1 = totalElasticEnergy(0);
             
             for (int i = 0; i<m_softBodies.size();++i)
             {
@@ -190,7 +194,7 @@ public:
             }
             counter = 0;
             
-            double f2 = totalElasticEnergy();
+            double f2 = totalElasticEnergy(0);
             
             //restore m_q
             for (int i = 0; i<m_softBodies.size();++i)
@@ -214,6 +218,7 @@ public:
         }
     }
     
+    // test for addScaledElasticForce function
     virtual void testHessian()
     {
         for (int i = 0; i<m_softBodies.size();++i)
@@ -337,14 +342,22 @@ public:
         }
     }
     
-    virtual double totalElasticEnergy()
+    //
+    virtual double totalElasticEnergy(btScalar dt)
     {
         return 0;
     }
     
-    double randomDouble(double low, double high)
+    //
+    virtual double totalDampingEnergy(btScalar dt)
     {
-        return low + static_cast<double>(rand()) / RAND_MAX * (high - low);
+        return 0;
+    }
+    
+    // total Energy takes dt as input because certain energies depend on dt
+    virtual double totalEnergy(btScalar dt)
+    {
+        return totalElasticEnergy(dt) + totalDampingEnergy(dt);
     }
 };
 #endif /* BT_DEFORMABLE_LAGRANGIAN_FORCE */

src/BulletSoftBody/btDeformableMassSpringForce.h

@@ -20,6 +20,8 @@
 
 class btDeformableMassSpringForce : public btDeformableLagrangianForce
 {
+    // If true, the damping force will be in the direction of the spring
+    // If false, the damping force will be in the direction of the velocity
     bool m_momentum_conserving;
     btScalar m_elasticStiffness, m_dampingStiffness;
 public:
@@ -62,9 +64,9 @@ public:
                 btVector3 scaled_force = scale * m_dampingStiffness * v_diff;
                 if (m_momentum_conserving)
                 {
-                    if ((node2->m_q - node1->m_q).norm() > SIMD_EPSILON)
+                    if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
                     {
-                        btVector3 dir = (node2->m_q - node1->m_q).normalized();
+                        btVector3 dir = (node2->m_x - node1->m_x).normalized();
                         scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir;
                     }
                 }
@@ -118,9 +120,9 @@ public:
                 btVector3 local_scaled_df = scaled_k_damp * (dv[id2] - dv[id1]);
                 if (m_momentum_conserving)
                 {
-                    if ((node2->m_q - node1->m_q).norm() > SIMD_EPSILON)
+                    if ((node2->m_x - node1->m_x).norm() > SIMD_EPSILON)
                     {
-                        btVector3 dir = (node2->m_q - node1->m_q).normalized();
+                        btVector3 dir = (node2->m_x - node1->m_x).normalized();
                         local_scaled_df= scaled_k_damp * (dv[id2] - dv[id1]).dot(dir) * dir;
                     }
                 }
@@ -129,7 +131,8 @@ public:
             }
         }
     }
-    virtual double totalElasticEnergy()
+    
+    virtual double totalElasticEnergy(btScalar dt)
     {
         double energy = 0;
         for (int i = 0; i < m_softBodies.size(); ++i)
@@ -144,7 +147,36 @@ public:
 
                 // elastic force
                 btVector3 dir = (node2->m_q - node1->m_q);
-                energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() -r );
+                energy += 0.5 * m_elasticStiffness * (dir.norm() - r) * (dir.norm() -r);
+            }
+        }
+        return energy;
+    }
+    
+    virtual double totalDampingEnergy(btScalar dt)
+    {
+        double energy = 0;
+        int sz = 0;
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            btSoftBody* psb = m_softBodies[i];
+            for (int j = 0; j < psb->m_nodes.size(); ++j)
+            {
+                sz = btMax(sz, psb->m_nodes[j].index);
+            }
+        }
+        TVStack dampingForce;
+        dampingForce.resize(sz+1);
+        for (int i = 0; i < dampingForce.size(); ++i)
+            dampingForce[i].setZero();
+        addScaledDampingForce(0.5, dampingForce);
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            btSoftBody* psb = m_softBodies[i];
+            for (int j = 0; j < psb->m_nodes.size(); ++j)
+            {
+                const btSoftBody::Node& node = psb->m_nodes[j];
+                energy -= dampingForce[node.index].dot(node.m_v) / dt;
             }
         }
         return energy;

src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.cpp

@@ -23,13 +23,21 @@ 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 = 50;
         int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations ? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations;
         for (int iteration = 0; iteration < maxIterations; iteration++)
         {
-            m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+            // rigid bodies are solved using solver body velocity, but rigid/deformable contact directly uses the velocity of the actual rigid body. So we have to do the following: Solve one iteration of the rigid/rigid contact, get the updated velocity in the solver body and update the velocity of the underlying rigid body. Then solve the rigid/deformable contact. Finally, grab the (once again) updated rigid velocity and update the velocity of the wrapping solver body
             
+            // solve rigid/rigid in solver body
+            m_leastSquaresResidual = solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
+            // solver body velocity -> rigid body velocity
             solverBodyWriteBack(infoGlobal);
+            
+            // update rigid body velocity in rigid/deformable contact
             m_leastSquaresResidual = btMax(m_leastSquaresResidual, m_deformableSolver->solveContactConstraints());
+            
+            // solver body velocity <- rigid body velocity
             writeToSolverBody(bodies, numBodies, infoGlobal);
             
             if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
@@ -51,3 +59,50 @@ btScalar btDeformableMultiBodyConstraintSolver::solveGroupCacheFriendlyIteration
     }
     return 0.f;
 }
+
+void btDeformableMultiBodyConstraintSolver::solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
+{
+    m_tmpMultiBodyConstraints = multiBodyConstraints;
+    m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
+    
+    // inherited from MultiBodyConstraintSolver
+    solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
+    
+    // overriden
+    solveGroupCacheFriendlyIterations(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
+    
+    // inherited from MultiBodyConstraintSolver
+    solveGroupCacheFriendlyFinish(bodies, numBodies, info);
+    
+    m_tmpMultiBodyConstraints = 0;
+    m_tmpNumMultiBodyConstraints = 0;
+}
+
+void btDeformableMultiBodyConstraintSolver::writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
+{
+    for (int i = 0; i < numBodies; i++)
+    {
+        int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
+
+        btRigidBody* body = btRigidBody::upcast(bodies[i]);
+        if (body && body->getInvMass())
+        {
+            btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
+            solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity;
+            solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity;
+        }
+    }
+}
+
+void btDeformableMultiBodyConstraintSolver::solverBodyWriteBack(const btContactSolverInfo& infoGlobal)
+{
+    for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
+    {
+        btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
+        if (body)
+        {
+            m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity);
+            m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity+m_tmpSolverBodyPool[i].m_deltaAngularVelocity);
+        }
+    }
+}

src/BulletSoftBody/btDeformableMultiBodyConstraintSolver.h

@@ -22,6 +22,13 @@
 
 class btDeformableBodySolver;
 
+// btDeformableMultiBodyConstraintSolver extendsn btMultiBodyConstraintSolver to solve for the contact among rigid/multibody and deformable bodies. Notice that the following constraints
+// 1. rigid/multibody against rigid/multibody
+// 2. rigid/multibody against deforamble
+// 3. deformable against deformable
+// 4. deformable self collision
+// 5. joint constraints
+// are all coupled in this solve.
 ATTRIBUTE_ALIGNED16(class)
 btDeformableMultiBodyConstraintSolver : public btMultiBodyConstraintSolver
 {
@@ -31,34 +38,11 @@ protected:
     // override the iterations method to include deformable/multibody contact
     virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
     
-    void solverBodyWriteBack(const btContactSolverInfo& infoGlobal)
+    // write the velocity of the the solver body to the underlying rigid body
-    {
+    void solverBodyWriteBack(const btContactSolverInfo& infoGlobal);
-        for (int i = 0; i < m_tmpSolverBodyPool.size(); i++)
+
-        {
+    // write the velocity of the underlying rigid body to the the the solver body
-            btRigidBody* body = m_tmpSolverBodyPool[i].m_originalBody;
+    void writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
-            if (body)
-            {
-                m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity + m_tmpSolverBodyPool[i].m_deltaLinearVelocity);
-                m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity+m_tmpSolverBodyPool[i].m_deltaAngularVelocity);
-            }
-        }
-    }
-    
-    void writeToSolverBody(btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal)
-    {
-        for (int i = 0; i < numBodies; i++)
-        {
-            int bodyId = getOrInitSolverBody(*bodies[i], infoGlobal.m_timeStep);
-            
-            btRigidBody* body = btRigidBody::upcast(bodies[i]);
-            if (body && body->getInvMass())
-            {
-                btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId];
-                solverBody.m_linearVelocity = body->getLinearVelocity() - solverBody.m_deltaLinearVelocity;
-                solverBody.m_angularVelocity = body->getAngularVelocity() - solverBody.m_deltaAngularVelocity;
-            }
-        }
-    }
     
 public:
     BT_DECLARE_ALIGNED_ALLOCATOR();
@@ -68,23 +52,7 @@ public:
         m_deformableSolver = deformableSolver;
     }
     
-    virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
+    virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
-    {
-        m_tmpMultiBodyConstraints = multiBodyConstraints;
-        m_tmpNumMultiBodyConstraints = numMultiBodyConstraints;
-        
-        // inherited from MultiBodyConstraintSolver
-        solveGroupCacheFriendlySetup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
-        
-        // overriden
-        solveGroupCacheFriendlyIterations(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, info, debugDrawer);
-        
-        // inherited from MultiBodyConstraintSolver
-        solveGroupCacheFriendlyFinish(bodies, numBodies, info);
-        
-        m_tmpMultiBodyConstraints = 0;
-        m_tmpNumMultiBodyConstraints = 0;
-    }
 };
 
 #endif /* BT_DEFORMABLE_MULTIBODY_CONSTRAINT_SOLVER_H */

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp

@@ -37,6 +37,10 @@ The algorithm also closely resembles the one in http://physbam.stanford.edu/~fed
 void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 {
     BT_PROFILE("internalSingleStepSimulation");
+    if (0 != m_internalPreTickCallback)
+    {
+        (*m_internalPreTickCallback)(this, timeStep);
+    }
     reinitialize(timeStep);
     // add gravity to velocity of rigid and multi bodys
     applyRigidBodyGravity(timeStep);
@@ -47,11 +51,16 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t
     ///perform collision detection
     btMultiBodyDynamicsWorld::performDiscreteCollisionDetection();
     
+    if (m_selfCollision)
+    {
+        softBodySelfCollision();
+    }
+    
     btMultiBodyDynamicsWorld::calculateSimulationIslands();
     
     beforeSolverCallbacks(timeStep);
     
-    ///solve deformable bodies constraints
+    ///solve contact constraints and then deformable bodies momemtum equation
     solveConstraints(timeStep);
     
     afterSolverCallbacks(timeStep);
@@ -66,86 +75,21 @@ void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar t
     // ///////////////////////////////
 }
 
-void btDeformableMultiBodyDynamicsWorld::positionCorrection(btScalar timeStep)
+void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision()
 {
-    // perform position correction for all constraints 
+    m_deformableBodySolver->updateSoftBodies();
-    BT_PROFILE("positionCorrection");
+    for (int i = 0; i < m_softBodies.size(); i++)
-    for (int index = 0; index < m_deformableBodySolver->m_objective->projection.m_constraints.size(); ++index)
     {
-        DeformableContactConstraint& constraint = *m_deformableBodySolver->m_objective->projection.m_constraints.getAtIndex(index);
+        btSoftBody* psb = (btSoftBody*)m_softBodies[i];
-        for (int j = 0; j < constraint.m_contact.size(); ++j)
+        psb->defaultCollisionHandler(psb);
-        {
-            const btSoftBody::RContact* c = constraint.m_contact[j];
-            // skip anchor points
-            if (c == NULL || c->m_node->m_im == 0)
-                continue;
-            const btSoftBody::sCti& cti = c->m_cti;
-            btVector3 va(0, 0, 0);
-            
-            // grab the velocity of the rigid body
-            if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
-            {
-                btRigidBody* rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj);
-                va = rigidCol ? (rigidCol->getVelocityInLocalPoint(c->m_c1)): btVector3(0, 0, 0);
-            }
-            else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
-            {
-                btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
-                if (multibodyLinkCol)
-                {
-                    const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
-                    const btScalar* J_n = &c->jacobianData_normal.m_jacobians[0];
-                    const btScalar* J_t1 = &c->jacobianData_t1.m_jacobians[0];
-                    const btScalar* J_t2 = &c->jacobianData_t2.m_jacobians[0];
-                    const btScalar* local_v = multibodyLinkCol->m_multiBody->getVelocityVector();
-                    // add in the normal component of the va
-                    btScalar vel = 0.0;
-                    for (int k = 0; k < ndof; ++k)
-                    {
-                        vel += local_v[k] * J_n[k];
-                    }
-                    va = cti.m_normal * vel;
-                    
-                    vel = 0.0;
-                    for (int k = 0; k < ndof; ++k)
-                    {
-                        vel += local_v[k] * J_t1[k];
-                    }
-                    va += c->t1 * vel;
-                    vel = 0.0;
-                    for (int k = 0; k < ndof; ++k)
-                    {
-                        vel += local_v[k] * J_t2[k];
-                    }
-                    va += c->t2 * vel;
-                }
-            }
-            else
-            {
-                // The object interacting with deformable node is not supported for position correction
-                btAssert(false);
-            }
-            
-            if (cti.m_colObj->hasContactResponse())
-            {
-                btScalar dp = cti.m_offset;
-                
-                // only perform position correction when penetrating
-                if (dp < 0)
-                {
-                    c->m_node->m_v -= dp * cti.m_normal / timeStep;
-                }
-            }
-        }
     }
 }
 
-
 void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 {
     BT_PROFILE("integrateTransforms");
-    m_deformableBodySolver->backupVelocity();
+    //m_deformableBodySolver->backupVelocity();
-    positionCorrection(timeStep);
+    //positionCorrection(timeStep);  // looks like position correction is no longer necessary
     btMultiBodyDynamicsWorld::integrateTransforms(timeStep);
     for (int i = 0; i < m_softBodies.size(); ++i)
     {
@@ -170,28 +114,35 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
             node.m_q = node.m_x;
             node.m_vn = node.m_v;
         }
+        psb->interpolateRenderMesh();
     }
-    m_deformableBodySolver->revertVelocity();
+    //m_deformableBodySolver->revertVelocity();
 }
 
 void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
 {
-    // save v_{n+1}^* velocity after explicit forces
+    if (!m_implicit)
-    m_deformableBodySolver->backupVelocity();
+    {
+        // save v_{n+1}^* velocity after explicit forces
+        m_deformableBodySolver->backupVelocity();
+    }
     
     // set up constraints among multibodies and between multibodies and deformable bodies
     setupConstraints();
-    solveMultiBodyRelatedConstraints();
     
-    if (m_implicit)
+    // solve contact constraints
-    {
+    solveContactConstraints();
-        // at this point dv = v_{n+1} - v_{n+1}^*
+    
-        // modify dv such that dv = v_{n+1} - v_n
+    // set up the directions in which the velocity does not change in the momentum solve
-        // modify m_backupVelocity so that it stores v_n instead of v_{n+1}^* as needed by Newton
+    m_deformableBodySolver->m_objective->m_projection.setProjection();
-        m_deformableBodySolver->backupVn();
+    
-    }
+    // for explicit scheme, m_backupVelocity = v_{n+1}^*
+    // for implicit scheme, m_backupVelocity = v_n
+    // Here, set dv = v_{n+1} - v_n for nodes in contact
+    m_deformableBodySolver->setupDeformableSolve(m_implicit);
     
     // At this point, dv should be golden for nodes in contact
+    // proceed to solve deformable momentum equation
     m_deformableBodySolver->solveDeformableConstraints(timeStep);
 }
 
@@ -207,7 +158,6 @@ void btDeformableMultiBodyDynamicsWorld::setupConstraints()
         btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
         btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
         m_solverMultiBodyIslandCallback->setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
-        m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
         
         // build islands
         m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld());
@@ -233,7 +183,7 @@ void btDeformableMultiBodyDynamicsWorld::sortConstraints()
 }
     
     
-void btDeformableMultiBodyDynamicsWorld::solveMultiBodyRelatedConstraints()
+void btDeformableMultiBodyDynamicsWorld::solveContactConstraints()
 {
     // process constraints on each island
     m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverMultiBodyIslandCallback);
@@ -315,12 +265,8 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
 {
     m_internalTime += timeStep;
     m_deformableBodySolver->setImplicit(m_implicit);
+    m_deformableBodySolver->setLineSearch(m_lineSearch);
     m_deformableBodySolver->reinitialize(m_softBodies, timeStep);
-//    if (m_implicit)
-//    {
-//        // todo: backup v_n velocity somewhere else
-//        m_deformableBodySolver->backupVelocity();
-//    }
     btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo();
     dispatchInfo.m_timeStep = timeStep;
     dispatchInfo.m_stepCount = 0;

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
+#ifndef BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H
-#define BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H
+#define BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H
 
 #include "btSoftMultiBodyDynamicsWorld.h"
 #include "btDeformableLagrangianForce.h"
@@ -36,7 +36,6 @@ typedef btAlignedObjectArray<btSoftBody*> btSoftBodyArray;
 class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld
 {
     typedef btAlignedObjectArray<btVector3> TVStack;
-//    using TVStack = btAlignedObjectArray<btVector3>;
     ///Solver classes that encapsulate multiple deformable bodies for solving
     btDeformableBodySolver* m_deformableBodySolver;
     btSoftBodyArray m_softBodies;
@@ -48,6 +47,8 @@ class btDeformableMultiBodyDynamicsWorld : public btMultiBodyDynamicsWorld
     btScalar m_internalTime;
     int m_contact_iterations;
     bool m_implicit;
+    bool m_lineSearch;
+    bool m_selfCollision;
     
     typedef void (*btSolverCallback)(btScalar time, btDeformableMultiBodyDynamicsWorld* world);
     btSolverCallback m_solverCallback;
@@ -73,7 +74,7 @@ public:
         m_sbi.m_broadphase = pairCache;
         m_sbi.m_dispatcher = dispatcher;
         m_sbi.m_sparsesdf.Initialize();
-        m_sbi.m_sparsesdf.setDefaultVoxelsz(0.025);
+        m_sbi.m_sparsesdf.setDefaultVoxelsz(0.005);
         m_sbi.m_sparsesdf.Reset();
         
         m_sbi.air_density = (btScalar)1.2;
@@ -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)
@@ -152,14 +154,21 @@ public:
     
     void solveMultiBodyConstraints();
     
-    void solveMultiBodyRelatedConstraints();
+    void solveContactConstraints();
     
     void sortConstraints();
     
+    void softBodySelfCollision();
+    
     void setImplicit(bool implicit)
     {
         m_implicit = implicit;
     }
+    
+    void setLineSearch(bool lineSearch)
+    {
+        m_lineSearch = lineSearch;
+    }
 };
 
-#endif  //BT_DEFORMABLE_RIGID_DYNAMICS_WORLD_H
+#endif  //BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD_H

src/BulletSoftBody/btDeformableNeoHookeanForce.h

@@ -17,7 +17,8 @@ subject to the following restrictions:
 #define BT_NEOHOOKEAN_H
 
 #include "btDeformableLagrangianForce.h"
-
+#include "LinearMath/btQuickprof.h"
+// This energy is as described in https://graphics.pixar.com/library/StableElasticity/paper.pdf
 class btDeformableNeoHookeanForce : public btDeformableLagrangianForce
 {
 public:
@@ -26,12 +27,12 @@ public:
     btScalar m_mu_damp, m_lambda_damp;
     btDeformableNeoHookeanForce(): m_mu(1), m_lambda(1)
     {
-        btScalar damping = 0.005;
+        btScalar damping = 0.05;
         m_mu_damp = damping * m_mu;
         m_lambda_damp = damping * m_lambda;
     }
     
-    btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0.005): m_mu(mu), m_lambda(lambda)
+    btDeformableNeoHookeanForce(btScalar mu, btScalar lambda, btScalar damping = 0): m_mu(mu), m_lambda(lambda)
     {
         m_mu_damp = damping * m_mu;
         m_lambda_damp = damping * m_lambda;
@@ -48,8 +49,11 @@ public:
         addScaledElasticForce(scale, force);
     }
     
+    // 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)
+            return;
         int numNodes = getNumNodes();
         btAssert(numNodes <= force.size());
         btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
@@ -69,10 +73,10 @@ public:
                 size_t id3 = node3->index;
                 btMatrix3x3 dF = DsFromVelocity(node0, node1, node2, node3) * tetra.m_Dm_inverse;
                 btMatrix3x3 dP;
-                firstPiolaDampingDifferential(tetra.m_F, dF, dP);
+                firstPiolaDampingDifferential(psb->m_tetraScratchesTn[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();
-                
+
                 // damping force differential
                 btScalar scale1 = scale * tetra.m_element_measure;
                 force[id0] -= scale1 * df_on_node0;
@@ -83,31 +87,58 @@ public:
         }
     }
     
-    virtual double totalElasticEnergy()
+    virtual double totalElasticEnergy(btScalar dt)
     {
         double energy = 0;
         for (int i = 0; i < m_softBodies.size(); ++i)
         {
             btSoftBody* psb = m_softBodies[i];
-            for (int j = 0; j < psb->m_tetras.size(); ++j)
+            for (int j = 0; j < psb->m_tetraScratches.size(); ++j)
             {
                 btSoftBody::Tetra& tetra = psb->m_tetras[j];
-                energy += tetra.m_element_measure * elasticEnergyDensity(tetra);
+                btSoftBody::TetraScratch& s = psb->m_tetraScratches[j];
+                energy += tetra.m_element_measure * elasticEnergyDensity(s);
             }
         }
         return energy;
     }
     
-    double elasticEnergyDensity(const btSoftBody::Tetra& t)
+    // The damping energy is formulated as in https://www.math.ucla.edu/~jteran/papers/GSSJT15.pdf to allow line search
+    virtual double totalDampingEnergy(btScalar dt)
+    {
+        double energy = 0;
+        int sz = 0;
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            btSoftBody* psb = m_softBodies[i];
+            for (int j = 0; j < psb->m_nodes.size(); ++j)
+            {
+                sz = btMax(sz, psb->m_nodes[j].index);
+            }
+        }
+        TVStack dampingForce;
+        dampingForce.resize(sz+1);
+        for (int i = 0; i < dampingForce.size(); ++i)
+            dampingForce[i].setZero();
+        addScaledDampingForce(0.5, dampingForce);
+        for (int i = 0; i < m_softBodies.size(); ++i)
+        {
+            btSoftBody* psb = m_softBodies[i];
+            for (int j = 0; j < psb->m_nodes.size(); ++j)
+            {
+                const btSoftBody::Node& node = psb->m_nodes[j];
+                energy -= dampingForce[node.index].dot(node.m_v) / dt;
+            }
+        }
+        return energy;
+    }
+    
+    double elasticEnergyDensity(const btSoftBody::TetraScratch& s)
     {
         double density = 0;
-        btMatrix3x3 F = t.m_F;
+        density += m_mu * 0.5 * (s.m_trace - 3.);
-        btMatrix3x3 C = F.transpose()*F;
+        density += m_lambda * 0.5 * (s.m_J - 1. - 0.75 * m_mu / m_lambda)* (s.m_J - 1. - 0.75 * m_mu / m_lambda);
-        double J = F.determinant();
+        density -= m_mu * 0.5 * log(s.m_trace+1);
-        double trace = C[0].getX() + C[1].getY() + C[2].getZ();
-        density += m_mu * 0.5 * (trace - 3.);
-        density += m_lambda * 0.5 * (J - 1. - 0.75 * m_mu / m_lambda)* (J - 1. - 0.75 * m_mu / m_lambda);
-        density -= m_mu * 0.5 * log(trace+1);
         return density;
     }
     
@@ -123,9 +154,10 @@ public:
             {
                 btSoftBody::Tetra& tetra = psb->m_tetras[j];
                 btMatrix3x3 P;
-                firstPiola(tetra.m_F,P);
+                firstPiola(psb->m_tetraScratches[j],P);
-                btVector3 force_on_node0 = P * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
+//                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();
+                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];
@@ -146,8 +178,11 @@ 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 addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
     {
+        if (m_mu_damp == 0 && m_lambda_damp == 0)
+            return;
         int numNodes = getNumNodes();
         btAssert(numNodes <= df.size());
         btVector3 grad_N_hat_1st_col = btVector3(-1,-1,-1);
@@ -167,10 +202,11 @@ public:
                 size_t id3 = node3->index;
                 btMatrix3x3 dF = Ds(id0, id1, id2, id3, dv) * tetra.m_Dm_inverse;
                 btMatrix3x3 dP;
-                firstPiolaDampingDifferential(tetra.m_F, dF, dP);
+                firstPiolaDampingDifferential(psb->m_tetraScratchesTn[j], dF, dP);
-                btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
+//                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();
-                
+                btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
+
                 // damping force differential
                 btScalar scale1 = scale * tetra.m_element_measure;
                 df[id0] -= scale1 * df_on_node0;
@@ -202,9 +238,10 @@ public:
                 size_t id3 = node3->index;
                 btMatrix3x3 dF = Ds(id0, id1, id2, id3, dx) * tetra.m_Dm_inverse;
                 btMatrix3x3 dP;
-                firstPiolaDifferential(tetra.m_F, dF, dP);
+                firstPiolaDifferential(psb->m_tetraScratches[j], dF, dP);
-                btVector3 df_on_node0 = dP * (tetra.m_Dm_inverse.transpose()*grad_N_hat_1st_col);
+//                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();
+                btVector3 df_on_node0 = df_on_node123 * grad_N_hat_1st_col;
                 
                 // elastic force differential
                 btScalar scale1 = scale * tetra.m_element_measure;
@@ -216,34 +253,35 @@ public:
         }
     }
     
-    void firstPiola(const btMatrix3x3& F, btMatrix3x3& P)
+    void firstPiola(const btSoftBody::TetraScratch& s, btMatrix3x3& P)
     {
-        btMatrix3x3 C = F.transpose()*F;
+        btScalar c1 = (m_mu * ( 1. - 1. / (s.m_trace + 1.)));
-        btScalar J = F.determinant();
+        btScalar c2 = (m_lambda * (s.m_J - 1.) - 0.75 * m_mu);
-        btScalar trace = C[0].getX() + C[1].getY() + C[2].getZ();
+        P = s.m_F * c1 + s.m_cofF * c2;
-        P = F * m_mu * ( 1. - 1. / (trace + 1.)) + F.adjoint().transpose() * (m_lambda * (J - 1.) - 0.75 * m_mu);
     }
     
-    void firstPiolaDifferential(const btMatrix3x3& F, const btMatrix3x3& dF,  btMatrix3x3& dP)
+    // Let P be the first piola stress.
+    // This function calculates the dP = dP/dF * dF
+    void firstPiolaDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF,  btMatrix3x3& dP)
     {
-        btScalar J = F.determinant();
+        btScalar c1 = m_mu * ( 1. - 1. / (s.m_trace + 1.));
-        btMatrix3x3 C = F.transpose()*F;
+        btScalar c2 = (2.*m_mu) * DotProduct(s.m_F, dF) * (1./((1.+s.m_trace)*(1.+s.m_trace)));
-        btScalar trace = C[0].getX() + C[1].getY() + C[2].getZ();
+        btScalar c3 = (m_lambda * DotProduct(s.m_cofF, dF));
-        dP = dF * m_mu * ( 1. - 1. / (trace + 1.)) + F * (2*m_mu) * DotProduct(F, dF) * (1./((1.+trace)*(1.+trace)));
+        dP = dF * c1 + s.m_F * c2;
-        
+        addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda*(s.m_J-1.) - 0.75*m_mu, dP);
-        addScaledCofactorMatrixDifferential(F, dF, m_lambda*(J-1.) - 0.75*m_mu, dP);
+        dP += s.m_cofF * c3;
-        dP += F.adjoint().transpose() * m_lambda * DotProduct(F.adjoint().transpose(), dF);
     }
     
-    void firstPiolaDampingDifferential(const btMatrix3x3& F, const btMatrix3x3& dF,  btMatrix3x3& dP)
+    // Let Q be the damping stress.
+    // This function calculates the dP = dQ/dF * dF
+    void firstPiolaDampingDifferential(const btSoftBody::TetraScratch& s, const btMatrix3x3& dF,  btMatrix3x3& dP)
     {
-        btScalar J = F.determinant();
+        btScalar c1 = (m_mu_damp * ( 1. - 1. / (s.m_trace + 1.)));
-        btMatrix3x3 C = F.transpose()*F;
+        btScalar c2 = ((2.*m_mu_damp) * DotProduct(s.m_F, dF) *(1./((1.+s.m_trace)*(1.+s.m_trace))));
-        btScalar trace = C[0].getX() + C[1].getY() + C[2].getZ();
+        btScalar c3 = (m_lambda_damp * DotProduct(s.m_cofF, dF));
-        dP = dF * m_mu_damp * ( 1. - 1. / (trace + 1.)) + F * (2*m_mu_damp) * DotProduct(F, dF) * (1./((1.+trace)*(1.+trace)));
+        dP = dF * c1 + s.m_F * c2;
-        
+        addScaledCofactorMatrixDifferential(s.m_F, dF, m_lambda_damp*(s.m_J-1.) - 0.75*m_mu_damp, dP);
-        addScaledCofactorMatrixDifferential(F, dF, m_lambda_damp*(J-1.) - 0.75*m_mu_damp, dP);
+        dP += s.m_cofF * c3;
-        dP += F.adjoint().transpose() * m_lambda_damp * DotProduct(F.adjoint().transpose(), dF);
     }
     
     btScalar DotProduct(const btMatrix3x3& A, const btMatrix3x3& B)
@@ -256,6 +294,9 @@ public:
         return ans;
     }
     
+    // Let C(A) be the cofactor of the matrix A
+    // Let H = the derivative of C(A) with respect to A evaluated at F = A
+    // This function calculates H*dF
     void addScaledCofactorMatrixDifferential(const btMatrix3x3& F, const btMatrix3x3& dF, btScalar scale, btMatrix3x3& M)
     {
         M[0][0] += scale * (dF[1][1] * F[2][2] + F[1][1] * dF[2][2] - dF[2][1] * F[1][2] - F[2][1] * dF[1][2]);

src/BulletSoftBody/btSoftBody.cpp

@@ -18,8 +18,12 @@ subject to the following restrictions:
 #include "BulletSoftBody/btSoftBodySolvers.h"
 #include "btSoftBodyData.h"
 #include "LinearMath/btSerializer.h"
+#include "LinearMath/btAlignedAllocator.h"
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
+#include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
+#include "BulletCollision/CollisionShapes/btTriangleShape.h"
+#include <iostream>
 //
 btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo, int node_count, const btVector3* x, const btScalar* m)
 	: m_softBodySolver(0), m_worldInfo(worldInfo)
@@ -86,6 +90,7 @@ void btSoftBody::initDefaults()
 	m_cfg.diterations = 0;
 	m_cfg.citerations = 4;
 	m_cfg.collisions = fCollision::Default;
+    m_cfg.collisions |= fCollision::VF_DD;
 	m_pose.m_bvolume = false;
 	m_pose.m_bframe = false;
 	m_pose.m_volume = 0;
@@ -2289,6 +2294,7 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap,
     }
     return (false);
 }
+
 //
 bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWrap,
 							  const btVector3& x,
@@ -2303,6 +2309,7 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr
     btTransform wtr = (predict) ?
     (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
                  : colObjWrap->getWorldTransform();
+    //const btTransform& wtr = colObjWrap->getWorldTransform();
 	btScalar dst =
 		m_worldInfo->m_sparsesdf.Evaluate(
 			wtr.invXform(x),
@@ -2320,6 +2327,63 @@ bool btSoftBody::checkDeformableContact(const btCollisionObjectWrapper* colObjWr
 	return (false);
 }
 
+//
+// Compute barycentric coordinates (u, v, w) for
+// point p with respect to triangle (a, b, c)
+static void getBarycentric(const btVector3& p, btVector3& a, btVector3& b, btVector3& c, btVector3& bary)
+{
+    btVector3 v0 = b - a, v1 = c - a, v2 = p - a;
+    btScalar d00 = v0.dot(v0);
+    btScalar d01 = v0.dot(v1);
+    btScalar d11 = v1.dot(v1);
+    btScalar d20 = v2.dot(v0);
+    btScalar d21 = v2.dot(v1);
+    btScalar denom = d00 * d11 - d01 * d01;
+    bary.setY((d11 * d20 - d01 * d21) / denom);
+    bary.setZ((d00 * d21 - d01 * d20) / denom);
+    bary.setX(btScalar(1) - bary.getY() - bary.getZ());
+}
+
+//
+bool btSoftBody::checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap,
+                                        const Face& f,
+                                        btVector3& contact_point,
+                                        btVector3& bary,
+                                        btScalar margin,
+                                        btSoftBody::sCti& cti, bool predict) const
+{
+    btVector3 nrm;
+    const btCollisionShape* shp = colObjWrap->getCollisionShape();
+    const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
+    // use the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg for collision detect
+    // but resolve contact at x_n
+    btTransform wtr = (predict) ?
+    (colObjWrap->m_preTransform != NULL ? tmpCollisionObj->getInterpolationWorldTransform()*(*colObjWrap->m_preTransform) : tmpCollisionObj->getInterpolationWorldTransform())
+    : colObjWrap->getWorldTransform();
+    
+    btGjkEpaSolver2::sResults results;
+    btTransform triangle_transform;
+    triangle_transform.setIdentity();
+    triangle_transform.setOrigin(f.m_n[0]->m_x);
+    btTriangleShape triangle(btVector3(0,0,0), f.m_n[1]->m_x-f.m_n[0]->m_x, f.m_n[2]->m_x-f.m_n[0]->m_x);
+    btVector3 guess(0,0,0);
+    const btConvexShape* csh = static_cast<const btConvexShape*>(shp);
+    btGjkEpaSolver2::SignedDistance(&triangle, triangle_transform, csh, wtr, guess, results);
+    btScalar dst = results.distance - margin;
+    contact_point = results.witnesses[0];
+    getBarycentric(contact_point, f.m_n[0]->m_x, f.m_n[1]->m_x, f.m_n[2]->m_x, bary);
+    if (!predict)
+    {
+        cti.m_colObj = colObjWrap->getCollisionObject();
+//        cti.m_normal = wtr.getBasis() * results.normal;
+        cti.m_normal = results.normal;
+        cti.m_offset = dst;
+    }
+    if (dst < 0)
+        return true;
+    return (false);
+}
+
 //
 void btSoftBody::updateNormals()
 {
@@ -2842,6 +2906,22 @@ void btSoftBody::updateDeformation()
                        c1.getY(), c2.getY(), c3.getY(),
                        c1.getZ(), c2.getZ(), c3.getZ());
         t.m_F = Ds * t.m_Dm_inverse;
+        
+        btSoftBody::TetraScratch& s = m_tetraScratches[i];
+        s.m_F = t.m_F;
+        s.m_J = t.m_F.determinant();
+        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();
+    }
+}
+
+void btSoftBody::advanceDeformation()
+{
+    updateDeformation();
+    for (int i = 0; i < m_tetras.size(); ++i)
+    {
+        m_tetraScratchesTn[i] = m_tetraScratches[i];
     }
 }
 //
@@ -3082,6 +3162,20 @@ void btSoftBody::applyForces()
 	}
 }
 
+//
+void btSoftBody::interpolateRenderMesh()
+{
+    for (int i = 0; i < m_renderNodes.size(); ++i)
+    {
+        Node& n = m_renderNodes[i];
+        n.m_x.setZero();
+        for (int j = 0; j < 4; ++j)
+        {
+            n.m_x += m_renderNodesParents[i][j]->m_x * m_renderNodesInterpolationWeights[i][j];
+        }
+    }
+}
+
 //
 void btSoftBody::PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti)
 {
@@ -3324,7 +3418,7 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
 		break;
         case fCollision::SDF_RD:
         {
-            btSoftColliders::CollideSDF_RD docollide;
+            
             btRigidBody* prb1 = (btRigidBody*)btRigidBody::upcast(pcoWrap->getCollisionObject());
             btTransform wtr = pcoWrap->getWorldTransform();
             
@@ -3340,21 +3434,75 @@ void btSoftBody::defaultCollisionHandler(const btCollisionObjectWrapper* pcoWrap
                                                   maxs);
             volume = btDbvtVolume::FromMM(mins, maxs);
             volume.Expand(btVector3(basemargin, basemargin, basemargin));
-            docollide.psb = this;
+            btSoftColliders::CollideSDF_RD docollideNode;
-            docollide.m_colObj1Wrap = pcoWrap;
+            docollideNode.psb = this;
-            docollide.m_rigidBody = prb1;
+            docollideNode.m_colObj1Wrap = pcoWrap;
+            docollideNode.m_rigidBody = prb1;
+            docollideNode.dynmargin = basemargin + timemargin;
+            docollideNode.stamargin = basemargin;
+            m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollideNode);
             
-            docollide.dynmargin = basemargin + timemargin;
+            btSoftColliders::CollideSDF_RDF docollideFace;
-            docollide.stamargin = basemargin;
+            docollideFace.psb = this;
-            m_ndbvt.collideTV(m_ndbvt.m_root, volume, docollide);
+            docollideFace.m_colObj1Wrap = pcoWrap;
+            docollideFace.m_rigidBody = prb1;
+            docollideFace.dynmargin = basemargin + timemargin;
+            docollideFace.stamargin = basemargin;
+            m_fdbvt.collideTV(m_fdbvt.m_root, volume, docollideFace);
         }
         break;
 	}
 }
 
+static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n)
+{
+    if (n == 0)
+        return 0;
+    btDbvntNode* root = new btDbvntNode(n);
+    if (n->isinternal())
+    {
+        btDbvntNode* c0 = copyToDbvnt(n->childs[0]);
+        root->childs[0] = c0;
+        btDbvntNode* c1 = copyToDbvnt(n->childs[1]);
+        root->childs[1] = c1;
+    }
+    return root;
+}
+
+static inline void calculateNormalCone(btDbvntNode* root)
+{
+    if (!root)
+        return;
+    if (root->isleaf())
+    {
+        const btSoftBody::Face* face = (btSoftBody::Face*)root->data;
+        root->normal = face->m_normal;
+        root->angle = 0;
+    }
+    else
+    {
+        btVector3 n0(0,0,0), n1(0,0,0);
+        btScalar a0 = 0, a1 = 0;
+        if (root->childs[0])
+        {
+            calculateNormalCone(root->childs[0]);
+            n0 = root->childs[0]->normal;
+            a0 = root->childs[0]->angle;
+        }
+        if (root->childs[1])
+        {
+            calculateNormalCone(root->childs[1]);
+            n1 = root->childs[1]->normal;
+            a1 = root->childs[1]->angle;
+        }
+        root->normal = (n0+n1).safeNormalize();
+        root->angle = btMax(a0,a1) + btAngle(n0, n1)*0.5;
+    }
+}
 //
 void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
 {
+    BT_PROFILE("Deformable Collision");
 	const int cf = m_cfg.collisions & psb->m_cfg.collisions;
 	switch (cf & fCollision::SVSmask)
 	{
@@ -3392,6 +3540,53 @@ void btSoftBody::defaultCollisionHandler(btSoftBody* psb)
 			}
 		}
 		break;
+        case fCollision::VF_DD:
+        {
+            if (this != psb)
+            {
+                btSoftColliders::CollideVF_DD docollide;
+                /* common                    */
+                docollide.mrg = getCollisionShape()->getMargin() +
+                psb->getCollisionShape()->getMargin();
+                /* psb0 nodes vs psb1 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);
+                /* psb1 nodes vs psb0 faces    */
+                docollide.psb[0] = psb;
+                docollide.psb[1] = this;
+                docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_ndbvt.m_root,
+                                                    docollide.psb[1]->m_fdbvt.m_root,
+                                                    docollide);
+            }
+            else
+            {
+                btSoftColliders::CollideFF_DD docollide;
+                docollide.mrg = getCollisionShape()->getMargin() +
+                psb->getCollisionShape()->getMargin();
+                docollide.psb[0] = this;
+                docollide.psb[1] = psb;
+                /* psb0 faces vs psb0 faces    */
+                btDbvntNode* root = copyToDbvnt(this->m_fdbvt.m_root);
+                calculateNormalCone(root);
+                this->m_fdbvt.selfCollideT(root,docollide);
+                delete root;
+                
+//                btSoftColliders::CollideFF_DD docollide;
+//                /* common                    */
+//                docollide.mrg = getCollisionShape()->getMargin() +
+//                psb->getCollisionShape()->getMargin();
+//                /* psb0 nodes vs psb1 faces    */
+//                docollide.psb[0] = this;
+//                docollide.psb[1] = psb;
+//                docollide.psb[0]->m_ndbvt.collideTT(docollide.psb[0]->m_fdbvt.m_root,
+//                                                    docollide.psb[1]->m_fdbvt.m_root,
+//                                                    docollide);
+            }
+        }
+        break;
 		default:
 		{
 		}

src/BulletSoftBody/btSoftBody.h

@@ -161,11 +161,13 @@ public:
 			SDF_RS = 0x0001,   ///SDF based rigid vs soft
 			CL_RS = 0x0002,    ///Cluster vs convex rigid vs soft
             SDF_RD = 0x0003,   ///DF based rigid vs deformable
+            SDF_RDF = 0x0004,   ///DF based rigid vs deformable faces
 
-			SVSmask = 0x0030,  ///Rigid versus soft mask
+			SVSmask = 0x00F0,  ///Rigid versus soft mask
 			VF_SS = 0x0010,    ///Vertex vs face soft vs soft handling
 			CL_SS = 0x0020,    ///Cluster vs cluster soft vs soft handling
 			CL_SELF = 0x0040,  ///Cluster soft body self collision
+            VF_DD = 0x0050,    ///Vertex vs face soft vs soft handling
 			/* presets	*/
 			Default = SDF_RS,
 			END
@@ -217,6 +219,7 @@ public:
 		const btCollisionObject* m_colObj; /* Rigid body			*/
 		btVector3 m_normal;                /* Outward normal		*/
 		btScalar m_offset;                 /* Offset from origin	*/
+        btVector3 m_bary;                  /* Barycentric weights for faces */
 	};
 
 	/* sMedium		*/
@@ -283,6 +286,7 @@ public:
 		btVector3 m_normal;  // Normal
 		btScalar m_ra;       // Rest area
 		btDbvtNode* m_leaf;  // Leaf data
+        int m_index;
 	};
 	/* Tetra		*/
 	struct Tetra : Feature
@@ -297,6 +301,16 @@ public:
         btMatrix3x3 m_F;
         btScalar m_element_measure;
 	};
+    
+    /*  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
+    };
+    
 	/* RContact		*/
 	struct RContact
 	{
@@ -315,6 +329,53 @@ public:
         btVector3 t1;
         btVector3 t2;
 	};
+    
+    class DeformableRigidContact
+    {
+    public:
+        sCti m_cti;        // Contact infos
+        btMatrix3x3 m_c0;  // Impulse matrix
+        btVector3 m_c1;    // Relative anchor
+        btScalar m_c2;     // inverse mass of node/face
+        btScalar m_c3;     // Friction
+        btScalar m_c4;     // Hardness
+        
+        // jacobians and unit impulse responses for multibody
+        btMultiBodyJacobianData jacobianData_normal;
+        btMultiBodyJacobianData jacobianData_t1;
+        btMultiBodyJacobianData jacobianData_t2;
+        btVector3 t1;
+        btVector3 t2;
+    };
+    
+    class DeformableNodeRigidContact : public DeformableRigidContact
+    {
+    public:
+        Node* m_node;      // Owner node
+    };
+    
+    class DeformableFaceRigidContact : public DeformableRigidContact
+    {
+    public:
+        Face* m_face;                   // Owner face
+        btVector3 m_contactPoint;       // Contact point
+        btVector3 m_bary;               // Barycentric weights
+        btVector3 m_weights;            // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v;
+    };
+    
+    struct DeformableFaceNodeContact
+    {
+        Node* m_node;         // Node
+        Face* m_face;         // Face
+        btVector3 m_bary;     // Barycentric weights
+        btVector3 m_weights;  // v_contactPoint * m_weights[i] = m_face->m_node[i]->m_v;
+        btVector3 m_normal;   // Normal
+        btScalar m_margin;    // Margin
+        btScalar m_friction;  // Friction
+        btScalar m_imf;       // inverse mass of the face at contact point
+        btScalar m_c0;        // scale of the impulse matrix;
+    };
+    
 	/* SContact		*/
 	struct SContact
 	{
@@ -703,11 +764,18 @@ public:
 	btSoftBodyWorldInfo* m_worldInfo;  // World info
 	tNoteArray m_notes;                // Notes
 	tNodeArray m_nodes;                // Nodes
+    tNodeArray m_renderNodes;                // Nodes
 	tLinkArray m_links;                // Links
 	tFaceArray m_faces;                // Faces
+    tFaceArray m_renderFaces;                // Faces
 	tTetraArray m_tetras;              // Tetras
+    btAlignedObjectArray<TetraScratch> m_tetraScratches;
+    btAlignedObjectArray<TetraScratch> m_tetraScratchesTn;
 	tAnchorArray m_anchors;            // Anchors
 	tRContactArray m_rcontacts;        // Rigid contacts
+    btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts;
+    btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts;
+    btAlignedObjectArray<DeformableFaceRigidContact> m_faceRigidContacts;
 	tSContactArray m_scontacts;        // Soft contacts
 	tJointArray m_joints;              // Joints
 	tMaterialArray m_materials;        // Materials
@@ -719,6 +787,9 @@ public:
 	btDbvt m_cdbvt;                    // Clusters tree
 	tClusterArray m_clusters;          // Clusters
     btScalar m_dampingCoefficient;     // Damping Coefficient
+    
+    btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights;
+    btAlignedObjectArray<btAlignedObjectArray<const btSoftBody::Node*> > m_renderNodesParents;
 
 	btAlignedObjectArray<bool> m_clusterConnectivity;  //cluster connectivity, for self-collision
 
@@ -1012,6 +1083,7 @@ public:
 	void initializeFaceTree();
 	btVector3 evaluateCom() const;
 	bool checkDeformableContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
+    bool checkDeformableFaceContact(const btCollisionObjectWrapper* colObjWrap, const Face& x, btVector3& contact_point, btVector3& bary, btScalar margin, btSoftBody::sCti& cti, bool predict = false) const;
     bool checkContact(const btCollisionObjectWrapper* colObjWrap, const btVector3& x, btScalar margin, btSoftBody::sCti& cti) const;
 	void updateNormals();
 	void updateBounds();
@@ -1029,7 +1101,9 @@ public:
     void setSpringStiffness(btScalar k);
     void initializeDmInverse();
     void updateDeformation();
+    void advanceDeformation();
 	void applyForces();
+    void interpolateRenderMesh();
 	static void PSolve_Anchors(btSoftBody* psb, btScalar kst, btScalar ti);
 	static void PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti);
 	static void PSolve_SContacts(btSoftBody* psb, btScalar, btScalar ti);
@@ -1042,8 +1116,6 @@ public:
   
 	///fills the dataBuffer and returns the struct name (and 0 on failure)
 	virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
-
-	//virtual void serializeSingleObject(class btSerializer* serializer) const;
 };
 
 #endif  //_BT_SOFT_BODY_H

src/BulletSoftBody/btSoftBodyHelpers.cpp

@@ -20,11 +20,13 @@ subject to the following restrictions:
 #include <iostream>
 #include <sstream>
 #include <string.h>
+#include <algorithm>
 #include "btSoftBodyHelpers.h"
 #include "LinearMath/btConvexHull.h"
 #include "LinearMath/btConvexHullComputer.h"
+#include <map>
+#include <vector>
 
-//
 static void drawVertex(btIDebugDraw* idraw,
 					   const btVector3& x, btScalar s, const btVector3& c)
 {
@@ -1232,6 +1234,8 @@ if(face&&face[0])
 		}
 	}
     psb->initializeDmInverse();
+    psb->m_tetraScratches.resize(psb->m_tetras.size());
+    psb->m_tetraScratchesTn.resize(psb->m_tetras.size());
 	printf("Nodes:  %u\r\n", psb->m_nodes.size());
 	printf("Links:  %u\r\n", psb->m_links.size());
 	printf("Faces:  %u\r\n", psb->m_faces.size());
@@ -1321,7 +1325,12 @@ btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo,
             psb->appendLink(ni[2], ni[3], 0, true);
         }
     }
+    
+    
+    generateBoundaryFaces(psb);
     psb->initializeDmInverse();
+    psb->m_tetraScratches.resize(psb->m_tetras.size());
+    psb->m_tetraScratchesTn.resize(psb->m_tetras.size());
     printf("Nodes:  %u\r\n", psb->m_nodes.size());
     printf("Links:  %u\r\n", psb->m_links.size());
     printf("Faces:  %u\r\n", psb->m_faces.size());
@@ -1330,3 +1339,243 @@ btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo,
     fs.close();
     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<int> Index;
+    btAlignedObjectArray<Index> 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<int>, std::vector<int> > dict;
+    for (int i = 0; i < indices.size(); ++i)
+    {
+        for (int j = 0; j < 4; ++j)
+        {
+            std::vector<int> 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<int> 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 (std::map<std::vector<int>, std::vector<int> >::iterator it = dict.begin(); it != dict.end(); ++it)
+    {
+        std::vector<int> f = it->second;
+        psb->appendFace(f[0], f[1], f[2]);
+    }
+}
+
+void btSoftBodyHelpers::writeObj(const char* filename, const btSoftBody* psb)
+{
+    std::ofstream fs;
+    fs.open(filename);
+    btAssert(fs);
+    for (int i = 0; i < psb->m_nodes.size(); ++i)
+    {
+        fs << "v";
+        for (int d = 0; d < 3; d++)
+        {
+             fs << " " << psb->m_nodes[i].m_x[d];
+        }
+        fs << "\n";
+    }
+    
+    for (int i = 0; i < psb->m_faces.size(); ++i)
+    {
+        fs << "f";
+        for (int n = 0; n < 3; n++)
+        {
+            fs << " " << psb->m_faces[i].m_n[n]->index + 1;
+        }
+        fs << "\n";
+    }
+    fs.close();
+}
+
+void btSoftBodyHelpers::duplicateFaces(const char* filename, const btSoftBody* psb)
+{
+    std::ifstream fs_read;
+    fs_read.open(filename);
+    std::string line;
+    btVector3 pos;
+    btAlignedObjectArray<btAlignedObjectArray<int> > additional_faces;
+    while (std::getline(fs_read, line))
+    {
+        std::stringstream ss(line);
+        if (line[0] == 'v')
+        {
+        }
+        else if (line[0] == 'f')
+        {
+            ss.ignore();
+            int id0, id1, id2;
+            ss >> id0;
+            ss >> id1;
+            ss >> id2;
+            btAlignedObjectArray<int> new_face;
+            new_face.push_back(id1);
+            new_face.push_back(id0);
+            new_face.push_back(id2);
+            additional_faces.push_back(new_face);
+        }
+    }
+    fs_read.close();
+
+    std::ofstream fs_write;
+    fs_write.open(filename, std::ios_base::app);
+    for (int i = 0; i < additional_faces.size(); ++i)
+    {
+        fs_write << "f";
+        for (int n = 0; n < 3; n++)
+        {
+            fs_write << " " << additional_faces[i][n];
+        }
+        fs_write << "\n";
+    }
+    fs_write.close();
+}
+
+// Given a simplex with vertices a,b,c,d, find the barycentric weights of p in this simplex
+void btSoftBodyHelpers::getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary)
+{
+    btVector3 vap = p - a;
+    btVector3 vbp = p - b;
+    
+    btVector3 vab = b - a;
+    btVector3 vac = c - a;
+    btVector3 vad = d - a;
+    
+    btVector3 vbc = c - b;
+    btVector3 vbd = d - b;
+    btScalar va6 = (vbp.cross(vbd)).dot(vbc);
+    btScalar vb6 = (vap.cross(vac)).dot(vad);
+    btScalar vc6 = (vap.cross(vad)).dot(vab);
+    btScalar vd6 = (vap.cross(vab)).dot(vac);
+    btScalar v6 = btScalar(1) / (vab.cross(vac).dot(vad));
+    bary = btVector4(va6*v6, vb6*v6, vc6*v6, vd6*v6);
+}
+
+void btSoftBodyHelpers::readRenderMeshFromObj(const char* file, btSoftBody* psb)
+{
+    std::ifstream fs;
+    fs.open(file);
+    std::string line;
+    btVector3 pos;
+    while (std::getline(fs, line))
+    {
+        std::stringstream ss(line);
+        if (line[0] == 'v' && line[1] != 't' && line[1] != 'n')
+        {
+            ss.ignore();
+            for (size_t i = 0; i < 3; i++)
+                ss >> pos[i];
+            btSoftBody::Node n;
+            n.m_x = pos;
+            psb->m_renderNodes.push_back(n);
+        }
+        else if (line[0] == 'f')
+        {
+            ss.ignore();
+            int id0, id1, id2;
+            ss >> id0;
+            ss >> id1;
+            ss >> id2;
+            btSoftBody::Face f;
+            f.m_n[0] = &psb->m_renderNodes[id0-1];
+            f.m_n[1] = &psb->m_renderNodes[id1-1];
+            f.m_n[2] = &psb->m_renderNodes[id2-1];
+            psb->m_renderFaces.push_back(f);
+        }
+    }
+    fs.close();
+}
+
+// Iterate through all render nodes to find the simulation tetrahedron that contains the render node and record the barycentric weights
+// If the node is not inside any tetrahedron, assign it to the tetrahedron in which the node has the least negative barycentric weight
+void btSoftBodyHelpers::interpolateBarycentricWeights(btSoftBody* psb)
+{
+    psb->m_renderNodesInterpolationWeights.resize(psb->m_renderNodes.size());
+    psb->m_renderNodesParents.resize(psb->m_renderNodes.size());
+    for (int i = 0; i < psb->m_renderNodes.size(); ++i)
+    {
+        const btVector3& p = psb->m_renderNodes[i].m_x;
+        btVector4 bary;
+        btVector4 optimal_bary;
+        btScalar min_bary_weight = -1e3;
+        btAlignedObjectArray<const btSoftBody::Node*> optimal_parents;
+        bool found = false;
+        for (int j = 0; j < psb->m_tetras.size(); ++j)
+        {
+            const btSoftBody::Tetra& t = psb->m_tetras[j];
+            getBarycentricWeights(t.m_n[0]->m_x, t.m_n[1]->m_x, t.m_n[2]->m_x, t.m_n[3]->m_x, p, bary);
+            btScalar new_min_bary_weight = bary[0];
+            for (int k = 1; k < 4; ++k)
+            {
+                new_min_bary_weight = btMin(new_min_bary_weight, bary[k]);
+            }
+            if (new_min_bary_weight > min_bary_weight)
+            {
+                btAlignedObjectArray<const btSoftBody::Node*> parents;
+                parents.push_back(t.m_n[0]);
+                parents.push_back(t.m_n[1]);
+                parents.push_back(t.m_n[2]);
+                parents.push_back(t.m_n[3]);
+                optimal_parents = parents;
+                optimal_bary = bary;
+                min_bary_weight = new_min_bary_weight;
+                // stop searching if p is inside the tetrahedron at hand
+                if (bary[0]>=0. && bary[1]>=0. && bary[2]>=0. && bary[3]>=0.)
+                {
+                    break;
+                }
+            }
+        }
+        psb->m_renderNodesInterpolationWeights[i] = optimal_bary;
+        psb->m_renderNodesParents[i] = optimal_parents;
+    }
+}

src/BulletSoftBody/btSoftBodyHelpers.h

@@ -144,8 +144,17 @@ struct btSoftBodyHelpers
 											bool bfacesfromtetras);
     static btSoftBody* CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo, const char* vtk_file);
 
+    static void writeObj(const char* file, const btSoftBody* psb);
     
+    static void getBarycentricWeights(const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, const btVector3& p, btVector4& bary);
+    
+    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
 	/// This tends to make adjacent loop iterations not dependent upon one another,

src/BulletSoftBody/btSoftBodyInternals.h

@@ -29,6 +29,8 @@ subject to the following restrictions:
 #include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
 #include <string.h>  //for memset
 #include <cmath>
+
+// Given a multibody link, a contact point and a contact direction, fill in the jacobian data needed to calculate the velocity change given an impulse in the contact direction
 static void findJacobian(const btMultiBodyLinkCollider* multibodyLinkCol,
                          btMultiBodyJacobianData& jacobianData,
                          const btVector3& contact_point,
@@ -501,6 +503,77 @@ static inline void ProjectOrigin(const btVector3& a,
 	}
 }
 
+//
+static inline bool rayIntersectsTriangle(const btVector3& origin, const btVector3& dir, const btVector3& v0, const btVector3& v1, const btVector3& v2, btScalar& t)
+{
+    btScalar a, f, u, v;
+    
+    btVector3 e1 = v1 - v0;
+    btVector3 e2 = v2 - v0;
+    btVector3 h = dir.cross(e2);
+    a = e1.dot(h);
+    
+    if (a > -0.00001 && a < 0.00001)
+        return (false);
+    
+    f = btScalar(1) / a;
+    btVector3 s = origin - v0;
+    u = f * s.dot(h);
+    
+    if (u < 0.0 || u > 1.0)
+        return (false);
+    
+    btVector3 q = s.cross(e1);
+    v = f * dir.dot(q);
+    if (v < 0.0 || u + v > 1.0)
+        return (false);
+    // at this stage we can compute t to find out where
+    // the intersection point is on the line
+    t = f * e2.dot(q);
+    if (t > 0)  // ray intersection
+        return (true);
+    else  // this means that there is a line intersection
+        // but not a ray intersection
+        return (false);
+}
+
+static inline bool lineIntersectsTriangle(const btVector3& rayStart, const btVector3& rayEnd, const btVector3& p1, const btVector3& p2, const btVector3& p3, btVector3& sect, btVector3& normal)
+{
+    btVector3 dir = rayEnd - rayStart;
+    btScalar dir_norm = dir.norm();
+    if (dir_norm < SIMD_EPSILON)
+        return false;
+    dir.normalize();
+
+    btScalar t;
+    
+    bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, t);
+    
+    if (ret)
+    {
+        if (t <= dir_norm)
+        {
+            sect = rayStart + dir * t;
+        }
+        else
+        {
+            ret = false;
+        }
+    }
+    
+    if (ret)
+    {
+        btVector3 n = (p3-p1).cross(p2-p1);
+        n.safeNormalize();
+        if (n.dot(dir) < 0)
+            normal = n;
+        else
+            normal = -n;
+    }
+    return ret;
+}
+
+
 //
 template <typename T>
 static inline T BaryEval(const T& a,
@@ -993,11 +1066,11 @@ struct btSoftColliders
 		void DoNode(btSoftBody::Node& n) const
 		{
 			const btScalar m = n.m_im > 0 ? dynmargin : stamargin;
-			btSoftBody::RContact c;
+			btSoftBody::DeformableNodeRigidContact c;
 
 			if (!n.m_battach)
             {
-                // check for collision at x_{n+1}^*
+                // check for collision at x_{n+1}^* as well at x_n
                 if (psb->checkDeformableContact(m_colObj1Wrap, n.m_x, m, c.m_cti, /*predict = */ true) || psb->checkDeformableContact(m_colObj1Wrap, n.m_q, m, c.m_cti, /*predict = */ true))
                 {
                     const btScalar ima = n.m_im;
@@ -1010,7 +1083,7 @@ struct btSoftColliders
                         btSoftBody::sCti& cti = c.m_cti;
                         c.m_node = &n;
                         const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
-                        c.m_c2 = ima * psb->m_sst.sdt;
+                        c.m_c2 = ima;
                         c.m_c3 = fc;
                         c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
                         
@@ -1021,7 +1094,7 @@ struct btSoftColliders
                             const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
                             const btVector3 ra = n.m_x - wtr.getOrigin();
                             
-                            c.m_c0 = ImpulseMatrix(psb->m_sst.sdt, ima, imb, iwi, ra);
+                            c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
                             c.m_c1 = ra;
                             if (m_rigidBody)
                                 m_rigidBody->activate();
@@ -1051,8 +1124,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;
-                                btScalar dt = psb->m_sst.sdt;
+                                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 = Diagonal(1/dt) * (Diagonal(n.m_im) + 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;
@@ -1061,7 +1133,7 @@ struct btSoftColliders
                                 c.t2 = t2;
                             }
                         }
-                        psb->m_rcontacts.push_back(c);
+                        psb->m_nodeRigidContacts.push_back(c);
                     }
                 }
 			}
@@ -1072,6 +1144,107 @@ struct btSoftColliders
 		btScalar dynmargin;
 		btScalar stamargin;
 	};
+    
+    //
+    // CollideSDF_RDF
+    //
+    struct CollideSDF_RDF : btDbvt::ICollide
+    {
+        void Process(const btDbvtNode* leaf)
+        {
+            btSoftBody::Face* face = (btSoftBody::Face*)leaf->data;
+            DoNode(*face);
+        }
+        void DoNode(btSoftBody::Face& f) const
+        {
+            btSoftBody::Node* n0 = f.m_n[0];
+            btSoftBody::Node* n1 = f.m_n[1];
+            btSoftBody::Node* n2 = f.m_n[2];
+            
+            const btScalar m = (n0->m_im > 0 && n1->m_im > 0 && n2->m_im > 0 )? dynmargin : stamargin;
+            btSoftBody::DeformableFaceRigidContact c;
+            btVector3 contact_point;
+            btVector3 bary;
+            if (psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, true))
+            {
+                btScalar ima = n0->m_im + n1->m_im + n2->m_im;
+                const btScalar imb = m_rigidBody ? m_rigidBody->getInvMass() : 0.f;
+                const btScalar ms = ima + imb;
+                if (ms > 0)
+                {
+                    // resolve contact at x_n
+                    psb->checkDeformableFaceContact(m_colObj1Wrap, f, contact_point, bary, m, c.m_cti, /*predict = */ false);
+                    btSoftBody::sCti& cti = c.m_cti;
+                    c.m_contactPoint = contact_point;
+                    c.m_bary = bary;
+                    // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices
+                    c.m_weights = btScalar(2)/(btScalar(1) + bary.length2()) * bary;
+                    c.m_face = &f;
+                    const btScalar fc = psb->m_cfg.kDF * m_colObj1Wrap->getCollisionObject()->getFriction();
+                    
+                    // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf
+                    ima = bary.getX()*c.m_weights.getX() * n0->m_im + bary.getY()*c.m_weights.getY() * n1->m_im + bary.getZ()*c.m_weights.getZ() * n2->m_im;
+                    
+                    c.m_c2 = ima;
+                    c.m_c3 = fc;
+                    c.m_c4 = m_colObj1Wrap->getCollisionObject()->isStaticOrKinematicObject() ? psb->m_cfg.kKHR : psb->m_cfg.kCHR;
+                    if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY)
+                    {
+                        const btTransform& wtr = m_rigidBody ? m_rigidBody->getWorldTransform() : m_colObj1Wrap->getCollisionObject()->getWorldTransform();
+                        static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
+                        const btMatrix3x3& iwi = m_rigidBody ? m_rigidBody->getInvInertiaTensorWorld() : iwiStatic;
+                        const btVector3 ra = contact_point - wtr.getOrigin();
+                        
+                        // we do not scale the impulse matrix by dt
+                        c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
+                        c.m_c1 = ra;
+                        if (m_rigidBody)
+                            m_rigidBody->activate();
+                    }
+                    else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+                    {
+                        btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+                        if (multibodyLinkCol)
+                        {
+                            btVector3 normal = cti.m_normal;
+                            btVector3 t1 = generateUnitOrthogonalVector(normal);
+                            btVector3 t2 = btCross(normal, t1);
+                            btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
+                            findJacobian(multibodyLinkCol, jacobianData_normal, contact_point, normal);
+                            findJacobian(multibodyLinkCol, jacobianData_t1, contact_point, t1);
+                            findJacobian(multibodyLinkCol, jacobianData_t2, contact_point, t2);
+                            
+                            btScalar* J_n = &jacobianData_normal.m_jacobians[0];
+                            btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
+                            btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
+                            
+                            btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+                            btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
+                            btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
+                            
+                            btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
+                                            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(ima) + 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;
+                            c.jacobianData_t2 = jacobianData_t2;
+                            c.t1 = t1;
+                            c.t2 = t2;
+                        }
+                    }
+                    psb->m_faceRigidContacts.push_back(c);
+                }
+            }
+        }
+        btSoftBody* psb;
+        const btCollisionObjectWrapper* m_colObj1Wrap;
+        btRigidBody* m_rigidBody;
+        btScalar dynmargin;
+        btScalar stamargin;
+    };
 	//
 	// CollideVF_SS
 	//
@@ -1082,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;
@@ -1121,6 +1300,182 @@ struct btSoftColliders
 		btSoftBody* psb[2];
 		btScalar mrg;
 	};
+    
+    //
+    // CollideVF_DD
+    //
+    struct CollideVF_DD : btDbvt::ICollide
+    {
+        void Process(const btDbvtNode* lnode,
+                     const btDbvtNode* lface)
+        {
+            btSoftBody::Node* node = (btSoftBody::Node*)lnode->data;
+            btSoftBody::Face* face = (btSoftBody::Face*)lface->data;
+            btVector3 o = node->m_x;
+            btVector3 p, normal;
+            const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
+            btVector3 dir = node->m_q - o;
+            btScalar l = dir.length();
+            if (l < SIMD_EPSILON)
+                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
+            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 = 1.5;
+            // 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;
+            bool intersect = lineIntersectsTriangle(btVector3(0,0,0), rayEnd, u0-o, u1-o, u2-o, p, normal);
+
+            if (intersect)
+            {
+                p += o;
+                const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p);
+                const btScalar ma = node->m_im;
+                btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
+                const btScalar ms = ma + mb;
+                if (ms > 0)
+                {
+                    btSoftBody::DeformableFaceNodeContact c;
+                    c.m_normal = normal;
+                    c.m_margin = mrg;
+                    c.m_node = node;
+                    c.m_face = face;
+                    c.m_bary = w;
+                    // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices
+                    c.m_weights = btScalar(2)/(btScalar(1) + w.length2()) * w;
+                    c.m_friction = btMax(psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF);
+                    // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf
+                    c.m_imf = c.m_bary[0]*c.m_weights[0] * n[0]->m_im + c.m_bary[1]*c.m_weights[1] * n[1]->m_im + c.m_bary[2]*c.m_weights[2] * n[2]->m_im;
+                    c.m_c0 = btScalar(1)/(ma + c.m_imf);
+                    psb[0]->m_faceNodeContacts.push_back(c);
+                }
+            }
+        }
+        btSoftBody* psb[2];
+        btScalar mrg;
+    };
+    
+    //
+    // CollideFF_DD
+    //
+    struct CollideFF_DD : btDbvt::ICollide
+    {
+        void Process(const btDbvntNode* lface1,
+                     const btDbvntNode* lface2)
+        {
+            btSoftBody::Face* f = (btSoftBody::Face*)lface1->data;
+            btSoftBody::Face* face = (btSoftBody::Face*)lface2->data;
+            for (int node_id = 0; node_id < 3; ++node_id)
+            {
+                btSoftBody::Node* node = f->m_n[node_id];
+                btVector3 o = node->m_x;
+                btVector3 p, normal;
+                const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
+                btVector3 dir = node->m_q - o;
+                btScalar l = dir.length();
+                if (l < SIMD_EPSILON)
+                    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
+                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 = 1.5;
+                // 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;
+                bool intersect = lineIntersectsTriangle(btVector3(0,0,0), rayEnd, u0-o, u1-o, u2-o, p, normal);
+                
+                if (intersect)
+                {
+                    p += o;
+                    const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p);
+                    const btScalar ma = node->m_im;
+                    btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
+                    const btScalar ms = ma + mb;
+                    if (ms > 0)
+                    {
+                        btSoftBody::DeformableFaceNodeContact c;
+                        c.m_normal = normal;
+                        c.m_margin = mrg;
+                        c.m_node = node;
+                        c.m_face = face;
+                        c.m_bary = w;
+                        // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices
+                        c.m_weights = btScalar(2)/(btScalar(1) + w.length2()) * w;
+                        c.m_friction = btMax(psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF);
+                        // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf
+                        c.m_imf = c.m_bary[0]*c.m_weights[0] * n[0]->m_im + c.m_bary[1]*c.m_weights[1] * n[1]->m_im + c.m_bary[2]*c.m_weights[2] * n[2]->m_im;
+                        c.m_c0 = btScalar(1)/(ma + c.m_imf);
+                        psb[0]->m_faceNodeContacts.push_back(c);
+                    }
+                }
+            }
+        }
+        void Process(const btDbvtNode* lface1,
+                     const btDbvtNode* lface2)
+        {
+            btSoftBody::Face* f = (btSoftBody::Face*)lface1->data;
+            btSoftBody::Face* face = (btSoftBody::Face*)lface2->data;
+            for (int node_id = 0; node_id < 3; ++node_id)
+            {
+                btSoftBody::Node* node = f->m_n[node_id];
+                btVector3 o = node->m_x;
+                btVector3 p, normal;
+                const btSoftBody::Node* n[] = {face->m_n[0], face->m_n[1], face->m_n[2]};
+                btVector3 dir = node->m_q - o;
+                btScalar l = dir.length();
+                if (l < SIMD_EPSILON)
+                    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
+                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 = 1.5;
+                // 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;
+                bool intersect = lineIntersectsTriangle(btVector3(0,0,0), rayEnd, u0-o, u1-o, u2-o, p, normal);
+                
+                if (intersect)
+                {
+                    p += o;
+                    const btVector3 w = BaryCoord(n[0]->m_x, n[1]->m_x, n[2]->m_x, p);
+                    const btScalar ma = node->m_im;
+                    btScalar mb = BaryEval(n[0]->m_im, n[1]->m_im, n[2]->m_im, w);
+                    const btScalar ms = ma + mb;
+                    if (ms > 0)
+                    {
+                        btSoftBody::DeformableFaceNodeContact c;
+                        c.m_normal = normal;
+                        c.m_margin = mrg;
+                        c.m_node = node;
+                        c.m_face = face;
+                        c.m_bary = w;
+                        // todo xuchenhan@: this is assuming mass of all vertices are the same. Need to modify if mass are different for distinct vertices
+                        c.m_weights = btScalar(2)/(btScalar(1) + w.length2()) * w;
+                        c.m_friction = btMax(psb[0]->m_cfg.kDF, psb[1]->m_cfg.kDF);
+                        // the effective inverse mass of the face as in https://graphics.stanford.edu/papers/cloth-sig02/cloth.pdf
+                        c.m_imf = c.m_bary[0]*c.m_weights[0] * n[0]->m_im + c.m_bary[1]*c.m_weights[1] * n[1]->m_im + c.m_bary[2]*c.m_weights[2] * n[2]->m_im;
+                        c.m_c0 = btScalar(1)/(ma + c.m_imf);
+                        psb[0]->m_faceNodeContacts.push_back(c);
+                    }
+                }
+            }
+        }
+        btSoftBody* psb[2];
+        btScalar mrg;
+    };
 };
 
 #endif  //_BT_SOFT_BODY_INTERNALS_H

src/LinearMath/btMatrix3x3.h

@@ -125,6 +125,13 @@ public:
 		m_el[2] = other.m_el[2];
 		return *this;
 	}
+    
+    SIMD_FORCE_INLINE btMatrix3x3(const btVector3& v0, const btVector3& v1, const btVector3& v2)
+    {
+        m_el[0] = v0;
+        m_el[1] = v1;
+        m_el[2] = v2;
+    }
 
 #endif