From a7b8b54c443fa4c65bd1264030b0e4a40015c8f7 Mon Sep 17 00:00:00 2001
From: Erwin Coumans <erwin.coumans@gmail.com>
Date: Tue, 16 Jun 2020 11:54:50 -0700
Subject: [PATCH] implement mouse picking for deformable objects in PyBullet
 (the one using p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD), not position
 based dynamics version)

---
 .../PhysicsServerCommandProcessor.cpp         | 98 ++++++++++++++++++-
 1 file changed, 96 insertions(+), 2 deletions(-)

diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 13e6e3ab1..7cf118a91 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -1655,7 +1655,9 @@ struct PhysicsServerCommandProcessorInternalData
 
 
 #ifndef SKIP_DEFORMABLE_BODY
-	
+	btSoftBody* m_pickedSoftBody;
+	btDeformableMousePickingForce* m_mouseForce;
+	btScalar m_maxPickingForce;
 	btDeformableBodySolver* m_deformablebodySolver;
     btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
 #endif
@@ -1685,6 +1687,7 @@ struct PhysicsServerCommandProcessorInternalData
 
 	//data for picking objects
 	class btRigidBody* m_pickedBody;
+	
 	int m_savedActivationState;
 	class btTypedConstraint* m_pickedConstraint;
 	class btMultiBodyPoint2Point* m_pickingMultiBodyPoint2Point;
@@ -1727,6 +1730,9 @@ struct PhysicsServerCommandProcessorInternalData
 		  m_solver(0),
 		  m_collisionConfiguration(0),
 #ifndef SKIP_DEFORMABLE_BODY
+		  m_pickedSoftBody(0),
+		  m_mouseForce(0),
+		  m_maxPickingForce(0.3),
 		  m_deformablebodySolver(0),
 #endif
 		  m_dynamicsWorld(0),
@@ -13775,10 +13781,15 @@ void PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags)
 	}
 }
 
+
+
 struct MyResultCallback : public btCollisionWorld::ClosestRayResultCallback
 {
+	int m_faceId;
+
 	MyResultCallback(const btVector3& rayFromWorld, const btVector3& rayToWorld)
-		: btCollisionWorld::ClosestRayResultCallback(rayFromWorld, rayToWorld)
+		: btCollisionWorld::ClosestRayResultCallback(rayFromWorld, rayToWorld),
+		m_faceId(-1)
 	{
 	}
 
@@ -13786,8 +13797,37 @@ struct MyResultCallback : public btCollisionWorld::ClosestRayResultCallback
 	{
 		return true;
 	}
+
+	virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult, bool normalInWorldSpace)
+	{
+		//caller already does the filter on the m_closestHitFraction
+		btAssert(rayResult.m_hitFraction <= m_closestHitFraction);
+
+		m_closestHitFraction = rayResult.m_hitFraction;
+		m_collisionObject = rayResult.m_collisionObject;
+		if (rayResult.m_localShapeInfo)
+		{
+			m_faceId = rayResult.m_localShapeInfo->m_triangleIndex;
+		}
+		else
+		{
+			m_faceId = -1;
+		}
+		if (normalInWorldSpace)
+		{
+			m_hitNormalWorld = rayResult.m_hitNormalLocal;
+		}
+		else
+		{
+			///need to transform normal into worldspace
+			m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis() * rayResult.m_hitNormalLocal;
+		}
+		m_hitPointWorld.setInterpolate3(m_rayFromWorld, m_rayToWorld, rayResult.m_hitFraction);
+		return rayResult.m_hitFraction;
+	}
 };
 
+
 bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
 {
 	if (m_data->m_dynamicsWorld == 0)
@@ -13843,6 +13883,31 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons
 				world->addMultiBodyConstraint(p2p);
 				m_data->m_pickingMultiBodyPoint2Point = p2p;
 			}
+			else
+			{
+#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
+				//deformable/soft body?
+				btSoftBody* psb = (btSoftBody*)btSoftBody::upcast(rayCallback.m_collisionObject);
+				if (psb)
+				{
+					btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
+					if (deformWorld)
+					{
+						int face_id = rayCallback.m_faceId;
+						if (face_id >= 0 && face_id < psb->m_faces.size())
+						{
+							m_data->m_pickedSoftBody = psb;
+							psb->setActivationState(DISABLE_DEACTIVATION);
+							const btSoftBody::Face& f = psb->m_faces[face_id];
+							btDeformableMousePickingForce* mouse_force = new btDeformableMousePickingForce(100, 0, f, pickPos, m_data->m_maxPickingForce);
+							m_data->m_mouseForce = mouse_force;
+
+							deformWorld->addForce(psb, mouse_force);
+						}
+					}
+				}
+#endif
+			}
 		}
 
 		//					pickObject(pickPos, rayCallback.m_collisionObject);
@@ -13886,6 +13951,21 @@ bool PhysicsServerCommandProcessor::movePickedBody(const btVector3& rayFromWorld
 		m_data->m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB);
 	}
 
+#ifndef SKIP_DEFORMABLE_BODY
+	if (m_data->m_pickedSoftBody)
+	{
+		if (m_data->m_pickedSoftBody && m_data->m_mouseForce)
+		{
+			btVector3 newPivot;
+			btVector3 dir = rayToWorld - rayFromWorld;
+			dir.normalize();
+			dir *= m_data->m_oldPickingDist;
+			newPivot = rayFromWorld + dir;
+			m_data->m_mouseForce->setMousePos(newPivot);
+		}
+	}
+#endif
+
 	return false;
 }
 
@@ -13907,6 +13987,20 @@ void PhysicsServerCommandProcessor::removePickingConstraint()
 		delete m_data->m_pickingMultiBodyPoint2Point;
 		m_data->m_pickingMultiBodyPoint2Point = 0;
 	}
+
+
+#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
+	//deformable/soft body?
+	btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
+	if (deformWorld && m_data->m_mouseForce)
+	{
+		deformWorld->removeForce(m_data->m_pickedSoftBody, m_data->m_mouseForce);
+		delete m_data->m_mouseForce;
+		m_data->m_mouseForce = 0;
+		m_data->m_pickedSoftBody = 0;
+	}
+#endif
+
 }
 
 void PhysicsServerCommandProcessor::enableCommandLogging(bool enable, const char* fileName)