Merge pull request #1487 from erwincoumans/master

expose link index in segmentation mask (getCameraImage) when a flag is set. preparation for restoreState (not implemented yet), allow multiple options in connect call.

Extras/Serialize/BulletWorldImporter/btMultiBodyWorldImporter.cpp

@@ -36,43 +36,87 @@ void btMultiBodyWorldImporter::deleteAllData()
 
 bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletFile2)
 {
-	bool result = btBulletWorldImporter::convertAllObjects(bulletFile2);
+	bool result = false;
+	btAlignedObjectArray<btQuaternion> scratchQ;
+	btAlignedObjectArray<btVector3> scratchM;
 
-	
+	if (m_importerFlags&eRESTORE_EXISTING_OBJECTS)
-	//convert all multibodies
-	for (int i=0;i<bulletFile2->m_multiBodies.size();i++)
 	{
-		
+		//check if the snapshot is valid for the existing world
-		if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
+		//equal number of objects, # links etc
+		if (bulletFile2->m_multiBodies.size() != m_data->m_mbDynamicsWorld->getNumMultibodies())
 		{
-			btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*) bulletFile2->m_multiBodies[i];
+			result = false;
-			bool isFixedBase = mbd->m_baseMass==0;
+			return result;
-			bool canSleep = false;
+		}
-			btVector3 baseInertia;
+		result = true;
-			baseInertia.deSerializeDouble(mbd->m_baseInertia);
-			btMultiBody* mb = new btMultiBody(mbd->m_numLinks,mbd->m_baseMass,baseInertia,isFixedBase,canSleep);
-			mb->setHasSelfCollision(false);
 
-			m_data->m_mbMap.insert(mbd,mb);
+		//convert all multibodies
-			for (int i=0;i<mbd->m_numLinks;i++)
+		for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++)
+		{
+
+			if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
 			{
-				btVector3 localInertiaDiagonal;
+				btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*)bulletFile2->m_multiBodies[i];
-				localInertiaDiagonal.deSerializeDouble(mbd->m_links[i].m_linkInertia);
+				bool isFixedBase = mbd->m_baseMass == 0;
-				btQuaternion parentRotToThis;
+				bool canSleep = false;
-				parentRotToThis.deSerializeDouble(mbd->m_links[i].m_zeroRotParentToThis);
+				btVector3 baseInertia;
-				btVector3 parentComToThisPivotOffset;
+				baseInertia.deSerializeDouble(mbd->m_baseInertia);
-				parentComToThisPivotOffset.deSerializeDouble(mbd->m_links[i].m_parentComToThisComOffset);
-				btVector3 thisPivotToThisComOffset;
-				thisPivotToThisComOffset.deSerializeDouble(mbd->m_links[i].m_thisPivotToThisComOffset);
 
-				switch (mbd->m_links[i].m_jointType)
+				btTransform tr; tr.deSerializeDouble(mbd->m_baseWorldTransform);
+				btMultiBody* mb = m_data->m_mbDynamicsWorld->getMultiBody(i);
+				mb->setBaseWorldTransform(tr);
+				for (int i = 0; i < mbd->m_numLinks; i++)
 				{
+				}
+				btVector3 linvel = mb->getBaseVel();
+				btVector3 angvel = mb->getBaseOmega();
+				mb->forwardKinematics(scratchQ, scratchM);
+				mb->updateCollisionObjectWorldTransforms(scratchQ, scratchM);
+			}
+		}
+	}
+	else
+	{
+		result = btBulletWorldImporter::convertAllObjects(bulletFile2);
+
+
+		//convert all multibodies
+		for (int i = 0; i < bulletFile2->m_multiBodies.size(); i++)
+		{
+
+			if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
+			{
+				btMultiBodyDoubleData* mbd = (btMultiBodyDoubleData*)bulletFile2->m_multiBodies[i];
+				bool isFixedBase = mbd->m_baseMass == 0;
+				bool canSleep = false;
+				btVector3 baseInertia;
+				baseInertia.deSerializeDouble(mbd->m_baseInertia);
+				btMultiBody* mb = new btMultiBody(mbd->m_numLinks, mbd->m_baseMass, baseInertia, isFixedBase, canSleep);
+				mb->setHasSelfCollision(false);
+				btTransform tr; tr.deSerializeDouble(mbd->m_baseWorldTransform);
+				mb->setBaseWorldTransform(tr);
+
+				m_data->m_mbMap.insert(mbd, mb);
+				for (int i = 0; i < mbd->m_numLinks; i++)
+				{
+					btVector3 localInertiaDiagonal;
+					localInertiaDiagonal.deSerializeDouble(mbd->m_links[i].m_linkInertia);
+					btQuaternion parentRotToThis;
+					parentRotToThis.deSerializeDouble(mbd->m_links[i].m_zeroRotParentToThis);
+					btVector3 parentComToThisPivotOffset;
+					parentComToThisPivotOffset.deSerializeDouble(mbd->m_links[i].m_parentComToThisComOffset);
+					btVector3 thisPivotToThisComOffset;
+					thisPivotToThisComOffset.deSerializeDouble(mbd->m_links[i].m_thisPivotToThisComOffset);
+
+					switch (mbd->m_links[i].m_jointType)
+					{
 					case btMultibodyLink::eFixed:
 					{
 
 
 						mb->setupFixed(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
-                                                            parentRotToThis, parentComToThisPivotOffset,thisPivotToThisComOffset);
+							parentRotToThis, parentComToThisPivotOffset, thisPivotToThisComOffset);
 						//search for the collider
 						//mbd->m_links[i].m_linkCollider
 						break;
@@ -82,8 +126,10 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 						btVector3 jointAxis;
 						jointAxis.deSerializeDouble(mbd->m_links[i].m_jointAxisBottom[0]);
 						bool disableParentCollision = true;//todo
-						mb->setupPrismatic(i,mbd->m_links[i].m_linkMass,localInertiaDiagonal,mbd->m_links[i].m_parentIndex,
+						mb->setupPrismatic(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
-							parentRotToThis,jointAxis, parentComToThisPivotOffset,thisPivotToThisComOffset,disableParentCollision);
+							parentRotToThis, jointAxis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
+						mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
+						mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
 						break;
 					}
 					case btMultibodyLink::eRevolute:
@@ -91,16 +137,23 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 						btVector3 jointAxis;
 						jointAxis.deSerializeDouble(mbd->m_links[i].m_jointAxisTop[0]);
 						bool disableParentCollision = true;//todo
-						mb->setupRevolute(i,mbd->m_links[i].m_linkMass,localInertiaDiagonal,mbd->m_links[i].m_parentIndex,
+						mb->setupRevolute(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
-							parentRotToThis, jointAxis,parentComToThisPivotOffset,thisPivotToThisComOffset,disableParentCollision);
+							parentRotToThis, jointAxis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
+						mb->setJointPos(i, mbd->m_links[i].m_jointPos[0]);
+						mb->setJointVel(i, mbd->m_links[i].m_jointVel[0]);
 						break;
 					}
 					case btMultibodyLink::eSpherical:
 					{
 						btAssert(0);
 						bool disableParentCollision = true;//todo
-						mb->setupSpherical(i,mbd->m_links[i].m_linkMass,localInertiaDiagonal,mbd->m_links[i].m_parentIndex,
+						mb->setupSpherical(i, mbd->m_links[i].m_linkMass, localInertiaDiagonal, mbd->m_links[i].m_parentIndex,
-							parentRotToThis,parentComToThisPivotOffset,thisPivotToThisComOffset,disableParentCollision);
+							parentRotToThis, parentComToThisPivotOffset, thisPivotToThisComOffset, disableParentCollision);
+						btScalar jointPos[3] = { mbd->m_links[i].m_jointPos[0], mbd->m_links[i].m_jointPos[1], mbd->m_links[i].m_jointPos[2] };
+						btScalar jointVel[3] = { mbd->m_links[i].m_jointVel[0], mbd->m_links[i].m_jointVel[1], mbd->m_links[i].m_jointVel[2] };
+						mb->setJointPosMultiDof(i, jointPos);
+						mb->setJointVelMultiDof(i, jointVel);
+
 						break;
 					}
 					case btMultibodyLink::ePlanar:
@@ -112,92 +165,110 @@ bool btMultiBodyWorldImporter::convertAllObjects(  bParse::btBulletFile* bulletF
 					{
 						btAssert(0);
 					}
+					}
 				}
 			}
 		}
-	}
 
-	//convert all multibody link colliders
+		//forward kinematics, so that the link world transforms are valid, for collision detection
-	for (int i=0;i<bulletFile2->m_multiBodyLinkColliders.size();i++)
+		for (int i = 0; i < m_data->m_mbMap.size(); i++)
-	{
-		if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
 		{
-			btMultiBodyLinkColliderDoubleData* mblcd = (btMultiBodyLinkColliderDoubleData*) bulletFile2->m_multiBodyLinkColliders[i];
+			btMultiBody**ptr = m_data->m_mbMap.getAtIndex(i);
-
-			btMultiBody** ptr = m_data->m_mbMap[mblcd->m_multiBody];
 			if (ptr)
 			{
-				btMultiBody* multiBody = *ptr;
+				btMultiBody* mb = *ptr;
-				
+				mb->finalizeMultiDof();
-
+				btVector3 linvel = mb->getBaseVel();
-				btCollisionShape** shapePtr = m_shapeMap.find(mblcd->m_colObjData.m_collisionShape);
+				btVector3 angvel = mb->getBaseOmega();
-				if (shapePtr && *shapePtr)
+				mb->forwardKinematics(scratchQ, scratchM);
-				{
-					btTransform startTransform;
-					mblcd->m_colObjData.m_worldTransform.m_origin.m_floats[3] = 0.f;
-					startTransform.deSerializeDouble(mblcd->m_colObjData.m_worldTransform);
-				
-					btCollisionShape* shape = (btCollisionShape*)*shapePtr;
-					if (shape)
-					{
-						btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(multiBody, mblcd->m_link);
-						col->setCollisionShape( shape );
-
-						//btCollisionObject* body = createCollisionObject(startTransform,shape,mblcd->m_colObjData.m_name);
-						col->setFriction(btScalar(mblcd->m_colObjData.m_friction));
-						col->setRestitution(btScalar(mblcd->m_colObjData.m_restitution));
-						//m_bodyMap.insert(colObjData,body);
-						if (mblcd->m_link==-1)
-						{
-							multiBody->setBaseCollider(col);
-						} else
-						{
-							multiBody->getLink(mblcd->m_link).m_collider = col;
-						}
-						int mbLinkIndex = mblcd->m_link;
-
-						bool isDynamic = (mbLinkIndex<0 && multiBody->hasFixedBase())? false : true;
-						int collisionFilterGroup = isDynamic? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
-						int collisionFilterMask = isDynamic? 	int(btBroadphaseProxy::AllFilter) : 	int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
-
-	#if 0
-						int colGroup=0, colMask=0;
-						int collisionFlags = mblcd->m_colObjData.m_collisionFlags;
-						if (collisionFlags & URDF_HAS_COLLISION_GROUP)
-						{
-							collisionFilterGroup = colGroup;
-						}
-						if (collisionFlags & URDF_HAS_COLLISION_MASK)
-						{
-							collisionFilterMask = colMask;
-						}
-	#endif
-						m_data->m_mbDynamicsWorld->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
-					}
-					
-				} else
-				{
-					printf("error: no shape found\n");
-				}
-#if 0
-				//base and fixed? -> static, otherwise flag as dynamic
-				
-				world1->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
-#endif
 			}
-
 		}
-	}
 
-	for (int i=0;i<m_data->m_mbMap.size();i++)
+		//convert all multibody link colliders
-	{
+		for (int i = 0; i < bulletFile2->m_multiBodyLinkColliders.size(); i++)
-		btMultiBody**ptr = m_data->m_mbMap.getAtIndex(i);
-		if (ptr)
 		{
-			btMultiBody* mb = *ptr;
+			if (bulletFile2->getFlags() & bParse::FD_DOUBLE_PRECISION)
-			mb->finalizeMultiDof();
+			{
+				btMultiBodyLinkColliderDoubleData* mblcd = (btMultiBodyLinkColliderDoubleData*)bulletFile2->m_multiBodyLinkColliders[i];
 
-			m_data->m_mbDynamicsWorld->addMultiBody(mb);
+				btMultiBody** ptr = m_data->m_mbMap[mblcd->m_multiBody];
+				if (ptr)
+				{
+					btMultiBody* multiBody = *ptr;
+
+
+					btCollisionShape** shapePtr = m_shapeMap.find(mblcd->m_colObjData.m_collisionShape);
+					if (shapePtr && *shapePtr)
+					{
+						btTransform startTransform;
+						mblcd->m_colObjData.m_worldTransform.m_origin.m_floats[3] = 0.f;
+						startTransform.deSerializeDouble(mblcd->m_colObjData.m_worldTransform);
+
+						btCollisionShape* shape = (btCollisionShape*)*shapePtr;
+						if (shape)
+						{
+							btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(multiBody, mblcd->m_link);
+							col->setCollisionShape(shape);
+							//btCollisionObject* body = createCollisionObject(startTransform,shape,mblcd->m_colObjData.m_name);
+							col->setFriction(btScalar(mblcd->m_colObjData.m_friction));
+							col->setRestitution(btScalar(mblcd->m_colObjData.m_restitution));
+							//m_bodyMap.insert(colObjData,body);
+							if (mblcd->m_link == -1)
+							{
+								col->setWorldTransform(multiBody->getBaseWorldTransform());
+								multiBody->setBaseCollider(col);
+							}
+							else
+							{
+								col->setWorldTransform(multiBody->getLink(mblcd->m_link).m_cachedWorldTransform);
+								multiBody->getLink(mblcd->m_link).m_collider = col;
+							}
+							int mbLinkIndex = mblcd->m_link;
+
+							bool isDynamic = (mbLinkIndex < 0 && multiBody->hasFixedBase()) ? false : true;
+							int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
+							int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+
+#if 0
+							int colGroup = 0, colMask = 0;
+							int collisionFlags = mblcd->m_colObjData.m_collisionFlags;
+							if (collisionFlags & URDF_HAS_COLLISION_GROUP)
+							{
+								collisionFilterGroup = colGroup;
+							}
+							if (collisionFlags & URDF_HAS_COLLISION_MASK)
+							{
+								collisionFilterMask = colMask;
+							}
+#endif
+							m_data->m_mbDynamicsWorld->addCollisionObject(col, collisionFilterGroup, collisionFilterMask);
+						}
+
+					}
+					else
+					{
+						printf("error: no shape found\n");
+					}
+#if 0
+					//base and fixed? -> static, otherwise flag as dynamic
+
+					world1->addCollisionObject(col, collisionFilterGroup, collisionFilterMask);
+#endif
+				}
+
+			}
+		}
+
+		for (int i = 0; i < m_data->m_mbMap.size(); i++)
+		{
+			btMultiBody**ptr = m_data->m_mbMap.getAtIndex(i);
+			if (ptr)
+			{
+				btMultiBody* mb = *ptr;
+				mb->finalizeMultiDof();
+
+				m_data->m_mbDynamicsWorld->addMultiBody(mb);
+			}
 		}
 	}
 	return result;

Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp

@@ -20,7 +20,8 @@ subject to the following restrictions:
 #endif
 btWorldImporter::btWorldImporter(btDynamicsWorld* world)
 :m_dynamicsWorld(world),
-m_verboseMode(0)
+m_verboseMode(0),
+m_importerFlags(0)
 {
 
 }

Extras/Serialize/BulletWorldImporter/btWorldImporter.h

@@ -59,6 +59,10 @@ struct btRigidBodyFloatData;
 #define btRigidBodyData btRigidBodyFloatData
 #endif//BT_USE_DOUBLE_PRECISION
 
+enum btWorldImporterFlags
+{
+	eRESTORE_EXISTING_OBJECTS=1,//don't create new objects
+};
 
 class btWorldImporter
 {
@@ -66,6 +70,7 @@ protected:
 	btDynamicsWorld* m_dynamicsWorld;
 	
 	int m_verboseMode;
+	int m_importerFlags;
 
 	btAlignedObjectArray<btCollisionShape*>  m_allocatedCollisionShapes;
 	btAlignedObjectArray<btCollisionObject*> m_allocatedRigidBodies;
@@ -131,6 +136,18 @@ public:
 		return m_verboseMode;
 	}
 
+	void	setImporterFlags(int importerFlags)
+	{
+		m_importerFlags = importerFlags;
+	}
+
+	int getImporterFlags() const
+	{
+		return m_importerFlags;
+	}
+
+	
+
 		// query for data
 	int	getNumCollisionShapes() const;
 	btCollisionShape* getCollisionShapeByIndex(int index);

data/cube_small.urdf

@@ -2,8 +2,8 @@
 <robot name="cube.urdf">
   <link name="baseLink">
     <contact>
-      <lateral_friction value="1.0"/>
+    	<friction_anchor/>
-      <inertia_scaling value="3.0"/>
+    	   <lateral_friction value="1.0"/>
     </contact>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0"/>

examples/Importers/ImportURDFDemo/urdfStringSplit.h

@@ -2,14 +2,20 @@
 #ifndef STRING_SPLIT_H
 #define STRING_SPLIT_H
 
+#ifdef __cplusplus
+
 #include <cstring>
 #include <string>
 #include "LinearMath/btAlignedObjectArray.h"
 
-
 void urdfStringSplit( btAlignedObjectArray<std::string>&pieces, const std::string& vector_str, const btAlignedObjectArray<std::string>& separators);
 
 void urdfIsAnyOf(const char* seps, btAlignedObjectArray<std::string>& strArray);
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
 
 ///The string split C code is by Lars Wirzenius
 ///See http://stackoverflow.com/questions/2531605/how-to-split-a-string-with-a-delimiter-larger-than-one-single-char
@@ -26,5 +32,9 @@ void urdfStrArrayFree(char** array);
 /* Return length of a NULL-delimited array of strings. */
 size_t urdfStrArrayLen(char** array);
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif //STRING_SPLIT_H
 

examples/RenderingExamples/TinyRendererSetup.cpp

@@ -195,7 +195,7 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui)
 					m_internalData->m_depthBuffer,
                     &m_internalData->m_shadowBuffer,
 					&m_internalData->m_segmentationMaskBuffer,
