add picking to shared memory physics server

examples/SharedMemory/PhysicsClientExample.cpp

@@ -112,6 +112,28 @@ public:
 			}
 		}
 
+	}
+
+	void prepareControlCommand(SharedMemoryCommand& command)
+	{
+		int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE;
+
+        command.m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
+            
+        for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
+        {
+            command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0;
+            command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 1000;
+        }
+        for (int i=0;i<m_numMotors;i++)
+        {
+            btScalar targetVel = m_motorTargetVelocities[i].m_velTarget;
+                
+            int uIndex = m_motorTargetVelocities[i].m_uIndex;
+            command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel;
+                
+        }
+            
 	}
 	virtual void	physicsDebugDraw(int debugFlags){}
 	virtual bool	mouseMoveCallback(float x,float y){return false;};
@@ -188,28 +210,9 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr)
 		{
 			
             command.m_type =CMD_SEND_DESIRED_STATE;
-            int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE;
+            
+			cl->prepareControlCommand(command);
 
-            command.m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
-            
-            for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
-            {
-                command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0;
-                command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 1000;
-            }
-            for (int i=0;i<cl->m_numMotors;i++)
-            {
-                btScalar targetVel = cl->m_motorTargetVelocities[i].m_velTarget;
-                
-                int uIndex = cl->m_motorTargetVelocities[i].m_uIndex;
-                if (targetVel>1)
-                {
-                    printf("testme");
-                }
-                command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel;
-                
-            }
-            
 			cl->enqueueCommand(command);		
 			break;
 		}
@@ -239,7 +242,7 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr)
 		m_userCommandRequests.push_back(orgCommand);
 		SharedMemoryCommand& cmd = m_userCommandRequests[m_userCommandRequests.size()-1];
 
-		b3Printf("User put command request %d on queue (queue length = %d)\n",cmd.m_type, m_userCommandRequests.size());
+		//b3Printf("User put command request %d on queue (queue length = %d)\n",cmd.m_type, m_userCommandRequests.size());
 	}
 
 
@@ -376,7 +379,7 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
 		{
 			if (m_userCommandRequests.size())
 			{
-				b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
+				//b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
 				SharedMemoryCommand command = m_userCommandRequests[0];
 
 				//a manual 'pop_front', we don't use 'remove' because it will re-order the commands
@@ -396,6 +399,21 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
 				}
 				
 				m_physicsClient.submitClientCommand(command);
+			}  else
+			{
+				if (m_numMotors)
+				{
+					SharedMemoryCommand command;
+					command.m_type =CMD_SEND_DESIRED_STATE;
+					prepareControlCommand(command);
+					enqueueCommand(command);		
+
+					command.m_type =CMD_STEP_FORWARD_SIMULATION;
+					enqueueCommand(command);
+
+					command.m_type = CMD_REQUEST_ACTUAL_STATE;
+					enqueueCommand(command);
+				}
 			}
 		}
 	}

examples/SharedMemory/PhysicsServer.cpp

@@ -103,6 +103,16 @@ struct PhysicsServerInternalData
 
 	bool m_verboseOutput;
 	
+	
+	//data for picking objects
+	class btRigidBody*	m_pickedBody;
+	class btTypedConstraint* m_pickedConstraint;
+	class btMultiBodyPoint2Point*		m_pickingMultiBodyPoint2Point;
+	btVector3 m_oldPickingPos;
+	btVector3 m_hitPos;
+	btScalar m_oldPickingDist;
+	bool m_prevCanSleep;
+
 	PhysicsServerInternalData()
 		:m_sharedMemory(0),
 		m_testBlock1(0),
@@ -112,7 +122,10 @@ struct PhysicsServerInternalData
 		m_debugDrawer(0),
 		m_guiHelper(0),
 		m_sharedMemoryKey(SHARED_MEMORY_KEY),
-		m_verboseOutput(false)
+		m_verboseOutput(false),
+		m_pickedBody(0),
+		m_pickedConstraint(0),
+		m_pickingMultiBodyPoint2Point(0)
 	{
         m_rootLocalInertialFrame.setIdentity();
 	}
