Add support for picking deformable with mouse

examples/CommonInterfaces/CommonDeformableBodyBase.h

@@ -0,0 +1,178 @@
+
+#ifndef COMMON_DEFORMABLE_BODY_SETUP_H
+#define COMMON_DEFORMABLE_BODY_SETUP_H
+#include "btBulletDynamicsCommon.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "btBulletDynamicsCommon.h"
+#include "CommonExampleInterface.h"
+#include "CommonGUIHelperInterface.h"
+#include "CommonRenderInterface.h"
+#include "CommonGraphicsAppInterface.h"
+#include "CommonWindowInterface.h"
+#include "CommonCameraInterface.h"
+#include "CommonMultiBodyBase.h"
+
+struct CommonDeformableBodyBase : public CommonMultiBodyBase
+{
+	btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
+	btSoftBody* m_pickedSoftBody;
+	btDeformableMousePickingForce* m_mouseForce;
+	btScalar m_maxPickingForce;
+	CommonDeformableBodyBase(GUIHelperInterface* helper)
+	: CommonMultiBodyBase(helper),
+	m_pickedSoftBody(0),
+	m_mouseForce(0),
+	m_maxPickingForce(0.3)
+	{
+	}
+
+	virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
+	{
+		return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
+	}
+
+	virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
+	{
+		return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
+	}
+    
+	virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
+	{
+		if (getDeformableDynamicsWorld() == 0)
+			return false;
+		btCollisionWorld::ClosestRayResultCallbackWithInfo rayCallback(rayFromWorld, rayToWorld);
+		getDeformableDynamicsWorld()->rayTest(rayFromWorld, rayToWorld, rayCallback);
+		if (rayCallback.hasHit())
+		{
+			btVector3 pickPos = rayCallback.m_hitPointWorld;
+			btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject);
+			btSoftBody* psb = (btSoftBody*)btSoftBody::upcast(rayCallback.m_collisionObject);
+			if (body)
+			{
+				if (!(body->isStaticObject() || body->isKinematicObject()))
+				{
+					m_pickedBody = body;
+					m_pickedBody->setActivationState(DISABLE_DEACTIVATION);
+					btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
+					btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot);
+					m_dynamicsWorld->addConstraint(p2p, true);
+					m_pickedConstraint = p2p;
+					btScalar mousePickClamping = 30.f;
+					p2p->m_setting.m_impulseClamp = mousePickClamping;
+					//very weak constraint for picking
+					p2p->m_setting.m_tau = 0.001f;
+				}
+			}
+			else if (psb)
+			{
+				int face_id = rayCallback.m_localShapeInfo->m_triangleIndex;
+				m_pickedSoftBody = psb;
+				psb->setActivationState(DISABLE_DEACTIVATION);
+				const btSoftBody::Face& f = psb->m_faces[face_id];
+				btDeformableMousePickingForce* mouse_force = new btDeformableMousePickingForce(100, 0.2, f, m_hitPos, m_maxPickingForce);
+				m_mouseForce = mouse_force;
+				getDeformableDynamicsWorld()->addForce(psb, mouse_force);
+			}
+			else
+			{
+				btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject);
+				if (multiCol && multiCol->m_multiBody)
+				{
+					m_prevCanSleep = multiCol->m_multiBody->getCanSleep();
+					multiCol->m_multiBody->setCanSleep(false);
+
+					btVector3 pivotInA = multiCol->m_multiBody->worldPosToLocal(multiCol->m_link, pickPos);
+
+					btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(multiCol->m_multiBody, multiCol->m_link, 0, pivotInA, pickPos);
+					//if you add too much energy to the system, causing high angular velocities, simulation 'explodes'
+					//see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949
+					//so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply
+					//it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?)
+					btScalar scaling = 1;
+					p2p->setMaxAppliedImpulse(2 * scaling);
+					btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_dynamicsWorld;
+					world->addMultiBodyConstraint(p2p);
+					m_pickingMultiBodyPoint2Point = p2p;
+				}
+			}
+			m_oldPickingPos = rayToWorld;
+			m_hitPos = pickPos;
+			m_oldPickingDist = (pickPos - rayFromWorld).length();
+		}
+		return false;
+	}
+
+	virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
+	{
+		if (m_pickedBody && m_pickedConstraint)
+		{
+			btPoint2PointConstraint* pickCon = static_cast<btPoint2PointConstraint*>(m_pickedConstraint);
+			if (pickCon)
+			{
+				//keep it at the same picking distance
+				btVector3 newPivotB;
+				btVector3 dir = rayToWorld - rayFromWorld;
+				dir.normalize();
+				dir *= m_oldPickingDist;
+				newPivotB = rayFromWorld + dir;
+				pickCon->setPivotB(newPivotB);
+				return true;
+			}
+		}
+		if (m_pickingMultiBodyPoint2Point)
+		{
+			//keep it at the same picking distance
+			btVector3 dir = rayToWorld - rayFromWorld;
+			dir.normalize();
+			dir *= m_oldPickingDist;
+			btVector3 newPivotB = rayFromWorld + dir;
+			m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB);
+		}
+		if (m_pickedSoftBody && m_mouseForce)
+		{
+			btVector3 newPivot;
+			btVector3 dir = rayToWorld - rayFromWorld;
+			dir.normalize();
+			dir *= m_oldPickingDist;
+			newPivot = rayFromWorld + dir;
+			m_mouseForce->setMousePos(newPivot);
+		}
+		return false;
+	}
+
+	virtual void removePickingConstraint()
+	{
+		if (m_pickedConstraint)
+		{
+			m_dynamicsWorld->removeConstraint(m_pickedConstraint);
+
+			if (m_pickedBody)
+			{
+				m_pickedBody->forceActivationState(ACTIVE_TAG);
+				m_pickedBody->activate(true);
+			}
+			delete m_pickedConstraint;
+			m_pickedConstraint = 0;
+			m_pickedBody = 0;
+		}
+		if (m_pickingMultiBodyPoint2Point)
+		{
+			m_pickingMultiBodyPoint2Point->getMultiBodyA()->setCanSleep(m_prevCanSleep);
+			btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_dynamicsWorld;
+			world->removeMultiBodyConstraint(m_pickingMultiBodyPoint2Point);
+			delete m_pickingMultiBodyPoint2Point;
+			m_pickingMultiBodyPoint2Point = 0;
+		}
+		if (m_pickedSoftBody)
+		{
+			getDeformableDynamicsWorld()->removeForce(m_pickedSoftBody, m_mouseForce);
+			delete m_mouseForce;
+			m_mouseForce = 0;
+			m_pickedSoftBody = 0;
+		}
+	}
+};
+#endif  //COMMON_MULTI_BODY_SETUP_H

