add first draft of contact point query in shared memory API

b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient);
void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA);
void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData);
Implemented for PhysicsClientSharedMemory, not for PhysicsDirect yet.
Add btCollisionObject::setUserIndex2

examples/SharedMemory/PhysicsClient.h

@@ -40,6 +40,8 @@ public:
 
 	virtual void getCachedCameraImage(struct b3CameraImageData* cameraData)=0;
 	
+	virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData)=0;
+	
 };
 
 #endif  // BT_PHYSICS_CLIENT_API_H

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -1103,6 +1103,51 @@ void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImage
 	}
 }
 
+///request an contact point information
+b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient)
+{
+    PhysicsClient* cl = (PhysicsClient* ) physClient;
+    b3Assert(cl);
+    b3Assert(cl->canSubmitCommand());
+    struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+    b3Assert(command);
+    command->m_type =CMD_REQUEST_CONTACT_POINT_INFORMATION;
+	command->m_requestContactPointArguments.m_startingContactPointIndex = 0;
+	command->m_requestContactPointArguments.m_objectAIndexFilter = -1;
+	command->m_requestContactPointArguments.m_objectBIndexFilter = -1;
+    command->m_updateFlags = 0;
+    return (b3SharedMemoryCommandHandle) command;
+}
+
+void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
+	command->m_requestContactPointArguments.m_objectAIndexFilter = bodyUniqueIdA;
+}
+
+void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
+	command->m_requestContactPointArguments.m_objectBIndexFilter = bodyUniqueIdB;
+}
+
+void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
+
+
+void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData)
+{
+    PhysicsClient* cl = (PhysicsClient* ) physClient;
+	if (cl)
+	{
+	    cl->getCachedContactPointInformation(contactPointData);
+	}
+}
+
+
 b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient)
 {
     PhysicsClient* cl = (PhysicsClient* ) physClient;

examples/SharedMemory/PhysicsClientC_API.h

@@ -80,6 +80,11 @@ void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle command,
 void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer);
 void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData);
 
+///request an contact point information
+b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient);
+void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA);
+void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
+void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData);
 
 b3SharedMemoryCommandHandle	b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
 int	b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz);

examples/SharedMemory/PhysicsClientExample.cpp

@@ -274,7 +274,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
         case CMD_CREATE_BOX_COLLISION_SHAPE:
         {
             b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle);
-            b3CreateBoxCommandSetStartPosition(commandHandle,0,0,-3);
+            b3CreateBoxCommandSetStartPosition(commandHandle,0,0,-1.5);
 			b3CreateBoxCommandSetColorRGBA(commandHandle,0,0,1,1);
             b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
             break;
@@ -415,6 +415,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
             b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
             break;
         }
+        
 		case CMD_CALCULATE_INVERSE_DYNAMICS:
 		{
 			if (m_selectedBody >= 0)
@@ -442,6 +443,14 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
 			}
 			break;
 		}
+		case CMD_REQUEST_CONTACT_POINT_INFORMATION:
+            {
+                b3SharedMemoryCommandHandle commandHandle = b3InitRequestContactPointInformation(m_physicsClientHandle);
+				b3SetContactFilterBodyA(commandHandle,0);
+				b3SetContactFilterBodyB(commandHandle,1);
+                b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
+                break;
+            }
         default:
         {
             b3Error("Unknown buttonId");
@@ -530,6 +539,7 @@ void	PhysicsClientExample::createButtons()
 				createButton("Initialize Pose",CMD_INIT_POSE,  isTrigger);
         createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger);
 		createButton("Compute Inverse Dynamics", CMD_CALCULATE_INVERSE_DYNAMICS, isTrigger);
+        createButton("Get Contact Point Info", CMD_REQUEST_CONTACT_POINT_INFORMATION, isTrigger);
 
         if (m_bodyUniqueIds.size())
         {
@@ -946,6 +956,17 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
 					
 				}
 			}