-					m_internalData->m_renderObjects.size());
+					m_internalData->m_renderObjects.size(), -1);
                 
                 meshData.m_gfxShape->m_scaling[0] = scaling[0];
                 meshData.m_gfxShape->m_scaling[1] = scaling[1];

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -107,6 +107,91 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClien
 	return 0;
 }
 
+
+B3_SHARED_API	b3SharedMemoryCommandHandle	b3LoadStateCommandInit(b3PhysicsClientHandle physClient)
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+
+	if (cl->canSubmitCommand())
+	{
+		struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+		b3Assert(command);
+		command->m_type = CMD_RESTORE_STATE;
+		command->m_updateFlags = 0;
+		command->m_loadStateArguments.m_fileName[0] = 0;
+		command->m_loadStateArguments.m_stateId = -1;
+		return (b3SharedMemoryCommandHandle)command;
+	}
+	return 0;
+}
+
+B3_SHARED_API int	b3LoadStateSetStateId(b3SharedMemoryCommandHandle commandHandle, int stateId)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_RESTORE_STATE);
+	if (command->m_type == CMD_RESTORE_STATE)
+	{
+		command->m_loadStateArguments.m_stateId = stateId;
+		command->m_updateFlags |= CMD_LOAD_STATE_HAS_STATEID;
+	}
+	return 0;
+
+}
+
+B3_SHARED_API int	b3LoadStateSetFileName(b3SharedMemoryCommandHandle commandHandle, const char* fileName)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_RESTORE_STATE);
+	if (command->m_type == CMD_RESTORE_STATE)
+	{
+		int len = strlen(fileName);
+		if (len < MAX_URDF_FILENAME_LENGTH)
+		{
+			strcpy(command->m_loadStateArguments.m_fileName, fileName);
+		}
+		else
+		{
+			command->m_loadStateArguments.m_fileName[0] = 0;
+		}
+		command->m_updateFlags |= CMD_LOAD_STATE_HAS_FILENAME;
+	}
+	return 0;
+}
+
+
+B3_SHARED_API b3SharedMemoryCommandHandle b3SaveStateCommandInit(b3PhysicsClientHandle physClient)
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+
+	if (cl->canSubmitCommand())
+	{
+		struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+		b3Assert(command);
+		command->m_type = CMD_SAVE_STATE;
+		command->m_updateFlags = 0;
+		return (b3SharedMemoryCommandHandle)command;
+	}
+	return 0;
+}
+
+B3_SHARED_API int b3GetStatusGetStateId(b3SharedMemoryStatusHandle statusHandle)
+{
+	int stateId = -1;
+
+	const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
+	b3Assert(status);
+	b3Assert(status->m_type == CMD_SAVE_STATE_COMPLETED);
+	if (status && status->m_type == CMD_SAVE_STATE_COMPLETED)
+	{
+		stateId =  status->m_saveStateResultArgs.m_stateId;
+	}
+	return stateId;
+}
+
 B3_SHARED_API b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
 {
 	PhysicsClient* cl = (PhysicsClient*)physClient;
@@ -2855,6 +2940,20 @@ B3_SHARED_API void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandl
 	}
 }
 