examples/CommonInterfaces/CommonMultiBodyBase.h

@@ -456,6 +456,7 @@ struct CommonMultiBodyBase : public CommonExampleInterface
 
 		return false;
 	}
+    
 	virtual void removePickingConstraint()
 	{
 		if (m_pickedConstraint)

examples/DeformableDemo/ClothFriction.cpp

@@ -22,18 +22,17 @@
 #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
 #include <stdio.h>  //printf debugging
 
-#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../CommonInterfaces/CommonDeformableBodyBase.h"
 #include "../Utils/b3ResourcePath.h"
 
 ///The ClothFriction shows the use of deformable friction.
-class ClothFriction : public CommonRigidBodyBase
+class ClothFriction : public CommonDeformableBodyBase
 {
-    btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
     btDeformableBodySolver* m_deformableBodySolver;
 public:
     ClothFriction(struct GUIHelperInterface* helper)
-    : CommonRigidBodyBase(helper),
-        m_deformableBodySolver(0)
+    : CommonDeformableBodyBase(helper),
+    m_deformableBodySolver(0)
     {
     }
     virtual ~ClothFriction()
@@ -59,19 +58,9 @@ public:
         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();
+        CommonDeformableBodyBase::renderScene();
         btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
         
         for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
@@ -210,7 +199,7 @@ void ClothFriction::initPhysics()
 void ClothFriction::exitPhysics()
 {
     //cleanup in the reverse order of creation/initialization
-    
+    removePickingConstraint();
     //remove the rigidbodies from the dynamics world and delete them
     int i;
     for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)

examples/DeformableDemo/DeformableClothAnchor.cpp

@@ -21,16 +21,15 @@
 #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
 #include <stdio.h>  //printf debugging
 
-#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../CommonInterfaces/CommonDeformableBodyBase.h"
 #include "../Utils/b3ResourcePath.h"
 
 ///The DeformableClothAnchor shows contact between deformable objects and rigid objects.
-class DeformableClothAnchor : public CommonRigidBodyBase
+class DeformableClothAnchor : public CommonDeformableBodyBase
 {
-    btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
 public:
     DeformableClothAnchor(struct GUIHelperInterface* helper)
-    : CommonRigidBodyBase(helper)
+    : CommonDeformableBodyBase(helper)
     {
     }
     virtual ~DeformableClothAnchor()
@@ -55,24 +54,10 @@ public:
         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();
+        CommonDeformableBodyBase::renderScene();
         btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
         
         for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
@@ -186,7 +171,7 @@ void DeformableClothAnchor::initPhysics()
 void DeformableClothAnchor::exitPhysics()
 {
     //cleanup in the reverse order of creation/initialization
-    
+    removePickingConstraint();
     //remove the rigidbodies from the dynamics world and delete them
     int i;
     for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)

examples/DeformableDemo/DeformableContact.cpp

@@ -22,17 +22,16 @@
 #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
 #include <stdio.h>  //printf debugging
 
-#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../CommonInterfaces/CommonDeformableBodyBase.h"
 #include "../Utils/b3ResourcePath.h"
 
 ///The DeformableContact shows the contact between deformable objects
 
-class DeformableContact : public CommonRigidBodyBase
+class DeformableContact : public CommonDeformableBodyBase
 {
-    btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
 public:
     DeformableContact(struct GUIHelperInterface* helper)
-    : CommonRigidBodyBase(helper)
+    : CommonDeformableBodyBase(helper)
     {
     }
     virtual ~DeformableContact()
@@ -57,19 +56,9 @@ public:
         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();
+        CommonDeformableBodyBase::renderScene();
         
 		
 		btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
@@ -218,7 +207,7 @@ void DeformableContact::initPhysics()
 void DeformableContact::exitPhysics()
 {
     //cleanup in the reverse order of creation/initialization
-    
+    removePickingConstraint();
     //remove the rigidbodies from the dynamics world and delete them
     int i;
     for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)

examples/DeformableDemo/DeformableMultibody.cpp

@@ -28,17 +28,16 @@
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
 #include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
 
-#include "../CommonInterfaces/CommonMultiBodyBase.h"
+#include "../CommonInterfaces/CommonDeformableBodyBase.h"
 #include "../Utils/b3ResourcePath.h"
 ///The DeformableMultibody demo deformable bodies self-collision
 static bool g_floatingBase = true;
 static float friction = 1.;
-class DeformableMultibody : public CommonMultiBodyBase
+class DeformableMultibody : public CommonDeformableBodyBase
 {
-    btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
 public:
 	DeformableMultibody(struct GUIHelperInterface* helper)
-		: CommonMultiBodyBase(helper)
+    :CommonDeformableBodyBase(helper)
 	{
 	}
     
@@ -65,20 +64,9 @@ public:
     
     void addColliders_testMultiDof(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents);
     
-
-    virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
-    {
-        return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
-    }
-    
-    virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
-    {
-        return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
-    }
-    
     virtual void renderScene()
     {
-        CommonMultiBodyBase::renderScene();
+        CommonDeformableBodyBase::renderScene();
         btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
         
         for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
@@ -229,7 +217,7 @@ void DeformableMultibody::initPhysics()
 void DeformableMultibody::exitPhysics()
 {
 	//cleanup in the reverse order of creation/initialization
-
+    removePickingConstraint();
 	//remove the rigidbodies from the dynamics world and delete them
 	int i;
 	for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)

examples/DeformableDemo/DeformableRigid.cpp

@@ -21,16 +21,15 @@
 #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
 #include <stdio.h>  //printf debugging
 
-#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../CommonInterfaces/CommonDeformableBodyBase.h"
 #include "../Utils/b3ResourcePath.h"
 
 ///The DeformableRigid shows contact between deformable objects and rigid objects.
-class DeformableRigid : public CommonRigidBodyBase
+class DeformableRigid : public CommonDeformableBodyBase
 {
-    btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
 public:
 	DeformableRigid(struct GUIHelperInterface* helper)
-		: CommonRigidBodyBase(helper)
+    :CommonDeformableBodyBase(helper)
 	{
 	}
 	virtual ~DeformableRigid()
@@ -115,7 +114,7 @@ public:
     
     virtual void renderScene()
     {
-        CommonRigidBodyBase::renderScene();
+        CommonDeformableBodyBase::renderScene();
         btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
         
         for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
@@ -241,7 +240,7 @@ void DeformableRigid::initPhysics()
 void DeformableRigid::exitPhysics()
 {
 	//cleanup in the reverse order of creation/initialization
-
+    removePickingConstraint();
 	//remove the rigidbodies from the dynamics world and delete them
 	int i;
 	for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)

examples/DeformableDemo/DeformableSelfCollision.cpp

@@ -21,16 +21,15 @@
 #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
 #include <stdio.h>  //printf debugging
 
-#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../CommonInterfaces/CommonDeformableBodyBase.h"
 #include "../Utils/b3ResourcePath.h"
 
 ///The DeformableSelfCollision shows deformable self collisions
-class DeformableSelfCollision : public CommonRigidBodyBase
+class DeformableSelfCollision : public CommonDeformableBodyBase
 {
-    btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
 public:
     DeformableSelfCollision(struct GUIHelperInterface* helper)
-    : CommonRigidBodyBase(helper)
+    : CommonDeformableBodyBase(helper)
     {
     }
     virtual ~DeformableSelfCollision()
@@ -57,23 +56,9 @@ public:
     
     void addCloth(btVector3 origin);
     
-    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();
+        CommonDeformableBodyBase::renderScene();
     }
 };
 
@@ -183,7 +168,7 @@ void DeformableSelfCollision::addCloth(btVector3 origin)
 void DeformableSelfCollision::exitPhysics()
 {
     //cleanup in the reverse order of creation/initialization
-    
+    removePickingConstraint();
     //remove the rigidbodies from the dynamics world and delete them
     int i;
     for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)

examples/DeformableDemo/GraspDeformable.cpp

@@ -23,7 +23,7 @@
 #include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
 #include <stdio.h>  //printf debugging
 
-#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../CommonInterfaces/CommonDeformableBodyBase.h"
 #include "../Utils/b3ResourcePath.h"
 #include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
 #include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
@@ -56,12 +56,12 @@ static bool supportsJointMotor(btMultiBody* mb, int mbLinkIndex)
     return canHaveMotor;
 }
 
-class GraspDeformable : public CommonRigidBodyBase
+class GraspDeformable : public CommonDeformableBodyBase
 {
     btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
 public:
 	GraspDeformable(struct GUIHelperInterface* helper)
-		: CommonRigidBodyBase(helper)
+    :CommonDeformableBodyBase(helper)
 	{
 	}
 	virtual ~GraspDeformable()
@@ -154,7 +154,7 @@ public:
     
     virtual void renderScene()
     {
-        CommonRigidBodyBase::renderScene();
+        CommonDeformableBodyBase::renderScene();
         btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
         
         for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
@@ -166,6 +166,16 @@ public:
             }
         }
     }
+    
+    virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
+    {
+        return false;
+    }
+    virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
+    {
+        return false;
+    }
+    virtual void removePickingConstraint(){}
 };
 
 
@@ -189,7 +199,7 @@ void GraspDeformable::initPhysics()
 	m_dynamicsWorld->setGravity(gravity);
     getDeformableDynamicsWorld()->getWorldInfo().m_gravity = gravity;
 	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
-
+    m_maxPickingForce = 0.001;
     // build a gripper
     if(1)
     {
@@ -275,13 +285,13 @@ void GraspDeformable::initPhysics()
 	{
 		char absolute_path[1024];
 		b3BulletDefaultFileIO fileio;
-        fileio.findResourcePath("ditto.vtk", absolute_path, 1024);
+//        fileio.findResourcePath("ditto.vtk", absolute_path, 1024);
 		//        fileio.findResourcePath("banana.vtk", absolute_path, 1024);
-		//        fileio.findResourcePath("ball.vtk", absolute_path, 1024);
+//                fileio.findResourcePath("ball.vtk", absolute_path, 1024);
 		//        fileio.findResourcePath("deformable_crumpled_napkin_sim.vtk", absolute_path, 1024);
 		//        fileio.findResourcePath("single_tet.vtk", absolute_path, 1024);
-//                fileio.findResourcePath("tube.vtk", absolute_path, 1024);
-		//        fileio.findResourcePath("torus.vtk", absolute_path, 1024);
+                fileio.findResourcePath("tube.vtk", absolute_path, 1024);
+//                fileio.findResourcePath("torus.vtk", absolute_path, 1024);
 		//        fileio.findResourcePath("paper_roll.vtk", absolute_path, 1024);
 		//        fileio.findResourcePath("bread.vtk", absolute_path, 1024);
 		//        fileio.findResourcePath("boot.vtk", absolute_path, 1024);
@@ -293,17 +303,17 @@ void GraspDeformable::initPhysics()
 		btSoftBody* psb = btSoftBodyHelpers::CreateFromVtkFile(getDeformableDynamicsWorld()->getWorldInfo(), absolute_path);
 
 //        psb->scale(btVector3(30, 30, 30)); // for banana
-        psb->scale(btVector3(.7, .7, .7));
+//        psb->scale(btVector3(.7, .7, .7));
 //        psb->scale(btVector3(2, 2, 2));
-//        psb->scale(btVector3(.3, .3, .3));  // for tube, torus, boot
-        psb->scale(btVector3(.1, .1, .1));  // for ditto
+        psb->scale(btVector3(.3, .3, .3));  // for tube, torus, boot
+//        psb->scale(btVector3(.1, .1, .1));  // for ditto
 //        psb->translate(btVector3(.25, 10, 0.4));
         psb->getCollisionShape()->setMargin(0.0005);
         psb->setMaxStress(50);
         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 = 20;
+        psb->m_cfg.kDF = 2;
         psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
         getDeformableDynamicsWorld()->addSoftBody(psb);
 
@@ -311,7 +321,7 @@ void GraspDeformable::initPhysics()
         getDeformableDynamicsWorld()->addForce(psb, gravity_force);
         m_forces.push_back(gravity_force);
 
-        btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(8,32, .05);
+        btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(2,8,.02);
         getDeformableDynamicsWorld()->addForce(psb, neohookean);
         m_forces.push_back(neohookean);
     }
@@ -376,7 +386,7 @@ void GraspDeformable::initPhysics()
 void GraspDeformable::exitPhysics()
 {
 	//cleanup in the reverse order of creation/initialization
-
+    removePickingConstraint();
 	//remove the rigidbodies from the dynamics world and delete them
 	int i;
 	for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)

examples/DeformableDemo/MultibodyClothAnchor.cpp

@@ -21,16 +21,15 @@
 #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
 #include <stdio.h>  //printf debugging
 
-#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../CommonInterfaces/CommonDeformableBodyBase.h"
 #include "../Utils/b3ResourcePath.h"
 
 ///The MultibodyClothAnchor shows contact between deformable objects and rigid objects.
-class MultibodyClothAnchor : public CommonRigidBodyBase
+class MultibodyClothAnchor : public CommonDeformableBodyBase
 {
-    btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
 public:
     MultibodyClothAnchor(struct GUIHelperInterface* helper)
-    : CommonRigidBodyBase(helper)
+    : CommonDeformableBodyBase(helper)
     {
     }
     virtual ~MultibodyClothAnchor()
@@ -56,23 +55,9 @@ public:
         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();
+        CommonDeformableBodyBase::renderScene();
         btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
         
         for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
@@ -234,7 +219,7 @@ void MultibodyClothAnchor::initPhysics()
 void MultibodyClothAnchor::exitPhysics()
 {
     //cleanup in the reverse order of creation/initialization
-    
+    removePickingConstraint();
     //remove the rigidbodies from the dynamics world and delete them
     int i;
     for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)

examples/DeformableDemo/Pinch.cpp

@@ -22,7 +22,7 @@
 #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
 #include <stdio.h>  //printf debugging
 
-#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../CommonInterfaces/CommonDeformableBodyBase.h"
 #include "../Utils/b3ResourcePath.h"
 
 ///The Pinch shows the frictional contact between kinematic rigid objects with deformable objects
@@ -32,12 +32,11 @@ struct TetraCube
 #include "../SoftDemo/cube.inl"
 };
 
-class Pinch : public CommonRigidBodyBase
+class Pinch : public CommonDeformableBodyBase
 {
-    btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
 public:
 	Pinch(struct GUIHelperInterface* helper)
-		: CommonRigidBodyBase(helper)
+		: CommonDeformableBodyBase(helper)
 	{
 	}
 	virtual ~Pinch()
@@ -81,19 +80,9 @@ public:
         }
     }
     
-    virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
-    {
-        return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
-    }
-    
-    virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
-    {
-        return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
-    }
-    
     virtual void renderScene()
     {
-        CommonRigidBodyBase::renderScene();
+        CommonDeformableBodyBase::renderScene();
         btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
         
         for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
@@ -337,7 +326,7 @@ void Pinch::initPhysics()
 void Pinch::exitPhysics()
 {
 	//cleanup in the reverse order of creation/initialization
-
+    removePickingConstraint();
 	//remove the rigidbodies from the dynamics world and delete them
 	int i;
 	for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)

examples/DeformableDemo/PinchFriction.cpp

@@ -21,7 +21,7 @@
 #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
 #include <stdio.h>  //printf debugging
 
-#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../CommonInterfaces/CommonDeformableBodyBase.h"
 #include "../Utils/b3ResourcePath.h"
 
 ///The PinchFriction shows the frictional contacts among volumetric deformable objects
@@ -31,12 +31,12 @@ struct TetraCube
 #include "../SoftDemo/cube.inl"
 };
 
-class PinchFriction : public CommonRigidBodyBase
+class PinchFriction : public CommonDeformableBodyBase
 {
     btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
 public:
     PinchFriction(struct GUIHelperInterface* helper)
-    : CommonRigidBodyBase(helper)
+    : CommonDeformableBodyBase(helper)
     {
     }
     virtual ~PinchFriction()
@@ -80,20 +80,20 @@ public:
         }
     }
     
-    virtual const btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld() const
-    {
-        return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
-    }
-    
-    virtual btDeformableMultiBodyDynamicsWorld* getDeformableDynamicsWorld()
-    {
-        return (btDeformableMultiBodyDynamicsWorld*)m_dynamicsWorld;
-    }
-    
     virtual void renderScene()
     {
-        CommonRigidBodyBase::renderScene();
+        CommonDeformableBodyBase::renderScene();
     }
+    
+    virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
+    {
+        return false;
+    }
+    virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
+    {
+        return false;
+    }
+    virtual void removePickingConstraint(){}
 };
 
 void dynamics2(btScalar time, btDeformableMultiBodyDynamicsWorld* world)
@@ -353,7 +353,7 @@ void PinchFriction::initPhysics()
 void PinchFriction::exitPhysics()
 {
     //cleanup in the reverse order of creation/initialization
-    
+    removePickingConstraint();
     //remove the rigidbodies from the dynamics world and delete them
     int i;
     for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)

examples/DeformableDemo/SplitImpulse.cpp

@@ -21,16 +21,15 @@
 #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
 #include <stdio.h>  //printf debugging
 
-#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../CommonInterfaces/CommonDeformableBodyBase.h"
 #include "../Utils/b3ResourcePath.h"
 
 ///The SplitImpulse shows the effect of split impulse in deformable rigid contact.
-class SplitImpulse : public CommonRigidBodyBase
+class SplitImpulse : public CommonDeformableBodyBase
 {
-    btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
 public:
 	SplitImpulse(struct GUIHelperInterface* helper)
-		: CommonRigidBodyBase(helper)
+		: CommonDeformableBodyBase(helper)
 	{
 	}
 	virtual ~SplitImpulse()
@@ -69,23 +68,9 @@ public:
         createRigidBody(mass, startTransform, shape[0]);
     }
     
-    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();
+        CommonDeformableBodyBase::renderScene();
         btDeformableMultiBodyDynamicsWorld* deformableWorld = getDeformableDynamicsWorld();
         
         for (int i = 0; i < deformableWorld->getSoftBodyArray().size(); i++)
@@ -202,7 +187,7 @@ void SplitImpulse::initPhysics()
 void SplitImpulse::exitPhysics()
 {
 	//cleanup in the reverse order of creation/initialization
-
+    removePickingConstraint();
 	//remove the rigidbodies from the dynamics world and delete them
 	int i;
 	for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)

examples/DeformableDemo/VolumetricDeformable.cpp

@@ -23,27 +23,28 @@
 #include "../CommonInterfaces/CommonParameterInterface.h"
 #include <stdio.h>  //printf debugging
 
-#include "../CommonInterfaces/CommonRigidBodyBase.h"
+#include "../CommonInterfaces/CommonDeformableBodyBase.h"
 #include "../Utils/b3ResourcePath.h"
 
 ///The VolumetricDeformable shows the contact between volumetric deformable objects and rigid objects.
 static btScalar E = 100;
 static btScalar nu = 0.3;
-static btScalar damping = 0.1;
+static btScalar damping = 0.01;
 
 struct TetraCube
 {
 #include "../SoftDemo/cube.inl"
 };
 
-class VolumetricDeformable : public CommonRigidBodyBase
+class VolumetricDeformable : public CommonDeformableBodyBase
 {
-    btAlignedObjectArray<btDeformableLagrangianForce*> m_forces;
 	btDeformableNeoHookeanForce* m_neohookean;
+
 public:
 	VolumetricDeformable(struct GUIHelperInterface* helper)
-		: CommonRigidBodyBase(helper)
+		: CommonDeformableBodyBase(helper)
 	{
+        m_neohookean = 0;
 	}
 	virtual ~VolumetricDeformable()
 	{
@@ -67,8 +68,8 @@ public:
 		m_neohookean->setYoungsModulus(E);
 		m_neohookean->setDamping(damping);
         //use a smaller internal timestep, there are stability issues
-        float internalTimeStep = 1. / 240.f;
-        m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
+        float internalTimeStep = 1. / 600.f;
+        m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep);
     }
     
     void createStaticBox(const btVector3& halfEdge, const btVector3& translation)
@@ -125,37 +126,20 @@ public:
         }
     }
     