+			if (statusType == CMD_CONTACT_POINT_INFORMATION_FAILED)
+			{
+				b3Warning("Cannot get contact information");
+			}
+			if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED)
+			{
+				b3ContactInformation contactPointData;
+				b3GetContactPointInformation(m_physicsClientHandle, &contactPointData);
+				b3Printf("Num Contacts: %d\n", contactPointData.m_numContactPoints);
+
+			}
 		}
 	}
     if (b3CanSubmitCommand(m_physicsClientHandle))

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -38,6 +38,9 @@ struct PhysicsClientSharedMemoryInternalData {
 	btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
 	btAlignedObjectArray<int> m_cachedSegmentationMaskBuffer;
 
+    btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
+    btAlignedObjectArray<b3ContactPointDynamicsData>m_cachedContactPointDynamics;
+
     btAlignedObjectArray<int> m_bodyIdsRequestInfo;
     SharedMemoryStatus m_tempBackupServerStatus;
     
@@ -58,6 +61,7 @@ struct PhysicsClientSharedMemoryInternalData {
 		  m_counter(0),
 		  m_cachedCameraPixelsWidth(0),
 		  m_cachedCameraPixelsHeight(0),
+		  
           m_isConnected(false),
           m_waitingForServer(false),
           m_hasLastServerStatus(false),
@@ -558,6 +562,33 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
 				b3Warning("Inverse Dynamics computations failed");
 				break;
 			}
+            case CMD_CONTACT_POINT_INFORMATION_COMPLETED:
+                {
+                    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);
+                    m_data->m_cachedContactPointDynamics.resize(0);
+                    
+					b3ContactPointData* contactData = (b3ContactPointData*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
+
+					for (int i=0;i<numContactsCopied;i++)
+					{
+						m_data->m_cachedContactPoints[startContactIndex+i] = contactData[i];
+					}
+
+                    break;
+                }
+            case CMD_CONTACT_POINT_INFORMATION_FAILED:
+                {
+                    b3Warning("Contact Point Information Request failed");
+                    break;
+                }
+		
             default: {
                 b3Error("Unknown server status\n");
                 btAssert(0);
@@ -620,6 +651,21 @@ 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];
+			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;
+				submitClientCommand(command);
+				return 0;
+			}
+        }
+        
 		if (serverCmd.m_type == CMD_CAMERA_IMAGE_COMPLETED)
 		{
 			SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
@@ -716,6 +762,14 @@ void PhysicsClientSharedMemory::getCachedCameraImage(struct b3CameraImageData* c
 	cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMaskBuffer.size()?&m_data->m_cachedSegmentationMaskBuffer[0] : 0;
 }
 
+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;
+
+}
+
 const float* PhysicsClientSharedMemory::getDebugLinesFrom() const {
     if (m_data->m_debugLinesFrom.size()) {
         return &m_data->m_debugLinesFrom[0].m_x;

examples/SharedMemory/PhysicsClientSharedMemory.h

@@ -48,6 +48,8 @@ public:
     virtual const float* getDebugLinesTo() const;
     virtual const float* getDebugLinesColor() const;
 	virtual void getCachedCameraImage(struct b3CameraImageData* cameraData);
+	
+	virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
 };
 
 #endif  // BT_PHYSICS_CLIENT_API_H

examples/SharedMemory/PhysicsDirect.cpp

@@ -487,3 +487,12 @@ void PhysicsDirect::getCachedCameraImage(b3CameraImageData* cameraData)
 		cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMask.size()? &m_data->m_cachedSegmentationMask[0] : 0;
 	}
 }
+
+void PhysicsDirect::getCachedContactPointInformation(struct b3ContactInformation* contactPointData)
+{
+    if (contactPointData)
+    {
+        contactPointData->m_numContactPoints = 0;
+    }
+}
+

examples/SharedMemory/PhysicsDirect.h

@@ -64,6 +64,8 @@ public:
 
 	virtual void getCachedCameraImage(b3CameraImageData* cameraData);
 