+B3_SHARED_API	void b3RequestCameraImageSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command);
+	b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
+	if(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA)
+	{
+		command->m_requestPixelDataArguments.m_flags = flags;
+		command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_FLAGS;
+	}
+}
+
+
+
 B3_SHARED_API void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHandle, float viewMatrix[16], float projectionMatrix[16])
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;

examples/SharedMemory/PhysicsClientC_API.h

@@ -209,6 +209,9 @@ B3_SHARED_API	void b3RequestCameraImageSetLightDiffuseCoeff(b3SharedMemoryComman
 B3_SHARED_API	void b3RequestCameraImageSetLightSpecularCoeff(b3SharedMemoryCommandHandle commandHandle, float lightSpecularCoeff);
 B3_SHARED_API	void b3RequestCameraImageSetShadow(b3SharedMemoryCommandHandle commandHandle, int hasShadow);
 B3_SHARED_API	void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer);
+B3_SHARED_API	void b3RequestCameraImageSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
+
+
 B3_SHARED_API	void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData);
 
 ///compute a view matrix, helper function for b3RequestCameraImageSetCameraMatrices
@@ -313,7 +316,16 @@ B3_SHARED_API int	b3LoadUrdfCommandSetGlobalScaling(b3SharedMemoryCommandHandle
 
 
 
+
+B3_SHARED_API b3SharedMemoryCommandHandle b3SaveStateCommandInit(b3PhysicsClientHandle physClient);
+B3_SHARED_API int b3GetStatusGetStateId(b3SharedMemoryStatusHandle statusHandle);
+
+B3_SHARED_API	b3SharedMemoryCommandHandle	b3LoadStateCommandInit(b3PhysicsClientHandle physClient);
+B3_SHARED_API int	b3LoadStateSetStateId(b3SharedMemoryCommandHandle commandHandle, int stateId);
+B3_SHARED_API int	b3LoadStateSetFileName(b3SharedMemoryCommandHandle commandHandle, const char* fileName);
+
 B3_SHARED_API	b3SharedMemoryCommandHandle	b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
+
 B3_SHARED_API	b3SharedMemoryCommandHandle	b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
 B3_SHARED_API	b3SharedMemoryCommandHandle	b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
 B3_SHARED_API	void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);

