add support for anchor constraint between deformable and rigid

examples/DeformableDemo/DeformableClothAnchor.cpp

@@ -0,0 +1,236 @@
+/*
+ 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 "DeformableClothAnchor.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 DeformableClothAnchor shows contact between deformable objects and rigid objects.
+class DeformableClothAnchor : public CommonRigidBodyBase
+{
+    btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
+public:
+    DeformableClothAnchor(struct GUIHelperInterface* helper)
+    : CommonRigidBodyBase(helper)
+    {
+    }
+    virtual ~DeformableClothAnchor()
+    {
+    }
+    void initPhysics();
+    
+    void exitPhysics();
+    
+    void resetCamera()
+    {
+        float dist = 20;
+        float pitch = -45;
+        float yaw = 100;
+        float targetPos[3] = {0, -3, 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);
+    }
+    
+    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];
+            //if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
+            {
+                btSoftBodyHelpers::DrawFrame(psb, deformableWorld->getDebugDrawer());
+                btSoftBodyHelpers::Draw(psb, deformableWorld->getDebugDrawer(), deformableWorld->getDrawFlags());
+            }
+        }
+    }
+};
+
+void DeformableClothAnchor::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, -10, 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, -50, 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(1);
+        
+        //add the ground to the dynamics world
+        m_dynamicsWorld->addRigidBody(body);
+    }
+    
+    // create a piece of cloth
+    {
+        const btScalar s = 4;
+        const btScalar h = 6;
+        const int r = 9;
+        btSoftBody* psb = btSoftBodyHelpers::CreatePatch(getDeformableDynamicsWorld()->getWorldInfo(), btVector3(-s, h, -s),
+                                                         btVector3(+s, h, -s),
+                                                         btVector3(-s, h, +s),
+                                                         btVector3(+s, h, +s), r, r, 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 = 2;
+        psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
+        getDeformableDynamicsWorld()->addSoftBody(psb);
+        
+        btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(100,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);
+        
+        btTransform startTransform;
+        startTransform.setIdentity();
+        startTransform.setOrigin(btVector3(0, h, -(s + 3.5)));
+        btRigidBody* body = createRigidBody(2, startTransform, new btBoxShape(btVector3(s, 1, 3)));
+        psb->appendDeformableAnchor(0, body);
+        psb->appendDeformableAnchor(r - 1, body);
+    }
+    getDeformableDynamicsWorld()->setImplicit(false);
+    getDeformableDynamicsWorld()->setLineSearch(false);
+    m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void DeformableClothAnchor::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* DeformableClothAnchorCreateFunc(struct CommonExampleOptions& options)
+{
+    return new DeformableClothAnchor(options.m_guiHelper);
+}
+
+

examples/DeformableDemo/DeformableClothAnchor.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_CLOTH_ANCHOR_H
+#define DEFORMABLE_CLOTH_ANCHOR_H
+
+class CommonExampleInterface* DeformableClothAnchorCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //DEFORMABLE_CLOTH_ANCHOR_H

examples/ExampleBrowser/CMakeLists.txt

@@ -371,6 +371,8 @@ SET(BulletExampleBrowser_SRCS
   ../DeformableDemo/DeformableRigid.h
   ../DeformableDemo/VolumetricDeformable.cpp
   ../DeformableDemo/VolumetricDeformable.h
+  ../DeformableDemo/DeformableClothAnchor.cpp
+  ../DeformableDemo/DeformableClothAnchor.h
   ../MultiBody/MultiDofDemo.cpp
   ../MultiBody/MultiDofDemo.h
   ../RigidBody/RigidBodySoftContact.cpp

examples/ExampleBrowser/ExampleEntries.cpp

@@ -53,6 +53,7 @@
 #include "../DeformableDemo/VolumetricDeformable.h"
 #include "../DeformableDemo/GraspDeformable.h"
 #include "../DeformableDemo/DeformableContact.h"
+#include "../DeformableDemo/DeformableClothAnchor.h"
 #include "../SharedMemory/PhysicsServerExampleBullet2.h"
 #include "../SharedMemory/PhysicsServerExample.h"
 #include "../SharedMemory/PhysicsClientExample.h"
@@ -198,6 +199,7 @@ static ExampleEntry gDefaultExamples[] =
         ExampleEntry(1, "Grasp Deformable Cube", "Grasping test", PinchCreateFunc),
         ExampleEntry(1, "Grasp Deformable with Motor", "Grasping test", GraspDeformableCreateFunc),
         ExampleEntry(1, "Volumetric Deformable Objects", "Volumetric Deformable test", VolumetricDeformableCreateFunc),
+        ExampleEntry(1, "Deformable Cloth Anchor", "Deformable Anchor test", DeformableClothAnchorCreateFunc),
         ExampleEntry(1, "Deformable-MultiBody Contact", "MultiBody and Deformable contact", DeformableMultibodyCreateFunc),
         // ExampleEntry(1, "MultiBody Baseline", "MultiBody Baseline", MultiBodyBaselineCreateFunc),
         

src/BulletSoftBody/btDeformableContactConstraint.cpp

@@ -15,6 +15,127 @@
 
 #include "btDeformableContactConstraint.h"
 
+/* ================   Deformable Node Anchor   =================== */
+btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& a)
+: m_anchor(&a)
+, btDeformableContactConstraint(a.m_cti.m_normal)
+{
+}
+
+btDeformableNodeAnchorConstraint::btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other)
+: m_anchor(other.m_anchor)
+, btDeformableContactConstraint(other)
+{
+}
+
+btVector3 btDeformableNodeAnchorConstraint::getVa() const
+{
+    const btSoftBody::sCti& cti = m_anchor->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_anchor->m_c1)) : btVector3(0, 0, 0);
+        }
+        else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+        {
+            // multibody anchor not supported yet
+            btAssert(false);
+            multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj);
+            if (multibodyLinkCol)
+            {
+                const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
+                const btScalar* J_n = &m_anchor->jacobianData_normal.m_jacobians[0];
+                const btScalar* J_t1 = &m_anchor->jacobianData_t1.m_jacobians[0];
+                const btScalar* J_t2 = &m_anchor->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_anchor->t1 * vel;
+                vel = 0.0;
+                for (int k = 0; k < ndof; ++k)
+                {
+                    vel += (local_v[k]+local_dv[k]) * J_t2[k];
+                }
+                va += m_anchor->t2 * vel;
+            }
+        }
+    }
+    return va;
+}
+
+btScalar btDeformableNodeAnchorConstraint::solveConstraint()
+{
+    const btSoftBody::sCti& cti = m_anchor->m_cti;
+    btVector3 va = getVa();
+    btVector3 vb = getVb();
+    btVector3 vr = (vb - va);
+    // + (m_anchor->m_node->m_x - cti.m_colObj->getWorldTransform() * m_anchor->m_local) * 10.0
+    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_anchor->m_c0 * vr;
+    // 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_anchor->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_anchor->jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
+            // apply normal component of the impulse
+            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_normal, impulse.dot(cti.m_normal));
+            // apply tangential component of the impulse
+            const btScalar* deltaV_t1 = &m_anchor->jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
+            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t1, impulse.dot(m_anchor->t1));
+            const btScalar* deltaV_t2 = &m_anchor->jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
+            multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof2(deltaV_t2, impulse.dot(m_anchor->t2));
+        }
+    }
+    return residualSquare;
+}
+
+btVector3 btDeformableNodeAnchorConstraint::getVb() const
+{
+    return m_anchor->m_node->m_v;
+}
+
+void btDeformableNodeAnchorConstraint::applyImpulse(const btVector3& impulse)
+{
+    btVector3 dv = impulse * m_anchor->m_c2;
+    m_anchor->m_node->m_v -= dv;
+}
+
 /* ================   Deformable vs. Rigid   =================== */
 btDeformableRigidContactConstraint::btDeformableRigidContactConstraint(const btSoftBody::DeformableRigidContact& c)
 : m_contact(&c)

