Merge pull request #768 from erwincoumans/master

implement pybullet.getContactPointData(), two optional object unique …

build3/premake4.lua

@@ -96,7 +96,7 @@ end
 
 		
 if os.is("Windows") then
- 		default_python_include_dir = "C:\Python-3.5.2/include"
+ 		default_python_include_dir = "C:/Python-3.5.2/include"
  		default_python_lib_dir = "C:/Python-3.5.2/libs"
 end
 

build_visual_studio.bat

@@ -3,6 +3,8 @@ rem premake4 --with-pe  vs2010
 rem premake4  --bullet2demos vs2010
 cd build3
 premake4   --enable_openvr  --targetdir="../bin" vs2010 
+
+rem premake4   --enable_pybullet --python_include_dir="c:/python-3.5.2/include" --python_lib_dir="c:/python-3.5.2/libs" --enable_openvr  --targetdir="../bin" vs2010 
 rem premake4   --enable_openvr --enable_pybullet --targetdir="../bin" vs2010 
 rem premake4   --targetdir="../server2bin" vs2010 
 rem cd vs2010

examples/ExampleBrowser/OpenGLExampleBrowser.cpp

@@ -893,7 +893,6 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
 		GL3TexLoader* myTexLoader = new GL3TexLoader;
 		m_internalData->m_myTexLoader = myTexLoader;
 
