Merge pull request #898 from erwincoumans/master

many pybullet / VR improvements

examples/BasicDemo/CMakeLists.txt

@@ -60,7 +60,7 @@ SET(AppBasicExampleGui_SRCS
 ADD_DEFINITIONS(-DB3_USE_STANDALONE_EXAMPLE)
 
 LINK_LIBRARIES(
-        BulletDynamics BulletCollision LinearMath OpenGLWindow Bullet3Common ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
+        BulletDynamics BulletCollision LinearMath OpenGLWindow Bullet3Common 
 )
 
 #some code to support OpenGL and Glew cross platform
@@ -69,6 +69,7 @@ IF (WIN32)
              ${BULLET_PHYSICS_SOURCE_DIR}/btgui/OpenGLWindow/GlewWindows
         )
         ADD_DEFINITIONS(-DGLEW_STATIC)
+	LINK_LIBRARIES( ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY} )
 ELSE(WIN32)
         IF(APPLE)
                 find_library(COCOA NAMES Cocoa)
@@ -83,7 +84,7 @@ ELSE(WIN32)
                 ADD_DEFINITIONS("-DGLEW_STATIC")
                 ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1")
 
-                LINK_LIBRARIES( X11 pthread dl Xext)
+                LINK_LIBRARIES(  pthread dl )
         ENDIF(APPLE)
 ENDIF(WIN32)
 

examples/ExampleBrowser/GwenGUISupport/GwenProfileWindow.cpp

@@ -44,7 +44,7 @@ public:
 	
 	
 	CProfileIterator* profIter;
-	
+
 	class MyMenuItems* m_menuItems;
 	MyProfileWindow (	Gwen::Controls::Base* pParent)
     : Gwen::Controls::WindowControl( pParent ),
@@ -192,7 +192,8 @@ public:
 			
 			
          //   Gwen::Controls::TreeNode* curParent = m_node;
-			
+
+
             double accumulated_time = dumpRecursive(profileIterator,m_node);
 			
             const char* name = profileIterator->Get_Current_Parent_Name();
@@ -278,7 +279,7 @@ MyProfileWindow* setupProfileWindow(GwenInternalData* data)
 	//profWindow->SetHidden(true);	
 	
 	profWindow->m_menuItems = menuItems;
-	//profWindow->profIter = CProfileManager::Get_Iterator();
+	profWindow->profIter = CProfileManager::Get_Iterator();
 	data->m_viewMenu->GetMenu()->AddItem( L"Profiler", menuItems,(Gwen::Event::Handler::Function)&MyMenuItems::MenuItemSelect);
 	
 	menuItems->m_profWindow = profWindow;
@@ -296,6 +297,11 @@ void	processProfileData( MyProfileWindow* profWindow, bool idle)
 	}	
 }
 
+bool isProfileWindowVisible(MyProfileWindow* window)
+{
+	return !window->Hidden();
+}
+
 void profileWindowSetVisible(MyProfileWindow* window, bool visible)
 {
 	window->SetHidden(!visible);

examples/ExampleBrowser/GwenGUISupport/GwenProfileWindow.h

@@ -4,6 +4,8 @@
 class MyProfileWindow* setupProfileWindow(struct GwenInternalData* data);
 void processProfileData(MyProfileWindow* window, bool idle);
 void profileWindowSetVisible(MyProfileWindow* window, bool visible);
+bool isProfileWindowVisible(MyProfileWindow* window);
+
 void destroyProfileWindow(MyProfileWindow* window);
 
 #endif//GWEN_PROFILE_WINDOW_H

examples/ExampleBrowser/OpenGLExampleBrowser.cpp

@@ -17,7 +17,7 @@
 #endif //_WIN32
 #endif//__APPLE__
 #include "../ThirdPartyLibs/Gwen/Renderers/OpenGL_DebugFont.h"
-
+#include "LinearMath/btThreads.h"
 #include "Bullet3Common/b3Vector3.h"
 #include "assert.h"
 #include <stdio.h>
@@ -25,7 +25,9 @@
 #include "GwenGUISupport/gwenUserInterface.h"
 #include "../Utils/b3Clock.h"
 #include "GwenGUISupport/GwenParameterInterface.h"
+#ifndef BT_NO_PROFILE
 #include "GwenGUISupport/GwenProfileWindow.h"
+#endif
 #include "GwenGUISupport/GwenTextureWindow.h"
 #include "GwenGUISupport/GraphingTexture.h"
 #include "../CommonInterfaces/Common2dCanvasInterface.h"
@@ -68,7 +70,9 @@ struct OpenGLExampleBrowserInternalData
 {
 	Gwen::Renderer::Base* m_gwenRenderer;
 	CommonGraphicsApp* m_app;
-//	MyProfileWindow* m_profWindow;
+#ifndef BT_NO_PROFILE
+	MyProfileWindow* m_profWindow;
+#endif //BT_NO_PROFILE
 	btAlignedObjectArray<Gwen::Controls::TreeNode*> m_nodes;
 	GwenUserInterface* m_gui;
 	GL3TexLoader* m_myTexLoader;
@@ -93,7 +97,9 @@ static CommonWindowInterface* s_window = 0;
 static CommonParameterInterface*	s_parameterInterface=0;
 static CommonRenderInterface*	s_instancingRenderer=0;
 static OpenGLGuiHelper*	s_guiHelper=0;
-//static MyProfileWindow* s_profWindow =0;
+#ifndef BT_NO_PROFILE
+static MyProfileWindow* s_profWindow =0;
+#endif //BT_NO_PROFILE
 static SharedMemoryInterface* sSharedMem = 0;
 
 #define DEMO_SELECTION_COMBOBOX 13
@@ -144,6 +150,198 @@ int gGpuArraySizeZ=45;
 
 
 
+struct btTiming
+{
+	const char* m_name;
+	int m_threadId;
+	unsigned long long int m_usStartTime;
+	unsigned long long int m_usEndTime;
+};
+
+FILE* gTimingFile = 0;
+#ifndef __STDC_FORMAT_MACROS
+#define __STDC_FORMAT_MACROS
+#endif //__STDC_FORMAT_MACROS
+#include <inttypes.h>
+#define BT_TIMING_CAPACITY 65536
+static bool m_firstTiming = true;
+
+
+struct btTimings
+{
+	btTimings()
+		:m_numTimings(0),
+		m_activeBuffer(0)
+	{
+		
+	}
+	void flush()
+	{
+		for (int i=0;i<m_numTimings;i++)
+		{
+			const char* name = m_timings[m_activeBuffer][i].m_name;
+			int threadId = m_timings[m_activeBuffer][i].m_threadId;
+			unsigned long long int startTime = m_timings[m_activeBuffer][i].m_usStartTime;
+			unsigned long long int endTime = m_timings[m_activeBuffer][i].m_usEndTime;
+
+			if (!m_firstTiming)
+			{
+				fprintf(gTimingFile,",\n");
+			}
+
+			m_firstTiming = false;
+
+            unsigned long long int startTimeDiv1000 = startTime/1000;
+            unsigned long long int endTimeDiv1000 = endTime/1000;
+
+#if 0
+
+            fprintf(gTimingFile,"{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%" PRIu64 ".123 ,\"ph\":\"B\",\"name\":\"%s\",\"args\":{}},\n",
+                    threadId, startTimeDiv1000, name);
+            fprintf(gTimingFile,"{\"cat\":\"timing\",\"pid\":1,\"tid\":%d,\"ts\":%" PRIu64 ".234 ,\"ph\":\"E\",\"name\":\"%s\",\"args\":{}}",
+                    threadId, endTimeDiv1000,name);
+
+#else
+         
+			
+			if (startTime>endTime)
+			{
+				endTime = startTime;
+			}
+            unsigned int startTimeRem1000 = startTime%1000;
+            unsigned int endTimeRem1000 = endTime%1000;
+
+            char startTimeRem1000Str[16];
+            char endTimeRem1000Str[16];
+            
+            if (startTimeRem1000<10)
+            {
+                sprintf(startTimeRem1000Str,"00%d",startTimeRem1000);
+            }
+            else
+            {
+                if (startTimeRem1000<100)
+                {
+                    sprintf(startTimeRem1000Str,"0%d",startTimeRem1000);
+                } else
+                {
+                    sprintf(startTimeRem1000Str,"%d",startTimeRem1000);
+                }
+            }
+            
+            if (endTimeRem1000<10)
+            {
+                sprintf(endTimeRem1000Str,"00%d",endTimeRem1000);
+            }
+            else
+            {
+                if (endTimeRem1000<100)
+                {
+                    sprintf(endTimeRem1000Str,"0%d",endTimeRem1000);
+                } else
+                {
+                    sprintf(endTimeRem1000Str,"%d",endTimeRem1000);
+                }
+            }
+            
+            char newname[1024];
+			static int counter2=0;
+         
+            sprintf(newname,"%s%d",name,counter2++);
+			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
+
+		}
+		m_numTimings = 0;
+
+	}
+
+	void addTiming(const char* name, int threadId, unsigned long long int startTime,  unsigned long long int endTime)
+	{
+		if (m_numTimings>=BT_TIMING_CAPACITY)
+		{
+			return;
+		}
+
+		
+
+		int slot = m_numTimings++;
+
+		m_timings[m_activeBuffer][slot].m_name = name;
+		m_timings[m_activeBuffer][slot].m_threadId = threadId;
+		m_timings[m_activeBuffer][slot].m_usStartTime = startTime;
+		m_timings[m_activeBuffer][slot].m_usEndTime = endTime;
+	}
+
+
+	int m_numTimings;
+	int m_activeBuffer;
+	btTiming m_timings[2][BT_TIMING_CAPACITY];
+};
+
+btTimings gTimings[BT_MAX_THREAD_COUNT];
+
+btClock clk;
+
+#define MAX_NESTING 1024
+
+bool gProfileDisabled = true;
+int gStackDepths[BT_MAX_THREAD_COUNT] = {0};
+const char* gFuncNames[BT_MAX_THREAD_COUNT][MAX_NESTING];
+unsigned long long int gStartTimes[BT_MAX_THREAD_COUNT][MAX_NESTING];
+
+void MyDummyEnterProfileZoneFunc(const char* msg)
+{
+}
+
+void MyDummyLeaveProfileZoneFunc()
+{
+}
+
+void MyEnterProfileZoneFunc(const char* msg)
+{
+	if (gProfileDisabled)
+		return;
+	int threadId = btGetCurrentThreadIndex();
+
+	if (gStackDepths[threadId]>=MAX_NESTING)
+	{
+		btAssert(0);
+		return;
+	}
+	gFuncNames[threadId][gStackDepths[threadId]] = msg;
+	gStartTimes[threadId][gStackDepths[threadId]] = clk.getTimeNanoseconds();
+	if (gStartTimes[threadId][gStackDepths[threadId]]<=gStartTimes[threadId][gStackDepths[threadId]-1])
+	{
+		gStartTimes[threadId][gStackDepths[threadId]]=1+gStartTimes[threadId][gStackDepths[threadId]-1];
+	}
+	gStackDepths[threadId]++;
+}
+void MyLeaveProfileZoneFunc()
+{
+	if (gProfileDisabled)
+		return;
+
+	int threadId = btGetCurrentThreadIndex();
+
+	if (gStackDepths[threadId]<=0)
+	{
+		return;
+	}
+
+	gStackDepths[threadId]--;
+
+	const char* name = gFuncNames[threadId][gStackDepths[threadId]];
+	unsigned long long int startTime = gStartTimes[threadId][gStackDepths[threadId]];
+	
+	unsigned long long int endTime = clk.getTimeNanoseconds();
+	gTimings[threadId].addTiming(name,threadId,startTime,endTime);
+}
+
+
 void deleteDemo()
 {
     if (sCurrentDemo)
@@ -154,6 +352,7 @@ void deleteDemo()
 		sCurrentDemo=0;
 		delete s_guiHelper;
 		s_guiHelper = 0;
+//		CProfileManager::CleanupMemory();
 	}
 }
 
@@ -237,6 +436,46 @@ void MyKeyboardCallback(int key, int state)
 		singleStepSimulation = true;
 	}
 
+	if (key=='p')
+	{
+		if (state)
+		{
+			m_firstTiming = true;
+			gProfileDisabled = false;//true;
+			b3SetCustomEnterProfileZoneFunc(MyEnterProfileZoneFunc);
+			b3SetCustomLeaveProfileZoneFunc(MyLeaveProfileZoneFunc);
+
+			//also for Bullet 2.x API
+			btSetCustomEnterProfileZoneFunc(MyEnterProfileZoneFunc);
+			btSetCustomLeaveProfileZoneFunc(MyLeaveProfileZoneFunc);
+		} else
+		{
+
+			b3SetCustomEnterProfileZoneFunc(MyDummyEnterProfileZoneFunc);
+			b3SetCustomLeaveProfileZoneFunc(MyDummyLeaveProfileZoneFunc);
+			//also for Bullet 2.x API
+			btSetCustomEnterProfileZoneFunc(MyDummyEnterProfileZoneFunc);
+			btSetCustomLeaveProfileZoneFunc(MyDummyLeaveProfileZoneFunc);
+			char fileName[1024];
+			static int fileCounter = 0;
+			sprintf(fileName,"timings_%d.json",fileCounter++);
+			gTimingFile = fopen(fileName,"w");
+			fprintf(gTimingFile,"{\"traceEvents\":[\n");
+			//dump the content to file
+			for (int i=0;i<BT_MAX_THREAD_COUNT;i++)
+			{
+				if (gTimings[i].m_numTimings)
+				{
+					printf("Writing %d timings for thread %d\n", gTimings[i].m_numTimings, i);
+					gTimings[i].flush();
+				}
+			}
+			fprintf(gTimingFile,"\n],\n\"displayTimeUnit\": \"ns\"}");
+			fclose(gTimingFile);
+			gTimingFile = 0;
+
+		}
+	}
 
 #ifndef NO_OPENGL3
 	if (key=='s' && state)