+    virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
+
 	//those 2 APIs are for internal use for visualization
 	virtual bool connect(struct GUIHelperInterface* guiHelper);
 	virtual void renderScene();

examples/SharedMemory/PhysicsLoopBack.cpp

@@ -105,10 +105,12 @@ const float* PhysicsLoopBack::getDebugLinesFrom() const
 {
 	return m_data->m_physicsClient->getDebugLinesFrom();
 }
+
 const float* PhysicsLoopBack::getDebugLinesTo() const
 {
 	return m_data->m_physicsClient->getDebugLinesTo();
 }
+
 const float* PhysicsLoopBack::getDebugLinesColor() const
 {
 	return m_data->m_physicsClient->getDebugLinesColor();
@@ -118,3 +120,8 @@ void PhysicsLoopBack::getCachedCameraImage(struct b3CameraImageData* cameraData)
 {
 	return m_data->m_physicsClient->getCachedCameraImage(cameraData);
 }
+
+void PhysicsLoopBack::getCachedContactPointInformation(struct b3ContactInformation* contactPointData)
+{
+    return m_data->m_physicsClient->getCachedContactPointInformation(contactPointData);
+}

examples/SharedMemory/PhysicsLoopBack.h

@@ -54,6 +54,8 @@ public:
     virtual const float* getDebugLinesColor() const;
 	virtual void getCachedCameraImage(struct b3CameraImageData* cameraData);
 	
+	virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
+
 };
 
 #endif //PHYSICS_LOOP_BACK_H

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -404,6 +404,8 @@ struct PhysicsServerCommandProcessorInternalData
 	btMultiBodyDynamicsWorld* m_dynamicsWorld;
 	SharedMemoryDebugDrawer*		m_remoteDebugDrawer;
 	
+	btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
+
 	btAlignedObjectArray<int> m_sdfRecentLoadedBodies;
 
 
@@ -759,6 +761,8 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
             int bodyUniqueId = m_data->allocHandle();
 
             InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