-		sth_stash* fontstash = simpleApp->getFontStash();
 		
 		if (sUseOpenGL2)
 		{

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -39,7 +39,6 @@ struct PhysicsClientSharedMemoryInternalData {
 	btAlignedObjectArray<int> m_cachedSegmentationMaskBuffer;
 
     btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
-    btAlignedObjectArray<b3ContactPointDynamicsData>m_cachedContactPointDynamics;
 
     btAlignedObjectArray<int> m_bodyIdsRequestInfo;
     SharedMemoryStatus m_tempBackupServerStatus;
@@ -572,7 +571,6 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
 					int numContactsCopied = serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
 
 					m_data->m_cachedContactPoints.resize(startContactIndex+numContactsCopied);
-                    m_data->m_cachedContactPointDynamics.resize(0);
                     
 					b3ContactPointData* contactData = (b3ContactPointData*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
 
@@ -653,8 +651,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
         
         if (serverCmd.m_type == CMD_CONTACT_POINT_INFORMATION_COMPLETED)
         {
-            //todo: request remaining points, if needed
+            SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
-			SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
 			if (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints>0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied)
 			{
 				command.m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION;
@@ -765,7 +762,6 @@ void PhysicsClientSharedMemory::getCachedCameraImage(struct b3CameraImageData* c
 void PhysicsClientSharedMemory::getCachedContactPointInformation(struct b3ContactInformation* contactPointData)
 {
 	contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size();
-	contactPointData->m_contactDynamicsData = 0;
 	contactPointData->m_contactPointData = contactPointData->m_numContactPoints? &m_data->m_cachedContactPoints[0] : 0;
 
 }

examples/SharedMemory/PhysicsDirect.cpp

@@ -39,6 +39,8 @@ struct PhysicsDirectInternalData
 	btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
 	btAlignedObjectArray<int> m_cachedSegmentationMask;
 	
+    btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
+    
 
 
 	PhysicsServerCommandProcessor* m_commandProcessor;
@@ -193,6 +195,51 @@ bool PhysicsDirect::processDebugLines(const struct SharedMemoryCommand& orgComma
 	return m_data->m_hasStatus;
 }
 
+bool PhysicsDirect::processContactPointData(const struct SharedMemoryCommand& orgCommand)
+{
+    SharedMemoryCommand command = orgCommand;
+    
+    const SharedMemoryStatus& serverCmd = m_data->m_serverStatus;
+    
+    do
+    {
+        bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+        m_data->m_hasStatus = hasStatus;
+        if (hasStatus)
+        {
+            if (m_data->m_verboseOutput)
+            {
+                b3Printf("Contact Point Information Request OK\n");
+            }
+            int startContactIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex;
+            int numContactsCopied = serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
+            
+            m_data->m_cachedContactPoints.resize(startContactIndex+numContactsCopied);
+            
+            b3ContactPointData* contactData = (b3ContactPointData*)&m_data->m_bulletStreamDataServerToClient[0];
+            
+            for (int i=0;i<numContactsCopied;i++)
+            {
+                m_data->m_cachedContactPoints[startContactIndex+i] = contactData[i];
+            }
+            
+            if (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints>0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied)
+            {
+                command.m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION;
+                command.m_requestContactPointArguments.m_startingContactPointIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex+serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
+                command.m_requestContactPointArguments.m_objectAIndexFilter = -1;
+                command.m_requestContactPointArguments.m_objectBIndexFilter = -1;
+                
+            }
+
+        }
+    } while (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints > 0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied);
+    
+    return m_data->m_hasStatus;
+
+}
+
+
 bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand)
 {
 	SharedMemoryCommand command = orgCommand;
@@ -326,6 +373,10 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
 	{
 		return processCamera(command);
 	}
+    if (command.m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION)
+    {
+        return processContactPointData(command);
+    }
 
 	bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
 	m_data->m_hasStatus = hasStatus;
@@ -490,9 +541,8 @@ void PhysicsDirect::getCachedCameraImage(b3CameraImageData* cameraData)
 
 void PhysicsDirect::getCachedContactPointInformation(struct b3ContactInformation* contactPointData)
 {
-    if (contactPointData)
+    contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size();
-    {
+    contactPointData->m_contactPointData = contactPointData->m_numContactPoints? &m_data->m_cachedContactPoints[0] : 0;
-        contactPointData->m_numContactPoints = 0;
+    
-    }
 }
 

examples/SharedMemory/PhysicsDirect.h

@@ -21,6 +21,8 @@ protected:
 
 	bool processCamera(const struct SharedMemoryCommand& orgCommand);
 
+    bool processContactPointData(const struct SharedMemoryCommand& orgCommand);
+
     void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
     
 public:

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -2151,7 +2151,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                             for (int i=0;i<numContactManifolds;i++)
                             {
 								const btPersistentManifold* manifold =  m_data->m_dynamicsWorld->getDispatcher()->getInternalManifoldPointer()[i];
-								
+                                int linkIndexA = -1;
+                                int linkIndexB = -1;
+                                
 								int objectIndexB = -1;
 
 								const btRigidBody* bodyB = btRigidBody::upcast(manifold->getBody1());
@@ -2162,6 +2164,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 								const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
 								if (mblB && mblB->m_multiBody)
 								{
+                                    linkIndexB = mblB->m_link;
 									objectIndexB = mblB->m_multiBody->getUserIndex2();
 								}
 
@@ -2174,12 +2177,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 								const btMultiBodyLinkCollider* mblA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
 								if (mblA && mblA->m_multiBody)
 								{
+                                    linkIndexA = mblA->m_link;
+
 									objectIndexA = mblA->m_multiBody->getUserIndex2();
 								}
 
 								btAssert(bodyA || mblA);
 
-								//apply the filter, if any
+								//apply the filter, if the user provides it
 								if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter>=0)
 								{
 									if ((clientCmd.m_requestContactPointArguments.m_objectAIndexFilter != objectIndexA) &&
@@ -2187,6 +2192,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 									continue;
 								}
 
+                                //apply the second object filter, if the user provides it
 								if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter>=0)
 								{
 									if ((clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexA) &&
@@ -2196,29 +2202,23 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 
 								for (int p=0;p<manifold->getNumContacts();p++)
 								{
-									//if the point passes the optional filter, add it
-
-									if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter>=0)
-									{
-										//one of the two unique Ids has to match...
-
-									}
 
 									b3ContactPointData pt;
-									pt.m_bodyUniqueIdA = -1;
+									pt.m_bodyUniqueIdA = objectIndexA;
-									pt.m_bodyUniqueIdB = -1;
+									pt.m_bodyUniqueIdB = objectIndexB;
 									const btManifoldPoint& srcPt = manifold->getContactPoint(p);
 									pt.m_contactDistance = srcPt.getDistance();
 									pt.m_contactFlags = 0;
-									pt.m_contactPointDynamicsIndex = -1;
+									pt.m_linkIndexA = linkIndexA;
-									pt.m_linkIndexA = -1;
+									pt.m_linkIndexB = linkIndexB;
-									pt.m_linkIndexB = -1;
 									for (int j=0;j<3;j++)
 									{
 										pt.m_contactNormalOnBInWS[j] = srcPt.m_normalWorldOnB[j];
 										pt.m_positionOnAInWS[j] = srcPt.getPositionWorldOnA()[j];
 										pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j];
 									}
+                                    pt.m_normalForce = srcPt.getAppliedImpulse()/m_data->m_physicsDeltaTime;
+//                                    pt.m_linearFrictionForce = srcPt.m_appliedImpulseLateral1;
 									m_data->m_cachedContactPoints.push_back (pt);
 								}
                             }
@@ -2298,8 +2298,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 							}
 							// Set the gravity to correspond to the world gravity
 							btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
-							if (-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force) &&
+                            
-								-1 != tree->setGravityInWorldFrame(id_grav))
+							if (-1 != tree->setGravityInWorldFrame(id_grav) &&
+                                -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
 							{
 								serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
 								serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs;
@@ -2649,7 +2650,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
 			loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &bufferServerToClient[0], bufferServerToClient.size());
 		}
 
-		m_data->m_dynamicsWorld->stepSimulation(dtInSec,10,1./240.);
+		m_data->m_dynamicsWorld->stepSimulation(dtInSec,10,m_data->m_physicsDeltaTime);
 	}
 }
 

examples/SharedMemory/SharedMemoryPublic.h

@@ -139,7 +139,8 @@ struct b3CameraImageData
 
 struct b3ContactPointData
 {
-    int m_contactFlags;//flag wether certain fields below are valid
+//todo: expose some contact flags, such as telling which fields below are valid
+    int m_contactFlags;
     int m_bodyUniqueIdA;
     int m_bodyUniqueIdB;
     int m_linkIndexA;
@@ -148,23 +149,23 @@ struct b3ContactPointData
     double m_positionOnBInWS[3];//contact point location on object A, in world space coordinates
     double m_contactNormalOnBInWS[3];//the separating contact normal, pointing from object B towards object A
     double m_contactDistance;//negative number is penetration, positive is distance.
-    int m_contactPointDynamicsIndex;
+    
+    double m_normalForce;
+
+//todo: expose the friction forces as well
+//    double m_linearFrictionDirection0[3];
+//    double m_linearFrictionForce0;
+//    double m_linearFrictionDirection1[3];
+//    double m_linearFrictionForce1;
+//    double m_angularFrictionDirection[3];
+//    double m_angularFrictionForce;
 };
 
-struct b3ContactPointDynamicsData
-{
-    double m_normalForce;
-    double m_linearFrictionDirection[3];
-    double m_linearFrictionForce;
-    double m_angularFrictionDirection[3];
-    double m_angularFrictionForce;    
-};
 
 struct b3ContactInformation
 {
 	int m_numContactPoints;
 	struct b3ContactPointData* m_contactPointData;
-	struct b3ContactPointDynamicsData* m_contactDynamicsData;
 };
 
 ///b3LinkState provides extra information such as the Cartesian world coordinates

examples/pybullet/pybullet.c

@@ -1113,6 +1113,116 @@ static int pybullet_internalSetVector(PyObject* objMat, float vector[3])
     return 0;
 }
 