@@ -333,15 +572,7 @@ void OpenGLExampleBrowser::registerFileImporter(const char* extension, CommonExa
 void openFileDemo(const char* filename)
 {
 
-    if (sCurrentDemo)
+	deleteDemo();
-    {
-		sCurrentDemo->exitPhysics();
-		s_instancingRenderer->removeAllInstances();
-		delete sCurrentDemo;
-		sCurrentDemo=0;
-		delete s_guiHelper;
-		s_guiHelper = 0;
-    }
    
 	s_guiHelper= new OpenGLGuiHelper(s_app, sUseOpenGL2);
     s_parameterInterface->removeAllParameters();
@@ -387,6 +618,7 @@ void selectDemo(int demoIndex)
 		demoIndex = 0;
 	}
 	deleteDemo();
+
     
 	CommonExampleInterface::CreateFunc* func = gAllExamples->getExampleCreateFunc(demoIndex);
 	if (func)
@@ -775,6 +1007,8 @@ OpenGLExampleBrowser::~OpenGLExampleBrowser()
 	gAllExamples = 0;
 }
 
+
+
 #include "EmptyExample.h"
 
 bool OpenGLExampleBrowser::init(int argc, char* argv[])
@@ -886,6 +1120,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
 	b3SetCustomPrintfFunc(MyGuiPrintf);
 	b3SetCustomErrorMessageFunc(MyStatusBarError);
 	
+	
 
     assert(glGetError()==GL_NO_ERROR);
 	
@@ -940,10 +1175,11 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
 
 		//gui->getInternalData()->pRenderer->setTextureLoader(myTexLoader);
 
-
+#ifndef BT_NO_PROFILE
-//		s_profWindow= setupProfileWindow(gui2->getInternalData());
+		s_profWindow= setupProfileWindow(gui2->getInternalData());
-		//m_internalData->m_profWindow = s_profWindow;
+		m_internalData->m_profWindow = s_profWindow;
-	//	profileWindowSetVisible(s_profWindow,false);
+		profileWindowSetVisible(s_profWindow,false);
+#endif //BT_NO_PROFILE
 		gui2->setFocus();
 
 		s_parameterInterface = s_app->m_parameterInterface = new GwenParameterInterface(gui2->getInternalData());
@@ -1073,7 +1309,6 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
 }
 
 
-
 CommonExampleInterface* OpenGLExampleBrowser::getCurrentExample()
 {
 	btAssert(sCurrentDemo);
@@ -1087,6 +1322,8 @@ bool OpenGLExampleBrowser::requestedExit()
 
 void OpenGLExampleBrowser::update(float deltaTime)
 {
+	gProfileDisabled = false;
+
 		B3_PROFILE("OpenGLExampleBrowser::update");
 		assert(glGetError()==GL_NO_ERROR);
 		s_instancingRenderer->init();
@@ -1136,7 +1373,7 @@ void OpenGLExampleBrowser::update(float deltaTime)
 		{
 			if (!pauseSimulation || singleStepSimulation)
 			{
-				singleStepSimulation = false;
+				
 				//printf("---------------------------------------------------\n");
 				//printf("Framecount = %d\n",frameCount);
 				B3_PROFILE("sCurrentDemo->stepSimulation");
@@ -1167,7 +1404,8 @@ void OpenGLExampleBrowser::update(float deltaTime)
 				}
                 BT_PROFILE("Render Scene");
                 sCurrentDemo->renderScene();
-            } else
+            }
+			//else
             {
 				B3_PROFILE("physicsDebugDraw");
 				glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
@@ -1198,9 +1436,18 @@ void OpenGLExampleBrowser::update(float deltaTime)
 		if (renderGui)
 		{
 			B3_PROFILE("renderGui");
-			//            if (!pauseSimulation)
+#ifndef BT_NO_PROFILE
-			//                processProfileData(s_profWindow,false);
 
+			if (!pauseSimulation || singleStepSimulation)
+			{
+				if (isProfileWindowVisible(s_profWindow))
+				{
+				    processProfileData(s_profWindow,false);
+				}
+			}
+#endif //#ifndef BT_NO_PROFILE
+
+			
 			if (sUseOpenGL2)
 			{
 
@@ -1219,7 +1466,7 @@ void OpenGLExampleBrowser::update(float deltaTime)
 
 		}
 	
-	
+	singleStepSimulation = false;
 	
 				
 		toggle=1-toggle;

examples/SharedMemory/PhysicsClient.h

@@ -52,6 +52,10 @@ public:
 
 	virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) = 0;
 
+	virtual void getCachedVREvents(struct b3VREventsData* vrEventsData) = 0;
+
+	virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits) = 0;
+
 };
 
 #endif  // BT_PHYSICS_CLIENT_API_H

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -1122,6 +1122,38 @@ b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle phys
     return (b3SharedMemoryCommandHandle)command;
 }
 
+b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsClientHandle physClient, double rayFromWorldX,
+                                       double rayFromWorldY, double rayFromWorldZ,
+                                       double rayToWorldX, double rayToWorldY, double rayToWorldZ)
+{
+    PhysicsClient *cl = (PhysicsClient *)physClient;
+    b3Assert(cl);
+    b3Assert(cl->canSubmitCommand());
+    struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand();
+    b3Assert(command);
+    command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS;
+	command->m_requestRaycastIntersections.m_rayFromPosition[0] = rayFromWorldX;
+	command->m_requestRaycastIntersections.m_rayFromPosition[1] = rayFromWorldY;
+	command->m_requestRaycastIntersections.m_rayFromPosition[2] = rayFromWorldZ;
+	command->m_requestRaycastIntersections.m_rayToPosition[0] = rayToWorldX;
+	command->m_requestRaycastIntersections.m_rayToPosition[1] = rayToWorldY;
+	command->m_requestRaycastIntersections.m_rayToPosition[2] = rayToWorldZ;
+
+    return (b3SharedMemoryCommandHandle)command;
+
+}
+
+void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo)
+{
+	PhysicsClient* cl = (PhysicsClient* ) physClient;
+	if (cl)
+	{
+		cl->getCachedRaycastHits(raycastInfo);
+	}
+}
+
+
+
 b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode)
 {
     PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -2135,3 +2167,26 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status
 
 	return true;
 }
+
+b3SharedMemoryCommandHandle	b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient)
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+	struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+	b3Assert(command);
+
+	command->m_type = CMD_REQUEST_VR_EVENTS_DATA;
+	command->m_updateFlags = 0;
+
+	return (b3SharedMemoryCommandHandle)command;
+}
+
+void b3GetVREventsData(b3PhysicsClientHandle physClient, struct b3VREventsData* vrEventsData)
+{
+	PhysicsClient* cl = (PhysicsClient* ) physClient;
+	if (cl)
+	{
+		cl->getCachedVREvents(vrEventsData);
+	}
+}

examples/SharedMemory/PhysicsClientC_API.h

@@ -291,6 +291,14 @@ b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, d
                                              double rayToWorldZ);
 b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient);
 
+b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsClientHandle physClient, double rayFromWorldX,
+                                       double rayFromWorldY, double rayFromWorldZ,
+                                       double rayToWorldX, double rayToWorldY, double rayToWorldZ);
+
+void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo);
+
+
+
 /// Apply external force at the body (or link) center of mass, in world space/Cartesian coordinates.
 b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient);
 void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags);
@@ -302,6 +310,12 @@ int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale)
 int b3LoadBunnySetMass(b3SharedMemoryCommandHandle commandHandle, double mass);
 int b3LoadBunnySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin);
 
+
+b3SharedMemoryCommandHandle	b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient);
+void b3GetVREventsData(b3PhysicsClientHandle physClient, struct b3VREventsData* vrEventsData);
+
+
+
 #ifdef __cplusplus
 }
 #endif

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -42,6 +42,8 @@ struct PhysicsClientSharedMemoryInternalData {
     btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
 	btAlignedObjectArray<b3OverlappingObject> m_cachedOverlappingObjects;
 	btAlignedObjectArray<b3VisualShapeData> m_cachedVisualShapes;
+	btAlignedObjectArray<b3VRControllerEvent> m_cachedVREvents;
+	btAlignedObjectArray<b3RayHitInfo>	m_raycastHits;
 
     btAlignedObjectArray<int> m_bodyIdsRequestInfo;
     SharedMemoryStatus m_tempBackupServerStatus;
@@ -631,6 +633,35 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
 				b3Warning("Overlapping object query failed");
 				break;
 			}
+
+				case CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED:
+				{
+					if (m_data->m_verboseOutput)
+					{
+						b3Printf("Raycast completed");
+					}
+					m_data->m_raycastHits.clear();
+					for (int i=0;i<serverCmd.m_raycastHits.m_numRaycastHits;i++)
+					{
+						m_data->m_raycastHits.push_back(serverCmd.m_raycastHits.m_rayHits[i]);
+					}
+					break;
+				}
+
+			case CMD_REQUEST_VR_EVENTS_DATA_COMPLETED:
+			{
+				if (m_data->m_verboseOutput)
+				{
+					b3Printf("Request VR Events completed");
+				}
+				m_data->m_cachedVREvents.clear();
+				for (int i=0;i< serverCmd.m_sendVREvents.m_numVRControllerEvents;i++)
+				{
+					m_data->m_cachedVREvents.push_back(serverCmd.m_sendVREvents.m_controllerEvents[i]);
+				}
+				break;
+			}
+
 			case CMD_REQUEST_AABB_OVERLAP_COMPLETED:
 			{
 				if (m_data->m_verboseOutput)
@@ -983,6 +1014,19 @@ void PhysicsClientSharedMemory::getCachedOverlappingObjects(struct b3AABBOverlap
 		&m_data->m_cachedOverlappingObjects[0] : 0;
 }
 
+void PhysicsClientSharedMemory::getCachedVREvents(struct b3VREventsData* vrEventsData)
+{
+	vrEventsData->m_numControllerEvents = m_data->m_cachedVREvents.size();
+	vrEventsData->m_controllerEvents = vrEventsData->m_numControllerEvents?
+							&m_data->m_cachedVREvents[0] : 0;
+}
+
+void PhysicsClientSharedMemory::getCachedRaycastHits(struct b3RaycastInformation* raycastHits)
+{
+	raycastHits->m_numRayHits = m_data->m_raycastHits.size();
+	raycastHits->m_rayHits = raycastHits->m_numRayHits? &m_data->m_raycastHits[0] : 0;
+}
+
 
 void PhysicsClientSharedMemory::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo)
 {
@@ -1010,3 +1054,4 @@ const float* PhysicsClientSharedMemory::getDebugLinesColor() const {
     return 0;
 }
 int PhysicsClientSharedMemory::getNumDebugLines() const { return m_data->m_debugLinesFrom.size(); }
+

examples/SharedMemory/PhysicsClientSharedMemory.h

@@ -60,6 +60,11 @@ public:
 	virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects);
 
 	virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
+
+	virtual void getCachedVREvents(struct b3VREventsData* vrEventsData);
+
+	virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits);
+
 };
 
 #endif  // BT_PHYSICS_CLIENT_API_H

examples/SharedMemory/PhysicsDirect.cpp

@@ -47,7 +47,10 @@ struct PhysicsDirectInternalData
 	btAlignedObjectArray<b3OverlappingObject> m_cachedOverlappingObjects;
 
 	btAlignedObjectArray<b3VisualShapeData> m_cachedVisualShapes;
-    
+    btAlignedObjectArray<b3VRControllerEvent> m_cachedVREvents;
+
+	btAlignedObjectArray<b3RayHitInfo>	m_raycastHits;
+
 	PhysicsCommandProcessorInterface* m_commandProcessor;
 	bool m_ownsCommandProcessor;
 