@@ -1160,3 +1173,123 @@ void    PhysicsServerSharedMemory::physicsDebugDraw(int debugDrawFlags)
 	}
 #endif
 }
+
+
+bool PhysicsServerSharedMemory::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
+{
+	if (m_data->m_dynamicsWorld==0)
+		return false;
+
+	btCollisionWorld::ClosestRayResultCallback rayCallback(rayFromWorld, rayToWorld);
+
+	m_data->m_dynamicsWorld->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_data->m_pickedBody = body;
+				m_data->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_data->m_dynamicsWorld->addConstraint(p2p, true);
+				m_data->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
+		{
+			btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject);
+			if (multiCol && multiCol->m_multiBody)
+			{
+						
+				m_data->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_data->m_dynamicsWorld;
+				world->addMultiBodyConstraint(p2p);
+				m_data->m_pickingMultiBodyPoint2Point =p2p; 
+			}
+		}
+
+
+
+		//					pickObject(pickPos, rayCallback.m_collisionObject);
+		m_data->m_oldPickingPos = rayToWorld;
+		m_data->m_hitPos = pickPos;
+		m_data->m_oldPickingDist = (pickPos - rayFromWorld).length();
+		//					printf("hit !\n");
+		//add p2p
+	}
+	return false;
+}
+bool PhysicsServerSharedMemory::movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
+{
+	if (m_data->m_pickedBody  && m_data->m_pickedConstraint)
+	{
+		btPoint2PointConstraint* pickCon = static_cast<btPoint2PointConstraint*>(m_data->m_pickedConstraint);
+		if (pickCon)
+		{
+			//keep it at the same picking distance
+		
+			btVector3 dir = rayToWorld-rayFromWorld;
+			dir.normalize();
+			dir *= m_data->m_oldPickingDist;
+
+			btVector3 newPivotB = rayFromWorld + dir;
+			pickCon->setPivotB(newPivotB);
+		}
+	}
+		
+	if (m_data->m_pickingMultiBodyPoint2Point)
+	{
+		//keep it at the same picking distance
+
+		
+		btVector3 dir = rayToWorld-rayFromWorld;
+		dir.normalize();
+		dir *= m_data->m_oldPickingDist;
+
+		btVector3 newPivotB = rayFromWorld + dir;
+			
+		m_data->m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB);
+	}
+		
+	return false;
+}
+void PhysicsServerSharedMemory::removePickingConstraint()
+{
+	if (m_data->m_pickedConstraint)
+	{
+		m_data->m_dynamicsWorld->removeConstraint(m_data->m_pickedConstraint);
+		delete m_data->m_pickedConstraint;
+		m_data->m_pickedConstraint = 0;
+		m_data->m_pickedBody = 0;
+	}
+	if (m_data->m_pickingMultiBodyPoint2Point)
+	{
+		m_data->m_pickingMultiBodyPoint2Point->getMultiBodyA()->setCanSleep(m_data->m_prevCanSleep);
+		btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
+		world->removeMultiBodyConstraint(m_data->m_pickingMultiBodyPoint2Point);
+		delete m_data->m_pickingMultiBodyPoint2Point;
+		m_data->m_pickingMultiBodyPoint2Point = 0;
+	}
+}

examples/SharedMemory/PhysicsServer.h

@@ -42,6 +42,12 @@ public:
 
 	bool	supportsJointMotor(class btMultiBody* body, int linkIndex);
 
+	//@todo(erwincoumans) Should we have shared memory commands for picking objects?
+	///The pickBody method will try to pick the first body along a ray, return true if succeeds, false otherwise
+	virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);
+	virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);
+	virtual void removePickingConstraint();
+
 	//for physicsDebugDraw and renderScene are mainly for debugging purposes
 	//and for physics visualization. The idea is that physicsDebugDraw can also send wireframe
 	//to a physics client, over shared memory

examples/SharedMemory/PhysicsServerExample.cpp

@@ -47,8 +47,65 @@ public:
 	virtual void    exitPhysics(){}
 
 	virtual void	physicsDebugDraw(int debugFlags);