src/BulletSoftBody/btDeformableContactConstraint.h

@@ -108,6 +108,32 @@ public:
     virtual void applyImpulse(const btVector3& impulse){}
 };
 
+//
+// Anchor Constraint between rigid and deformable node
+class btDeformableNodeAnchorConstraint : public btDeformableContactConstraint
+{
+public:
+    const btSoftBody::DeformableNodeRigidAnchor* m_anchor;
+    
+    btDeformableNodeAnchorConstraint(){}
+    btDeformableNodeAnchorConstraint(const btSoftBody::DeformableNodeRigidAnchor& c);
+    btDeformableNodeAnchorConstraint(const btDeformableNodeAnchorConstraint& other);
+    virtual ~btDeformableNodeAnchorConstraint()
+    {
+    }
+    virtual btScalar solveConstraint();
+    // object A is the rigid/multi body, and object B is the deformable node/face
+    virtual btVector3 getVa() const;
+    // get the velocity of the deformable node in contact
+    virtual btVector3 getVb() const;
+    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

src/BulletSoftBody/btDeformableContactProjection.cpp

@@ -32,6 +32,14 @@ btScalar btDeformableContactProjection::update()
         }
     }
     
+    // anchor constraints
+    for (int index = 0; index < m_nodeAnchorConstraints.size(); ++index)
+    {
+        btDeformableNodeAnchorConstraint& constraint = *m_nodeAnchorConstraints.getAtIndex(index);
+        btScalar localResidualSquare = constraint.solveConstraint();
+        residualSquare = btMax(residualSquare, localResidualSquare);
+    }
+    
     // face constraints
     for (int index = 0; index < m_allFaceConstraints.size(); ++index)
     {
@@ -73,6 +81,24 @@ void btDeformableContactProjection::setConstraints()
             continue;
         }
 