@@ -581,6 +584,34 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
 	switch (serverCmd.m_type)
 	{
 	
+	case CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED:
+	{
+		if (m_data->m_verboseOutput)
+		{
+			b3Printf("Raycast completed");
+		}
+		m_data->m_raycastHits.clear();
+		for (int i=0;i<serverCmd.m_raycastHits.m_numRaycastHits;i++)
+		{
+			m_data->m_raycastHits.push_back(serverCmd.m_raycastHits.m_rayHits[i]);
+		}
+		break;
+	}
+	case CMD_REQUEST_VR_EVENTS_DATA_COMPLETED:
+	{
+
+		if (m_data->m_verboseOutput)
+		{
+			b3Printf("Request VR Events completed");
+		}
+		m_data->m_cachedVREvents.clear();
+		for (int i=0;i< serverCmd.m_sendVREvents.m_numVRControllerEvents;i++)
+		{
+			m_data->m_cachedVREvents.push_back(serverCmd.m_sendVREvents.m_controllerEvents[i]);
+		}
+		break;
+	}
+
 	case CMD_REQUEST_INTERNAL_DATA_COMPLETED:
 	{
 		if (serverCmd.m_numDataStreamBytes)
@@ -851,3 +882,16 @@ void PhysicsDirect::getCachedVisualShapeInformation(struct b3VisualShapeInformat
 	visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size();
 	visualShapesInfo->m_visualShapeData = visualShapesInfo->m_numVisualShapes ? &m_data->m_cachedVisualShapes[0] : 0;
 }
+
+void PhysicsDirect::getCachedVREvents(struct b3VREventsData* vrEventsData)
+{
+	vrEventsData->m_numControllerEvents = m_data->m_cachedVREvents.size();
+	vrEventsData->m_controllerEvents = vrEventsData->m_numControllerEvents?
+							&m_data->m_cachedVREvents[0] : 0;
+}
+
+void PhysicsDirect::getCachedRaycastHits(struct b3RaycastInformation* raycastHits)
+{
+	raycastHits->m_numRayHits = m_data->m_raycastHits.size();
+	raycastHits->m_rayHits = raycastHits->m_numRayHits? &m_data->m_raycastHits[0] : 0;
+}

examples/SharedMemory/PhysicsDirect.h

@@ -81,6 +81,9 @@ public:
 
 	virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
 	
+	virtual void getCachedVREvents(struct b3VREventsData* vrEventsData);
+
+	virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits);
 
 	//those 2 APIs are for internal use for visualization
 	virtual bool connect(struct GUIHelperInterface* guiHelper);

examples/SharedMemory/PhysicsLoopBack.cpp

@@ -146,8 +146,17 @@ void PhysicsLoopBack::getCachedVisualShapeInformation(struct b3VisualShapeInform
 	return m_data->m_physicsClient->getCachedVisualShapeInformation(visualShapesInfo);
 }
 
+void PhysicsLoopBack::getCachedVREvents(struct b3VREventsData* vrEventsData)
+{
+	return m_data->m_physicsClient->getCachedVREvents(vrEventsData);
+}
+
 void PhysicsLoopBack::getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects)
 {
 	return m_data->m_physicsClient->getCachedOverlappingObjects(overlappingObjects);
 }
 
+void PhysicsLoopBack::getCachedRaycastHits(struct b3RaycastInformation* raycastHits)
+{
+	return m_data->m_physicsClient->getCachedRaycastHits(raycastHits);
+}

examples/SharedMemory/PhysicsLoopBack.h

@@ -65,6 +65,11 @@ public:
 	virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects);
 
 	virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
+
+	virtual void getCachedVREvents(struct b3VREventsData* vrEventsData);
+
+	virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits);
+
 };
 
 #endif //PHYSICS_LOOP_BACK_H

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -463,6 +463,8 @@ struct PhysicsServerCommandProcessorInternalData
 	bool m_allowRealTimeSimulation;
 	bool m_hasGround;
 
+	b3VRControllerEvent m_vrEvents[MAX_VR_CONTROLLERS];
+	
 	btMultiBodyFixedConstraint* m_gripperRigidbodyFixed;
 	btMultiBody* m_gripperMultiBody;
 	btMultiBodyFixedConstraint* m_kukaGripperFixed;
@@ -562,6 +564,15 @@ struct PhysicsServerCommandProcessorInternalData
 		m_pickedConstraint(0),
 		m_pickingMultiBodyPoint2Point(0)
 	{
+		for (int i=0;i<MAX_VR_CONTROLLERS;i++)
+		{
+			m_vrEvents[i].m_numButtonEvents = 0;
+			m_vrEvents[i].m_numMoveEvents = 0;
+			for (int b=0;b<MAX_VR_BUTTONS;b++)
+			{
+				m_vrEvents[i].m_buttons[b] = 0;
+			}
+		}
 
 		initHandles();
 #if 0
@@ -718,7 +729,9 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 	m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
 	m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
 	m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = 1e-7;
-	
+//	m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 2;
+	//todo: islands/constraints are buggy in btMultiBodyDynamicsWorld! (performance + see slipping grasp)
+
 }
 
 void PhysicsServerCommandProcessor::deleteCachedInverseKinematicsBodies()
@@ -1306,6 +1319,85 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 				}
 #endif
 
+				case CMD_REQUEST_VR_EVENTS_DATA:
+				{
+					serverStatusOut.m_sendVREvents.m_numVRControllerEvents = 0;
+					for (int i=0;i<MAX_VR_CONTROLLERS;i++)
+					{
+						if (m_data->m_vrEvents[i].m_numButtonEvents + m_data->m_vrEvents[i].m_numMoveEvents)
+						{
+							serverStatusOut.m_sendVREvents.m_controllerEvents[serverStatusOut.m_sendVREvents.m_numVRControllerEvents++] = m_data->m_vrEvents[i];
+							m_data->m_vrEvents[i].m_numButtonEvents = 0;
+							m_data->m_vrEvents[i].m_numMoveEvents = 0;
+							for (int b=0;b<MAX_VR_BUTTONS;b++)
+							{
+								m_data->m_vrEvents[i].m_buttons[b] = 0;
+							}
+						}
+					}
+					serverStatusOut.m_type = CMD_REQUEST_VR_EVENTS_DATA_COMPLETED;
+					hasStatus = true;
+					break;
+				};
+				case CMD_REQUEST_RAY_CAST_INTERSECTIONS:
+				{
+					btVector3 rayFromWorld(clientCmd.m_requestRaycastIntersections.m_rayFromPosition[0],
+						clientCmd.m_requestRaycastIntersections.m_rayFromPosition[1],
+						clientCmd.m_requestRaycastIntersections.m_rayFromPosition[2]);
+					btVector3 rayToWorld(clientCmd.m_requestRaycastIntersections.m_rayToPosition[0],
+						clientCmd.m_requestRaycastIntersections.m_rayToPosition[1],
+						clientCmd.m_requestRaycastIntersections.m_rayToPosition[2]);
+					btCollisionWorld::ClosestRayResultCallback rayResultCallback(rayFromWorld,rayToWorld);
+					m_data->m_dynamicsWorld->rayTest(rayFromWorld,rayToWorld,rayResultCallback);
+					serverStatusOut.m_raycastHits.m_numRaycastHits = 0;
+
+					if (rayResultCallback.hasHit())
+					{
+						serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitFraction 
+							= rayResultCallback.m_closestHitFraction;
+
+						int objectUniqueId = -1;
+						int linkIndex = -1;
+
+						const btRigidBody* body = btRigidBody::upcast(rayResultCallback.m_collisionObject);
+						if (body)
+						{
+							objectUniqueId = rayResultCallback.m_collisionObject->getUserIndex2();
+						} else
+						{
+							const btMultiBodyLinkCollider* mblB = btMultiBodyLinkCollider::upcast(rayResultCallback.m_collisionObject);
+							if (mblB && mblB->m_multiBody)
+							{
+								linkIndex = mblB->m_link;
+								objectUniqueId = mblB->m_multiBody->getUserIndex2();
+							}
+						}
+
+						serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectUniqueId 
+							= objectUniqueId;
+						serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitObjectLinkIndex
+							= linkIndex;
+
+						serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitPositionWorld[0] 
+							= rayResultCallback.m_hitPointWorld[0];
+						serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitPositionWorld[1] 
+							= rayResultCallback.m_hitPointWorld[1];
+						serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitPositionWorld[2] 
+							= rayResultCallback.m_hitPointWorld[2];
+						
+						serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitNormalWorld[0] 
+							= rayResultCallback.m_hitNormalWorld[0]; 
+						serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitNormalWorld[1] 
+							= rayResultCallback.m_hitNormalWorld[1]; 
+						serverStatusOut.m_raycastHits.m_rayHits[serverStatusOut.m_raycastHits.m_numRaycastHits].m_hitNormalWorld[2] 
+							= rayResultCallback.m_hitNormalWorld[2]; 
+
+						serverStatusOut.m_raycastHits.m_numRaycastHits++;
+					}
+					serverStatusOut.m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED;
+					hasStatus = true;
+					break;
+				};
 				case CMD_REQUEST_DEBUG_LINES:
 					{
 						int curFlags =m_data->m_remoteDebugDrawer->getDebugMode();
@@ -4052,8 +4144,46 @@ void PhysicsServerCommandProcessor::enableRealTimeSimulation(bool enableRealTime
 	m_data->m_allowRealTimeSimulation = enableRealTimeSim;
 }
 
-void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
+void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec,	const struct b3VRControllerEvent* vrEvents, int numVREvents)
 {
+	//update m_vrEvents
+	for (int i=0;i<numVREvents;i++)
+	{
+		int controlledId = vrEvents[i].m_controllerId;
+		if (vrEvents[i].m_numMoveEvents)
+		{
+			m_data->m_vrEvents[controlledId].m_analogAxis = vrEvents[i].m_analogAxis;
+		}
+
+		if (vrEvents[i].m_numMoveEvents+vrEvents[i].m_numButtonEvents)
+		{
+			m_data->m_vrEvents[controlledId].m_controllerId = vrEvents[i].m_controllerId;
+
+			m_data->m_vrEvents[controlledId].m_pos[0] = vrEvents[i].m_pos[0];
+			m_data->m_vrEvents[controlledId].m_pos[1] = vrEvents[i].m_pos[1];
+			m_data->m_vrEvents[controlledId].m_pos[2] = vrEvents[i].m_pos[2];
+			
+			m_data->m_vrEvents[controlledId].m_orn[0] = vrEvents[i].m_orn[0];
+			m_data->m_vrEvents[controlledId].m_orn[1] = vrEvents[i].m_orn[1];
+			m_data->m_vrEvents[controlledId].m_orn[2] = vrEvents[i].m_orn[2];
+			m_data->m_vrEvents[controlledId].m_orn[3] = vrEvents[i].m_orn[3];
+		}
+
+		m_data->m_vrEvents[controlledId].m_numButtonEvents += vrEvents[i].m_numButtonEvents;
+		m_data->m_vrEvents[controlledId].m_numMoveEvents += vrEvents[i].m_numMoveEvents;
+		for (int b=0;b<MAX_VR_BUTTONS;b++)
+		{
+			m_data->m_vrEvents[controlledId].m_buttons[b] |= vrEvents[i].m_buttons[b];
+			if (vrEvents[i].m_buttons[b] & eButtonIsDown)
+			{
+				m_data->m_vrEvents[controlledId].m_buttons[b] |= eButtonIsDown;
+			} else
+			{
+				m_data->m_vrEvents[controlledId].m_buttons[b] &= ~eButtonIsDown;
+			}
+		}
+	}
+
 	if (gResetSimulation)
 	{
 		resetSimulation();
@@ -4208,39 +4338,46 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
 
 		loadUrdf("kuka_iiwa/model_vr_limits.urdf", btVector3(1.4, -0.2, 0.6), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 		m_data->m_KukaId = bodyId;
-		InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId);
+		if (m_data->m_KukaId>=0)
-		if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs() == 7)
 		{
-			btScalar q[7];
+			InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId);
-			q[0] = 0;// -SIMD_HALF_PI;
+			if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs() == 7)
-			q[1] = 0;
-			q[2] = 0;
-			q[3] = SIMD_HALF_PI;
-			q[4] = 0;
-			q[5] = -SIMD_HALF_PI*0.66;
-			q[6] = 0;
-
-			for (int i = 0; i < 7; i++)
 			{
-				kukaBody->m_multiBody->setJointPos(i, q[i]);
+				btScalar q[7];
+				q[0] = 0;// -SIMD_HALF_PI;
+				q[1] = 0;
+				q[2] = 0;
+				q[3] = SIMD_HALF_PI;
+				q[4] = 0;
+				q[5] = -SIMD_HALF_PI*0.66;
+				q[6] = 0;
+
+				for (int i = 0; i < 7; i++)
+				{
+					kukaBody->m_multiBody->setJointPos(i, q[i]);
+				}
+				btAlignedObjectArray<btQuaternion> scratch_q;
+				btAlignedObjectArray<btVector3> scratch_m;
+				kukaBody->m_multiBody->forwardKinematics(scratch_q, scratch_m);
+				int nLinks = kukaBody->m_multiBody->getNumLinks();
+				scratch_q.resize(nLinks + 1);
+				scratch_m.resize(nLinks + 1);
+				kukaBody->m_multiBody->updateCollisionObjectWorldTransforms(scratch_q, scratch_m);
 			}
-			btAlignedObjectArray<btQuaternion> scratch_q;
-			btAlignedObjectArray<btVector3> scratch_m;
-			kukaBody->m_multiBody->forwardKinematics(scratch_q, scratch_m);
-			int nLinks = kukaBody->m_multiBody->getNumLinks();
-			scratch_q.resize(nLinks + 1);
-			scratch_m.resize(nLinks + 1);
-			kukaBody->m_multiBody->updateCollisionObjectWorldTransforms(scratch_q, scratch_m);
 		}
+#if 1
 		loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 		loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .8), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 		loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .9), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		loadUrdf("r2d2.urdf", btVector3(-2, -4, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+#endif
 
+		//		loadUrdf("r2d2.urdf", btVector3(-2, -4, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+
+#if 1
 		// Load one motor gripper for kuka
 		loadSdf("gripper/wsg50_one_motor_gripper_new_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true);
 		m_data->m_gripperId = bodyId + 1;
-		
+		{
 		InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId);
 
 		// Reset the default gripper motor maximum torque for damping to 0
@@ -4255,12 +4392,14 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
 				}
 			}
 		}
