Merge pull request #911 from erwincoumans/master

Add pybullet setVRCameraState, allow 2 simultaneous shared memory connections (with a few restrictions)

examples/ExampleBrowser/OpenGLExampleBrowser.cpp

@@ -162,7 +162,12 @@ FILE* gTimingFile = 0;
 #ifndef __STDC_FORMAT_MACROS
 #define __STDC_FORMAT_MACROS
 #endif //__STDC_FORMAT_MACROS
+
+//see http://stackoverflow.com/questions/18107426/printf-format-for-unsigned-int64-on-windows
+#ifndef _WIN32
 #include <inttypes.h>
+#endif
+
 #define BT_TIMING_CAPACITY 16*65536
 static bool m_firstTiming = true;
 
@@ -246,12 +251,21 @@ struct btTimings
             
             char newname[1024];
 			static int counter2=0;
-         
             sprintf(newname,"%s%d",name,counter2++);
+         
+#ifdef _WIN32
+			
+			fprintf(gTimingFile,"{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%I64d.%s ,\"ph\":\"B\",\"name\":\"%s\",\"args\":{}},\n",
+				threadId, startTimeDiv1000,startTimeRem1000Str, newname);
+			fprintf(gTimingFile,"{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%I64d.%s ,\"ph\":\"E\",\"name\":\"%s\",\"args\":{}}",
+				threadId, endTimeDiv1000,endTimeRem1000Str,newname);
+
+#else
 			fprintf(gTimingFile,"{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%" PRIu64 ".%s ,\"ph\":\"B\",\"name\":\"%s\",\"args\":{}},\n",
 				threadId, startTimeDiv1000,startTimeRem1000Str, newname);
 			fprintf(gTimingFile,"{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%" PRIu64 ".%s ,\"ph\":\"E\",\"name\":\"%s\",\"args\":{}}",
 				threadId, endTimeDiv1000,endTimeRem1000Str,newname);
+#endif
 #endif
 
 		}

examples/OpenGLWindow/SimpleCamera.cpp

@@ -71,6 +71,21 @@ void	SimpleCamera::setVRCamera(const float viewMat[16], const float projectionMa
 	}
 }
 
+bool	SimpleCamera::getVRCamera(float viewMat[16], float projectionMatrix[16])
+{
+	if (m_data->m_enableVR)
+	{
+		for (int i=0;i<16;i++)
+		{
+			viewMat[i] = m_data->m_viewMatrixVR[i];
+			projectionMatrix[i] = m_data->m_projectionMatrixVR[i];
+		}
+	}
+	return false;
+}
+
+
+
 void SimpleCamera::disableVRCamera()
 {
 	m_data->m_enableVR = false;

examples/OpenGLWindow/SimpleCamera.h

@@ -15,6 +15,8 @@ struct SimpleCamera : public CommonCameraInterface
 	virtual void getCameraViewMatrix(float m[16]) const;
 	
 	virtual void	setVRCamera(const float viewMat[16], const float projectionMatrix[16]);
+	virtual bool	getVRCamera(float viewMat[16], float projectionMatrix[16]);
+
 	virtual void	setVRCameraOffsetTransform(const float offset[16]);
 	virtual void disableVRCamera();
 

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -2191,3 +2191,54 @@ void b3GetVREventsData(b3PhysicsClientHandle physClient, struct b3VREventsData*
 		cl->getCachedVREvents(vrEventsData);
 	}
 }
+
+b3SharedMemoryCommandHandle	b3SetVRCameraStateCommandInit(b3PhysicsClientHandle physClient)
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+	struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+	b3Assert(command);
+
+	command->m_type = CMD_SET_VR_CAMERA_STATE;
+	command->m_updateFlags = 0;
+
+	return (b3SharedMemoryCommandHandle)command;
+
+}
+
+int b3SetVRCameraRootPosition(b3SharedMemoryCommandHandle commandHandle, double rootPos[3])
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_SET_VR_CAMERA_STATE);
+    command->m_updateFlags |= VR_CAMERA_ROOT_POSITION;
+	command->m_vrCameraStateArguments.m_rootPosition[0] = rootPos[0];
+	command->m_vrCameraStateArguments.m_rootPosition[1] = rootPos[1];
+	command->m_vrCameraStateArguments.m_rootPosition[2] = rootPos[2];
+	return 0;
+
+}
+
+int b3SetVRCameraRootOrientation(b3SharedMemoryCommandHandle commandHandle, double rootOrn[4])
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_SET_VR_CAMERA_STATE);
+    command->m_updateFlags |= VR_CAMERA_ROOT_ORIENTATION;
+	command->m_vrCameraStateArguments.m_rootOrientation[0] = rootOrn[0];
+	command->m_vrCameraStateArguments.m_rootOrientation[1] = rootOrn[1];
+	command->m_vrCameraStateArguments.m_rootOrientation[2] = rootOrn[2];
+	return 0;
+}
+
+int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_SET_VR_CAMERA_STATE);
+    command->m_updateFlags |= VR_CAMERA_ROOT_TRACKING_OBJECT;
+	command->m_vrCameraStateArguments.m_trackingObjectUniqueId = objectUniqueId;
+	return 0;
+}
+

examples/SharedMemory/PhysicsClientC_API.h

@@ -314,6 +314,12 @@ int b3LoadBunnySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, dou
 b3SharedMemoryCommandHandle	b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient);
 void b3GetVREventsData(b3PhysicsClientHandle physClient, struct b3VREventsData* vrEventsData);
 
+b3SharedMemoryCommandHandle	b3SetVRCameraStateCommandInit(b3PhysicsClientHandle physClient);
+int b3SetVRCameraRootPosition(b3SharedMemoryCommandHandle commandHandle, double rootPos[3]);
+int b3SetVRCameraRootOrientation(b3SharedMemoryCommandHandle commandHandle, double rootOrn[4]);
+int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId);
+
+
 
 
 #ifdef __cplusplus

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -44,11 +44,14 @@ bool gEnableRealTimeSimVR=false;
 bool gCreateDefaultRobotAssets = false;
 int gInternalSimFlags = 0;
 bool gResetSimulation = 0;
