Merge remote-tracking branch 'upstream/master

Extras/Serialize/BulletFileLoader/bDNA.cpp

@@ -390,16 +390,8 @@ void bDNA::init(char *data, int len, bool swap)
 	}
 
 
-	{
-		nr= (long)cp;
-	//long mask=3;
-		nr= ((nr+3)&~3)-nr;
-		while (nr--)
-		{
-			cp++;
-		}
-	}
 	
+	cp = btAlignPointer(cp,4);
 	
 	/*
 		TYPE (4 bytes)
@@ -426,16 +418,7 @@ void bDNA::init(char *data, int len, bool swap)
 		cp++;
 	}
 
-{
+	cp = btAlignPointer(cp,4);
-		nr= (long)cp;
-	//	long mask=3;
-		nr= ((nr+3)&~3)-nr;
-		while (nr--)
-		{
-			cp++;
-		}
-	}
-
 	
 	/*
 		TLEN (4 bytes)

Extras/Serialize/BulletFileLoader/bFile.cpp

@@ -460,15 +460,7 @@ void bFile::swapDNA(char* ptr)
 	}
 
 	
-	{
+	cp = btAlignPointer(cp,4);
-		nr= (long)cp;
-	//long mask=3;
-		nr= ((nr+3)&~3)-nr;
-		while (nr--)
-		{
-			cp++;
-		}
-	}
 
 
 	/*
@@ -497,16 +489,7 @@ void bFile::swapDNA(char* ptr)
 		cp++;
 	}
 
-{
+	cp = btAlignPointer(cp,4);
-		nr= (long)cp;
-	//	long mask=3;
-		nr= ((nr+3)&~3)-nr;
-		while (nr--)
-		{
-			cp++;
-		}
-	}
-
 
 	/*
 		TLEN (4 bytes)

build3/premake4.lua

@@ -29,6 +29,11 @@
 		description = "Try to link and use the system OpenGL headers version instead of dynamically loading OpenGL (dlopen is default)"
 	}
 
+	newoption
+	{
+		trigger = "enable_openvr",
+		description = "Enable experimental Virtual Reality examples, using OpenVR for HTC Vive and Oculus Rift"
+	}
 	newoption
 	{
 		trigger = "enable_system_x11",

build_visual_studio.bat

@@ -2,7 +2,7 @@
 rem premake4 --with-pe  vs2010
 rem premake4  --bullet2demos vs2010
 cd build3
-premake4   --targetdir="../bin" vs2010 
+premake4   --enable_openvr --targetdir="../bin" vs2010 
 rem premake4   --targetdir="../server2bin" vs2010 
 rem cd vs2010
 rem rename 0_Bullet3Solution.sln 0_server.sln

bullet.pc.cmake

@@ -3,4 +3,4 @@ Description: Bullet Continuous Collision Detection and Physics Library
 Requires:
 Version: @BULLET_VERSION@
 Libs: -L@CMAKE_INSTALL_PREFIX@/@LIB_DESTINATION@ -lBulletSoftBody -lBulletDynamics -lBulletCollision -lLinearMath
-Cflags: @BULLET_DOUBLE_DEF@ -I@INCLUDE_INSTALL_DIR@ -I@CMAKE_INSTALL_PREFIX@/include
+Cflags: @BULLET_DOUBLE_DEF@ -I@CMAKE_INSTALL_PREFIX@/@INCLUDE_INSTALL_DIR@ -I@CMAKE_INSTALL_PREFIX@/include

data/cube.mtl

@@ -12,3 +12,5 @@ newmtl cube
   map_Ka cube.tga
   map_Kd cube.png
   
+  
+

data/cube.urdf

@@ -0,0 +1,32 @@
+<?xml version="0.0" ?>
+<robot name="cube.urdf">
+  <link name="baseLink">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0"/>
+      <contact_cfm value="0.0"/>
+      <contact_erp value="1.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value="1.0"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+				<mesh filename="cube.obj" scale="1 1 1"/>
+      </geometry>
+       <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+	 	<box size="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
+

data/plane.mtl

@@ -1,11 +1,15 @@
-# Blender MTL File: 'None'
-# Material Count: 1
-
 newmtl Material
-Ns 96.078431
+  Ns 10.0000
-Ka 0.000000 0.000000 0.000000
+  Ni 1.5000
-Kd 0.640000 0.640000 0.640000
+  d 1.0000
-Ks 0.500000 0.500000 0.500000
+  Tr 0.0000
-Ni 1.000000
+  Tf 1.0000 1.0000 1.0000 
-d 1.000000
   illum 2
+  Ka 0.0000 0.0000 0.0000
+  Kd 0.5880 0.5880 0.5880
+  Ks 0.0000 0.0000 0.0000
+  Ke 0.0000 0.0000 0.0000
+  map_Ka cube.tga
+  map_Kd checker_grid.jpg
+  
+  

data/plane.obj

@@ -2,11 +2,17 @@
 # www.blender.org
 mtllib plane.mtl
 o Plane
-v 1.000000 0.000000 -1.000000
+v 5.000000 -5.000000 0.000000
-v 1.000000 0.000000 1.000000
+v 5.000000 5.000000 0.000000
-v -1.000000 0.000000 1.000000
+v -5.000000 5.000000 0.000000
-v -1.000000 0.000000 -1.000000
+v -5.000000  -5.000000 0.000000
+
+vt 1.000000 0.000000
+vt 1.000000 1.000000
+vt 0.000000 1.000000
+vt 0.000000 0.000000
+
 usemtl Material
 s off
-f 1 2 3
+f 1/1 2/2 3/3
-f 1 3 4
+f 1/1 3/3 4/4

data/r2d2.urdf

@@ -79,7 +79,7 @@
         <cylinder length=".1" radius="0.035"/>
       </geometry>
       <material name="black">
-        <color rgba="0 0 0 1"/>
+        <color rgba="0.5 0.5 0.5 1"/>
       </material>
     </visual>
     <collision>

data/sphere2.urdf

@@ -11,6 +11,9 @@
       <geometry>
 				<mesh filename="textured_sphere_smooth.obj" scale="0.5 0.5 0.5"/>      
       </geometry>
+      <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
     </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
@@ -19,55 +22,5 @@
       </geometry>
     </collision>
   </link>
-   <link name="childA">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="10.0"/>
-      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <sphere radius="0.5"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <geometry>
-        <sphere radius="0.5"/>
-      </geometry>
-    </collision>
-  </link>
-   <joint name="joint_baseLink_childA" type="fixed">
-    <parent link="baseLink"/>
-    <child link="childA"/>
-    <origin xyz="0 0 1.0"/>
-  </joint>
- <link name="childB">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0.5"/>
-      <mass value="10.0"/>
-      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
-    </inertial>
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.5"/>
-      <geometry>
-        <sphere radius="0.5"/>
-      </geometry>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.5"/>
-      <geometry>
-        <sphere radius="0.5"/>
-      </geometry>
-    </collision>
-  </link>
- <joint name="joint_childA_childB" type="continuous">
-    <parent link="childA"/>
-    <child link="childB"/>
-<axis xyz="0 1 0"/>
-    <origin xyz="0 0 0.5"/>
-  </joint>
-
 </robot>
 

examples/BasicDemo/BasicExample.cpp

@@ -39,7 +39,7 @@ struct BasicExample : public CommonRigidBodyBase
 	virtual void renderScene();
 	void resetCamera()
 	{
-		float dist = 41;
+		float dist = 4;
 		float pitch = 52;
 		float yaw = 35;
 		float targetPos[3]={0,0,0};
@@ -81,7 +81,7 @@ void BasicExample::initPhysics()
 		//create a few dynamic rigidbodies
 		// Re-using the same collision is better for memory usage and performance
 
-		btBoxShape* colShape = createBoxShape(btVector3(1,1,1));
+		btBoxShape* colShape = createBoxShape(btVector3(.1,.1,.1));
 		
 
 		//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
@@ -108,9 +108,9 @@ void BasicExample::initPhysics()
 				for(int j = 0;j<ARRAY_SIZE_Z;j++)
 				{
 					startTransform.setOrigin(btVector3(
-										btScalar(2.0*i),
+										btScalar(0.2*i),
-										btScalar(20+2.0*k),
+										btScalar(2+.2*k),
-										btScalar(2.0*j)));
+										btScalar(0.2*j)));
 
 			
 					createRigidBody(mass,startTransform,colShape);
@@ -121,7 +121,9 @@ void BasicExample::initPhysics()
 		}
 	}
 
+	
 	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+	
 }
 
 

examples/BasicDemo/CMakeLists.txt

@@ -53,6 +53,7 @@ SET(AppBasicExampleGui_SRCS
         ../ExampleBrowser/OpenGLGuiHelper.cpp
         ../ExampleBrowser/GL_ShapeDrawer.cpp
         ../ExampleBrowser/CollisionShape2TriangleMesh.cpp
+		../Utils/b3Clock.cpp
 )
 
 #this define maps StandaloneExampleCreateFunc to the right 'CreateFunc'

examples/BasicDemo/premake4.lua

@@ -49,6 +49,8 @@ files {
 	"../ExampleBrowser/OpenGLGuiHelper.cpp",
 	"../ExampleBrowser/GL_ShapeDrawer.cpp",
 	"../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+	"../Utils/b3Clock.cpp",
+	"../Utils/b3Clock.h",
 }
 
 if os.is("Linux") then initX11() end
@@ -59,6 +61,7 @@ end
                           
 
 
+
 project "App_BasicExampleGuiWithSoftwareRenderer"
 
 if _OPTIONS["ios"] then
@@ -92,7 +95,9 @@ files {
 	"../TinyRenderer/tgaimage.cpp",
 	"../TinyRenderer/our_gl.cpp",
 	"../TinyRenderer/TinyRenderer.cpp",
-	"../Utils/b3ResourcePath.cpp"
+	"../Utils/b3ResourcePath.cpp",
+	"../Utils/b3Clock.cpp",
+	"../Utils/b3Clock.h",
 }
 
 if os.is("Linux") then initX11() end
@@ -131,5 +136,68 @@ files {
 	"../TinyRenderer/tgaimage.cpp",
 	"../TinyRenderer/our_gl.cpp",
 	"../TinyRenderer/TinyRenderer.cpp",
-	"../Utils/b3ResourcePath.cpp"
+	"../Utils/b3ResourcePath.cpp",
+	"../Utils/b3Clock.cpp",
+	"../Utils/b3Clock.h",
 }
+
+
+
+
+		if _OPTIONS["enable_openvr"] then
+
+project "App_BasicExampleVR"
+
+if _OPTIONS["ios"] then
+        kind "WindowedApp"
+else
+        kind "ConsoleApp"
+end
+defines {"B3_USE_STANDALONE_EXAMPLE","BT_ENABLE_VR"}
+
+
+
+includedirs {"../../src", 
+					"../ThirdPartyLibs/openvr/headers",
+					"../ThirdPartyLibs/openvr/samples/shared"}
+
+links {
+        "BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common", "openvr_api"
+}
+
+	initOpenGL()
+  initGlew()
+
+
+language "C++"
+
+files {
+        "BasicExample.cpp",
+        "*.h",
+        "../StandaloneMain/hellovr_opengl_main.cpp",
+				"../ExampleBrowser/OpenGLGuiHelper.cpp",
+				"../ExampleBrowser/GL_ShapeDrawer.cpp",
+				"../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+				"../ThirdPartyLibs/openvr/samples/shared/lodepng.cpp",
+				"../ThirdPartyLibs/openvr/samples/shared/lodepng.h",
+				"../ThirdPartyLibs/openvr/samples/shared/Matrices.cpp",
+				"../ThirdPartyLibs/openvr/samples/shared/Matrices.h",
+				"../ThirdPartyLibs/openvr/samples/shared/pathtools.cpp",
+				"../ThirdPartyLibs/openvr/samples/shared/pathtools.h",
+				"../ThirdPartyLibs/openvr/samples/shared/Vectors.h",
+				"../Utils/b3Clock.cpp",
+				"../Utils/b3Clock.h",
+				
+}
+
+if os.is("Windows") then 
+	libdirs {"../ThirdPartyLibs/openvr/lib/win32"}
+end
+
+if os.is("Linux") then initX11() end
+
+if os.is("MacOSX") then
+        links{"Cocoa.framework"}
+end
+
+end

examples/CommonInterfaces/CommonCameraInterface.h

@@ -7,6 +7,8 @@ struct CommonCameraInterface
 	virtual void getCameraViewMatrix(float m[16]) const = 0;
 		
 	virtual void setVRCamera(const float viewMat[16], const float projectionMatrix[16])=0;
+	virtual void disableVRCamera()=0;
+	virtual bool isVRCamera() const =0;
 
 	virtual void getCameraTargetPosition(float pos[3]) const = 0;
 	virtual void getCameraPosition(float pos[3]) const = 0;

examples/CommonInterfaces/CommonGUIHelperInterface.h

@@ -29,9 +29,10 @@ struct GUIHelperInterface
 
 	virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld)=0;
 
-	virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices) =0;
+	virtual int	registerTexture(const unsigned char* texels, int width, int height)=0;
-
+	virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId) = 0;
 	virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) =0;
+    virtual void removeAllGraphicsInstances()=0;
 	
 	virtual Common2dCanvasInterface* get2dCanvasInterface()=0;
 	
@@ -73,9 +74,10 @@ struct DummyGUIHelper : public GUIHelperInterface
 
 	virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld){}
 
-	virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices) { return -1; }
+	virtual int	registerTexture(const unsigned char* texels, int width, int height){return -1;}
-
+	virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId){return -1;}
 	virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) {return -1;}
+    virtual void removeAllGraphicsInstances(){}
 	
 	virtual Common2dCanvasInterface* get2dCanvasInterface()
 	{

examples/CommonInterfaces/CommonRenderInterface.h

@@ -52,6 +52,10 @@ struct CommonRenderInterface
 	virtual void writeSingleInstanceTransformToCPU(const double* position, const double* orientation, int srcIndex)=0;
 	virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex)=0;
 	virtual void writeSingleInstanceColorToCPU(double* color, int srcIndex)=0;
+	virtual void writeSingleInstanceScaleToCPU(float* scale, int srcIndex)=0;
+	virtual void writeSingleInstanceScaleToCPU(double* scale, int srcIndex)=0;
+    
+    virtual int getTotalNumInstances() const = 0;
     
 	virtual void writeTransforms()=0;
     virtual void enableBlend(bool blend)=0;

examples/ExampleBrowser/CMakeLists.txt

@@ -208,7 +208,10 @@ SET(BulletExampleBrowser_SRCS
 	../RenderingExamples/TimeSeriesCanvas.h
 	../RenderingExamples/TimeSeriesFontData.cpp
 	../RenderingExamples/TimeSeriesFontData.h
-
+	../RoboticsLearning/b3RobotSimAPI.cpp
+	../RoboticsLearning/b3RobotSimAPI.h
+	../RoboticsLearning/R2D2GraspExample.cpp
+	../RoboticsLearning/R2D2GraspExample.h
 	../RenderingExamples/CoordinateSystemDemo.cpp
 	../RenderingExamples/CoordinateSystemDemo.h
 	../RenderingExamples/RaytracerSetup.cpp

examples/ExampleBrowser/ExampleEntries.cpp

@@ -45,6 +45,8 @@
 #include "../Tutorial/Dof6ConstraintTutorial.h"
 #include "../MultiThreading/MultiThreadingExample.h"
 #include "../InverseDynamics/InverseDynamicsExample.h"
+#include "../RoboticsLearning/R2D2GraspExample.h"
+
 #ifdef ENABLE_LUA
 #include "../LuaDemo/LuaPhysicsSetup.h"
 #endif
@@ -93,11 +95,11 @@ struct ExampleEntry
 static ExampleEntry gDefaultExamples[]=
 {
 
+
 	ExampleEntry(0,"API"),
 
 	ExampleEntry(1,"Basic Example","Create some rigid bodies using box collision shapes. This is a good example to familiarize with the basic initialization of Bullet. The Basic Example can also be compiled without graphical user interface, as a console application. Press W for wireframe, A to show AABBs, I to suspend/restart physics simulation. Press D to toggle auto-deactivation of the simulation. ", BasicExampleCreateFunc),
 
-
 	ExampleEntry(1,"Rolling Friction", "Damping is often not good enough to keep rounded objects from rolling down a sloped surface. Instead, you can set the rolling friction of a rigid body. Generally it is best to leave the rolling friction to zero, to avoid artifacts.", RollingFrictionCreateFunc),
 
 	ExampleEntry(1,"Constraints","Show the use of the various constraints in Bullet. Press the L key to visualize the constraint limits. Press the C key to visualize the constraint frames.",
@@ -117,8 +119,6 @@ static ExampleEntry gDefaultExamples[]=
 
 	ExampleEntry(1,"Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.",RigidBodySoftContactCreateFunc),
 
-
-
 	ExampleEntry(0,"MultiBody"),
 	ExampleEntry(1,"MultiDofCreateFunc","Create a basic btMultiBody with 3-DOF spherical joints (mobilizers). The demo uses a fixed base or a floating base at restart.", MultiDofCreateFunc),
 	ExampleEntry(1,"TestJointTorque","Apply a torque to a btMultiBody with 1-DOF joints (mobilizers). This setup is similar to API/TestHingeTorque.", TestJointTorqueCreateFunc),
@@ -244,10 +244,11 @@ static ExampleEntry gDefaultExamples[]=
 			PhysicsServerCreateFunc,PHYSICS_SERVER_ENABLE_COMMAND_LOGGING),
 	ExampleEntry(1,"Physics Server (Replay Log)", "Create a physics server that replay a command log from disk.",
 			PhysicsServerCreateFunc,PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG),
-
 	ExampleEntry(1, "Physics Client (Shared Mem)", "Create a physics client that can communicate with a physics server over shared memory.", PhysicsClientCreateFunc),
 	ExampleEntry(1, "Physics Client (Direct)", "Create a physics client that can communicate with a physics server directly in-process.", PhysicsClientCreateFunc,eCLIENTEXAMPLE_DIRECT),
 
+	ExampleEntry(1,"R2D2 Grasp","Load the R2D2 robot from URDF file and control it to grasp objects", R2D2GraspExampleCreateFunc, eROBOTIC_LEARN_GRASP),
+	ExampleEntry(1,"URDF Compliant Contact","Experiment/improve compliant rigid contact using parameters from URDF file (contact_cfm, contact_erp, lateral_friction, rolling_friction)", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_COMPLIANT_CONTACT),
 	
 
 

examples/ExampleBrowser/InProcessExampleBrowser.cpp

@@ -226,7 +226,7 @@ enum TestExampleBrowserCommunicationEnums
 
 void	ExampleBrowserThreadFunc(void* userPtr,void* lsMemory)
 {
-	printf("thread started\n");
+	printf("ExampleBrowserThreadFunc started\n");
 
 	ExampleBrowserThreadLocalStorage* localStorage = (ExampleBrowserThreadLocalStorage*) lsMemory;
 
@@ -369,7 +369,7 @@ void btShutDownExampleBrowser(btInProcessExampleBrowserInternalData* data)
                         }
                 };
 
-	printf("stopping threads\n");
+	printf("btShutDownExampleBrowser stopping threads\n");
 	delete data->m_threadSupport;
 	delete data->m_sharedMem;
 	delete data;

examples/ExampleBrowser/OpenGLExampleBrowser.cpp

@@ -399,7 +399,6 @@ static void loadCurrentSettings(const char* startFileName, b3CommandLineArgs& ar
 	FILE* f = fopen(startFileName,"r");
 	if (f)
 	{
-		int result;
 		char oneline[1024];
 		char* argv[] = {0,&oneline[0]};
 		

examples/ExampleBrowser/OpenGLGuiHelper.cpp

@@ -199,9 +199,16 @@ void OpenGLGuiHelper::createCollisionObjectGraphicsObject(btCollisionObject* bod
 	}
 }
 
-int OpenGLGuiHelper::registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices)
+int	OpenGLGuiHelper::registerTexture(const unsigned char* texels, int width, int height)
 {
-	int shapeId = m_data->m_glApp->m_renderer->registerShape(vertices, numvertices,indices,numIndices);
+	int textureId = m_data->m_glApp->m_renderer->registerTexture(texels,width,height);
+	return textureId;
+}
+
+
+int OpenGLGuiHelper::registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId)
+{
+	int shapeId = m_data->m_glApp->m_renderer->registerShape(vertices, numvertices,indices,numIndices,primitiveType, textureId);
 	return shapeId;
 }
 
@@ -210,6 +217,10 @@ int OpenGLGuiHelper::registerGraphicsInstance(int shapeIndex, const float* posit
 	return m_data->m_glApp->m_renderer->registerGraphicsInstance(shapeIndex,position,quaternion,color,scaling);
 }
 
+void OpenGLGuiHelper::removeAllGraphicsInstances()
+{
+    m_data->m_glApp->m_renderer->removeAllInstances();
+}
 
 void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* collisionShape)
 {
@@ -247,7 +258,7 @@ void OpenGLGuiHelper::createCollisionShapeGraphicsObject(btCollisionShape* colli
 
 	if (gfxVertices.size() && indices.size())
 	{
-		int shapeId = registerGraphicsShape(&gfxVertices[0].xyzw[0],gfxVertices.size(),&indices[0],indices.size());
+		int shapeId = registerGraphicsShape(&gfxVertices[0].xyzw[0],gfxVertices.size(),&indices[0],indices.size(),B3_GL_TRIANGLES,-1);
 		collisionShape->setUserIndex(shapeId);
 	}
 		

examples/ExampleBrowser/OpenGLGuiHelper.h

@@ -20,11 +20,10 @@ struct OpenGLGuiHelper : public GUIHelperInterface
 
 	virtual void createCollisionObjectGraphicsObject(btCollisionObject* body, const btVector3& color);
 
-	virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices);
+	virtual int	registerTexture(const unsigned char* texels, int width, int height);
-
+	virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId);
 	virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling);
-
+	virtual void removeAllGraphicsInstances();
-	
 	
 	virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape);
 

examples/ExampleBrowser/premake4.lua

@@ -88,6 +88,7 @@ project "App_BulletExampleBrowser"
 		"../Tutorial/*",
 		"../ExtendedTutorials/*",
 		"../Collision/*",
+		"../RoboticsLearning/*",
 		"../Collision/Internal/*",
 		"../Benchmarks/*",
 		"../CommonInterfaces/*",

examples/ExtendedTutorials/RigidBodyFromObj.cpp

@@ -134,7 +134,8 @@ void RigidBodyFromObjExample::initPhysics()
 		int shapeId = m_guiHelper->registerGraphicsShape(&glmesh->m_vertices->at(0).xyzw[0], 
 																		glmesh->m_numvertices, 
 																		&glmesh->m_indices->at(0), 
-																		glmesh->m_numIndices);
+																		glmesh->m_numIndices,
+																		B3_GL_TRIANGLES, -1);
 		shape->setUserIndex(shapeId);
 		int renderInstance = m_guiHelper->registerGraphicsInstance(shapeId,pos,orn,color,scaling);
 		body->setUserIndex(renderInstance);

examples/ExtendedTutorials/premake4.lua

@@ -92,6 +92,8 @@ files {
 			"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
 			"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
       "../ThirdPartyLibs/stb_image/stb_image.cpp",
+      "../Utils/b3Clock.cpp",
+			"../Utils/b3Clock.h",
 }
 
 if os.is("Linux") then initX11() end

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -108,7 +108,8 @@ struct BulletErrorLogger : public ErrorLogger
 bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
 {
 
-	
+	if (strlen(fileName)==0)
+        return false;
 
 //int argc=0;
 	char relativeFileName[1024];
@@ -969,16 +970,17 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP
 //		graphicsIndex  = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
 		//graphicsIndex  = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());
 		
-		CommonRenderInterface* renderer = m_data->m_guiHelper->getRenderInterface();
+		//CommonRenderInterface* renderer = m_data->m_guiHelper->getRenderInterface();
 		
-		if (renderer)
+		if (1)
 		{
 			int textureIndex = -1;
 			if (textures.size())
 			{
-				textureIndex = renderer->registerTexture(textures[0].textureData,textures[0].m_width,textures[0].m_height);
+				
+				textureIndex = m_data->m_guiHelper->registerTexture(textures[0].textureData,textures[0].m_width,textures[0].m_height);
 			}
-			graphicsIndex = renderer->registerShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(),B3_GL_TRIANGLES,textureIndex);
+			graphicsIndex = m_data->m_guiHelper->registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(),B3_GL_TRIANGLES,textureIndex);
 			
 		}
 	}
@@ -1003,6 +1005,17 @@ bool BulletURDFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const
 	return false;
 }
 
+bool BulletURDFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const
+{
+	UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
+	if (linkPtr)
+	{
+		const UrdfLink* link = *linkPtr;
+		contactInfo = link->m_contactInfo;
+		return true;
+	}
+	return false;
+}
 
 void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj) const
 {

examples/Importers/ImportURDFDemo/BulletUrdfImporter.h

@@ -38,6 +38,8 @@ public:
 
 	virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const;
 
+	virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const;
+	
     virtual std::string getJointName(int linkIndex) const;
     
     virtual void  getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;

examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp

@@ -65,7 +65,8 @@ btAlignedObjectArray<std::string> gFileNameArray;
 struct ImportUrdfInternalData
 {
     ImportUrdfInternalData()
-    :m_numMotors(0)
+    :m_numMotors(0),
+    m_mb(0)
     {
 		for (int i=0;i<MAX_NUM_MOTORS;i++)
 		{
@@ -74,10 +75,13 @@ struct ImportUrdfInternalData
 		}
     }
 
+    
     btScalar m_motorTargetVelocities[MAX_NUM_MOTORS];
     btMultiBodyJointMotor* m_jointMotors [MAX_NUM_MOTORS];
 	btGeneric6DofSpring2Constraint* m_generic6DofJointMotors [MAX_NUM_MOTORS];
     int m_numMotors;
+    btMultiBody* m_mb;
+    btRigidBody* m_rb;
 
 };
 
@@ -128,7 +132,7 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
 		
 		if (gFileNameArray.size()==0)
 		{
-			gFileNameArray.push_back("sphere2.urdf");
+			gFileNameArray.push_back("r2d2.urdf");
 
 		}
 
@@ -196,7 +200,7 @@ void ImportUrdfSetup::initPhysics()
 
 
 	btVector3 gravity(0,0,0);
-	gravity[upAxis]=-9.8;
+	//gravity[upAxis]=-9.8;
 
 	m_dynamicsWorld->setGravity(gravity);
 
@@ -223,7 +227,6 @@ void ImportUrdfSetup::initPhysics()
 		{
 
 
-			btMultiBody* mb = 0;
 
 
 			//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
@@ -232,7 +235,9 @@ void ImportUrdfSetup::initPhysics()
 			MyMultiBodyCreator creation(m_guiHelper);
 
 			ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix());
-			mb = creation.getBulletMultiBody();
+			m_data->m_rb = creation.getRigidBody();
+			m_data->m_mb = creation.getBulletMultiBody();
+			btMultiBody* mb = m_data->m_mb;
             
 			if (m_useMultiBody && mb )
 			{
@@ -337,7 +342,7 @@ void ImportUrdfSetup::initPhysics()
 
 
 
-		bool createGround=true;
+		bool createGround=false;
 		if (createGround)
 		{
 			btVector3 groundHalfExtents(20,20,20);
@@ -357,8 +362,7 @@ void ImportUrdfSetup::initPhysics()
 			m_guiHelper->createRigidBodyGraphicsObject(body,color);
 		}
 
-		///this extra stepSimulation call makes sure that all the btMultibody transforms are properly propagates.
+		
-		m_dynamicsWorld->stepSimulation(1. / 240., 0);// 1., 10, 1. / 240.);
 	}
 
 #ifdef TEST_MULTIBODY_SERIALIZATION

examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp

@@ -13,6 +13,7 @@
 
 MyMultiBodyCreator::MyMultiBodyCreator(GUIHelperInterface* guiHelper)
 	:m_bulletMultiBody(0),
+	m_rigidBody(0),
 m_guiHelper(guiHelper)
 {
 }
@@ -31,10 +32,10 @@ class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btSc
 {
     btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal);
     rbci.m_startWorldTransform = initialWorldTrans;
-    btRigidBody* body = new btRigidBody(rbci);
+    m_rigidBody = new btRigidBody(rbci);
-	body->forceActivationState(DISABLE_DEACTIVATION);
+	m_rigidBody->forceActivationState(DISABLE_DEACTIVATION);
 	
-    return body;
+    return m_rigidBody;
 }
     
 class btMultiBodyLinkCollider* MyMultiBodyCreator::allocateMultiBodyLinkCollider(int /*urdfLinkIndex*/, int mbLinkIndex, btMultiBody* multiBody) 

examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h

@@ -24,6 +24,7 @@ class MyMultiBodyCreator : public MultiBodyCreationInterface
 protected:
 
 	btMultiBody* m_bulletMultiBody;
+	btRigidBody* m_rigidBody;
     
 	struct GUIHelperInterface* m_guiHelper;
 
@@ -62,6 +63,10 @@ public:
     virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex);
 
 	btMultiBody* getBulletMultiBody();
+	btRigidBody* getRigidBody()
+	{
+	    return m_rigidBody;
+    }
 	
 	int	getNum6DofConstraints() const
 	{

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -392,9 +392,18 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
                 
                 u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,col);
 
-                btScalar friction = 0.5f;
+				URDFLinkContactInfo contactInfo;
+				u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
+
+				if ((contactInfo.m_flags & URDF_CONTACT_HAS_LATERAL_FRICTION)!=0)
+				{
+					col->setFriction(contactInfo.m_lateralFriction);
+				}
+				if ((contactInfo.m_flags & URDF_CONTACT_HAS_ROLLING_FRICTION)!=0)
+				{
+					col->setRollingFriction(contactInfo.m_rollingFriction);
+				}
 				