-				
+		}
+#endif
+#if 1
 		for (int i = 0; i < 6; i++)
 		{
 			loadUrdf("jenga/jenga.urdf", btVector3(1.3-0.1*i,-0.7,  .75), btQuaternion(btVector3(0,1,0),SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 		}
-
+#endif
 		//loadUrdf("nao/nao.urdf", btVector3(2,5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 
 		// Add slider joint for fingers
@@ -4274,6 +4413,10 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
 		btMatrix3x3 frameInParent2(btQuaternion(0, 0, 0, 1.0));
 		btMatrix3x3 frameInChild2(btQuaternion(0, 0, 1.0, 0));
 		btVector3 jointAxis2(1.0, 0, 0);
+
+		if (m_data->m_gripperId>=0)
+		{
+		InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId);
 		m_data->m_kukaGripperRevolute1 = new btMultiBodyPoint2Point(gripperBody->m_multiBody, 2, gripperBody->m_multiBody, 4, pivotInParent1, pivotInChild1);
 		m_data->m_kukaGripperRevolute1->setMaxAppliedImpulse(5.0);
 		m_data->m_kukaGripperRevolute2 = new btMultiBodyPoint2Point(gripperBody->m_multiBody, 3, gripperBody->m_multiBody, 6, pivotInParent2, pivotInChild2);
@@ -4282,9 +4425,17 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
 		m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute1);
 		m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute2);
 
-		kukaBody = m_data->getHandle(m_data->m_KukaId);
+		}
+
+		if (m_data->m_KukaId>=0)
+		{
+		InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId);
 		if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs()==7)
 		{
+			if (m_data->m_gripperId>=0)
+			{
+			InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId);
+
 			gripperBody->m_multiBody->setHasSelfCollision(0);
 			btVector3 pivotInParent(0, 0, 0.05);
 			btMatrix3x3 frameInParent;
@@ -4297,16 +4448,20 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
 			m_data->m_kukaGripperMultiBody = gripperBody->m_multiBody;
 			m_data->m_kukaGripperFixed->setMaxAppliedImpulse(500);
 			m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperFixed);
+			}
 		}
+		}
+#if 0
 
 		for (int i = 0; i < 10; i++)
 		{
 			loadUrdf("cube.urdf", btVector3(-4, -2, 0.5 + i), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 		}
+
 		loadUrdf("sphere2.urdf", btVector3(-5, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 		loadUrdf("sphere2.urdf", btVector3(-5, 0, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 		loadUrdf("sphere2.urdf", btVector3(-5, 0, 3), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-			
+#endif		
 		btTransform objectLocalTr[] = {
 			btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.0, 0.0, 0.0)),
 			btTransform(btQuaternion(btVector3(0,0,1),-SIMD_HALF_PI), btVector3(0.0, 0.15, 0.64)),
@@ -4340,6 +4495,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
 		//loadUrdf("cup_small.urdf", objectWorldTr[2].getOrigin(), objectWorldTr[2].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 		//loadUrdf("pitcher_small.urdf", objectWorldTr[3].getOrigin(), objectWorldTr[3].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 		loadUrdf("teddy_vhacd.urdf", objectWorldTr[4].getOrigin(), objectWorldTr[4].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+
 		loadUrdf("cube_small.urdf", objectWorldTr[5].getOrigin(), objectWorldTr[5].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 		loadUrdf("sphere_small.urdf", objectWorldTr[6].getOrigin(), objectWorldTr[6].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 		loadUrdf("duck_vhacd.urdf", objectWorldTr[7].getOrigin(), objectWorldTr[7].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());

examples/SharedMemory/PhysicsServerCommandProcessor.h

@@ -84,7 +84,7 @@ public:
 	void enableCommandLogging(bool enable, const char* fileName);
 	void replayFromLogFile(const char* fileName);
 	void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes );
-	void stepSimulationRealTime(double dtInSec);
+	void stepSimulationRealTime(double dtInSec,	const struct b3VRControllerEvent* vrEvents, int numVREvents);
 	void enableRealTimeSimulation(bool enableRealTimeSim);
 	void applyJointDamping(int bodyUniqueId);
 };

examples/SharedMemory/PhysicsServerExample.cpp

@@ -14,6 +14,7 @@
 #include "Bullet3Common/b3Matrix3x3.h"
 #include "../Utils/b3Clock.h"
 #include "../MultiThreading/b3ThreadSupportInterface.h"
+#include "SharedMemoryPublic.h"
 #ifdef BT_ENABLE_VR
 #include "../RenderingExamples/TinyVRGui.h"
 #endif//BT_ENABLE_VR
@@ -21,7 +22,6 @@
 
 #include "../CommonInterfaces/CommonParameterInterface.h"
 
-#define MAX_VR_CONTROLLERS 8
 
 
 //@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
@@ -44,7 +44,7 @@ extern bool gResetSimulation;
 extern int gEnableKukaControl;
 int gGraspingController = -1;
 extern btScalar simTimeScalingFactor;
-
+bool gBatchUserDebugLines = true;
 extern bool gVRGripperClosed;
 
 const char* startFileNameVR = "0_VRDemoSettings.txt";
@@ -111,7 +111,7 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void
 				gVRTeleportRotZ= getParamf(-3.1415, 3.1415, message->at(2));
 				gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ);
 				saveCurrentSettingsVR();
-				b3Printf("gVRTeleportOrn rotZ = %f\n", gVRTeleportRotZ);
+//				b3Printf("gVRTeleportOrn rotZ = %f\n", gVRTeleportRotZ);
 			}
 
 			if (message->at(1) == 32)
@@ -125,7 +125,7 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void
 				{
 					gVRTeleportPos1[i] = getParamf(-2, 2, message->at(2));
 					saveCurrentSettingsVR();
-					b3Printf("gVRTeleportPos[%d] =  %f\n", i,gVRTeleportPos1[i]);
+//					b3Printf("gVRTeleportPos[%d] =  %f\n", i,gVRTeleportPos1[i]);
 
 				}
 			}
@@ -225,6 +225,13 @@ struct	MotionArgs
 	{
 		for (int i=0;i<MAX_VR_CONTROLLERS;i++)
 		{
+			m_vrControllerEvents[i].m_numButtonEvents = 0;
+			m_vrControllerEvents[i].m_numMoveEvents = 0;
+			for (int b=0;b<MAX_VR_BUTTONS;b++)
+			{
+				m_vrControllerEvents[i].m_buttons[b]=0;
+			}
+
 			m_isVrControllerPicking[i] = false;
 			m_isVrControllerDragging[i] = false;
 			m_isVrControllerReleasing[i] = false;
@@ -234,7 +241,11 @@ struct	MotionArgs
 	b3CriticalSection* m_cs;
 
 	btAlignedObjectArray<MyMouseCommand> m_mouseCommands;
+
+	b3VRControllerEvent m_vrControllerEvents[MAX_VR_CONTROLLERS];
 	
+	b3VRControllerEvent m_sendVrControllerEvents[MAX_VR_CONTROLLERS];
+
 	PhysicsServerSharedMemory*	m_physicsServerPtr;
 	b3AlignedObjectArray<b3Vector3> m_positions;
 
@@ -344,11 +355,37 @@ void	MotionThreadFunc(void* userPtr,void* lsMemory)
 				if (deltaTimeInSeconds>clampedDeltaTime)
 				{
 					deltaTimeInSeconds = clampedDeltaTime;
-					b3Warning("Clamp deltaTime from %f to %f",deltaTimeInSeconds, clampedDeltaTime);
+					//b3Warning("Clamp deltaTime from %f to %f",deltaTimeInSeconds, clampedDeltaTime);
 				}
 				
 				clock.reset();
-				args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds);
+
+				args->m_cs->lock();
+
+				int numSendVrControllers = 0;
+				for (int i=0;i<MAX_VR_CONTROLLERS;i++)
+				{
+					if (args->m_vrControllerEvents[i].m_numButtonEvents+args->m_vrControllerEvents[i].m_numMoveEvents)
+					{
+						args->m_sendVrControllerEvents[numSendVrControllers++] =
+							args->m_vrControllerEvents[i];
+
+
+						if (args->m_vrControllerEvents[i].m_numButtonEvents)
+						{
+							for (int b=0;b<MAX_VR_BUTTONS;b++)
+							{
+								args->m_vrControllerEvents[i].m_buttons[b] &= eButtonIsDown;
+							}
+						}
+						args->m_vrControllerEvents[i].m_numMoveEvents = 0;
+						args->m_vrControllerEvents[i].m_numButtonEvents = 0;
+					}
+				}
+
+				args->m_cs->unlock();
+
+				args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds, args->m_sendVrControllerEvents,numSendVrControllers);
 				deltaTimeInSeconds = 0;
 				
 			}
@@ -1060,7 +1097,7 @@ m_options(options)
 
 #endif
 	m_multiThreadedHelper = helper;
-	b3Printf("Started PhysicsServer\n");
+//	b3Printf("Started PhysicsServer\n");
 }
 
 
@@ -1394,6 +1431,32 @@ extern double gSubStep;
 extern int gHuskyId;
 extern btTransform huskyTr;
 
+  struct LineSegment
+                {
+                        btVector3 m_from;
+                        btVector3 m_to;
+                };
+
+                struct ColorWidth
+                {
+                        btVector3FloatData m_color;
+                        int width;
+                        int getHash() const
+                        {
+                                unsigned char r = (unsigned char) m_color.m_floats[0]*255;
+                                unsigned char g = (unsigned char) m_color.m_floats[1]*255;
+                                unsigned char b = (unsigned char) m_color.m_floats[2]*255;
+                                unsigned char w = width;
+                                return r+(256*g)+(256*256*b)+(256*256*256*w);
+                        }
+                        bool equals(const ColorWidth& other) const
+                        {
+                                bool same = ((width == other.width) && (m_color.m_floats[0] == other.m_color.m_floats[0]) &&
+                                        (m_color.m_floats[1] == other.m_color.m_floats[1]) &&
+                                        (m_color.m_floats[2] == other.m_color.m_floats[2]));
+                                return same;
+                        }
+                };
 
 void PhysicsServerExample::drawUserDebugLines()
 {
@@ -1408,6 +1471,14 @@ void PhysicsServerExample::drawUserDebugLines()
 	if (m_multiThreadedHelper)
 	{
 
+
+		//if gBatchUserDebugLines is true, batch lines based on color+width, to reduce line draw calls
+
+		btAlignedObjectArray< btAlignedObjectArray<unsigned int> > sortedIndices;
+		btAlignedObjectArray< btAlignedObjectArray<btVector3FloatData> > sortedLines;
+
+		btHashMap<ColorWidth,int> hashedLines;
+
 		for (int i = 0; i<m_multiThreadedHelper->m_userDebugLines.size(); i++)
 		{
 			btVector3 from;
@@ -1423,9 +1494,56 @@ void PhysicsServerExample::drawUserDebugLines()
 			color.setValue(m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[0],
 				m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[1],
 				m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[2]);
+			ColorWidth cw;
+			color.serializeFloat(cw.m_color);
+			cw.width = m_multiThreadedHelper->m_userDebugLines[i].m_lineWidth;
+			int index = -1;
+
+			if (gBatchUserDebugLines)
+			{
+				int* indexPtr = hashedLines.find(cw);
+				if (indexPtr)
+				{
+					index = *indexPtr;
+				} else
+				{
+					index = sortedLines.size();
+					sortedLines.expand();
+					sortedIndices.expand();
+					hashedLines.insert(cw,index);
+				}
+				btAssert(index>=0);
+				if (index>=0)
+				{
+					btVector3FloatData from1,toX1;
+					sortedIndices[index].push_back(sortedLines[index].size());
+					from.serializeFloat(from1);
+					sortedLines[index].push_back(from1);
+					sortedIndices[index].push_back(sortedLines[index].size());
+					toX.serializeFloat(toX1);
+					sortedLines[index].push_back(toX1);
+				}
+			}
+			else
+			{
+				m_guiHelper->getAppInterface()->m_renderer->drawLine(from, toX, color, m_multiThreadedHelper->m_userDebugLines[i].m_lineWidth);
+			}
+		}
 
 
-			m_guiHelper->getAppInterface()->m_renderer->drawLine(from, toX, color, m_multiThreadedHelper->m_userDebugLines[i].m_lineWidth);
+		if (gBatchUserDebugLines)
+		{
+			for (int i=0;i<hashedLines.size();i++)
+			{
+				ColorWidth cw = hashedLines.getKeyAtIndex(i);
+				int index = *hashedLines.getAtIndex(i);
+				int stride = sizeof(btVector3FloatData);
+				const float* positions = &sortedLines[index][0].m_floats[0];
+				int numPoints = sortedLines[index].size();
+				const unsigned int* indices = &sortedIndices[index][0];
+				int numIndices = sortedIndices[index].size();
+				m_guiHelper->getAppInterface()->m_renderer->drawLines(positions,cw.m_color.m_floats,numPoints, stride, indices,numIndices,cw.width);
+			}
 		}
 
 		for (int i = 0; i<m_multiThreadedHelper->m_userDebugText.size(); i++)
@@ -1713,11 +1831,35 @@ void	PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
 		gGraspingController = controllerId;
 		gEnableKukaControl = true;
 	}
+
+	btTransform trLocal;
+	trLocal.setIdentity();
+	trLocal.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)*btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI));
+
+	btTransform trOrg;
+	trOrg.setIdentity();
+	trOrg.setOrigin(btVector3(pos[0], pos[1], pos[2]));
+	trOrg.setRotation(btQuaternion(orn[0], orn[1], orn[2], orn[3]));
+
+	btTransform tr2a;
+	tr2a.setIdentity();
+	btTransform tr2;
+	tr2.setIdentity();
+
+
+
+	tr2.setOrigin(gVRTeleportPos1);
+	tr2a.setRotation(gVRTeleportOrn);
+
+
+	btTransform trTotal = tr2*tr2a*trOrg*trLocal;
+
+
 	if (controllerId != gGraspingController)
 	{
 		if (button == 1 && state == 0)
 		{
-			gResetSimulation = true;
+			//gResetSimulation = true;
 			//gVRTeleportPos1 = gLastPickPos;
 		}
 	} else