-	virtual bool	mouseMoveCallback(float x,float y){return false;};
+	
-	virtual bool	mouseButtonCallback(int button, int state, float x, float y){return false;}
+	btVector3	getRayTo(int x,int y);
+	
+	virtual bool	mouseMoveCallback(float x,float y)
+	{
+		CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();
+		
+		if (!renderer)
+		{
+			btAssert(0);
+			return false;
+		}
+
+		btVector3 rayTo = getRayTo(int(x), int(y));
+		btVector3 rayFrom;
+		renderer->getActiveCamera()->getCameraPosition(rayFrom);
+		m_physicsServer.movePickedBody(rayFrom,rayTo);
+		return false;
+	};
+
+	virtual bool	mouseButtonCallback(int button, int state, float x, float y)
+	{
+		CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();
+		
+		if (!renderer)
+		{
+			btAssert(0);
+			return false;
+		}
+		
+		CommonWindowInterface* window = m_guiHelper->getAppInterface()->m_window;
+
+	
+		if (state==1)
+		{
+			if(button==0 && (!window->isModifierKeyPressed(B3G_ALT) && !window->isModifierKeyPressed(B3G_CONTROL) ))
+			{
+				btVector3 camPos;
+				renderer->getActiveCamera()->getCameraPosition(camPos);
+
+				btVector3 rayFrom = camPos;
+				btVector3 rayTo = getRayTo(int(x),int(y));
+
+				m_physicsServer.pickBody(rayFrom, rayTo);
+
+
+			}
+		} else
+		{
+			if (button==0)
+			{
+				m_physicsServer.removePickingConstraint();
+				//remove p2p
+			}
+		}
+
+		//printf("button=%d, state=%d\n",button,state);
+		return false;
+	}
 	virtual bool	keyboardCallback(int key, int state){return false;}
 
 	virtual void setSharedMemoryKey(int key)
@@ -142,4 +199,67 @@ class CommonExampleInterface*    PhysicsServerCreateFunc(struct CommonExampleOpt
 	
 }
 
+btVector3	PhysicsServerExample::getRayTo(int x,int y)
+{
+	CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();
+		
+	if (!renderer)
+	{
+		btAssert(0);
+		return btVector3(0,0,0);
+	}
+
+	float top = 1.f;
+	float bottom = -1.f;
+	float nearPlane = 1.f;
+	float tanFov = (top-bottom)*0.5f / nearPlane;
+	float fov = btScalar(2.0) * btAtan(tanFov);
+
+	btVector3 camPos,camTarget;
+	renderer->getActiveCamera()->getCameraPosition(camPos);
+	renderer->getActiveCamera()->getCameraTargetPosition(camTarget);
+
+	btVector3	rayFrom = camPos;
+	btVector3 rayForward = (camTarget-camPos);
+	rayForward.normalize();
+	float farPlane = 10000.f;
+	rayForward*= farPlane;
+
+	btVector3 rightOffset;
+	btVector3 cameraUp=btVector3(0,0,0);
+	cameraUp[m_guiHelper->getAppInterface()->getUpAxis()]=1;
+
+	btVector3 vertical = cameraUp;
+
+	btVector3 hor;
+	hor = rayForward.cross(vertical);
+	hor.normalize();
+	vertical = hor.cross(rayForward);
+	vertical.normalize();
+
+	float tanfov = tanf(0.5f*fov);
+
+
+	hor *= 2.f * farPlane * tanfov;
+	vertical *= 2.f * farPlane * tanfov;
+
+	btScalar aspect;
+	float width = float(renderer->getScreenWidth());
+	float height = float (renderer->getScreenHeight());
+
+	aspect =  width / height;
+
+	hor*=aspect;
+
+
+	btVector3 rayToCenter = rayFrom + rayForward;
+	btVector3 dHor = hor * 1.f/width;
+	btVector3 dVert = vertical * 1.f/height;
+
+
+	btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
+	rayTo += btScalar(x) * dHor;
+	rayTo -= btScalar(y) * dVert;
+	return rayTo;
+}