-    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();
+        CommonDeformableBodyBase::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());
             }
         }
     }
-	
-	virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);
 };
 
 void VolumetricDeformable::initPhysics()
@@ -237,7 +221,7 @@ void VolumetricDeformable::initPhysics()
         getDeformableDynamicsWorld()->addForce(psb, gravity_force);
         m_forces.push_back(gravity_force);
         
-        btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(30,100,0.05);
+        btDeformableNeoHookeanForce* neohookean = new btDeformableNeoHookeanForce(30,100,0.01);
 		m_neohookean = neohookean;
         getDeformableDynamicsWorld()->addForce(psb, neohookean);
         m_forces.push_back(neohookean);
@@ -258,7 +242,7 @@ void VolumetricDeformable::initPhysics()
 	}
 	{
 		SliderParams slider("Poisson Ratio", &nu);
-		slider.m_minVal = 0;
+		slider.m_minVal = 0.1;
 		slider.m_maxVal = 0.4;
 		if (m_guiHelper->getParameterInterface())
 			m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
@@ -266,73 +250,16 @@ void VolumetricDeformable::initPhysics()
 	{
 		SliderParams slider("Damping", &damping);
 		slider.m_minVal = 0.01;
-		slider.m_maxVal = 1;
+		slider.m_maxVal = 0.2;
 		if (m_guiHelper->getParameterInterface())
 			m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
 	}
 }
 