+        // set up deformable anchors
+        for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
+        {
+            btSoftBody::DeformableNodeRigidAnchor& anchor = psb->m_deformableAnchors[j];
+            // skip fixed points
+            if (anchor.m_node->m_im == 0)
+            {
+                continue;
+            }
+            
+            if (m_nodeAnchorConstraints.find(anchor.m_node->index) == NULL)
+            {
+                anchor.m_c1 = anchor.m_cti.m_colObj->getWorldTransform().getBasis() * anchor.m_local;
+                btDeformableNodeAnchorConstraint constraint(anchor);
+                m_nodeAnchorConstraints.insert(anchor.m_node->index, constraint);
+            }
+        }
+        
         // set Deformable Node vs. Rigid constraint
         for (int j = 0; j < psb->m_nodeRigidContacts.size(); ++j)
         {
@@ -262,7 +288,7 @@ void btDeformableContactProjection::setProjection()
             bool existStaticConstraint = false;
             btVector3 averagedNormal(0,0,0);
             btAlignedObjectArray<btVector3> normals;
-            if (m_staticConstraints.find(index) != NULL)
+            if (m_staticConstraints.find(index) != NULL || m_nodeAnchorConstraints.find(index) != NULL)
             {
                 existStaticConstraint = true;
                 hasConstraint = true;
@@ -451,6 +477,7 @@ void btDeformableContactProjection::applyDynamicFriction(TVStack& f)
 void btDeformableContactProjection::reinitialize(bool nodeUpdated)
 {
     m_staticConstraints.clear();
+    m_nodeAnchorConstraints.clear();
     m_nodeRigidConstraints.clear();
     m_faceRigidConstraints.clear();
     m_deformableConstraints.clear();

src/BulletSoftBody/btDeformableContactProjection.h

@@ -36,6 +36,8 @@ public:
     btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceRigidContactConstraint*> > m_faceRigidConstraints;
     // map from node index to deformable constraint
     btHashMap<btHashInt, btAlignedObjectArray<btDeformableFaceNodeContactConstraint*> > m_deformableConstraints;
+    // map from node index to node anchor constraint
+    btHashMap<btHashInt, btDeformableNodeAnchorConstraint> m_nodeAnchorConstraints;
     
     // all constraints involving face
     btAlignedObjectArray<btDeformableContactConstraint*> m_allFaceConstraints;

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp

@@ -140,6 +140,13 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
             node.m_q = node.m_x;
             node.m_vn = node.m_v;
         }
+        // enforce anchor constraints
+        for (int j = 0; j < psb->m_deformableAnchors.size();++j)
+        {
+            btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j];
+            btSoftBody::Node* n = a.m_node;
+            n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local;
+        }
         psb->interpolateRenderMesh();
     }
 }

