support anchor constraint between deformable and multibody

examples/DeformableDemo/DeformableMultibody.cpp

@@ -145,7 +145,6 @@ void DeformableMultibody::initPhysics()
         m_dynamicsWorld->addRigidBody(body,1,1+2);
     }
 
-    
     {
         bool damping = true;
         bool gyro = false;

examples/DeformableDemo/MultibodyClothAnchor.cpp

@@ -0,0 +1,395 @@
+/*
+ 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 "MultibodyClothAnchor.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 MultibodyClothAnchor shows contact between deformable objects and rigid objects.
+class MultibodyClothAnchor : public CommonRigidBodyBase
+{
+    btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
+public:
+    MultibodyClothAnchor(struct GUIHelperInterface* helper)
+    : CommonRigidBodyBase(helper)
+    {
+    }
+    virtual ~MultibodyClothAnchor()
+    {
+    }
+    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());
+            }
+        }
+    }
+    
+    btMultiBody* createMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool floating = false);
+    
+    void addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents);
+};
+
+void MultibodyClothAnchor::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, -20, 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(1);
+        
+        //add the ground to the dynamics world
+        m_dynamicsWorld->addRigidBody(body,1,1+2);
+    }
+    
+    // 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(30,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);
+        
+        bool damping = true;
+        bool gyro = false;
+        int numLinks = 5;
+        bool spherical = false;  //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
+        bool canSleep = false;
+        bool selfCollide = true;
+        btVector3 linkHalfExtents(1.5, .5, .5);
+        btVector3 baseHalfExtents(1.5, .5, .5);
+        
+        btMultiBody* mbC = createMultiBody(getDeformableDynamicsWorld(), numLinks, btVector3(s+3.5f, h, -s-0.6f), linkHalfExtents, baseHalfExtents, spherical, true);
+        
+        mbC->setCanSleep(canSleep);
+        mbC->setHasSelfCollision(selfCollide);
+        mbC->setUseGyroTerm(gyro);
+        //
+        if (!damping)
+        {
+            mbC->setLinearDamping(0.0f);
+            mbC->setAngularDamping(0.0f);
+        }
+        else
+        {
+            mbC->setLinearDamping(0.04f);
+            mbC->setAngularDamping(0.04f);
+        }
+        
+        if (numLinks > 0)
+        {
+            btScalar q0 = 0.f * SIMD_PI / 180.f;
+            if (!spherical)
+            {
+                mbC->setJointPosMultiDof(0, &q0);
+            }
+            else
+            {
+                btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
+                quat0.normalize();
+                mbC->setJointPosMultiDof(0, quat0);
+            }
+        }
+        ///
+        addColliders(mbC, getDeformableDynamicsWorld(), baseHalfExtents, linkHalfExtents);
+
+        // quick hack: advance time to populate the variables in multibody
+        m_dynamicsWorld->stepSimulation(SIMD_EPSILON, 0);
+        btAlignedObjectArray<btQuaternion> scratch_q;
+        btAlignedObjectArray<btVector3> scratch_m;
+        mbC->forwardKinematics(scratch_q, scratch_m);
+        psb->appendDeformableAnchor(0, mbC->getLink(3).m_collider);
+        psb->appendDeformableAnchor(r - 1, mbC->getLink(0).m_collider);
+    }
+    getDeformableDynamicsWorld()->setImplicit(false);
+    getDeformableDynamicsWorld()->setLineSearch(false);
+    m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void MultibodyClothAnchor::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;
+}
+
+btMultiBody* MultibodyClothAnchor::createMultiBody(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool floating)
+{
+    //init the base
+    btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
+    float baseMass = 1.f;
+
+    if (baseMass)
+    {
+        btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
+        pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
+        delete pTempBox;
+    }
+    
+    bool canSleep = false;
+    
+    btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
+    
+    btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
+    pMultiBody->setBasePos(basePosition);
+    pMultiBody->setWorldToBaseRot(baseOriQuat);
+    btVector3 vel(0, 0, 0);
+
+    //init the links
+    btVector3 hingeJointAxis(0, 1, 0);
+    float linkMass = 1.f;
+    btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
+    
+    btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
+    pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag);
+    delete pTempBox;
+    
+    //y-axis assumed up
+    btVector3 parentComToCurrentCom(-linkHalfExtents[0] * 2.f, 0, 0);                      //par body's COM to cur body's COM offset
+    btVector3 currentPivotToCurrentCom(-linkHalfExtents[0], 0, 0);                         //cur body's COM to cur body's PIV offset
+    btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom;  //par body's COM to cur body's PIV offset
+    
+    //////
+    btScalar q0 = 0.f * SIMD_PI / 180.f;
+    btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
+    quat0.normalize();
+    /////
+    
+    for (int i = 0; i < numLinks; ++i)
+    {
+        if (!spherical)
+            pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true);
+        else
+            pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true);
+    }
+    
+    pMultiBody->finalizeMultiDof();
+    
+    ///
+    pWorld->addMultiBody(pMultiBody);
+    ///
+    return pMultiBody;
+}
+
+void MultibodyClothAnchor::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents)
+{
+    btAlignedObjectArray<btQuaternion> world_to_local;
+    world_to_local.resize(pMultiBody->getNumLinks() + 1);
+    
+    btAlignedObjectArray<btVector3> local_origin;
+    local_origin.resize(pMultiBody->getNumLinks() + 1);
+    world_to_local[0] = pMultiBody->getWorldToBaseRot();
+    local_origin[0] = pMultiBody->getBasePos();
+    
+    {
+        btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
+
+        btCollisionShape* box = new btBoxShape(baseHalfExtents);
+        btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
+        col->setCollisionShape(box);
+
+        btTransform tr;
+        tr.setIdentity();
+        tr.setOrigin(local_origin[0]);
+        tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+        col->setWorldTransform(tr);
+        pWorld->addCollisionObject(col, 2, 1+2);
+        col->setFriction(1);
+        pMultiBody->setBaseCollider(col);
+    }
+
+    for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
+    {
+        const int parent = pMultiBody->getParent(i);
+        world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1];
+        local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i)));
+    }
+
+    for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
+    {
+        btVector3 posr = local_origin[i + 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()};
+        
+        btCollisionShape* box = new btBoxShape(linkHalfExtents);
+        btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
+        
+        col->setCollisionShape(box);
+        btTransform tr;
+        tr.setIdentity();
+        tr.setOrigin(posr);
+        tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+        col->setWorldTransform(tr);
+        col->setFriction(1);
+        pWorld->addCollisionObject(col, 2, 1+2);
+        pMultiBody->getLink(i).m_collider = col;
+    }
+}
+
+class CommonExampleInterface* MultibodyClothAnchorCreateFunc(struct CommonExampleOptions& options)
+{
+    return new MultibodyClothAnchor(options.m_guiHelper);
+}
+
+

examples/DeformableDemo/MultibodyClothAnchor.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 MULTIBODY_CLOTH_ANCHOR_H
+#define MULTIBODY_CLOTH_ANCHOR_H
+
+class CommonExampleInterface* MultibodyClothAnchorCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //MULTIBODY_CLOTH_ANCHOR_H

examples/ExampleBrowser/CMakeLists.txt

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

examples/ExampleBrowser/ExampleEntries.cpp

@@ -54,6 +54,7 @@
 #include "../DeformableDemo/GraspDeformable.h"
 #include "../DeformableDemo/DeformableContact.h"
 #include "../DeformableDemo/DeformableClothAnchor.h"
+#include "../DeformableDemo/MultibodyClothAnchor.h"
 #include "../SharedMemory/PhysicsServerExampleBullet2.h"
 #include "../SharedMemory/PhysicsServerExample.h"
 #include "../SharedMemory/PhysicsClientExample.h"
@@ -199,7 +200,8 @@ 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, "Rigid Cloth Anchor", "Deformable Rigid body Anchor test", DeformableClothAnchorCreateFunc),
+        ExampleEntry(1, "Multibody Cloth Anchor", "Deformable Multibody Anchor test", MultibodyClothAnchorCreateFunc),
         ExampleEntry(1, "Deformable-MultiBody Contact", "MultiBody and Deformable contact", DeformableMultibodyCreateFunc),
         // ExampleEntry(1, "MultiBody Baseline", "MultiBody Baseline", MultiBodyBaselineCreateFunc),
         

src/BulletSoftBody/btDeformableContactConstraint.cpp

@@ -45,8 +45,6 @@ btVector3 btDeformableNodeAnchorConstraint::getVa() const
         }
         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)
             {

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp

@@ -33,7 +33,7 @@ The algorithm also closely resembles the one in http://physbam.stanford.edu/~fed
 #include "btDeformableMultiBodyDynamicsWorld.h"
 #include "btDeformableBodySolver.h"
 #include "LinearMath/btQuickprof.h"
-
+#include "btSoftBodyInternals.h"
 void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
 {
     BT_PROFILE("internalSingleStepSimulation");
@@ -146,6 +146,51 @@ void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
             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;
+            
+            // update multibody anchor info
+            if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
+            {
+                btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj);
+                if (multibodyLinkCol)
+                {
+                    btVector3 nrm;
+                    const btCollisionShape* shp = multibodyLinkCol->getCollisionShape();
+                    const btTransform& wtr = multibodyLinkCol->getWorldTransform();
+                    psb->m_worldInfo->m_sparsesdf.Evaluate(
+                                                      wtr.invXform(n->m_x),
+                                                      shp,
+                                                      nrm,
+                                                      0);
+                    a.m_cti.m_normal = wtr.getBasis() * nrm;
+                    btVector3 normal = a.m_cti.m_normal;
+                    btVector3 t1 = generateUnitOrthogonalVector(normal);
+                    btVector3 t2 = btCross(normal, t1);
+                    btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
+                    findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal);
+                    findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1);
+                    findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, 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(n->m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
+                    a.m_c0 =  rot.transpose() * local_impulse_matrix * rot;
+                    a.jacobianData_normal = jacobianData_normal;
+                    a.jacobianData_t1 = jacobianData_t1;
+                    a.jacobianData_t2 = jacobianData_t2;
+                    a.t1 = t1;
+                    a.t2 = t2;
+                }
+            }
         }
         psb->interpolateRenderMesh();
     }

src/BulletSoftBody/btSoftBody.cpp

@@ -443,9 +443,66 @@ void btSoftBody::appendDeformableAnchor(int node, btRigidBody* body)
     c.m_c0 = ImpulseMatrix(1, ima, imb, iwi, ra);
     c.m_c1 = ra;
     c.m_local = body->getWorldTransform().inverse() * m_nodes[node].m_x;
+    c.m_node->m_battach = 1;
     m_deformableAnchors.push_back(c);
 }
 
+//
+void btSoftBody::appendDeformableAnchor(int node, btMultiBodyLinkCollider* link)
+{
+    DeformableNodeRigidAnchor c;
+    btSoftBody::Node& n = m_nodes[node];
+    const btScalar ima = n.m_im;
+    btVector3 nrm;
+    const btCollisionShape* shp = link->getCollisionShape();
+    const btTransform& wtr = link->getWorldTransform();
+    btScalar dst =
+    m_worldInfo->m_sparsesdf.Evaluate(
+                                      wtr.invXform(m_nodes[node].m_x),
+                                      shp,
+                                      nrm,
+                                      0);
+    c.m_cti.m_colObj = link;
+    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 * link->getFriction();
+    c.m_c2 = ima;
+    c.m_c3 = fc;
+    c.m_c4 = link->isStaticOrKinematicObject() ? m_cfg.kKHR : m_cfg.kCHR;
+    btVector3 normal = c.m_cti.m_normal;
+    btVector3 t1 = generateUnitOrthogonalVector(normal);
+    btVector3 t2 = btCross(normal, t1);
+    btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
+    findJacobian(link, jacobianData_normal, c.m_node->m_x, normal);
+    findJacobian(link, jacobianData_t1, c.m_node->m_x, t1);
+    findJacobian(link, jacobianData_t2, c.m_node->m_x, 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 = link->m_multiBody->getNumDofs() + 6;
+    btMatrix3x3 local_impulse_matrix = (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;
+    c.jacobianData_t2 = jacobianData_t2;
+    c.t1 = t1;
+    c.t2 = t2;
+    const btVector3 ra = n.m_x - wtr.getOrigin();
+    c.m_c1 = ra;
+    c.m_local = link->getWorldTransform().inverse() * m_nodes[node].m_x;
+    c.m_node->m_battach = 1;
+    m_deformableAnchors.push_back(c);
+}
 //
 void btSoftBody::appendLinearJoint(const LJoint::Specs& specs, Cluster* body0, Body body1)
 {

src/BulletSoftBody/btSoftBody.h

@@ -905,6 +905,7 @@ public:
 
 	/* Append anchor														*/
     void appendDeformableAnchor(int node, btRigidBody* body);
+    void appendDeformableAnchor(int node, btMultiBodyLinkCollider* link);
     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);