+			
+
             u2b.setBodyUniqueId(bodyUniqueId);
             {
                 btScalar mass = 0;
@@ -782,10 +786,17 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
 
             mb = creation.getBulletMultiBody();
             rb = creation.getRigidBody();
+			if (rb)
+				rb->setUserIndex2(bodyUniqueId);
+
+			if (mb)
+				mb->setUserIndex2(bodyUniqueId);
+
             if (mb)
             {
                 bodyHandle->m_multiBody = mb;
 				
+
 				m_data->m_sdfRecentLoadedBodies.push_back(bodyUniqueId);
 
 				createJointMotors(mb);
@@ -857,6 +868,8 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
         u2b.setBodyUniqueId(bodyUniqueId);
 		InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
 		
+
+
         {
             btScalar mass = 0;
             bodyHandle->m_rootLocalInertialFrame.setIdentity();
@@ -888,13 +901,14 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
         btMultiBody* mb = creation.getBulletMultiBody();
         btRigidBody* rb = creation.getRigidBody();
 
+        
 		if (useMultiBody)
 		{
 
 
 			if (mb)
 			{
-
+				mb->setUserIndex2(bodyUniqueId);
 				bodyHandle->m_multiBody = mb;
 
 				createJointMotors(mb);
@@ -958,6 +972,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
             if (rb)
             {
                 bodyHandle->m_rigidBody = rb;
+				rb->setUserIndex2(bodyUniqueId);
                 return true;
             }
 		}
@@ -2079,6 +2094,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 						int bodyUniqueId = m_data->allocHandle();
 						InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
 						serverCmd.m_rigidBodyCreateArgs.m_bodyUniqueId = bodyUniqueId;
+						rb->setUserIndex2(bodyUniqueId);
 						bodyHandle->m_rootLocalInertialFrame.setIdentity();
 						bodyHandle->m_rigidBody = rb;
 						hasStatus = true;
@@ -2121,6 +2137,124 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 						serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
 						hasStatus = true;
                         break;
+                    }
+                case CMD_REQUEST_CONTACT_POINT_INFORMATION:
+                    {
+                        SharedMemoryStatus& serverCmd =serverStatusOut;
+                        serverCmd.m_sendContactPointArgs.m_numContactPointsCopied = 0;
+                        
+                        //make a snapshot of the contact manifolds into individual contact points
+                        if (clientCmd.m_requestContactPointArguments.m_startingContactPointIndex==0)
+                        {
+                            int numContactManifolds = m_data->m_dynamicsWorld->getDispatcher()->getNumManifolds();
+							m_data->m_cachedContactPoints.resize(0);
+							m_data->m_cachedContactPoints.reserve(numContactManifolds*4);
+                            for (int i=0;i<numContactManifolds;i++)
+                            {
+								const btPersistentManifold* manifold =  m_data->m_dynamicsWorld->getDispatcher()->getInternalManifoldPointer()[i];
+								
+								int objectIndexB = -1;
+
+								const btRigidBody* bodyB = btRigidBody::upcast(manifold->getBody1());
+								if (bodyB)
+								{
+									objectIndexB = bodyB->getUserIndex2();
+								}
+								const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
+								if (mblB && mblB->m_multiBody)
+								{
+									objectIndexB = mblB->m_multiBody->getUserIndex2();
+								}
+
+								int objectIndexA = -1;
+								const btRigidBody* bodyA = btRigidBody::upcast(manifold->getBody0());
+								if (bodyA)
+								{
+									objectIndexA  = bodyA->getUserIndex2();
+								}
+								const btMultiBodyLinkCollider* mblA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
+								if (mblA && mblA->m_multiBody)
+								{
+									objectIndexA = mblA->m_multiBody->getUserIndex2();
+								}
+
+								btAssert(bodyA || mblA);
+
+								//apply the filter, if any
+								if (clientCmd.m_requestContactPointArguments.m_objectAIndexFilter>=0)
+								{
+									if ((clientCmd.m_requestContactPointArguments.m_objectAIndexFilter != objectIndexA) &&
+										(clientCmd.m_requestContactPointArguments.m_objectAIndexFilter != objectIndexB))
+									continue;
+								}
+
+								if (clientCmd.m_requestContactPointArguments.m_objectBIndexFilter>=0)
+								{
+									if ((clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexA) &&
+										(clientCmd.m_requestContactPointArguments.m_objectBIndexFilter != objectIndexB))
+									continue;
+								}
+
+								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_bodyUniqueIdB = -1;
+									const btManifoldPoint& srcPt = manifold->getContactPoint(p);
+									pt.m_contactDistance = srcPt.getDistance();
+									pt.m_contactFlags = 0;
+									pt.m_contactPointDynamicsIndex = -1;
+									pt.m_linkIndexA = -1;
+									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];
+									}
+									m_data->m_cachedContactPoints.push_back (pt);
+								}
+                            }
+                        }
+                        
+						int numContactPoints = m_data->m_cachedContactPoints.size();
+						
+
+						//b3ContactPoint
+						//struct b3ContactPointDynamics
+
+						int totalBytesPerContact = sizeof(b3ContactPointData);
+						int contactPointStorage = bufferSizeInBytes/totalBytesPerContact-1;
+
+						b3ContactPointData* contactData = (b3ContactPointData*)bufferServerToClient;
+
+						int startContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex;
+						int numContactPointBatch = btMin(numContactPoints,contactPointStorage);
+
+						int endContactPointIndex = startContactPointIndex+numContactPointBatch;
+
+						for (int i=startContactPointIndex;i<endContactPointIndex ;i++)
+						{
+							const b3ContactPointData& srcPt = m_data->m_cachedContactPoints[i];
+							b3ContactPointData& destPt = contactData[serverCmd.m_sendContactPointArgs.m_numContactPointsCopied];
+							destPt = srcPt;
+							serverCmd.m_sendContactPointArgs.m_numContactPointsCopied++;
+						}
+						
+						serverCmd.m_sendContactPointArgs.m_startingContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex;
+						serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints = numContactPoints - clientCmd.m_requestContactPointArguments.m_startingContactPointIndex - serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
+
+						serverCmd.m_type = CMD_CONTACT_POINT_INFORMATION_COMPLETED; //CMD_CONTACT_POINT_INFORMATION_FAILED,
+						hasStatus = true;
+                        break;
                     }
 				case CMD_CALCULATE_INVERSE_DYNAMICS:
 				{

examples/SharedMemory/SharedMemoryCommands.h

@@ -144,6 +144,12 @@ enum EnumRequestPixelDataUpdateFlags
 	
 };
 