-int gHuskyId = -1;
+int gVRTrackingObjectUniqueId = -1;
-btTransform huskyTr = btTransform::getIdentity();
+btTransform gVRTrackingObjectTr = btTransform::getIdentity();
 
 int gCreateObjectSimVR = -1;
 int gEnableKukaControl = 0;
+btVector3 gVRTeleportPos1(0,0,0);
+btQuaternion gVRTeleportOrn(0, 0, 0,1);
+
 
 btScalar simTimeScalingFactor = 1;
 btScalar gRhsClamp = 1.f;
@@ -1369,6 +1372,32 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 				}
 #endif
 
+				case CMD_SET_VR_CAMERA_STATE:
+				{
+
+					if (clientCmd.m_updateFlags & 	VR_CAMERA_ROOT_POSITION)
+					{
+						gVRTeleportPos1[0] = clientCmd.m_vrCameraStateArguments.m_rootPosition[0];
+						gVRTeleportPos1[1] = clientCmd.m_vrCameraStateArguments.m_rootPosition[1];
+						gVRTeleportPos1[2] = clientCmd.m_vrCameraStateArguments.m_rootPosition[2];
+					}
+					if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_ORIENTATION)
+					{
+						gVRTeleportOrn[0] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[0];
+						gVRTeleportOrn[1] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[1];
+						gVRTeleportOrn[2] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[2];
+						gVRTeleportOrn[3] = clientCmd.m_vrCameraStateArguments.m_rootOrientation[3];
+					}
+
+					if (clientCmd.m_updateFlags & VR_CAMERA_ROOT_TRACKING_OBJECT)
+					{
+						gVRTrackingObjectUniqueId = clientCmd.m_vrCameraStateArguments.m_trackingObjectUniqueId;
+					}
+
+					serverStatusOut.m_type  = CMD_CLIENT_COMMAND_COMPLETED;
+					hasStatus = true;
+					break;
+				}
 				case CMD_REQUEST_VR_EVENTS_DATA:
 				{
 					serverStatusOut.m_sendVREvents.m_numVRControllerEvents = 0;
@@ -4287,12 +4316,12 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec,	const
 			gSubStep = m_data->m_physicsDeltaTime;
 		}
 		
-		if (gHuskyId >= 0)
+		if (gVRTrackingObjectUniqueId >= 0)
 		{
-			InternalBodyHandle* bodyHandle = m_data->getHandle(gHuskyId);
+			InternalBodyHandle* bodyHandle = m_data->getHandle(gVRTrackingObjectUniqueId);
 			if (bodyHandle && bodyHandle->m_multiBody)
 			{
-				huskyTr = bodyHandle->m_multiBody->getBaseWorldTransform();
+				gVRTrackingObjectTr  = bodyHandle->m_multiBody->getBaseWorldTransform();
 			}
 		}
 
@@ -4595,8 +4624,6 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
 		//loadUrdf("knight.urdf", btVector3(-1.2, 0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 
 		loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		gHuskyId = bodyId;
-		b3Printf("huskyId = %d", gHuskyId);
 		m_data->m_huskyId = bodyId;
 
 		m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10));

examples/SharedMemory/PhysicsServerExample.cpp

@@ -26,9 +26,14 @@
 
 //@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
 extern btVector3 gLastPickPos;
-btVector3 gVRTeleportPos1(0,0,0);
+
+btVector3 gVRTeleportPosLocal(0,0,0);
+btQuaternion gVRTeleportOrnLocal(0,0,0,1);
+
+extern btVector3 gVRTeleportPos1;
+extern btQuaternion gVRTeleportOrn;
 btScalar gVRTeleportRotZ = 0;
-btQuaternion gVRTeleportOrn(0, 0, 0,1);
+
 extern btVector3 gVRGripperPos;
 extern btQuaternion gVRGripperOrn;
 extern btVector3 gVRController2Pos;
@@ -57,9 +62,9 @@ static void saveCurrentSettingsVR()
 	FILE* f = fopen(startFileNameVR, "w");
 	if (f)
 	{
-		fprintf(f, "--camPosX= %f\n", gVRTeleportPos1[0]);
+		fprintf(f, "--camPosX= %f\n", gVRTeleportPosLocal[0]);
-		fprintf(f, "--camPosY= %f\n", gVRTeleportPos1[1]);
+		fprintf(f, "--camPosY= %f\n", gVRTeleportPosLocal[1]);
-		fprintf(f, "--camPosZ= %f\n", gVRTeleportPos1[2]);
+		fprintf(f, "--camPosZ= %f\n", gVRTeleportPosLocal[2]);
 		fprintf(f, "--camRotZ= %f\n", gVRTeleportRotZ);
 		fclose(f);
 	}
@@ -109,9 +114,9 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void
 			if (message->at(1) == 16)
 			{
 				gVRTeleportRotZ= getParamf(-3.1415, 3.1415, message->at(2));
-				gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ);
+				gVRTeleportOrnLocal = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ);
 				saveCurrentSettingsVR();
-//				b3Printf("gVRTeleportOrn rotZ = %f\n", gVRTeleportRotZ);
+//				b3Printf("gVRTeleportOrnLocal rotZ = %f\n", gVRTeleportRotZ);
 			}
 
 			if (message->at(1) == 32)
@@ -123,9 +128,9 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void
 			{
 				if (message->at(1) == i)
 				{
-					gVRTeleportPos1[i] = getParamf(-2, 2, message->at(2));
+					gVRTeleportPosLocal[i] = getParamf(-2, 2, message->at(2));
 					saveCurrentSettingsVR();
-//					b3Printf("gVRTeleportPos[%d] =  %f\n", i,gVRTeleportPos1[i]);
+//					b3Printf("gVRTeleportPos[%d] =  %f\n", i,gVRTeleportPosLocal[i]);
 
 				}
 			}