+
+
+static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args)
+{
+    int size= PySequence_Size(args);
+    int objectUniqueIdA = -1;
+    int objectUniqueIdB = -1;
+    b3SharedMemoryCommandHandle commandHandle;
+    struct b3ContactInformation contactPointData;
+    b3SharedMemoryStatusHandle statusHandle;
+    int statusType;
+    
+    PyObject* pyResultList=0;
+    
+    if (size==1)
+    {
+        if (!PyArg_ParseTuple(args, "i", &objectUniqueIdA))
+        {
+            PyErr_SetString(SpamError, "Error parsing object unique id");
+            return NULL;
+        }
+    }
+    if (size==2)
+    {
+        if (!PyArg_ParseTuple(args, "ii", &objectUniqueIdA,&objectUniqueIdB))
+        {
+            PyErr_SetString(SpamError, "Error parsing object unique id");
+            return NULL;
+        }
+    }
+    
+    commandHandle = b3InitRequestContactPointInformation(sm);
+    b3SetContactFilterBodyA(commandHandle,objectUniqueIdA);
+    b3SetContactFilterBodyB(commandHandle,objectUniqueIdB);
+    b3SubmitClientCommand(sm, commandHandle);
+    int i; 
+    statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+    statusType = b3GetStatusType(statusHandle);
+    if (statusType==CMD_CONTACT_POINT_INFORMATION_COMPLETED)
+    {
+        
+        /*
+         0     int m_contactFlags;
+         1     int m_bodyUniqueIdA;
+         2     int m_bodyUniqueIdB;
+         3     int m_linkIndexA;
+         4     int m_linkIndexB;
+         5-6-7     double m_positionOnAInWS[3];//contact point location on object A, in world space coordinates
+         8-9-10     double m_positionOnBInWS[3];//contact point location on object A, in world space coordinates
+         11-12-13     double m_contactNormalOnBInWS[3];//the separating contact normal, pointing from object B towards object A
+         14     double m_contactDistance;//negative number is penetration, positive is distance.
+         
+         15    double m_normalForce;
+         */
+        
+        b3GetContactPointInformation(sm, &contactPointData);
+        pyResultList =  PyTuple_New(contactPointData.m_numContactPoints);
+        for (i=0;i<contactPointData.m_numContactPoints;i++)
+        {
+            
+            PyObject* contactObList = PyTuple_New(16);//see above 16 fields
+            PyObject* item;
+            item = PyInt_FromLong(contactPointData.m_contactPointData[i].m_contactFlags);
+            PyTuple_SetItem(contactObList,0,item);
+            item = PyInt_FromLong(contactPointData.m_contactPointData[i].m_bodyUniqueIdA);
+            PyTuple_SetItem(contactObList,1,item);
+            item = PyInt_FromLong(contactPointData.m_contactPointData[i].m_bodyUniqueIdB);
+            PyTuple_SetItem(contactObList,2,item);
+            item = PyInt_FromLong(contactPointData.m_contactPointData[i].m_linkIndexA);
+            PyTuple_SetItem(contactObList,3,item);
+            item = PyInt_FromLong(contactPointData.m_contactPointData[i].m_linkIndexB);
+            PyTuple_SetItem(contactObList,4,item);
+            item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_positionOnAInWS[0]);
+            PyTuple_SetItem(contactObList,5,item);
+            item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_positionOnAInWS[1]);
+            PyTuple_SetItem(contactObList,6,item);
+            item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_positionOnAInWS[2]);
+            PyTuple_SetItem(contactObList,7,item);
+            
+            item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_positionOnBInWS[0]);
+            PyTuple_SetItem(contactObList,8,item);
+            item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_positionOnBInWS[1]);
+            PyTuple_SetItem(contactObList,9,item);
+            item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_positionOnBInWS[2]);
+            PyTuple_SetItem(contactObList,10,item);
+            
+            item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[0]);
+            PyTuple_SetItem(contactObList,11,item);
+            item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[1]);
+            PyTuple_SetItem(contactObList,12,item);
+            item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_contactNormalOnBInWS[2]);
+            PyTuple_SetItem(contactObList,13,item);
+            
+            item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_contactDistance);
+            PyTuple_SetItem(contactObList,14,item);
+            item = PyFloat_FromDouble(contactPointData.m_contactPointData[i].m_normalForce);
+            PyTuple_SetItem(contactObList,15,item);
+            
+            PyTuple_SetItem(pyResultList, i, contactObList);
+            
+        }
+        return pyResultList;
+    }
+    
+    Py_INCREF(Py_None);
+    return Py_None;
+}
+
+
+
 // Render an image from the current timestep of the simulation
 //
 // Examples:
@@ -1794,6 +1904,10 @@ static PyMethodDef SpamMethods[] = {
 	{"renderImage", pybullet_renderImage, METH_VARARGS,
 	"Render an image (given the pixel resolution width, height, camera view matrix, projection matrix, near, and far values), and return the 8-8-8bit RGB pixel data and floating point depth values"},	
 
+    {"getContactPointData", pybullet_getContactPointData, METH_VARARGS,
+        "Return the contact point information for all or some of pairwise object-object collisions. Optional arguments one or two object unique ids, that need to be involved in the contact."},
+    
+ 
     {"getQuaternionFromEuler", pybullet_getQuaternionFromEuler, METH_VARARGS,
         "Convert Euler [roll, pitch, yaw] as in URDF/SDF convention, to quaternion [x,y,z,w]"},
     

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -1053,8 +1053,8 @@ void	btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
 
 			if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
 			{
-				//only a single rollingFriction per manifold
+//disabled: only a single rollingFriction per manifold
-				rollingFriction--;
+//				rollingFriction--;
 				if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
 				{
 					relAngVel.normalize();

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -963,8 +963,8 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 
 			if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
 			{
-				//only a single rollingFriction per manifold
+//disabled: only a single rollingFriction per manifold
-				rollingFriction--;
+//rollingFriction--;
 				if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
 				{
 					relAngVel.normalize();