-                col->setFriction(friction);
 
                 if (mbLinkIndex>=0) //???? double-check +/- 1
                 {

examples/Importers/ImportURDFDemo/URDFImporterInterface.h

@@ -6,6 +6,7 @@
 #include "LinearMath/btTransform.h"
 #include "URDFJointTypes.h"
 
+
 class URDFImporterInterface
 {
 
@@ -29,6 +30,9 @@ public:
 	/// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise
 	virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const { return false;}
 
+	///this API will likely change, don't override it!
+	virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const  { return false;}
+    
     virtual std::string getJointName(int linkIndex) const = 0;
 
     //fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity.

examples/Importers/ImportURDFDemo/URDFJointTypes.h

@@ -11,6 +11,33 @@ enum UrdfJointTypes
 		URDFPlanarJoint,
 		URDFFixedJoint,
 };
+#include "LinearMath/btScalar.h"
+
+enum URDF_LinkContactFlags
+{
+	URDF_CONTACT_HAS_LATERAL_FRICTION=1,
+	URDF_CONTACT_HAS_ROLLING_FRICTION=2,
+	URDF_CONTACT_HAS_CONTACT_CFM=4,
+	URDF_CONTACT_HAS_CONTACT_ERP=8
+};
+
+struct URDFLinkContactInfo
+{
+	btScalar m_lateralFriction;
+	btScalar m_rollingFriction;
+	btScalar m_contactCfm;
+	btScalar m_contactErp;
+	int m_flags;
+
+	URDFLinkContactInfo()
+		:m_lateralFriction(0.5),
+		m_rollingFriction(0),
+		m_contactCfm(0),
+		m_contactErp(0)
+	{
+		m_flags = URDF_CONTACT_HAS_LATERAL_FRICTION;
+	}
+};
 
 
 #endif //URDF_JOINT_TYPES_H

examples/Importers/ImportURDFDemo/UrdfParser.cpp

@@ -569,6 +569,31 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
         }
     }
 
+	{
+		//optional 'contact' parameters
+	 TiXmlElement* ci = config->FirstChildElement("contact");
+	  if (ci)
+	  {
+		TiXmlElement *friction_xml = ci->FirstChildElement("lateral_friction");
+		if (friction_xml)
+		{
+			if (m_parseSDF)
+			{
+				link.m_contactInfo.m_lateralFriction = urdfLexicalCast<double>(friction_xml->GetText());
+			} else
+			{
+				if (!friction_xml->Attribute("value"))
+				{
+				  logger->reportError("Link/contact: lateral_friction element must have value attribute");
+				  return false;
+				}
+
+				link.m_contactInfo.m_lateralFriction = urdfLexicalCast<double>(friction_xml->Attribute("value"));
+			}
+		}
+	  }
+	}
+
   // Inertial (optional)
   TiXmlElement *i = config->FirstChildElement("inertial");
   if (i)

examples/Importers/ImportURDFDemo/UrdfParser.h

@@ -99,6 +99,8 @@ struct UrdfLink
 	
 	int m_linkIndex;
 	
+	URDFLinkContactInfo m_contactInfo;
+
 	UrdfLink()
 		:m_parentLink(0),
 		m_parentJoint(0)

examples/InverseDynamics/premake4.lua

@@ -92,6 +92,7 @@ files {
 			"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
 			"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
 			"../ThirdPartyLibs/stb_image/stb_image.cpp",
+			"../Utils/b3Clock.cpp",
 }
 
 if os.is("Linux") then initX11() end

examples/MultiBody/TestJointTorqueSetup.cpp

@@ -367,9 +367,8 @@ void TestJointTorqueSetup::initPhysics()
 
 	btSerializer* s = new btDefaultSerializer;
 	m_dynamicsWorld->serialize(s);
-	b3ResourcePath p;
 	char resourcePath[1024];
-	if (p.findResourcePath("multibody.bullet",resourcePath,1024))
+	if (b3ResourcePath::findResourcePath("multibody.bullet",resourcePath,1024))
 	{
 		FILE* f = fopen(resourcePath,"wb");
 		fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);

examples/MultiThreading/MultiThreadingExample.cpp

@@ -119,7 +119,7 @@ struct SampleThreadLocalStorage
 
 void	SampleThreadFunc(void* userPtr,void* lsMemory)
 {
-	printf("thread started\n");
+	printf("SampleThreadFunc thread started\n");
 
 	SampleThreadLocalStorage* localStorage = (SampleThreadLocalStorage*) lsMemory;
 

examples/MultiThreading/b3PosixThreadSupport.cpp

@@ -53,7 +53,7 @@ static sem_t* createSem(const char* baseName)
 #ifdef NAMED_SEMAPHORES
         /// Named semaphore begin
         char name[32];
-        snprintf(name, 32, "/%s-%d-%4.4d", baseName, getpid(), semCount++);
+        snprintf(name, 32, "/%8.s-%4.d-%4.4d", baseName, getpid(), semCount++);
         sem_t* tempSem = sem_open(name, O_CREAT, 0600, 0);
 
         if (tempSem != reinterpret_cast<sem_t *>(SEM_FAILED))

examples/MultiThreading/b3Win32ThreadSupport.cpp

@@ -223,7 +223,8 @@ bool b3Win32ThreadSupport::isTaskCompleted(int *puiArgument0, int *puiArgument1,
 
 void b3Win32ThreadSupport::startThreads(const Win32ThreadConstructionInfo& threadConstructionInfo)
 {
-
+	static int uniqueId = 0;
+	uniqueId++;
 	m_activeThreadStatus.resize(threadConstructionInfo.m_numThreads);
 	m_completeHandles.resize(threadConstructionInfo.m_numThreads);
 
@@ -244,17 +245,39 @@ void b3Win32ThreadSupport::startThreads(const Win32ThreadConstructionInfo& threa
 
 		threadStatus.m_userPtr=0;
 
-		sprintf(threadStatus.m_eventStartHandleName,"eventStart%s%d",threadConstructionInfo.m_uniqueName,i);
+		sprintf(threadStatus.m_eventStartHandleName,"es%.8s%d%d",threadConstructionInfo.m_uniqueName,uniqueId,i);
 		threadStatus.m_eventStartHandle = CreateEventA (0,false,false,threadStatus.m_eventStartHandleName);
 
-		sprintf(threadStatus.m_eventCompletetHandleName,"eventComplete%s%d",threadConstructionInfo.m_uniqueName,i);
+		sprintf(threadStatus.m_eventCompletetHandleName,"ec%.8s%d%d",threadConstructionInfo.m_uniqueName,uniqueId,i);
 		threadStatus.m_eventCompletetHandle = CreateEventA (0,false,false,threadStatus.m_eventCompletetHandleName);
 
 		m_completeHandles[i] = threadStatus.m_eventCompletetHandle;
 
 		HANDLE handle = CreateThread(lpThreadAttributes,dwStackSize,lpStartAddress,lpParameter,	dwCreationFlags,lpThreadId);
-		//SetThreadPriority(handle,THREAD_PRIORITY_HIGHEST);
+		switch(threadConstructionInfo.m_priority)
+		{
+		case 0:
+		{
+			SetThreadPriority(handle,THREAD_PRIORITY_HIGHEST);
+			break;
+		}
+		case 1:
+		{
 			SetThreadPriority(handle,THREAD_PRIORITY_TIME_CRITICAL);
+			break;
+		}
+		case 2:
+		{
+			SetThreadPriority(handle,THREAD_PRIORITY_BELOW_NORMAL);
+			break;
+		}
+		
+		default:
+		{
+			
+		}
+
+		}
 		
 		SetThreadAffinityMask(handle, 1<<i);
 
@@ -265,7 +288,7 @@ void b3Win32ThreadSupport::startThreads(const Win32ThreadConstructionInfo& threa
 		threadStatus.m_lsMemory = threadConstructionInfo.m_lsMemoryFunc();
 		threadStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc;
 
-		printf("started thread %d with threadHandle %p\n",i,handle);
+		printf("started %s thread %d with threadHandle %p\n",threadConstructionInfo.m_uniqueName,i,handle);
 		
 	}
 

examples/MultiThreading/b3Win32ThreadSupport.h

@@ -74,7 +74,8 @@ public:
 									m_userThreadFunc(userThreadFunc),
 									m_lsMemoryFunc(lsMemoryFunc),
 									m_numThreads(numThreads),
-									m_threadStackSize(threadStackSize)
+									m_threadStackSize(threadStackSize),
+									m_priority(0)
 		{
 
 		}
@@ -84,6 +85,7 @@ public:
 		b3Win32lsMemorySetupFunc	m_lsMemoryFunc;
 		int						m_numThreads;
 		int						m_threadStackSize;
+		int						m_priority;
 
 	};
 

examples/OpenGLWindow/GLInstancingRenderer.cpp

@@ -17,8 +17,8 @@ subject to the following restrictions:
 
 ///todo: make this configurable in the gui
 bool useShadowMap=true;//false;//true;
-int shadowMapWidth=8192;
+int shadowMapWidth=4096;//8192;
-int shadowMapHeight=8192;
+int shadowMapHeight=4096;
 float shadowMapWorldSize=50;
 
 #define MAX_POINTS_IN_BATCH 1024
@@ -158,10 +158,13 @@ struct InternalDataRenderer : public GLInstanceRendererInternalData
 	GLRenderToTexture*	m_shadowMap;
 	GLuint				m_shadowTexture;
 	
+	GLuint				m_renderFrameBuffer;
+	
 	
 	InternalDataRenderer() :
 		m_shadowMap(0),
-		m_shadowTexture(0)
+		m_shadowTexture(0),
+		m_renderFrameBuffer(0)
 	{
 		//clear to zero to make it obvious if the matrix is used uninitialized
 		for (int i=0;i<16;i++)
@@ -258,6 +261,8 @@ GLInstancingRenderer::GLInstancingRenderer(int maxNumObjectCapacity, int maxShap
 
 void GLInstancingRenderer::removeAllInstances()
 {
+	m_data->m_totalNumInstances = 0;
+
 	for (int i=0;i<m_graphicsInstances.size();i++)
 	{
 		if (m_graphicsInstances[i]->m_index_vbo)
@@ -273,6 +278,7 @@ void GLInstancingRenderer::removeAllInstances()
 	m_graphicsInstances.clear();
 }
 
+
 GLInstancingRenderer::~GLInstancingRenderer()
 {
 	delete m_data->m_shadowMap;
@@ -320,6 +326,7 @@ void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* positi
 	*/
 }
 
+
 void GLInstancingRenderer::writeSingleInstanceColorToCPU(double* color, int srcIndex)
 {
 	m_data->m_instance_colors_ptr[srcIndex*4+0]=float(color[0]);
@@ -337,7 +344,19 @@ void GLInstancingRenderer::writeSingleInstanceColorToCPU(float* color, int srcIn
 	m_data->m_instance_colors_ptr[srcIndex*4+3]=color[3];
 }
 
+void GLInstancingRenderer::writeSingleInstanceScaleToCPU(float* scale, int srcIndex)
+{
+	m_data->m_instance_scale_ptr[srcIndex*3+0]=scale[0];
+	m_data->m_instance_scale_ptr[srcIndex*3+1]=scale[1];
+	m_data->m_instance_scale_ptr[srcIndex*3+2]=scale[2];
+}
 
+void GLInstancingRenderer::writeSingleInstanceScaleToCPU(double* scale, int srcIndex)
+{
+	m_data->m_instance_scale_ptr[srcIndex*3+0]=scale[0];
+	m_data->m_instance_scale_ptr[srcIndex*3+1]=scale[1];
+	m_data->m_instance_scale_ptr[srcIndex*3+2]=scale[2];
+}
 
 void GLInstancingRenderer::writeSingleInstanceTransformToGPU(float* position, float* orientation, int objectIndex)
 {
@@ -383,25 +402,46 @@ void GLInstancingRenderer::writeTransforms()
 
 
 	glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vbo);
-	glFlush();
+	//glFlush();
 
 	b3Assert(glGetError() ==GL_NO_ERROR);
 	
 
+#ifdef B3_DEBUG
+	{
+		int totalNumInstances= 0;
+		for (int k=0;k<m_graphicsInstances.size();k++)
+		{
+			b3GraphicsInstance* gfxObj = m_graphicsInstances[k];
+			totalNumInstances+=gfxObj->m_numGraphicsInstances;
+		}
+		b3Assert(m_data->m_totalNumInstances == totalNumInstances);
+	}
+#endif//B3_DEBUG
+
+	int POSITION_BUFFER_SIZE = (m_data->m_totalNumInstances*sizeof(float)*4);
+	int ORIENTATION_BUFFER_SIZE = (m_data->m_totalNumInstances*sizeof(float)*4);
+	int COLOR_BUFFER_SIZE = (m_data->m_totalNumInstances*sizeof(float)*4);
+//	int SCALE_BUFFER_SIZE = (totalNumInstances*sizeof(float)*3);
+
+#if 1
+	glBufferSubData(	GL_ARRAY_BUFFER,m_data->m_maxShapeCapacityInBytes,m_data->m_totalNumInstances*sizeof(float)*4,
+ 						&m_data->m_instance_positions_ptr[0]);
+	glBufferSubData(	GL_ARRAY_BUFFER,m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE,m_data->m_totalNumInstances*sizeof(float)*4,
+ 						&m_data->m_instance_quaternion_ptr[0]);
+	glBufferSubData(	GL_ARRAY_BUFFER,m_data->m_maxShapeCapacityInBytes+ POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE, m_data->m_totalNumInstances*sizeof(float)*4,
+ 						&m_data->m_instance_colors_ptr[0]);
+	glBufferSubData(	GL_ARRAY_BUFFER, m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE,m_data->m_totalNumInstances*sizeof(float)*3,
+					 	&m_data->m_instance_scale_ptr[0]);
+#else
+
 	char* orgBase =  (char*)glMapBuffer( GL_ARRAY_BUFFER,GL_READ_WRITE);
 	if (orgBase)
 	{
 
 
-		int totalNumInstances= 0;
 		
-		for (int k=0;k<m_graphicsInstances.size();k++)
-		{
-			b3GraphicsInstance* gfxObj = m_graphicsInstances[k];
-			totalNumInstances+=gfxObj->m_numGraphicsInstances;
-		}
 		
-		m_data->m_totalNumInstances = totalNumInstances;
 
 		for (int k=0;k<m_graphicsInstances.size();k++)
 		{
@@ -410,10 +450,7 @@ void GLInstancingRenderer::writeTransforms()
 
 
 
-			int POSITION_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4);
+			
-			int ORIENTATION_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4);
-			int COLOR_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4);
-		//	int SCALE_BUFFER_SIZE = (totalNumInstances*sizeof(float)*3);
 
 			char* base = orgBase;
 
@@ -462,6 +499,9 @@ void GLInstancingRenderer::writeTransforms()
 	//if this glFinish is removed, the animation is not always working/blocks
 	//@todo: figure out why
 	glFlush();
+
+#endif
+
 	glBindBuffer(GL_ARRAY_BUFFER, 0);//m_data->m_vbo);
 
     b3Assert(glGetError() ==GL_NO_ERROR);
@@ -1072,6 +1112,9 @@ void GLInstancingRenderer::drawPoints(const float* positions, const float color[
 
 void GLInstancingRenderer::drawLines(const float* positions, const float color[4], int numPoints, int pointStrideInBytes, const unsigned int* indices, int numIndices, float lineWidthIn)
 {
+	glActiveTexture(GL_TEXTURE0);
+	glBindTexture(GL_TEXTURE_2D,0);
+
 	float lineWidth = lineWidthIn;
 	b3Clamp(lineWidth,(float)lineWidthRange[0],(float)lineWidthRange[1]);
 	glLineWidth(lineWidth);
@@ -1631,6 +1674,7 @@ b3Assert(glGetError() ==GL_NO_ERROR);
                             printf("gfxObj->m_numGraphicsInstances=%d\n",gfxObj->m_numGraphicsInstances);
                             printf("indexCount=%d\n",indexCount);
 				*/
+
 							glUseProgram(createShadowMapInstancingShader);
 							glUniformMatrix4fv(createShadow_depthMVP, 1, false, &depthMVP[0][0]);
 							glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances);
@@ -1663,7 +1707,8 @@ b3Assert(glGetError() ==GL_NO_ERROR);
                             {
                                 glDisable (GL_BLEND);
                             }
-                            
+                            glActiveTexture(GL_TEXTURE0);
+							glBindTexture(GL_TEXTURE_2D,0);
                             break;
 						}
 					default:
@@ -1688,6 +1733,7 @@ b3Assert(glGetError() ==GL_NO_ERROR);
 	{
 	//	writeTextureToPng(shadowMapWidth,shadowMapHeight,"shadowmap.png",4);
 		m_data->m_shadowMap->disable();
+		glBindFramebuffer( GL_FRAMEBUFFER, m_data->m_renderFrameBuffer);
 		glViewport(dims[0],dims[1],dims[2],dims[3]);
 
 	}
@@ -1733,4 +1779,16 @@ int GLInstancingRenderer::getInstanceCapacity() const
 {
 	return m_data->m_maxNumObjectCapacity;
 }
+
+void GLInstancingRenderer::setRenderFrameBuffer(unsigned int renderFrameBuffer)
+{
+	m_data->m_renderFrameBuffer = (GLuint) renderFrameBuffer;
+}
+
+int GLInstancingRenderer::getTotalNumInstances() const
+{
+    return m_data->m_totalNumInstances;
+}
+
+
 #endif //NO_OPENGL3

examples/OpenGLWindow/GLInstancingRenderer.h

@@ -95,6 +95,9 @@ public:
 	virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex);
 	virtual void writeSingleInstanceColorToCPU(double* color, int srcIndex);
 
+	virtual void writeSingleInstanceScaleToCPU(float* scale, int srcIndex);
+	virtual void writeSingleInstanceScaleToCPU(double* scale, int srcIndex);
+
 	
 	virtual struct	GLInstanceRendererInternalData* getInternalData();
 
@@ -125,6 +128,8 @@ public:
 	
 	virtual int getInstanceCapacity() const;
 	
+	virtual int getTotalNumInstances() const;
+	
 	virtual void enableShadowMap();
     virtual void enableBlend(bool blend)
     {
@@ -132,6 +137,7 @@ public:
     }
 	virtual void clearZBuffer();
 
+	virtual void setRenderFrameBuffer(unsigned int renderFrameBuffer);
 };
 
 #endif //GL_INSTANCING_RENDERER_H

examples/OpenGLWindow/SimpleCamera.cpp

@@ -62,6 +62,16 @@ void	SimpleCamera::setVRCamera(const float viewMat[16], const float projectionMa
 	}
 }
 
+void SimpleCamera::disableVRCamera()
+{
+	m_data->m_enableVR = false;
+}
+
+bool SimpleCamera::isVRCamera() const
+{
+	return m_data->m_enableVR ;
+}
+
 
 static void    b3CreateFrustum(
                         float left,

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 void disableVRCamera();
+	virtual bool isVRCamera() const;
 
 	virtual void getCameraTargetPosition(float pos[3]) const;
 	virtual void getCameraPosition(float pos[3]) const;

examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp

@@ -64,6 +64,20 @@ void SimpleOpenGL2Renderer::writeSingleInstanceColorToCPU(double* color, int src
 {
     
 }
+
+void SimpleOpenGL2Renderer::writeSingleInstanceScaleToCPU(float* scale, int srcIndex)
+{
+}
+void SimpleOpenGL2Renderer::writeSingleInstanceScaleToCPU(double* scale, int srcIndex)
+{
+}
+
+
+int SimpleOpenGL2Renderer::getTotalNumInstances() const
+{
+    return 0;
+}
+
 void	SimpleOpenGL2Renderer::getCameraViewMatrix(float viewMat[16]) const
 {
     b3Assert(0);

examples/OpenGLWindow/SimpleOpenGL2Renderer.h

@@ -32,6 +32,8 @@ struct SimpleOpenGL2Renderer : public CommonRenderInterface
     
     virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex);
     virtual void writeSingleInstanceColorToCPU(double* color, int srcIndex);
+	virtual void writeSingleInstanceScaleToCPU(float* scale, int srcIndex);
+    virtual void writeSingleInstanceScaleToCPU(double* scale, int srcIndex);
     virtual void	getCameraViewMatrix(float viewMat[16]) const;
     virtual void	getCameraProjectionMatrix(float projMat[16]) const;
 
@@ -68,6 +70,8 @@ struct SimpleOpenGL2Renderer : public CommonRenderInterface
     
     virtual void writeSingleInstanceTransformToCPU(const double* position, const double* orientation, int srcIndex);
     
+    virtual int getTotalNumInstances() const;
+    
     virtual void writeTransforms();
     
     virtual void drawLine(const double from[4], const double to[4], const double color[4], double lineWidth);
@@ -82,6 +86,7 @@ struct SimpleOpenGL2Renderer : public CommonRenderInterface
 
 	virtual void clearZBuffer();
 
+
 	virtual struct	GLInstanceRendererInternalData* getInternalData()
 	{
 		return 0;

examples/OpenGLWindow/Win32Window.cpp

@@ -435,11 +435,10 @@ void Win32Window::setWindowTitle(const char* titleChar)
 	wchar_t  windowTitle[1024];
 	swprintf(windowTitle, 1024, L"%hs", titleChar);
 
-	DWORD dwResult;
-
 #ifdef _WIN64
 		SetWindowTextW(m_data->m_hWnd, windowTitle);
 #else
+		DWORD dwResult;
 		SendMessageTimeoutW(m_data->m_hWnd, WM_SETTEXT, 0,
 				reinterpret_cast<LPARAM>(windowTitle),
 				SMTO_ABORTIFHUNG, 2000, &dwResult);

examples/RenderingExamples/RaytracerSetup.cpp

@@ -295,7 +295,7 @@ void RaytracerPhysicsSetup::stepSimulation(float deltaTime)
 
 	for (x=0;x<m_internalData->m_width;x++)
 	{
-		for (int y=0;y<m_internalData->m_height;y++)
+		for (y=0;y<m_internalData->m_height;y++)
 		{
 			btVector4 rgba(0,0,0,0);
 			btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;

examples/RoboticsLearning/R2D2GraspExample.cpp

@@ -0,0 +1,164 @@
+
+#include "R2D2GraspExample.h"
+
+#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
+#include "Bullet3Common/b3Quaternion.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "../CommonInterfaces/CommonRenderInterface.h"
+#include "../CommonInterfaces/CommonExampleInterface.h"
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "../SharedMemory/PhysicsServerSharedMemory.h"
+#include "../SharedMemory/PhysicsClientC_API.h"
+#include <string>
+
+#include "b3RobotSimAPI.h"
+#include "../Utils/b3Clock.h"
+
+///quick demo showing the right-handed coordinate system and positive rotations around each axis
+class R2D2GraspExample : public CommonExampleInterface
+{
+    CommonGraphicsApp* m_app;
+	GUIHelperInterface* m_guiHelper;
+	b3RobotSimAPI m_robotSim;
+	int m_options;
+	int m_r2d2Index;
+	
+    float m_x;
+    float m_y;
+    float m_z;
+	b3AlignedObjectArray<int> m_movingInstances;
+	enum
+	{
+		numCubesX = 20,
+		numCubesY = 20
+	};
+public:
+    
+    R2D2GraspExample(GUIHelperInterface* helper, int options)
+    :m_app(helper->getAppInterface()),
+	m_guiHelper(helper),
+	m_options(options),
+	m_r2d2Index(-1),
+    m_x(0),
+    m_y(0),
+	m_z(0)
+    {
+		m_app->setUpAxis(2);
+    }
+    virtual ~R2D2GraspExample()
+    {
+        m_app->m_renderer->enableBlend(false);
+    }
+
+    
+    virtual void physicsDebugDraw(int debugDrawMode)
+    {
+        
+    }
+    virtual void    initPhysics()
+    {
+		bool connected = m_robotSim.connect(m_guiHelper);
+		b3Printf("robotSim connected = %d",connected);
+		b3RobotSimLoadURDFArgs args("");
+		
+		if ((m_options & eROBOTIC_LEARN_GRASP)!=0)
+		{
+			args.m_urdfFileName = "r2d2.urdf";
+			args.m_startPosition.setValue(0,0,.5);
+			m_r2d2Index = m_robotSim.loadURDF(args);
+			int numJoints = m_robotSim.getNumJoints(m_r2d2Index);
+			b3Printf("numJoints = %d",numJoints);
+
+			for (int i=0;i<numJoints;i++)
+			{
+				b3JointInfo jointInfo;
+				m_robotSim.getJointInfo(m_r2d2Index,i,&jointInfo);
+				b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
+			}
+			int wheelJointIndices[4]={2,3,6,7};
+			int wheelTargetVelocities[4]={30,30,30,30};
+			for (int i=0;i<4;i++)
+			{
+				b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
+				controlArgs.m_targetVelocity = wheelTargetVelocities[i];
+				controlArgs.m_maxTorqueValue = 1e30;
+				m_robotSim.setJointMotorControl(m_r2d2Index,wheelJointIndices[i],controlArgs);
+			}
+		}
+
+		if ((m_options & eROBOTIC_LEARN_COMPLIANT_CONTACT)!=0)
+		{
+		
+		
+			args.m_urdfFileName = "cube.urdf";
+			args.m_startPosition.setValue(0,0,2.5);
+			m_robotSim.loadURDF(args);
+			args.m_startOrientation.setEulerZYX(0,0.2,0);
+		}
+	
+		args.m_urdfFileName = "plane.urdf";
+		args.m_startPosition.setValue(0,0,0);
+		
+		args.m_forceOverrideFixedBase = true;
+		m_robotSim.loadURDF(args);
+		m_robotSim.setGravity(b3MakeVector3(0,0,-10));
+
+    }
+    virtual void    exitPhysics()
+    {
+		m_robotSim.disconnect();
+    }
+    virtual void	stepSimulation(float deltaTime)
+	{
+		m_robotSim.stepSimulation();
+    }
+    virtual void	renderScene()
+    {
+		m_robotSim.renderScene();
+
+		//m_app->m_renderer->renderScene();
+    }
+
+	
+    virtual void	physicsDebugDraw()
+    {
+      	
+    }
+    virtual bool	mouseMoveCallback(float x,float y)
+    {
+		return false;   
+    }
+    virtual bool	mouseButtonCallback(int button, int state, float x, float y)
+    {
+        return false;   
+    }
+    virtual bool	keyboardCallback(int key, int state)
+    {
+        return false;   
+    }
+    
+	virtual void resetCamera()
+	{
+		float dist = 10;
+		float pitch = 50;
+		float yaw = 13;
+		float targetPos[3]={-1,0,-0.3};
+		if (m_app->m_renderer  && m_app->m_renderer->getActiveCamera())
+		{
+			m_app->m_renderer->getActiveCamera()->setCameraDistance(dist);
+			m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch);
+			m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw);
+			m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0],targetPos[1],targetPos[2]);
+		}
+	}
+
+};
+
+
+class	CommonExampleInterface*    R2D2GraspExampleCreateFunc(struct CommonExampleOptions& options)
+{
+	return new R2D2GraspExample(options.m_guiHelper, options.m_option);
+}
+
+
+

examples/RoboticsLearning/R2D2GraspExample.h

@@ -0,0 +1,28 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2016 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef R2D2_GRASP_EXAMPLE_H
+#define R2D2_GRASP_EXAMPLE_H
+
+enum RobotLearningExampleOptions
+{
+	eROBOTIC_LEARN_GRASP=1,
+	eROBOTIC_LEARN_COMPLIANT_CONTACT=2,
+};
+
+class CommonExampleInterface*    R2D2GraspExampleCreateFunc(struct CommonExampleOptions& options);
+
+
+#endif //R2D2_GRASP_EXAMPLE_H

examples/RoboticsLearning/b3RobotSimAPI.cpp

@@ -0,0 +1,729 @@
+#include "b3RobotSimAPI.h"
+
+//#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
+#include "Bullet3Common/b3Quaternion.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "../CommonInterfaces/CommonRenderInterface.h"
+//#include "../CommonInterfaces/CommonExampleInterface.h"
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "../SharedMemory/PhysicsServerSharedMemory.h"
+#include "../SharedMemory/PhysicsClientC_API.h"
+#include <string>
+
+
+
+
+
+
+#include "../Utils/b3Clock.h"
+#include "../MultiThreading/b3ThreadSupportInterface.h"
+
+
+void	RobotThreadFunc(void* userPtr,void* lsMemory);
+void*	RobotlsMemoryFunc();
+#define MAX_ROBOT_NUM_THREADS 1
+enum
+	{
+		numCubesX = 20,
+		numCubesY = 20
+	};
+
+
+enum TestRobotSimCommunicationEnums
+{
+	eRequestTerminateRobotSim= 13,
+	eRobotSimIsUnInitialized,
+	eRobotSimIsInitialized,
+	eRobotSimInitializationFailed,
+	eRobotSimHasTerminated
+};
+
+enum MultiThreadedGUIHelperCommunicationEnums
+{
+	eRobotSimGUIHelperIdle= 13,
+	eRobotSimGUIHelperRegisterTexture,
+	eRobotSimGUIHelperRegisterGraphicsShape,
+	eRobotSimGUIHelperRegisterGraphicsInstance,
+	eRobotSimGUIHelperCreateCollisionShapeGraphicsObject,
+	eRobotSimGUIHelperCreateCollisionObjectGraphicsObject,
+	eRobotSimGUIHelperRemoveAllGraphicsInstances,
+};
+
+#include <stdio.h>
+//#include "BulletMultiThreaded/PlatformDefinitions.h"
+
+#ifndef _WIN32
+#include "../MultiThreading/b3PosixThreadSupport.h"
+
+b3ThreadSupportInterface* createRobotSimThreadSupport(int numThreads)
+{
+	b3PosixThreadSupport::ThreadConstructionInfo constructionInfo("RobotSimThreads",
+                                                                RobotThreadFunc,
+                                                                RobotlsMemoryFunc,
+                                                                numThreads);
+    b3ThreadSupportInterface* threadSupport = new b3PosixThreadSupport(constructionInfo);
+
+	return threadSupport;
+
+}
+
+
+#elif defined( _WIN32)
+#include "../MultiThreading/b3Win32ThreadSupport.h"
+
+b3ThreadSupportInterface* createRobotSimThreadSupport(int numThreads)
+{
+	b3Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("RobotSimThreads",RobotThreadFunc,RobotlsMemoryFunc,numThreads);
+	b3Win32ThreadSupport* threadSupport = new b3Win32ThreadSupport(threadConstructionInfo);
+	return threadSupport;
+
+}
+#endif
+
+
+
+struct	RobotSimArgs
+{
+	RobotSimArgs()
+		:m_physicsServerPtr(0)
+	{
+	}
+	b3CriticalSection* m_cs;
+	
+	PhysicsServerSharedMemory*	m_physicsServerPtr;
+	b3AlignedObjectArray<b3Vector3> m_positions;
+};
+
+struct RobotSimThreadLocalStorage
+{
+	int threadId;
+};
+
+
+void	RobotThreadFunc(void* userPtr,void* lsMemory)
+{
+	printf("RobotThreadFunc thread started\n");
+	RobotSimThreadLocalStorage* localStorage = (RobotSimThreadLocalStorage*) lsMemory;
+
+	RobotSimArgs* args = (RobotSimArgs*) userPtr;
+	int workLeft = true;
+	b3Clock clock;
+	clock.reset();
+	bool init = true;
+	if (init)
+	{
+
+		args->m_cs->lock();
+		args->m_cs->setSharedParam(0,eRobotSimIsInitialized);
+		args->m_cs->unlock();
+
+		do
+		{
+//todo(erwincoumans): do we want some sleep to reduce CPU resources in this thread?
+#if 0
+			double deltaTimeInSeconds = double(clock.getTimeMicroseconds())/1000000.;
+			if (deltaTimeInSeconds<(1./260.))
+			{
+				if (deltaTimeInSeconds<.001)
+					continue;
+			}
+
+			clock.reset();
+#endif //
+			args->m_physicsServerPtr->processClientCommands();
+			
+		} while (args->m_cs->getSharedParam(0)!=eRequestTerminateRobotSim);
+	} else
+	{
+		args->m_cs->lock();
+		args->m_cs->setSharedParam(0,eRobotSimInitializationFailed);
+		args->m_cs->unlock();
+	}
+	//do nothing
+}
+
+
+
+void*	RobotlsMemoryFunc()
+{
+	//don't create local store memory, just return 0
+	return new RobotSimThreadLocalStorage;
+}
+
+
+
+ATTRIBUTE_ALIGNED16(class) MultiThreadedOpenGLGuiHelper : public GUIHelperInterface
+{
+	CommonGraphicsApp* m_app;
+	
+	b3CriticalSection* m_cs;
+
+	
+public:
+	
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	GUIHelperInterface* m_childGuiHelper;
+
+	const unsigned char* m_texels;
+	int m_textureWidth;
+	int m_textureHeight;
+
+
+	int m_shapeIndex;
+	const float* m_position;
+	const float* m_quaternion;
+	const float* m_color;
+	const float* m_scaling;
+
+	const float* m_vertices;
+	int m_numvertices;
+	const int* m_indices;
+	int m_numIndices;
+	int m_primitiveType;
+	int m_textureId;
+	int m_instanceId;
+	
+
+	MultiThreadedOpenGLGuiHelper(CommonGraphicsApp* app, GUIHelperInterface* guiHelper)
+		:m_app(app)
+		,m_cs(0),
+		m_texels(0),
+		m_textureId(-1)
+	{
+		m_childGuiHelper = guiHelper;;
+
+	}
+
+	virtual ~MultiThreadedOpenGLGuiHelper()
+	{
+		delete m_childGuiHelper;
+	}
+
+	void setCriticalSection(b3CriticalSection* cs)
+	{
+		m_cs = cs;
+	}
+
+	b3CriticalSection* getCriticalSection()
+	{
+		return m_cs;
+	}
+
+	virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color){}
+
+	btCollisionObject* m_obj;
+	btVector3 m_color2;
+
+	virtual void createCollisionObjectGraphicsObject(btCollisionObject* obj,const btVector3& color)
+	{
+		m_obj = obj;
+		m_color2 = color;
+		m_cs->lock();
+		m_cs->setSharedParam(1,eRobotSimGUIHelperCreateCollisionObjectGraphicsObject);
+		m_cs->unlock();
+		while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
+		{
+		}
+
+	}
+
+	btCollisionShape* m_colShape;
+	virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape)
+	{
+		m_colShape = collisionShape;
+		m_cs->lock();
+		m_cs->setSharedParam(1,eRobotSimGUIHelperCreateCollisionShapeGraphicsObject);
+		m_cs->unlock();
+		while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
+		{
+		}
+
+	}
+
+	virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld)
+	{
+	    //this check is to prevent a crash, in case we removed all graphics instances, but there are still physics objects.
+	    //the check will be obsolete, once we have a better/safer way of synchronizing physics->graphics transforms
+        if ( m_childGuiHelper->getRenderInterface()->getTotalNumInstances()>0)
+        {
+            m_childGuiHelper->syncPhysicsToGraphics(rbWorld);
+        }
+	}
+
+	virtual void render(const btDiscreteDynamicsWorld* rbWorld) 
+	{
+		m_childGuiHelper->render(0);
+	}
+
+	virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld){}
+
+	virtual int	registerTexture(const unsigned char* texels, int width, int height)
+	{
+		m_texels = texels;
+		m_textureWidth = width;
+		m_textureHeight = height;
+
+		m_cs->lock();
+		m_cs->setSharedParam(1,eRobotSimGUIHelperRegisterTexture);
+		m_cs->unlock();
+		while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
+		{
+		}
+		return m_textureId;
+	}
+	virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId)
+	{
+		m_vertices = vertices;
+		m_numvertices = numvertices;
+		m_indices = indices;
+		m_numIndices = numIndices;
+		m_primitiveType = primitiveType;
+		m_textureId = textureId;
+
+		m_cs->lock();
+		m_cs->setSharedParam(1,eRobotSimGUIHelperRegisterGraphicsShape);
+		m_cs->unlock();
+		while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
+		{
+		}
+		return m_shapeIndex;
+	}
+	virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) 
+	{
+		m_shapeIndex = shapeIndex;
+		m_position = position;
+		m_quaternion = quaternion;
+		m_color = color;
+		m_scaling = scaling;
+
+		m_cs->lock();
+		m_cs->setSharedParam(1,eRobotSimGUIHelperRegisterGraphicsInstance);
+		m_cs->unlock();
+		while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
+		{
+		}
+		return m_instanceId;
+	}
+
+    virtual void removeAllGraphicsInstances()
+    {
+        m_cs->lock();
+		m_cs->setSharedParam(1,eRobotSimGUIHelperRemoveAllGraphicsInstances);
+		m_cs->unlock();
+		while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
+		{
+		}
+    }
+
+	virtual Common2dCanvasInterface* get2dCanvasInterface()
+	{
+		return 0;
+	}
+	
+	virtual CommonParameterInterface* getParameterInterface()
+	{
+		return 0;
+	}
+
+	virtual CommonRenderInterface* getRenderInterface()
+	{
+		return 0;
+	}
+	
+	virtual CommonGraphicsApp* getAppInterface()
+	{
+		return m_childGuiHelper->getAppInterface();
+	}
+
+
+	virtual void setUpAxis(int axis)
+	{
+		m_childGuiHelper->setUpAxis(axis);
+	}
+	virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ)
+	{
+	}
+
+	virtual void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* width, int* height, int* numPixelsCopied)
+	{
+	     if (width)
+            *width = 0;
+        if (height)
+            *height = 0;
+        if (numPixelsCopied)
+            *numPixelsCopied = 0;
+	}
+
+	virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) 
+	{
+	}
+    
+	virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)
+	{
+	}
+};
+
+
+
+
+
+
+
+
+struct b3RobotSimAPI_InternalData
+{
+	//GUIHelperInterface* m_guiHelper;
+	PhysicsServerSharedMemory	m_physicsServer;
+	b3PhysicsClientHandle m_physicsClient;
+
+	b3ThreadSupportInterface* m_threadSupport;
+	RobotSimArgs m_args[MAX_ROBOT_NUM_THREADS];
+	MultiThreadedOpenGLGuiHelper* m_multiThreadedHelper;
+
+	bool m_connected;
+
+	b3RobotSimAPI_InternalData()
+		:m_multiThreadedHelper(0),
+		m_physicsClient(0),
+		m_connected(false)
+	{
+	}
+};
+
+b3RobotSimAPI::b3RobotSimAPI()
+{
+	m_data = new b3RobotSimAPI_InternalData;
+}
+
+void b3RobotSimAPI::stepSimulation()
+{
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	b3Assert(b3CanSubmitCommand(m_data->m_physicsClient));
+    if (b3CanSubmitCommand(m_data->m_physicsClient))
+    {
+        statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, b3InitStepSimulationCommand(m_data->m_physicsClient));
+        statusType = b3GetStatusType(statusHandle);
+        b3Assert(statusType==CMD_STEP_FORWARD_SIMULATION_COMPLETED);
+    }
+}
+
+void b3RobotSimAPI::setGravity(const b3Vector3& gravityAcceleration)
+{
+    b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClient);
+    b3SharedMemoryStatusHandle statusHandle;
+	b3PhysicsParamSetGravity(command,  gravityAcceleration[0],gravityAcceleration[1],gravityAcceleration[2]);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
+    b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED);
+
+}
+
+b3RobotSimAPI::~b3RobotSimAPI()
+{
+	delete m_data;
+}
+
+void b3RobotSimAPI::processMultiThreadedGraphicsRequests()
+{
+	switch (m_data->m_multiThreadedHelper->getCriticalSection()->getSharedParam(1))
+	{
+	case eRobotSimGUIHelperCreateCollisionShapeGraphicsObject:
+	{
+		m_data->m_multiThreadedHelper->m_childGuiHelper->createCollisionShapeGraphicsObject(m_data->m_multiThreadedHelper->m_colShape);
+		m_data->m_multiThreadedHelper->getCriticalSection()->lock();
+		m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
+		m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
+
+		break;
+	}
+	case eRobotSimGUIHelperCreateCollisionObjectGraphicsObject:
+	{
+		m_data->m_multiThreadedHelper->m_childGuiHelper->createCollisionObjectGraphicsObject(m_data->m_multiThreadedHelper->m_obj,
+			m_data->m_multiThreadedHelper->m_color2);
+		m_data->m_multiThreadedHelper->getCriticalSection()->lock();
+		m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
+		m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
+		break;
+	}
+	case eRobotSimGUIHelperRegisterTexture:
+	{
+		
+		m_data->m_multiThreadedHelper->m_textureId = m_data->m_multiThreadedHelper->m_childGuiHelper->registerTexture(m_data->m_multiThreadedHelper->m_texels,
+						m_data->m_multiThreadedHelper->m_textureWidth,m_data->m_multiThreadedHelper->m_textureHeight);
+
+		m_data->m_multiThreadedHelper->getCriticalSection()->lock();
+		m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
+		m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
+		
+		break;
+	}
+	case eRobotSimGUIHelperRegisterGraphicsShape:
+	{
+		m_data->m_multiThreadedHelper->m_shapeIndex = m_data->m_multiThreadedHelper->m_childGuiHelper->registerGraphicsShape(
+				m_data->m_multiThreadedHelper->m_vertices,
+				m_data->m_multiThreadedHelper->m_numvertices,
+				m_data->m_multiThreadedHelper->m_indices,
+				m_data->m_multiThreadedHelper->m_numIndices,
+				m_data->m_multiThreadedHelper->m_primitiveType,
+				m_data->m_multiThreadedHelper->m_textureId);
+
+		m_data->m_multiThreadedHelper->getCriticalSection()->lock();
+		m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
+		m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
+		break;
+	}
+	case eRobotSimGUIHelperRegisterGraphicsInstance:
+	{
+		m_data->m_multiThreadedHelper->m_instanceId = m_data->m_multiThreadedHelper->m_childGuiHelper->registerGraphicsInstance(
+				m_data->m_multiThreadedHelper->m_shapeIndex,
+				m_data->m_multiThreadedHelper->m_position,
+				m_data->m_multiThreadedHelper->m_quaternion,
+				m_data->m_multiThreadedHelper->m_color,
+				m_data->m_multiThreadedHelper->m_scaling);
+
+		m_data->m_multiThreadedHelper->getCriticalSection()->lock();
+		m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
+		m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
+		break;
+	}
+	case eRobotSimGUIHelperRemoveAllGraphicsInstances:
+        {
+            m_data->m_multiThreadedHelper->m_childGuiHelper->removeAllGraphicsInstances();
+			int numRenderInstances = m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->getTotalNumInstances();
+			b3Assert(numRenderInstances==0);
+
+            m_data->m_multiThreadedHelper->getCriticalSection()->lock();
+            m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
+            m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
+            break;
+        }
+	case eRobotSimGUIHelperIdle:
+	default:
+		{
+			
+		}
+	}
+	
+
+
+
+	#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
+
+	
+}
+
+b3SharedMemoryStatusHandle b3RobotSimAPI::submitClientCommandAndWaitStatusMultiThreaded(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle)
+{
+	int timeout = 1024*1024*1024;
+    b3SharedMemoryStatusHandle statusHandle=0;
+    
+    b3SubmitClientCommand(physClient,commandHandle);
+    
+    while ((statusHandle==0) && (timeout-- > 0))
+    {
+        statusHandle =b3ProcessServerStatus(physClient);
+		processMultiThreadedGraphicsRequests();
+    }
+    return (b3SharedMemoryStatusHandle) statusHandle;
+}
+
+int b3RobotSimAPI::getNumJoints(int bodyUniqueId) const
+{
+	return b3GetNumJoints(m_data->m_physicsClient,bodyUniqueId);
+}
+
+bool b3RobotSimAPI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo)
+{
+	return (b3GetJointInfo(m_data->m_physicsClient,bodyUniqueId, jointIndex,jointInfo)!=0);
+}
+
+void b3RobotSimAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3JointMotorArgs& args)
+{
+	b3SharedMemoryStatusHandle statusHandle;
+	switch (args.m_controlMode)
+	{
+		case CONTROL_MODE_VELOCITY:
+		{
+			b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_data->m_physicsClient, CONTROL_MODE_VELOCITY);
+			b3JointInfo jointInfo;
+			b3GetJointInfo(m_data->m_physicsClient, bodyUniqueId, jointIndex, &jointInfo);
+			int uIndex = jointInfo.m_uIndex;
+			b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity);
+			b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue);
+			statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
+			break;
+		}
+		case CONTROL_MODE_POSITION_VELOCITY_PD:
+		{
+			b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_data->m_physicsClient, CONTROL_MODE_POSITION_VELOCITY_PD);
+			b3JointInfo jointInfo;
+			b3GetJointInfo(m_data->m_physicsClient, bodyUniqueId, jointIndex, &jointInfo);
+			int uIndex = jointInfo.m_uIndex;
+			int qIndex = jointInfo.m_qIndex;
+
+			b3JointControlSetDesiredPosition(command,qIndex,args.m_targetPosition);
+			b3JointControlSetKp(command,uIndex,args.m_kp);
+			b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity);
+			b3JointControlSetKd(command,uIndex,args.m_kd);
+			b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue);
+			statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
+			break;
+		}
+		case CONTROL_MODE_TORQUE:
+		{
+			b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_data->m_physicsClient, CONTROL_MODE_TORQUE);
+			b3JointInfo jointInfo;
+			b3GetJointInfo(m_data->m_physicsClient, bodyUniqueId, jointIndex, &jointInfo);
+			int uIndex = jointInfo.m_uIndex;
+			b3JointControlSetDesiredForceTorque(command,uIndex,args.m_maxTorqueValue);
+			statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
+			break;
+		}
+		default:
+		{
+			b3Error("Unknown control command in b3RobotSimAPI::setJointMotorControl");
+		}
+	}
+}
+
+int b3RobotSimAPI::loadURDF(const b3RobotSimLoadURDFArgs& args)
+{
+	int robotUniqueId = -1;
+	b3Assert(m_data->m_connected);
+
+	 {
+            b3SharedMemoryStatusHandle statusHandle;
+			int statusType;
+			b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClient, args.m_urdfFileName.c_str());
+			
+            //setting the initial position, orientation and other arguments are optional
+            
+            b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0],
+															args.m_startPosition[1],
+															args.m_startPosition[2]);
+			b3LoadUrdfCommandSetStartOrientation(command,args.m_startOrientation[0]
+													,args.m_startOrientation[1]
+													,args.m_startOrientation[2]
+													,args.m_startOrientation[3]);
+			if (args.m_forceOverrideFixedBase)
+			{
+				b3LoadUrdfCommandSetUseFixedBase(command,true);
+			}
+            statusHandle = submitClientCommandAndWaitStatusMultiThreaded(m_data->m_physicsClient, command);
+			statusType = b3GetStatusType(statusHandle);
+			b3Assert(statusType==CMD_URDF_LOADING_COMPLETED);
+			robotUniqueId = b3GetStatusBodyIndex(statusHandle);
+        }
+
+	return robotUniqueId;
+}
+
+bool b3RobotSimAPI::connect(GUIHelperInterface* guiHelper)
+{
+	m_data->m_multiThreadedHelper  = new MultiThreadedOpenGLGuiHelper(guiHelper->getAppInterface(),guiHelper);
+	
+	MultiThreadedOpenGLGuiHelper* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper(guiHelper->getAppInterface(),guiHelper);
+
+	
+	
+
+	m_data->m_threadSupport = createRobotSimThreadSupport(MAX_ROBOT_NUM_THREADS);
+
+	for (int i=0;i<m_data->m_threadSupport->getNumTasks();i++)
+	{
+		RobotSimThreadLocalStorage* storage = (RobotSimThreadLocalStorage*) m_data->m_threadSupport->getThreadLocalMemory(i);
+		b3Assert(storage);
+		storage->threadId = i;
+		//storage->m_sharedMem = data->m_sharedMem;
+	}
+
+
+		
+
+	for (int w=0;w<MAX_ROBOT_NUM_THREADS;w++)
+	{
+		m_data->m_args[w].m_cs = m_data->m_threadSupport->createCriticalSection();
+		m_data->m_args[w].m_cs->setSharedParam(0,eRobotSimIsUnInitialized);
+		int numMoving = 0;
+ 		m_data->m_args[w].m_positions.resize(numMoving);
+		m_data->m_args[w].m_physicsServerPtr = &m_data->m_physicsServer;
+		int index = 0;
+			
+		m_data->m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*) &m_data->m_args[w], w);
+		while (m_data->m_args[w].m_cs->getSharedParam(0)==eRobotSimIsUnInitialized)
+		{
+		}
+	}
+
+	m_data->m_args[0].m_cs->setSharedParam(1,eRobotSimGUIHelperIdle);
+	m_data->m_multiThreadedHelper->setCriticalSection(m_data->m_args[0].m_cs);
+
+	m_data->m_connected = m_data->m_physicsServer.connectSharedMemory( m_data->m_multiThreadedHelper);
+
+		b3Assert(m_data->m_connected);
+
+	m_data->m_physicsClient = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
+	int canSubmit = b3CanSubmitCommand(m_data->m_physicsClient);
+	b3Assert(canSubmit);
+	return m_data->m_connected && canSubmit;
+}
+
+void b3RobotSimAPI::disconnect()
+{
+
+	for (int i=0;i<MAX_ROBOT_NUM_THREADS;i++)
+	{
+		m_data->m_args[i].m_cs->lock();
+		m_data->m_args[i].m_cs->setSharedParam(0,eRequestTerminateRobotSim);
+		m_data->m_args[i].m_cs->unlock();
+	}
+	int numActiveThreads = MAX_ROBOT_NUM_THREADS;
+
+	while (numActiveThreads)
+            {
+		int arg0,arg1;
+                    if (m_data->m_threadSupport->isTaskCompleted(&arg0,&arg1,0))
+                    {
+                            numActiveThreads--;
+                            printf("numActiveThreads = %d\n",numActiveThreads);
+
+                    } else
+                    {
+                    }
+            };
+
+	printf("stopping threads\n");
+
+	delete m_data->m_threadSupport;   
+	m_data->m_threadSupport = 0;
+
+	b3DisconnectSharedMemory(m_data->m_physicsClient);
+	m_data->m_physicsServer.disconnectSharedMemory(true);
+	m_data->m_connected = false;
+}
+
+void b3RobotSimAPI::renderScene()
+{
+	if (m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface())
+	{
+		m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms();
+	}
+
+	m_data->m_physicsServer.renderScene();
+}