@@ -1040,19 +1045,19 @@ public:
 			setSharedMemoryKey(shmemKey);
 		}
 
-		if (args.GetCmdLineArgument("camPosX", gVRTeleportPos1[0]))
+		if (args.GetCmdLineArgument("camPosX", gVRTeleportPosLocal[0]))
 		{
-			printf("camPosX=%f\n", gVRTeleportPos1[0]);
+			printf("camPosX=%f\n", gVRTeleportPosLocal[0]);
 		}
 
-		if (args.GetCmdLineArgument("camPosY", gVRTeleportPos1[1]))
+		if (args.GetCmdLineArgument("camPosY", gVRTeleportPosLocal[1]))
 		{
-			printf("camPosY=%f\n", gVRTeleportPos1[1]);
+			printf("camPosY=%f\n", gVRTeleportPosLocal[1]);
 		}
 
-		if (args.GetCmdLineArgument("camPosZ", gVRTeleportPos1[2]))
+		if (args.GetCmdLineArgument("camPosZ", gVRTeleportPosLocal[2]))
 		{
-			printf("camPosZ=%f\n", gVRTeleportPos1[2]);
+			printf("camPosZ=%f\n", gVRTeleportPosLocal[2]);
 		}
 
 		float camRotZ = 0.f;
@@ -1060,7 +1065,7 @@ public:
 		{
 			printf("camRotZ = %f\n", camRotZ);
 			btQuaternion ornZ(btVector3(0, 0, 1), camRotZ);
-			gVRTeleportOrn = ornZ;
+			gVRTeleportOrnLocal = ornZ;
 		}
 
 		if (args.CheckCmdLineFlag("robotassets"))
@@ -1439,23 +1444,7 @@ void	PhysicsServerExample::stepSimulation(float deltaTime)
 
 
 