examples/SharedMemory/PhysicsClientExample.cpp

@@ -940,8 +940,11 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
                                                         btVector4(32,255,255,255)};
                                 if (segmentationMask>=0)
                                 {
-                                    btVector4 rgb = palette[segmentationMask&3];
+									int obIndex = segmentationMask&((1<<24)-1);
-                                     m_canvas->setPixel(m_canvasSegMaskIndex,i,j,
+									int linkIndex = (segmentationMask>>24)-1;
+									
+									btVector4 rgb = palette[(obIndex+linkIndex)&3];
+									m_canvas->setPixel(m_canvasSegMaskIndex,i,j,
                                         rgb.x(),
                                         rgb.y(),
                                         rgb.z(), 255); //alpha set to 255

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -1236,6 +1236,19 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
 			{
 				break;
 			}
+			case CMD_RESTORE_STATE_FAILED:
+			{
+				b3Warning("restoreState failed");
+				break;
+			}
+			case CMD_RESTORE_STATE_COMPLETED:
+			{
+				break;
+			}
+			case CMD_BULLET_SAVING_COMPLETED:
+			{
+				break;
+			}
             default: {
                 b3Error("Unknown server status %d\n", serverCmd.m_type);
                 btAssert(0);

examples/SharedMemory/PhysicsDirect.cpp

@@ -1007,6 +1007,19 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
 		{
 			break;
 		}
+	case CMD_RESTORE_STATE_FAILED:
+	{
+		b3Warning("restoreState failed");
+		break;
+	}
+	case CMD_RESTORE_STATE_COMPLETED:
+	{
+		break;
+	}
+	case CMD_BULLET_SAVING_COMPLETED:
+	{
+		break;
+	}
 	default:
 	{
 		//b3Warning("Unknown server status type");

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -3017,7 +3017,12 @@ bool PhysicsServerCommandProcessor::processRequestCameraImageCommand(const struc
 			m_data->m_visualConverter.setWidthAndHeight(clientCmd.m_requestPixelDataArguments.m_pixelWidth,
 														clientCmd.m_requestPixelDataArguments.m_pixelHeight);
 	}
-
+	int flags = 0;
+	if (clientCmd.m_updateFlags&REQUEST_PIXEL_ARGS_HAS_FLAGS)
+	{
+		flags = clientCmd.m_requestPixelDataArguments.m_flags;
+	}
+	m_data->m_visualConverter.setFlags(flags);
 
 	int numTotalPixels = width*height;
 	int numRemainingPixels = numTotalPixels - startPixelIndex;
@@ -8121,6 +8126,62 @@ bool PhysicsServerCommandProcessor::processLoadTextureCommand(const struct Share
 	return hasStatus;
 }
 
+bool PhysicsServerCommandProcessor::processRestoreStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
+{
+	BT_PROFILE("CMD_RESTORE_STATE");
+	bool hasStatus = true;
+	SharedMemoryStatus& serverCmd = serverStatusOut;
+	serverCmd.m_type = CMD_RESTORE_STATE_FAILED;
+
+	btMultiBodyWorldImporter* importer = new btMultiBodyWorldImporter(m_data->m_dynamicsWorld);
+	importer->setImporterFlags(eRESTORE_EXISTING_OBJECTS);
+
+	bool ok = false;
+
+	if (clientCmd.m_loadStateArguments.m_stateId >= 0)
+	{
+		char* memoryBuffer = 0;
+		int len = 0;
+		ok = importer->loadFileFromMemory(memoryBuffer, len);
+	}
+	else
+	{
+		const char* prefix[] = { "", "./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/" };
+		int numPrefixes = sizeof(prefix) / sizeof(const char*);
+		char relativeFileName[1024];
+		FILE* f = 0;
+		bool found = false;
+
+		for (int i = 0; !f && i<numPrefixes; i++)
+		{
+			sprintf(relativeFileName, "%s%s", prefix[i], clientCmd.m_fileArguments.m_fileName);
+			f = fopen(relativeFileName, "rb");
+			if (f)
+			{
+				found = true;
+				break;
+			}
+		}
+		if (f)
+		{
+			fclose(f);
+		}
+
+		if (found)
+		{
+			ok = importer->loadFile(relativeFileName);
+		}
+	}
+
+	if (ok)
+	{
+		serverCmd.m_type = CMD_RESTORE_STATE_COMPLETED;
+	}
+	
+
+	return hasStatus;
+}
+
 bool PhysicsServerCommandProcessor::processLoadBulletCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
 {
 	BT_PROFILE("CMD_LOAD_BULLET");
@@ -8541,6 +8602,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 			hasStatus = processLoadTextureCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
 			break;
 		}
+	case CMD_RESTORE_STATE:
+	{
+		hasStatus = processRestoreStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
+		break;
+	}
+
 	case CMD_LOAD_BULLET:
 		{
 			hasStatus = processLoadBulletCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);

examples/SharedMemory/PhysicsServerCommandProcessor.h

@@ -80,6 +80,7 @@ protected:
 	bool processLoadBulletCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processSaveBulletCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processLoadMJCFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
+	bool processRestoreStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 
 
 	bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, btScalar globalScaling);

examples/SharedMemory/PhysicsServerExample.cpp

@@ -2169,7 +2169,11 @@ void	PhysicsServerExample::updateGraphics()
 															btVector4(32,255,255,255)};
 									if (segmentationMask>=0)
 									{
-										btVector4 rgb = palette[segmentationMask&3];
+										int obIndex = segmentationMask&((1<<24)-1);
+										int linkIndex = (segmentationMask>>24)-1;
+										
+										btVector4 rgb = palette[(obIndex+linkIndex)&3];
+										
 										 m_canvas->setPixel(m_canvasSegMaskIndex,i,j,
 											rgb.x(),
 											rgb.y(),

examples/SharedMemory/SharedMemoryCommands.h

@@ -69,6 +69,13 @@ struct SdfArgs
 struct FileArgs
 {
 	char m_fileName[MAX_URDF_FILENAME_LENGTH];
+	int m_stateId;
+};
+
+enum EnumLoadStateArgsUpdateFlags
+{
+	CMD_LOAD_STATE_HAS_STATEID=1,
+	CMD_LOAD_STATE_HAS_FILENAME=2,
 };
 
 enum EnumUrdfArgsUpdateFlags
@@ -231,6 +238,7 @@ struct RequestPixelDataArgs
     float m_lightDiffuseCoeff;
     float m_lightSpecularCoeff;
     int m_hasShadow;
+	int m_flags;
 };
 
 enum EnumRequestPixelDataUpdateFlags
@@ -244,6 +252,8 @@ enum EnumRequestPixelDataUpdateFlags
     REQUEST_PIXEL_ARGS_SET_AMBIENT_COEFF=64,
     REQUEST_PIXEL_ARGS_SET_DIFFUSE_COEFF=128,
     REQUEST_PIXEL_ARGS_SET_SPECULAR_COEFF=256,
+	REQUEST_PIXEL_ARGS_HAS_FLAGS = 512,
+
 	//don't exceed (1<<15), because this enum is shared with EnumRenderer in SharedMemoryPublic.h
 	
 };
@@ -922,6 +932,12 @@ struct b3ChangeTextureArgs
 	int m_height;
 };
 
+struct b3StateSerializationArguments
+{
+	char m_fileName[MAX_URDF_FILENAME_LENGTH];
+	int m_stateId;
+};
+
 struct SharedMemoryCommand
 {
 	int m_type;
@@ -976,6 +992,7 @@ struct SharedMemoryCommand
 		struct b3ChangeTextureArgs m_changeTextureArgs;
 		struct b3SearchPathfArgs m_searchPathArgs;
 		struct b3CustomCommand m_customCommandArgs;
+		struct b3StateSerializationArguments m_loadStateArguments;
     };
 };
 