examples/RoboticsLearning/b3RobotSimAPI.h

@@ -0,0 +1,81 @@
+#ifndef B3_ROBOT_SIM_API_H
+#define B3_ROBOT_SIM_API_H
+
+///todo: remove those includes from this header
+#include "../SharedMemory/PhysicsClientC_API.h"
+#include "../SharedMemory/SharedMemoryPublic.h"
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Common/b3Quaternion.h"
+#include <string>
+
+
+struct b3RobotSimLoadURDFArgs
+{
+	std::string m_urdfFileName;
+	b3Vector3 m_startPosition;
+	b3Quaternion m_startOrientation;
+	bool m_forceOverrideFixedBase;
+
+
+	b3RobotSimLoadURDFArgs(const std::string& urdfFileName)
+		:m_urdfFileName(urdfFileName),
+		m_startPosition(b3MakeVector3(0,0,0)),
+		m_startOrientation(b3Quaternion(0,0,0,1)),
+		m_forceOverrideFixedBase(false)
+	{
+	}
+};
+
+struct b3JointMotorArgs
+{
+	int m_controlMode;
+	
+	double m_targetPosition;
+	double m_kp;
+
+	double m_targetVelocity;
+	double m_kd;
+
+	double m_maxTorqueValue;
+
+	b3JointMotorArgs(int controlMode)
+		:m_controlMode(controlMode),
+		m_targetPosition(0),
+		m_kp(0.1),
+		m_targetVelocity(0),
+		m_kd(0.1),
+		m_maxTorqueValue(1000)
+	{
+	}
+};
+
+
+class b3RobotSimAPI
+{
+	struct b3RobotSimAPI_InternalData* m_data;
+	void processMultiThreadedGraphicsRequests();
+	b3SharedMemoryStatusHandle  submitClientCommandAndWaitStatusMultiThreaded(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle);
+	
+public:
+	b3RobotSimAPI();
+	virtual ~b3RobotSimAPI();
+
+	bool connect(struct GUIHelperInterface* guiHelper);
+	void disconnect();
+
+	int loadURDF(const struct b3RobotSimLoadURDFArgs& args);
+
+	int getNumJoints(int bodyUniqueId) const;
+
+	bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo);
+
+	void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3JointMotorArgs& args);
+
+	void stepSimulation();
+
+	void setGravity(const b3Vector3& gravityAcceleration);
+
+	void renderScene();
+};
+
+#endif //B3_ROBOT_SIM_API_H

examples/RollingFrictionDemo/RollingFrictionDemo.cpp

@@ -234,9 +234,8 @@ void	RollingFrictionDemo::initPhysics()
 	{	
 	btSerializer* s = new btDefaultSerializer;
 	m_dynamicsWorld->serialize(s);
-	b3ResourcePath p;
 	char resourcePath[1024];
-	if (p.findResourcePath("slope.bullet",resourcePath,1024))
+	if (b3ResourcePath::findResourcePath("slope.bullet",resourcePath,1024))
 	{
 		FILE* f = fopen(resourcePath,"wb");
 		fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);

examples/SharedMemory/PhysicsClientExample.cpp

@@ -835,7 +835,7 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
             enqueueCommand(CMD_STEP_FORWARD_SIMULATION);
 			if (m_options != eCLIENTEXAMPLE_SERVER)
 			{
-				enqueueCommand(CMD_REQUEST_DEBUG_LINES);
+				//enqueueCommand(CMD_REQUEST_DEBUG_LINES);
 			}
         }
     }
@@ -855,3 +855,4 @@ class CommonExampleInterface*    PhysicsClientCreateFunc(struct CommonExampleOpt
 	}
 	return example;
 }
+

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -659,6 +659,8 @@ bool PhysicsClientSharedMemory::canSubmitCommand() const {
 }
 
 struct SharedMemoryCommand* PhysicsClientSharedMemory::getAvailableSharedMemoryCommand() {
+    static int sequence = 0;
+    m_data->m_testBlock1->m_clientCommands[0].m_sequenceNumber = sequence++;
     return &m_data->m_testBlock1->m_clientCommands[0];
 }
 

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -10,7 +10,7 @@
 #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
 #include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
-#include "../CommonInterfaces/CommonRenderInterface.h"
+
 
 #include "btBulletDynamicsCommon.h"
 
@@ -1760,6 +1760,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 					if (m_data->m_verboseOutput)
 					{
 						b3Printf("Step simulation request");
+						b3Printf("CMD_STEP_FORWARD_SIMULATION clientCmd = %d\n", clientCmd.m_sequenceNumber);
 					}
                     ///todo(erwincoumans) move this damping inside Bullet
                     for (int i=0;i<m_data->m_bodyHandles.size();i++)
@@ -1864,16 +1865,21 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                 case CMD_RESET_SIMULATION:
                 {
 					//clean up all data
-					if (m_data && m_data->m_guiHelper && m_data->m_guiHelper->getRenderInterface())
+					
+					
+					
+					if (m_data && m_data->m_guiHelper)
 					{
-                        m_data->m_guiHelper->getRenderInterface()->removeAllInstances();
+						m_data->m_guiHelper->removeAllGraphicsInstances();
 					}
 					if (m_data)
 					{
 						m_data->m_visualConverter.resetAll();
 					}
+					
 					deleteDynamicsWorld();
 					createEmptyDynamicsWorld();
+					
 					m_data->exitHandles();
 					m_data->initHandles();
 
@@ -2058,6 +2064,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                     }
                 case CMD_APPLY_EXTERNAL_FORCE:
                 {
+                	if (m_data->m_verboseOutput)
+									{
+                    b3Printf("CMD_APPLY_EXTERNAL_FORCE clientCmd = %d\n", clientCmd.m_sequenceNumber);
+                  }
                     for (int i = 0; i < clientCmd.m_externalForceArguments.m_numForcesAndTorques; ++i)
                     {
                         InteralBodyData* body = m_data->getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]);

examples/SharedMemory/PhysicsServerExample.cpp

@@ -1,5 +1,6 @@
 
 
+//todo(erwincoumans): re-use the upcoming b3RobotSimAPI here
 
 #include "PhysicsServerExample.h"
 
@@ -9,11 +10,369 @@
 
 #include "SharedMemoryCommon.h"
 