-	#if 0
+	
-	if (m_options == PHYSICS_SERVER_USE_RTC_CLOCK)
-	{
-		btClock rtc;
-		btScalar endTime = rtc.getTimeMilliseconds() + deltaTime*btScalar(800);
-
-		while (rtc.getTimeMilliseconds()<endTime)
-		{
-			m_physicsServer.processClientCommands();
-		}
-	} else
-	{
-        //for (int i=0;i<10;i++)
-			m_physicsServer.processClientCommands();
-	}
-	#endif
-
 	{
 		if (m_multiThreadedHelper->m_childGuiHelper->getRenderInterface())
 		{
@@ -1474,8 +1463,8 @@ extern int gDroppedSimulationSteps;
 extern int gNumSteps;
 extern double gDtInSec;
 extern double gSubStep;
-extern int gHuskyId;
+extern int gVRTrackingObjectUniqueId;
-extern btTransform huskyTr;
+extern btTransform gVRTrackingObjectTr;
 
   struct LineSegment
                 {
@@ -1607,15 +1596,22 @@ void PhysicsServerExample::drawUserDebugLines()
 
 void PhysicsServerExample::renderScene()
 {
+	btTransform vrTrans;
+	gVRTeleportPos1 = gVRTeleportPosLocal;
+	gVRTeleportOrn = gVRTeleportOrnLocal;
 
-#if 0
 	///little VR test to follow/drive Husky vehicle
-	if (gHuskyId >= 0)
+	if (gVRTrackingObjectUniqueId >= 0)
 	{
-		gVRTeleportPos1 = huskyTr.getOrigin();
+		btTransform vrTrans;
-		gVRTeleportOrn = huskyTr.getRotation();
+		vrTrans.setOrigin(gVRTeleportPosLocal);
+		vrTrans.setRotation(gVRTeleportOrnLocal);
+			
+		vrTrans = vrTrans * gVRTrackingObjectTr;
+
+		gVRTeleportPos1 = vrTrans.getOrigin();
+		gVRTeleportOrn = vrTrans.getRotation();
 	}
-#endif
 		
 
 	B3_PROFILE("PhysicsServerExample::RenderScene");

examples/SharedMemory/PhysicsServerSharedMemory.cpp

@@ -14,7 +14,8 @@
 
 #include "PhysicsServerCommandProcessor.h"
 
-
+//number of shared memory blocks == number of simultaneous connections
+#define MAX_SHARED_MEMORY_BLOCKS 2
 
 struct PhysicsServerSharedMemoryInternalData
 {
@@ -25,36 +26,39 @@ struct PhysicsServerSharedMemoryInternalData
 	SharedMemoryInterface* m_sharedMemory;
 	bool m_ownsSharedMemory;
 
-    SharedMemoryBlock* m_testBlock1;
+    SharedMemoryBlock* m_testBlocks[MAX_SHARED_MEMORY_BLOCKS];
 	int m_sharedMemoryKey;
-	bool m_isConnected;
+	bool m_areConnected[MAX_SHARED_MEMORY_BLOCKS];
 	bool m_verboseOutput;
 	PhysicsServerCommandProcessor* m_commandProcessor;
 	
 	PhysicsServerSharedMemoryInternalData()
 		:m_sharedMemory(0),
 		m_ownsSharedMemory(false),
-		m_testBlock1(0),
+  		m_sharedMemoryKey(SHARED_MEMORY_KEY),
-		m_sharedMemoryKey(SHARED_MEMORY_KEY),
+    	m_verboseOutput(false),
-		m_isConnected(false),
-		m_verboseOutput(false),
 		m_commandProcessor(0)
 		
 	{
-    
+ 
+        for (int i=0;i<MAX_SHARED_MEMORY_BLOCKS;i++)
+        {
+            m_testBlocks[i]=0;
+            m_areConnected[i]=false;
+        }
 	}
 
-	SharedMemoryStatus& createServerStatus(int statusType, int sequenceNumber, int timeStamp)
+	SharedMemoryStatus& createServerStatus(int statusType, int sequenceNumber, int timeStamp, int blockIndex)
 	{
-		SharedMemoryStatus& serverCmd =m_testBlock1->m_serverCommands[0];
+		SharedMemoryStatus& serverCmd =m_testBlocks[blockIndex]->m_serverCommands[0];
 		serverCmd .m_type = statusType;
 		serverCmd.m_sequenceNumber = sequenceNumber;
 		serverCmd.m_timeStamp = timeStamp;
 		return serverCmd;
 	}
-	void submitServerStatus(SharedMemoryStatus& status)
+	void submitServerStatus(SharedMemoryStatus& status,int blockIndex)
 	{
-		m_testBlock1->m_numServerCommands++;
+		m_testBlocks[blockIndex]->m_numServerCommands++;
 	}
 
 };
@@ -84,9 +88,25 @@ PhysicsServerSharedMemory::PhysicsServerSharedMemory(SharedMemoryInterface* shar
 
 PhysicsServerSharedMemory::~PhysicsServerSharedMemory()
 {
+
 	m_data->m_commandProcessor->deleteDynamicsWorld();
 	delete m_data->m_commandProcessor;
-	delete m_data;
+
+    if (m_data->m_sharedMemory)
+    {
+        if (m_data->m_verboseOutput)
+        {
+            b3Printf("m_sharedMemory\n");
+        }
+        if (m_data->m_ownsSharedMemory)
+        {
+            delete m_data->m_sharedMemory;
+        }
+        m_data->m_sharedMemory = 0;
+    }
+
+    
+    delete m_data;
 }
 
 void PhysicsServerSharedMemory::resetDynamicsWorld()
@@ -107,55 +127,61 @@ bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface*
 
 	
 	bool allowCreation = true;
+    bool allConnected = false;
+    
 	
 
-    if (m_data->m_isConnected)
+   
-    {
-        b3Warning("connectSharedMemory, while already connected");
-        return m_data->m_isConnected;
-    }
     
     
 	int counter = 0;
-	do 
+    for (int block=0;block<MAX_SHARED_MEMORY_BLOCKS;block++)
-	{
+    {
+        if (m_data->m_areConnected[block])
+        {
+            allConnected = true;
+            b3Warning("connectSharedMemory, while already connected");
+            continue;
+        }
+        do
+        {
 
-		m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE,allowCreation);
+            m_data->m_testBlocks[block] = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey+block, SHARED_MEMORY_SIZE,allowCreation);
-		if (m_data->m_testBlock1)
+            if (m_data->m_testBlocks[block])
-		{
+            {
-			int magicId =m_data->m_testBlock1->m_magicId;
+                int magicId =m_data->m_testBlocks[block]->m_magicId;
-			if (m_data->m_verboseOutput)
+                if (m_data->m_verboseOutput)
-			{
+                {
-				b3Printf("magicId = %d\n", magicId);
+                    b3Printf("magicId = %d\n", magicId);
-			}
+                }
-        
+            
-			if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
+                if (m_data->m_testBlocks[block]->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
-			{
+                {
-				InitSharedMemoryBlock(m_data->m_testBlock1);
+                    InitSharedMemoryBlock(m_data->m_testBlocks[block]);
-				if (m_data->m_verboseOutput)
+                    if (m_data->m_verboseOutput)
-				{
+                    {
-					b3Printf("Created and initialized shared memory block\n");
+                        b3Printf("Created and initialized shared memory block\n");
-				}
+                    }
-				m_data->m_isConnected = true;
+                    m_data->m_areConnected[block] = true;
-			} else
+                } else
-			{
+                {
-				m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE);
+                    m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey+block, SHARED_MEMORY_SIZE);
-				m_data->m_testBlock1 = 0;
+                    m_data->m_testBlocks[block] = 0;
-				m_data->m_isConnected = false;
+                    m_data->m_areConnected[block] = false;
-			}
+                }
-		} else
+            } else
-		{
+            {
-			b3Error("Cannot connect to shared memory");
+                b3Error("Cannot connect to shared memory");
-			m_data->m_isConnected = false;
+                m_data->m_areConnected[block] = false;
-		}
+            }
-	} while (counter++ < 10 && !m_data->m_isConnected);
+        } while (counter++ < 10 && !m_data->m_areConnected[block]);
-
+        if (!m_data->m_areConnected[block])
-	if (!m_data->m_isConnected)
+        {
-	{
+            b3Error("Server cannot connect to shared memory.\n");
-		b3Error("Server cannot connect to shared memory.\n");
+        }
-	}
+    }
 	
-	return m_data->m_isConnected;
+	return allConnected;
 }
 
 
@@ -167,72 +193,34 @@ void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMe
 	{
 		b3Printf("releaseSharedMemory1\n");
 	}
-	if (m_data->m_testBlock1)
+    for (int block = 0;block<MAX_SHARED_MEMORY_BLOCKS;block++)
-	{
+    {
-		if (m_data->m_verboseOutput)
+        if (m_data->m_testBlocks[block])
-		{
+        {
-			b3Printf("m_testBlock1\n");
+            if (m_data->m_verboseOutput)
-		}
+            {
-		if (deInitializeSharedMemory)
+                b3Printf("m_testBlock1\n");
-		{
+            }
-			m_data->m_testBlock1->m_magicId = 0;
+            if (deInitializeSharedMemory)
-			if (m_data->m_verboseOutput)
+            {
-			{
+                m_data->m_testBlocks[block]->m_magicId = 0;
-				b3Printf("De-initialized shared memory, magic id = %d\n",m_data->m_testBlock1->m_magicId);
+                if (m_data->m_verboseOutput)
-			}
+                {
-		}
+                    b3Printf("De-initialized shared memory, magic id = %d\n",m_data->m_testBlocks[block]->m_magicId);
-		btAssert(m_data->m_sharedMemory);
+                }
-		m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE);
+            }
-	}
+            btAssert(m_data->m_sharedMemory);
-	if (m_data->m_sharedMemory)
+            m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey+block, SHARED_MEMORY_SIZE);
-	{
+        }
-		if (m_data->m_verboseOutput)
+        m_data->m_testBlocks[block] = 0;
-		{
+        m_data->m_areConnected[block] = false;
-			b3Printf("m_sharedMemory\n");
+        
-		}
+    }
-		if (m_data->m_ownsSharedMemory)
-		{
-			delete m_data->m_sharedMemory;
-		}
-		m_data->m_sharedMemory = 0;
-		m_data->m_testBlock1 = 0;
-	}
 }
 
 void PhysicsServerSharedMemory::releaseSharedMemory()
 {
-	if (m_data->m_verboseOutput)
+    disconnectSharedMemory(true);
-	{
-		b3Printf("releaseSharedMemory1\n");
-	}
-    if (m_data->m_testBlock1)
-    {
-		if (m_data->m_verboseOutput)
-		{
-			b3Printf("m_testBlock1\n");
-		}
-        m_data->m_testBlock1->m_magicId = 0;
-		if (m_data->m_verboseOutput)
-		{
-			b3Printf("magic id = %d\n",m_data->m_testBlock1->m_magicId);
-		}
-        btAssert(m_data->m_sharedMemory);
-		m_data->m_sharedMemory->releaseSharedMemory(	m_data->m_sharedMemoryKey
-, SHARED_MEMORY_SIZE);
-    }
-    if (m_data->m_sharedMemory)
-    {
-		if (m_data->m_verboseOutput)
-		{
-			b3Printf("m_sharedMemory\n");
-		}
-		if (m_data->m_ownsSharedMemory)
-		{
-	        delete m_data->m_sharedMemory;
-		}
-        m_data->m_sharedMemory = 0;
-        m_data->m_testBlock1 = 0;
-    }
 }
 
 