@@ -1050,6 +1067,7 @@ struct SharedMemoryStatus
 		struct b3LoadTextureResultArgs m_loadTextureResultArguments;
 		struct b3CustomCommandResultArgs m_customCommandResultArgs;
 		struct b3PhysicsSimulationParameters m_simulationParameterResultArgs;
+		struct b3StateSerializationArguments m_saveStateResultArgs;
 	};
 };
 

examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp

@@ -112,12 +112,13 @@ public:
 	{
 		int newargc = argc+2;
 		m_newargv = (char**)malloc(sizeof(void*)*newargc);
-		for (int i=0;i<argc;i++)
+		char* t0 = (char*)"--unused";
-			m_newargv[i] = argv[i];
+		m_newargv[0] = t0;
+
+		for (int i=0;i<argc;i++)
+			m_newargv[i+1] = argv[i];
 		
-		char* t0 = (char*)"--logtostderr";
 		char* t1 = (char*)"--start_demo_name=Physics Server";
-		m_newargv[argc] = t0;
 		m_newargv[argc+1] = t1;
 		m_data = btCreateInProcessExampleBrowser(newargc,m_newargv, useInProcessMemory);
 		SharedMemoryInterface* shMem = btGetSharedMemoryInterface(m_data);

examples/SharedMemory/SharedMemoryPublic.h

@@ -77,6 +77,8 @@ enum EnumSharedMemoryClientCommand
 	CMD_SET_ADDITIONAL_SEARCH_PATH,
 	CMD_CUSTOM_COMMAND,
 	CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS,
+	CMD_SAVE_STATE,
+	CMD_RESTORE_STATE,
     //don't go beyond this command!
     CMD_MAX_CLIENT_COMMANDS,
     
@@ -178,7 +180,11 @@ enum EnumSharedMemoryServerStatus
 		CMD_CUSTOM_COMMAND_COMPLETED,
 		CMD_CUSTOM_COMMAND_FAILED,
 		CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED,
-        //don't go beyond 'CMD_MAX_SERVER_COMMANDS!
+		CMD_SAVE_STATE_FAILED,
+		CMD_SAVE_STATE_COMPLETED,
+		CMD_RESTORE_STATE_FAILED,
+		CMD_RESTORE_STATE_COMPLETED,
+		//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
         CMD_MAX_SERVER_COMMANDS
 };
 