+struct RequestContactDataArgs
+{
+    int m_startingContactPointIndex;
+    int m_objectAIndexFilter;
+	int m_objectBIndexFilter;
+};
 
 
 struct SendDebugLinesArgs
@@ -406,6 +412,7 @@ struct SharedMemoryCommand
         struct ExternalForceArgs m_externalForceArguments;
 		struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments;
         struct CreateJointArgs m_createJointArguments;
+        struct RequestContactDataArgs m_requestContactPointArguments;
     };
 };
 
@@ -414,6 +421,13 @@ struct RigidBodyCreateArgs
 	int m_bodyUniqueId; 
 };
 
+struct SendContactDataArgs
+{
+    int m_startingContactPointIndex;
+    int m_numContactPointsCopied;
+    int m_numRemainingContactPoints;
+};
+
 struct SharedMemoryStatus
 {
 	int m_type;
@@ -430,6 +444,7 @@ struct SharedMemoryStatus
 		struct SendPixelDataArgs m_sendPixelDataArguments;
 		struct RigidBodyCreateArgs m_rigidBodyCreateArgs;
 		struct CalculateInverseDynamicsResultArgs m_inverseDynamicsResultArgs;
+		struct SendContactDataArgs m_sendContactPointArgs;
 	};
 };
 

examples/SharedMemory/SharedMemoryPublic.h

@@ -29,8 +29,11 @@ enum EnumSharedMemoryClientCommand
     CMD_APPLY_EXTERNAL_FORCE,
 	CMD_CALCULATE_INVERSE_DYNAMICS,
     CMD_CALCULATE_INVERSE_KINEMATICS,
+    CMD_CREATE_JOINT,
+    CMD_REQUEST_CONTACT_POINT_INFORMATION,
+    //don't go beyond this command!
     CMD_MAX_CLIENT_COMMANDS,
-    CMD_CREATE_JOINT
+    
 };
 
 enum EnumSharedMemoryServerStatus
@@ -64,6 +67,8 @@ enum EnumSharedMemoryServerStatus
 		CMD_INVALID_STATUS,
 		CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED,
 		CMD_CALCULATED_INVERSE_DYNAMICS_FAILED,
+		CMD_CONTACT_POINT_INFORMATION_COMPLETED,
+		CMD_CONTACT_POINT_INFORMATION_FAILED,
         CMD_MAX_SERVER_COMMANDS
 };
 
@@ -131,6 +136,37 @@ struct b3CameraImageData
 	const int* m_segmentationMaskValues;//m_pixelWidth*m_pixelHeight ints
 };
 
+
+struct b3ContactPointData
+{
+    int m_contactFlags;//flag wether certain fields below are valid
+    int m_bodyUniqueIdA;
+    int m_bodyUniqueIdB;
+    int m_linkIndexA;
+    int m_linkIndexB;
+    double m_positionOnAInWS[3];//contact point location on object A, in world space coordinates
+    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;
+};
+
+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
 ///center of mass (COM) of the link, relative to the world reference frame.
 ///Orientation is a quaternion x,y,z,w

examples/TinyRenderer/tgaimage.h

@@ -30,20 +30,20 @@ struct TGAColor {
 			bgra[i] = 0;
     }
 