@@ -1786,27 +1928,6 @@ void	PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
 			m_args[0].m_isVrControllerReleasing[controllerId] = (state == 0);
 		}
 		
-		btTransform trLocal;
-		trLocal.setIdentity();
-		trLocal.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)*btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI));
-
-		btTransform trOrg;
-		trOrg.setIdentity();
-		trOrg.setOrigin(btVector3(pos[0], pos[1], pos[2]));
-		trOrg.setRotation(btQuaternion(orn[0], orn[1], orn[2], orn[3]));
-
-		btTransform tr2a;
-		tr2a.setIdentity();
-		btTransform tr2;
-		tr2.setIdentity();
-
-
-
-		tr2.setOrigin(gVRTeleportPos1);
-		tr2a.setRotation(gVRTeleportOrn);
-
-
-		btTransform trTotal = tr2*tr2a*trOrg*trLocal;
 
 		if ((button == 33) || (button == 1))
 		{
@@ -1817,6 +1938,26 @@ void	PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
 		}
 		
 	}
+
+	m_args[0].m_cs->lock();
+	m_args[0].m_vrControllerEvents[controllerId].m_controllerId = controllerId;
+	m_args[0].m_vrControllerEvents[controllerId].m_pos[0] = trTotal.getOrigin()[0];
+	m_args[0].m_vrControllerEvents[controllerId].m_pos[1] = trTotal.getOrigin()[1];
+	m_args[0].m_vrControllerEvents[controllerId].m_pos[2] = trTotal.getOrigin()[2];
+	m_args[0].m_vrControllerEvents[controllerId].m_orn[0] = trTotal.getRotation()[0];
+	m_args[0].m_vrControllerEvents[controllerId].m_orn[1] = trTotal.getRotation()[1];
+	m_args[0].m_vrControllerEvents[controllerId].m_orn[2] = trTotal.getRotation()[2];
+	m_args[0].m_vrControllerEvents[controllerId].m_orn[3] = trTotal.getRotation()[3];
+	m_args[0].m_vrControllerEvents[controllerId].m_numButtonEvents++;
+	if (state)
+	{
+		m_args[0].m_vrControllerEvents[controllerId].m_buttons[button]|=eButtonIsDown+eButtonTriggered;
+	} else
+	{
+		m_args[0].m_vrControllerEvents[controllerId].m_buttons[button]|=eButtonReleased;
+		m_args[0].m_vrControllerEvents[controllerId].m_buttons[button] &= ~eButtonIsDown;
+	}
+	m_args[0].m_cs->unlock();
 }
 
 
@@ -1868,5 +2009,18 @@ void	PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[
 		m_args[0].m_vrControllerOrn[controllerId] = trTotal.getRotation();
 	}
 
+	m_args[0].m_cs->lock();
+	m_args[0].m_vrControllerEvents[controllerId].m_controllerId = controllerId;
+	m_args[0].m_vrControllerEvents[controllerId].m_pos[0] = trTotal.getOrigin()[0];
+	m_args[0].m_vrControllerEvents[controllerId].m_pos[1] = trTotal.getOrigin()[1];
+	m_args[0].m_vrControllerEvents[controllerId].m_pos[2] = trTotal.getOrigin()[2];
+	m_args[0].m_vrControllerEvents[controllerId].m_orn[0] = trTotal.getRotation()[0];
+	m_args[0].m_vrControllerEvents[controllerId].m_orn[1] = trTotal.getRotation()[1];
+	m_args[0].m_vrControllerEvents[controllerId].m_orn[2] = trTotal.getRotation()[2];
+	m_args[0].m_vrControllerEvents[controllerId].m_orn[3] = trTotal.getRotation()[3];
+	m_args[0].m_vrControllerEvents[controllerId].m_numMoveEvents++;
+	m_args[0].m_vrControllerEvents[controllerId].m_analogAxis = analogAxis;
+	m_args[0].m_cs->unlock();
+
 }
 B3_STANDALONE_EXAMPLE(PhysicsServerCreateFunc)

examples/SharedMemory/PhysicsServerSharedMemory.cpp

@@ -236,9 +236,9 @@ void PhysicsServerSharedMemory::releaseSharedMemory()
 }
 
 
-void PhysicsServerSharedMemory::stepSimulationRealTime(double dtInSec)
+void PhysicsServerSharedMemory::stepSimulationRealTime(double dtInSec, const struct b3VRControllerEvent* vrEvents, int numVREvents)
 {
-	m_data->m_commandProcessor->stepSimulationRealTime(dtInSec);
+	m_data->m_commandProcessor->stepSimulationRealTime(dtInSec,vrEvents, numVREvents);
 }
 
 void PhysicsServerSharedMemory::enableRealTimeSimulation(bool enableRealTimeSim)

examples/SharedMemory/PhysicsServerSharedMemory.h

@@ -26,7 +26,7 @@ public:
 
 	virtual void processClientCommands();
 
-	virtual void stepSimulationRealTime(double dtInSec);
+	virtual void stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrEvents, int numVREvents);
 
 	virtual void enableRealTimeSimulation(bool enableRealTimeSim);
 

examples/SharedMemory/SharedMemoryCommands.h

@@ -174,6 +174,18 @@ enum EnumRequestContactDataUpdateFlags
 	CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER = 8,
 };
 
+struct RequestRaycastIntersections
+{
+	double m_rayFromPosition[3];
+	double m_rayToPosition[3];
+};
+
+struct SendRaycastHits
+{
+	int m_numRaycastHits;
+	b3RayHitInfo m_rayHits[MAX_RAY_HITS];
+};
+
 struct RequestContactDataArgs
 {
     int m_startingContactPointIndex;
@@ -615,6 +627,7 @@ struct SharedMemoryCommand
         struct LoadTextureArgs m_loadTextureArguments;
 		struct CalculateInverseKinematicsArgs m_calculateInverseKinematicsArguments;
 		struct UserDebugDrawArgs m_userDebugDrawArgs;
+		struct RequestRaycastIntersections m_requestRaycastIntersections;
         struct LoadBunnyArgs m_loadBunnyArguments;
     };
 };
@@ -638,6 +651,14 @@ struct SendOverlappingObjectsArgs
 	int m_numRemainingOverlappingObjects;
 };
 
+struct SendVREvents
+{
+	int m_numVRControllerEvents;
+	b3VRControllerEvent m_controllerEvents[MAX_VR_CONTROLLERS];
+};
+
+
+
 struct SharedMemoryStatus
 {
 	int m_type;
@@ -665,6 +686,8 @@ struct SharedMemoryStatus
 		struct SendVisualShapeDataArgs m_sendVisualShapeArgs;
 		struct UserDebugDrawResultArgs m_userDebugDrawArgs;
 		struct UserConstraintResultArgs m_userConstraintResultArgs;
+		struct SendVREvents m_sendVREvents;
+		struct SendRaycastHits m_raycastHits;
 	};
 };
 

examples/SharedMemory/SharedMemoryPublic.h

@@ -35,14 +35,17 @@ enum EnumSharedMemoryClientCommand
     CMD_CALCULATE_JACOBIAN,
     CMD_USER_CONSTRAINT,
     CMD_REQUEST_CONTACT_POINT_INFORMATION,
+    CMD_REQUEST_RAY_CAST_INTERSECTIONS,
+
 	CMD_REQUEST_AABB_OVERLAP,
+
 	CMD_SAVE_WORLD,
 	CMD_REQUEST_VISUAL_SHAPE_INFO,
     CMD_UPDATE_VISUAL_SHAPE,
     CMD_LOAD_TEXTURE,
     CMD_SET_SHADOW,
 	CMD_USER_DEBUG_DRAW,
-
+	CMD_REQUEST_VR_EVENTS_DATA,
     //don't go beyond this command!
     CMD_MAX_CLIENT_COMMANDS,
     
@@ -107,6 +110,8 @@ enum EnumSharedMemoryServerStatus
 		CMD_USER_DEBUG_DRAW_FAILED,
 		CMD_USER_CONSTRAINT_COMPLETED,
 		CMD_USER_CONSTRAINT_FAILED,
+		CMD_REQUEST_VR_EVENTS_DATA_COMPLETED,
+		CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED,
         //don't go beyond 'CMD_MAX_SERVER_COMMANDS!
         CMD_MAX_SERVER_COMMANDS
 };
@@ -199,6 +204,44 @@ struct b3CameraImageData
 };
 
 
+enum b3VREventType
+{
+	VR_CONTROLLER_MOVE_EVENT=1,
+	VR_CONTROLLER_BUTTON_EVENT
+};
+
+#define MAX_VR_BUTTONS 64
+#define MAX_VR_CONTROLLERS 8
+#define MAX_RAY_HITS 128
+
+enum b3VRButtonInfo
+{
+	eButtonIsDown = 1,
+	eButtonTriggered = 2,
+	eButtonReleased = 4,
+};
+
+struct b3VRControllerEvent
+{
+	int m_controllerId;//valid for VR_CONTROLLER_MOVE_EVENT and VR_CONTROLLER_BUTTON_EVENT
+	int m_numMoveEvents;
+	int m_numButtonEvents;
+	
+	float m_pos[4];//valid for VR_CONTROLLER_MOVE_EVENT and VR_CONTROLLER_BUTTON_EVENT
+	float m_orn[4];//valid for VR_CONTROLLER_MOVE_EVENT and VR_CONTROLLER_BUTTON_EVENT
+
+	float m_analogAxis;//valid if VR_CONTROLLER_MOVE_EVENT
+
+	int m_buttons[MAX_VR_BUTTONS];//valid if VR_CONTROLLER_BUTTON_EVENT, see b3VRButtonInfo
+};
+
+struct b3VREventsData
+{
+	int m_numControllerEvents;
+	struct b3VRControllerEvent* m_controllerEvents;
+};
+
+
 struct b3ContactPointData
 {
 //todo: expose some contact flags, such as telling which fields below are valid
@@ -237,6 +280,22 @@ struct b3ContactInformation
 	struct b3ContactPointData* m_contactPointData;
 };
 
+struct b3RayHitInfo
+{
+	double m_hitFraction;
+	int m_hitObjectUniqueId;
+	int m_hitObjectLinkIndex;
+	double m_hitPositionWorld[3];
+	double m_hitNormalWorld[3];
+};
+
+struct b3RaycastInformation
+{
+	int m_numRayHits;
+	struct b3RayHitInfo* m_rayHits;
+};
+
+
 #define VISUAL_SHAPE_MAX_PATH_LEN 128
 
 struct b3VisualShapeData

examples/StandaloneMain/hellovr_opengl_main.cpp

@@ -21,6 +21,7 @@
 #include "LinearMath/btIDebugDraw.h"
 int gSharedMemoryKey = -1;
 int  gDebugDrawFlags = 0;
+bool gDisplayDistortion = false;
 
 //how can you try typing on a keyboard, without seeing it?
 //it is pretty funny, to see the desktop in VR!
@@ -704,7 +705,7 @@ bool CMainApplication::HandleInput()
 //							printf("Device PRESSED: %d, button %d\n", unDevice, button);
 							if (button==2)
 							{
-								glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
+								//glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
 								///todo(erwincoumans) can't use reguar debug drawer, because physics/graphics are not in sync
 								///so it can (and likely will) cause crashes
 								///add a special debug drawer that deals with this
@@ -831,15 +832,31 @@ void CMainApplication::RenderFrame()
 			DrawControllers();
 		}
 		RenderStereoTargets();
+
+		if (gDisplayDistortion)
 		{
 			B3_PROFILE("RenderDistortion");
 			RenderDistortion();
+		} else
+		{
+			glBindFramebuffer( GL_FRAMEBUFFER, 0 );
+			glDisable( GL_MULTISAMPLE );
+	 		glBindFramebuffer(GL_READ_FRAMEBUFFER, rightEyeDesc.m_nRenderFramebufferId );
+			glBindFramebuffer(GL_DRAW_FRAMEBUFFER, 0);
+	
+			glBlitFramebuffer( 0, 0, m_nRenderWidth, m_nRenderHeight, 0, 0, m_nRenderWidth, m_nRenderHeight, 
+				GL_COLOR_BUFFER_BIT,
+ 				GL_LINEAR  );
+
+ 			glBindFramebuffer(GL_READ_FRAMEBUFFER, 0);
+			glBindFramebuffer(GL_DRAW_FRAMEBUFFER, 0 );
 		}
 
 		vr::Texture_t leftEyeTexture = {(void*)leftEyeDesc.m_nResolveTextureId, vr::API_OpenGL, vr::ColorSpace_Gamma };
 		vr::VRCompositor()->Submit(vr::Eye_Left, &leftEyeTexture );
 		vr::Texture_t rightEyeTexture = {(void*)rightEyeDesc.m_nResolveTextureId, vr::API_OpenGL, vr::ColorSpace_Gamma };
 		vr::VRCompositor()->Submit(vr::Eye_Right, &rightEyeTexture );