+#include "../Utils/b3Clock.h"
+#include "../MultiThreading/b3ThreadSupportInterface.h"
+
+
+void	MotionThreadFunc(void* userPtr,void* lsMemory);
+void*	MotionlsMemoryFunc();
+#define MAX_MOTION_NUM_THREADS 1
+enum
+	{
+		numCubesX = 20,
+		numCubesY = 20
+	};
+
+
+enum TestExampleBrowserCommunicationEnums
+{
+	eRequestTerminateMotion= 13,
+	eMotionIsUnInitialized,
+	eMotionIsInitialized,
+	eMotionInitializationFailed,
+	eMotionHasTerminated
+};
+
+enum MultiThreadedGUIHelperCommunicationEnums
+{
+	eGUIHelperIdle= 13,
+	eGUIHelperRegisterTexture,
+	eGUIHelperRegisterGraphicsShape,
+	eGUIHelperRegisterGraphicsInstance,
+	eGUIHelperCreateCollisionShapeGraphicsObject,
+	eGUIHelperCreateCollisionObjectGraphicsObject,
+	eGUIHelperRemoveAllGraphicsInstances,
+};
+
+#include <stdio.h>
+//#include "BulletMultiThreaded/PlatformDefinitions.h"
+
+#ifndef _WIN32
+#include "../MultiThreading/b3PosixThreadSupport.h"
+
+b3ThreadSupportInterface* createMotionThreadSupport(int numThreads)
+{
+	b3PosixThreadSupport::ThreadConstructionInfo constructionInfo("MotionThreads",
+                                                                MotionThreadFunc,
+                                                                MotionlsMemoryFunc,
+                                                                numThreads);
+    b3ThreadSupportInterface* threadSupport = new b3PosixThreadSupport(constructionInfo);
+
+	return threadSupport;
+
+}
+
+
+#elif defined( _WIN32)
+#include "../MultiThreading/b3Win32ThreadSupport.h"
+
+b3ThreadSupportInterface* createMotionThreadSupport(int numThreads)
+{
+	b3Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("MotionThreads",MotionThreadFunc,MotionlsMemoryFunc,numThreads);
+	b3Win32ThreadSupport* threadSupport = new b3Win32ThreadSupport(threadConstructionInfo);
+	return threadSupport;
+
+}
+#endif
+
+
+
+struct	MotionArgs
+{
+	MotionArgs()
+		:m_physicsServerPtr(0)
+	{
+	}
+	b3CriticalSection* m_cs;
+	
+	PhysicsServerSharedMemory*	m_physicsServerPtr;
+	b3AlignedObjectArray<b3Vector3> m_positions;
+};
+
+struct MotionThreadLocalStorage
+{
+	int threadId;
+};
+
+int skip = 0;
+
+void	MotionThreadFunc(void* userPtr,void* lsMemory)
+{
+	printf("MotionThreadFunc thread started\n");
+	MotionThreadLocalStorage* localStorage = (MotionThreadLocalStorage*) lsMemory;
+
+	MotionArgs* args = (MotionArgs*) userPtr;
+	int workLeft = true;
+	b3Clock clock;
+	clock.reset();
+	bool init = true;
+	if (init)
+	{
+
+		args->m_cs->lock();
+		args->m_cs->setSharedParam(0,eMotionIsInitialized);
+		args->m_cs->unlock();
+
+		do
+		{
+//todo(erwincoumans): do we want some sleep to reduce CPU resources in this thread?
+#if 0
+			double deltaTimeInSeconds = double(clock.getTimeMicroseconds())/1000000.;
+			if (deltaTimeInSeconds<(1./260.))
+			{
+				skip++;
+				if (deltaTimeInSeconds<.001)
+					continue;
+			}
+
+			clock.reset();
+#endif
+			args->m_physicsServerPtr->processClientCommands();
+			
+			
+		} while (args->m_cs->getSharedParam(0)!=eRequestTerminateMotion);
+	} else
+	{
+		args->m_cs->lock();
+		args->m_cs->setSharedParam(0,eMotionInitializationFailed);
+		args->m_cs->unlock();
+	}
+
+
+	printf("finished, #skip = %d\n",skip);
+	skip=0;
+	//do nothing
+
+}
+
+
+
+void*	MotionlsMemoryFunc()
+{
+	//don't create local store memory, just return 0
+	return new MotionThreadLocalStorage;
+}
+
+
+
+class MultiThreadedOpenGLGuiHelper : public GUIHelperInterface
+{
+	CommonGraphicsApp* m_app;
+	
+	b3CriticalSection* m_cs;
+
+	
+public:
+
+	GUIHelperInterface* m_childGuiHelper;
+
+	const unsigned char* m_texels;
+	int m_textureWidth;
+	int m_textureHeight;
+
+
+	int m_shapeIndex;
+	const float* m_position;
+	const float* m_quaternion;
+	const float* m_color;
+	const float* m_scaling;
+
+	const float* m_vertices;
+	int m_numvertices;
+	const int* m_indices;
+	int m_numIndices;
+	int m_primitiveType;
+	int m_textureId;
+	int m_instanceId;
+	
+
+	MultiThreadedOpenGLGuiHelper(CommonGraphicsApp* app, GUIHelperInterface* guiHelper)
+		:m_app(app)
+		,m_cs(0),
+		m_texels(0),
+		m_textureId(-1)
+	{
+		m_childGuiHelper = guiHelper;;
+
+	}
+
+	virtual ~MultiThreadedOpenGLGuiHelper()
+	{
+		delete m_childGuiHelper;
+	}
+
+	void setCriticalSection(b3CriticalSection* cs)
+	{
+		m_cs = cs;
+	}
+
+	b3CriticalSection* getCriticalSection()
+	{
+		return m_cs;
+	}
+
+	virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color){}
+
+	btCollisionObject* m_obj;
+	btVector3 m_color2;
+
+	virtual void createCollisionObjectGraphicsObject(btCollisionObject* obj,const btVector3& color)
+	{
+		m_obj = obj;
+		m_color2 = color;
+		m_cs->lock();
+		m_cs->setSharedParam(1,eGUIHelperCreateCollisionObjectGraphicsObject);
+		m_cs->unlock();
+		while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
+		{
+		}
+
+	}
+
+	btCollisionShape* m_colShape;
+	virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape)
+	{
+		m_colShape = collisionShape;
+		m_cs->lock();
+		m_cs->setSharedParam(1,eGUIHelperCreateCollisionShapeGraphicsObject);
+		m_cs->unlock();
+		while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
+		{
+		}
+
+	}
+
+	virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld)
+	{
+	    //this check is to prevent a crash, in case we removed all graphics instances, but there are still physics objects.
+	    //the check will be obsolete, once we have a better/safer way of synchronizing physics->graphics transforms
+        if ( m_childGuiHelper->getRenderInterface()->getTotalNumInstances()>0)
+        {
+            m_childGuiHelper->syncPhysicsToGraphics(rbWorld);
+        }
+	}
+
+	virtual void render(const btDiscreteDynamicsWorld* rbWorld) 
+	{
+		m_childGuiHelper->render(0);
+	}
+
+	virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld){}
+
+	virtual int	registerTexture(const unsigned char* texels, int width, int height)
+	{
+		m_texels = texels;
+		m_textureWidth = width;
+		m_textureHeight = height;
+
+		m_cs->lock();
+		m_cs->setSharedParam(1,eGUIHelperRegisterTexture);
+		m_cs->unlock();
+		while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
+		{
+		}
+		return m_textureId;
+	}
+	virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId)
+	{
+		m_vertices = vertices;
+		m_numvertices = numvertices;
+		m_indices = indices;
+		m_numIndices = numIndices;
+		m_primitiveType = primitiveType;
+		m_textureId = textureId;
+
+		m_cs->lock();
+		m_cs->setSharedParam(1,eGUIHelperRegisterGraphicsShape);
+		m_cs->unlock();
+		while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
+		{
+		}
+		return m_shapeIndex;
+	}
+	virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) 
+	{
+		m_shapeIndex = shapeIndex;
+		m_position = position;
+		m_quaternion = quaternion;
+		m_color = color;
+		m_scaling = scaling;
+
+		m_cs->lock();
+		m_cs->setSharedParam(1,eGUIHelperRegisterGraphicsInstance);
+		m_cs->unlock();
+		while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
+		{
+		}
+		return m_instanceId;
+	}
+
+    virtual void removeAllGraphicsInstances()
+    {
+        m_cs->lock();
+		m_cs->setSharedParam(1,eGUIHelperRemoveAllGraphicsInstances);
+		m_cs->unlock();
+		while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
+		{
+		}
+    }
+
+	virtual Common2dCanvasInterface* get2dCanvasInterface()
+	{
+		return 0;
+	}
+	
+	virtual CommonParameterInterface* getParameterInterface()
+	{
+		return 0;
+	}
+
+	virtual CommonRenderInterface* getRenderInterface()
+	{
+		return 0;
+	}
+	
+	virtual CommonGraphicsApp* getAppInterface()
+	{
+		return m_childGuiHelper->getAppInterface();
+	}
+
+
+	virtual void setUpAxis(int axis)
+	{
+		m_childGuiHelper->setUpAxis(axis);
+	}
+	virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ)
+	{
+	}
+
+	virtual void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* width, int* height, int* numPixelsCopied)
+	{
+	     if (width)
+            *width = 0;
+        if (height)
+            *height = 0;
+        if (numPixelsCopied)
+            *numPixelsCopied = 0;
+	}
+
+	virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) 
+	{
+	}
+    
+	virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)
+	{
+	}
+};
+
+
 
 class PhysicsServerExample : public SharedMemoryCommon
 {
 	PhysicsServerSharedMemory	m_physicsServer;
-
+	b3ThreadSupportInterface* m_threadSupport;
+	MotionArgs m_args[MAX_MOTION_NUM_THREADS];
+	MultiThreadedOpenGLGuiHelper* m_multiThreadedHelper;
     bool m_wantsShutdown;
 
     bool m_isConnected;
@@ -23,7 +382,7 @@ class PhysicsServerExample : public SharedMemoryCommon
 
 public:
 
-	PhysicsServerExample(GUIHelperInterface* helper, SharedMemoryInterface* sharedMem=0, int options=0);
+	PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper, SharedMemoryInterface* sharedMem=0, int options=0);
 
 	virtual ~PhysicsServerExample();
 
@@ -56,7 +415,7 @@ public:
     virtual bool wantsTermination();
     virtual bool isConnected();
 	virtual void	renderScene();
-	virtual void    exitPhysics(){}
+	virtual void    exitPhysics();
 
 	virtual void	physicsDebugDraw(int debugFlags);
 
@@ -71,7 +430,6 @@ public:
 
 		if (!renderer)
 		{
-			btAssert(0);
 			return false;
 		}
 
@@ -91,7 +449,6 @@ public:
 
 		if (!renderer)
 		{
-			btAssert(0);
 			return false;
 		}
 
@@ -134,7 +491,7 @@ public:
 
 };
 
-PhysicsServerExample::PhysicsServerExample(GUIHelperInterface* helper, SharedMemoryInterface* sharedMem, int options)
+PhysicsServerExample::PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper, SharedMemoryInterface* sharedMem, int options)
 :SharedMemoryCommon(helper),
 m_physicsServer(sharedMem),
 m_wantsShutdown(false),
@@ -142,6 +499,7 @@ m_isConnected(false),
 m_replay(false),
 m_options(options)
 {
+	m_multiThreadedHelper = helper;
 	b3Printf("Started PhysicsServer\n");
 }
 
@@ -165,22 +523,80 @@ void	PhysicsServerExample::initPhysics()
 	int upAxis = 2;
 	m_guiHelper->setUpAxis(upAxis);
 
-#if 0
 
-    createEmptyDynamicsWorld();
 	
-	//todo: create a special debug drawer that will cache the lines, so we can send the debug info over the wire
-	btVector3 grav(0,0,0);
-	grav[upAxis] = 0;//-9.8;
-	this->m_dynamicsWorld->setGravity(grav);
 
-#endif
 
+	m_threadSupport = createMotionThreadSupport(MAX_MOTION_NUM_THREADS);
+		
+		
+
+		for (int i=0;i<m_threadSupport->getNumTasks();i++)
+		{
+			MotionThreadLocalStorage* storage = (MotionThreadLocalStorage*) m_threadSupport->getThreadLocalMemory(i);
+			b3Assert(storage);
+			storage->threadId = i;
+			//storage->m_sharedMem = data->m_sharedMem;
+		}
+
+
+		
+
+		for (int w=0;w<MAX_MOTION_NUM_THREADS;w++)
+		{
+			m_args[w].m_cs = m_threadSupport->createCriticalSection();
+			m_args[w].m_cs->setSharedParam(0,eMotionIsUnInitialized);
+			int numMoving = 0;
+ 			m_args[w].m_positions.resize(numMoving);
+			m_args[w].m_physicsServerPtr = &m_physicsServer;
+			int index = 0;
+			
+			m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*) &this->m_args[w], w);
+			while (m_args[w].m_cs->getSharedParam(0)==eMotionIsUnInitialized)
+			{
+			}
+		}
+
+		m_args[0].m_cs->setSharedParam(1,eGUIHelperIdle);
+		m_multiThreadedHelper->setCriticalSection(m_args[0].m_cs);
 		m_isConnected = m_physicsServer.connectSharedMemory( m_guiHelper);
+}
+
+
+void    PhysicsServerExample::exitPhysics()
+{
+		for (int i=0;i<MAX_MOTION_NUM_THREADS;i++)
+		{
+			m_args[i].m_cs->lock();
+			m_args[i].m_cs->setSharedParam(0,eRequestTerminateMotion);
+			m_args[i].m_cs->unlock();
+		}
+		int numActiveThreads = MAX_MOTION_NUM_THREADS;
+
+		while (numActiveThreads)
+                {
+			int arg0,arg1;
+                        if (m_threadSupport->isTaskCompleted(&arg0,&arg1,0))
+                        {
+                                numActiveThreads--;
+                                printf("numActiveThreads = %d\n",numActiveThreads);
+
+                        } else
+                        {
+                        }
+                };
+
+		printf("stopping threads\n");
+
+		delete m_threadSupport;   
+		m_threadSupport = 0;
+
+		//m_physicsServer.resetDynamicsWorld();
 		
 }
 
 
+
 bool PhysicsServerExample::wantsTermination()
 {
     return m_wantsShutdown;
@@ -190,6 +606,93 @@ bool PhysicsServerExample::wantsTermination()
 
 void	PhysicsServerExample::stepSimulation(float deltaTime)
 {
+	//this->m_physicsServer.processClientCommands();
+
+	//check if any graphics related tasks are requested
+	
+	switch (m_multiThreadedHelper->getCriticalSection()->getSharedParam(1))
+	{
+	case eGUIHelperCreateCollisionShapeGraphicsObject:
+	{
+		m_multiThreadedHelper->m_childGuiHelper->createCollisionShapeGraphicsObject(m_multiThreadedHelper->m_colShape);
+		m_multiThreadedHelper->getCriticalSection()->lock();
+		m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
+		m_multiThreadedHelper->getCriticalSection()->unlock();
+
+		break;
+	}
+	case eGUIHelperCreateCollisionObjectGraphicsObject:
+	{
+		m_multiThreadedHelper->m_childGuiHelper->createCollisionObjectGraphicsObject(m_multiThreadedHelper->m_obj,
+			m_multiThreadedHelper->m_color2);
+		m_multiThreadedHelper->getCriticalSection()->lock();
+		m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
+		m_multiThreadedHelper->getCriticalSection()->unlock();
+		break;
+	}
+	case eGUIHelperRegisterTexture:
+	{
+		
+		m_multiThreadedHelper->m_textureId = m_multiThreadedHelper->m_childGuiHelper->registerTexture(m_multiThreadedHelper->m_texels,
+						m_multiThreadedHelper->m_textureWidth,m_multiThreadedHelper->m_textureHeight);
+
+		m_multiThreadedHelper->getCriticalSection()->lock();
+		m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
+		m_multiThreadedHelper->getCriticalSection()->unlock();
+		
+		break;
+	}
+	case eGUIHelperRegisterGraphicsShape:
+	{
+		m_multiThreadedHelper->m_shapeIndex = m_multiThreadedHelper->m_childGuiHelper->registerGraphicsShape(
+				m_multiThreadedHelper->m_vertices,
+				m_multiThreadedHelper->m_numvertices,
+				m_multiThreadedHelper->m_indices,
+				m_multiThreadedHelper->m_numIndices,
+				m_multiThreadedHelper->m_primitiveType,
+				m_multiThreadedHelper->m_textureId);
+
+		m_multiThreadedHelper->getCriticalSection()->lock();
+		m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
+		m_multiThreadedHelper->getCriticalSection()->unlock();
+		break;
+	}
+	case eGUIHelperRegisterGraphicsInstance:
+	{
+		m_multiThreadedHelper->m_instanceId = m_multiThreadedHelper->m_childGuiHelper->registerGraphicsInstance(
+				m_multiThreadedHelper->m_shapeIndex,
+				m_multiThreadedHelper->m_position,
+				m_multiThreadedHelper->m_quaternion,
+				m_multiThreadedHelper->m_color,
+				m_multiThreadedHelper->m_scaling);
+
+		m_multiThreadedHelper->getCriticalSection()->lock();
+		m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
+		m_multiThreadedHelper->getCriticalSection()->unlock();
+		break;
+	}
+	case eGUIHelperRemoveAllGraphicsInstances:
+        {
+            m_multiThreadedHelper->m_childGuiHelper->removeAllGraphicsInstances();
+			int numRenderInstances = m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->getTotalNumInstances();
+			b3Assert(numRenderInstances==0);
+
+            m_multiThreadedHelper->getCriticalSection()->lock();
+            m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
+            m_multiThreadedHelper->getCriticalSection()->unlock();
+            break;
+        }
+	case eGUIHelperIdle:
+	default:
+		{
+			
+		}
+	}
+	
+
+
+
+	#if 0
 	if (m_options == PHYSICS_SERVER_USE_RTC_CLOCK)
 	{
 		btClock rtc;
@@ -201,15 +704,59 @@ void	PhysicsServerExample::stepSimulation(float deltaTime)
 		}
 	} else
 	{
-        for (int i=0;i<10;i++)
+        //for (int i=0;i<10;i++)
 			m_physicsServer.processClientCommands();
 	}
+	#endif
+
+	{
+		if (m_multiThreadedHelper->m_childGuiHelper->getRenderInterface())
+		{
+			m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms();
 		}
+	}
+}
+
 
 void PhysicsServerExample::renderScene()
 {
 	///debug rendering
+	//m_args[0].m_cs->lock();
+	
 	m_physicsServer.renderScene();
+
+	if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
+	{
+		//some little experiment to add text/HUD to a VR camera (HTC Vive/Oculus Rift)
+
+		static int frameCount=0;
+		frameCount++;
+		char bla[1024];
+		sprintf(bla,"VR sub-title text test, frame %d", frameCount/2);
+		float pos[4];
+		m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->getCameraTargetPosition(pos);
+		btTransform viewTr;
+		btScalar m[16];
+		float mf[16];
+		m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->getCameraViewMatrix(mf);
+		for (int i=0;i<16;i++)
+		{
+			m[i] = mf[i];
+		}
+		viewTr.setFromOpenGLMatrix(m);
+		btTransform viewTrInv = viewTr.inverse();
+		float upMag = -.6;
+		btVector3 side = viewTrInv.getBasis().getColumn(0);
+		btVector3 up = viewTrInv.getBasis().getColumn(1);
+		up+=0.35*side;
+		m_guiHelper->getAppInterface()->drawText3D(bla,pos[0]+upMag*up[0],pos[1]+upMag*up[1],pos[2]+upMag*up[2],1);
+		//btVector3 fwd = viewTrInv.getBasis().getColumn(2);
+		sprintf(bla,"VR line 2 sub-title text test, frame %d", frameCount/2);
+		upMag = -0.7;
+		m_guiHelper->getAppInterface()->drawText3D(bla,pos[0]+upMag*up[0],pos[1]+upMag*up[1],pos[2]+upMag*up[2],1);
+	}
+
+	//m_args[0].m_cs->unlock();
 }
 
 void    PhysicsServerExample::physicsDebugDraw(int debugDrawFlags)