@@ -578,6 +584,10 @@ enum EnumRenderer
     //ER_FIRE_RAYS=(1<<18),
 };
 
+enum EnumRendererAuxFlags
+{
+	ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX=1,
+};
 ///flags to pick the IK solver and other options
 enum EnumCalculateInverseKinematicsFlags
 {

examples/SharedMemory/TinyRendererVisualShapeConverter.cpp

@@ -82,6 +82,7 @@ struct TinyRendererVisualShapeConverterInternalData
     float m_lightSpecularCoeff;
     bool m_hasLightSpecularCoeff;
     bool m_hasShadow;
+	int m_flags;
 	SimpleCamera m_camera;
 	
 	
@@ -102,7 +103,8 @@ struct TinyRendererVisualShapeConverterInternalData
     m_hasLightDiffuseCoeff(false),
 	m_lightSpecularCoeff(0.05),
     m_hasLightSpecularCoeff(false),
-	m_hasShadow(false)
+	m_hasShadow(false),
+	m_flags(0)
 	{
 	    m_depthBuffer.resize(m_swWidth*m_swHeight);
         m_shadowBuffer.resize(m_swWidth*m_swHeight);
@@ -154,6 +156,11 @@ void TinyRendererVisualShapeConverter::setShadow(bool hasShadow)
 {
     m_data->m_hasShadow = hasShadow;
 }
+void TinyRendererVisualShapeConverter::setFlags(int flags)
+{
+	m_data->m_flags = flags;
+}
+
 
 void TinyRendererVisualShapeConverter::setLightAmbientCoeff(float ambientCoeff)
 {
@@ -629,7 +636,7 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(
 
             if (vertices.size() && indices.size())
             {
-                TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, &m_data->m_shadowBuffer, &m_data->m_segmentationMaskBuffer, bodyUniqueId);
+                TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, &m_data->m_shadowBuffer, &m_data->m_segmentationMaskBuffer, bodyUniqueId, linkIndex);
 				unsigned char* textureImage1=0;
 				int textureWidth=0;
 				int textureHeight=0;
@@ -1043,7 +1050,17 @@ void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixels
 			}
 			if (segmentationMaskBuffer)
             {
-                segmentationMaskBuffer[i] = m_data->m_segmentationMaskBuffer[i+startPixelIndex];
+				int segMask = m_data->m_segmentationMaskBuffer[i + startPixelIndex];
+				if ((m_data->m_flags & ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX)==0)
+				{
+					//if we don't explicitly request link index, clear it out
+					//object index are the lower 24bits
+					if (segMask >= 0)
+					{
+						segMask &= ((1 << 24) - 1);
+					}
+				}
+				segmentationMaskBuffer[i] = segMask;
             }
 			
             if (pixelsRGBA)

examples/SharedMemory/TinyRendererVisualShapeConverter.h

@@ -43,6 +43,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter
     void setLightDiffuseCoeff(float diffuseCoeff);
     void setLightSpecularCoeff(float specularCoeff);
     void setShadow(bool hasShadow);
+	void setFlags(int flags);
 
     void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels,int* segmentationMaskBuffer, int segmentationMaskSizeInPixels,  int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied);
     

examples/TinyRenderer/TinyRenderer.cpp

@@ -199,7 +199,7 @@ m_objectIndex(-1)
     
 }
 
-TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer, b3AlignedObjectArray<float>* shadowBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer, int objectIndex)
+TinyRenderObjectData::TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer, b3AlignedObjectArray<float>* shadowBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer, int objectIndex, int linkIndex)
 :m_model(0),
 m_rgbColorBuffer(rgbColorBuffer),
 m_depthBuffer(depthBuffer),
@@ -207,7 +207,8 @@ m_shadowBuffer(shadowBuffer),
 m_segmentationMaskBufferPtr(segmentationMaskBuffer),
 m_userData(0),
 m_userIndex(-1),
-m_objectIndex(objectIndex)
+m_objectIndex(objectIndex),
+m_linkIndex(linkIndex)
 {
     Vec3f       eye(1,1,3);
     Vec3f    center(0,0,0);
@@ -561,12 +562,12 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData)
 			{
 				for (int t=0;t<clippedTriangles.size();t++)
 				{
-					triangleClipped(clippedTriangles[t], shader.varying_tri, shader, frame, &zbuffer[0], segmentationMaskBufferPtr, renderData.m_viewportMatrix, renderData.m_objectIndex);
+					triangleClipped(clippedTriangles[t], shader.varying_tri, shader, frame, &zbuffer[0], segmentationMaskBufferPtr, renderData.m_viewportMatrix, renderData.m_objectIndex+((renderData.m_linkIndex + 1) << 24));
 				}
 			}
 			else
 			{
-				triangle(shader.varying_tri, shader, frame, &zbuffer[0], segmentationMaskBufferPtr, renderData.m_viewportMatrix, renderData.m_objectIndex);
+				triangle(shader.varying_tri, shader, frame, &zbuffer[0], segmentationMaskBufferPtr, renderData.m_viewportMatrix, renderData.m_objectIndex+((renderData.m_linkIndex + 1) << 24));
 			}
         }
 		}

examples/TinyRenderer/TinyRenderer.h

@@ -39,7 +39,7 @@ struct TinyRenderObjectData
     TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer);
     TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer,b3AlignedObjectArray<int>* segmentationMaskBuffer,int objectIndex);
     TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer,b3AlignedObjectArray<float>* shadowBuffer);
-    TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer,b3AlignedObjectArray<float>* shadowBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer,int objectIndex);
+    TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray<float>&depthBuffer,b3AlignedObjectArray<float>* shadowBuffer, b3AlignedObjectArray<int>* segmentationMaskBuffer,int objectIndex, int linkIndex);
     virtual ~TinyRenderObjectData();
     
     void loadModel(const char* fileName);