src/BulletSoftBody/btSoftBody.cpp

@@ -411,6 +411,41 @@ void btSoftBody::appendAnchor(int node, btRigidBody* body, const btVector3& loca
 	m_anchors.push_back(a);
 }
 
+//
+void btSoftBody::appendDeformableAnchor(int node, btRigidBody* body)
+{
+    DeformableNodeRigidAnchor c;
+    btSoftBody::Node& n = m_nodes[node];
+    const btScalar ima = n.m_im;
+    const btScalar imb = body->getInvMass();
+    btVector3 nrm;
+    const btCollisionShape* shp = body->getCollisionShape();
+    const btTransform& wtr = body->getWorldTransform();
+    btScalar dst =
+    m_worldInfo->m_sparsesdf.Evaluate(
+                                      wtr.invXform(m_nodes[node].m_x),
+                                      shp,
+                                      nrm,
+                                      0);
+
+    c.m_cti.m_colObj = body;
+    c.m_cti.m_normal = wtr.getBasis() * nrm;
+    c.m_cti.m_offset = dst;
+    c.m_node = &m_nodes[node];
+    const btScalar fc = m_cfg.kDF * body->getFriction();
+    c.m_c2 = ima;
+    c.m_c3 = fc;
+    c.m_c4 = body->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR;
+    static const btMatrix3x3 iwiStatic(0, 0, 0, 0, 0, 0, 0, 0, 0);
+    const btMatrix3x3& iwi = body->getInvInertiaTensorWorld();
+    const btVector3 ra = n.m_x - wtr.getOrigin();
+
+    c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
+    c.m_c1 = ra;
+    c.m_local = body->getWorldTransform().inverse() * m_nodes[node].m_x;
+    m_deformableAnchors.push_back(c);
+}
+
 //
 void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1)
 {

src/BulletSoftBody/btSoftBody.h

@@ -354,6 +354,12 @@ public:
         Node* m_node;      // Owner node
     };
     
+    class DeformableNodeRigidAnchor : public DeformableNodeRigidContact
+    {
+    public:
+        btVector3 m_local;    // Anchor position in body space
+    };
+    
     class DeformableFaceRigidContact : public DeformableRigidContact
     {
     public:
@@ -774,6 +780,7 @@ public:
     btAlignedObjectArray<TetraScratch> m_tetraScratches;
     btAlignedObjectArray<TetraScratch> m_tetraScratchesTn;
 	tAnchorArray m_anchors;            // Anchors
+    btAlignedObjectArray<DeformableNodeRigidAnchor> m_deformableAnchors;
 	tRContactArray m_rcontacts;        // Rigid contacts
     btAlignedObjectArray<DeformableNodeRigidContact> m_nodeRigidContacts;
     btAlignedObjectArray<DeformableFaceNodeContact> m_faceNodeContacts;
@@ -897,6 +904,7 @@ public:
 					 Material* mat = 0);
 
 	/* Append anchor														*/
+    void appendDeformableAnchor(int node, btRigidBody* body);
     void appendAnchor(int node,
 					  btRigidBody* body, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1);
 	void appendAnchor(int node, btRigidBody* body, const btVector3& localPivot, bool disableCollisionBetweenLinkedBodies = false, btScalar influence = 1);