@@ -250,30 +238,34 @@ void PhysicsServerSharedMemory::enableRealTimeSimulation(bool enableRealTimeSim)
 
 void PhysicsServerSharedMemory::processClientCommands()
 {
-	if (m_data->m_isConnected && m_data->m_testBlock1)
+    for (int block = 0;block<MAX_SHARED_MEMORY_BLOCKS;block++)
     {
-        m_data->m_commandProcessor->replayLogCommand(&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
         
-        ///we ignore overflow of integer for now
+        if (m_data->m_areConnected[block] && m_data->m_testBlocks[block])
-        if (m_data->m_testBlock1->m_numClientCommands> m_data->m_testBlock1->m_numProcessedClientCommands)
         {
-           
+            m_data->m_commandProcessor->replayLogCommand(&m_data->m_testBlocks[block]->m_bulletStreamDataServerToClientRefactor[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
-
-            //until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands
-            btAssert(m_data->m_testBlock1->m_numClientCommands==m_data->m_testBlock1->m_numProcessedClientCommands+1);
             
-			const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0];
+            ///we ignore overflow of integer for now
+            if (m_data->m_testBlocks[block]->m_numClientCommands> m_data->m_testBlocks[block]->m_numProcessedClientCommands)
+            {
+               
 
-			m_data->m_testBlock1->m_numProcessedClientCommands++;
+                //until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands
-			//todo, timeStamp 
+                btAssert(m_data->m_testBlocks[block]->m_numClientCommands==m_data->m_testBlocks[block]->m_numProcessedClientCommands+1);
-			int timeStamp = 0;
+                
-			SharedMemoryStatus& serverStatusOut = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
+                const SharedMemoryCommand& clientCmd =m_data->m_testBlocks[block]->m_clientCommands[0];
-			bool hasStatus = m_data->m_commandProcessor->processCommand(clientCmd, serverStatusOut,&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+
-			if (hasStatus)
+                m_data->m_testBlocks[block]->m_numProcessedClientCommands++;
-			{
+                //todo, timeStamp 
-				m_data->submitServerStatus(serverStatusOut);
+                int timeStamp = 0;
-			}
+                SharedMemoryStatus& serverStatusOut = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp,block);
-			       
+                bool hasStatus = m_data->m_commandProcessor->processCommand(clientCmd, serverStatusOut,&m_data->m_testBlocks[block]->m_bulletStreamDataServerToClientRefactor[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+                if (hasStatus)
+                {
+                    m_data->submitServerStatus(serverStatusOut,block);
+                }
+                       
+            }
         }
     }
 }

examples/SharedMemory/PosixSharedMemory.cpp

@@ -1,6 +1,7 @@
 #include "PosixSharedMemory.h"
 #include "Bullet3Common/b3Logging.h"
 #include "LinearMath/btScalar.h" //for btAssert
+#include "LinearMath/btAlignedObjectArray.h"
 
 //Windows implementation is in Win32SharedMemory.cpp
 #ifndef _WIN32
@@ -16,16 +17,28 @@
 
 #endif
 
+struct btSharedMemorySegment
+{
+    int m_key;
+    int m_sharedMemoryId;
+    void* m_sharedMemoryPtr;
+    bool m_createdSharedMemory;
+    
+    btSharedMemorySegment()
+    :    m_sharedMemoryId(-1),
+    m_sharedMemoryPtr(0),
+    m_createdSharedMemory(true)
+    {
+    }
+    
+};
+
 struct PosixSharedMemoryInteralData
 {
-	bool m_createdSharedMemory;
+    btAlignedObjectArray<btSharedMemorySegment> m_segments;
-	int m_sharedMemoryId;
+    
-	void* m_sharedMemoryPtr;
+		
-	
 	PosixSharedMemoryInteralData()
-	:m_createdSharedMemory(false),
-	m_sharedMemoryId(-1),
-	m_sharedMemoryPtr(0)
 	{
 	}
 };
@@ -53,6 +66,27 @@ struct btPointerCaster
 void*   PosixSharedMemory::allocateSharedMemory(int key, int size,  bool allowCreation)
 {
 #ifdef TEST_SHARED_MEMORY
+    
+    {
+        btSharedMemorySegment* seg = 0;
+        int i=0;
+        
+        for (i=0;i<m_internalData->m_segments.size();i++)
+        {
+            if (m_internalData->m_segments[i].m_key == key)
+            {
+                seg = &m_internalData->m_segments[i];
+                break;
+            }
+        }
+        if (seg)
+        {
+            b3Error("already created shared memory segment using same key");
+            return seg->m_sharedMemoryPtr;
+        }
+
+    }
+    
     int flags = (allowCreation ? IPC_CREAT : 0) | 0666;
     int id = shmget((key_t) key, (size_t) size,flags);
     if (id < 0)
@@ -67,9 +101,12 @@ void*   PosixSharedMemory::allocateSharedMemory(int key, int size,  bool allowCr
             b3Error("shmat returned -1");
         } else
         {
-			m_internalData->m_createdSharedMemory = allowCreation;
+            btSharedMemorySegment seg;
-			m_internalData->m_sharedMemoryId = id;
+            seg.m_key = key;
-			m_internalData->m_sharedMemoryPtr = result.ptr;
+            seg.m_createdSharedMemory = allowCreation;
+            seg.m_sharedMemoryId = id;
+            seg.m_sharedMemoryPtr = result.ptr;
+            m_internalData->m_segments.push_back(seg);
             return result.ptr;
         }
     }
@@ -82,14 +119,33 @@ void*   PosixSharedMemory::allocateSharedMemory(int key, int size,  bool allowCr
 void PosixSharedMemory::releaseSharedMemory(int key, int size)
 {
 #ifdef TEST_SHARED_MEMORY
-    if (m_internalData->m_sharedMemoryId < 0)
+    
+    btSharedMemorySegment* seg = 0;
+    int i=0;
+    
+    for (i=0;i<m_internalData->m_segments.size();i++)
+    {
+        if (m_internalData->m_segments[i].m_key == key)
+        {
+            seg = &m_internalData->m_segments[i];
+            break;
+        }
+    }
+
+    if (0==seg)
+    {
+        b3Error("PosixSharedMemory::releaseSharedMemory: shared memory key not found");
+        return;
+    }
+    
+    if (seg->m_sharedMemoryId < 0)
     {
         b3Error("PosixSharedMemory::releaseSharedMemory: shared memory id is not set");
     } else
     {
-		if (m_internalData->m_createdSharedMemory)
+		if (seg->m_createdSharedMemory)
 		{
-			int result = shmctl(m_internalData->m_sharedMemoryId,IPC_RMID,0);
+			int result = shmctl(seg->m_sharedMemoryId,IPC_RMID,0);
 			if (result == -1)
 			{
 				b3Error("PosixSharedMemory::releaseSharedMemory: shmat returned -1");
@@ -97,15 +153,18 @@ void PosixSharedMemory::releaseSharedMemory(int key, int size)
 			{
 				b3Printf("PosixSharedMemory::releaseSharedMemory removed shared memory");
 			}
-			m_internalData->m_createdSharedMemory = false;
+			seg->m_createdSharedMemory = false;
-			m_internalData->m_sharedMemoryId = -1;
+			seg->m_sharedMemoryId = -1;
 		}
-		if (m_internalData->m_sharedMemoryPtr)
+		if (seg->m_sharedMemoryPtr)
 		{
-			shmdt(m_internalData->m_sharedMemoryPtr);
+			shmdt(seg->m_sharedMemoryPtr);
-			m_internalData->m_sharedMemoryPtr  = 0;
+			seg->m_sharedMemoryPtr  = 0;
 			b3Printf("PosixSharedMemory::releaseSharedMemory detached shared memory\n");
 		}
     }
+
+    m_internalData->m_segments.removeAtIndex(i);
+    
 #endif
 }

examples/SharedMemory/SharedMemoryCommands.h

@@ -594,6 +594,26 @@ struct UserDebugDrawResultArgs
 	int m_debugItemUniqueId;
 };
 
+struct SendVREvents
+{
+	int m_numVRControllerEvents;
+	b3VRControllerEvent m_controllerEvents[MAX_VR_CONTROLLERS];
+};
+
+enum eVRCameraEnums
+{
+	VR_CAMERA_ROOT_POSITION=1,
+	VR_CAMERA_ROOT_ORIENTATION=2,
+	VR_CAMERA_ROOT_TRACKING_OBJECT=4
+};
+
+struct VRCameraState
+{
+	double m_rootPosition[3];
+	double m_rootOrientation[4];
+	int m_trackingObjectUniqueId;
+};
+
 
 struct SharedMemoryCommand
 {
@@ -635,6 +655,7 @@ struct SharedMemoryCommand
 		struct UserDebugDrawArgs m_userDebugDrawArgs;
 		struct RequestRaycastIntersections m_requestRaycastIntersections;
         struct LoadBunnyArgs m_loadBunnyArguments;
+		struct VRCameraState m_vrCameraStateArguments;
     };
 };
 
@@ -657,11 +678,8 @@ struct SendOverlappingObjectsArgs
 	int m_numRemainingOverlappingObjects;
 };
 
-struct SendVREvents
+
-{
+
-	int m_numVRControllerEvents;
-	b3VRControllerEvent m_controllerEvents[MAX_VR_CONTROLLERS];
-};
 
 
 

examples/SharedMemory/SharedMemoryPublic.h

@@ -46,6 +46,8 @@ enum EnumSharedMemoryClientCommand
     CMD_SET_SHADOW,
 	CMD_USER_DEBUG_DRAW,
 	CMD_REQUEST_VR_EVENTS_DATA,
+	CMD_SET_VR_CAMERA_STATE,
+
     //don't go beyond this command!
     CMD_MAX_CLIENT_COMMANDS,
     

examples/SharedMemory/Win32SharedMemory.cpp

@@ -2,22 +2,36 @@
 #include "Win32SharedMemory.h"
 #include "Bullet3Common/b3Logging.h"
 #include "Bullet3Common/b3Scalar.h"
+#include "LinearMath/btAlignedObjectArray.h"
 
 #include <windows.h>
 #include <stdio.h>
 //see also https://msdn.microsoft.com/en-us/library/windows/desktop/aa366551%28v=vs.85%29.aspx
 
 
-struct Win32SharedMemoryInteralData
+struct Win32SharedMemorySegment
 {
+	int m_key;
 	HANDLE m_hMapFile;
-
 	void*	m_buf;
 	TCHAR m_szName[1024];
+
+	Win32SharedMemorySegment()
+			:m_hMapFile(0),
+			m_buf(0),
+			m_key(-1)
+	{
+		m_szName[0] = 0;
+	}
+
+};
+
+struct Win32SharedMemoryInteralData
+{
 	
+	btAlignedObjectArray<Win32SharedMemorySegment> m_segments;
+
 	Win32SharedMemoryInteralData()
-		:m_hMapFile(0),
-		m_buf(0)
 	{
 	}
 };
@@ -33,32 +47,53 @@ Win32SharedMemory::~Win32SharedMemory()
 
 void*   Win32SharedMemory::allocateSharedMemory(int key, int size, bool allowCreation)
 {
-	b3Assert(m_internalData->m_buf==0);
+	{
+		
+        Win32SharedMemorySegment* seg = 0;
+        int i=0;
+        
+        for (i=0;i<m_internalData->m_segments.size();i++)
+        {
+            if (m_internalData->m_segments[i].m_key == key)
+            {
+                seg = &m_internalData->m_segments[i];
+                break;
+            }
+        }
+        if (seg)
+        {
+            b3Error("already created shared memory segment using same key");
+            return seg->m_buf;
+        }
 
+    }
+
+	Win32SharedMemorySegment seg;
+	seg.m_key = key;
 #ifdef UNICODE
-	swprintf_s (m_internalData->m_szName,TEXT("MyFileMappingObject%d"),key);
+	swprintf_s (seg.m_szName,TEXT("MyFileMappingObject%d"),key);
 #else
 	
-	sprintf(m_internalData->m_szName,"MyFileMappingObject%d",key);	
+	sprintf(seg.m_szName,"MyFileMappingObject%d",key);	
 #endif
 
 
-	m_internalData->m_hMapFile = OpenFileMapping(
+	seg.m_hMapFile = OpenFileMapping(
                    FILE_MAP_ALL_ACCESS,   // read/write access
                    FALSE,                 // do not inherit the name
-                   m_internalData->m_szName);               // name of mapping object
+                   seg.m_szName);               // name of mapping object
 
-	if (m_internalData->m_hMapFile==NULL)
+	if (seg.m_hMapFile==NULL)
 	{
 		 if (allowCreation)
 		 {
-			  m_internalData->m_hMapFile = CreateFileMapping(
+			  seg.m_hMapFile = CreateFileMapping(
                  INVALID_HANDLE_VALUE,    // use paging file
                  NULL,                    // default security
                  PAGE_READWRITE,          // read/write access
                  0,                       // maximum object size (high-order DWORD)
                  size,						// maximum object size (low-order DWORD)
-                 m_internalData->m_szName);                 // name of mapping object
+                 seg.m_szName);                 // name of mapping object
 		 } else
 		 {
 			   b3Warning("Could not create file mapping object (%d).\n",GetLastError());
@@ -67,37 +102,56 @@ void*   Win32SharedMemory::allocateSharedMemory(int key, int size, bool allowCre
 
 	}
 
-   m_internalData->m_buf =  MapViewOfFile(m_internalData->m_hMapFile,   // handle to map object
+   seg.m_buf =  MapViewOfFile(seg.m_hMapFile,   // handle to map object
                         FILE_MAP_ALL_ACCESS, // read/write permission
                         0,
                         0,
                         size);
 
-   if (m_internalData->m_buf == NULL)
+   if (seg.m_buf == NULL)
 	{
 		b3Warning("Could not map view of file (%d).\n",GetLastError());
-		CloseHandle(m_internalData->m_hMapFile);
+		CloseHandle(seg.m_hMapFile);
 		return 0;
    }
 
-   return m_internalData->m_buf;
+   m_internalData->m_segments.push_back(seg);
+   return seg.m_buf;
 }
 void Win32SharedMemory::releaseSharedMemory(int key, int size)
 {
 
-	if (m_internalData->m_buf)
+    Win32SharedMemorySegment* seg = 0;
-	{
+    int i=0;
+        
+    for (i=0;i<m_internalData->m_segments.size();i++)
+    {
+        if (m_internalData->m_segments[i].m_key == key)
+        {
+            seg = &m_internalData->m_segments[i];
+            break;
+        }
+    }
 
-		UnmapViewOfFile(m_internalData->m_buf);
+	if (seg==0)
-		m_internalData->m_buf=0;
+	{
+		b3Error("Couldn't find shared memory segment");
+		return;
 	}
 
-	if (m_internalData->m_hMapFile)
+	if (seg->m_buf)
 	{
-		CloseHandle(m_internalData->m_hMapFile);
+		UnmapViewOfFile(seg->m_buf);
-		m_internalData->m_hMapFile = 0;
+		seg->m_buf=0;
 	}
 
+	if (seg->m_hMapFile)
+	{
+		CloseHandle(seg->m_hMapFile);
+		seg->m_hMapFile = 0;
+	}
+
+	m_internalData->m_segments.removeAtIndex(i);
 }
 
 Win32SharedMemoryServer::Win32SharedMemoryServer()

examples/pybullet/pybullet.c

@@ -1174,6 +1174,8 @@ static PyObject* pybullet_setGravity(PyObject* self, PyObject* args, PyObject* k
     double gravZ = -10.0;
     int ret;
 	b3PhysicsClientHandle sm  = 0;
+	b3SharedMemoryCommandHandle command;
+	b3SharedMemoryStatusHandle statusHandle;
 
 	int physicsClientId = 0;
 	static char *kwlist[] = { "gravX", "gravY", "gravZ", "physicsClientId", NULL };
@@ -1189,9 +1191,8 @@ static PyObject* pybullet_setGravity(PyObject* self, PyObject* args, PyObject* k
 	}
 	
 
-    b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
+    command = b3InitPhysicsParamCommand(sm);
-    b3SharedMemoryStatusHandle statusHandle;
+    
-
     ret = b3PhysicsParamSetGravity(command, gravX, gravY, gravZ);
     // ret = b3PhysicsParamSetTimeStep(command,  timeStep);
     statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
@@ -2431,6 +2432,56 @@ static PyObject* pybullet_getMatrixFromQuaterion(PyObject* self, PyObject* args)
 };
 
 
+static PyObject* pybullet_setVRCameraState(PyObject* self, PyObject* args, PyObject *keywds) 
+{
+	b3SharedMemoryCommandHandle commandHandle;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	int physicsClientId = 0;
+	b3PhysicsClientHandle sm = 0;
+	PyObject* rootPosObj=0;
+	PyObject* rootOrnObj=0;
+	int trackObjectUid=-2;
+	double rootPos[3];
+	double rootOrn[4];
+	
+	static char *kwlist[] = {"rootPosition", "rootOrientation", "trackObject", "physicsClientId", NULL };
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|OOii", kwlist,&rootPosObj, &rootOrnObj, &trackObjectUid,&physicsClientId))
+	{
+		return NULL;
+	}
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+
+	commandHandle = b3SetVRCameraStateCommandInit(sm);
+
+	if (pybullet_internalSetVectord(rootPosObj,rootPos))
+	{
+		b3SetVRCameraRootPosition(commandHandle,rootPos);
+	}
+	if (pybullet_internalSetVector4d(rootOrnObj,rootOrn))
+	{
+		b3SetVRCameraRootOrientation(commandHandle,rootOrn);
+	}
+
+	if (trackObjectUid>=-1)
+	{
+		b3SetVRCameraTrackingObject(commandHandle,trackObjectUid);
+	}
+
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+	statusType = b3GetStatusType(statusHandle);
+
+	Py_INCREF(Py_None);
+	return Py_None;
+
+}
+
+
 static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject *keywds) 
 {
 	b3SharedMemoryCommandHandle commandHandle;
@@ -3732,10 +3783,10 @@ static PyObject* pybullet_renderImageObsolete(PyObject* self, PyObject* args) {
 static PyObject* pybullet_applyExternalForce(PyObject* self, PyObject* args,PyObject* keywds) {
  
   {
-    int objectUniqueId, linkIndex, flags;
+    int objectUniqueId=-1, linkIndex=-1, flags;
     double force[3];
     double position[3] = {0.0, 0.0, 0.0};
-    PyObject* forceObj, *posObj;
+    PyObject* forceObj=0, *posObj=0;
 
     b3SharedMemoryCommandHandle command;
     b3SharedMemoryStatusHandle statusHandle;
@@ -4304,15 +4355,15 @@ static PyMethodDef SpamMethods[] = {
      "Set a single joint motor control mode and desired target value. There is "
      "no immediate state change, stepSimulation will process the motors."},
 
-    {"applyExternalForce",(PyCFunction) pybullet_applyExternalForce, METH_VARARGS,
+    {"applyExternalForce",(PyCFunction) pybullet_applyExternalForce, METH_VARARGS| METH_KEYWORDS,
      "for objectUniqueId, linkIndex (-1 for base/root link), apply a force "
      "[x,y,z] at the a position [x,y,z], flag to select FORCE_IN_LINK_FRAME or "
-     "FORCE_IN_WORLD_FRAME coordinates"},
+     "WORLD_FRAME coordinates"},
 
     {"applyExternalTorque", (PyCFunction)pybullet_applyExternalTorque, METH_VARARGS| METH_KEYWORDS,
      "for objectUniqueId, linkIndex (-1 for base/root link) apply a torque "
      "[x,y,z] in Cartesian coordinates, flag to select TORQUE_IN_LINK_FRAME or "
-     "TORQUE_IN_WORLD_FRAME coordinates"},
+     "WORLD_FRAME coordinates"},
 
     {"renderImage", pybullet_renderImageObsolete, METH_VARARGS,
      "obsolete, please use getCameraImage and getViewProjectionMatrices instead"
@@ -4420,6 +4471,11 @@ static PyMethodDef SpamMethods[] = {
 	{	"getVREvents", (PyCFunction)pybullet_getVREvents, METH_VARARGS | METH_KEYWORDS,
 		"Get Virtual Reality events, for example to track VR controllers position/buttons"
 	  },
+	{	"setVRCameraState", (PyCFunction)pybullet_setVRCameraState, METH_VARARGS | METH_KEYWORDS,
+		"Set properties of the VR Camera such as its root transform "
+		"for teleporting or to track objects (camera inside a vehicle for example)."
+	  },
+
 
 	{	"rayTest", (PyCFunction)pybullet_rayTest, METH_VARARGS | METH_KEYWORDS,
 				"Cast a ray and return the first object hit, if any. "

src/LinearMath/btAlignedObjectArray.h

@@ -476,15 +476,18 @@ protected:
 		return index;
 	}
 
+    void removeAtIndex(int index)
+    {
+        if (index<size())
+        {
+            swap( index,size()-1);
+            pop_back();
+        }
+    }
 	void	remove(const T& key)
 	{
-
 		int findIndex = findLinearSearch(key);
-		if (findIndex<size())
+        removeAtIndex(findIndex);
-		{
-			swap( findIndex,size()-1);
-			pop_back();
-		}
 	}
 
 	//PCK: whole function