-    TGAColor(unsigned char R, unsigned char G, unsigned char B, unsigned char A=255) : bgra(), bytespp(4) {
+    TGAColor(unsigned char R, unsigned char G, unsigned char B, unsigned char A=255) : bytespp(4) {
         bgra[0] = B;
         bgra[1] = G;
         bgra[2] = R;
         bgra[3] = A;
     }
 
-    TGAColor(unsigned char v) : bgra(), bytespp(1) {
+    TGAColor(unsigned char v) :  bytespp(1) {
         for (int i=0; i<4; i++) bgra[i] = 0;
         bgra[0] = v;
     }
 
 
-    TGAColor(const unsigned char *p, unsigned char bpp) : bgra(), bytespp(bpp) {
+    TGAColor(const unsigned char *p, unsigned char bpp) :  bytespp(bpp) {
         for (int i=0; i<(int)bpp; i++) {
             bgra[i] = p[i];
         }

src/BulletCollision/CollisionDispatch/btCollisionObject.cpp

@@ -35,6 +35,7 @@ btCollisionObject::btCollisionObject()
 		m_rollingFriction(0.0f),
 		m_internalType(CO_COLLISION_OBJECT),
 		m_userObjectPointer(0),
+		m_userIndex2(-1),
 		m_userIndex(-1),
 		m_hitFraction(btScalar(1.)),
 		m_ccdSweptSphereRadius(btScalar(0.)),

src/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -95,6 +95,8 @@ protected:
 
 	void*			m_userObjectPointer;
 
+	int				m_userIndex2;
+	
     int	m_userIndex;
 
 	///time of impact calculation
@@ -476,6 +478,12 @@ public:
 	{
 		return m_userIndex;
 	}
+	
+	int	getUserIndex2() const
+	{
+		return m_userIndex2;
+	}
+	
 	///users can point to their objects, userPointer is not used by Bullet
 	void	setUserPointer(void* userPointer)
 	{
@@ -488,6 +496,11 @@ public:
 		m_userIndex = index;
 	}
 	
+	void	setUserIndex2(int index)
+	{
+		m_userIndex2 = index;
+	}
+
 	int	getUpdateRevisionInternal() const
 	{
 		return m_updateRevision;

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -106,7 +106,9 @@ btMultiBody::btMultiBody(int n_links,
 		m_awake(true),
 		m_canSleep(canSleep),
 		m_sleepTimer(0),
-		
+		m_userObjectPointer(0),
+		m_userIndex2(-1),
+		m_userIndex(-1),
 		m_linearDamping(0.04f),
 		m_angularDamping(0.04f),
 		m_useGyroTerm(true),

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -577,6 +577,38 @@ void addJointTorque(int i, btScalar Q);
 		m_baseName = name;
 	}
 
+	///users can point to their objects, userPointer is not used by Bullet
+	void*	getUserPointer() const
+	{
+		return m_userObjectPointer;
+	}
+
+	int	getUserIndex() const
+	{
+		return m_userIndex;
+	}
+
+	int	getUserIndex2() const
+	{
+		return m_userIndex2;
+	}
+	///users can point to their objects, userPointer is not used by Bullet
+	void	setUserPointer(void* userPointer)
+	{
+		m_userObjectPointer = userPointer;
+	}
+
+	///users can point to their objects, userPointer is not used by Bullet
+	void	setUserIndex(int index)
+	{
+		m_userIndex = index;
+	}
+
+	void	setUserIndex2(int index)
+	{
+		m_userIndex2 = index;
+	}
+
 private:
     btMultiBody(const btMultiBody &);  // not implemented
     void operator=(const btMultiBody &);  // not implemented
@@ -653,6 +685,10 @@ private:
     bool m_canSleep;
     btScalar m_sleepTimer;
 
+	void* m_userObjectPointer;
+	int m_userIndex2;
+	int m_userIndex;
+
 	int	m_companionId;
 	btScalar	m_linearDamping;
 	btScalar	m_angularDamping;