@@ -290,7 +837,13 @@ extern int gSharedMemoryKey;
 
 class CommonExampleInterface*    PhysicsServerCreateFunc(struct CommonExampleOptions& options)
 {
-  	PhysicsServerExample* example = new PhysicsServerExample(options.m_guiHelper, options.m_sharedMem, options.m_option);
+
+	MultiThreadedOpenGLGuiHelper* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper(options.m_guiHelper->getAppInterface(),options.m_guiHelper);
+
+  	PhysicsServerExample* example = new PhysicsServerExample(guiHelperWrapper, 
+		options.m_sharedMem, 
+		options.m_option);
+
 	if (gSharedMemoryKey>=0)
 	{
 		example->setSharedMemoryKey(gSharedMemoryKey);
@@ -306,3 +859,5 @@ class CommonExampleInterface*    PhysicsServerCreateFunc(struct CommonExampleOpt
 	return example;
 
 }
+
+B3_STANDALONE_EXAMPLE(PhysicsServerCreateFunc)

examples/SharedMemory/PhysicsServerSharedMemory.cpp

@@ -90,6 +90,11 @@ PhysicsServerSharedMemory::~PhysicsServerSharedMemory()
 	delete m_data;
 }
 
+void PhysicsServerSharedMemory::resetDynamicsWorld()
+{
+	m_data->m_commandProcessor->deleteDynamicsWorld();
+	m_data->m_commandProcessor ->createEmptyDynamicsWorld();
+}
 void PhysicsServerSharedMemory::setSharedMemoryKey(int key)
 {
 	m_data->m_sharedMemoryKey = key;

examples/SharedMemory/PhysicsServerSharedMemory.h

@@ -43,6 +43,7 @@ public:
 	void enableCommandLogging(bool enable, const char* fileName);
 	void replayFromLogFile(const char* fileName);
 	
+	void resetDynamicsWorld();
 
 };
 

examples/SharedMemory/premake4.lua

@@ -15,7 +15,8 @@ links {
 
 language "C++"
 
-files {
+myfiles = 
+{
 	"PhysicsClient.cpp",
 	"PhysicsClientSharedMemory.cpp",
 	"PhysicsClientExample.cpp",
@@ -24,7 +25,6 @@ files {
 	"PhysicsServerSharedMemory.h",
 	"PhysicsServer.cpp",
 	"PhysicsServer.h",
-	"main.cpp",
 	"PhysicsClientC_API.cpp",
 	"SharedMemoryCommands.h",
 	"SharedMemoryPublic.h",
@@ -84,3 +84,203 @@ files {
 	"../ThirdPartyLibs/stb_image/stb_image.cpp",     
 }
 
+files {
+	myfiles,
+	"main.cpp",
+}
+
+
+files {
+		"../MultiThreading/b3ThreadSupportInterface.cpp",
+		"../MultiThreading/b3ThreadSupportInterface.h"
+	}
+	if os.is("Windows") then
+
+		files {
+                "../MultiThreading/b3Win32ThreadSupport.cpp",  
+                "../MultiThreading/b3Win32ThreadSupport.h" 
+		}
+		--links {"winmm"}
+		--defines {"__WINDOWS_MM__", "WIN32"}
+	end
+
+	if os.is("Linux") then 
+		files {
+                "../MultiThreading/b3PosixThreadSupport.cpp",  
+                "../MultiThreading/b3PosixThreadSupport.h"    
+        	}
+
+		links {"pthread"}
+	end
+
+	if os.is("MacOSX") then
+		files {
+                "../MultiThreading/b3PosixThreadSupport.cpp",
+                "../MultiThreading/b3PosixThreadSupport.h"    
+                }
+
+		links {"pthread"}
+		--links{"CoreAudio.framework", "coreMIDI.framework", "Cocoa.framework"}
+		--defines {"__MACOSX_CORE__"}
+	end
+
+
+project "App_SharedMemoryPhysics_GUI"
+
+if _OPTIONS["ios"] then
+        kind "WindowedApp"
+else
+        kind "ConsoleApp"
+end
+defines {"B3_USE_STANDALONE_EXAMPLE"}
+
+includedirs {"../../src"}
+
+links {
+        "BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common"
+}
+	initOpenGL()
+  initGlew()
+
+language "C++"
+
+files {
+        myfiles,
+        "../StandaloneMain/main_opengl_single_example.cpp",
+				"../ExampleBrowser/OpenGLGuiHelper.cpp",
+				"../ExampleBrowser/GL_ShapeDrawer.cpp",
+				"../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+}
+
+if os.is("Linux") then initX11() end
+
+if os.is("MacOSX") then
+        links{"Cocoa.framework"}
+end
+
+
+files {
+		"../MultiThreading/b3ThreadSupportInterface.cpp",
+		"../MultiThreading/b3ThreadSupportInterface.h"
+	}
+if os.is("Windows") then
+
+	files {
+              "../MultiThreading/b3Win32ThreadSupport.cpp",  
+              "../MultiThreading/b3Win32ThreadSupport.h" 
+	}
+	--links {"winmm"}
+	--defines {"__WINDOWS_MM__", "WIN32"}
+end
+
+if os.is("Linux") then 
+	files {
+              "../MultiThreading/b3PosixThreadSupport.cpp",  
+              "../MultiThreading/b3PosixThreadSupport.h"    
+      	}
+
+	links {"pthread"}
+end
+
+if os.is("MacOSX") then
+	files {
+              "../MultiThreading/b3PosixThreadSupport.cpp",
+              "../MultiThreading/b3PosixThreadSupport.h"    
+              }
+
+	links {"pthread"}
+	--links{"CoreAudio.framework", "coreMIDI.framework", "Cocoa.framework"}
+	--defines {"__MACOSX_CORE__"}
+end
+
+if os.is("Windows") then 
+	project "App_SharedMemoryPhysics_VR"
+	--for now, only enable VR under Windows, until compilation issues are resolved on Mac/Linux
+	defines {"B3_USE_STANDALONE_EXAMPLE","BT_ENABLE_VR"}
+	
+	if _OPTIONS["ios"] then
+		kind "WindowedApp"
+	else	
+		kind "ConsoleApp"
+	end
+	
+	includedirs {
+			".","../../src", "../ThirdPartyLibs",
+			"../ThirdPartyLibs/openvr/headers",
+			"../ThirdPartyLibs/openvr/samples/shared"
+		}
+						
+	links {
+		"Bullet3Common",	"BulletDynamics","BulletCollision", "LinearMath","OpenGL_Window","openvr_api"
+	}
+	
+	
+	language "C++"
+	
+	
+		initOpenGL()
+	  initGlew()
+	
+	
+	files
+	{
+		myfiles,
+		 "../StandaloneMain/hellovr_opengl_main.cpp",
+					"../ExampleBrowser/OpenGLGuiHelper.cpp",
+					"../ExampleBrowser/GL_ShapeDrawer.cpp",
+					"../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+					"../ThirdPartyLibs/openvr/samples/shared/lodepng.cpp",
+					"../ThirdPartyLibs/openvr/samples/shared/lodepng.h",
+					"../ThirdPartyLibs/openvr/samples/shared/Matrices.cpp",
+					"../ThirdPartyLibs/openvr/samples/shared/Matrices.h",
+					"../ThirdPartyLibs/openvr/samples/shared/pathtools.cpp",
+					"../ThirdPartyLibs/openvr/samples/shared/pathtools.h",
+					"../ThirdPartyLibs/openvr/samples/shared/Vectors.h",
+	}
+	if os.is("Windows") then 
+		libdirs {"../ThirdPartyLibs/openvr/lib/win32"}
+	end
+	
+	if os.is("Linux") then initX11() end
+	
+	if os.is("MacOSX") then
+	        links{"Cocoa.framework"}
+	end
+	
+	
+	files {
+			"../MultiThreading/b3ThreadSupportInterface.cpp",
+			"../MultiThreading/b3ThreadSupportInterface.h"
+		}
+		if os.is("Windows") then
+	
+			files {
+	                "../MultiThreading/b3Win32ThreadSupport.cpp",  
+	                "../MultiThreading/b3Win32ThreadSupport.h" 
+			}
+			--links {"winmm"}
+			--defines {"__WINDOWS_MM__", "WIN32"}
+		end
+	
+		if os.is("Linux") then 
+			files {
+	                "../MultiThreading/b3PosixThreadSupport.cpp",  
+	                "../MultiThreading/b3PosixThreadSupport.h"    
+	        	}
+	
+			links {"pthread"}
+		end
+	
+		if os.is("MacOSX") then
+			files {
+	                "../MultiThreading/b3PosixThreadSupport.cpp",
+	                "../MultiThreading/b3PosixThreadSupport.h"    
+	                }
+	
+			links {"pthread"}
+			--links{"CoreAudio.framework", "coreMIDI.framework", "Cocoa.framework"}
+			--defines {"__MACOSX_CORE__"}
+		end
+
+
+end

examples/StandaloneMain/main_opengl_single_example.cpp

@@ -17,14 +17,9 @@ subject to the following restrictions:
 
 #include "../CommonInterfaces/CommonExampleInterface.h"
 #include "../CommonInterfaces/CommonGUIHelperInterface.h"
-#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "../Utils/b3Clock.h"
-#include "BulletCollision/CollisionShapes/btCollisionShape.h"
-#include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h"
 
 
-#include "LinearMath/btTransform.h"
-#include "LinearMath/btHashMap.h"
-
 
 
 #include "../OpenGLWindow/SimpleOpenGL3App.h"
@@ -32,6 +27,7 @@ subject to the following restrictions:
 #include "../ExampleBrowser/OpenGLGuiHelper.h"
 
 CommonExampleInterface*    example;
+int gSharedMemoryKey=-1;
 
 b3MouseMoveCallback prevMouseMoveCallback = 0;
 static void OnMouseMove( float x, float y)
@@ -57,6 +53,20 @@ static void OnMouseDown(int button, int state, float x, float y) {
 	}
 }
 
+class LessDummyGuiHelper : public DummyGUIHelper
+{
+	CommonGraphicsApp* m_app;
+public:
+	virtual CommonGraphicsApp* getAppInterface()
+	{
+		return m_app;
+	}
+
+	LessDummyGuiHelper(CommonGraphicsApp* app)
+		:m_app(app)
+	{
+	}
+};
 int main(int argc, char* argv[])
 {
 	
@@ -69,6 +79,8 @@ int main(int argc, char* argv[])
 	app->m_window->setMouseMoveCallback((b3MouseMoveCallback)OnMouseMove);
 	
 	OpenGLGuiHelper gui(app,false);
+	//LessDummyGuiHelper gui(app);
+	//DummyGUIHelper gui;
 
 	CommonExampleOptions options(&gui);
 
@@ -76,12 +88,16 @@ int main(int argc, char* argv[])
 	example->initPhysics();
 	example->resetCamera();
 	
+	b3Clock clock;
+
 	do
 	{
 		app->m_instancingRenderer->init();
         app->m_instancingRenderer->updateCamera(app->getUpAxis());
 
-		example->stepSimulation(1./60.);
+		btScalar dtSec = btScalar(clock.getTimeInSeconds());
+		example->stepSimulation(dtSec);
+	  	clock.reset();
 
 		example->renderScene();
  	

examples/StandaloneMain/main_sw_tinyrenderer_single_example.cpp

@@ -105,9 +105,15 @@ public:
 		
 	}
 	
-   	virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices)
+
+	virtual int	registerTexture(const unsigned char* texels, int width, int height)
 	{
-   	    int shapeIndex = OpenGLGuiHelper::registerGraphicsShape(vertices,numvertices,indices,numIndices);
+		return -1;
+	}
+	
+	virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId)
+   	{
+   	    int shapeIndex = OpenGLGuiHelper::registerGraphicsShape(vertices,numvertices,indices,numIndices,primitiveType, textureId);
    	    if (shapeIndex>=0)
         {
             TinyRenderObjectData* swObj = new TinyRenderObjectData(m_rgbColorBuffer,m_depthBuffer);

examples/StandaloneMain/main_tinyrenderer_single_example.cpp

@@ -37,6 +37,12 @@ static btVector4 sMyColors[4] =
 	//btVector4(1,1,0,1),
 };
 
+struct TinyRendererTexture
+{
+	const unsigned char* m_texels;
+	int m_width;
+	int m_height;
+};
 
 struct TinyRendererGUIHelper : public GUIHelperInterface
 {
@@ -45,6 +51,7 @@ struct TinyRendererGUIHelper : public GUIHelperInterface
 
 	btHashMap<btHashInt,TinyRenderObjectData*> m_swRenderObjects;
 	btHashMap<btHashInt,int> m_swInstances;
+	btHashMap<btHashInt,TinyRendererTexture> m_textures;
 	
 	int m_swWidth;
 	int m_swHeight;
@@ -151,7 +158,8 @@ struct TinyRendererGUIHelper : public GUIHelperInterface
 
 	if (gfxVertices.size() && indices.size())
 	{
-		int shapeId = registerGraphicsShape(&gfxVertices[0].xyzw[0],gfxVertices.size(),&indices[0],indices.size());
+		int shapeId = registerGraphicsShape(&gfxVertices[0].xyzw[0],gfxVertices.size(),&indices[0],indices.size(),
+			1,-1);
 		collisionShape->setUserIndex(shapeId);
 	}
 	}
@@ -249,18 +257,41 @@ struct TinyRendererGUIHelper : public GUIHelperInterface
 
 	virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld){}
 
-	virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices) 
+	virtual int	registerTexture(const unsigned char* texels, int width, int height)
+	{
+		//do we need to make a copy?
+		int textureId = m_textures.size();
+		TinyRendererTexture t;
+		t.m_texels = texels;
+		t.m_width = width;
+		t.m_height = height;
+		this->m_textures.insert(textureId,t);
+		return textureId;
+	}
+
+	virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId)
 	{
 		int shapeIndex = m_swRenderObjects.size();
 
 		TinyRenderObjectData* swObj = new TinyRenderObjectData(m_rgbColorBuffer,m_depthBuffer);
         float rgbaColor[4] = {1,1,1,1};
+
+		//if (textureId>=0)
+		//{
+		//	swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor);
+		//} else
+		{
 			swObj->registerMeshShape(vertices,numvertices,indices,numIndices,rgbaColor);
+		}
 		//swObj->createCube(1,1,1);//MeshShape(vertices,numvertices,indices,numIndices);
         m_swRenderObjects.insert(shapeIndex,swObj);
 		return shapeIndex;
 	}
 
+	 virtual void removeAllGraphicsInstances()
+	 {
+	 }
+
 	virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) 
 	{
 		int colIndex = m_colObjUniqueIndex++;

examples/ThirdPartyLibs/openvr/LICENSE

@@ -0,0 +1,27 @@
+Copyright (c) 2015, Valve Corporation
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the following disclaimer in the documentation and/or
+other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its contributors
+may be used to endorse or promote products derived from this software without
+specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

examples/ThirdPartyLibs/openvr/README

@@ -0,0 +1,9 @@
+OpenVR is an API and runtime that allows access to VR hardware from multiple 
+vendors without requiring that applications have specific knowledge of the 
+hardware they are targeting. This repository is an SDK that contains the API 
+and samples. The runtime is under SteamVR in Tools on Steam. 
+
+Documentation for the API is available in the wiki: https://github.com/ValveSoftware/openvr/wiki/API-Documentation
+
+More information on OpenVR and SteamVR can be found on http://steamvr.com
+

examples/ThirdPartyLibs/openvr/samples/shared/Matrices.cpp

@@ -0,0 +1,581 @@
+///////////////////////////////////////////////////////////////////////////////
+// Matrice.cpp
+// ===========
+// NxN Matrix Math classes
+//
+// The elements of the matrix are stored as column major order.
+// | 0 2 |    | 0 3 6 |    |  0  4  8 12 |
+// | 1 3 |    | 1 4 7 |    |  1  5  9 13 |
+//            | 2 5 8 |    |  2  6 10 14 |
+//                         |  3  7 11 15 |
+//
+//  AUTHOR: Song Ho Ahn (song.ahn@gmail.com)
+// CREATED: 2005-06-24
+// UPDATED: 2014-09-21
+//
+// Copyright (C) 2005 Song Ho Ahn
+///////////////////////////////////////////////////////////////////////////////
+
+#include <cmath>
+#include <algorithm>
+#include "Matrices.h"
+
+const float DEG2RAD = 3.141593f / 180;
+const float EPSILON = 0.00001f;
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// transpose 2x2 matrix
+///////////////////////////////////////////////////////////////////////////////
+Matrix2& Matrix2::transpose()
+{
+    std::swap(m[1],  m[2]);
+    return *this;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// return the determinant of 2x2 matrix
+///////////////////////////////////////////////////////////////////////////////
+float Matrix2::getDeterminant()
+{
+    return m[0] * m[3] - m[1] * m[2];
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// inverse of 2x2 matrix
+// If cannot find inverse, set identity matrix
+///////////////////////////////////////////////////////////////////////////////
+Matrix2& Matrix2::invert()
+{
+    float determinant = getDeterminant();
+    if(fabs(determinant) <= EPSILON)
+    {
+        return identity();
+    }
+
+    float tmp = m[0];   // copy the first element
+    float invDeterminant = 1.0f / determinant;
+    m[0] =  invDeterminant * m[3];
+    m[1] = -invDeterminant * m[1];
+    m[2] = -invDeterminant * m[2];
+    m[3] =  invDeterminant * tmp;
+
+    return *this;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// transpose 3x3 matrix
+///////////////////////////////////////////////////////////////////////////////
+Matrix3& Matrix3::transpose()
+{
+    std::swap(m[1],  m[3]);
+    std::swap(m[2],  m[6]);
+    std::swap(m[5],  m[7]);
+
+    return *this;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// return determinant of 3x3 matrix
+///////////////////////////////////////////////////////////////////////////////
+float Matrix3::getDeterminant()
+{
+    return m[0] * (m[4] * m[8] - m[5] * m[7]) -
+           m[1] * (m[3] * m[8] - m[5] * m[6]) +
+           m[2] * (m[3] * m[7] - m[4] * m[6]);
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// inverse 3x3 matrix
+// If cannot find inverse, set identity matrix
+///////////////////////////////////////////////////////////////////////////////
+Matrix3& Matrix3::invert()
+{
+    float determinant, invDeterminant;
+    float tmp[9];
+
+    tmp[0] = m[4] * m[8] - m[5] * m[7];
+    tmp[1] = m[2] * m[7] - m[1] * m[8];
+    tmp[2] = m[1] * m[5] - m[2] * m[4];
+    tmp[3] = m[5] * m[6] - m[3] * m[8];
+    tmp[4] = m[0] * m[8] - m[2] * m[6];
+    tmp[5] = m[2] * m[3] - m[0] * m[5];
+    tmp[6] = m[3] * m[7] - m[4] * m[6];
+    tmp[7] = m[1] * m[6] - m[0] * m[7];
+    tmp[8] = m[0] * m[4] - m[1] * m[3];
+
+    // check determinant if it is 0
+    determinant = m[0] * tmp[0] + m[1] * tmp[3] + m[2] * tmp[6];
+    if(fabs(determinant) <= EPSILON)
+    {
+        return identity(); // cannot inverse, make it idenety matrix
+    }
+
+    // divide by the determinant
+    invDeterminant = 1.0f / determinant;
+    m[0] = invDeterminant * tmp[0];
+    m[1] = invDeterminant * tmp[1];
+    m[2] = invDeterminant * tmp[2];
+    m[3] = invDeterminant * tmp[3];
+    m[4] = invDeterminant * tmp[4];
+    m[5] = invDeterminant * tmp[5];
+    m[6] = invDeterminant * tmp[6];
+    m[7] = invDeterminant * tmp[7];
+    m[8] = invDeterminant * tmp[8];
+
+    return *this;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// transpose 4x4 matrix
+///////////////////////////////////////////////////////////////////////////////
+Matrix4& Matrix4::transpose()
+{
+    std::swap(m[1],  m[4]);
+    std::swap(m[2],  m[8]);
+    std::swap(m[3],  m[12]);
+    std::swap(m[6],  m[9]);
+    std::swap(m[7],  m[13]);
+    std::swap(m[11], m[14]);
+
+    return *this;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// inverse 4x4 matrix
+///////////////////////////////////////////////////////////////////////////////
+Matrix4& Matrix4::invert()
+{
+    // If the 4th row is [0,0,0,1] then it is affine matrix and
+    // it has no projective transformation.
+    if(m[3] == 0 && m[7] == 0 && m[11] == 0 && m[15] == 1)
+        this->invertAffine();
+    else
+    {
+        this->invertGeneral();
+        /*@@ invertProjective() is not optimized (slower than generic one)
+        if(fabs(m[0]*m[5] - m[1]*m[4]) > EPSILON)
+            this->invertProjective();   // inverse using matrix partition
+        else
+            this->invertGeneral();      // generalized inverse
+        */
+    }
+
+    return *this;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// compute the inverse of 4x4 Euclidean transformation matrix
+//
+// Euclidean transformation is translation, rotation, and reflection.
+// With Euclidean transform, only the position and orientation of the object
+// will be changed. Euclidean transform does not change the shape of an object
+// (no scaling). Length and angle are reserved.
+//
+// Use inverseAffine() if the matrix has scale and shear transformation.
+//
+// M = [ R | T ]
+//     [ --+-- ]    (R denotes 3x3 rotation/reflection matrix)
+//     [ 0 | 1 ]    (T denotes 1x3 translation matrix)
+//
+// y = M*x  ->  y = R*x + T  ->  x = R^-1*(y - T)  ->  x = R^T*y - R^T*T
+// (R is orthogonal,  R^-1 = R^T)
+//
+//  [ R | T ]-1    [ R^T | -R^T * T ]    (R denotes 3x3 rotation matrix)
+//  [ --+-- ]   =  [ ----+--------- ]    (T denotes 1x3 translation)
+//  [ 0 | 1 ]      [  0  |     1    ]    (R^T denotes R-transpose)
+///////////////////////////////////////////////////////////////////////////////
+Matrix4& Matrix4::invertEuclidean()
+{
+    // transpose 3x3 rotation matrix part
+    // | R^T | 0 |
+    // | ----+-- |
+    // |  0  | 1 |
+    float tmp;
+    tmp = m[1];  m[1] = m[4];  m[4] = tmp;
+    tmp = m[2];  m[2] = m[8];  m[8] = tmp;
+    tmp = m[6];  m[6] = m[9];  m[9] = tmp;
+
+    // compute translation part -R^T * T
+    // | 0 | -R^T x |
+    // | --+------- |
+    // | 0 |   0    |
+    float x = m[12];
+    float y = m[13];
+    float z = m[14];
+    m[12] = -(m[0] * x + m[4] * y + m[8] * z);
+    m[13] = -(m[1] * x + m[5] * y + m[9] * z);
+    m[14] = -(m[2] * x + m[6] * y + m[10]* z);
+
+    // last row should be unchanged (0,0,0,1)
+
+    return *this;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// compute the inverse of a 4x4 affine transformation matrix
+//
+// Affine transformations are generalizations of Euclidean transformations.
+// Affine transformation includes translation, rotation, reflection, scaling,
+// and shearing. Length and angle are NOT preserved.
+// M = [ R | T ]
+//     [ --+-- ]    (R denotes 3x3 rotation/scale/shear matrix)
+//     [ 0 | 1 ]    (T denotes 1x3 translation matrix)
+//
+// y = M*x  ->  y = R*x + T  ->  x = R^-1*(y - T)  ->  x = R^-1*y - R^-1*T
+//
+//  [ R | T ]-1   [ R^-1 | -R^-1 * T ]
+//  [ --+-- ]   = [ -----+---------- ]
+//  [ 0 | 1 ]     [  0   +     1     ]
+///////////////////////////////////////////////////////////////////////////////
+Matrix4& Matrix4::invertAffine()
+{
+    // R^-1
+    Matrix3 r(m[0],m[1],m[2], m[4],m[5],m[6], m[8],m[9],m[10]);
+    r.invert();
+    m[0] = r[0];  m[1] = r[1];  m[2] = r[2];
+    m[4] = r[3];  m[5] = r[4];  m[6] = r[5];
+    m[8] = r[6];  m[9] = r[7];  m[10]= r[8];
+
+    // -R^-1 * T
+    float x = m[12];
+    float y = m[13];
+    float z = m[14];
+    m[12] = -(r[0] * x + r[3] * y + r[6] * z);
+    m[13] = -(r[1] * x + r[4] * y + r[7] * z);
+    m[14] = -(r[2] * x + r[5] * y + r[8] * z);
+
+    // last row should be unchanged (0,0,0,1)
+    //m[3] = m[7] = m[11] = 0.0f;
+    //m[15] = 1.0f;
+
+    return * this;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// inverse matrix using matrix partitioning (blockwise inverse)
+// It devides a 4x4 matrix into 4 of 2x2 matrices. It works in case of where
+// det(A) != 0. If not, use the generic inverse method
+// inverse formula.
+// M = [ A | B ]    A, B, C, D are 2x2 matrix blocks
+//     [ --+-- ]    det(M) = |A| * |D - ((C * A^-1) * B)|
+//     [ C | D ]
+//
+// M^-1 = [ A' | B' ]   A' = A^-1 - (A^-1 * B) * C'
+//        [ ---+--- ]   B' = (A^-1 * B) * -D'
+//        [ C' | D' ]   C' = -D' * (C * A^-1)
+//                      D' = (D - ((C * A^-1) * B))^-1
+//
+// NOTE: I wrap with () if it it used more than once.
+//       The matrix is invertable even if det(A)=0, so must check det(A) before
+//       calling this function, and use invertGeneric() instead.
+///////////////////////////////////////////////////////////////////////////////
+Matrix4& Matrix4::invertProjective()
+{
+    // partition
+    Matrix2 a(m[0], m[1], m[4], m[5]);
+    Matrix2 b(m[8], m[9], m[12], m[13]);
+    Matrix2 c(m[2], m[3], m[6], m[7]);
+    Matrix2 d(m[10], m[11], m[14], m[15]);
+
+    // pre-compute repeated parts
+    a.invert();             // A^-1
+    Matrix2 ab = a * b;     // A^-1 * B
+    Matrix2 ca = c * a;     // C * A^-1
+    Matrix2 cab = ca * b;   // C * A^-1 * B
+    Matrix2 dcab = d - cab; // D - C * A^-1 * B
+
+    // check determinant if |D - C * A^-1 * B| = 0
+    //NOTE: this function assumes det(A) is already checked. if |A|=0 then,
+    //      cannot use this function.
+    float determinant = dcab[0] * dcab[3] - dcab[1] * dcab[2];
+    if(fabs(determinant) <= EPSILON)
+    {
+        return identity();
+    }
+
+    // compute D' and -D'
+    Matrix2 d1 = dcab;      //  (D - C * A^-1 * B)
+    d1.invert();            //  (D - C * A^-1 * B)^-1
+    Matrix2 d2 = -d1;       // -(D - C * A^-1 * B)^-1
+
+    // compute C'
+    Matrix2 c1 = d2 * ca;   // -D' * (C * A^-1)
+
+    // compute B'
+    Matrix2 b1 = ab * d2;   // (A^-1 * B) * -D'
+
+    // compute A'
+    Matrix2 a1 = a - (ab * c1); // A^-1 - (A^-1 * B) * C'
+
+    // assemble inverse matrix
+    m[0] = a1[0];  m[4] = a1[2]; /*|*/ m[8] = b1[0];  m[12]= b1[2];
+    m[1] = a1[1];  m[5] = a1[3]; /*|*/ m[9] = b1[1];  m[13]= b1[3];
+    /*-----------------------------+-----------------------------*/
+    m[2] = c1[0];  m[6] = c1[2]; /*|*/ m[10]= d1[0];  m[14]= d1[2];
+    m[3] = c1[1];  m[7] = c1[3]; /*|*/ m[11]= d1[1];  m[15]= d1[3];
+
+    return *this;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// compute the inverse of a general 4x4 matrix using Cramer's Rule
+// If cannot find inverse, return indentity matrix
+// M^-1 = adj(M) / det(M)
+///////////////////////////////////////////////////////////////////////////////
+Matrix4& Matrix4::invertGeneral()
+{
+    // get cofactors of minor matrices
+    float cofactor0 = getCofactor(m[5],m[6],m[7], m[9],m[10],m[11], m[13],m[14],m[15]);
+    float cofactor1 = getCofactor(m[4],m[6],m[7], m[8],m[10],m[11], m[12],m[14],m[15]);
+    float cofactor2 = getCofactor(m[4],m[5],m[7], m[8],m[9], m[11], m[12],m[13],m[15]);
+    float cofactor3 = getCofactor(m[4],m[5],m[6], m[8],m[9], m[10], m[12],m[13],m[14]);
+
+    // get determinant
+    float determinant = m[0] * cofactor0 - m[1] * cofactor1 + m[2] * cofactor2 - m[3] * cofactor3;
+    if(fabs(determinant) <= EPSILON)
+    {
+        return identity();
+    }
+
+    // get rest of cofactors for adj(M)
+    float cofactor4 = getCofactor(m[1],m[2],m[3], m[9],m[10],m[11], m[13],m[14],m[15]);
+    float cofactor5 = getCofactor(m[0],m[2],m[3], m[8],m[10],m[11], m[12],m[14],m[15]);
+    float cofactor6 = getCofactor(m[0],m[1],m[3], m[8],m[9], m[11], m[12],m[13],m[15]);
+    float cofactor7 = getCofactor(m[0],m[1],m[2], m[8],m[9], m[10], m[12],m[13],m[14]);
+
+    float cofactor8 = getCofactor(m[1],m[2],m[3], m[5],m[6], m[7],  m[13],m[14],m[15]);
+    float cofactor9 = getCofactor(m[0],m[2],m[3], m[4],m[6], m[7],  m[12],m[14],m[15]);
+    float cofactor10= getCofactor(m[0],m[1],m[3], m[4],m[5], m[7],  m[12],m[13],m[15]);
+    float cofactor11= getCofactor(m[0],m[1],m[2], m[4],m[5], m[6],  m[12],m[13],m[14]);
+
+    float cofactor12= getCofactor(m[1],m[2],m[3], m[5],m[6], m[7],  m[9], m[10],m[11]);
+    float cofactor13= getCofactor(m[0],m[2],m[3], m[4],m[6], m[7],  m[8], m[10],m[11]);
+    float cofactor14= getCofactor(m[0],m[1],m[3], m[4],m[5], m[7],  m[8], m[9], m[11]);
+    float cofactor15= getCofactor(m[0],m[1],m[2], m[4],m[5], m[6],  m[8], m[9], m[10]);
+
+    // build inverse matrix = adj(M) / det(M)
+    // adjugate of M is the transpose of the cofactor matrix of M
+    float invDeterminant = 1.0f / determinant;
+    m[0] =  invDeterminant * cofactor0;
+    m[1] = -invDeterminant * cofactor4;
+    m[2] =  invDeterminant * cofactor8;
+    m[3] = -invDeterminant * cofactor12;
+
+    m[4] = -invDeterminant * cofactor1;
+    m[5] =  invDeterminant * cofactor5;
+    m[6] = -invDeterminant * cofactor9;
+    m[7] =  invDeterminant * cofactor13;
+
+    m[8] =  invDeterminant * cofactor2;
+    m[9] = -invDeterminant * cofactor6;
+    m[10]=  invDeterminant * cofactor10;
+    m[11]= -invDeterminant * cofactor14;
+
+    m[12]= -invDeterminant * cofactor3;
+    m[13]=  invDeterminant * cofactor7;
+    m[14]= -invDeterminant * cofactor11;
+    m[15]=  invDeterminant * cofactor15;
+
+    return *this;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// return determinant of 4x4 matrix
+///////////////////////////////////////////////////////////////////////////////
+float Matrix4::getDeterminant()
+{
+    return m[0] * getCofactor(m[5],m[6],m[7], m[9],m[10],m[11], m[13],m[14],m[15]) -
+           m[1] * getCofactor(m[4],m[6],m[7], m[8],m[10],m[11], m[12],m[14],m[15]) +
+           m[2] * getCofactor(m[4],m[5],m[7], m[8],m[9], m[11], m[12],m[13],m[15]) -
+           m[3] * getCofactor(m[4],m[5],m[6], m[8],m[9], m[10], m[12],m[13],m[14]);
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// compute cofactor of 3x3 minor matrix without sign
+// input params are 9 elements of the minor matrix
+// NOTE: The caller must know its sign.
+///////////////////////////////////////////////////////////////////////////////
+float Matrix4::getCofactor(float m0, float m1, float m2,
+                           float m3, float m4, float m5,
+                           float m6, float m7, float m8)
+{
+    return m0 * (m4 * m8 - m5 * m7) -
+           m1 * (m3 * m8 - m5 * m6) +
+           m2 * (m3 * m7 - m4 * m6);
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// translate this matrix by (x, y, z)
+///////////////////////////////////////////////////////////////////////////////
+Matrix4& Matrix4::translate(const Vector3& v)
+{
+    return translate(v.x, v.y, v.z);
+}
+
+Matrix4& Matrix4::translate(float x, float y, float z)
+{
+    m[0] += m[3] * x;   m[4] += m[7] * x;   m[8] += m[11]* x;   m[12]+= m[15]* x;
+    m[1] += m[3] * y;   m[5] += m[7] * y;   m[9] += m[11]* y;   m[13]+= m[15]* y;
+    m[2] += m[3] * z;   m[6] += m[7] * z;   m[10]+= m[11]* z;   m[14]+= m[15]* z;
+
+    return *this;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// uniform scale
+///////////////////////////////////////////////////////////////////////////////
+Matrix4& Matrix4::scale(float s)
+{
+    return scale(s, s, s);
+}
+
+Matrix4& Matrix4::scale(float x, float y, float z)
+{
+    m[0] *= x;   m[4] *= x;   m[8] *= x;   m[12] *= x;
+    m[1] *= y;   m[5] *= y;   m[9] *= y;   m[13] *= y;
+    m[2] *= z;   m[6] *= z;   m[10]*= z;   m[14] *= z;
+    return *this;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// build a rotation matrix with given angle(degree) and rotation axis, then
+// multiply it with this object
+///////////////////////////////////////////////////////////////////////////////
+Matrix4& Matrix4::rotate(float angle, const Vector3& axis)
+{
+    return rotate(angle, axis.x, axis.y, axis.z);
+}
+
+Matrix4& Matrix4::rotate(float angle, float x, float y, float z)
+{
+    float c = cosf(angle * DEG2RAD);    // cosine
+    float s = sinf(angle * DEG2RAD);    // sine
+    float c1 = 1.0f - c;                // 1 - c
+    float m0 = m[0],  m4 = m[4],  m8 = m[8],  m12= m[12],
+          m1 = m[1],  m5 = m[5],  m9 = m[9],  m13= m[13],
+          m2 = m[2],  m6 = m[6],  m10= m[10], m14= m[14];
+
+    // build rotation matrix
+    float r0 = x * x * c1 + c;
+    float r1 = x * y * c1 + z * s;
+    float r2 = x * z * c1 - y * s;
+    float r4 = x * y * c1 - z * s;
+    float r5 = y * y * c1 + c;
+    float r6 = y * z * c1 + x * s;
+    float r8 = x * z * c1 + y * s;
+    float r9 = y * z * c1 - x * s;
+    float r10= z * z * c1 + c;
+
+    // multiply rotation matrix
+    m[0] = r0 * m0 + r4 * m1 + r8 * m2;
+    m[1] = r1 * m0 + r5 * m1 + r9 * m2;
+    m[2] = r2 * m0 + r6 * m1 + r10* m2;
+    m[4] = r0 * m4 + r4 * m5 + r8 * m6;
+    m[5] = r1 * m4 + r5 * m5 + r9 * m6;
+    m[6] = r2 * m4 + r6 * m5 + r10* m6;
+    m[8] = r0 * m8 + r4 * m9 + r8 * m10;
+    m[9] = r1 * m8 + r5 * m9 + r9 * m10;
+    m[10]= r2 * m8 + r6 * m9 + r10* m10;
+    m[12]= r0 * m12+ r4 * m13+ r8 * m14;
+    m[13]= r1 * m12+ r5 * m13+ r9 * m14;
+    m[14]= r2 * m12+ r6 * m13+ r10* m14;
+
+    return *this;
+}
+
+Matrix4& Matrix4::rotateX(float angle)
+{
+    float c = cosf(angle * DEG2RAD);
+    float s = sinf(angle * DEG2RAD);
+    float m1 = m[1],  m2 = m[2],
+          m5 = m[5],  m6 = m[6],
+          m9 = m[9],  m10= m[10],
+          m13= m[13], m14= m[14];
+
+    m[1] = m1 * c + m2 *-s;
+    m[2] = m1 * s + m2 * c;
+    m[5] = m5 * c + m6 *-s;
+    m[6] = m5 * s + m6 * c;
+    m[9] = m9 * c + m10*-s;
+    m[10]= m9 * s + m10* c;
+    m[13]= m13* c + m14*-s;
+    m[14]= m13* s + m14* c;
+
+    return *this;
+}
+
+Matrix4& Matrix4::rotateY(float angle)
+{
+    float c = cosf(angle * DEG2RAD);
+    float s = sinf(angle * DEG2RAD);
+    float m0 = m[0],  m2 = m[2],
+          m4 = m[4],  m6 = m[6],
+          m8 = m[8],  m10= m[10],
+          m12= m[12], m14= m[14];
+
+    m[0] = m0 * c + m2 * s;
+    m[2] = m0 *-s + m2 * c;
+    m[4] = m4 * c + m6 * s;
+    m[6] = m4 *-s + m6 * c;
+    m[8] = m8 * c + m10* s;
+    m[10]= m8 *-s + m10* c;
+    m[12]= m12* c + m14* s;
+    m[14]= m12*-s + m14* c;
+
+    return *this;
+}
+
+Matrix4& Matrix4::rotateZ(float angle)
+{
+    float c = cosf(angle * DEG2RAD);
+    float s = sinf(angle * DEG2RAD);
+    float m0 = m[0],  m1 = m[1],
+          m4 = m[4],  m5 = m[5],
+          m8 = m[8],  m9 = m[9],
+          m12= m[12], m13= m[13];
+
+    m[0] = m0 * c + m1 *-s;
+    m[1] = m0 * s + m1 * c;
+    m[4] = m4 * c + m5 *-s;
+    m[5] = m4 * s + m5 * c;
+    m[8] = m8 * c + m9 *-s;
+    m[9] = m8 * s + m9 * c;
+    m[12]= m12* c + m13*-s;
+    m[13]= m12* s + m13* c;
+
+    return *this;
+}

examples/ThirdPartyLibs/openvr/samples/shared/Matrices.h

@@ -0,0 +1,909 @@
+///////////////////////////////////////////////////////////////////////////////
+// Matrice.h
+// =========
+// NxN Matrix Math classes
+//
+// The elements of the matrix are stored as column major order.
+// | 0 2 |    | 0 3 6 |    |  0  4  8 12 |
+// | 1 3 |    | 1 4 7 |    |  1  5  9 13 |
+//            | 2 5 8 |    |  2  6 10 14 |
+//                         |  3  7 11 15 |
+//
+//  AUTHOR: Song Ho Ahn (song.ahn@gmail.com)
+// CREATED: 2005-06-24
+// UPDATED: 2013-09-30
+//
+// Copyright (C) 2005 Song Ho Ahn
+///////////////////////////////////////////////////////////////////////////////
+
+#ifndef MATH_MATRICES_H
+#define MATH_MATRICES_H
+
+#include <iostream>
+#include <iomanip>
+#include "Vectors.h"
+
+///////////////////////////////////////////////////////////////////////////
+// 2x2 matrix
+///////////////////////////////////////////////////////////////////////////
+class Matrix2
+{
+public:
+    // constructors
+    Matrix2();  // init with identity
+    Matrix2(const float src[4]);
+    Matrix2(float m0, float m1, float m2, float m3);
+
+    void        set(const float src[4]);
+    void        set(float m0, float m1, float m2, float m3);
+    void        setRow(int index, const float row[2]);
+    void        setRow(int index, const Vector2& v);
+    void        setColumn(int index, const float col[2]);
+    void        setColumn(int index, const Vector2& v);
+
+    const float* get() const;
+    float       getDeterminant();
+
+    Matrix2&    identity();
+    Matrix2&    transpose();                            // transpose itself and return reference
+    Matrix2&    invert();
+
+    // operators
+    Matrix2     operator+(const Matrix2& rhs) const;    // add rhs
+    Matrix2     operator-(const Matrix2& rhs) const;    // subtract rhs
+    Matrix2&    operator+=(const Matrix2& rhs);         // add rhs and update this object
+    Matrix2&    operator-=(const Matrix2& rhs);         // subtract rhs and update this object
+    Vector2     operator*(const Vector2& rhs) const;    // multiplication: v' = M * v
+    Matrix2     operator*(const Matrix2& rhs) const;    // multiplication: M3 = M1 * M2
+    Matrix2&    operator*=(const Matrix2& rhs);         // multiplication: M1' = M1 * M2
+    bool        operator==(const Matrix2& rhs) const;   // exact compare, no epsilon
+    bool        operator!=(const Matrix2& rhs) const;   // exact compare, no epsilon
+    float       operator[](int index) const;            // subscript operator v[0], v[1]
+    float&      operator[](int index);                  // subscript operator v[0], v[1]
+
+    friend Matrix2 operator-(const Matrix2& m);                     // unary operator (-)
+    friend Matrix2 operator*(float scalar, const Matrix2& m);       // pre-multiplication
+    friend Vector2 operator*(const Vector2& vec, const Matrix2& m); // pre-multiplication
+    friend std::ostream& operator<<(std::ostream& os, const Matrix2& m);
+
+protected:
+
+private:
+    float m[4];
+
+};
+
+
+
+///////////////////////////////////////////////////////////////////////////
+// 3x3 matrix
+///////////////////////////////////////////////////////////////////////////
+class Matrix3
+{
+public:
+    // constructors
+    Matrix3();  // init with identity
+    Matrix3(const float src[9]);
+    Matrix3(float m0, float m1, float m2,           // 1st column
+            float m3, float m4, float m5,           // 2nd column
+            float m6, float m7, float m8);          // 3rd column
+
+    void        set(const float src[9]);
+    void        set(float m0, float m1, float m2,   // 1st column
+                    float m3, float m4, float m5,   // 2nd column
+                    float m6, float m7, float m8);  // 3rd column
+    void        setRow(int index, const float row[3]);
+    void        setRow(int index, const Vector3& v);
+    void        setColumn(int index, const float col[3]);
+    void        setColumn(int index, const Vector3& v);
+
+    const float* get() const;
+    float       getDeterminant();
+
+    Matrix3&    identity();
+    Matrix3&    transpose();                            // transpose itself and return reference
+    Matrix3&    invert();
+
+    // operators
+    Matrix3     operator+(const Matrix3& rhs) const;    // add rhs
+    Matrix3     operator-(const Matrix3& rhs) const;    // subtract rhs
+    Matrix3&    operator+=(const Matrix3& rhs);         // add rhs and update this object
+    Matrix3&    operator-=(const Matrix3& rhs);         // subtract rhs and update this object
+    Vector3     operator*(const Vector3& rhs) const;    // multiplication: v' = M * v
+    Matrix3     operator*(const Matrix3& rhs) const;    // multiplication: M3 = M1 * M2
+    Matrix3&    operator*=(const Matrix3& rhs);         // multiplication: M1' = M1 * M2
+    bool        operator==(const Matrix3& rhs) const;   // exact compare, no epsilon
+    bool        operator!=(const Matrix3& rhs) const;   // exact compare, no epsilon
+    float       operator[](int index) const;            // subscript operator v[0], v[1]
+    float&      operator[](int index);                  // subscript operator v[0], v[1]
+
+    friend Matrix3 operator-(const Matrix3& m);                     // unary operator (-)
+    friend Matrix3 operator*(float scalar, const Matrix3& m);       // pre-multiplication
+    friend Vector3 operator*(const Vector3& vec, const Matrix3& m); // pre-multiplication
+    friend std::ostream& operator<<(std::ostream& os, const Matrix3& m);
+
+protected:
+
+private:
+    float m[9];
+
+};
+
+
+
+///////////////////////////////////////////////////////////////////////////
+// 4x4 matrix
+///////////////////////////////////////////////////////////////////////////
+class Matrix4
+{
+public:
+    // constructors
+    Matrix4();  // init with identity
+    Matrix4(const float src[16]);
+    Matrix4(float m00, float m01, float m02, float m03, // 1st column
+            float m04, float m05, float m06, float m07, // 2nd column
+            float m08, float m09, float m10, float m11, // 3rd column
+            float m12, float m13, float m14, float m15);// 4th column
+
+    void        set(const float src[16]);
+    void        set(float m00, float m01, float m02, float m03, // 1st column
+                    float m04, float m05, float m06, float m07, // 2nd column
+                    float m08, float m09, float m10, float m11, // 3rd column
+                    float m12, float m13, float m14, float m15);// 4th column
+    void        setRow(int index, const float row[4]);
+    void        setRow(int index, const Vector4& v);
+    void        setRow(int index, const Vector3& v);
+    void        setColumn(int index, const float col[4]);
+    void        setColumn(int index, const Vector4& v);
+    void        setColumn(int index, const Vector3& v);
+
+    const float* get() const;
+    const float* getTranspose();                        // return transposed matrix
+    float        getDeterminant();
+
+    Matrix4&    identity();
+    Matrix4&    transpose();                            // transpose itself and return reference
+    Matrix4&    invert();                               // check best inverse method before inverse
+    Matrix4&    invertEuclidean();                      // inverse of Euclidean transform matrix
+    Matrix4&    invertAffine();                         // inverse of affine transform matrix
+    Matrix4&    invertProjective();                     // inverse of projective matrix using partitioning
+    Matrix4&    invertGeneral();                        // inverse of generic matrix
+
+    // transform matrix
+    Matrix4&    translate(float x, float y, float z);   // translation by (x,y,z)
+    Matrix4&    translate(const Vector3& v);            //
+    Matrix4&    rotate(float angle, const Vector3& axis); // rotate angle(degree) along the given axix
+    Matrix4&    rotate(float angle, float x, float y, float z);
+    Matrix4&    rotateX(float angle);                   // rotate on X-axis with degree
+    Matrix4&    rotateY(float angle);                   // rotate on Y-axis with degree
+    Matrix4&    rotateZ(float angle);                   // rotate on Z-axis with degree
+    Matrix4&    scale(float scale);                     // uniform scale
+    Matrix4&    scale(float sx, float sy, float sz);    // scale by (sx, sy, sz) on each axis
+
+    // operators
+    Matrix4     operator+(const Matrix4& rhs) const;    // add rhs
+    Matrix4     operator-(const Matrix4& rhs) const;    // subtract rhs
+    Matrix4&    operator+=(const Matrix4& rhs);         // add rhs and update this object
+    Matrix4&    operator-=(const Matrix4& rhs);         // subtract rhs and update this object
+    Vector4     operator*(const Vector4& rhs) const;    // multiplication: v' = M * v
+    Vector3     operator*(const Vector3& rhs) const;    // multiplication: v' = M * v
+    Matrix4     operator*(const Matrix4& rhs) const;    // multiplication: M3 = M1 * M2
+    Matrix4&    operator*=(const Matrix4& rhs);         // multiplication: M1' = M1 * M2
+    bool        operator==(const Matrix4& rhs) const;   // exact compare, no epsilon
+    bool        operator!=(const Matrix4& rhs) const;   // exact compare, no epsilon
+    float       operator[](int index) const;            // subscript operator v[0], v[1]
+    float&      operator[](int index);                  // subscript operator v[0], v[1]
+
+    friend Matrix4 operator-(const Matrix4& m);                     // unary operator (-)
+    friend Matrix4 operator*(float scalar, const Matrix4& m);       // pre-multiplication
+    friend Vector3 operator*(const Vector3& vec, const Matrix4& m); // pre-multiplication
+    friend Vector4 operator*(const Vector4& vec, const Matrix4& m); // pre-multiplication
+    friend std::ostream& operator<<(std::ostream& os, const Matrix4& m);
+
+protected:
+
+private:
+    float       getCofactor(float m0, float m1, float m2,
+                            float m3, float m4, float m5,
+                            float m6, float m7, float m8);
+
+    float m[16];
+    float tm[16];                                       // transpose m
+
+};
+
+
+
+///////////////////////////////////////////////////////////////////////////
+// inline functions for Matrix2
+///////////////////////////////////////////////////////////////////////////
+inline Matrix2::Matrix2()
+{
+    // initially identity matrix
+    identity();
+}
+
+
+
+inline Matrix2::Matrix2(const float src[4])
+{
+    set(src);
+}
+
+
+
+inline Matrix2::Matrix2(float m0, float m1, float m2, float m3)
+{
+    set(m0, m1, m2, m3);
+}
+
+
+
+inline void Matrix2::set(const float src[4])
+{
+    m[0] = src[0];  m[1] = src[1];  m[2] = src[2];  m[3] = src[3];
+}
+
+
+
+inline void Matrix2::set(float m0, float m1, float m2, float m3)
+{
+    m[0]= m0;  m[1] = m1;  m[2] = m2;  m[3]= m3;
+}
+
+
+
+inline void Matrix2::setRow(int index, const float row[2])
+{
+    m[index] = row[0];  m[index + 2] = row[1];
+}
+
+
+
+inline void Matrix2::setRow(int index, const Vector2& v)
+{
+    m[index] = v.x;  m[index + 2] = v.y;
+}
+
+
+
+inline void Matrix2::setColumn(int index, const float col[2])
+{
+    m[index*2] = col[0];  m[index*2 + 1] = col[1];
+}
+
+
+
+inline void Matrix2::setColumn(int index, const Vector2& v)
+{
+    m[index*2] = v.x;  m[index*2 + 1] = v.y;
+}
+
+
+
+inline const float* Matrix2::get() const
+{
+    return m;
+}
+
+
+
+inline Matrix2& Matrix2::identity()
+{
+    m[0] = m[3] = 1.0f;
+    m[1] = m[2] = 0.0f;
+    return *this;
+}
+
+
+
+inline Matrix2 Matrix2::operator+(const Matrix2& rhs) const
+{
+    return Matrix2(m[0]+rhs[0], m[1]+rhs[1], m[2]+rhs[2], m[3]+rhs[3]);
+}
+
+
+
+inline Matrix2 Matrix2::operator-(const Matrix2& rhs) const
+{
+    return Matrix2(m[0]-rhs[0], m[1]-rhs[1], m[2]-rhs[2], m[3]-rhs[3]);
+}
+
+
+
+inline Matrix2& Matrix2::operator+=(const Matrix2& rhs)
+{
+    m[0] += rhs[0];  m[1] += rhs[1];  m[2] += rhs[2];  m[3] += rhs[3];
+    return *this;
+}
+
+
+
+inline Matrix2& Matrix2::operator-=(const Matrix2& rhs)
+{
+    m[0] -= rhs[0];  m[1] -= rhs[1];  m[2] -= rhs[2];  m[3] -= rhs[3];
+    return *this;
+}
+
+
+
+inline Vector2 Matrix2::operator*(const Vector2& rhs) const
+{
+    return Vector2(m[0]*rhs.x + m[2]*rhs.y,  m[1]*rhs.x + m[3]*rhs.y);
+}
+
+
+
+inline Matrix2 Matrix2::operator*(const Matrix2& rhs) const
+{
+    return Matrix2(m[0]*rhs[0] + m[2]*rhs[1],  m[1]*rhs[0] + m[3]*rhs[1],
+                   m[0]*rhs[2] + m[2]*rhs[3],  m[1]*rhs[2] + m[3]*rhs[3]);
+}
+
+
+
+inline Matrix2& Matrix2::operator*=(const Matrix2& rhs)
+{
+    *this = *this * rhs;
+    return *this;
+}
+
+
+
+inline bool Matrix2::operator==(const Matrix2& rhs) const
+{
+    return (m[0] == rhs[0]) && (m[1] == rhs[1]) && (m[2] == rhs[2]) && (m[3] == rhs[3]);
+}
+
+
+
+inline bool Matrix2::operator!=(const Matrix2& rhs) const
+{
+    return (m[0] != rhs[0]) || (m[1] != rhs[1]) || (m[2] != rhs[2]) || (m[3] != rhs[3]);
+}
+
+
+
+inline float Matrix2::operator[](int index) const
+{
+    return m[index];
+}
+
+
+
+inline float& Matrix2::operator[](int index)
+{
+    return m[index];
+}
+
+
+
+inline Matrix2 operator-(const Matrix2& rhs)
+{
+    return Matrix2(-rhs[0], -rhs[1], -rhs[2], -rhs[3]);
+}
+
+
+
+inline Matrix2 operator*(float s, const Matrix2& rhs)
+{
+    return Matrix2(s*rhs[0], s*rhs[1], s*rhs[2], s*rhs[3]);
+}
+
+
+
+inline Vector2 operator*(const Vector2& v, const Matrix2& rhs)
+{
+    return Vector2(v.x*rhs[0] + v.y*rhs[1],  v.x*rhs[2] + v.y*rhs[3]);
+}
+
+
+
+inline std::ostream& operator<<(std::ostream& os, const Matrix2& m)
+{
+    os << std::fixed << std::setprecision(5);
+    os << "[" << std::setw(10) << m[0] << " " << std::setw(10) << m[2] << "]\n"
+       << "[" << std::setw(10) << m[1] << " " << std::setw(10) << m[3] << "]\n";
+    os << std::resetiosflags(std::ios_base::fixed | std::ios_base::floatfield);
+    return os;
+}
+// END OF MATRIX2 INLINE //////////////////////////////////////////////////////
+
+
+
+
+///////////////////////////////////////////////////////////////////////////
+// inline functions for Matrix3
+///////////////////////////////////////////////////////////////////////////
+inline Matrix3::Matrix3()
+{
+    // initially identity matrix
+    identity();
+}
+
+
+
+inline Matrix3::Matrix3(const float src[9])
+{
+    set(src);
+}
+
+
+
+inline Matrix3::Matrix3(float m0, float m1, float m2,
+                        float m3, float m4, float m5,
+                        float m6, float m7, float m8)
+{
+    set(m0, m1, m2,  m3, m4, m5,  m6, m7, m8);
+}
+
+
+
+inline void Matrix3::set(const float src[9])
+{
+    m[0] = src[0];  m[1] = src[1];  m[2] = src[2];
+    m[3] = src[3];  m[4] = src[4];  m[5] = src[5];
+    m[6] = src[6];  m[7] = src[7];  m[8] = src[8];
+}
+
+
+
+inline void Matrix3::set(float m0, float m1, float m2,
+                         float m3, float m4, float m5,
+                         float m6, float m7, float m8)
+{
+    m[0] = m0;  m[1] = m1;  m[2] = m2;
+    m[3] = m3;  m[4] = m4;  m[5] = m5;
+    m[6] = m6;  m[7] = m7;  m[8] = m8;
+}
+
+
+
+inline void Matrix3::setRow(int index, const float row[3])
+{
+    m[index] = row[0];  m[index + 3] = row[1];  m[index + 6] = row[2];
+}
+
+
+
+inline void Matrix3::setRow(int index, const Vector3& v)
+{
+    m[index] = v.x;  m[index + 3] = v.y;  m[index + 6] = v.z;
+}
+
+
+
+inline void Matrix3::setColumn(int index, const float col[3])
+{
+    m[index*3] = col[0];  m[index*3 + 1] = col[1];  m[index*3 + 2] = col[2];
+}
+
+
+
+inline void Matrix3::setColumn(int index, const Vector3& v)
+{
+    m[index*3] = v.x;  m[index*3 + 1] = v.y;  m[index*3 + 2] = v.z;
+}
+
+
+
+inline const float* Matrix3::get() const
+{
+    return m;
+}
+
+
+
+inline Matrix3& Matrix3::identity()
+{
+    m[0] = m[4] = m[8] = 1.0f;
+    m[1] = m[2] = m[3] = m[5] = m[6] = m[7] = 0.0f;
+    return *this;
+}
+
+
+
+inline Matrix3 Matrix3::operator+(const Matrix3& rhs) const
+{
+    return Matrix3(m[0]+rhs[0], m[1]+rhs[1], m[2]+rhs[2],
+                   m[3]+rhs[3], m[4]+rhs[4], m[5]+rhs[5],
+                   m[6]+rhs[6], m[7]+rhs[7], m[8]+rhs[8]);
+}
+
+
+
+inline Matrix3 Matrix3::operator-(const Matrix3& rhs) const
+{
+    return Matrix3(m[0]-rhs[0], m[1]-rhs[1], m[2]-rhs[2],
+                   m[3]-rhs[3], m[4]-rhs[4], m[5]-rhs[5],
+                   m[6]-rhs[6], m[7]-rhs[7], m[8]-rhs[8]);
+}
+
+
+
+inline Matrix3& Matrix3::operator+=(const Matrix3& rhs)
+{
+    m[0] += rhs[0];  m[1] += rhs[1];  m[2] += rhs[2];
+    m[3] += rhs[3];  m[4] += rhs[4];  m[5] += rhs[5];
+    m[6] += rhs[6];  m[7] += rhs[7];  m[8] += rhs[8];
+    return *this;
+}
+
+
+
+inline Matrix3& Matrix3::operator-=(const Matrix3& rhs)
+{
+    m[0] -= rhs[0];  m[1] -= rhs[1];  m[2] -= rhs[2];
+    m[3] -= rhs[3];  m[4] -= rhs[4];  m[5] -= rhs[5];
+    m[6] -= rhs[6];  m[7] -= rhs[7];  m[8] -= rhs[8];
+    return *this;
+}
+
+
+
+inline Vector3 Matrix3::operator*(const Vector3& rhs) const
+{
+    return Vector3(m[0]*rhs.x + m[3]*rhs.y + m[6]*rhs.z,
+                   m[1]*rhs.x + m[4]*rhs.y + m[7]*rhs.z,
+                   m[2]*rhs.x + m[5]*rhs.y + m[8]*rhs.z);
+}
+
+
+
+inline Matrix3 Matrix3::operator*(const Matrix3& rhs) const
+{
+    return Matrix3(m[0]*rhs[0] + m[3]*rhs[1] + m[6]*rhs[2],  m[1]*rhs[0] + m[4]*rhs[1] + m[7]*rhs[2],  m[2]*rhs[0] + m[5]*rhs[1] + m[8]*rhs[2],
+                   m[0]*rhs[3] + m[3]*rhs[4] + m[6]*rhs[5],  m[1]*rhs[3] + m[4]*rhs[4] + m[7]*rhs[5],  m[2]*rhs[3] + m[5]*rhs[4] + m[8]*rhs[5],
+                   m[0]*rhs[6] + m[3]*rhs[7] + m[6]*rhs[8],  m[1]*rhs[6] + m[4]*rhs[7] + m[7]*rhs[8],  m[2]*rhs[6] + m[5]*rhs[7] + m[8]*rhs[8]);
+}
+
+
+
+inline Matrix3& Matrix3::operator*=(const Matrix3& rhs)
+{
+    *this = *this * rhs;
+    return *this;
+}
+
+
+
+inline bool Matrix3::operator==(const Matrix3& rhs) const
+{
+    return (m[0] == rhs[0]) && (m[1] == rhs[1]) && (m[2] == rhs[2]) &&
+           (m[3] == rhs[3]) && (m[4] == rhs[4]) && (m[5] == rhs[5]) &&
+           (m[6] == rhs[6]) && (m[7] == rhs[7]) && (m[8] == rhs[8]);
+}
+
+
+
+inline bool Matrix3::operator!=(const Matrix3& rhs) const
+{
+    return (m[0] != rhs[0]) || (m[1] != rhs[1]) || (m[2] != rhs[2]) ||
+           (m[3] != rhs[3]) || (m[4] != rhs[4]) || (m[5] != rhs[5]) ||
+           (m[6] != rhs[6]) || (m[7] != rhs[7]) || (m[8] != rhs[8]);
+}
+
+
+
+inline float Matrix3::operator[](int index) const
+{
+    return m[index];
+}
+
+
+
+inline float& Matrix3::operator[](int index)
+{
+    return m[index];
+}
+
+
+
+inline Matrix3 operator-(const Matrix3& rhs)
+{
+    return Matrix3(-rhs[0], -rhs[1], -rhs[2], -rhs[3], -rhs[4], -rhs[5], -rhs[6], -rhs[7], -rhs[8]);
+}
+
+
+
+inline Matrix3 operator*(float s, const Matrix3& rhs)
+{
+    return Matrix3(s*rhs[0], s*rhs[1], s*rhs[2], s*rhs[3], s*rhs[4], s*rhs[5], s*rhs[6], s*rhs[7], s*rhs[8]);
+}
+
+
+
+inline Vector3 operator*(const Vector3& v, const Matrix3& m)
+{
+    return Vector3(v.x*m[0] + v.y*m[1] + v.z*m[2],  v.x*m[3] + v.y*m[4] + v.z*m[5],  v.x*m[6] + v.y*m[7] + v.z*m[8]);
+}
+
+
+
+inline std::ostream& operator<<(std::ostream& os, const Matrix3& m)
+{
+    os << std::fixed << std::setprecision(5);
+    os << "[" << std::setw(10) << m[0] << " " << std::setw(10) << m[3] << " " << std::setw(10) << m[6] << "]\n"
+       << "[" << std::setw(10) << m[1] << " " << std::setw(10) << m[4] << " " << std::setw(10) << m[7] << "]\n"
+       << "[" << std::setw(10) << m[2] << " " << std::setw(10) << m[5] << " " << std::setw(10) << m[8] << "]\n";
+    os << std::resetiosflags(std::ios_base::fixed | std::ios_base::floatfield);
+    return os;
+}
+// END OF MATRIX3 INLINE //////////////////////////////////////////////////////
+
+
+
+
+///////////////////////////////////////////////////////////////////////////
+// inline functions for Matrix4
+///////////////////////////////////////////////////////////////////////////
+inline Matrix4::Matrix4()
+{
+    // initially identity matrix
+    identity();
+}
+
+
+
+inline Matrix4::Matrix4(const float src[16])
+{
+    set(src);
+}
+
+
+
+inline Matrix4::Matrix4(float m00, float m01, float m02, float m03,
+                        float m04, float m05, float m06, float m07,
+                        float m08, float m09, float m10, float m11,
+                        float m12, float m13, float m14, float m15)
+{
+    set(m00, m01, m02, m03,  m04, m05, m06, m07,  m08, m09, m10, m11,  m12, m13, m14, m15);
+}
+
+
+
+inline void Matrix4::set(const float src[16])
+{
+    m[0] = src[0];  m[1] = src[1];  m[2] = src[2];  m[3] = src[3];
+    m[4] = src[4];  m[5] = src[5];  m[6] = src[6];  m[7] = src[7];
+    m[8] = src[8];  m[9] = src[9];  m[10]= src[10]; m[11]= src[11];
+    m[12]= src[12]; m[13]= src[13]; m[14]= src[14]; m[15]= src[15];
+}
+
+
+
+inline void Matrix4::set(float m00, float m01, float m02, float m03,
+                         float m04, float m05, float m06, float m07,
+                         float m08, float m09, float m10, float m11,
+                         float m12, float m13, float m14, float m15)
+{
+    m[0] = m00;  m[1] = m01;  m[2] = m02;  m[3] = m03;
+    m[4] = m04;  m[5] = m05;  m[6] = m06;  m[7] = m07;
+    m[8] = m08;  m[9] = m09;  m[10]= m10;  m[11]= m11;
+    m[12]= m12;  m[13]= m13;  m[14]= m14;  m[15]= m15;
+}
+
+
+
+inline void Matrix4::setRow(int index, const float row[4])
+{
+    m[index] = row[0];  m[index + 4] = row[1];  m[index + 8] = row[2];  m[index + 12] = row[3];
+}
+
+
+
+inline void Matrix4::setRow(int index, const Vector4& v)
+{
+    m[index] = v.x;  m[index + 4] = v.y;  m[index + 8] = v.z;  m[index + 12] = v.w;
+}
+
+
+
+inline void Matrix4::setRow(int index, const Vector3& v)
+{
+    m[index] = v.x;  m[index + 4] = v.y;  m[index + 8] = v.z;
+}
+
+
+
+inline void Matrix4::setColumn(int index, const float col[4])
+{
+    m[index*4] = col[0];  m[index*4 + 1] = col[1];  m[index*4 + 2] = col[2];  m[index*4 + 3] = col[3];
+}
+
+
+
+inline void Matrix4::setColumn(int index, const Vector4& v)
+{
+    m[index*4] = v.x;  m[index*4 + 1] = v.y;  m[index*4 + 2] = v.z;  m[index*4 + 3] = v.w;
+}
+
+
+
+inline void Matrix4::setColumn(int index, const Vector3& v)
+{
+    m[index*4] = v.x;  m[index*4 + 1] = v.y;  m[index*4 + 2] = v.z;
+}
+
+
+
+inline const float* Matrix4::get() const
+{
+    return m;
+}
+
+
+
+inline const float* Matrix4::getTranspose()
+{
+    tm[0] = m[0];   tm[1] = m[4];   tm[2] = m[8];   tm[3] = m[12];
+    tm[4] = m[1];   tm[5] = m[5];   tm[6] = m[9];   tm[7] = m[13];
+    tm[8] = m[2];   tm[9] = m[6];   tm[10]= m[10];  tm[11]= m[14];
+    tm[12]= m[3];   tm[13]= m[7];   tm[14]= m[11];  tm[15]= m[15];
+    return tm;
+}
+
+
+
+inline Matrix4& Matrix4::identity()
+{
+    m[0] = m[5] = m[10] = m[15] = 1.0f;
+    m[1] = m[2] = m[3] = m[4] = m[6] = m[7] = m[8] = m[9] = m[11] = m[12] = m[13] = m[14] = 0.0f;
+    return *this;
+}
+
+
+
+inline Matrix4 Matrix4::operator+(const Matrix4& rhs) const
+{
+    return Matrix4(m[0]+rhs[0],   m[1]+rhs[1],   m[2]+rhs[2],   m[3]+rhs[3],
+                   m[4]+rhs[4],   m[5]+rhs[5],   m[6]+rhs[6],   m[7]+rhs[7],
+                   m[8]+rhs[8],   m[9]+rhs[9],   m[10]+rhs[10], m[11]+rhs[11],
+                   m[12]+rhs[12], m[13]+rhs[13], m[14]+rhs[14], m[15]+rhs[15]);
+}
+
+
+
+inline Matrix4 Matrix4::operator-(const Matrix4& rhs) const
+{
+    return Matrix4(m[0]-rhs[0],   m[1]-rhs[1],   m[2]-rhs[2],   m[3]-rhs[3],
+                   m[4]-rhs[4],   m[5]-rhs[5],   m[6]-rhs[6],   m[7]-rhs[7],
+                   m[8]-rhs[8],   m[9]-rhs[9],   m[10]-rhs[10], m[11]-rhs[11],
+                   m[12]-rhs[12], m[13]-rhs[13], m[14]-rhs[14], m[15]-rhs[15]);
+}
+
+
+
+inline Matrix4& Matrix4::operator+=(const Matrix4& rhs)
+{
+    m[0] += rhs[0];   m[1] += rhs[1];   m[2] += rhs[2];   m[3] += rhs[3];
+    m[4] += rhs[4];   m[5] += rhs[5];   m[6] += rhs[6];   m[7] += rhs[7];
+    m[8] += rhs[8];   m[9] += rhs[9];   m[10]+= rhs[10];  m[11]+= rhs[11];
+    m[12]+= rhs[12];  m[13]+= rhs[13];  m[14]+= rhs[14];  m[15]+= rhs[15];
+    return *this;
+}
+
+
+
+inline Matrix4& Matrix4::operator-=(const Matrix4& rhs)
+{
+    m[0] -= rhs[0];   m[1] -= rhs[1];   m[2] -= rhs[2];   m[3] -= rhs[3];
+    m[4] -= rhs[4];   m[5] -= rhs[5];   m[6] -= rhs[6];   m[7] -= rhs[7];
+    m[8] -= rhs[8];   m[9] -= rhs[9];   m[10]-= rhs[10];  m[11]-= rhs[11];
+    m[12]-= rhs[12];  m[13]-= rhs[13];  m[14]-= rhs[14];  m[15]-= rhs[15];
+    return *this;
+}
+
+
+
+inline Vector4 Matrix4::operator*(const Vector4& rhs) const
+{
+    return Vector4(m[0]*rhs.x + m[4]*rhs.y + m[8]*rhs.z  + m[12]*rhs.w,
+                   m[1]*rhs.x + m[5]*rhs.y + m[9]*rhs.z  + m[13]*rhs.w,
+                   m[2]*rhs.x + m[6]*rhs.y + m[10]*rhs.z + m[14]*rhs.w,
+                   m[3]*rhs.x + m[7]*rhs.y + m[11]*rhs.z + m[15]*rhs.w);
+}
+
+
+
+inline Vector3 Matrix4::operator*(const Vector3& rhs) const
+{
+    return Vector3(m[0]*rhs.x + m[4]*rhs.y + m[8]*rhs.z,
+                   m[1]*rhs.x + m[5]*rhs.y + m[9]*rhs.z,
+                   m[2]*rhs.x + m[6]*rhs.y + m[10]*rhs.z);
+}
+
+
+
+inline Matrix4 Matrix4::operator*(const Matrix4& n) const
+{
+    return Matrix4(m[0]*n[0]  + m[4]*n[1]  + m[8]*n[2]  + m[12]*n[3],   m[1]*n[0]  + m[5]*n[1]  + m[9]*n[2]  + m[13]*n[3],   m[2]*n[0]  + m[6]*n[1]  + m[10]*n[2]  + m[14]*n[3],   m[3]*n[0]  + m[7]*n[1]  + m[11]*n[2]  + m[15]*n[3],
+                   m[0]*n[4]  + m[4]*n[5]  + m[8]*n[6]  + m[12]*n[7],   m[1]*n[4]  + m[5]*n[5]  + m[9]*n[6]  + m[13]*n[7],   m[2]*n[4]  + m[6]*n[5]  + m[10]*n[6]  + m[14]*n[7],   m[3]*n[4]  + m[7]*n[5]  + m[11]*n[6]  + m[15]*n[7],
+                   m[0]*n[8]  + m[4]*n[9]  + m[8]*n[10] + m[12]*n[11],  m[1]*n[8]  + m[5]*n[9]  + m[9]*n[10] + m[13]*n[11],  m[2]*n[8]  + m[6]*n[9]  + m[10]*n[10] + m[14]*n[11],  m[3]*n[8]  + m[7]*n[9]  + m[11]*n[10] + m[15]*n[11],
+                   m[0]*n[12] + m[4]*n[13] + m[8]*n[14] + m[12]*n[15],  m[1]*n[12] + m[5]*n[13] + m[9]*n[14] + m[13]*n[15],  m[2]*n[12] + m[6]*n[13] + m[10]*n[14] + m[14]*n[15],  m[3]*n[12] + m[7]*n[13] + m[11]*n[14] + m[15]*n[15]);
+}
+
+
+
+inline Matrix4& Matrix4::operator*=(const Matrix4& rhs)
+{
+    *this = *this * rhs;
+    return *this;
+}
+
+
+
+inline bool Matrix4::operator==(const Matrix4& n) const
+{
+    return (m[0] == n[0])  && (m[1] == n[1])  && (m[2] == n[2])  && (m[3] == n[3])  &&
+           (m[4] == n[4])  && (m[5] == n[5])  && (m[6] == n[6])  && (m[7] == n[7])  &&
+           (m[8] == n[8])  && (m[9] == n[9])  && (m[10]== n[10]) && (m[11]== n[11]) &&
+           (m[12]== n[12]) && (m[13]== n[13]) && (m[14]== n[14]) && (m[15]== n[15]);
+}
+
+
+
+inline bool Matrix4::operator!=(const Matrix4& n) const
+{
+    return (m[0] != n[0])  || (m[1] != n[1])  || (m[2] != n[2])  || (m[3] != n[3])  ||
+           (m[4] != n[4])  || (m[5] != n[5])  || (m[6] != n[6])  || (m[7] != n[7])  ||
+           (m[8] != n[8])  || (m[9] != n[9])  || (m[10]!= n[10]) || (m[11]!= n[11]) ||
+           (m[12]!= n[12]) || (m[13]!= n[13]) || (m[14]!= n[14]) || (m[15]!= n[15]);
+}
+
+
+
+inline float Matrix4::operator[](int index) const
+{
+    return m[index];
+}
+
+
+
+inline float& Matrix4::operator[](int index)
+{
+    return m[index];
+}
+
+
+
+inline Matrix4 operator-(const Matrix4& rhs)
+{
+    return Matrix4(-rhs[0], -rhs[1], -rhs[2], -rhs[3], -rhs[4], -rhs[5], -rhs[6], -rhs[7], -rhs[8], -rhs[9], -rhs[10], -rhs[11], -rhs[12], -rhs[13], -rhs[14], -rhs[15]);
+}
+
+
+
+inline Matrix4 operator*(float s, const Matrix4& rhs)
+{
+    return Matrix4(s*rhs[0], s*rhs[1], s*rhs[2], s*rhs[3], s*rhs[4], s*rhs[5], s*rhs[6], s*rhs[7], s*rhs[8], s*rhs[9], s*rhs[10], s*rhs[11], s*rhs[12], s*rhs[13], s*rhs[14], s*rhs[15]);
+}
+
+
+
+inline Vector4 operator*(const Vector4& v, const Matrix4& m)
+{
+    return Vector4(v.x*m[0] + v.y*m[1] + v.z*m[2] + v.w*m[3],  v.x*m[4] + v.y*m[5] + v.z*m[6] + v.w*m[7],  v.x*m[8] + v.y*m[9] + v.z*m[10] + v.w*m[11], v.x*m[12] + v.y*m[13] + v.z*m[14] + v.w*m[15]);
+}
+
+
+
+inline Vector3 operator*(const Vector3& v, const Matrix4& m)
+{
+    return Vector3(v.x*m[0] + v.y*m[1] + v.z*m[2],  v.x*m[4] + v.y*m[5] + v.z*m[6],  v.x*m[8] + v.y*m[9] + v.z*m[10]);
+}
+
+
+
+inline std::ostream& operator<<(std::ostream& os, const Matrix4& m)
+{
+    os << std::fixed << std::setprecision(5);
+    os << "[" << std::setw(10) << m[0] << " " << std::setw(10) << m[4] << " " << std::setw(10) << m[8]  <<  " " << std::setw(10) << m[12] << "]\n"
+       << "[" << std::setw(10) << m[1] << " " << std::setw(10) << m[5] << " " << std::setw(10) << m[9]  <<  " " << std::setw(10) << m[13] << "]\n"
+       << "[" << std::setw(10) << m[2] << " " << std::setw(10) << m[6] << " " << std::setw(10) << m[10] <<  " " << std::setw(10) << m[14] << "]\n"
+       << "[" << std::setw(10) << m[3] << " " << std::setw(10) << m[7] << " " << std::setw(10) << m[11] <<  " " << std::setw(10) << m[15] << "]\n";
+    os << std::resetiosflags(std::ios_base::fixed | std::ios_base::floatfield);
+    return os;
+}
+// END OF MATRIX4 INLINE //////////////////////////////////////////////////////
+#endif

examples/ThirdPartyLibs/openvr/samples/shared/Vectors.h

@@ -0,0 +1,530 @@
+///////////////////////////////////////////////////////////////////////////////
+// Vectors.h
+// =========
+// 2D/3D/4D vectors
+//
+//  AUTHOR: Song Ho Ahn (song.ahn@gmail.com)
+// CREATED: 2007-02-14
+// UPDATED: 2013-01-20
+//
+// Copyright (C) 2007-2013 Song Ho Ahn
+///////////////////////////////////////////////////////////////////////////////
+
+
+#ifndef VECTORS_H_DEF
+#define VECTORS_H_DEF
+
+#include <cmath>
+#include <iostream>
+
+///////////////////////////////////////////////////////////////////////////////
+// 2D vector
+///////////////////////////////////////////////////////////////////////////////
+struct Vector2
+{
+    float x;
+    float y;
+
+    // ctors
+    Vector2() : x(0), y(0) {};
+    Vector2(float x, float y) : x(x), y(y) {};
+
+    // utils functions
+    void        set(float x, float y);
+    float       length() const;                         //
+    float       distance(const Vector2& vec) const;     // distance between two vectors
+    Vector2&    normalize();                            //
+    float       dot(const Vector2& vec) const;          // dot product
+    bool        equal(const Vector2& vec, float e) const; // compare with epsilon
+
+    // operators
+    Vector2     operator-() const;                      // unary operator (negate)
+    Vector2     operator+(const Vector2& rhs) const;    // add rhs
+    Vector2     operator-(const Vector2& rhs) const;    // subtract rhs
+    Vector2&    operator+=(const Vector2& rhs);         // add rhs and update this object
+    Vector2&    operator-=(const Vector2& rhs);         // subtract rhs and update this object
+    Vector2     operator*(const float scale) const;     // scale
+    Vector2     operator*(const Vector2& rhs) const;    // multiply each element
+    Vector2&    operator*=(const float scale);          // scale and update this object
+    Vector2&    operator*=(const Vector2& rhs);         // multiply each element and update this object
+    Vector2     operator/(const float scale) const;     // inverse scale
+    Vector2&    operator/=(const float scale);          // scale and update this object
+    bool        operator==(const Vector2& rhs) const;   // exact compare, no epsilon
+    bool        operator!=(const Vector2& rhs) const;   // exact compare, no epsilon
+    bool        operator<(const Vector2& rhs) const;    // comparison for sort
+    float       operator[](int index) const;            // subscript operator v[0], v[1]
+    float&      operator[](int index);                  // subscript operator v[0], v[1]
+
+    friend Vector2 operator*(const float a, const Vector2 vec);
+    friend std::ostream& operator<<(std::ostream& os, const Vector2& vec);
+};
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// 3D vector
+///////////////////////////////////////////////////////////////////////////////
+struct Vector3
+{
+    float x;
+    float y;
+    float z;
+
+    // ctors
+    Vector3() : x(0), y(0), z(0) {};
+    Vector3(float x, float y, float z) : x(x), y(y), z(z) {};
+
+    // utils functions
+    void        set(float x, float y, float z);
+    float       length() const;                         //
+    float       distance(const Vector3& vec) const;     // distance between two vectors
+    Vector3&    normalize();                            //
+    float       dot(const Vector3& vec) const;          // dot product
+    Vector3     cross(const Vector3& vec) const;        // cross product
+    bool        equal(const Vector3& vec, float e) const; // compare with epsilon
+
+    // operators
+    Vector3     operator-() const;                      // unary operator (negate)
+    Vector3     operator+(const Vector3& rhs) const;    // add rhs
+    Vector3     operator-(const Vector3& rhs) const;    // subtract rhs
+    Vector3&    operator+=(const Vector3& rhs);         // add rhs and update this object
+    Vector3&    operator-=(const Vector3& rhs);         // subtract rhs and update this object
+    Vector3     operator*(const float scale) const;     // scale
+    Vector3     operator*(const Vector3& rhs) const;    // multiplay each element
+    Vector3&    operator*=(const float scale);          // scale and update this object
+    Vector3&    operator*=(const Vector3& rhs);         // product each element and update this object
+    Vector3     operator/(const float scale) const;     // inverse scale
+    Vector3&    operator/=(const float scale);          // scale and update this object
+    bool        operator==(const Vector3& rhs) const;   // exact compare, no epsilon
+    bool        operator!=(const Vector3& rhs) const;   // exact compare, no epsilon
+    bool        operator<(const Vector3& rhs) const;    // comparison for sort
+    float       operator[](int index) const;            // subscript operator v[0], v[1]
+    float&      operator[](int index);                  // subscript operator v[0], v[1]
+
+    friend Vector3 operator*(const float a, const Vector3 vec);
+    friend std::ostream& operator<<(std::ostream& os, const Vector3& vec);
+};
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// 4D vector
+///////////////////////////////////////////////////////////////////////////////
+struct Vector4
+{
+    float x;
+    float y;
+    float z;
+    float w;
+
+    // ctors
+    Vector4() : x(0), y(0), z(0), w(0) {};
+    Vector4(float x, float y, float z, float w) : x(x), y(y), z(z), w(w) {};
+
+    // utils functions
+    void        set(float x, float y, float z, float w);
+    float       length() const;                         //
+    float       distance(const Vector4& vec) const;     // distance between two vectors
+    Vector4&    normalize();                            //
+    float       dot(const Vector4& vec) const;          // dot product
+    bool        equal(const Vector4& vec, float e) const; // compare with epsilon
+
+    // operators
+    Vector4     operator-() const;                      // unary operator (negate)
+    Vector4     operator+(const Vector4& rhs) const;    // add rhs
+    Vector4     operator-(const Vector4& rhs) const;    // subtract rhs
+    Vector4&    operator+=(const Vector4& rhs);         // add rhs and update this object
+    Vector4&    operator-=(const Vector4& rhs);         // subtract rhs and update this object
+    Vector4     operator*(const float scale) const;     // scale
+    Vector4     operator*(const Vector4& rhs) const;    // multiply each element
+    Vector4&    operator*=(const float scale);          // scale and update this object
+    Vector4&    operator*=(const Vector4& rhs);         // multiply each element and update this object
+    Vector4     operator/(const float scale) const;     // inverse scale
+    Vector4&    operator/=(const float scale);          // scale and update this object
+    bool        operator==(const Vector4& rhs) const;   // exact compare, no epsilon
+    bool        operator!=(const Vector4& rhs) const;   // exact compare, no epsilon
+    bool        operator<(const Vector4& rhs) const;    // comparison for sort
+    float       operator[](int index) const;            // subscript operator v[0], v[1]
+    float&      operator[](int index);                  // subscript operator v[0], v[1]
+
+    friend Vector4 operator*(const float a, const Vector4 vec);
+    friend std::ostream& operator<<(std::ostream& os, const Vector4& vec);
+};
+
+
+
+// fast math routines from Doom3 SDK
+inline float invSqrt(float x)
+{
+    float xhalf = 0.5f * x;
+    int i = *(int*)&x;          // get bits for floating value
+    i = 0x5f3759df - (i>>1);    // gives initial guess
+    x = *(float*)&i;            // convert bits back to float
+    x = x * (1.5f - xhalf*x*x); // Newton step
+    return x;
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// inline functions for Vector2
+///////////////////////////////////////////////////////////////////////////////
+inline Vector2 Vector2::operator-() const {
+    return Vector2(-x, -y);
+}
+
+inline Vector2 Vector2::operator+(const Vector2& rhs) const {
+    return Vector2(x+rhs.x, y+rhs.y);
+}
+
+inline Vector2 Vector2::operator-(const Vector2& rhs) const {
+    return Vector2(x-rhs.x, y-rhs.y);
+}
+
+inline Vector2& Vector2::operator+=(const Vector2& rhs) {
+    x += rhs.x; y += rhs.y; return *this;
+}
+
+inline Vector2& Vector2::operator-=(const Vector2& rhs) {
+    x -= rhs.x; y -= rhs.y; return *this;
+}
+
+inline Vector2 Vector2::operator*(const float a) const {
+    return Vector2(x*a, y*a);
+}
+
+inline Vector2 Vector2::operator*(const Vector2& rhs) const {
+    return Vector2(x*rhs.x, y*rhs.y);
+}
+
+inline Vector2& Vector2::operator*=(const float a) {
+    x *= a; y *= a; return *this;
+}
+
+inline Vector2& Vector2::operator*=(const Vector2& rhs) {
+    x *= rhs.x; y *= rhs.y; return *this;
+}
+
+inline Vector2 Vector2::operator/(const float a) const {
+    return Vector2(x/a, y/a);
+}
+
+inline Vector2& Vector2::operator/=(const float a) {
+    x /= a; y /= a; return *this;
+}
+
+inline bool Vector2::operator==(const Vector2& rhs) const {
+    return (x == rhs.x) && (y == rhs.y);
+}
+
+inline bool Vector2::operator!=(const Vector2& rhs) const {
+    return (x != rhs.x) || (y != rhs.y);
+}
+
+inline bool Vector2::operator<(const Vector2& rhs) const {
+    if(x < rhs.x) return true;
+    if(x > rhs.x) return false;
+    if(y < rhs.y) return true;
+    if(y > rhs.y) return false;
+    return false;
+}
+
+inline float Vector2::operator[](int index) const {
+    return (&x)[index];
+}
+
+inline float& Vector2::operator[](int index) {
+    return (&x)[index];
+}
+
+inline void Vector2::set(float x, float y) {
+    this->x = x; this->y = y;
+}
+
+inline float Vector2::length() const {
+    return sqrtf(x*x + y*y);
+}
+
+inline float Vector2::distance(const Vector2& vec) const {
+    return sqrtf((vec.x-x)*(vec.x-x) + (vec.y-y)*(vec.y-y));
+}
+
+inline Vector2& Vector2::normalize() {
+    //@@const float EPSILON = 0.000001f;
+    float xxyy = x*x + y*y;
+    //@@if(xxyy < EPSILON)
+    //@@    return *this;
+
+    //float invLength = invSqrt(xxyy);
+    float invLength = 1.0f / sqrtf(xxyy);
+    x *= invLength;
+    y *= invLength;
+    return *this;
+}
+
+inline float Vector2::dot(const Vector2& rhs) const {
+    return (x*rhs.x + y*rhs.y);
+}
+
+inline bool Vector2::equal(const Vector2& rhs, float epsilon) const {
+    return fabs(x - rhs.x) < epsilon && fabs(y - rhs.y) < epsilon;
+}
+
+inline Vector2 operator*(const float a, const Vector2 vec) {
+    return Vector2(a*vec.x, a*vec.y);
+}
+
+inline std::ostream& operator<<(std::ostream& os, const Vector2& vec) {
+    os << "(" << vec.x << ", " << vec.y << ")";
+    return os;
+}
+// END OF VECTOR2 /////////////////////////////////////////////////////////////
+
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// inline functions for Vector3
+///////////////////////////////////////////////////////////////////////////////
+inline Vector3 Vector3::operator-() const {
+    return Vector3(-x, -y, -z);
+}
+
+inline Vector3 Vector3::operator+(const Vector3& rhs) const {
+    return Vector3(x+rhs.x, y+rhs.y, z+rhs.z);
+}
+
+inline Vector3 Vector3::operator-(const Vector3& rhs) const {
+    return Vector3(x-rhs.x, y-rhs.y, z-rhs.z);
+}
+
+inline Vector3& Vector3::operator+=(const Vector3& rhs) {
+    x += rhs.x; y += rhs.y; z += rhs.z; return *this;
+}
+
+inline Vector3& Vector3::operator-=(const Vector3& rhs) {
+    x -= rhs.x; y -= rhs.y; z -= rhs.z; return *this;
+}
+
+inline Vector3 Vector3::operator*(const float a) const {
+    return Vector3(x*a, y*a, z*a);
+}
+
+inline Vector3 Vector3::operator*(const Vector3& rhs) const {
+    return Vector3(x*rhs.x, y*rhs.y, z*rhs.z);
+}
+
+inline Vector3& Vector3::operator*=(const float a) {
+    x *= a; y *= a; z *= a; return *this;
+}
+
+inline Vector3& Vector3::operator*=(const Vector3& rhs) {
+    x *= rhs.x; y *= rhs.y; z *= rhs.z; return *this;
+}
+
+inline Vector3 Vector3::operator/(const float a) const {
+    return Vector3(x/a, y/a, z/a);
+}
+
+inline Vector3& Vector3::operator/=(const float a) {
+    x /= a; y /= a; z /= a; return *this;
+}
+
+inline bool Vector3::operator==(const Vector3& rhs) const {
+    return (x == rhs.x) && (y == rhs.y) && (z == rhs.z);
+}
+
+inline bool Vector3::operator!=(const Vector3& rhs) const {
+    return (x != rhs.x) || (y != rhs.y) || (z != rhs.z);
+}
+
+inline bool Vector3::operator<(const Vector3& rhs) const {
+    if(x < rhs.x) return true;
+    if(x > rhs.x) return false;
+    if(y < rhs.y) return true;
+    if(y > rhs.y) return false;
+    if(z < rhs.z) return true;
+    if(z > rhs.z) return false;
+    return false;
+}
+
+inline float Vector3::operator[](int index) const {
+    return (&x)[index];
+}
+
+inline float& Vector3::operator[](int index) {
+    return (&x)[index];
+}
+
+inline void Vector3::set(float x, float y, float z) {
+    this->x = x; this->y = y; this->z = z;
+}
+
+inline float Vector3::length() const {
+    return sqrtf(x*x + y*y + z*z);
+}
+
+inline float Vector3::distance(const Vector3& vec) const {
+    return sqrtf((vec.x-x)*(vec.x-x) + (vec.y-y)*(vec.y-y) + (vec.z-z)*(vec.z-z));
+}
+
+inline Vector3& Vector3::normalize() {
+    //@@const float EPSILON = 0.000001f;
+    float xxyyzz = x*x + y*y + z*z;
+    //@@if(xxyyzz < EPSILON)
+    //@@    return *this; // do nothing if it is ~zero vector
+
+    //float invLength = invSqrt(xxyyzz);
+    float invLength = 1.0f / sqrtf(xxyyzz);
+    x *= invLength;
+    y *= invLength;
+    z *= invLength;
+    return *this;
+}
+
+inline float Vector3::dot(const Vector3& rhs) const {
+    return (x*rhs.x + y*rhs.y + z*rhs.z);
+}
+
+inline Vector3 Vector3::cross(const Vector3& rhs) const {
+    return Vector3(y*rhs.z - z*rhs.y, z*rhs.x - x*rhs.z, x*rhs.y - y*rhs.x);
+}
+
+inline bool Vector3::equal(const Vector3& rhs, float epsilon) const {
+    return fabs(x - rhs.x) < epsilon && fabs(y - rhs.y) < epsilon && fabs(z - rhs.z) < epsilon;
+}
+
+inline Vector3 operator*(const float a, const Vector3 vec) {
+    return Vector3(a*vec.x, a*vec.y, a*vec.z);
+}
+
+inline std::ostream& operator<<(std::ostream& os, const Vector3& vec) {
+    os << "(" << vec.x << ", " << vec.y << ", " << vec.z << ")";
+    return os;
+}
+// END OF VECTOR3 /////////////////////////////////////////////////////////////
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+// inline functions for Vector4
+///////////////////////////////////////////////////////////////////////////////
+inline Vector4 Vector4::operator-() const {
+    return Vector4(-x, -y, -z, -w);
+}
+
+inline Vector4 Vector4::operator+(const Vector4& rhs) const {
+    return Vector4(x+rhs.x, y+rhs.y, z+rhs.z, w+rhs.w);
+}
+
+inline Vector4 Vector4::operator-(const Vector4& rhs) const {
+    return Vector4(x-rhs.x, y-rhs.y, z-rhs.z, w-rhs.w);
+}
+
+inline Vector4& Vector4::operator+=(const Vector4& rhs) {
+    x += rhs.x; y += rhs.y; z += rhs.z; w += rhs.w; return *this;
+}
+
+inline Vector4& Vector4::operator-=(const Vector4& rhs) {
+    x -= rhs.x; y -= rhs.y; z -= rhs.z; w -= rhs.w; return *this;
+}
+
+inline Vector4 Vector4::operator*(const float a) const {
+    return Vector4(x*a, y*a, z*a, w*a);
+}
+
+inline Vector4 Vector4::operator*(const Vector4& rhs) const {
+    return Vector4(x*rhs.x, y*rhs.y, z*rhs.z, w*rhs.w);
+}
+
+inline Vector4& Vector4::operator*=(const float a) {
+    x *= a; y *= a; z *= a; w *= a; return *this;
+}
+
+inline Vector4& Vector4::operator*=(const Vector4& rhs) {
+    x *= rhs.x; y *= rhs.y; z *= rhs.z; w *= rhs.w; return *this;
+}
+
+inline Vector4 Vector4::operator/(const float a) const {
+    return Vector4(x/a, y/a, z/a, w/a);
+}
+
+inline Vector4& Vector4::operator/=(const float a) {
+    x /= a; y /= a; z /= a; w /= a; return *this;
+}
+
+inline bool Vector4::operator==(const Vector4& rhs) const {
+    return (x == rhs.x) && (y == rhs.y) && (z == rhs.z) && (w == rhs.w);
+}
+
+inline bool Vector4::operator!=(const Vector4& rhs) const {
+    return (x != rhs.x) || (y != rhs.y) || (z != rhs.z) || (w != rhs.w);
+}
+
+inline bool Vector4::operator<(const Vector4& rhs) const {
+    if(x < rhs.x) return true;
+    if(x > rhs.x) return false;
+    if(y < rhs.y) return true;
+    if(y > rhs.y) return false;
+    if(z < rhs.z) return true;
+    if(z > rhs.z) return false;
+    if(w < rhs.w) return true;
+    if(w > rhs.w) return false;
+    return false;
+}
+
+inline float Vector4::operator[](int index) const {
+    return (&x)[index];
+}
+
+inline float& Vector4::operator[](int index) {
+    return (&x)[index];
+}
+
+inline void Vector4::set(float x, float y, float z, float w) {
+    this->x = x; this->y = y; this->z = z; this->w = w;
+}
+
+inline float Vector4::length() const {
+    return sqrtf(x*x + y*y + z*z + w*w);
+}
+
+inline float Vector4::distance(const Vector4& vec) const {
+    return sqrtf((vec.x-x)*(vec.x-x) + (vec.y-y)*(vec.y-y) + (vec.z-z)*(vec.z-z) + (vec.w-w)*(vec.w-w));
+}
+
+inline Vector4& Vector4::normalize() {
+    //NOTE: leave w-component untouched
+    //@@const float EPSILON = 0.000001f;
+    float xxyyzz = x*x + y*y + z*z;
+    //@@if(xxyyzz < EPSILON)
+    //@@    return *this; // do nothing if it is zero vector
+
+    //float invLength = invSqrt(xxyyzz);
+    float invLength = 1.0f / sqrtf(xxyyzz);
+    x *= invLength;
+    y *= invLength;
+    z *= invLength;
+    return *this;
+}
+
+inline float Vector4::dot(const Vector4& rhs) const {
+    return (x*rhs.x + y*rhs.y + z*rhs.z + w*rhs.w);
+}
+
+inline bool Vector4::equal(const Vector4& rhs, float epsilon) const {
+    return fabs(x - rhs.x) < epsilon && fabs(y - rhs.y) < epsilon &&
+           fabs(z - rhs.z) < epsilon && fabs(w - rhs.w) < epsilon;
+}
+
+inline Vector4 operator*(const float a, const Vector4 vec) {
+    return Vector4(a*vec.x, a*vec.y, a*vec.z, a*vec.w);
+}
+
+inline std::ostream& operator<<(std::ostream& os, const Vector4& vec) {
+    os << "(" << vec.x << ", " << vec.y << ", " << vec.z << ", " << vec.w << ")";
+    return os;
+}
+// END OF VECTOR4 /////////////////////////////////////////////////////////////
+
+#endif

examples/ThirdPartyLibs/openvr/samples/shared/pathtools.cpp

@@ -0,0 +1,560 @@
+//========= Copyright Valve Corporation ============//
+#include "pathtools.h"
+//#include "hmdplatform_private.h"
+//#include "vrcommon/strtools.h"
+
+#if defined( _WIN32)
+#include <Windows.h>
+#include <direct.h>
+#include <Shobjidl.h>
+#include <KnownFolders.h>
+#elif defined OSX
+#include <mach-o/dyld.h>
+#include <dlfcn.h>
+#include "osxfilebridge.h"
+#define _S_IFDIR S_IFDIR     // really from tier0/platform.h which we dont have yet
+#define _MAX_PATH MAX_PATH   // yet another form of _PATH define we use
+#elif defined(LINUX)
+#include <dlfcn.h>
+#include <stdio.h>
+#endif
+
+#include <sys/stat.h>
+
+#include <algorithm>
+
+/** Returns the path (including filename) to the current executable */
+std::string Path_GetExecutablePath()
+{
+	bool bSuccess = false;
+	char rchPath[ 1024 ];
+	size_t nBuff = sizeof(rchPath);
+#if defined( _WIN32 )
+	bSuccess = ::GetModuleFileNameA(NULL, rchPath, (DWORD)nBuff) > 0;
+#elif defined OSX
+	uint32_t _nBuff = nBuff; 
+	bSuccess = _NSGetExecutablePath(rchPath, &_nBuff) == 0;
+	rchPath[nBuff-1] = '\0';
+#elif defined LINUX
+	ssize_t nRead = readlink("/proc/self/exe", rchPath, nBuff-1 );
+	if ( nRead != -1 )
+	{
+		rchPath[ nRead ] = 0;
+		bSuccess = true;
+	}
+	else
+	{
+		rchPath[ 0 ] = '\0';
+	}
+#else
+	AssertMsg( false, "Implement Plat_GetExecutablePath" );
+#endif
+
+	if( bSuccess )
+		return rchPath;
+	else
+		return "";
+}
+
+/** Returns the path of the current working directory */
+std::string Path_GetWorkingDirectory()
+{
+	std::string sPath;
+	char buf[ 1024 ];
+#if defined( _WIN32 )
+	sPath = _getcwd( buf, sizeof( buf ) );
+#else
+	sPath = getcwd( buf, sizeof( buf ) );
+#endif
+	return sPath;
+}
+
+/** Sets the path of the current working directory. Returns true if this was successful. */
+bool Path_SetWorkingDirectory( const std::string & sPath )
+{
+	bool bSuccess;
+#if defined( _WIN32 )
+	bSuccess = 0 == _chdir( sPath.c_str() );
+#else
+	bSuccess = 0 == chdir( sPath.c_str() );
+#endif
+	return bSuccess;
+}
+
+std::string Path_GetModulePath()
+{
+#if defined( _WIN32 )
+	char path[32768];
+	HMODULE hm = NULL;
+
+	if (!GetModuleHandleExA(GET_MODULE_HANDLE_EX_FLAG_FROM_ADDRESS,
+		(LPCSTR) &Path_GetModulePath, 
+		&hm))
+	{
+		int ret = GetLastError();
+		fprintf(stderr, "GetModuleHandle returned %d\n", ret);
+		return "";
+	}
+	GetModuleFileNameA(hm, path, sizeof(path));
+	FreeLibrary( hm );
+	return path;
+#else
+	Dl_info dl_info;
+	dladdr((void *)Path_GetModulePath, &dl_info);
+	return dl_info.dli_fname;
+#endif
+}
+
+/** Returns the specified path without its filename */
+std::string Path_StripFilename( const std::string & sPath, char slash )
+{
+	if( slash == 0 )
+		slash = Path_GetSlash();
+
+	std::string::size_type n = sPath.find_last_of( slash );
+	if( n == std::string::npos )
+		return sPath;
+	else
+		return std::string( sPath.begin(), sPath.begin() + n );
+}
+
+/** returns just the filename from the provided full or relative path. */
+std::string Path_StripDirectory( const std::string & sPath, char slash )
+{
+	if( slash == 0 )
+		slash = Path_GetSlash();
+
+	std::string::size_type n = sPath.find_last_of( slash );
+	if( n == std::string::npos )
+		return sPath;
+	else
+		return std::string( sPath.begin() + n + 1, sPath.end() );
+}
+
+/** returns just the filename with no extension of the provided filename. 
+* If there is a path the path is left intact. */
+std::string Path_StripExtension( const std::string & sPath )
+{
+	for( std::string::const_reverse_iterator i = sPath.rbegin(); i != sPath.rend(); i++ )
+	{
+		if( *i == '.' )
+		{
+			return std::string( sPath.begin(), i.base() - 1 );
+		}
+
+		// if we find a slash there is no extension
+		if( *i == '\\' || *i == '/' )
+			break;
+	}
+
+	// we didn't find an extension
+	return sPath;
+}
+
+
+bool Path_IsAbsolute( const std::string & sPath )
+{
+	if( sPath.empty() )
+		return false;
+
+	if( sPath.find( ':' ) != std::string::npos )
+		return true;
+
+	if( sPath[0] == '\\' || sPath[0] == '/' )
+		return true;
+
+	return false;
+}
+
+
+/** Makes an absolute path from a relative path and a base path */
+std::string Path_MakeAbsolute( const std::string & sRelativePath, const std::string & sBasePath, char slash )
+{
+	if( slash == 0 )
+		slash = Path_GetSlash();
+
+	if( Path_IsAbsolute( sRelativePath ) )
+		return sRelativePath;
+	else
+	{
+		if( !Path_IsAbsolute( sBasePath ) )
+			return "";
+
+		std::string sCompacted = Path_Compact( Path_Join( sBasePath, sRelativePath, slash ), slash );
+		if( Path_IsAbsolute( sCompacted ) )
+			return sCompacted;
+		else
+			return "";
+	}
+}
+
+
+/** Fixes the directory separators for the current platform */
+std::string Path_FixSlashes( const std::string & sPath, char slash )
+{
+	if( slash == 0 )
+		slash = Path_GetSlash();
+
+	std::string sFixed = sPath;
+	for( std::string::iterator i = sFixed.begin(); i != sFixed.end(); i++ )
+	{
+		if( *i == '/' || *i == '\\' )
+			*i = slash;
+	}
+
+	return sFixed;
+}
+
+
+char Path_GetSlash()
+{
+#if defined(_WIN32)
+	return '\\';
+#else
+	return '/';
+#endif
+}
+
+/** Jams two paths together with the right kind of slash */
+std::string Path_Join( const std::string & first, const std::string & second, char slash )
+{
+	if( slash == 0 )
+		slash = Path_GetSlash();
+
+	// only insert a slash if we don't already have one
+	std::string::size_type nLen = first.length();
+#if defined(_WIN32)
+	if( first.back() == '\\' || first.back() == '/' )
+	    nLen--;
+#else
+	char last_char = first[first.length()-1];
+	if (last_char == '\\' || last_char == '/')
+	    nLen--;
+#endif
+
+	return first.substr( 0, nLen ) + std::string( 1, slash ) + second;
+}
+
+
+std::string Path_Join( const std::string & first, const std::string & second, const std::string & third, char slash )
+{
+	return Path_Join( Path_Join( first, second, slash ), third, slash );
+}
+
+std::string Path_Join( const std::string & first, const std::string & second, const std::string & third, const std::string &fourth, char slash )
+{
+	return Path_Join( Path_Join( Path_Join( first, second, slash ), third, slash ), fourth, slash );
+}
+
+std::string Path_Join( 
+	const std::string & first, 
+	const std::string & second, 
+	const std::string & third, 
+	const std::string & fourth, 
+	const std::string & fifth, 
+	char slash )
+{
+	return Path_Join( Path_Join( Path_Join( Path_Join( first, second, slash ), third, slash ), fourth, slash ), fifth, slash );
+}
+
+/** Removes redundant <dir>/.. elements in the path. Returns an empty path if the 
+* specified path has a broken number of directories for its number of ..s */
+std::string Path_Compact( const std::string & sRawPath, char slash )
+{
+	if( slash == 0 )
+		slash = Path_GetSlash();
+
+	std::string sPath = Path_FixSlashes( sRawPath, slash );
+	std::string sSlashString( 1, slash );
+
+	// strip out all /./
+	for( std::string::size_type i = 0; (i + 3) < sPath.length();  )
+	{
+		if( sPath[ i ] == slash && sPath[ i+1 ] == '.' && sPath[ i+2 ] == slash )
+		{
+			sPath.replace( i, 3, sSlashString );
+		}
+		else
+		{
+			++i;
+		}
+	}
+
+
+	// get rid of trailing /. but leave the path separator
+	if( sPath.length() > 2 )
+	{
+		std::string::size_type len = sPath.length();
+		if( sPath[ len-1 ] == '.'  && sPath[ len-2 ] == slash )
+		{
+		  // sPath.pop_back();
+		  sPath[len-1] = 0;  // for now, at least
+		}
+	}
+
+	// get rid of leading ./ 
+	if( sPath.length() > 2 )
+	{
+		if( sPath[ 0 ] == '.'  && sPath[ 1 ] == slash )
+		{
+			sPath.replace( 0, 2, "" );
+		}
+	}
+
+	// each time we encounter .. back up until we've found the previous directory name
+	// then get rid of both
+	std::string::size_type i = 0;
+	while( i < sPath.length() )
+	{
+		if( i > 0 && sPath.length() - i >= 2 
+			&& sPath[i] == '.'
+			&& sPath[i+1] == '.'
+			&& ( i + 2 == sPath.length() || sPath[ i+2 ] == slash )
+			&& sPath[ i-1 ] == slash )
+		{
+			// check if we've hit the start of the string and have a bogus path
+			if( i == 1 )
+				return "";
+			
+			// find the separator before i-1
+			std::string::size_type iDirStart = i-2;
+			while( iDirStart > 0 && sPath[ iDirStart - 1 ] != slash )
+				--iDirStart;
+
+			// remove everything from iDirStart to i+2
+			sPath.replace( iDirStart, (i - iDirStart) + 3, "" );
+
+			// start over
+			i = 0;
+		}
+		else
+		{
+			++i;
+		}
+	}
+
+	return sPath;
+}
+
+#define MAX_UNICODE_PATH			32768
+#define MAX_UNICODE_PATH_IN_UTF8	( MAX_UNICODE_PATH * 4 )
+
+/** Returns the path to the current DLL or exe */
+std::string GetThisModulePath()
+{
+	// gets the path of vrclient.dll itself
+#ifdef WIN32
+	HMODULE hmodule = NULL;
+
+	::GetModuleHandleEx(GET_MODULE_HANDLE_EX_FLAG_FROM_ADDRESS | GET_MODULE_HANDLE_EX_FLAG_UNCHANGED_REFCOUNT, reinterpret_cast<LPCTSTR>(GetThisModulePath), &hmodule);
+
+	wchar_t *pwchPath = new wchar_t[MAX_UNICODE_PATH];
+	char *pchPath = new char[ MAX_UNICODE_PATH_IN_UTF8 ];
+	::GetModuleFileNameW( hmodule, pwchPath, MAX_UNICODE_PATH );
+	WideCharToMultiByte( CP_UTF8, 0, pwchPath, -1, pchPath, MAX_UNICODE_PATH_IN_UTF8, NULL, NULL );
+	delete[] pwchPath;
+
+	std::string sPath = pchPath;
+	delete [] pchPath;
+	return sPath;
+
+#elif defined( OSX ) || defined( LINUX )
+	// get the addr of a function in vrclient.so and then ask the dlopen system about it
+	Dl_info info;
+	dladdr( (void *)GetThisModulePath, &info );
+	return info.dli_fname;
+#endif
+
+}
+
+
+/** returns true if the specified path exists and is a directory */
+bool Path_IsDirectory( const std::string & sPath )
+{
+	std::string sFixedPath = Path_FixSlashes( sPath );
+	if( sFixedPath.empty() )
+		return false;
+	char cLast = sFixedPath[ sFixedPath.length() - 1 ];
+	if( cLast == '/' || cLast == '\\' )
+		sFixedPath.erase( sFixedPath.end() - 1, sFixedPath.end() );
+
+	// see if the specified path actually exists.
+	struct	stat	buf;
+	if ( stat ( sFixedPath.c_str(), &buf ) == -1)
+	{
+		return false;
+	}
+
+#if defined(LINUX)
+	return S_ISDIR( buf.st_mode );
+#else
+	return ( buf.st_mode & _S_IFDIR ) != 0;
+#endif
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: returns true if the the path exists
+//-----------------------------------------------------------------------------
+bool Path_Exists( const std::string & sPath )
+{
+	std::string sFixedPath = Path_FixSlashes( sPath );
+	if( sFixedPath.empty() )
+		return false;
+
+	struct stat buf;
+	if ( stat ( sFixedPath.c_str(), &buf ) == -1)
+	{
+		return false;
+	}
+
+	return true;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: helper to find a directory upstream from a given path
+//-----------------------------------------------------------------------------
+std::string Path_FindParentDirectoryRecursively( const std::string &strStartDirectory, const std::string &strDirectoryName )
+{
+	std::string strFoundPath = "";
+	std::string strCurrentPath = Path_FixSlashes( strStartDirectory );
+	if ( strCurrentPath.length() == 0 )
+		return "";
+
+	bool bExists = Path_Exists( strCurrentPath );
+	std::string strCurrentDirectoryName = Path_StripDirectory( strCurrentPath );
+	if ( bExists && stricmp( strCurrentDirectoryName.c_str(), strDirectoryName.c_str() ) == 0 )
+		return strCurrentPath;
+
+	while( bExists && strCurrentPath.length() != 0 )
+	{
+		strCurrentPath = Path_StripFilename( strCurrentPath );
+		strCurrentDirectoryName = Path_StripDirectory( strCurrentPath );
+		bExists = Path_Exists( strCurrentPath );
+		if ( bExists && stricmp( strCurrentDirectoryName.c_str(), strDirectoryName.c_str() ) == 0 )
+			return strCurrentPath;
+	}
+
+	return "";
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: helper to find a subdirectory upstream from a given path
+//-----------------------------------------------------------------------------
+std::string Path_FindParentSubDirectoryRecursively( const std::string &strStartDirectory, const std::string &strDirectoryName )
+{
+	std::string strFoundPath = "";
+	std::string strCurrentPath = Path_FixSlashes( strStartDirectory );
+	if ( strCurrentPath.length() == 0 )
+		return "";
+
+	bool bExists = Path_Exists( strCurrentPath );
+	while( bExists && strCurrentPath.length() != 0 )
+	{
+		strCurrentPath = Path_StripFilename( strCurrentPath );
+		bExists = Path_Exists( strCurrentPath );
+
+		if( Path_Exists( Path_Join( strCurrentPath, strDirectoryName ) ) )
+		{
+			strFoundPath = Path_Join( strCurrentPath, strDirectoryName );
+			break;
+		}
+	}
+	return strFoundPath;
+}
+
+
+//-----------------------------------------------------------------------------
+// Purpose: reading and writing files in the vortex directory
+//-----------------------------------------------------------------------------
+unsigned char * Path_ReadBinaryFile( const std::string &strFilename, int *pSize )
+{
+	FILE *f;
+#if defined( POSIX )
+	f = fopen( strFilename.c_str(), "rb" );
+#else
+	errno_t err = fopen_s(&f, strFilename.c_str(), "rb");
+	if ( err != 0 )
+	{
+		f = NULL;
+	}
+#endif
+	
+	unsigned char* buf = NULL;
+
+	if ( f != NULL )
+	{
+		fseek(f, 0, SEEK_END);
+		int size = ftell(f);
+		fseek(f, 0, SEEK_SET);
+
+		buf = new unsigned char[size];
+		if (buf && fread(buf, size, 1, f) == 1)
+		{
+			if (pSize)
+				*pSize = size;
+		}
+		else
+		{
+			delete[] buf;
+			buf = 0;
+		}
+
+		fclose(f);
+	}
+
+	return buf;
+}
+
+
+std::string Path_ReadTextFile( const std::string &strFilename )
+{
+	// doing it this way seems backwards, but I don't
+	// see an easy way to do this with C/C++ style IO
+	// that isn't worse...
+	int size;
+	unsigned char* buf = Path_ReadBinaryFile( strFilename, &size );
+	if (!buf)
+		return "";
+
+	// convert CRLF -> LF
+	int outsize = 1;
+	for (int i=1; i < size; i++)
+	{
+		if (buf[i] == '\n' && buf[i-1] == '\r') // CRLF
+			buf[outsize-1] = '\n'; // ->LF
+		else
+			buf[outsize++] = buf[i]; // just copy
+	}
+
+	std::string ret((char *)buf, (char *)(buf + outsize));
+	delete[] buf;
+	return ret;
+}
+
+
+bool Path_WriteStringToTextFile( const std::string &strFilename, const char *pchData )
+{
+	FILE *f;
+#if defined( POSIX )
+	f = fopen( strFilename.c_str(), "w" );
+#else
+	errno_t err = fopen_s(&f, strFilename.c_str(), "w");
+	if ( err != 0 )
+	{
+		f = NULL;
+	}
+#endif
+	
+	bool ok = false;
+
+	if ( f != NULL )
+	{
+		ok = fputs( pchData, f) >= 0;
+		fclose(f);
+	}
+
+	return ok;
+}

examples/ThirdPartyLibs/openvr/samples/shared/pathtools.h

@@ -0,0 +1,98 @@
+//========= Copyright Valve Corporation ============//
+#pragma once
+
+#include <string>
+
+/** Returns the path (including filename) to the current executable */
+std::string Path_GetExecutablePath();
+
+/** Returns the path of the current working directory */
+std::string Path_GetWorkingDirectory();
+
+/** Sets the path of the current working directory. Returns true if this was successful. */
+bool Path_SetWorkingDirectory( const std::string & sPath );
+
+/** returns the path (including filename) of the current shared lib or DLL */
+std::string Path_GetModulePath();
+
+/** Returns the specified path without its filename.
+* If slash is unspecified the native path separator of the current platform
+* will be used. */
+std::string Path_StripFilename( const std::string & sPath, char slash = 0 );
+
+/** returns just the filename from the provided full or relative path. */
+std::string Path_StripDirectory( const std::string & sPath, char slash = 0 );
+
+/** returns just the filename with no extension of the provided filename. 
+* If there is a path the path is left intact. */
+std::string Path_StripExtension( const std::string & sPath );
+
+/** Returns true if the path is absolute */
+bool Path_IsAbsolute( const std::string & sPath );
+
+/** Makes an absolute path from a relative path and a base path */
+std::string Path_MakeAbsolute( const std::string & sRelativePath, const std::string & sBasePath, char slash = 0 );
+
+/** Fixes the directory separators for the current platform.
+* If slash is unspecified the native path separator of the current platform
+* will be used. */
+std::string Path_FixSlashes( const std::string & sPath, char slash = 0 );
+
+/** Returns the path separator for the current platform */
+char Path_GetSlash();
+
+/** Jams two paths together with the right kind of slash */
+std::string Path_Join( const std::string & first, const std::string & second, char slash = 0 );
+std::string Path_Join( const std::string & first, const std::string & second, const std::string & third, char slash = 0 );
+std::string Path_Join( const std::string & first, const std::string & second, const std::string & third, const std::string &fourth, char slash = 0 );
+std::string Path_Join( 
+	const std::string & first, 
+	const std::string & second, 
+	const std::string & third, 
+	const std::string & fourth, 
+	const std::string & fifth, 
+	char slash = 0 );
+
+
+/** Removes redundant <dir>/.. elements in the path. Returns an empty path if the 
+* specified path has a broken number of directories for its number of ..s.
+* If slash is unspecified the native path separator of the current platform
+* will be used. */
+std::string Path_Compact( const std::string & sRawPath, char slash = 0 );
+
+/** returns true if the specified path exists and is a directory */
+bool Path_IsDirectory( const std::string & sPath );
+
+/** Returns the path to the current DLL or exe */
+std::string GetThisModulePath();
+
+/** returns true if the the path exists */
+bool Path_Exists( const std::string & sPath );
+
+/** Helper functions to find parent directories or subdirectories of parent directories */
+std::string Path_FindParentDirectoryRecursively( const std::string &strStartDirectory, const std::string &strDirectoryName );
+std::string Path_FindParentSubDirectoryRecursively( const std::string &strStartDirectory, const std::string &strDirectoryName );
+
+/** Path operations to read or write text/binary files */
+unsigned char * Path_ReadBinaryFile( const std::string &strFilename, int *pSize );
+std::string Path_ReadTextFile( const std::string &strFilename );
+bool Path_WriteStringToTextFile( const std::string &strFilename, const char *pchData );
+
+//-----------------------------------------------------------------------------
+#if defined(_WIN32)
+#define DYNAMIC_LIB_EXT	".dll"
+#ifdef _WIN64
+#define PLATSUBDIR	"win64"
+#else
+#define PLATSUBDIR	"win32"
+#endif
+#elif defined(OSX)
+#define DYNAMIC_LIB_EXT	".dylib"
+#define PLATSUBDIR	"osx32"
+#elif defined(LINUX)
+#define DYNAMIC_LIB_EXT	".so"
+#define PLATSUBDIR	"linux32"
+#else
+#warning "Unknown platform for PLATSUBDIR"
+#define PLATSUBDIR	"unknown_platform"
+#endif

examples/Utils/b3Clock.cpp

@@ -166,7 +166,7 @@ unsigned long int b3Clock::getTimeMilliseconds()
 
 	/// Returns the time in us since the last call to reset or since 
 	/// the Clock was created.
-unsigned long int b3Clock::getTimeMicroseconds()
+unsigned long long int b3Clock::getTimeMicroseconds()
 {
 #ifdef B3_USE_WINDOWS_TIMERS
 		LARGE_INTEGER currentTime;
@@ -175,14 +175,14 @@ unsigned long int b3Clock::getTimeMicroseconds()
 			m_data->mStartTime.QuadPart;
 
 		// Compute the number of millisecond ticks elapsed.
-		unsigned long msecTicks = (unsigned long)(1000 * elapsedTime / 
+		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 elapsedTicks = GetTickCount() - m_data->mStartTick;
+		unsigned long long elapsedTicks = GetTickCount() - m_data->mStartTick;
-		signed long msecOff = (signed long)(msecTicks - elapsedTicks);
+		signed long long msecOff = (signed long)(msecTicks - elapsedTicks);
 		if (msecOff < -100 || msecOff > 100)
 		{
 			// Adjust the starting time forwards.
@@ -197,7 +197,7 @@ unsigned long int b3Clock::getTimeMicroseconds()
 		m_data->mPrevElapsedTime = elapsedTime;
 
 		// Convert to microseconds.
-		unsigned long usecTicks = (unsigned long)(1000000 * elapsedTime / 
+		unsigned long long usecTicks = (unsigned long)(1000000 * elapsedTime / 
 			m_data->mClockFrequency.QuadPart);
 
 		return usecTicks;
@@ -222,3 +222,8 @@ unsigned long int b3Clock::getTimeMicroseconds()
 #endif 
 }
 
+double b3Clock::getTimeInSeconds()
+{
+	return double(getTimeMicroseconds()/1.e6);
+}
+

examples/Utils/b3Clock.h

@@ -22,7 +22,12 @@ public:
 
 	/// 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();
+
+	/// Returns the time in seconds since the last call to reset or since 
+	/// the Clock was created.
+	double getTimeInSeconds();
+
 private:
 	struct b3ClockData* m_data;
 };

examples/Utils/b3ResourcePath.cpp

@@ -4,7 +4,7 @@
 #include <mach-o/dyld.h>	/* _NSGetExecutablePath */
 #else
 #ifdef _WIN32
-#include <Windows.h>
+#include <windows.h>
 #else
 //not Mac, not Windows, let's cross the fingers it is Linux :-)
 #include <unistd.h>

examples/Vehicles/Hinge2Vehicle.cpp

@@ -568,11 +568,6 @@ void Hinge2Vehicle::renderScene()
 	
 	m_guiHelper->render(m_dynamicsWorld);
 
-	
-
-	ATTRIBUTE_ALIGNED16(btScalar) m[16];
-	int i;
-
 	btVector3 wheelColor(1,0,0);
 
 	btVector3	worldBoundsMin,worldBoundsMax;

examples/pybullet/pybullet.c

@@ -172,6 +172,8 @@ pybullet_loadURDF(PyObject* self, PyObject* args)
 			&startOrnX,&startOrnY,&startOrnZ, &startOrnW))
                 return NULL;
 	}
+	
+		if (strlen(urdfFileName))
     {
       // printf("(%f, %f, %f) (%f, %f, %f, %f)\n", startPosX,startPosY,startPosZ,startOrnX, startOrnY,startOrnZ, startOrnW);
         
@@ -190,6 +192,10 @@ pybullet_loadURDF(PyObject* self, PyObject* args)
 					return NULL;
 				}
         bodyIndex = b3GetStatusBodyIndex(statusHandle);
+    } else
+    	{
+    		PyErr_SetString(SpamError, "Empty filename, method expects 1, 4 or 8 arguments.");
+				return NULL;
     	}
 	return PyLong_FromLong(bodyIndex);
 }
@@ -368,8 +374,8 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
         {
             case CONTROL_MODE_VELOCITY:
             {
-                b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetValue);
 				double kd = gains;
+                b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetValue);
                 b3JointControlSetKd(commandHandle,info.m_uIndex,kd);
                 b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,maxForce);
                 break;
@@ -383,8 +389,8 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
                     
             case CONTROL_MODE_POSITION_VELOCITY_PD:
             {
-                b3JointControlSetDesiredPosition( commandHandle, info.m_qIndex, targetValue);
 				double kp = gains;
+                b3JointControlSetDesiredPosition( commandHandle, info.m_qIndex, targetValue);
                 b3JointControlSetKp(commandHandle,info.m_uIndex,kp);
                 b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,maxForce);
                 break;
@@ -942,6 +948,32 @@ static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16])
     return 0;
 }
 
+// internal function to set a float vector[3]
+// used to initialize camera position with
+// a view and projection matrix in renderImage()
+//
+// // Args:
+//  matrix - float[16] which will be set by values from objMat
+static int pybullet_internalSetVector(PyObject* objMat, float vector[3])
+{
+    int i, len;
+    PyObject* seq;
+
+    seq = PySequence_Fast(objMat, "expected a sequence");
+    len = PySequence_Size(objMat);
+    if (len==3)
+    {
+        for (i = 0; i < len; i++)
+        {
+            vector[i] = pybullet_internalGetFloatFromSequence(seq,i);
+        }
+        Py_DECREF(seq);
+        return 1;
+    }
+    Py_DECREF(seq);
+    return 0;
+}
+
 // Render an image from the current timestep of the simulation
 //
 // Examples:
@@ -961,11 +993,22 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
   ///request an image from a simulated camera, using a software renderer.
   struct b3CameraImageData imageData;
   PyObject* objViewMat,* objProjMat;
+  PyObject* objCameraPos,*objTargetPos,* objCameraUp;
+
   int width, height;
   int size= PySequence_Size(args);
   float viewMatrix[16];
   float projectionMatrix[16];
   
+  float cameraPos[3];
+  float targetPos[3];
+  float cameraUp[3];
+
+  float left, right, bottom, top, aspect;
+  float nearVal, farVal;
+  // float nearVal = .001;
+  // float farVal = 1000;
+
   // inialize cmd
   b3SharedMemoryCommandHandle command;
   
@@ -1001,6 +1044,47 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
        }
    }
 
+   if (size==7) // set camera resoluation and view and projection matrix
+   {
+     if (PyArg_ParseTuple(args, "iiOOOii", &width, &height, &objCameraPos, &objTargetPos, &objCameraUp, &nearVal, &farVal))
+     {
+       b3RequestCameraImageSetPixelResolution(command,width,height);
+       if (pybullet_internalSetVector(objCameraPos, cameraPos) &&
+           pybullet_internalSetVector(objTargetPos, targetPos) &&
+           pybullet_internalSetVector(objCameraUp, cameraUp))
+           {
+              // printf("\ncamera pos:\n");
+              //   for(int i =0;i<3; i++) {
+              //     printf(" %f", cameraPos[i]);
+              //   }
+              // 
+              //   printf("\ntargetPos pos:\n");
+              //     for(int i =0;i<3; i++) {
+              //       printf(" %f", targetPos[i]);
+              //     }
+              // 
+              //   printf("\ncameraUp pos:\n");
+              //     for(int i =0;i<3; i++) {
+              //       printf(" %f", cameraUp[i]);
+              //     }
+              //     printf("\n");
+                b3RequestCameraImageSetViewMatrix(command, cameraPos, targetPos, cameraUp);
+                // printf("\n");
+
+           }
+           
+           aspect = width/height;
+           left = -aspect * nearVal;
+           right = aspect * nearVal;
+           bottom = -nearVal;
+           top = nearVal;
+           
+           b3RequestCameraImageSetProjectionMatrix(command, left, right, bottom, top, nearVal, farVal);
+          //  printf("\n");
+
+      }
+    }
+    
 	if (b3CanSubmitCommand(sm))
 	{
 		b3SharedMemoryStatusHandle statusHandle;
@@ -1186,10 +1270,12 @@ static PyObject* pybullet_applyExternalTorque(PyObject* self, PyObject* args)
                 PyErr_SetString(SpamError, "flag has to be either WORLD_FRAME or LINK_FRAME");
                 return NULL;
             }
+			{
 				b3SharedMemoryStatusHandle statusHandle;
 				b3SharedMemoryCommandHandle command = b3ApplyExternalForceCommandInit(sm);
 				b3ApplyExternalTorque(command,objectUniqueId,-1,torque, flags);
 				statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+			}
 
         }
     }
@@ -1305,12 +1391,13 @@ static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, PyObject* args)
 
     {
         double rpy[3];
+        double sarg;
 		sqx = quat[0] * quat[0];
         sqy = quat[1] * quat[1];
         sqz = quat[2] * quat[2];
         squ = quat[3] * quat[3];
         rpy[0] = atan2(2 * (quat[1]*quat[2] + quat[3]*quat[0]), squ - sqx - sqy + sqz);
-        double sarg = -2 * (quat[0]*quat[2] - quat[3] * quat[1]);
+        sarg = -2 * (quat[0]*quat[2] - quat[3] * quat[1]);
         rpy[1] = sarg <= -1.0 ? -0.5*3.141592538 : (sarg >= 1.0 ? 0.5*3.141592538 : asin(sarg));
         rpy[2] = atan2(2 * (quat[0]*quat[1] + quat[3]*quat[2]), squ + sqx - sqy - sqz);
         {
@@ -1381,7 +1468,7 @@ static PyMethodDef SpamMethods[] = {
         "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"},
     
 	{"renderImage", pybullet_renderImage, METH_VARARGS,
-	"Render an image (given the pixel resolution width & height and camera view & projection matrices), and return the 8-8-8bit RGB pixel data and floating point depth values"},	
+	"Render an image (given the pixel resolution width, height, camera view matrix, projection matrix, near, and far values), and return the 8-8-8bit RGB pixel data and floating point depth values"},	
 
     {"getQuaternionFromEuler", pybullet_getQuaternionFromEuler, METH_VARARGS,
         "Convert Euler [roll, pitch, yaw] as in URDF/SDF convention, to quaternion [x,y,z,w]"},
@@ -1397,6 +1484,10 @@ static PyMethodDef SpamMethods[] = {
     //collision info
     //raycast info
         
+    //applyBaseForce
+    //applyLinkForce
+     
+
     {NULL, NULL, 0, NULL}        /* Sentinel */
 };
 

src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h

@@ -965,8 +965,6 @@ inline void		b3DynamicBvh::rayTestInternal(	const b3DbvtNode* root,
 	B3_DBVT_CHECKTYPE
 	if(root)
 	{
-		b3Vector3 resultNormal;
-
 		int								depth=1;
 		int								treshold=B3_DOUBLE_STACKSIZE-2;
 		b3AlignedObjectArray<const b3DbvtNode*>&	stack = m_rayTestStack;

src/Bullet3OpenCL/NarrowphaseCollision/b3ConvexHullContact.cpp

@@ -692,8 +692,6 @@ static bool findSeparatingAxis(	const b3ConvexPolyhedronData& hullA, const b3Con
 		}
 	}
 
-	b3Vector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
-
 	int curEdgeEdge = 0;
 	// Test edges
 	for(int e0=0;e0<hullA.m_numUniqueEdges;e0++)
@@ -1292,7 +1290,6 @@ int clipHullHullSingle(
 			B3_PROFILE("overlap");
 
 			float4 normalOnSurfaceB = (float4&)hostNormal;
-			float4 centerOut;
 			
 			b3Int4 contactIdx;
 			contactIdx.x = 0;
@@ -2777,8 +2774,6 @@ int computeContactConvexConvex2(
     hullB = convexShapes[colB.m_shapeIndex];
     //printf("numvertsB = %d\n",hullB.m_numVertices);
 
-	
-	float4 contactsOut[MAX_VERTS];
 	int contactCapacity = MAX_VERTS;
 	int numContactsOut=0;
 

src/Bullet3OpenCL/NarrowphaseCollision/b3QuantizedBvh.cpp

@@ -619,8 +619,8 @@ void	b3QuantizedBvh::walkStacklessQuantizedTreeAgainstRay(b3NodeOverlapCallback*
 			/* Add box cast extents */
 			bounds[0] -= aabbMax;
 			bounds[1] -= aabbMin;
-			b3Vector3 normal;
 #if 0
+			b3Vector3 normal;
 			bool ra2 = b3RayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0, lambda_max);
 			bool ra = b3RayAabb (raySource, rayTarget, bounds[0], bounds[1], param, normal);
 			if (ra2 != ra)

src/Bullet3Serialize/Bullet2FileLoader/b3DNA.cpp

@@ -389,15 +389,7 @@ void bDNA::init(char *data, int len, bool swap)
 	}
 
 	
-	{
+	cp = b3AlignPointer(cp,4);
-		nr= (long)cp;
-	//long mask=3;
-		nr= ((nr+3)&~3)-nr;
-		while (nr--)
-		{
-			cp++;
-		}
-	}
 	
 
 	/*
@@ -425,16 +417,8 @@ void bDNA::init(char *data, int len, bool swap)
 		cp++;
 	}
 
-{
-		nr= (long)cp;
-	//	long mask=3;
-		nr= ((nr+3)&~3)-nr;
-		while (nr--)
-		{
-			cp++;
-		}
-	}
 
+	cp = b3AlignPointer(cp,4);
 
 	/*
 		TLEN (4 bytes)

src/Bullet3Serialize/Bullet2FileLoader/b3File.cpp

@@ -428,16 +428,7 @@ void bFile::swapDNA(char* ptr)
 	}
 
 
-	{
+	cp = b3AlignPointer(cp,4);
-		nr= (long)cp;
-	//long mask=3;
-		nr= ((nr+3)&~3)-nr;
-		while (nr--)
-		{
-			cp++;
-		}
-	}
-
 
 	/*
 		TYPE (4 bytes)
@@ -465,16 +456,7 @@ void bFile::swapDNA(char* ptr)
 		cp++;
 	}
 
-{
+	cp = b3AlignPointer(cp,4);
-		nr= (long)cp;
-	//	long mask=3;
-		nr= ((nr+3)&~3)-nr;
-		while (nr--)
-		{
-			cp++;
-		}
-	}
-
 
 	/*
 		TLEN (4 bytes)