@@ -52,6 +52,7 @@ struct TinyRenderObjectData
     void* m_userData;
     int m_userIndex;
     int m_objectIndex;
+    int m_linkIndex;
 };
 
 

examples/TinyRenderer/our_gl.cpp

@@ -75,7 +75,7 @@ void triangleClipped(mat<4,3,float> &clipc, mat<4,3,float> &orgClipc, IShader &s
     triangleClipped(clipc, orgClipc,shader,image,zbuffer,0,viewPortMatrix,0);
 }
 
-void triangleClipped(mat<4,3,float> &clipc, mat<4,3,float> &orgClipc, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectIndex) 
+void triangleClipped(mat<4,3,float> &clipc, mat<4,3,float> &orgClipc, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectAndLinkIndex)
 {
 
 	mat<3,4,float> screenSpacePts  = (viewPortMatrix*clipc).transpose(); // transposed to ease access to each of the points
@@ -135,7 +135,7 @@ void triangleClipped(mat<4,3,float> &clipc, mat<4,3,float> &orgClipc, IShader &s
                 zbuffer[P.x+P.y*image.get_width()] = frag_depth;
                 if (segmentationMaskBuffer)
                 {
-                    segmentationMaskBuffer[P.x+P.y*image.get_width()] = objectIndex;
+                    segmentationMaskBuffer[P.x+P.y*image.get_width()] = objectAndLinkIndex;
                 }
                 image.set(P.x, P.y, color);
             }
@@ -149,7 +149,7 @@ void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zb
     triangle(clipc,shader,image,zbuffer,0,viewPortMatrix,0);
 }
 
-void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectIndex) {
+void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zbuffer, int* segmentationMaskBuffer, const Matrix& viewPortMatrix, int objectAndLinkIndex) {
 	mat<3,4,float> pts  = (viewPortMatrix*clipc).transpose(); // transposed to ease access to each of the points
     
 	
@@ -189,7 +189,7 @@ void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zb
                 zbuffer[P.x+P.y*image.get_width()] = frag_depth;
                 if (segmentationMaskBuffer)
                 {
-                    segmentationMaskBuffer[P.x+P.y*image.get_width()] = objectIndex;
+						segmentationMaskBuffer[P.x+P.y*image.get_width()] = objectAndLinkIndex;
                 }
                 image.set(P.x, P.y, color);
             }

examples/pybullet/examples/saveRestoreState.py

@@ -0,0 +1,30 @@
+import pybullet as p
+import math, time
+
+p.connect(p.GUI, options="--width=1024 --height=768")
+
+p.loadURDF("plane.urdf")
+for i in range (10):
+	cube = p.loadURDF("cube_small.urdf",0,i*0.01,i*0.5)
+p.setGravity(0,0,-10)
+
+
+#for i in range (500):
+#	p.stepSimulation()
+
+
+
+saveState = 1
+
+if saveState:
+	for i in range (500):
+		p.stepSimulation()
+	p.saveBullet("state.bullet")
+else:
+	p.restoreState(fileName="state.bullet")
+
+for i in range (10):
+	print("pos[",i,"]=",p.getBasePositionAndOrientation(i))
+	
+while (p.getConnectionInfo()["isConnected"]):
+	time.sleep(1)

examples/pybullet/examples/segmask_linkindex.py

@@ -0,0 +1,42 @@
+import pybullet as p
+p.connect(p.GUI)
+r2d2 = p.loadURDF("r2d2.urdf",[0,0,1])
+for l in range (p.getNumJoints(r2d2)):
+	print(p.getJointInfo(r2d2,l))
+
+p.loadURDF("r2d2.urdf",[2,0,1])
+p.loadURDF("r2d2.urdf",[4,0,1])
+
+p.getCameraImage(320,200,flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX)
+segLinkIndex=1
+verbose = 0
+
+while (1):
+	keys = p.getKeyboardEvents()
+	#for k in keys:
+	#	print("key=",k,"state=", keys[k])
+	if ord('1') in keys:
+		state = keys[ord('1')]
+		if (state & p.KEY_WAS_RELEASED):
+			verbose = 1-verbose
+	if ord('s') in keys:
+		state = keys[ord('s')]
+		if (state & p.KEY_WAS_RELEASED):
+			segLinkIndex = 1-segLinkIndex
+			#print("segLinkIndex=",segLinkIndex)
+	flags = 0
+	if (segLinkIndex):
+		flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX
+		
+	img = p.getCameraImage(320,200,flags=flags)
+	#print(img[0],img[1])
+	seg=img[4]
+	if (verbose):
+		for pixel in seg:
+			if (pixel>=0):
+				obUid = pixel & ((1<<24)-1)
+				linkIndex = (pixel >> 24)-1
+				print("obUid=",obUid,"linkIndex=",linkIndex)
+	
+	
+	p.stepSimulation()

examples/pybullet/pybullet.c

@@ -15,6 +15,8 @@
 #include <Python.h>
 #endif
 
+#include "../Importers/ImportURDFDemo/urdfStringSplit.h"
+
 #ifdef B3_DUMP_PYTHON_VERSION
 #define B3_VALUE_TO_STRING(x) #x
 #define B3_VALUE(x) B3_VALUE_TO_STRING(x)
@@ -298,7 +300,7 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
 	int freeIndex = -1;
 	int method = eCONNECT_GUI;
 	int i;
-	char* options="";
+	char* options=0;
 
 	b3PhysicsClientHandle sm = 0;
 
@@ -352,12 +354,23 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
 			}
 		}
 
+		int argc = 0;
+		char** argv=0;
+		if (options)
+		{
+			int i;
+			argv = urdfStrSplit(options, " ");
+			argc = urdfStrArrayLen(argv);
+			for (i = 0; i < argc; i++)
+			{
+				printf("argv[%d]=%s\n", i, argv[i]);
+			}
+		}
 		switch (method)
 		{
 			case eCONNECT_GUI:
 			{
-				int argc = 2;
+				
-				char* argv[2] = {"unused",options};
 
 #ifdef __APPLE__
 				sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
@@ -368,8 +381,6 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
 			}
 			case eCONNECT_GUI_SERVER:
 			{
-				int argc = 2;
-				char* argv[2] = {"unused",options};
 				
 #ifdef __APPLE__
 				sm = b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(argc, argv);
@@ -419,6 +430,11 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
 				return NULL;
 			}
 		};
+
+		if (options)
+		{
+			urdfStrArrayFree(argv);
+		}
 	}
 
 	if (sm && b3CanSubmitCommand(sm))