-bool VolumetricDeformable::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
-{
-	if (getDeformableDynamicsWorld() == 0)
-		return false;
-	
-	btCollisionWorld::ClosestRayResultCallbackWithInfo rayCallback(rayFromWorld, rayToWorld);
-	
-	rayCallback.m_flags |= btTriangleRaycastCallback::kF_UseGjkConvexCastRaytest;
-	getDeformableDynamicsWorld()->rayTest(rayFromWorld, rayToWorld, rayCallback);
-	if (rayCallback.hasHit())
-	{
-		btVector3 pickPos = rayCallback.m_hitPointWorld;
-		btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject);
-		if (body)
-		{
-			//other exclusions?
-			if (!(body->isStaticObject() || body->isKinematicObject()))
-			{
-				m_pickedBody = body;
-				m_savedState = m_pickedBody->getActivationState();
-				m_pickedBody->setActivationState(DISABLE_DEACTIVATION);
-				//printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
-				btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
-				btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot);
-				m_dynamicsWorld->addConstraint(p2p, true);
-				m_pickedConstraint = p2p;
-				btScalar mousePickClamping = 30.f;
-				p2p->m_setting.m_impulseClamp = mousePickClamping;
-				//very weak constraint for picking
-				p2p->m_setting.m_tau = 0.001f;
-			}
-		}
-		btSoftBody* psb = (btSoftBody*)btSoftBody::upcast(rayCallback.m_collisionObject);
-		if (psb)
-		{
-			m_savedState = psb->getActivationState();
-			m_pickedBody->setActivationState(DISABLE_DEACTIVATION);
-//			btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
-//			btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot);
-//			m_dynamicsWorld->addConstraint(p2p, true);
-//			m_pickedConstraint = p2p;
-//			btScalar mousePickClamping = 30.f;
-//			p2p->m_setting.m_impulseClamp = mousePickClamping;
-//			//very weak constraint for picking
-//			p2p->m_setting.m_tau = 0.001f;
-		}
-		
-		//					pickObject(pickPos, rayCallback.m_collisionObject);
-		m_oldPickingPos = rayToWorld;
-		m_hitPos = pickPos;
-		m_oldPickingDist = (pickPos - rayFromWorld).length();
-		//					printf("hit !\n");
-		//add p2p
-	}
-	return false;
-}
-
 void VolumetricDeformable::exitPhysics()
 {
 	//cleanup in the reverse order of creation/initialization
-
+    removePickingConstraint();
 	//remove the rigidbodies from the dynamics world and delete them
 	int i;
 	for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)