+		
 	}
 
 	if ( m_bVblank && m_bGlFinishHack )

examples/Utils/b3Clock.cpp

@@ -47,7 +47,6 @@ struct b3ClockData
 #ifdef B3_USE_WINDOWS_TIMERS
 	LARGE_INTEGER mClockFrequency;
 	DWORD mStartTick;
-	LONGLONG mPrevElapsedTime;
 	LARGE_INTEGER mStartTime;
 #else
 #ifdef __CELLOS_LV2__
@@ -94,7 +93,6 @@ void b3Clock::reset()
 #ifdef B3_USE_WINDOWS_TIMERS
 	QueryPerformanceCounter(&m_data->mStartTime);
 	m_data->mStartTick = GetTickCount();
-	m_data->mPrevElapsedTime = 0;
 #else
 #ifdef __CELLOS_LV2__
 
@@ -121,27 +119,7 @@ unsigned long int b3Clock::getTimeMilliseconds()
 		// Compute the number of millisecond ticks elapsed.
 	unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / 
 		m_data->mClockFrequency.QuadPart);
-		// Check for unexpected leaps in the Win32 performance counter.  
+	
-	// (This is caused by unexpected data across the PCI to ISA 
-		// bridge, aka south bridge.  See Microsoft KB274323.)
-		unsigned long elapsedTicks = GetTickCount() - m_data->mStartTick;
-		signed long msecOff = (signed long)(msecTicks - elapsedTicks);
-		if (msecOff < -100 || msecOff > 100)
-		{
-			// Adjust the starting time forwards.
-			LONGLONG msecAdjustment = b3ClockMin(msecOff * 
-				m_data->mClockFrequency.QuadPart / 1000, elapsedTime - 
-				m_data->mPrevElapsedTime);
-			m_data->mStartTime.QuadPart += msecAdjustment;
-			elapsedTime -= msecAdjustment;
-
-			// Recompute the number of millisecond ticks elapsed.
-			msecTicks = (unsigned long)(1000 * elapsedTime / 
-				m_data->mClockFrequency.QuadPart);
-		}
-
-		// Store the current elapsed time for adjustments next time.
-		m_data->mPrevElapsedTime = elapsedTime;
 
 		return msecTicks;
 #else
@@ -170,38 +148,16 @@ unsigned long int b3Clock::getTimeMilliseconds()
 unsigned long long int b3Clock::getTimeMicroseconds()
 {
 #ifdef B3_USE_WINDOWS_TIMERS
-		LARGE_INTEGER currentTime;
+	//see https://msdn.microsoft.com/en-us/library/windows/desktop/dn553408(v=vs.85).aspx
+		LARGE_INTEGER currentTime, elapsedTime;
+		
 		QueryPerformanceCounter(&currentTime);
-		LONGLONG elapsedTime = currentTime.QuadPart - 
+		elapsedTime.QuadPart = currentTime.QuadPart - 
 			m_data->mStartTime.QuadPart;
+		elapsedTime.QuadPart *= 1000000;
+		elapsedTime.QuadPart /= m_data->mClockFrequency.QuadPart;
 
-		// Compute the number of millisecond ticks elapsed.
+		return (unsigned long long) elapsedTime.QuadPart;
-		unsigned long long msecTicks = (unsigned long long)(1000 * elapsedTime / 
-			m_data->mClockFrequency.QuadPart);
-
-		// Check for unexpected leaps in the Win32 performance counter.  
-		// (This is caused by unexpected data across the PCI to ISA 
-		// bridge, aka south bridge.  See Microsoft KB274323.)
-		unsigned long long elapsedTicks = GetTickCount() - m_data->mStartTick;
-		signed long long msecOff = (signed long)(msecTicks - elapsedTicks);
-		if (msecOff < -100 || msecOff > 100)
-		{
-			// Adjust the starting time forwards.
-			LONGLONG msecAdjustment = b3ClockMin(msecOff * 
-				m_data->mClockFrequency.QuadPart / 1000, elapsedTime - 
-				m_data->mPrevElapsedTime);
-			m_data->mStartTime.QuadPart += msecAdjustment;
-			elapsedTime -= msecAdjustment;
-		}
-
-		// Store the current elapsed time for adjustments next time.
-		m_data->mPrevElapsedTime = elapsedTime;
-
-		// Convert to microseconds.
-		unsigned long long usecTicks = (unsigned long)(1000000 * elapsedTime / 
-			m_data->mClockFrequency.QuadPart);
-
-		return usecTicks;
 #else
 
 #ifdef __CELLOS_LV2__

examples/pybullet/pybullet.c

@@ -1956,6 +1956,188 @@ static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args
 	return Py_None;
 }
 
+
+static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject *keywds) 
+{
+	b3SharedMemoryCommandHandle commandHandle;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	PyObject* rayFromObj=0;
+	PyObject* rayToObj=0;
+	double from[3];
+	double to[3];
+	static char *kwlist[] = { "rayFromPosition", "rayToPosition", NULL };
+
+	if (0 == sm) {
+	  PyErr_SetString(SpamError, "Not connected to physics server.");
+	  return NULL;
+  }
+
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO", kwlist,
+		&rayFromObj, &rayToObj))
+		return NULL;
+
+	pybullet_internalSetVectord(rayFromObj,from);
+	pybullet_internalSetVectord(rayToObj,to);
+
+	commandHandle = b3CreateRaycastCommandInit(sm, from[0],from[1],from[2],
+		to[0],to[1],to[2]);
+
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+	statusType = b3GetStatusType(statusHandle);
+	if (statusType==CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED)
+	{
+		struct b3RaycastInformation raycastInfo;
+		PyObject* rayHitsObj = 0;
+		int i;
+		b3GetRaycastInformation(sm, &raycastInfo);
+		
+		rayHitsObj = PyTuple_New(raycastInfo.m_numRayHits);
+		for (i=0;i<raycastInfo.m_numRayHits;i++)
+		{
+			PyObject* singleHitObj  = PyTuple_New(5);
+			{
+				PyObject* ob = PyInt_FromLong(raycastInfo.m_rayHits[i].m_hitObjectUniqueId);
+				PyTuple_SetItem(singleHitObj,0,ob);
+			}
+			{
+				PyObject* ob = PyInt_FromLong(raycastInfo.m_rayHits[i].m_hitObjectLinkIndex);
+				PyTuple_SetItem(singleHitObj,1,ob);
+			}
+			{
+				PyObject* ob = PyFloat_FromDouble(raycastInfo.m_rayHits[i].m_hitFraction);
+				PyTuple_SetItem(singleHitObj,2,ob);
+			}
+			{
+				PyObject* posObj  = PyTuple_New(3);
+				int p;
+				for (p=0;p<3;p++)
+				{
+					PyObject* ob = PyFloat_FromDouble(raycastInfo.m_rayHits[i].m_hitPositionWorld[p]);
+					PyTuple_SetItem(posObj,p,ob);
+				}
+				PyTuple_SetItem(singleHitObj,3,posObj);
+			}
+			{
+				PyObject* normalObj  = PyTuple_New(3);
+				int p;
+				for (p=0;p<3;p++)
+				{
+					PyObject* ob = PyFloat_FromDouble(raycastInfo.m_rayHits[i].m_hitNormalWorld[p]);
+					PyTuple_SetItem(normalObj,p,ob);
+				}
+				PyTuple_SetItem(singleHitObj,4,normalObj);
+			}
+			PyTuple_SetItem(rayHitsObj,i,singleHitObj);
+		}
+		return rayHitsObj;
+	}
+
+	Py_INCREF(Py_None);
+	return Py_None;
+}
+
+static PyObject* pybullet_getMatrixFromQuaterion(PyObject* self, PyObject* args) 
+{
+	PyObject* quatObj;
+	double quat[4];
+	if (PyArg_ParseTuple(args, "O", &quatObj)) 
+	{
+		if (pybullet_internalSetVector4d(quatObj,quat))
+		{
+			///see btMatrix3x3::setRotation
+			int i;
+			double d = quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3];
+			double s = 2.0 / d;
+			double xs = quat[0] * s,   ys = quat[1] * s,   zs = quat[2] * s;
+			double wx = quat[3] * xs,  wy = quat[3] * ys,  wz = quat[3] * zs;
+			double xx = quat[0] * xs,  xy = quat[0] * ys,  xz = quat[0] * zs;
+			double yy = quat[1] * ys,  yz = quat[1] * zs,  zz = quat[2] * zs;
+			double mat3x3[9] = {
+				1.0 - (yy + zz), xy - wz, xz + wy,
+				xy + wz, 1.0 - (xx + zz), yz - wx,
+				xz - wy, yz + wx, 1.0 - (xx + yy)};
+			PyObject* matObj = PyTuple_New(9);
+			for (i=0;i<9;i++)
+			{
+				PyTuple_SetItem(matObj,i,PyFloat_FromDouble(mat3x3[i]));
+			}
+			return matObj;
+		}
+	}
+	PyErr_SetString(SpamError, "Couldn't convert quaternion [x,y,z,w].");
+	return NULL;
+};
+
+
+static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject *keywds) 
+{
+	b3SharedMemoryCommandHandle commandHandle;
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+
+	if (0 == sm) {
+	  PyErr_SetString(SpamError, "Not connected to physics server.");
+	  return NULL;
+  }
+
+	commandHandle = b3RequestVREventsCommandInit(sm);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+	statusType = b3GetStatusType(statusHandle);
+	if (statusType==CMD_REQUEST_VR_EVENTS_DATA_COMPLETED)
+	{
+		struct b3VREventsData vrEvents;
+		PyObject* vrEventsObj;
+		int i=0;
+		b3GetVREventsData(sm,&vrEvents);
+		
+		vrEventsObj = PyTuple_New(vrEvents.m_numControllerEvents);
+		for (i=0;i<vrEvents.m_numControllerEvents;i++)
+		{
+			PyObject* vrEventObj = PyTuple_New(7);
+
+			PyTuple_SetItem(vrEventObj,0,PyInt_FromLong(vrEvents.m_controllerEvents[i].m_controllerId));
+			{
+				PyObject* posObj = PyTuple_New(3);
+				PyTuple_SetItem(posObj,0,PyFloat_FromDouble(vrEvents.m_controllerEvents[i].m_pos[0]));
+				PyTuple_SetItem(posObj,1,PyFloat_FromDouble(vrEvents.m_controllerEvents[i].m_pos[1]));
+				PyTuple_SetItem(posObj,2,PyFloat_FromDouble(vrEvents.m_controllerEvents[i].m_pos[2]));
+				PyTuple_SetItem(vrEventObj,1,posObj);
+			}
+			{
+				PyObject* ornObj = PyTuple_New(4);
+				PyTuple_SetItem(ornObj,0,PyFloat_FromDouble(vrEvents.m_controllerEvents[i].m_orn[0]));
+				PyTuple_SetItem(ornObj,1,PyFloat_FromDouble(vrEvents.m_controllerEvents[i].m_orn[1]));
+				PyTuple_SetItem(ornObj,2,PyFloat_FromDouble(vrEvents.m_controllerEvents[i].m_orn[2]));
+				PyTuple_SetItem(ornObj,3,PyFloat_FromDouble(vrEvents.m_controllerEvents[i].m_orn[3]));
+				PyTuple_SetItem(vrEventObj,2,ornObj);
+			}
+
+			PyTuple_SetItem(vrEventObj,3,PyFloat_FromDouble(vrEvents.m_controllerEvents[i].m_analogAxis));
+			PyTuple_SetItem(vrEventObj,4,PyInt_FromLong(vrEvents.m_controllerEvents[i].m_numButtonEvents));
+			PyTuple_SetItem(vrEventObj,5,PyInt_FromLong(vrEvents.m_controllerEvents[i].m_numMoveEvents));
+			{
+				PyObject* buttonsObj = PyTuple_New(MAX_VR_BUTTONS);
+				int b;
+				for (b=0;b<MAX_VR_BUTTONS;b++)
+				{
+					PyObject* button = PyInt_FromLong(vrEvents.m_controllerEvents[i].m_buttons[b]);
+					PyTuple_SetItem(buttonsObj,b,button);
+				}
+				PyTuple_SetItem(vrEventObj,6,buttonsObj);
+			}
+			PyTuple_SetItem(vrEventsObj,i,vrEventObj);
+		}
+	   return vrEventsObj;
+	}
+
+	Py_INCREF(Py_None);
+	return Py_None;
+
+}
+
+
+
 static PyObject* pybullet_setDebugObjectColor(PyObject* self, PyObject* args, PyObject *keywds)
 {
 	PyObject* objectColorRGBObj = 0;
@@ -3820,6 +4002,9 @@ static PyMethodDef SpamMethods[] = {
      "Convert quaternion [x,y,z,w] to Euler [roll, pitch, yaw] as in URDF/SDF "
      "convention"},
 
+	 {"getMatrixFromQuaterion", pybullet_getMatrixFromQuaterion,METH_VARARGS,
+		"Compute the 3x3 matrix from a quaternion, as a list of 9 values (row-major)"},
+
     {"calculateInverseDynamics", pybullet_calculateInverseDynamics,
      METH_VARARGS,
      "Given an object id, joint positions, joint velocities and joint "
@@ -3832,12 +4017,21 @@ static PyMethodDef SpamMethods[] = {
         " for the end effector,"
         "compute the inverse kinematics and return the new joint state"},
     
+	{	"getVREvents", (PyCFunction)pybullet_getVREvents, METH_VARARGS | METH_KEYWORDS,
+		"Get Virtual Reality events, for example to track VR controllers position/buttons"
+	  },
+
+	{	"rayTest", (PyCFunction)pybullet_rayTest, METH_VARARGS | METH_KEYWORDS,
+				"Cast a ray and return the first object hit, if any. "
+				"Takes two arguments (from position [x,y,z] and to position [x,y,z] in Cartesian world coordinates"
+	  },
+
+
     // todo(erwincoumans)
     // saveSnapshot
     // loadSnapshot
     // raycast info
     // object names
-	// collision query
 
     {NULL, NULL, 0, NULL} /* Sentinel */
 };
@@ -3902,6 +4096,14 @@ initpybullet(void)
   PyModule_AddIntConstant(m, "CONTACT_REPORT_EXISTING", CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS);
   PyModule_AddIntConstant(m, "CONTACT_RECOMPUTE_CLOSEST", CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS);
 
+  PyModule_AddIntConstant(m, "VR_BUTTON_IS_DOWN", eButtonIsDown);
+  PyModule_AddIntConstant(m, "VR_BUTTON_WAS_TRIGGERED", eButtonTriggered);
+  PyModule_AddIntConstant(m, "VR_BUTTON_WAS_RELEASED", eButtonReleased);
+  
+  PyModule_AddIntConstant(m, "VR_MAX_CONTROLLERS", MAX_VR_CONTROLLERS);
+  PyModule_AddIntConstant(m, "VR_MAX_BUTTONS", MAX_VR_BUTTONS);
+
+
   SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
   Py_INCREF(SpamError);
   PyModule_AddObject(m, "error", SpamError);

examples/pybullet/vrEvent.py

@@ -0,0 +1,57 @@
+# See pybullet quickstart guide here:
+# https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#
+# Create a Tiltbrush-like app, drawing lines using any controller
+# Line width can be changed
+
+import pybullet as p
+
+CONTROLLER_ID = 0
+POSITION=1
+BUTTONS=6
+
+#assume that the VR physics server is already started before
+p.connect(p.SHARED_MEMORY)
+p.setInternalSimFlags(0)#don't load default robot assets etc
+p.resetSimulation()
+p.loadURDF("plane.urdf")
+
+prevPosition=[None]*p.VR_MAX_CONTROLLERS
+colors=[0.,0.5,0.5]*p.VR_MAX_CONTROLLERS
+widths = [3]*p.VR_MAX_CONTROLLERS
+
+#use a few default colors
+colors[0] = [0,0,0]
+colors[1] = [0.5,0,0]
+colors[2] = [0,0.5,0]
+colors[3] = [0,0,0.5]
+colors[4] = [0.5,0.5,0.]
+colors[5] = [.5,.5,.5]
+
+while True:
+	events = p.getVREvents()
+
+	for e in (events):
+		if (e[BUTTONS][33]&p.VR_BUTTON_WAS_TRIGGERED):
+			prevPosition[e[CONTROLLER_ID]] = e[POSITION]
+		if (e[BUTTONS][32]&p.VR_BUTTON_WAS_TRIGGERED):
+			widths[e[CONTROLLER_ID]]=widths[e[0]]+1
+			if (widths[e[CONTROLLER_ID]]>20):
+				widths[e[CONTROLLER_ID]] = 1
+		if (e[BUTTONS][1]&p.VR_BUTTON_WAS_TRIGGERED):
+			p.resetSimulation()
+			#p.setGravity(0,0,-10)
+			p.removeAllUserDebugItems()
+			p.loadURDF("plane.urdf")
+		if (e[BUTTONS][33]==p.VR_BUTTON_IS_DOWN):
+			pt = prevPosition[e[CONTROLLER_ID]]
+			
+			#print(prevPosition[e[0]])
+			#print(e[1])
+			diff = [pt[0]-e[POSITION][0],pt[1]-e[POSITION][1],pt[2]-e[POSITION][2]]
+			lenSqr =	diff[0]*diff[0]+diff[1]*diff[1]+diff[2]*diff[2]
+			ptDistThreshold = 0.01
+			if (lenSqr>(ptDistThreshold*ptDistThreshold)):
+				p.addUserDebugLine(e[POSITION],prevPosition[e[CONTROLLER_ID]],colors[e[CONTROLLER_ID]],widths[e[CONTROLLER_ID]])
+				#p.loadURDF("cube_small.urdf",e[1])
+				colors[e[CONTROLLER_ID]] = [1-colors[e[CONTROLLER_ID]][0],1-colors[e[CONTROLLER_ID]][1],1-colors[e[CONTROLLER_ID]][2]]
+				prevPosition[e[CONTROLLER_ID]] = e[POSITION]			

src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp

@@ -239,10 +239,7 @@ public:
 
 	virtual bool	processOverlap(btBroadphasePair& pair)
 	{
-		BT_PROFILE("btCollisionDispatcher::processOverlap");
-
 		(*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo);
-
 		return false;
 	}
 };