@@ -588,7 +604,7 @@ static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args, PyObject* k
 	b3PhysicsClientHandle sm = 0;
 
 	int physicsClientId = 0;
-	static char* kwlist[] = {"bulletFileName", "physicsClientId", NULL};
+	static char* kwlist[] = {"bulletFileName",  "physicsClientId", NULL};
 	if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &bulletFileName, &physicsClientId))
 	{
 		return NULL;
@@ -662,6 +678,93 @@ static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args, PyObject* k
 	return Py_None;
 }
 
+
+static PyObject* pybullet_restoreState(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	const char* fileName = "";
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	int stateId = -1;
+	b3SharedMemoryCommandHandle command;
+
+	PyObject* pylist = 0;
+	b3PhysicsClientHandle sm = 0;
+
+	int physicsClientId = 0;
+	static char* kwlist[] = { "stateId", "fileName", "physicsClientId", NULL };
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|isi", kwlist, &stateId, &fileName, &physicsClientId))
+	{
+		return NULL;
+	}
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0)
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+
+	command = b3LoadStateCommandInit(sm);
+	if (stateId >= 0)
+	{
+		b3LoadStateSetStateId(command, stateId);
+	}
+	if (fileName)
+	{
+		b3LoadStateSetFileName(command, fileName);
+	}
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	statusType = b3GetStatusType(statusHandle);
+	if (statusType != CMD_RESTORE_STATE_COMPLETED)
+	{
+		PyErr_SetString(SpamError, "Couldn't restore state.");
+		return NULL;
+	}
+
+	Py_INCREF(Py_None);
+	return Py_None;
+
+}
+
+static PyObject* pybullet_saveState(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	b3SharedMemoryCommandHandle command;
+	b3PhysicsClientHandle sm = 0;
+	int stateId = -1;
+
+	int physicsClientId = 0;
+	static char* kwlist[] = { "physicsClientId", NULL };
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId))
+	{
+		return NULL;
+	}
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0)
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+
+	command = b3SaveStateCommandInit(sm);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	statusType = b3GetStatusType(statusHandle);
+
+	if (statusType != CMD_SAVE_STATE_COMPLETED)
+	{
+		PyErr_SetString(SpamError, "Couldn't save state");
+		return NULL;
+	}
+
+	{
+		stateId = b3GetStatusGetStateId(statusHandle);
+		PyObject* pylist = 0;
+		pylist = PyTuple_New(1);
+		PyTuple_SetItem(pylist, 0, PyInt_FromLong(stateId));
+		return pylist;
+	}
+}
+
 static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	const char* mjcfFileName = "";
@@ -5944,16 +6047,16 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec
 	float lightAmbientCoeff = -1;
 	float lightDiffuseCoeff = -1;
 	float lightSpecularCoeff = -1;
-
+	int flags = -1;
 	int renderer = -1;
 	// inialize cmd
 	b3SharedMemoryCommandHandle command;
 	int physicsClientId = 0;
 	b3PhysicsClientHandle sm = 0;
 	// set camera resolution, optionally view, projection matrix, light direction, light color, light distance, shadow
-	static char* kwlist[] = {"width", "height", "viewMatrix", "projectionMatrix", "lightDirection", "lightColor", "lightDistance", "shadow", "lightAmbientCoeff", "lightDiffuseCoeff", "lightSpecularCoeff", "renderer", "physicsClientId", NULL};
+	static char* kwlist[] = {"width", "height", "viewMatrix", "projectionMatrix", "lightDirection", "lightColor", "lightDistance", "shadow", "lightAmbientCoeff", "lightDiffuseCoeff", "lightSpecularCoeff", "renderer", "flags", "physicsClientId", NULL};
 
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|OOOOfifffii", kwlist, &width, &height, &objViewMat, &objProjMat, &lightDirObj, &lightColorObj, &lightDist, &hasShadow, &lightAmbientCoeff, &lightDiffuseCoeff, &lightSpecularCoeff, &renderer, &physicsClientId))
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|OOOOfifffiii", kwlist, &width, &height, &objViewMat, &objProjMat, &lightDirObj, &lightColorObj, &lightDist, &hasShadow, &lightAmbientCoeff, &lightDiffuseCoeff, &lightSpecularCoeff, &renderer, &flags, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -6005,6 +6108,10 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec
 		b3RequestCameraImageSetLightSpecularCoeff(command, lightSpecularCoeff);
 	}
 
+	if (flags >= 0)
+	{
+		b3RequestCameraImageSetFlags(command, flags);
+	}
 	if (renderer>=0)
 	{
 		b3RequestCameraImageSelectRenderer(command, renderer);//renderer could be ER_BULLET_HARDWARE_OPENGL
@@ -7761,11 +7868,17 @@ static PyMethodDef SpamMethods[] = {
 	 "Load multibodies from an SDF file."},
 
 	{"loadBullet", (PyCFunction)pybullet_loadBullet, METH_VARARGS | METH_KEYWORDS,
-	 "Restore the full state of the world from a .bullet file."},
+	 "Load a world from a .bullet file."},
 
 	{"saveBullet", (PyCFunction)pybullet_saveBullet, METH_VARARGS | METH_KEYWORDS,
 	 "Save the full state of the world to a .bullet file."},
 
+	 { "restoreState", (PyCFunction)pybullet_restoreState, METH_VARARGS | METH_KEYWORDS,
+	"Restore the full state of an existing world." },
+
+//	{ "saveState", (PyCFunction)pybullet_saveState, METH_VARARGS | METH_KEYWORDS,
+//	"Save the full state of the world to memory." },
+
 	{"loadMJCF", (PyCFunction)pybullet_loadMJCF, METH_VARARGS | METH_KEYWORDS,
 	 "Load multibodies from an MJCF file."},
 
@@ -8257,6 +8370,7 @@ initpybullet(void)
 
 	PyModule_AddIntConstant(m, "ER_TINY_RENDERER", ER_TINY_RENDERER);
 	PyModule_AddIntConstant(m, "ER_BULLET_HARDWARE_OPENGL", ER_BULLET_HARDWARE_OPENGL);
+	PyModule_AddIntConstant(m, "ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX", ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX);
 
 	PyModule_AddIntConstant(m, "IK_DLS", IK_DLS);
 	PyModule_AddIntConstant(m, "IK_SDLS", IK_SDLS);

setup.py

@@ -443,7 +443,7 @@ print("-----")
 
 setup(
 	name = 'pybullet',
-	version='1.7.5',
+	version='1.7.6',
 	description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
 	long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
 	url='https://github.com/bulletphysics/bullet3',