src/BulletSoftBody/btDeformableBackwardEulerObjective.h

@@ -20,6 +20,7 @@
 #include "btDeformableMassSpringForce.h"
 #include "btDeformableGravityForce.h"
 #include "btDeformableCorotatedForce.h"
+#include "btDeformableMousePickingForce.h"
 #include "btDeformableLinearElasticityForce.h"
 #include "btDeformableNeoHookeanForce.h"
 #include "btDeformableContactProjection.h"

src/BulletSoftBody/btDeformableLagrangianForce.h

@@ -26,7 +26,8 @@ enum btDeformableLagrangianForceType
     BT_MASSSPRING_FORCE = 2,
     BT_COROTATED_FORCE = 3,
     BT_NEOHOOKEAN_FORCE = 4,
-    BT_LINEAR_ELASTICITY_FORCE = 5
+    BT_LINEAR_ELASTICITY_FORCE = 5,
+    BT_MOUSE_PICKING_FORCE = 6
 };
 
 static inline double randomDouble(double low, double high)
@@ -85,6 +86,11 @@ public:
         m_softBodies.push_back(psb);
     }
     
+    virtual void removeSoftBody(btSoftBody* psb)
+    {
+        m_softBodies.remove(psb);
+    }
+    
     virtual void setIndices(const btAlignedObjectArray<btSoftBody::Node*>* nodes)
     {
         m_nodes = nodes;

src/BulletSoftBody/btDeformableMousePickingForce.h

@@ -0,0 +1,143 @@
+/*
+ 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_MOUSE_PICKING_FORCE_H
+#define BT_MOUSE_PICKING_FORCE_H
+
+#include "btDeformableLagrangianForce.h"
+
+class btDeformableMousePickingForce : 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
+    btScalar m_elasticStiffness, m_dampingStiffness;
+    const btSoftBody::Face& m_face;
+    btVector3 m_mouse_pos;
+    btScalar m_maxForce;
+public:
+    typedef btAlignedObjectArray<btVector3> TVStack;
+    btDeformableMousePickingForce(btScalar k, btScalar d, const btSoftBody::Face& face, btVector3 mouse_pos, btScalar maxForce = 0.3) : m_elasticStiffness(k), m_dampingStiffness(d), m_face(face), m_mouse_pos(mouse_pos),  m_maxForce(maxForce)
+    {
+    }
+    
+    virtual void addScaledForces(btScalar scale, TVStack& force)
+    {
+        addScaledDampingForce(scale, force);
+        addScaledElasticForce(scale, force);
+    }
+    
+    virtual void addScaledExplicitForce(btScalar scale, TVStack& force)
+    {
+        addScaledElasticForce(scale, force);
+    }
+    
+    virtual void addScaledDampingForce(btScalar scale, TVStack& force)
+    {
+        for (int i = 0; i < 3; ++i)
+        {
+            btVector3 v_diff = m_face.m_n[i]->m_v;
+            btVector3 scaled_force = scale * m_dampingStiffness * v_diff;
+            if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON)
+            {
+                btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized();
+                scaled_force = scale * m_dampingStiffness * v_diff.dot(dir) * dir;
+            }
+            force[m_face.m_n[i]->index] -= scaled_force;
+        }
+    }
+    
+    virtual void addScaledElasticForce(btScalar scale, TVStack& force)
+    {
+        btScalar scaled_stiffness = scale * m_elasticStiffness;
+        for (int i = 0; i < 3; ++i)
+        {
+            btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos);
+            btVector3 scaled_force = scaled_stiffness * dir;
+            if (scaled_force.safeNorm() > m_maxForce)
+            {
+                scaled_force.safeNormalize();
+                scaled_force *= m_maxForce;
+            }
+            force[m_face.m_n[i]->index] -= scaled_force;
+        }
+    }
+    
+    virtual void addScaledDampingForceDifferential(btScalar scale, const TVStack& dv, TVStack& df)
+    {
+        btScalar scaled_k_damp = m_dampingStiffness * scale;
+        for (int i = 0; i < 3; ++i)
+        {
+            btVector3 local_scaled_df = scaled_k_damp * dv[m_face.m_n[i]->index];
+            if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON)
+            {
+                btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized();
+                local_scaled_df= scaled_k_damp * dv[m_face.m_n[i]->index].dot(dir) * dir;
+            }
+            df[m_face.m_n[i]->index] -= local_scaled_df;
+        }
+    }
+    
+    virtual double totalElasticEnergy(btScalar dt)
+    {
+        double energy = 0;
+        for (int i = 0; i < 3; ++i)
+        {
+            btVector3 dir = (m_face.m_n[i]->m_q - m_mouse_pos);
+            btVector3 scaled_force = m_elasticStiffness * dir;
+            if (scaled_force.safeNorm() > m_maxForce)
+            {
+                scaled_force.safeNormalize();
+                scaled_force *= m_maxForce;
+            }
+            energy += 0.5 * scaled_force.dot(dir);
+        }
+        return energy;
+    }
+    
+    virtual double totalDampingEnergy(btScalar dt)
+    {
+        double energy = 0;
+        for (int i = 0; i < 3; ++i)
+        {
+            btVector3 v_diff = m_face.m_n[i]->m_v;
+            btVector3 scaled_force = m_dampingStiffness * v_diff;
+            if ((m_face.m_n[i]->m_x - m_mouse_pos).norm() > SIMD_EPSILON)
+            {
+                btVector3 dir = (m_face.m_n[i]->m_x - m_mouse_pos).normalized();
+                scaled_force = m_dampingStiffness * v_diff.dot(dir) * dir;
+            }
+            energy -= scaled_force.dot(m_face.m_n[i]->m_v) / dt;
+        }
+        return energy;
+    }
+    
+    virtual void addScaledElasticForceDifferential(btScalar scale, const TVStack& dx, TVStack& df)
+    {
+        //TODO
+    }
+    
+    void setMousePos(const btVector3& p)
+    {
+        m_mouse_pos = p;
+    }
+    
+    virtual btDeformableLagrangianForceType getForceType()
+    {
+        return BT_MOUSE_PICKING_FORCE;
+    }
+    
+};
+
+#endif /* btMassSpring_h */

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp

@@ -688,6 +688,24 @@ void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableL
     }
 }
 