src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -257,7 +257,7 @@ void	btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
 
 
     int iObj = collisionObject->getWorldArrayIndex();
-    btAssert(iObj >= 0 && iObj < m_collisionObjects.size()); // trying to remove an object that was never added or already removed previously?
+//    btAssert(iObj >= 0 && iObj < m_collisionObjects.size()); // trying to remove an object that was never added or already removed previously?
     if (iObj >= 0 && iObj < m_collisionObjects.size())
     {
         btAssert(collisionObject == m_collisionObjects[iObj]);

src/LinearMath/btQuickprof.cpp

@@ -16,9 +16,7 @@
 #include "btQuickprof.h"
 
 
-#if BT_THREADSAFE
+
-#include "btThreads.h"
-#endif //#if BT_THREADSAFE
 
 
 #ifdef __CELLOS_LV2__
@@ -30,6 +28,9 @@
 #if defined (SUNOS) || defined (__SUNOS__)
 #include <stdio.h>
 #endif
+#ifdef __APPLE__
+#include <mach/mach_time.h>
+#endif
 
 #if defined(WIN32) || defined(_WIN32)
 
@@ -55,6 +56,12 @@
 
 #else //_WIN32
 #include <sys/time.h>
+
+#ifdef BT_LINUX_REALTIME
+//required linking against rt (librt)
+#include <time.h>
+#endif //BT_LINUX_REALTIME
+
 #endif //_WIN32
 
 #define mymin(a,b) (a > b ? a : b)
@@ -65,12 +72,14 @@ struct btClockData
 #ifdef BT_USE_WINDOWS_TIMERS
 	LARGE_INTEGER mClockFrequency;
 	LONGLONG mStartTick;
-	LONGLONG mPrevElapsedTime;
 	LARGE_INTEGER mStartTime;
 #else
 #ifdef __CELLOS_LV2__
 	uint64_t	mStartTime;
 #else
+#ifdef __APPLE__
+    uint64_t mStartTimeNano;
+#endif
 	struct timeval mStartTime;
 #endif
 #endif //__CELLOS_LV2__
@@ -111,7 +120,6 @@ void btClock::reset()
 #ifdef BT_USE_WINDOWS_TIMERS
 	QueryPerformanceCounter(&m_data->mStartTime);
 	m_data->mStartTick = GetTickCount64();
-	m_data->mPrevElapsedTime = 0;
 #else
 #ifdef __CELLOS_LV2__
 
@@ -121,6 +129,9 @@ void btClock::reset()
 	SYS_TIMEBASE_GET( newTime );
 	m_data->mStartTime = newTime;
 #else
+#ifdef __APPLE__
+    m_data->mStartTimeNano = mach_absolute_time();
+#endif
 	gettimeofday(&m_data->mStartTime, 0);
 #endif
 #endif
@@ -128,7 +139,7 @@ void btClock::reset()
 
 /// Returns the time in ms since the last call to reset or since
 /// the btClock was created.
-unsigned long int btClock::getTimeMilliseconds()
+unsigned long long int btClock::getTimeMilliseconds()
 {
 #ifdef BT_USE_WINDOWS_TIMERS
 	LARGE_INTEGER currentTime;
@@ -138,27 +149,6 @@ unsigned long int btClock::getTimeMilliseconds()
 		// Compute the number of millisecond ticks elapsed.
 	unsigned long msecTicks = (unsigned long)(1000 * elapsedTime /
 		m_data->mClockFrequency.QuadPart);
-		// Check for unexpected leaps in the Win32 performance counter.
-		// (This is caused by unexpected data across the PCI to ISA
-		// bridge, aka south bridge.  See Microsoft KB274323.)
-		unsigned long elapsedTicks = (unsigned long)(GetTickCount64() - m_data->mStartTick);
-		signed long msecOff = (signed long)(msecTicks - elapsedTicks);
-		if (msecOff < -100 || msecOff > 100)
-		{
-			// Adjust the starting time forwards.
-			LONGLONG msecAdjustment = mymin(msecOff *
-				m_data->mClockFrequency.QuadPart / 1000, elapsedTime -
-				m_data->mPrevElapsedTime);
-			m_data->mStartTime.QuadPart += msecAdjustment;
-			elapsedTime -= msecAdjustment;
-
-			// Recompute the number of millisecond ticks elapsed.
-			msecTicks = (unsigned long)(1000 * elapsedTime /
-				m_data->mClockFrequency.QuadPart);
-		}
-
-		// Store the current elapsed time for adjustments next time.
-		m_data->mPrevElapsedTime = elapsedTime;
 
 		return msecTicks;
 #else
@@ -184,41 +174,19 @@ unsigned long int btClock::getTimeMilliseconds()
 
 	/// Returns the time in us since the last call to reset or since
 	/// the Clock was created.
-unsigned long int btClock::getTimeMicroseconds()
+unsigned long long int btClock::getTimeMicroseconds()
 {
 #ifdef BT_USE_WINDOWS_TIMERS
-		LARGE_INTEGER currentTime;
+	//see https://msdn.microsoft.com/en-us/library/windows/desktop/dn553408(v=vs.85).aspx	
+		LARGE_INTEGER currentTime, elapsedTime;
+		
 		QueryPerformanceCounter(&currentTime);
-		LONGLONG elapsedTime = currentTime.QuadPart -
+		elapsedTime.QuadPart = currentTime.QuadPart - 
 			m_data->mStartTime.QuadPart;
+		elapsedTime.QuadPart *= 1000000;
+		elapsedTime.QuadPart /= m_data->mClockFrequency.QuadPart;
 
-		// Compute the number of millisecond ticks elapsed.
+		return (unsigned long long) elapsedTime.QuadPart;
-		unsigned long msecTicks = (unsigned long)(1000 * elapsedTime /
-			m_data->mClockFrequency.QuadPart);
-
-		// Check for unexpected leaps in the Win32 performance counter.
-		// (This is caused by unexpected data across the PCI to ISA
-		// bridge, aka south bridge.  See Microsoft KB274323.)
-		unsigned long elapsedTicks = (unsigned long)(GetTickCount64() - m_data->mStartTick);
-		signed long msecOff = (signed long)(msecTicks - elapsedTicks);
-		if (msecOff < -100 || msecOff > 100)
-		{
-			// Adjust the starting time forwards.
-			LONGLONG msecAdjustment = mymin(msecOff *
-				m_data->mClockFrequency.QuadPart / 1000, elapsedTime -
-				m_data->mPrevElapsedTime);
-			m_data->mStartTime.QuadPart += msecAdjustment;
-			elapsedTime -= msecAdjustment;
-		}
-
-		// Store the current elapsed time for adjustments next time.
-		m_data->mPrevElapsedTime = elapsedTime;
-
-		// Convert to microseconds.
-		unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime /
-			m_data->mClockFrequency.QuadPart);
-
-		return usecTicks;
 #else
 
 #ifdef __CELLOS_LV2__
@@ -240,6 +208,66 @@ unsigned long int btClock::getTimeMicroseconds()
 #endif
 }
 
+unsigned long long int btClock::getTimeNanoseconds()
+{
+#ifdef BT_USE_WINDOWS_TIMERS
+	//see https://msdn.microsoft.com/en-us/library/windows/desktop/dn553408(v=vs.85).aspx
+		LARGE_INTEGER currentTime, elapsedTime;
+		
+		QueryPerformanceCounter(&currentTime);
+		elapsedTime.QuadPart = currentTime.QuadPart - 
+			m_data->mStartTime.QuadPart;
+		elapsedTime.QuadPart *= 1e9;
+		elapsedTime.QuadPart /= m_data->mClockFrequency.QuadPart;
+
+		return (unsigned long long) elapsedTime.QuadPart;
+#else
+
+#ifdef __CELLOS_LV2__
+		uint64_t freq=sys_time_get_timebase_frequency();
+		double dFreq=((double) freq)/ 1e9;
+		typedef uint64_t  ClockSize;
+		ClockSize newTime;
+		//__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
+		SYS_TIMEBASE_GET( newTime );
+
+		return (unsigned long int)((double(newTime-m_data->mStartTime)) / dFreq);
+#else
+#ifdef __APPLE__
+    uint64_t ticks = mach_absolute_time() - m_data->mStartTimeNano;
+    static long double conversion = 0.0L;
+    if( 0.0L == conversion )
+    {
+        // attempt to get conversion to nanoseconds
+        mach_timebase_info_data_t info;
+        int err = mach_timebase_info( &info );
+        if( err )
+        {
+            btAssert(0);
+            conversion = 1.;
+        }
+        conversion = info.numer / info.denom;
+    }
+    return (ticks * conversion);
+
+    
+#else//__APPLE__
+    
+#ifdef BT_LINUX_REALTIME
+    timespec ts;
+    clock_gettime(CLOCK_REALTIME,&ts);
+    return 1000000000*ts.tv_sec + ts.tv_nsec;
+#else
+    	struct timeval currentTime;
+		gettimeofday(&currentTime, 0);
+		return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1e9 +
+			(currentTime.tv_usec - m_data->mStartTime.tv_usec)*1000;
+#endif //BT_LINUX_REALTIME
+
+#endif//__APPLE__
+#endif//__CELLOS_LV2__
+#endif 
+}
 
 
 /// Returns the time in s since the last call to reset or since 
@@ -370,6 +398,7 @@ bool	CProfileNode::Return( void )
 	if ( --RecursionCounter == 0 && TotalCalls != 0 ) {
 		unsigned long int time;
 		Profile_Get_Ticks(&time);
+
 		time-=StartTime;
 		TotalTime += (float)time / Profile_Get_Tick_Rate();
 	}
@@ -437,11 +466,69 @@ void	CProfileIterator::Enter_Parent( void )
 **
 ***************************************************************************************************/
 
-CProfileNode	CProfileManager::Root( "Root", NULL );
+#include "btThreads.h"
-CProfileNode *	CProfileManager::CurrentNode = &CProfileManager::Root;
+
+
+
+CProfileNode	gRoots[BT_MAX_THREAD_COUNT]={
+	CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
+	CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
+	CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
+	CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
+	CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
+	CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
+	CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
+	CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
+	CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
+	CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
+	CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
+	CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
+	CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
+	CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
+	CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
+	CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL)
+};
+
+
+CProfileNode* gCurrentNodes[BT_MAX_THREAD_COUNT]=
+{
+	&gRoots[ 0],	&gRoots[ 1],	&gRoots[ 2],	&gRoots[ 3],
+	&gRoots[ 4],	&gRoots[ 5],	&gRoots[ 6],	&gRoots[ 7],
+	&gRoots[ 8],	&gRoots[ 9],	&gRoots[10],	&gRoots[11],
+	&gRoots[12],	&gRoots[13],	&gRoots[14],	&gRoots[15],
+	&gRoots[16],	&gRoots[17],	&gRoots[18],	&gRoots[19],
+	&gRoots[20],	&gRoots[21],	&gRoots[22],	&gRoots[23],
+	&gRoots[24],	&gRoots[25],	&gRoots[26],	&gRoots[27],
+	&gRoots[28],	&gRoots[29],	&gRoots[30],	&gRoots[31],
+	&gRoots[32],	&gRoots[33],	&gRoots[34],	&gRoots[35],
+	&gRoots[36],	&gRoots[37],	&gRoots[38],	&gRoots[39],
+	&gRoots[40],	&gRoots[41],	&gRoots[42],	&gRoots[43],
+	&gRoots[44],	&gRoots[45],	&gRoots[46],	&gRoots[47],
+	&gRoots[48],	&gRoots[49],	&gRoots[50],	&gRoots[51],
+	&gRoots[52],	&gRoots[53],	&gRoots[54],	&gRoots[55],
+	&gRoots[56],	&gRoots[57],	&gRoots[58],	&gRoots[59],
+	&gRoots[60],	&gRoots[61],	&gRoots[62],	&gRoots[63],
+};
+
+
 int				CProfileManager::FrameCounter = 0;
 unsigned long int			CProfileManager::ResetTime = 0;
 
+CProfileIterator *	CProfileManager::Get_Iterator( void )
+{ 
+
+		int threadIndex = btGetCurrentThreadIndex();
+		return new CProfileIterator( &gRoots[threadIndex]); 
+}
+
+void						CProfileManager::CleanupMemory(void)
+{
+	for (int i=0;i<BT_MAX_THREAD_COUNT;i++)
+	{
+		gRoots[i].CleanupMemory();
+	}
+}
+
 
 /***********************************************************************************************
  * CProfileManager::Start_Profile -- Begin a named profile                                    *
@@ -458,19 +545,12 @@ unsigned long int			CProfileManager::ResetTime = 0;
  *=============================================================================================*/
 void	CProfileManager::Start_Profile( const char * name )
 {
-#if BT_THREADSAFE
+	int threadIndex = btGetCurrentThreadIndex();
-    // profile system is not designed for profiling multiple threads
+	if (name != gCurrentNodes[threadIndex]->Get_Name()) {
-    // disable collection on all but the main thread
+		gCurrentNodes[threadIndex] = gCurrentNodes[threadIndex]->Get_Sub_Node( name );
-    if ( !btIsMainThread() )
-    {
-        return;
-    }
-#endif //#if BT_THREADSAFE
-	if (name != CurrentNode->Get_Name()) {
-		CurrentNode = CurrentNode->Get_Sub_Node( name );
 	}
 
-	CurrentNode->Call();
+	gCurrentNodes[threadIndex]->Call();
 }
 
 
@@ -479,22 +559,26 @@ void	CProfileManager::Start_Profile( const char * name )
  *=============================================================================================*/
 void	CProfileManager::Stop_Profile( void )
 {
-#if BT_THREADSAFE
+	int threadIndex = btGetCurrentThreadIndex();
-    // profile system is not designed for profiling multiple threads
-    // disable collection on all but the main thread
-    if ( !btIsMainThread() )
-    {
-        return;
-    }
-#endif //#if BT_THREADSAFE
 	// Return will indicate whether we should back up to our parent (we may
 	// be profiling a recursive function)
-	if (CurrentNode->Return()) {
+	if (gCurrentNodes[threadIndex]->Return()) {
-		CurrentNode = CurrentNode->Get_Parent();
+		gCurrentNodes[threadIndex] = gCurrentNodes[threadIndex]->Get_Parent();
 	}
 }
 
 
+void	btEnterProfileZoneDefault(const char* name)
+{
+	CProfileManager::Start_Profile( name ); 
+}
+void	btLeaveProfileZoneDefault()
+{
+	CProfileManager::Stop_Profile(); 
+}
+
+
+
 /***********************************************************************************************
  * CProfileManager::Reset -- Reset the contents of the profiling system                       *
  *                                                                                             *
@@ -503,8 +587,8 @@ void	CProfileManager::Stop_Profile( void )
 void	CProfileManager::Reset( void )
 {
 	gProfileClock.reset();
-	Root.Reset();
+	gRoots[btGetCurrentThreadIndex()].Reset();
-    Root.Call();
+	gRoots[btGetCurrentThreadIndex()].Call();
 	FrameCounter = 0;
 	Profile_Get_Ticks(&ResetTime);
 }
@@ -592,6 +676,56 @@ void	CProfileManager::dumpAll()
 }
 
 
+#else
 
+void	btEnterProfileZoneDefault(const char* name)
+{
+}
+void	btLeaveProfileZoneDefault()
+{
+}
 
 #endif //BT_NO_PROFILE
+
+
+static btEnterProfileZoneFunc* bts_enterFunc = btEnterProfileZoneDefault;
+static btLeaveProfileZoneFunc* bts_leaveFunc = btLeaveProfileZoneDefault;
+
+void btEnterProfileZone(const char* name)
+{
+	(bts_enterFunc)(name);
+}
+void btLeaveProfileZone()
+{
+	(bts_leaveFunc)();
+}
+
+btEnterProfileZoneFunc* btGetCurrentEnterProfileZoneFunc()
+{
+	return bts_enterFunc ;
+}
+btLeaveProfileZoneFunc* btGetCurrentLeaveProfileZoneFunc()
+{
+	return bts_leaveFunc;
+}
+
+
+void btSetCustomEnterProfileZoneFunc(btEnterProfileZoneFunc* enterFunc)
+{
+	bts_enterFunc = enterFunc;
+}
+void btSetCustomLeaveProfileZoneFunc(btLeaveProfileZoneFunc* leaveFunc)
+{
+	bts_leaveFunc = leaveFunc;
+}
+
+CProfileSample::CProfileSample( const char * name )
+{ 
+	btEnterProfileZone(name);
+}
+
+CProfileSample::~CProfileSample( void )					
+{ 
+	btLeaveProfileZone();
+}
+

src/LinearMath/btQuickprof.h

@@ -36,12 +36,14 @@ public:
 
 	/// Returns the time in ms since the last call to reset or since 
 	/// the btClock was created.
-	unsigned long int getTimeMilliseconds();
+	unsigned long long int getTimeMilliseconds();
 
 	/// Returns the time in us since the last call to reset or since 
 	/// the Clock was created.
-	unsigned long int getTimeMicroseconds();
+	unsigned long long int getTimeMicroseconds();
 	
+	unsigned long long int getTimeNanoseconds();
+
 	/// Returns the time in s since the last call to reset or since 
 	/// the Clock was created.
 	btScalar getTimeSeconds();
@@ -52,9 +54,18 @@ private:
 
 #endif //USE_BT_CLOCK
 
+typedef void (btEnterProfileZoneFunc)(const char* msg);
+typedef void (btLeaveProfileZoneFunc)();
+
+btEnterProfileZoneFunc* btGetCurrentEnterProfileZoneFunc();
+btLeaveProfileZoneFunc* btGetCurrentLeaveProfileZoneFunc();
+
+void btSetCustomEnterProfileZoneFunc(btEnterProfileZoneFunc* enterFunc);
+void btSetCustomLeaveProfileZoneFunc(btLeaveProfileZoneFunc* leaveFunc);
+
 
 //To disable built-in profiling, please comment out next line
-#define BT_NO_PROFILE 1
+//#define BT_NO_PROFILE 1
 #ifndef BT_NO_PROFILE
 #include <stdio.h>//@todo remove this, backwards compatibility
 
@@ -151,21 +162,21 @@ public:
 	static	void						Start_Profile( const char * name );
 	static	void						Stop_Profile( void );
 
-	static	void						CleanupMemory(void)
+	static	void						CleanupMemory(void);
-	{
+//	{
-		Root.CleanupMemory();
+//		Root.CleanupMemory();
-	}
+//	}
 
 	static	void						Reset( void );
 	static	void						Increment_Frame_Counter( void );
 	static	int						Get_Frame_Count_Since_Reset( void )		{ return FrameCounter; }
 	static	float						Get_Time_Since_Reset( void );
 
-	static	CProfileIterator *	Get_Iterator( void )	
+	static	CProfileIterator *	Get_Iterator( void );	
-	{ 
+//	{ 
-		
+//		
-		return new CProfileIterator( &Root ); 
+//		return new CProfileIterator( &Root ); 
-	}
+//	}
 	static	void						Release_Iterator( CProfileIterator * iterator ) { delete ( iterator); }
 
 	static void	dumpRecursive(CProfileIterator* profileIterator, int spacing);
@@ -173,37 +184,27 @@ public:
 	static void	dumpAll();
 
 private:
-	static	CProfileNode			Root;
+
-	static	CProfileNode *			CurrentNode;
 	static	int						FrameCounter;
 	static	unsigned long int					ResetTime;
 };
 
 
+
+
+#endif //#ifndef BT_NO_PROFILE
+
 ///ProfileSampleClass is a simple way to profile a function's scope
 ///Use the BT_PROFILE macro at the start of scope to time
 class	CProfileSample {
 public:
-	CProfileSample( const char * name )
+	CProfileSample( const char * name );
-	{ 
-		CProfileManager::Start_Profile( name ); 
-	}
 
-	~CProfileSample( void )					
+	~CProfileSample( void );
-	{ 
-		CProfileManager::Stop_Profile(); 
-	}
 };
 
-
 #define	BT_PROFILE( name )			CProfileSample __profile( name )
 
-#else
-
-#define	BT_PROFILE( name )
-
-#endif //#ifndef BT_NO_PROFILE
-
 
 
 #endif //BT_QUICK_PROF_H

src/LinearMath/btThreads.cpp

@@ -226,5 +226,23 @@ bool btSpinMutex::tryLock()
     return true;
 }
 
-#endif // #else // #if BT_THREADSAFE
+unsigned int btGetCurrentThreadIndex()
+{
+	const unsigned int kNullIndex = ~0U;
+#ifdef _WIN32
+	__declspec( thread ) static unsigned int sThreadIndex = kNullIndex;
+
+#else
+	static __thread unsigned int sThreadIndex = kNullIndex;
+#endif
+	static int gThreadCounter=0;
+
+	if ( sThreadIndex == kNullIndex )
+	{
+		sThreadIndex = gThreadCounter++;
+	}
+	return sThreadIndex;
+}
+
+#endif // #if BT_THREADSAFE
 

src/LinearMath/btThreads.h

@@ -69,7 +69,8 @@ const unsigned int BT_MAX_THREAD_COUNT = 64;
 SIMD_FORCE_INLINE void btMutexLock( btSpinMutex* ) {}
 SIMD_FORCE_INLINE void btMutexUnlock( btSpinMutex* ) {}
 SIMD_FORCE_INLINE bool btMutexTryLock( btSpinMutex* ) {return true;}
-
+unsigned int btGetCurrentThreadIndex();
+const unsigned int BT_MAX_THREAD_COUNT = 64;
 #endif