+void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformableLagrangianForce* force)
+{
+    btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
+    int removed_index = -1;
+    for (int i = 0; i < forces.size(); ++i)
+    {
+        if (forces[i]->getForceType() == force->getForceType())
+        {
+            forces[i]->removeSoftBody(psb);
+            if (forces[i]->m_softBodies.size() == 0)
+                removed_index = i;
+            break;
+        }
+    }
+    if (removed_index >= 0)
+        forces.removeAtIndex(removed_index);
+}
+
 void btDeformableMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body)
 {
     m_softBodies.remove(body);

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h

@@ -131,6 +131,8 @@ public:
     
     void addForce(btSoftBody* psb, btDeformableLagrangianForce* force);
     
+    void removeForce(btSoftBody* psb, btDeformableLagrangianForce* force);
+    
     void removeSoftBody(btSoftBody* body);
     
     void removeCollisionObject(btCollisionObject* collisionObject);
@@ -267,7 +269,7 @@ public:
             if (softBody)
             {
                 btSoftBody::sRayCast softResult;
-                if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult))
+                if (softBody->rayFaceTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult))
                 {
                     if (softResult.fraction <= resultCallback.m_closestHitFraction)
                     {
@@ -278,8 +280,6 @@ public:
                         btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
                         btVector3 normal = -rayDir;
                         normal.normalize();
-                        
-                        if (softResult.feature == btSoftBody::eFeature::Face)
                         {
                             normal = softBody->m_faces[softResult.index].m_normal;
                             if (normal.dot(rayDir) > 0)

src/BulletSoftBody/btSoftBody.cpp

@@ -1936,6 +1936,25 @@ bool btSoftBody::rayTest(const btVector3& rayFrom,
 	return (rayTest(rayFrom, rayTo, results.fraction, results.feature, results.index, false) != 0);
 }
 
+bool btSoftBody::rayFaceTest(const btVector3& rayFrom,
+                         const btVector3& rayTo,
+                         sRayCast& results)
+{
+	if (m_faces.size() == 0)
+		return false;
+	else
+	{
+    	if (m_fdbvt.empty())
+        	initializeFaceTree();
+	}
+    
+    results.body = this;
+    results.fraction = 1.f;
+    results.index = -1;
+    
+    return (rayFaceTest(rayFrom, rayTo, results.fraction,  results.index) != 0);
+}
+
 //
 void btSoftBody::setSolver(eSolverPresets::_ preset)
 {
@@ -2441,6 +2460,25 @@ int btSoftBody::rayTest(const btVector3& rayFrom, const btVector3& rayTo,
 	return (cnt);
 }
 
+int btSoftBody::rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo,
+						btScalar& mint, int& index) const
+{
+	int cnt = 0;
+	{ /* Use dbvt	*/
+		RayFromToCaster collider(rayFrom, rayTo, mint);
+		
+		btDbvt::rayTest(m_fdbvt.m_root, rayFrom, rayTo, collider);
+		if (collider.m_face)
+		{
+			mint = collider.m_mint;
+			index = (int)(collider.m_face - &m_faces[0]);
+			cnt = 1;
+		}
+	}
+	return (cnt);
+}
+
+
 //
 static inline btDbvntNode* copyToDbvnt(const btDbvtNode* n)
 {

src/BulletSoftBody/btSoftBody.h

@@ -1031,6 +1031,11 @@ public:
 	bool rayTest(const btVector3& rayFrom,
 				 const btVector3& rayTo,
 				 sRayCast& results);
+	bool rayFaceTest(const btVector3& rayFrom,
+					 const btVector3& rayTo,
+					 sRayCast& results);
+	int rayFaceTest(const btVector3& rayFrom, const btVector3& rayTo,
+					btScalar& mint, int& index) const;
 	/* Solver presets														*/
 	void setSolver(eSolverPresets::_ preset);
 	/* predictMotion														*/