Merge remote-tracking branch 'upstream/master'

CMakeLists.txt

@@ -28,7 +28,15 @@ OPTION(USE_DOUBLE_PRECISION "Use double precision"	OFF)
 OPTION(USE_GRAPHICAL_BENCHMARK "Use Graphical Benchmark" ON)
 OPTION(BUILD_SHARED_LIBS "Use shared libraries" OFF)
 OPTION(USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD "Use btSoftMultiBodyDynamicsWorld" OFF)
-OPTION(BULLET2_USE_THREAD_LOCKS "Build Bullet 2 libraries with mutex locking around certain operations" OFF)
+
+OPTION(BULLET2_USE_THREAD_LOCKS "Build Bullet 2 libraries with mutex locking around certain operations (required for multi-threading)" OFF)
+IF (BULLET2_USE_THREAD_LOCKS)
+    OPTION(BULLET2_USE_OPEN_MP_MULTITHREADING "Build Bullet 2 with support for multi-threading with OpenMP (requires a compiler with OpenMP support)" OFF)
+    OPTION(BULLET2_USE_TBB_MULTITHREADING "Build Bullet 2 with support for multi-threading with Intel Threading Building Blocks (requires the TBB library to be already installed)" OFF)
+    IF (MSVC)
+        OPTION(BULLET2_USE_PPL_MULTITHREADING "Build Bullet 2 with support for multi-threading with Microsoft Parallel Patterns Library (requires MSVC compiler)" OFF)
+    ENDIF (MSVC)
+ENDIF (BULLET2_USE_THREAD_LOCKS)
 
 OPTION(USE_MSVC_INCREMENTAL_LINKING "Use MSVC Incremental Linking" OFF)
 OPTION(USE_CUSTOM_VECTOR_MATH "Use custom vectormath library" OFF)
@@ -208,6 +216,30 @@ IF(BULLET2_USE_THREAD_LOCKS)
 	ENDIF (NOT MSVC)
 ENDIF (BULLET2_USE_THREAD_LOCKS)
 
+IF (BULLET2_USE_OPEN_MP_MULTITHREADING)
+    ADD_DEFINITIONS("-DBT_USE_OPENMP=1")
+    IF (MSVC)
+        SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /openmp")
+    ELSE (MSVC)
+        # GCC, Clang
+        SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp")
+    ENDIF (MSVC)
+ENDIF (BULLET2_USE_OPEN_MP_MULTITHREADING)
+
+IF (BULLET2_USE_TBB_MULTITHREADING)
+    SET (BULLET2_TBB_INCLUDE_DIR "not found" CACHE PATH "Directory for Intel TBB includes.")
+    SET (BULLET2_TBB_LIB_DIR "not found" CACHE PATH "Directory for Intel TBB libraries.")
+    find_library(TBB_LIBRARY tbb PATHS ${BULLET2_TBB_LIB_DIR})
+    find_library(TBBMALLOC_LIBRARY tbbmalloc PATHS ${BULLET2_TBB_LIB_DIR})
+    ADD_DEFINITIONS("-DBT_USE_TBB=1")
+    INCLUDE_DIRECTORIES( ${BULLET2_TBB_INCLUDE_DIR} )
+    LINK_LIBRARIES( ${TBB_LIBRARY} ${TBBMALLOC_LIBRARY} )
+ENDIF (BULLET2_USE_TBB_MULTITHREADING)
+
+IF (BULLET2_USE_PPL_MULTITHREADING)
+    ADD_DEFINITIONS("-DBT_USE_PPL=1")
+ENDIF (BULLET2_USE_PPL_MULTITHREADING)
+
 IF (WIN32)
 OPTION(USE_GLUT "Use Glut"	ON)
 ADD_DEFINITIONS( -D_CRT_SECURE_NO_WARNINGS )
@@ -350,14 +382,6 @@ IF(BUILD_BULLET2_DEMOS)
 		SUBDIRS(examples)
 	ENDIF()
 
-        IF (BULLET2_USE_THREAD_LOCKS)
-            OPTION(BULLET2_MULTITHREADED_OPEN_MP_DEMO "Build Bullet 2 MultithreadedDemo using OpenMP (requires a compiler with OpenMP support)" OFF)
-            OPTION(BULLET2_MULTITHREADED_TBB_DEMO "Build Bullet 2 MultithreadedDemo using Intel Threading Building Blocks (requires the TBB library to be already installed)" OFF)
-            IF (MSVC)
-                OPTION(BULLET2_MULTITHREADED_PPL_DEMO "Build Bullet 2 MultithreadedDemo using Microsoft Parallel Patterns Library (requires MSVC compiler)" OFF)
-            ENDIF (MSVC)
-        ENDIF (BULLET2_USE_THREAD_LOCKS)
-
 ENDIF(BUILD_BULLET2_DEMOS)
 
 

Extras/obj2sdf/obj2sdf.cpp

@@ -16,6 +16,24 @@
 
 #define MAX_PATH_LEN 1024
 
+std::string 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;
+}
+
 int main(int argc, char* argv[])
 {
 
@@ -23,6 +41,8 @@ int main(int argc, char* argv[])
 	char* fileName;
 	args.GetCmdLineArgument("fileName",fileName);
 
+	std::string matLibName = StripExtension(fileName);
+
 	printf("fileName = %s\n", fileName);
 	if (fileName==0)
 	{
@@ -58,7 +78,13 @@ int main(int argc, char* argv[])
 		}
 
 		char objFileName[MAX_PATH_LEN];
-		sprintf(objFileName,"%s/part%d.obj",materialPrefixPath,s);
+		if (strlen(materialPrefixPath)>0)
+		{
+			sprintf(objFileName,"%s/part%d.obj",materialPrefixPath,s);
+		} else
+		{
+			sprintf(objFileName,"part%d.obj",s);
+		}
 		FILE* f = fopen(objFileName,"w");
 		if (f==0)
 		{
@@ -66,7 +92,15 @@ int main(int argc, char* argv[])
 			exit(0);
 		}
 		fprintf(f,"# Exported using automatic converter by Erwin Coumans\n");
-		fprintf(f,"mtllib bedroom.mtl\n");
+		if (matLibName.length())
+		{
+			fprintf(f,"mtllib %s.mtl\n", matLibName.c_str());
+		} else
+		{
+			fprintf(f,"mtllib bedroom.mtl\n");
+
+		}
+
 
 		int faceCount = shape.mesh.indices.size();
 		int vertexCount = shape.mesh.positions.size();

data/kitchens/1.sdf

@@ -4291,45 +4291,7 @@
 			 </visual>
 			 </link>
 			</model>
-		<model name='part110.obj'>
+	
-			<static>1</static>
-			<pose frame=''>-12.0 -13.9 0 0 0 0</pose>
-			<link name='link_d110'>
-			<inertial>
-			<mass>0</mass>
-			<inertia>
-			<ixx>0.166667</ixx>
-			<ixy>0</ixy>
-			<ixz>0</ixz>
-			<iyy>0.166667</iyy>
-			<iyz>0</iyz>
-			<izz>0.166667</izz>
-			</inertia>
-			</inertial>
-			<collision concave='yes' name='collision_110'>
-			<geometry>
-			<mesh>
-			<scale>.1 .1 .1</scale>
-				<uri>fatihrmutfak/part110.obj</uri>
-			</mesh>
-			</geometry>
-			  </collision>
-			<visual name='visual'>
-				<geometry>
-				<mesh>
-					<scale>.1 .1 .1</scale>
-					<uri>fatihrmutfak/part110.obj</uri>
-				</mesh>
-				</geometry>
-			<material>
-				<ambient>1 0 0 1</ambient>
-				<diffuse>1.000000 1.000000 1.000000 1</diffuse>
-				<specular>0.1 0.1 0.1 1</specular>
-				<emissive>0 0 0 0</emissive>
-			 </material>
-			 </visual>
-			 </link>
-			</model>
 		<model name='part111.obj'>
 			<static>1</static>
 			<pose frame=''>-12.0 -13.9 0 0 0 0</pose>

data/kuka_iiwa/meshes/link_0.mtl

@@ -0,0 +1,10 @@
+# Blender MTL File: 'None'
+# Material Count: 1
+
+newmtl None
+Ns 0
+Ka 0.000000 0.000000 0.000000
+Kd 0.8 0.8 0.8
+Ks 0.8 0.8 0.8
+d 1
+illum 2

data/kuka_iiwa/meshes/link_1.mtl

@@ -0,0 +1,10 @@
+# Blender MTL File: 'None'
+# Material Count: 1
+
+newmtl None
+Ns 0
+Ka 0.000000 0.000000 0.000000
+Kd 0.8 0.8 0.8
+Ks 0.8 0.8 0.8
+d 1
+illum 2

data/kuka_iiwa/meshes/link_2.mtl

@@ -0,0 +1,10 @@
+# Blender MTL File: 'None'
+# Material Count: 1
+
+newmtl None
+Ns 0
+Ka 0.000000 0.000000 0.000000
+Kd 0.8 0.8 0.8
+Ks 0.8 0.8 0.8
+d 1
+illum 2

data/kuka_iiwa/meshes/link_3.mtl

@@ -0,0 +1,10 @@
+# Blender MTL File: 'None'
+# Material Count: 1
+
+newmtl None
+Ns 0
+Ka 0.000000 0.000000 0.000000
+Kd 0.8 0.8 0.8
+Ks 0.8 0.8 0.8
+d 1
+illum 2

data/kuka_iiwa/meshes/link_4.mtl

@@ -0,0 +1,10 @@
+# Blender MTL File: 'None'
+# Material Count: 1
+
+newmtl None
+Ns 0
+Ka 0.000000 0.000000 0.000000
+Kd 0.8 0.8 0.8
+Ks 0.8 0.8 0.8
+d 1
+illum 2

data/kuka_iiwa/meshes/link_5.mtl

@@ -0,0 +1,10 @@
+# Blender MTL File: 'None'
+# Material Count: 1
+
+newmtl None
+Ns 0
+Ka 0.000000 0.000000 0.000000
+Kd 0.8 0.8 0.8
+Ks 0.8 0.8 0.8
+d 1
+illum 2

data/kuka_iiwa/meshes/link_6.mtl

@@ -0,0 +1,10 @@
+# Blender MTL File: 'None'
+# Material Count: 1
+
+newmtl None
+Ns 0
+Ka 0.000000 0.000000 0.000000
+Kd 0.8 0.8 0.8
+Ks 0.8 0.8 0.8
+d 1
+illum 2

data/kuka_iiwa/meshes/link_7.mtl

@@ -0,0 +1,10 @@
+# Blender MTL File: 'None'
+# Material Count: 1
+
+newmtl None
+Ns 0
+Ka 0.000000 0.000000 0.000000
+Kd 0.8 0.8 0.8
+Ks 0.8 0.8 0.8
+d 1
+illum 2

data/kuka_iiwa/model.urdf

@@ -70,7 +70,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="meshes/link_0.stl"/>
+        <mesh filename="meshes/link_0.obj"/>
       </geometry>
       <material name="Grey"/>
     </visual>
@@ -99,7 +99,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="meshes/link_1.stl"/>
+        <mesh filename="meshes/link_1.obj"/>
       </geometry>
       <material name="Blue"/>
     </visual>
@@ -128,7 +128,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="meshes/link_2.stl"/>
+        <mesh filename="meshes/link_2.obj"/>
       </geometry>
       <material name="Blue"/>
     </visual>
@@ -157,7 +157,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="meshes/link_3.stl"/>
+        <mesh filename="meshes/link_3.obj"/>
       </geometry>
       <material name="Orange"/>
     </visual>
@@ -186,7 +186,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="meshes/link_4.stl"/>
+        <mesh filename="meshes/link_4.obj"/>
       </geometry>
       <material name="Blue"/>
     </visual>
@@ -215,7 +215,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="meshes/link_5.stl"/>
+        <mesh filename="meshes/link_5.obj"/>
       </geometry>
       <material name="Blue"/>
     </visual>
@@ -244,7 +244,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="meshes/link_6.stl"/>
+        <mesh filename="meshes/link_6.obj"/>
       </geometry>
       <material name="Orange"/>
     </visual>
@@ -273,7 +273,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="meshes/link_7.stl"/>
+        <mesh filename="meshes/link_7.obj"/>
       </geometry>
       <material name="Grey"/>
     </visual>

data/plane_with_restitution.urdf

@@ -0,0 +1,29 @@
+<?xml version="0.0" ?>
+<robot name="plane">
+  <link name="planeLink">
+  <contact>
+      <restitution value="0.5"/>
+  </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".0"/>
+       <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+				<mesh filename="plane.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 -5"/>
+      <geometry>
+	 	<box size="30 30 10"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
+

data/roboschool/models_outdoor/stadium/stadium.mtl

@@ -0,0 +1,16 @@
+newmtl stadium_white
+Ns 96.078431
+Ka 0.000000 0.000000 0.000000
+Kd 1.000000 1.000000 1.000000
+Ks 0.500000 0.500000 0.500000
+
+newmtl stadium_grass
+Ka 0.000000 0.000000 0.000000
+Kd 0.000000 0.500000 0.000000
+Ks 0.000000 0.000000 0.000000
+map_Kd stadium_grass.jpg
+
+newmtl stadium_dirt
+Ka 0.000000 0.000000 0.000000
+Kd 0.600000 0.400000 0.000000
+Ks 0.000000 0.000000 0.000000

data/sphere2.urdf

@@ -18,6 +18,7 @@
       </geometry>
       <material name="white">
         <color rgba="1 1 1 1"/>
+        <specular rgb="11 1 1"/>
       </material>
     </visual>
     <collision>

data/sphere_with_restitution.urdf

@@ -0,0 +1,33 @@
+<?xml version="0.0" ?>
+<robot name="urdf_robot">
+  <link name="baseLink">
+  	<contact>
+      <restitution value="0.5" />
+      <rolling_friction value="0.001"/>
+      <spinning_friction value="0.001"/>
+      
+    </contact>
+
+    <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>
+				<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"/>
+      <geometry>
+	 <sphere radius="0.5"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
+

data/stadium.sdf

@@ -0,0 +1,107 @@
+<sdf version='1.6'>
+	<world name='default'>
+	<gravity>0 0 -9.8</gravity>
+		<model name='roboschool/models_outdoor/stadium/part0.obj'>
+			<static>1</static>
+			<pose frame=''>0 0 0 0 0 0</pose>
+			<link name='link_d0'>
+			<inertial>
+			<mass>0</mass>
+			<inertia>
+			<ixx>0.166667</ixx>
+			<ixy>0</ixy>
+			<ixz>0</ixz>
+			<iyy>0.166667</iyy>
+			<iyz>0</iyz>
+			<izz>0.166667</izz>
+			</inertia>
+			</inertial>
+			<visual name='visual'>
+				<geometry>
+				<mesh>
+					<scale>1 1 1</scale>
+					<uri>roboschool/models_outdoor/stadium/part0.obj</uri>
+				</mesh>
+				</geometry>
+			<material>
+				<ambient>1 1 1 1</ambient>
+				<diffuse>1.00000 1.00000 1.000000 1</diffuse>
+				<specular>0.1 .1 .1 1</specular>
+				<emissive>0 0 0 0</emissive>
+			 </material>
+			 </visual>
+			 </link>
+			</model>
+		<model name='roboschool/models_outdoor/stadium/part1.obj'>
+			<static>1</static>
+			<pose frame=''>0 0 0 0 0 0</pose>
+			<link name='link_d1'>
+			<inertial>
+			<mass>0</mass>
+			<inertia>
+			<ixx>0.166667</ixx>
+			<ixy>0</ixy>
+			<ixz>0</ixz>
+			<iyy>0.166667</iyy>
+			<iyz>0</iyz>
+			<izz>0.166667</izz>
+			</inertia>
+			</inertial>
+			<collision name='collision_1'>
+			<geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+			  </collision>
+			<visual name='visual'>
+				<geometry>
+				<mesh>
+					<scale>1 1 1</scale>
+					<uri>roboschool/models_outdoor/stadium/part1.obj</uri>
+				</mesh>
+				</geometry>
+			<material>
+				<ambient>1 1 1 1</ambient>
+				<diffuse>0.600000 0.400000 0.000000 1</diffuse>
+				<specular>.5 .5 .5 1</specular>
+				<emissive>0 0 0 0</emissive>
+			 </material>
+			 </visual>
+			 </link>
+			</model>
+		<model name='part2.obj'>
+			<static>1</static>
+			<pose frame=''>0 0 0 0 0 0</pose>
+			<link name='link_d2'>
+			<inertial>
+			<mass>0</mass>
+			<inertia>
+			<ixx>0.166667</ixx>
+			<ixy>0</ixy>
+			<ixz>0</ixz>
+			<iyy>0.166667</iyy>
+			<iyz>0</iyz>
+			<izz>0.166667</izz>
+			</inertia>
+			</inertial>
+			
+			<visual name='visual'>
+				<geometry>
+				<mesh>
+					<scale>1 1 1</scale>
+					<uri>roboschool/models_outdoor/stadium/part2.obj</uri>
+				</mesh>
+				</geometry>
+			<material>
+				<ambient>1 0 0 1</ambient>
+				<diffuse>0.000000 0.500000 0.000000 1</diffuse>
+				<specular>0.4 0.4 0.4 1</specular>
+				<emissive>0 0 0 0</emissive>
+			 </material>
+			 </visual>
+			 </link>
+			</model>
+	</world>
+</sdf>

data/two_cubes.sdf

@@ -20,7 +20,7 @@
         <collision name='collision'>
           <geometry>
             <plane>
-              <normal>1 2 3</normal>
+              <normal>0 0 1</normal>
               <size>100 100</size>
             </plane>
           </geometry>
@@ -46,7 +46,7 @@
           <cast_shadows>0</cast_shadows>
           <geometry>
             <plane>
-              <normal>4 5 6</normal>
+              <normal>0 0 1</normal>
               <size>100 100</size>
             </plane>
           </geometry>
@@ -89,6 +89,7 @@
  <static>1</static>
       <pose frame=''>0.512455 -1.58317 0.5 0 -0 0</pose>
       <link name='unit_box_0::link'>
+      
         <inertial>
           <mass>1</mass>
           <inertia>
@@ -154,26 +155,7 @@
             <izz>0.166667</izz>
           </inertia>
         </inertial>
-        <collision name='collision'>
+       
-          <geometry>
-            <box>
-              <size>1 1 1</size>
-            </box>
-          </geometry>
-          <max_contacts>10</max_contacts>
-          <surface>
-            <contact>
-              <ode/>
-            </contact>
-            <bounce/>
-            <friction>
-              <torsional>
-                <ode/>
-              </torsional>
-              <ode/>
-            </friction>
-          </surface>
-        </collision>
          <visual name='visual'>
           <geometry>
           <mesh>

examples/Benchmarks/BenchmarkDemo.cpp

@@ -32,7 +32,6 @@ subject to the following restrictions:
 
 #include "LinearMath/btAlignedObjectArray.h"
 #include "LinearMath/btTransform.h"
-#include "../MultiThreadedDemo/ParallelFor.h"
 
 class btDynamicsWorld;
 
@@ -230,7 +229,7 @@ public:
         }
     }
 
-    struct CastRaysLoopBody
+    struct CastRaysLoopBody : public btIParallelForBody
     {
         btCollisionWorld* mWorld;
 		btRaycastBar2* mRaycasts;
@@ -274,7 +273,7 @@ public:
         {
             CastRaysLoopBody rayLooper(cw, this);
             int grainSize = 20;  // number of raycasts per task
-            parallelFor( 0, NUMRAYS, grainSize, rayLooper );
+            btParallelFor( 0, NUMRAYS, grainSize, rayLooper );
         }
         else
 #endif // USE_PARALLEL_RAYCASTS

examples/CommonInterfaces/CommonExampleInterface.h

@@ -3,6 +3,13 @@
 #ifndef COMMON_EXAMPLE_INTERFACE_H
 #define COMMON_EXAMPLE_INTERFACE_H
 
+struct CommandProcessorCreationInterface
+{
+	virtual class CommandProcessorInterface* createCommandProcessor()=0;
+	virtual void deleteCommandProcessor(CommandProcessorInterface*)=0;
+};
+
+
 struct CommonExampleOptions
 {
 	struct GUIHelperInterface*	m_guiHelper;
@@ -11,13 +18,14 @@ struct CommonExampleOptions
 	int			m_option;
 	const char* m_fileName;
 	class SharedMemoryInterface* m_sharedMem;
-
+	CommandProcessorCreationInterface* m_commandProcessorCreation;
 	
 	CommonExampleOptions(struct GUIHelperInterface*	helper, int option=0)
 		:m_guiHelper(helper),
 		m_option(option),
 		m_fileName(0),
-		m_sharedMem(0)
+		m_sharedMem(0),
+		m_commandProcessorCreation(0)
 	{
 	}
 

examples/CommonInterfaces/CommonGUIHelperInterface.h

@@ -39,6 +39,8 @@ struct GUIHelperInterface
     virtual void removeAllGraphicsInstances()=0;
 	virtual void removeGraphicsInstance(int graphicsUid) {}
 	virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]) {}
+	virtual void changeSpecularColor(int instanceUid, const double specularColor[3]) {}
+
 	
 	virtual Common2dCanvasInterface* get2dCanvasInterface()=0;
 	

examples/CommonInterfaces/CommonRenderInterface.h

@@ -79,6 +79,9 @@ struct CommonRenderInterface
 	virtual void writeSingleInstanceColorToCPU(const double* color, int srcIndex)=0;
 	virtual void writeSingleInstanceScaleToCPU(const float* scale, int srcIndex)=0;
 	virtual void writeSingleInstanceScaleToCPU(const double* scale, int srcIndex)=0;
+	virtual void writeSingleInstanceSpecularColorToCPU(const double* specular, int srcIndex)=0;
+	virtual void writeSingleInstanceSpecularColorToCPU(const float* specular, int srcIndex)=0;
+	
     
     virtual int getTotalNumInstances() const = 0;
     

examples/ExampleBrowser/CMakeLists.txt

@@ -110,29 +110,6 @@ ELSE(WIN32)
 	ENDIF(APPLE)
 ENDIF(WIN32)
 
-IF (BULLET2_MULTITHREADED_OPEN_MP_DEMO)
-    ADD_DEFINITIONS("-DBT_USE_OPENMP=1")
-    IF (MSVC)
-        SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /openmp")
-    ELSE (MSVC)
-        # GCC, Clang
-        SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp")
-    ENDIF (MSVC)
-ENDIF (BULLET2_MULTITHREADED_OPEN_MP_DEMO)
-
-IF (BULLET2_MULTITHREADED_PPL_DEMO)
-    ADD_DEFINITIONS("-DBT_USE_PPL=1")
-ENDIF (BULLET2_MULTITHREADED_PPL_DEMO)
-
-IF (BULLET2_MULTITHREADED_TBB_DEMO)
-    SET (BULLET2_TBB_INCLUDE_DIR "not found" CACHE PATH "Directory for Intel TBB includes.")
-    SET (BULLET2_TBB_LIB_DIR "not found" CACHE PATH "Directory for Intel TBB libraries.")
-    find_library(TBB_LIBRARY tbb PATHS ${BULLET2_TBB_LIB_DIR})
-    find_library(TBBMALLOC_LIBRARY tbbmalloc PATHS ${BULLET2_TBB_LIB_DIR})
-    ADD_DEFINITIONS("-DBT_USE_TBB=1")
-    INCLUDE_DIRECTORIES( ${BULLET2_TBB_INCLUDE_DIR} )
-    LINK_LIBRARIES( ${TBB_LIBRARY} ${TBBMALLOC_LIBRARY} )
-ENDIF (BULLET2_MULTITHREADED_TBB_DEMO)
 
 SET(ExtendedTutorialsSources
 	../ExtendedTutorials/Chain.cpp 
@@ -175,6 +152,7 @@ SET(BulletExampleBrowser_SRCS
 	../SharedMemory/PhysicsClient.cpp
 	../SharedMemory/PhysicsClientC_API.cpp
 	../SharedMemory/PhysicsServerExample.cpp
+	../SharedMemory/PhysicsServerExampleBullet2.cpp
 	../SharedMemory/PhysicsClientExample.cpp
 	../SharedMemory/PosixSharedMemory.cpp
 	../SharedMemory/Win32SharedMemory.cpp
@@ -207,7 +185,6 @@ SET(BulletExampleBrowser_SRCS
 	../MultiThreadedDemo/MultiThreadedDemo.h
 	../MultiThreadedDemo/CommonRigidBodyMTBase.cpp
 	../MultiThreadedDemo/CommonRigidBodyMTBase.h
-	../MultiThreadedDemo/ParallelFor.h
 	../Tutorial/Tutorial.cpp
 	../Tutorial/Tutorial.h
 	../Tutorial/Dof6ConstraintTutorial.cpp
@@ -244,6 +221,7 @@ SET(BulletExampleBrowser_SRCS
 	../MultiThreading/b3PosixThreadSupport.cpp
 	../MultiThreading/b3Win32ThreadSupport.cpp
 	../MultiThreading/b3ThreadSupportInterface.cpp
+	../MultiThreading/btTaskScheduler.cpp
 	../RenderingExamples/TinyRendererSetup.cpp
 	../RenderingExamples/TimeSeriesCanvas.cpp
 	../RenderingExamples/TimeSeriesCanvas.h
@@ -386,7 +364,7 @@ ADD_CUSTOM_COMMAND(
                 COMMAND ${CMAKE_COMMAND} ARGS -E copy_directory ${BULLET_PHYSICS_SOURCE_DIR}/data ${PROJECT_BINARY_DIR}/data
         )
 
-IF (BULLET2_MULTITHREADED_TBB_DEMO AND WIN32)
+IF (BULLET2_USE_TBB_MULTITHREADING AND WIN32)
     # add a post build command to copy some dlls to the executable directory
     set(TBB_VC_VER "vc12")
     set(TBB_VC_ARCH "ia32")
@@ -400,7 +378,7 @@ IF (BULLET2_MULTITHREADED_TBB_DEMO AND WIN32)
     COMMAND ${CMAKE_COMMAND} -E copy_if_different
         "${BULLET2_TBB_INCLUDE_DIR}/../bin/${TBB_VC_ARCH}/${TBB_VC_VER}/tbbmalloc.dll"
         $<TARGET_FILE_DIR:App_ExampleBrowser>)
-ENDIF (BULLET2_MULTITHREADED_TBB_DEMO AND WIN32)
+ENDIF (BULLET2_USE_TBB_MULTITHREADING AND WIN32)
 
 
 IF (INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)

examples/ExampleBrowser/ExampleEntries.cpp

@@ -39,7 +39,7 @@
 #include "../FractureDemo/FractureDemo.h"
 #include "../DynamicControlDemo/MotorDemo.h"
 #include "../RollingFrictionDemo/RollingFrictionDemo.h"
-#include "../SharedMemory/PhysicsServerExample.h"
+#include "../SharedMemory/PhysicsServerExampleBullet2.h"
 #include "../SharedMemory/PhysicsClientExample.h"
 #include "../Constraints/TestHingeTorque.h"
 #include "../RenderingExamples/TimeSeriesExample.h"
@@ -138,7 +138,7 @@ static ExampleEntry gDefaultExamples[]=
 
 	ExampleEntry(0,"Physics Client-Server"),
 	ExampleEntry(1,"Physics Server", "Create a physics server that communicates with a physics client over shared memory. You can connect to the server using pybullet, a PhysicsClient or a UDP/TCP Bridge.",
-			PhysicsServerCreateFunc),
+			PhysicsServerCreateFuncBullet2),
 	ExampleEntry(1, "Physics Client (Shared Mem)", "Create a physics client that can communicate with a physics server over shared memory.", PhysicsClientCreateFunc),
 
 //	ExampleEntry(1,"Physics Server (Logging)", "Create a physics server that communicates with a physics client over shared memory. It will log all commands to a file.",
@@ -295,7 +295,7 @@ static ExampleEntry gDefaultExamples[]=
 	ExampleEntry(1,"Fracture demo", "Create a basic custom implementation to model fracturing objects, based on a btCompoundShape. It explicitly propagates the collision impulses and breaks the rigid body into multiple rigid bodies. Press F to toggle fracture and glue mode.", FractureDemoCreateFunc),
 
 	ExampleEntry(1,"Planar 2D","Show the use of 2D collision shapes and rigid body simulation. The collision shape is wrapped into a btConvex2dShape. The rigid bodies are restricted in a plane using the 'setAngularFactor' and 'setLinearFactor' API call.",Planar2DCreateFunc),
-#if BT_USE_OPENMP || BT_USE_TBB || BT_USE_PPL
+#if BT_THREADSAFE
     // only enable MultiThreaded demo if a task scheduler is available
     ExampleEntry( 1, "Multithreaded Demo",
     "Stacks of boxes that do not sleep. Good for testing performance with large numbers of bodies and contacts. Sliders can be used to change the number of stacks (restart needed after each change)."

examples/ExampleBrowser/InProcessExampleBrowser.cpp

@@ -28,6 +28,8 @@ void*	ExampleBrowserMemoryFunc();
 #include "EmptyExample.h"
 
 #include "../SharedMemory/PhysicsServerExample.h"
+#include "../SharedMemory/PhysicsServerExampleBullet2.h"
+
 #include "../SharedMemory/PhysicsClientExample.h"
 
 #ifndef _WIN32
@@ -124,14 +126,14 @@ static ExampleEntryPhysicsServer gDefaultExamplesPhysicsServer[]=
 	ExampleEntryPhysicsServer(0,"Robotics Control"),
 
 	ExampleEntryPhysicsServer(1,"Physics Server", "Create a physics server that communicates with a physics client over shared memory",
-			PhysicsServerCreateFunc),
+			PhysicsServerCreateFuncBullet2),
     ExampleEntryPhysicsServer(1,"Physics Server (RTC)", "Create a physics server that communicates with a physics client over shared memory. At each update, the Physics Server will continue calling 'stepSimulation' based on the real-time clock (RTC).",
-			PhysicsServerCreateFunc,PHYSICS_SERVER_USE_RTC_CLOCK),
+			PhysicsServerCreateFuncBullet2,PHYSICS_SERVER_USE_RTC_CLOCK),
 
 	ExampleEntryPhysicsServer(1,"Physics Server (Logging)", "Create a physics server that communicates with a physics client over shared memory. It will log all commands to a file.",
-			PhysicsServerCreateFunc,PHYSICS_SERVER_ENABLE_COMMAND_LOGGING),
+			PhysicsServerCreateFuncBullet2,PHYSICS_SERVER_ENABLE_COMMAND_LOGGING),
 	ExampleEntryPhysicsServer(1,"Physics Server (Replay Log)", "Create a physics server that replay a command log from disk.",
-			PhysicsServerCreateFunc,PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG),
+			PhysicsServerCreateFuncBullet2,PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG),
 
 
 };

examples/ExampleBrowser/OpenGLGuiHelper.cpp

@@ -325,7 +325,13 @@ void OpenGLGuiHelper::changeRGBAColor(int instanceUid, const double rgbaColor[4]
 		m_data->m_glApp->m_renderer->writeSingleInstanceColorToCPU(rgbaColor,instanceUid);
 	};
 }
-
+void OpenGLGuiHelper::changeSpecularColor(int instanceUid, const double specularColor[3])
+{
+	if (instanceUid>=0)
+	{
+		m_data->m_glApp->m_renderer->writeSingleInstanceSpecularColorToCPU(specularColor,instanceUid);
+	};
+}
 int OpenGLGuiHelper::createCheckeredTexture(int red,int green, int blue)
 {
 	int texWidth=1024;

examples/ExampleBrowser/OpenGLGuiHelper.h

@@ -27,7 +27,8 @@ struct OpenGLGuiHelper : public GUIHelperInterface
 	virtual void removeAllGraphicsInstances();
 	virtual void removeGraphicsInstance(int graphicsUid);
 	virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4]);
-	
+	virtual void changeSpecularColor(int instanceUid, const double specularColor[3]);
+
 	virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape);
 
 	virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld);

examples/ExampleBrowser/premake4.lua

@@ -81,6 +81,7 @@ project "App_BulletExampleBrowser"
 		"../SharedMemory/PhysicsClientC_API.cpp",
 		"../SharedMemory/PhysicsClientC_API.h",
 		"../SharedMemory/PhysicsServerExample.cpp",
+		"../SharedMemory/PhysicsServerExampleBullet2.cpp",
 		"../SharedMemory/PhysicsClientExample.cpp",
 		"../SharedMemory/PhysicsServer.cpp",
 		"../SharedMemory/PhysicsServerSharedMemory.cpp",

examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp

@@ -706,7 +706,7 @@ struct BulletMJCFImporterInternalData
 		if (!rgba.empty())
 		{
 			// "0 0.7 0.7 1"
-			parseVector4(geom.m_localMaterial.m_rgbaColor, rgba);
+			parseVector4(geom.m_localMaterial.m_matColor.m_rgbaColor, rgba);
 			geom.m_hasLocalMaterial = true;
 			geom.m_localMaterial.m_name = rgba;
 		}

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -42,6 +42,7 @@ struct MyTexture
 	unsigned char* textureData;
 };
 
+
 ATTRIBUTE_ALIGNED16(struct) BulletURDFInternalData
 {
 	BT_DECLARE_ALIGNED_ALLOCATOR();
@@ -51,7 +52,7 @@ ATTRIBUTE_ALIGNED16(struct) BulletURDFInternalData
 	std::string m_sourceFile;
 	char m_pathPrefix[1024];
 	int m_bodyId;
-	btHashMap<btHashInt,btVector4> m_linkColors;
+	btHashMap<btHashInt,UrdfMaterialColor> m_linkColors;
     btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
 	
 	LinkVisualShapesConverter* m_customVisualShapesConverter;
@@ -567,6 +568,15 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
 
     switch (collision->m_geometry.m_type)
     {
+	case URDF_GEOM_PLANE:
+		{
+			btVector3 planeNormal = collision->m_geometry.m_planeNormal;
+			btScalar planeConstant = 0;//not available?
+			btStaticPlaneShape* plane = new btStaticPlaneShape(planeNormal,planeConstant);
+			shape = plane;
+			shape ->setMargin(gUrdfDefaultCollisionMargin);
+			break;
+		}
 		case URDF_GEOM_CAPSULE:
         {
 			btScalar radius = collision->m_geometry.m_capsuleRadius;
@@ -788,7 +798,7 @@ upAxisMat.setIdentity();
 
         default:
 		b3Warning("Error: unknown collision geometry type %i\n", collision->m_geometry.m_type);
-		// for example, URDF_GEOM_PLANE
+		
 	}
 	return shape;
 }
@@ -1094,7 +1104,10 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP
 			{
 				UrdfMaterial *const  mat = *matPtr;
 				//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
-				m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor);
+				UrdfMaterialColor matCol;
+				matCol.m_rgbaColor = mat->m_matColor.m_rgbaColor;
+				matCol.m_specularColor = mat->m_matColor.m_specularColor;
+				m_data->m_linkColors.insert(linkIndex,matCol);
 			}
 			convertURDFToVisualShapeInternal(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures);
 		
@@ -1132,10 +1145,21 @@ int BulletURDFImporter::convertLinkVisualShapes(int linkIndex, const char* pathP
 
 bool BulletURDFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const
 {
-	const btVector4* rgbaPtr = m_data->m_linkColors[linkIndex];
+	const UrdfMaterialColor* matColPtr = m_data->m_linkColors[linkIndex];
-	if (rgbaPtr)
+	if (matColPtr)
 	{
-		colorRGBA = *rgbaPtr;
+		colorRGBA = matColPtr->m_rgbaColor;
+		return true;
+	}
+	return false;
+}
+
+bool BulletURDFImporter::getLinkColor2(int linkIndex, UrdfMaterialColor& matCol) const
+{
+	UrdfMaterialColor* matColPtr = m_data->m_linkColors[linkIndex];
+	if (matColPtr)
+	{
+		matCol = *matColPtr;
 		return true;
 	}
 	return false;

examples/Importers/ImportURDFDemo/BulletUrdfImporter.h

@@ -41,6 +41,8 @@ public:
 
 	virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const;
 
+	virtual bool getLinkColor2(int linkIndex, UrdfMaterialColor& matCol) const;
+
 	virtual bool getLinkContactInfo(int urdflinkIndex, URDFLinkContactInfo& contactInfo ) const;
 	
 	virtual bool getLinkAudioSource(int linkIndex, SDFAudioSource& audioSource) const;

examples/Importers/ImportURDFDemo/MultiBodyCreationInterface.h

@@ -11,9 +11,17 @@ public:
 
     
     virtual void createRigidBodyGraphicsInstance(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) = 0;
-  
+	virtual void createRigidBodyGraphicsInstance2(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, const btVector3& specularColor, int graphicsIndex)
+	{
+		createRigidBodyGraphicsInstance(linkIndex,body,colorRgba,graphicsIndex);
+	}
+
     ///optionally create some graphical representation from a collision object, usually for visual debugging purposes.
     virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba) = 0;
+    virtual void createCollisionObjectGraphicsInstance2(int linkIndex, class btCollisionObject* col, const btVector4& colorRgba, const btVector3& specularColor)
+	{
+		createCollisionObjectGraphicsInstance(linkIndex,col,colorRgba);
+	}
     
     virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep)  =0;
     

examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp

@@ -194,15 +194,37 @@ void MyMultiBodyCreator::addLinkMapping(int urdfLinkIndex, int mbLinkIndex)
 
 void MyMultiBodyCreator::createRigidBodyGraphicsInstance(int linkIndex, btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) 
 {
-        
     m_guiHelper->createRigidBodyGraphicsObject(body, colorRgba);
 }
+
+void MyMultiBodyCreator::createRigidBodyGraphicsInstance2(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, const btVector3& specularColor, int graphicsIndex)
+{        
+    m_guiHelper->createRigidBodyGraphicsObject(body, colorRgba);
+	int graphicsInstanceId = body->getUserIndex();
+	btVector3DoubleData speculard;
+	specularColor.serializeDouble(speculard);
+	m_guiHelper->changeSpecularColor(graphicsInstanceId,speculard.m_floats);
+}
+
+
+
     
 void MyMultiBodyCreator::createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* colObj, const btVector3& colorRgba) 
 {
     m_guiHelper->createCollisionObjectGraphicsObject(colObj,colorRgba);
 }
 
+void MyMultiBodyCreator::createCollisionObjectGraphicsInstance2(int linkIndex, class btCollisionObject* col, const btVector4& colorRgba, const btVector3& specularColor)
+{
+	createCollisionObjectGraphicsInstance(linkIndex,col,colorRgba);
+	int graphicsInstanceId = col->getUserIndex();
+	btVector3DoubleData speculard;
+	specularColor.serializeDouble(speculard);
+	m_guiHelper->changeSpecularColor(graphicsInstanceId,speculard.m_floats);
+
+}
+
+
 btMultiBody* MyMultiBodyCreator::getBulletMultiBody()
 {
     return m_bulletMultiBody;

examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h

@@ -41,10 +41,12 @@ public:
 	virtual ~MyMultiBodyCreator() {}
 
     virtual void createRigidBodyGraphicsInstance(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) ;
+    virtual void createRigidBodyGraphicsInstance2(int linkIndex, class btRigidBody* body, const btVector3& colorRgba, const btVector3& specularColor, int graphicsIndex) ;
   
     ///optionally create some graphical representation from a collision object, usually for visual debugging purposes.
     virtual void createCollisionObjectGraphicsInstance(int linkIndex, class btCollisionObject* col, const btVector3& colorRgba);
-    
+    virtual void createCollisionObjectGraphicsInstance2(int linkIndex, class btCollisionObject* col, const btVector4& colorRgba, const btVector3& specularColor);
+
     virtual class btMultiBody* allocateMultiBody(int urdfLinkIndex, int totalNumJoints,btScalar mass, const btVector3& localInertiaDiagonal, bool isFixedBase, bool canSleep);
     
     virtual class btRigidBody* allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape);

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -272,8 +272,15 @@ void ConvertURDF2BulletInternal(
     {
 
 
-        btVector4 color = selectColor2();
+		UrdfMaterialColor matColor;
-		u2b.getLinkColor(urdfLinkIndex,color);
+        btVector4 color2 = selectColor2();
+		btVector3 specular(0.5,0.5,0.5);
+		if (u2b.getLinkColor2(urdfLinkIndex,matColor))
+		{
+			color2 = matColor.m_rgbaColor;
+			specular = matColor.m_specularColor;
+		}
+
         /*
          if (visual->material.get())
          {
@@ -315,7 +322,7 @@ void ConvertURDF2BulletInternal(
 			u2b.getLinkContactInfo(urdfLinkIndex, contactInfo);
 
 			processContactParameters(contactInfo, body);
-            creation.createRigidBodyGraphicsInstance(urdfLinkIndex, body, color, graphicsIndex);
+            creation.createRigidBodyGraphicsInstance2(urdfLinkIndex, body, color2,specular, graphicsIndex);
             cache.registerRigidBody(urdfLinkIndex, body, inertialFrameInWorldSpace, mass, localInertiaDiagonal, compoundShape, localInertialFrame);
             
 
@@ -362,8 +369,14 @@ void ConvertURDF2BulletInternal(
 
             switch (jointType)
             {
+				case URDFFloatingJoint:
+				case URDFPlanarJoint:
                 case URDFFixedJoint:
                 {
+					if ((jointType==URDFFloatingJoint)||(jointType==URDFPlanarJoint))
+					{
+						printf("Warning: joint unsupported, creating a fixed joint instead.");
+					}
                     if (createMultiBody)
                     {
                         //todo: adjust the center of mass transform and pivot axis properly
@@ -484,9 +497,16 @@ void ConvertURDF2BulletInternal(
 				}
                 world1->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
 
-                btVector4 color = selectColor2();//(0.0,0.0,0.5);
+                btVector4 color2 = selectColor2();//(0.0,0.0,0.5);
-				u2b.getLinkColor(urdfLinkIndex,color);
+				btVector3 specularColor(1,1,1);
-                creation.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color);
+				UrdfMaterialColor matCol;
+				if (u2b.getLinkColor2(urdfLinkIndex,matCol))
+				{
+					color2 = matCol.m_rgbaColor;
+					specularColor = matCol.m_specularColor;
+				}
+
+                creation.createCollisionObjectGraphicsInstance2(urdfLinkIndex,col,color2,specularColor);
 
                 u2b.convertLinkVisualShapes2(mbLinkIndex, urdfLinkIndex, pathPrefix, localInertialFrame,col, u2b.getBodyUniqueId());
 

examples/Importers/ImportURDFDemo/URDFImporterInterface.h

@@ -36,6 +36,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;}
 
+	virtual bool getLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const { return false;}
+
+
 	virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const { return 0;}
 	///this API will likely change, don't override it!
 	virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const  { return false;}

examples/Importers/ImportURDFDemo/URDFJointTypes.h

@@ -63,4 +63,15 @@ enum UrdfCollisionFlags
 	URDF_HAS_COLLISION_MASK=4,
 };
 
+struct UrdfMaterialColor
+{
+	btVector4 m_rgbaColor;
+	btVector3 m_specularColor;
+	UrdfMaterialColor()
+		:m_rgbaColor(0.8, 0.8, 0.8, 1),
+		m_specularColor(0.4,0.4,0.4)
+	{
+	}
+};
+
 #endif //URDF_JOINT_TYPES_H

examples/Importers/ImportURDFDemo/UrdfParser.cpp

@@ -100,17 +100,33 @@ bool UrdfParser::parseMaterial(UrdfMaterial& material, TiXmlElement *config, Err
 	}
 		
 	// color
-	TiXmlElement *c = config->FirstChildElement("color");
-	if (c)
 	{
-	  if (c->Attribute("rgba")) 
+		TiXmlElement *c = config->FirstChildElement("color");
-	  {
+		if (c)
-		  if (!parseVector4(material.m_rgbaColor,c->Attribute("rgba")))
+		{
+		  if (c->Attribute("rgba")) 
 		  {
-			  std::string msg = material.m_name+" has no rgba";
+			  if (!parseVector4(material.m_matColor.m_rgbaColor,c->Attribute("rgba")))
-			  logger->reportWarning(msg.c_str());
+			  {
+				  std::string msg = material.m_name+" has no rgba";
+				  logger->reportWarning(msg.c_str());
+			  }
 		  }
-	  }
+		}
+	}
+
+	{
+		// specular (non-standard)
+		TiXmlElement *s = config->FirstChildElement("specular");
+		if (s)
+		{
+		  if (s->Attribute("rgb")) 
+		  {
+			  if (!parseVector3(material.m_matColor.m_specularColor,s->Attribute("rgb"),logger))
+			  {
+			  }
+		  }
+		}
 	}
 	return true;
 
@@ -552,16 +568,29 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
 		if (name_char)
 			matPtr->m_name = name_char;
 		model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
-        TiXmlElement *diffuse = mat->FirstChildElement("diffuse");
+		{
-        if (diffuse) {
+			TiXmlElement *diffuse = mat->FirstChildElement("diffuse");
-            std::string diffuseText = diffuse->GetText();
+			if (diffuse) {
-            btVector4 rgba(1,0,0,1);
+				std::string diffuseText = diffuse->GetText();
-            parseVector4(rgba,diffuseText);
+				btVector4 rgba(1,0,0,1);
-            matPtr->m_rgbaColor = rgba;
+				parseVector4(rgba,diffuseText);
+				matPtr->m_matColor.m_rgbaColor = rgba;
             
-            visual.m_materialName = matPtr->m_name;
+				visual.m_materialName = matPtr->m_name;
-            visual.m_geometry.m_hasLocalMaterial = true;
+				visual.m_geometry.m_hasLocalMaterial = true;
-        }
+			}
+		}
+		{
+			TiXmlElement *specular = mat->FirstChildElement("specular");
+			if (specular) {
+				std::string specularText = specular->GetText();
+				btVector3 rgba(1,1,1);
+				parseVector3(rgba,specularText,logger);
+				matPtr->m_matColor.m_specularColor = rgba;
+				visual.m_materialName = matPtr->m_name;
+				visual.m_geometry.m_hasLocalMaterial = true;
+			}
+		}
     } 
     else
       {
@@ -577,7 +606,8 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
           
           TiXmlElement *t = mat->FirstChildElement("texture");
           TiXmlElement *c = mat->FirstChildElement("color");
-          if (t||c)
+		  TiXmlElement *s = mat->FirstChildElement("specular");
+		  if (t||c||s)
           {
               if (parseMaterial(visual.m_geometry.m_localMaterial, mat,logger))
               {

examples/Importers/ImportURDFDemo/UrdfParser.h

@@ -17,13 +17,15 @@ struct ErrorLogger
 	virtual void printMessage(const char* msg)=0;
 };
 
+
+
 struct UrdfMaterial
 {
 	std::string m_name;
 	std::string m_textureFilename;
-	btVector4 m_rgbaColor; // [0]==r [1]==g [2]==b [3]==a
+	UrdfMaterialColor m_matColor;
-	UrdfMaterial():
+
-		m_rgbaColor(0.8, 0.8, 0.8, 1)
+	UrdfMaterial()
 	{
 	}
 };

examples/MultiThreadedDemo/CommonRigidBodyMTBase.cpp

@@ -23,10 +23,10 @@ class btCollisionShape;
 
 #include "CommonRigidBodyMTBase.h"
 #include "../CommonInterfaces/CommonParameterInterface.h"
-#include "ParallelFor.h"
 #include "LinearMath/btAlignedObjectArray.h"
 #include "LinearMath/btPoolAllocator.h"
 #include "btBulletCollisionCommon.h"
+#include "BulletCollision/CollisionDispatch/btCollisionDispatcherMt.h"
 #include "BulletDynamics/Dynamics/btSimulationIslandManagerMt.h"  // for setSplitIslands()
 #include "BulletDynamics/Dynamics/btDiscreteDynamicsWorldMt.h"
 #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
@@ -35,21 +35,8 @@ class btCollisionShape;
 #include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
 #include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
 #include "BulletDynamics/MLCPSolvers/btLemkeSolver.h"
+#include "../MultiThreading/btTaskScheduler.h"
 
-TaskManager gTaskMgr;
-
-#define USE_PARALLEL_NARROWPHASE 1  // detect collisions in parallel
-#define USE_PARALLEL_ISLAND_SOLVER 1   // solve simulation islands in parallel
-#define USE_PARALLEL_CREATE_PREDICTIVE_CONTACTS 1
-#define USE_PARALLEL_INTEGRATE_TRANSFORMS 1
-#define USE_PARALLEL_PREDICT_UNCONSTRAINED_MOTION 1
-
-#if defined (_MSC_VER) && _MSC_VER >= 1600
-// give us a compile error if any signatures of overriden methods is changed
-#define BT_OVERRIDE override
-#else
-#define BT_OVERRIDE
-#endif
 
 static int gNumIslands = 0;
 
@@ -124,7 +111,7 @@ public:
 };
 
 
-Profiler gProfiler;
+static Profiler gProfiler;
 
 class ProfileHelper
 {
@@ -141,457 +128,84 @@ public:
     }
 };
 
-int gThreadsRunningCounter = 0;
+static void profileBeginCallback( btDynamicsWorld *world, btScalar timeStep )
-btSpinMutex gThreadsRunningCounterMutex;
-
-void btPushThreadsAreRunning()
 {
-    gThreadsRunningCounterMutex.lock();
+    gProfiler.begin( Profiler::kRecordInternalTimeStep );
-    gThreadsRunningCounter++;
-    gThreadsRunningCounterMutex.unlock();
 }
 
-void btPopThreadsAreRunning()
+static void profileEndCallback( btDynamicsWorld *world, btScalar timeStep )
 {
-    gThreadsRunningCounterMutex.lock();
+    gProfiler.end( Profiler::kRecordInternalTimeStep );
-    gThreadsRunningCounter--;
-    gThreadsRunningCounterMutex.unlock();
-}
-
-bool btThreadsAreRunning()
-{
-    return gThreadsRunningCounter != 0;
 }
 
 
-#if USE_PARALLEL_NARROWPHASE
+///
-
+/// MyCollisionDispatcher -- subclassed for profiling purposes
-class MyCollisionDispatcher : public btCollisionDispatcher
+///
+class MyCollisionDispatcher : public btCollisionDispatcherMt
 {
-    btSpinMutex m_manifoldPtrsMutex;
+    typedef btCollisionDispatcherMt ParentClass;
-
 public:
-    MyCollisionDispatcher( btCollisionConfiguration* config ) : btCollisionDispatcher( config )
+    MyCollisionDispatcher( btCollisionConfiguration* config, int grainSize ) : btCollisionDispatcherMt( config, grainSize )
     {
     }
 
-    virtual ~MyCollisionDispatcher()
-    {
-    }
-
-    btPersistentManifold* getNewManifold( const btCollisionObject* body0, const btCollisionObject* body1 ) BT_OVERRIDE
-    {
-        // added spin-locks
-        //optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance)
-
-        btScalar contactBreakingThreshold = ( m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD ) ?
-            btMin( body0->getCollisionShape()->getContactBreakingThreshold( gContactBreakingThreshold ), body1->getCollisionShape()->getContactBreakingThreshold( gContactBreakingThreshold ) )
-            : gContactBreakingThreshold;
-
-        btScalar contactProcessingThreshold = btMin( body0->getContactProcessingThreshold(), body1->getContactProcessingThreshold() );
-
-        void* mem = m_persistentManifoldPoolAllocator->allocate( sizeof( btPersistentManifold ) );
-        if (NULL == mem)
-        {
-            //we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert.
-            if ( ( m_dispatcherFlags&CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION ) == 0 )
-            {
-                mem = btAlignedAlloc( sizeof( btPersistentManifold ), 16 );
-            }
-            else
-            {
-                btAssert( 0 );
-                //make sure to increase the m_defaultMaxPersistentManifoldPoolSize in the btDefaultCollisionConstructionInfo/btDefaultCollisionConfiguration
-                return 0;
-            }
-        }
-        btPersistentManifold* manifold = new(mem) btPersistentManifold( body0, body1, 0, contactBreakingThreshold, contactProcessingThreshold );
-        m_manifoldPtrsMutex.lock();
-        manifold->m_index1a = m_manifoldsPtr.size();
-        m_manifoldsPtr.push_back( manifold );
-        m_manifoldPtrsMutex.unlock();
-
-        return manifold;
-    }
-
-    void releaseManifold( btPersistentManifold* manifold ) BT_OVERRIDE
-    {
-        clearManifold( manifold );
-
-        m_manifoldPtrsMutex.lock();
-        int findIndex = manifold->m_index1a;
-        btAssert( findIndex < m_manifoldsPtr.size() );
-        m_manifoldsPtr.swap( findIndex, m_manifoldsPtr.size() - 1 );
-        m_manifoldsPtr[ findIndex ]->m_index1a = findIndex;
-        m_manifoldsPtr.pop_back();
-        m_manifoldPtrsMutex.unlock();
-
-        manifold->~btPersistentManifold();
-        if ( m_persistentManifoldPoolAllocator->validPtr( manifold ) )
-        {
-            m_persistentManifoldPoolAllocator->freeMemory( manifold );
-        }
-        else
-        {
-            btAlignedFree( manifold );
-        }
-    }
-
-    struct Updater
-    {
-        btBroadphasePair* mPairArray;
-        btNearCallback mCallback;
-        btCollisionDispatcher* mDispatcher;
-        const btDispatcherInfo* mInfo;
-
-        Updater()
-        {
-            mPairArray = NULL;
-            mCallback = NULL;
-            mDispatcher = NULL;
-            mInfo = NULL;
-        }
-        void forLoop( int iBegin, int iEnd ) const
-        {
-            for ( int i = iBegin; i < iEnd; ++i )
-            {
-                btBroadphasePair* pair = &mPairArray[ i ];
-                mCallback( *pair, *mDispatcher, *mInfo );
-            }
-        }
-    };
-
     virtual void dispatchAllCollisionPairs( btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher ) BT_OVERRIDE
     {
-        ProfileHelper prof(Profiler::kRecordDispatchAllCollisionPairs);
+        ProfileHelper prof( Profiler::kRecordDispatchAllCollisionPairs );
-        int grainSize = 40;  // iterations per task
+        ParentClass::dispatchAllCollisionPairs( pairCache, info, dispatcher );
-        int pairCount = pairCache->getNumOverlappingPairs();
-        Updater updater;
-        updater.mCallback = getNearCallback();
-        updater.mPairArray = pairCount > 0 ? pairCache->getOverlappingPairArrayPtr() : NULL;
-        updater.mDispatcher = this;
-        updater.mInfo = &info;
-
-        btPushThreadsAreRunning();
-        parallelFor( 0, pairCount, grainSize, updater );
-        btPopThreadsAreRunning();
-        
-        if (m_manifoldsPtr.size() < 1)
-            return;
-
-        // reconstruct the manifolds array to ensure determinism
-        m_manifoldsPtr.resizeNoInitialize(0);
-        btBroadphasePair* pairs = pairCache->getOverlappingPairArrayPtr();
-        for (int i = 0; i < pairCount; ++i)
-        {
-            btCollisionAlgorithm* algo = pairs[i].m_algorithm;
-            if (algo) algo->getAllContactManifolds(m_manifoldsPtr);
-        }
-
-        // update the indices (used when releasing manifolds)
-        for (int i = 0; i < m_manifoldsPtr.size(); ++i)
-            m_manifoldsPtr[i]->m_index1a = i;
     }
 };
 
-#endif
 
-
-#if USE_PARALLEL_ISLAND_SOLVER
 ///
-/// MyConstraintSolverPool - masquerades as a constraint solver, but really it is a threadsafe pool of them.
+/// myParallelIslandDispatch -- wrap default parallel dispatch for profiling and to get the number of simulation islands
-///
+//
-///  Each solver in the pool is protected by a mutex.  When solveGroup is called from a thread,
+void myParallelIslandDispatch( btAlignedObjectArray<btSimulationIslandManagerMt::Island*>* islandsPtr, btSimulationIslandManagerMt::IslandCallback* callback )
-///  the pool looks for a solver that isn't being used by another thread, locks it, and dispatches the
-///  call to the solver.
-///  So long as there are at least as many solvers as there are hardware threads, it should never need to
-///  spin wait.
-///
-class MyConstraintSolverPool : public btConstraintSolver
 {
-    const static size_t kCacheLineSize = 128;
+    ProfileHelper prof( Profiler::kRecordDispatchIslands );
-    struct ThreadSolver
-    {
-        btConstraintSolver* solver;
-        btSpinMutex mutex;
-        char _cachelinePadding[ kCacheLineSize - sizeof( btSpinMutex ) - sizeof( void* ) ];  // keep mutexes from sharing a cache line
-    };
-    btAlignedObjectArray<ThreadSolver> m_solvers;
-    btConstraintSolverType m_solverType;
-
-    ThreadSolver* getAndLockThreadSolver()
-    {
-        while ( true )
-        {
-            for ( int i = 0; i < m_solvers.size(); ++i )
-            {
-                ThreadSolver& solver = m_solvers[ i ];
-                if ( solver.mutex.tryLock() )
-                {
-                    return &solver;
-                }
-            }
-        }
-        return NULL;
-    }
-    void init( btConstraintSolver** solvers, int numSolvers )
-    {
-        m_solverType = BT_SEQUENTIAL_IMPULSE_SOLVER;
-        m_solvers.resize( numSolvers );
-        for ( int i = 0; i < numSolvers; ++i )
-        {
-            m_solvers[ i ].solver = solvers[ i ];
-        }
-        if ( numSolvers > 0 )
-        {
-            m_solverType = solvers[ 0 ]->getSolverType();
-        }
-    }
-public:
-    // create the solvers for me
-    explicit MyConstraintSolverPool( int numSolvers )
-    {
-        btAlignedObjectArray<btConstraintSolver*> solvers;
-        solvers.reserve( numSolvers );
-        for ( int i = 0; i < numSolvers; ++i )
-        {
-            btConstraintSolver* solver = new btSequentialImpulseConstraintSolver();
-            solvers.push_back( solver );
-        }
-        init( &solvers[ 0 ], numSolvers );
-    }
-
-    // pass in fully constructed solvers (destructor will delete them)
-    MyConstraintSolverPool( btConstraintSolver** solvers, int numSolvers )
-    {
-        init( solvers, numSolvers );
-    }
-    virtual ~MyConstraintSolverPool()
-    {
-        // delete all solvers
-        for ( int i = 0; i < m_solvers.size(); ++i )
-        {
-            ThreadSolver& solver = m_solvers[ i ];
-            delete solver.solver;
-            solver.solver = NULL;
-        }
-    }
-
-    //virtual void prepareSolve( int /* numBodies */, int /* numManifolds */ ) { ; } // does nothing
-
-    ///solve a group of constraints
-    virtual btScalar solveGroup( btCollisionObject** bodies,
-                                 int numBodies,
-                                 btPersistentManifold** manifolds,
-                                 int numManifolds,
-                                 btTypedConstraint** constraints,
-                                 int numConstraints,
-                                 const btContactSolverInfo& info,
-                                 btIDebugDraw* debugDrawer,
-                                 btDispatcher* dispatcher
-                                 )
-    {
-        ThreadSolver* solver = getAndLockThreadSolver();
-        solver->solver->solveGroup( bodies, numBodies, manifolds, numManifolds, constraints, numConstraints, info, debugDrawer, dispatcher );
-        solver->mutex.unlock();
-        return 0.0f;
-    }
-
-    //virtual void allSolved( const btContactSolverInfo& /* info */, class btIDebugDraw* /* debugDrawer */ ) { ; } // does nothing
-
-    ///clear internal cached data and reset random seed
-    virtual	void reset()
-    {
-        for ( int i = 0; i < m_solvers.size(); ++i )
-        {
-            ThreadSolver& solver = m_solvers[ i ];
-            solver.mutex.lock();
-            solver.solver->reset();
-            solver.mutex.unlock();
-        }
-    }
-
-    virtual btConstraintSolverType getSolverType() const
-    {
-        return m_solverType;
-    }
-};
-
-struct UpdateIslandDispatcher
-{
-    btAlignedObjectArray<btSimulationIslandManagerMt::Island*>* islandsPtr;
-    btSimulationIslandManagerMt::IslandCallback* callback;
-
-    void forLoop( int iBegin, int iEnd ) const
-    {
-        for ( int i = iBegin; i < iEnd; ++i )
-        {
-            btSimulationIslandManagerMt::Island* island = ( *islandsPtr )[ i ];
-            btPersistentManifold** manifolds = island->manifoldArray.size() ? &island->manifoldArray[ 0 ] : NULL;
-            btTypedConstraint** constraintsPtr = island->constraintArray.size() ? &island->constraintArray[ 0 ] : NULL;
-            callback->processIsland( &island->bodyArray[ 0 ],
-                                     island->bodyArray.size(),
-                                     manifolds,
-                                     island->manifoldArray.size(),
-                                     constraintsPtr,
-                                     island->constraintArray.size(),
-                                     island->id
-                                     );
-        }
-    }
-};
-
-void parallelIslandDispatch( btAlignedObjectArray<btSimulationIslandManagerMt::Island*>* islandsPtr, btSimulationIslandManagerMt::IslandCallback* callback )
-{
-    ProfileHelper prof(Profiler::kRecordDispatchIslands);
     gNumIslands = islandsPtr->size();
-    int grainSize = 1;  // iterations per task
+    btSimulationIslandManagerMt::parallelIslandDispatch( islandsPtr, callback );
-    UpdateIslandDispatcher dispatcher;
-    dispatcher.islandsPtr = islandsPtr;
-    dispatcher.callback = callback;
-    btPushThreadsAreRunning();
-    parallelFor( 0, islandsPtr->size(), grainSize, dispatcher );
-    btPopThreadsAreRunning();
-}
-#endif //#if USE_PARALLEL_ISLAND_SOLVER
-
-
-void profileBeginCallback(btDynamicsWorld *world, btScalar timeStep)
-{
-    gProfiler.begin(Profiler::kRecordInternalTimeStep);
 }
 
-void profileEndCallback(btDynamicsWorld *world, btScalar timeStep)
-{
-    gProfiler.end(Profiler::kRecordInternalTimeStep);
-}
 
 ///
-/// MyDiscreteDynamicsWorld
+/// MyDiscreteDynamicsWorld -- subclassed for profiling purposes
-///
-///  Should function exactly like btDiscreteDynamicsWorld.
-///  3 methods that iterate over all of the rigidbodies can run in parallel:
-///     - predictUnconstraintMotion
-///     - integrateTransforms
-///     - createPredictiveContacts
 ///
 ATTRIBUTE_ALIGNED16( class ) MyDiscreteDynamicsWorld : public btDiscreteDynamicsWorldMt
 {
-    typedef btDiscreteDynamicsWorld ParentClass;
+    typedef btDiscreteDynamicsWorldMt ParentClass;
 
 protected:
-#if USE_PARALLEL_PREDICT_UNCONSTRAINED_MOTION
-    struct UpdaterUnconstrainedMotion
-    {
-        btScalar timeStep;
-        btRigidBody** rigidBodies;
-
-        void forLoop( int iBegin, int iEnd ) const
-        {
-            for ( int i = iBegin; i < iEnd; ++i )
-            {
-                btRigidBody* body = rigidBodies[ i ];
-                if ( !body->isStaticOrKinematicObject() )
-                {
-                    //don't integrate/update velocities here, it happens in the constraint solver
-                    body->applyDamping( timeStep );
-                    body->predictIntegratedTransform( timeStep, body->getInterpolationWorldTransform() );
-                }
-            }
-        }
-    };
 
     virtual void predictUnconstraintMotion( btScalar timeStep ) BT_OVERRIDE
     {
         ProfileHelper prof( Profiler::kRecordPredictUnconstrainedMotion );
-        BT_PROFILE( "predictUnconstraintMotion" );
+        ParentClass::predictUnconstraintMotion( timeStep );
-        int grainSize = 50;  // num of iterations per task for TBB
-        int bodyCount = m_nonStaticRigidBodies.size();
-        UpdaterUnconstrainedMotion update;
-        update.timeStep = timeStep;
-        update.rigidBodies = bodyCount ? &m_nonStaticRigidBodies[ 0 ] : NULL;
-        btPushThreadsAreRunning();
-        parallelFor( 0, bodyCount, grainSize, update );
-        btPopThreadsAreRunning();
     }
-#endif // #if USE_PARALLEL_PREDICT_UNCONSTRAINED_MOTION
+    virtual void createPredictiveContacts( btScalar timeStep ) BT_OVERRIDE
-
-#if USE_PARALLEL_CREATE_PREDICTIVE_CONTACTS
-    struct UpdaterCreatePredictiveContacts
-    {
-        btScalar timeStep;
-        btRigidBody** rigidBodies;
-        MyDiscreteDynamicsWorld* world;
-
-        void forLoop( int iBegin, int iEnd ) const
-        {
-            world->createPredictiveContactsInternal( &rigidBodies[ iBegin ], iEnd - iBegin, timeStep );
-        }
-    };
-
-    virtual void createPredictiveContacts( btScalar timeStep )
     {
         ProfileHelper prof( Profiler::kRecordCreatePredictiveContacts );
-        releasePredictiveContacts();
+        ParentClass::createPredictiveContacts( timeStep );
-        int grainSize = 50;  // num of iterations per task for TBB or OPENMP
-        if ( int bodyCount = m_nonStaticRigidBodies.size() )
-        {
-            UpdaterCreatePredictiveContacts update;
-            update.world = this;
-            update.timeStep = timeStep;
-            update.rigidBodies = &m_nonStaticRigidBodies[ 0 ];
-            btPushThreadsAreRunning();
-            parallelFor( 0, bodyCount, grainSize, update );
-            btPopThreadsAreRunning();
-        }
     }
-#endif // #if USE_PARALLEL_CREATE_PREDICTIVE_CONTACTS
-
-#if USE_PARALLEL_INTEGRATE_TRANSFORMS
-    struct UpdaterIntegrateTransforms
-    {
-        btScalar timeStep;
-        btRigidBody** rigidBodies;
-        MyDiscreteDynamicsWorld* world;
-
-        void forLoop( int iBegin, int iEnd ) const
-        {
-            world->integrateTransformsInternal( &rigidBodies[ iBegin ], iEnd - iBegin, timeStep );
-        }
-    };
-
     virtual void integrateTransforms( btScalar timeStep ) BT_OVERRIDE
     {
         ProfileHelper prof( Profiler::kRecordIntegrateTransforms );
-        BT_PROFILE( "integrateTransforms" );
+        ParentClass::integrateTransforms( timeStep );
-        int grainSize = 50;  // num of iterations per task for TBB or OPENMP
-        if ( int bodyCount = m_nonStaticRigidBodies.size() )
-        {
-            UpdaterIntegrateTransforms update;
-            update.world = this;
-            update.timeStep = timeStep;
-            update.rigidBodies = &m_nonStaticRigidBodies[ 0 ];
-            btPushThreadsAreRunning();
-            parallelFor( 0, bodyCount, grainSize, update );
-            btPopThreadsAreRunning();
-        }
     }
-#endif // #if USE_PARALLEL_INTEGRATE_TRANSFORMS
 
 public:
     BT_DECLARE_ALIGNED_ALLOCATOR();
 
     MyDiscreteDynamicsWorld( btDispatcher* dispatcher,
                              btBroadphaseInterface* pairCache,
-                             btConstraintSolver* constraintSolver,
+                             btConstraintSolverPoolMt* constraintSolver,
                              btCollisionConfiguration* collisionConfiguration
                              ) :
                              btDiscreteDynamicsWorldMt( dispatcher, pairCache, constraintSolver, collisionConfiguration )
     {
-#if USE_PARALLEL_ISLAND_SOLVER
         btSimulationIslandManagerMt* islandMgr = static_cast<btSimulationIslandManagerMt*>( m_islandManager );
-        islandMgr->setIslandDispatchFunction( parallelIslandDispatch );
+        islandMgr->setIslandDispatchFunction( myParallelIslandDispatch );
-#endif //#if USE_PARALLEL_ISLAND_SOLVER
     }
 
 };
@@ -625,8 +239,69 @@ btConstraintSolver* createSolverByType( SolverType t )
 }
 
 
+///
+/// btTaskSchedulerManager -- manage a number of task schedulers so we can switch between them
+///
+class btTaskSchedulerManager
+{
+    btAlignedObjectArray<btITaskScheduler*> m_taskSchedulers;
+    btAlignedObjectArray<btITaskScheduler*> m_allocatedTaskSchedulers;
+
+public:
+    btTaskSchedulerManager() {}
+    void init()
+    {
+        addTaskScheduler( btGetSequentialTaskScheduler() );
+#if BT_THREADSAFE
+        if ( btITaskScheduler* ts = createDefaultTaskScheduler() )
+        {
+            m_allocatedTaskSchedulers.push_back( ts );
+            addTaskScheduler( ts );
+        }
+        addTaskScheduler( btGetOpenMPTaskScheduler() );
+        addTaskScheduler( btGetTBBTaskScheduler() );
+        addTaskScheduler( btGetPPLTaskScheduler() );
+        if ( getNumTaskSchedulers() > 1 )
+        {
+            // prefer a non-sequential scheduler if available
+            btSetTaskScheduler( m_taskSchedulers[ 1 ] );
+        }
+        else
+        {
+            btSetTaskScheduler( m_taskSchedulers[ 0 ] );
+        }
+#endif // #if BT_THREADSAFE
+    }
+    void shutdown()
+    {
+        for ( int i = 0; i < m_allocatedTaskSchedulers.size(); ++i )
+        {
+            delete m_allocatedTaskSchedulers[ i ];
+        }
+        m_allocatedTaskSchedulers.clear();
+    }
+
+    void addTaskScheduler( btITaskScheduler* ts )
+    {
+        if ( ts )
+        {
+            m_taskSchedulers.push_back( ts );
+        }
+    }
+    int getNumTaskSchedulers() const { return m_taskSchedulers.size(); }
+    btITaskScheduler* getTaskScheduler( int i ) { return m_taskSchedulers[ i ]; }
+};
+
+
+static btTaskSchedulerManager gTaskSchedulerMgr;
+
+#if BT_THREADSAFE
+static bool gMultithreadedWorld = true;
+static bool gDisplayProfileInfo = true;
+#else
 static bool gMultithreadedWorld = false;
 static bool gDisplayProfileInfo = false;
+#endif
 static SolverType gSolverType = SOLVER_TYPE_SEQUENTIAL_IMPULSE;
 static int gSolverMode = SOLVER_SIMD |
                         SOLVER_USE_WARMSTARTING |
@@ -652,15 +327,17 @@ CommonRigidBodyMTBase::CommonRigidBodyMTBase( struct GUIHelperInterface* helper
 {
     m_multithreadedWorld = false;
     m_multithreadCapable = false;
-    gTaskMgr.init();
+    if ( gTaskSchedulerMgr.getNumTaskSchedulers() == 0 )
+    {
+        gTaskSchedulerMgr.init();
+    }
 }
 
 CommonRigidBodyMTBase::~CommonRigidBodyMTBase()
 {
-    gTaskMgr.shutdown();
 }
 
-void boolPtrButtonCallback(int buttonId, bool buttonState, void* userPointer)
+static void boolPtrButtonCallback(int buttonId, bool buttonState, void* userPointer)
 {
     if (bool* val = static_cast<bool*>(userPointer))
     {
@@ -668,7 +345,7 @@ void boolPtrButtonCallback(int buttonId, bool buttonState, void* userPointer)
     }
 }
 
-void toggleSolverModeCallback(int buttonId, bool buttonState, void* userPointer)
+static void toggleSolverModeCallback(int buttonId, bool buttonState, void* userPointer)
 {
     if (buttonState)
     {
@@ -687,7 +364,7 @@ void toggleSolverModeCallback(int buttonId, bool buttonState, void* userPointer)
     }
 }
 
-void setSolverTypeCallback(int buttonId, bool buttonState, void* userPointer)
+static void setSolverTypeCallback(int buttonId, bool buttonState, void* userPointer)
 {
     if (buttonId >= 0 && buttonId < SOLVER_TYPE_COUNT)
     {
@@ -695,32 +372,34 @@ void setSolverTypeCallback(int buttonId, bool buttonState, void* userPointer)
     }
 }
 
-void apiSelectButtonCallback(int buttonId, bool buttonState, void* userPointer)
+static void setNumThreads( int numThreads )
 {
-    gTaskMgr.setApi(static_cast<TaskManager::Api>(buttonId));
+#if BT_THREADSAFE
-    if (gTaskMgr.getApi()==TaskManager::apiNone)
+    int newNumThreads = ( std::min )( numThreads, int( BT_MAX_THREAD_COUNT ) );
+    int oldNumThreads = btGetTaskScheduler()->getNumThreads();
+    // only call when the thread count is different
+    if ( newNumThreads != oldNumThreads )
     {
-        gSliderNumThreads = 1.0f;
+        btGetTaskScheduler()->setNumThreads( newNumThreads );
-    }
-    else
-    {
-        gSliderNumThreads = float(gTaskMgr.getNumThreads());
     }
+#endif // #if BT_THREADSAFE
 }
 
-void setThreadCountCallback(float val, void* userPtr)
+static void apiSelectButtonCallback(int buttonId, bool buttonState, void* userPointer)
 {
-    if (gTaskMgr.getApi()==TaskManager::apiNone)
+#if BT_THREADSAFE
-    {
+    // change the task scheduler
-        gSliderNumThreads = 1.0f;
+    btSetTaskScheduler( gTaskSchedulerMgr.getTaskScheduler( buttonId ) );
-    }
+    setNumThreads( int( gSliderNumThreads ) );
-    else
+#endif // #if BT_THREADSAFE
-    {
-        gTaskMgr.setNumThreads( int( gSliderNumThreads ) );
-    }
 }
 
-void setSolverIterationCountCallback(float val, void* userPtr)
+static void setThreadCountCallback(float val, void* userPtr)
+{
+    setNumThreads( int( gSliderNumThreads ) );
+}
+
+static void setSolverIterationCountCallback(float val, void* userPtr)
 {
     if (btDiscreteDynamicsWorld* world = reinterpret_cast<btDiscreteDynamicsWorld*>(userPtr))
     {
@@ -733,40 +412,37 @@ void CommonRigidBodyMTBase::createEmptyDynamicsWorld()
     gNumIslands = 0;
     m_solverType = gSolverType;
 #if BT_THREADSAFE && (BT_USE_OPENMP || BT_USE_PPL || BT_USE_TBB)
+    btAssert( btGetTaskScheduler() != NULL );
     m_multithreadCapable = true;
 #endif
     if ( gMultithreadedWorld )
     {
+#if BT_THREADSAFE
         m_dispatcher = NULL;
         btDefaultCollisionConstructionInfo cci;
         cci.m_defaultMaxPersistentManifoldPoolSize = 80000;
         cci.m_defaultMaxCollisionAlgorithmPoolSize = 80000;
         m_collisionConfiguration = new btDefaultCollisionConfiguration( cci );
 
-#if USE_PARALLEL_NARROWPHASE
+        m_dispatcher = new MyCollisionDispatcher( m_collisionConfiguration, 40 );
-        m_dispatcher = new	MyCollisionDispatcher( m_collisionConfiguration );
-#else
-        m_dispatcher = new	btCollisionDispatcher( m_collisionConfiguration );
-#endif //USE_PARALLEL_NARROWPHASE
-
         m_broadphase = new btDbvtBroadphase();
 
-#if BT_THREADSAFE && USE_PARALLEL_ISLAND_SOLVER
+        btConstraintSolverPoolMt* solverPool;
         {
             btConstraintSolver* solvers[ BT_MAX_THREAD_COUNT ];
-            int maxThreadCount = btMin( int(BT_MAX_THREAD_COUNT), TaskManager::getMaxNumThreads() );
+            int maxThreadCount = BT_MAX_THREAD_COUNT;
             for ( int i = 0; i < maxThreadCount; ++i )
             {
                 solvers[ i ] = createSolverByType( m_solverType );
             }
-            m_solver = new MyConstraintSolverPool( solvers, maxThreadCount );
+            solverPool = new btConstraintSolverPoolMt( solvers, maxThreadCount );
+            m_solver = solverPool;
         }
-#else
+        btDiscreteDynamicsWorld* world = new MyDiscreteDynamicsWorld( m_dispatcher, m_broadphase, solverPool, m_collisionConfiguration );
-        m_solver = createSolverByType( m_solverType );
-#endif //#if USE_PARALLEL_ISLAND_SOLVER
-        btDiscreteDynamicsWorld* world = new MyDiscreteDynamicsWorld( m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration );
         m_dynamicsWorld = world;
         m_multithreadedWorld = true;
+        btAssert( btGetTaskScheduler() != NULL );
+#endif // #if BT_THREADSAFE
     }
     else
     {
@@ -885,29 +561,32 @@ void CommonRigidBodyMTBase::createDefaultParameters()
     }
     if (m_multithreadedWorld)
     {
+#if BT_THREADSAFE
         // create a button for each supported threading API
-        for (int iApi = 0; iApi < TaskManager::apiCount; ++iApi)
+        for ( int iApi = 0; iApi < gTaskSchedulerMgr.getNumTaskSchedulers(); ++iApi )
         {
-            TaskManager::Api api = static_cast<TaskManager::Api>(iApi);
+            char str[ 1024 ];
-            if (gTaskMgr.isSupported(api))
+            sprintf( str, "API %s", gTaskSchedulerMgr.getTaskScheduler(iApi)->getName() );
-            {
+            ButtonParams button( str, iApi, false );
-                char str[1024];
+            button.m_callback = apiSelectButtonCallback;
-                sprintf(str, "API %s", gTaskMgr.getApiName(api));
+            m_guiHelper->getParameterInterface()->registerButtonParameter( button );
-                ButtonParams button( str, iApi, false );
-                button.m_callback = apiSelectButtonCallback;
-                m_guiHelper->getParameterInterface()->registerButtonParameter( button );
-            }
         }
         {
             // create a slider to set the number of threads to use
-            gSliderNumThreads = float(gTaskMgr.getNumThreads());
+            int numThreads = btGetTaskScheduler()->getNumThreads();
+            // if slider has not been set yet (by another demo),
+            if ( gSliderNumThreads <= 1.0f )
+            {
+                gSliderNumThreads = float( numThreads );
+            }
 			SliderParams slider("Thread count", &gSliderNumThreads);
 			slider.m_minVal = 1.0f;
-			slider.m_maxVal = float(gTaskMgr.getMaxNumThreads()*2);
+			slider.m_maxVal = float( BT_MAX_THREAD_COUNT );
 			slider.m_callback = setThreadCountCallback;
             slider.m_clampToIntegers = true;
             m_guiHelper->getParameterInterface()->registerSliderFloatParameter( slider );
         }
+#endif // #if BT_THREADSAFE
     }
 }
 
@@ -939,6 +618,7 @@ void CommonRigidBodyMTBase::drawScreenText()
     {
         if ( m_multithreadedWorld )
         {
+#if BT_THREADSAFE
             int numManifolds = m_dispatcher->getNumManifolds();
             int numContacts = 0;
             for ( int i = 0; i < numManifolds; ++i )
@@ -946,17 +626,18 @@ void CommonRigidBodyMTBase::drawScreenText()
                 const btPersistentManifold* man = m_dispatcher->getManifoldByIndexInternal( i );
                 numContacts += man->getNumContacts();
             }
-            const char* mtApi = TaskManager::getApiName( gTaskMgr.getApi() );
+            const char* mtApi = btGetTaskScheduler()->getName();
             sprintf( msg, "islands=%d bodies=%d manifolds=%d contacts=%d [%s] threads=%d",
                      gNumIslands,
                      m_dynamicsWorld->getNumCollisionObjects(),
                      numManifolds,
                      numContacts,
                      mtApi,
-                     gTaskMgr.getApi() == TaskManager::apiNone ? 1 : gTaskMgr.getNumThreads()
+                     btGetTaskScheduler()->getNumThreads()
                      );
             m_guiHelper->getAppInterface()->drawText( msg, 100, yCoord, 0.4f );
             yCoord += yStep;
+#endif // #if BT_THREADSAFE
         }
         {
             int sm = gSolverMode;

examples/MultiThreadedDemo/MultiThreadedDemo.cpp

@@ -28,11 +28,10 @@ class btCollisionShape;
 #include "btBulletCollisionCommon.h"
 
 
-#define BT_OVERRIDE
 
 static btScalar gSliderStackRows = 8.0f;
 static btScalar gSliderStackColumns = 6.0f;
-static btScalar gSliderStackHeight = 15.0f;
+static btScalar gSliderStackHeight = 10.0f;
 static btScalar gSliderGroundHorizontalAmplitude = 0.0f;
 static btScalar gSliderGroundVerticalAmplitude = 0.0f;
 
@@ -240,7 +239,7 @@ void MultiThreadedDemo::createSceneObjects()
         const btVector3 halfExtents = btVector3( 0.5f, 0.25f, 0.5f );
         int numStackRows = btMax(1, int(gSliderStackRows));
         int numStackCols = btMax(1, int(gSliderStackColumns));
-        int stackHeight = 15;
+        int stackHeight = int(gSliderStackHeight);
         float stackZSpacing = 3.0f;
         float stackXSpacing = 20.0f;
 

examples/MultiThreadedDemo/ParallelFor.h

@@ -1,336 +0,0 @@
-/*
-Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
-
-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.
-*/
-
-#include <stdio.h> //printf debugging
-#include <algorithm>
-
-
-// choose threading providers:
-#if BT_USE_TBB
-#define USE_TBB 1     // use Intel Threading Building Blocks for thread management
-#endif
-
-#if BT_USE_PPL
-#define USE_PPL 1     // use Microsoft Parallel Patterns Library (installed with Visual Studio 2010 and later)
-#endif // BT_USE_PPL
-
-#if BT_USE_OPENMP
-#define USE_OPENMP 1  // use OpenMP (also need to change compiler options for OpenMP support)
-#endif
-
-
-#if USE_OPENMP
-
-#include <omp.h>
-
-#endif // #if USE_OPENMP
-
-
-#if USE_PPL
-
-#include <ppl.h>  // if you get a compile error here, check whether your version of Visual Studio includes PPL
-// Visual Studio 2010 and later should come with it
-#include <concrtrm.h>  // for GetProcessorCount()
-#endif // #if USE_PPL
-
-
-#if USE_TBB
-
-#define __TBB_NO_IMPLICIT_LINKAGE 1
-#include <tbb/tbb.h>
-#include <tbb/task_scheduler_init.h>
-#include <tbb/parallel_for.h>
-#include <tbb/blocked_range.h>
-
-#endif // #if USE_TBB
-
-
-
-class TaskManager
-{
-public:
-    enum Api
-    {
-        apiNone,
-        apiOpenMP,
-        apiTbb,
-        apiPpl,
-        apiCount
-    };
-    static const char* getApiName( Api api )
-    {
-        switch ( api )
-        {
-        case apiNone: return "None";
-        case apiOpenMP: return "OpenMP";
-        case apiTbb: return "Intel TBB";
-        case apiPpl: return "MS PPL";
-        default: return "unknown";
-        }
-    }
-
-    TaskManager()
-    {
-        m_api = apiNone;
-        m_numThreads = 0;
-#if USE_TBB
-        m_tbbSchedulerInit = NULL;
-#endif // #if USE_TBB
-    }
-
-    Api getApi() const
-    {
-        return m_api;
-    }
-
-    bool isSupported( Api api ) const
-    {
-#if USE_OPENMP
-        if ( api == apiOpenMP )
-        {
-            return true;
-        }
-#endif
-#if USE_TBB
-        if ( api == apiTbb )
-        {
-            return true;
-        }
-#endif
-#if USE_PPL
-        if ( api == apiPpl )
-        {
-            return true;
-        }
-#endif
-        // apiNone is always "supported"
-        return api == apiNone;
-    }
-
-    void setApi( Api api )
-    {
-        if (isSupported(api))
-        {
-            m_api = api;
-        }
-        else
-        {
-            // no compile time support for selected API, fallback to "none"
-            m_api = apiNone;
-        }
-    }
-
-    static int getMaxNumThreads()
-    {
-#if USE_OPENMP
-        return omp_get_max_threads();
-#elif USE_PPL
-        return concurrency::GetProcessorCount();
-#elif USE_TBB
-        return tbb::task_scheduler_init::default_num_threads();
-#endif
-        return 1;
-    }
-
-    int getNumThreads() const
-    {
-        return m_numThreads;
-    }
-
-    int setNumThreads( int numThreads )
-    {
-        m_numThreads = ( std::max )( 1, numThreads );
-
-#if USE_OPENMP
-        omp_set_num_threads( m_numThreads );
-#endif
-
-#if USE_PPL
-        {
-            using namespace concurrency;
-            if ( CurrentScheduler::Id() != -1 )
-            {
-                CurrentScheduler::Detach();
-            }
-            SchedulerPolicy policy;
-            policy.SetConcurrencyLimits( m_numThreads, m_numThreads );
-            CurrentScheduler::Create( policy );
-        }
-#endif
-
-#if USE_TBB
-        if ( m_tbbSchedulerInit )
-        {
-            delete m_tbbSchedulerInit;
-            m_tbbSchedulerInit = NULL;
-        }
-        m_tbbSchedulerInit = new tbb::task_scheduler_init( m_numThreads );
-#endif
-        return m_numThreads;
-    }
-
-    void init()
-    {
-        if (m_numThreads == 0)
-        {
-#if USE_PPL
-            setApi( apiPpl );
-#endif
-#if USE_TBB
-            setApi( apiTbb );
-#endif
-#if USE_OPENMP
-            setApi( apiOpenMP );
-#endif
-            setNumThreads(getMaxNumThreads());
-        }
-        else
-        {
-            setNumThreads(m_numThreads);
-        }
-    }
-
-    void shutdown()
-    {
-#if USE_TBB
-        if ( m_tbbSchedulerInit )
-        {
-            delete m_tbbSchedulerInit;
-            m_tbbSchedulerInit = NULL;
-        }
-#endif
-    }
-
-private:
-    Api m_api;
-    int m_numThreads;
-#if USE_TBB
-    tbb::task_scheduler_init* m_tbbSchedulerInit;
-#endif // #if USE_TBB
-};
-
-extern TaskManager gTaskMgr;
-
-
-inline static void initTaskScheduler()
-{
-    gTaskMgr.init();
-}
-
-inline static void cleanupTaskScheduler()
-{
-    gTaskMgr.shutdown();
-}
-
-
-#if USE_TBB
-///
-/// TbbBodyAdapter -- Converts a body object that implements the
-///                   "forLoop(int iBegin, int iEnd) const" function
-///  into a TBB compatible object that takes a tbb::blocked_range<int> type.
-///
-template <class TBody>
-struct TbbBodyAdapter
-{
-    const TBody* mBody;
-
-    void operator()( const tbb::blocked_range<int>& range ) const
-    {
-        mBody->forLoop( range.begin(), range.end() );
-    }
-};
-#endif // #if USE_TBB
-
-#if USE_PPL
-///
-/// PplBodyAdapter -- Converts a body object that implements the
-///                   "forLoop(int iBegin, int iEnd) const" function
-///  into a PPL compatible object that implements "void operator()( int ) const"
-///
-template <class TBody>
-struct PplBodyAdapter
-{
-    const TBody* mBody;
-    int mGrainSize;
-    int mIndexEnd;
-
-    void operator()( int i ) const
-    {
-        mBody->forLoop( i, (std::min)(i + mGrainSize, mIndexEnd) );
-    }
-};
-#endif // #if USE_PPL
-
-
-///
-/// parallelFor -- interface for submitting work expressed as a for loop to the worker threads
-///
-template <class TBody>
-void parallelFor( int iBegin, int iEnd, int grainSize, const TBody& body )
-{
-#if USE_OPENMP
-    if ( gTaskMgr.getApi() == TaskManager::apiOpenMP )
-    {
-#pragma omp parallel for schedule(static, 1)
-        for ( int i = iBegin; i < iEnd; i += grainSize )
-        {
-            body.forLoop( i, (std::min)( i + grainSize, iEnd ) );
-        }
-        return;
-    }
-#endif // #if USE_OPENMP
-
-#if USE_PPL
-    if ( gTaskMgr.getApi() == TaskManager::apiPpl )
-    {
-        // PPL dispatch
-        PplBodyAdapter<TBody> pplBody;
-        pplBody.mBody = &body;
-        pplBody.mGrainSize = grainSize;
-        pplBody.mIndexEnd = iEnd;
-        // note: MSVC 2010 doesn't support partitioner args, so avoid them
-        concurrency::parallel_for( iBegin,
-                                   iEnd,
-                                   grainSize,
-                                   pplBody
-                                   );
-        return;
-    }
-#endif //#if USE_PPL
-
-#if USE_TBB
-    if ( gTaskMgr.getApi() == TaskManager::apiTbb )
-    {
-        // TBB dispatch
-        TbbBodyAdapter<TBody> tbbBody;
-        tbbBody.mBody = &body;
-        tbb::parallel_for( tbb::blocked_range<int>( iBegin, iEnd, grainSize ),
-                           tbbBody,
-                           tbb::simple_partitioner()
-                           );
-        return;
-    }
-#endif // #if USE_TBB
-
-    {
-        // run on main thread
-        body.forLoop( iBegin, iEnd );
-    }
-
-}
-
-
-
-

examples/MultiThreading/b3Win32ThreadSupport.cpp

@@ -279,9 +279,9 @@ void b3Win32ThreadSupport::startThreads(const Win32ThreadConstructionInfo& threa
 
 		}
 		
-		DWORD mask = 1;
+        //SetThreadAffinityMask(handle, 1 << 1); // this is what it was doing originally, a complete disaster for threading performance!
-		mask = 1<<mask;
+		//SetThreadAffinityMask(handle, 1 << i); // I'm guessing this was the intention, but is still bad for performance due to one of the threads
-		SetThreadAffinityMask(handle, mask);
+        //  sometimes unable to execute because it wants to be on the same processor as the main thread (my guess)
 
 		threadStatus.m_taskId = i;
 		threadStatus.m_commandId = 0;

examples/MultiThreading/btTaskScheduler.cpp

@@ -0,0 +1,448 @@
+
+#include "LinearMath/btTransform.h"
+#include "../Utils/b3Clock.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btThreads.h"
+#include "LinearMath/btQuickprof.h"
+#include <stdio.h>
+#include <algorithm>
+
+
+typedef void( *btThreadFunc )( void* userPtr, void* lsMemory );
+typedef void* ( *btThreadLocalStorageFunc )();
+
+#if BT_THREADSAFE
+
+#if defined( _WIN32 )
+
+#include "b3Win32ThreadSupport.h"
+
+b3ThreadSupportInterface* createThreadSupport( int numThreads, btThreadFunc threadFunc, btThreadLocalStorageFunc localStoreFunc, const char* uniqueName )
+{
+    b3Win32ThreadSupport::Win32ThreadConstructionInfo constructionInfo( uniqueName, threadFunc, localStoreFunc, numThreads );
+    //constructionInfo.m_priority = 0;  // highest priority (the default) -- can cause erratic performance when numThreads > numCores
+    //                                     we don't want worker threads to be higher priority than the main thread or the main thread could get
+    //                                     totally shut out and unable to tell the workers to stop
+    constructionInfo.m_priority = -1;  // normal priority
+    b3Win32ThreadSupport* threadSupport = new b3Win32ThreadSupport( constructionInfo );
+    return threadSupport;
+}
+
+#else // #if defined( _WIN32 )
+
+#include "b3PosixThreadSupport.h"
+
+b3ThreadSupportInterface* createThreadSupport( int numThreads, btThreadFunc threadFunc, btThreadLocalStorageFunc localStoreFunc, const char* uniqueName)
+{
+    b3PosixThreadSupport::ThreadConstructionInfo constructionInfo( uniqueName, threadFunc, localStoreFunc, numThreads );
+    b3ThreadSupportInterface* threadSupport = new b3PosixThreadSupport( constructionInfo );
+    return threadSupport;
+}
+
+#endif // #else // #if defined( _WIN32 )
+
+
+///
+/// getNumHardwareThreads()
+///
+///
+/// https://stackoverflow.com/questions/150355/programmatically-find-the-number-of-cores-on-a-machine
+///
+#if __cplusplus >= 201103L
+
+#include <thread>
+
+int getNumHardwareThreads()
+{
+    return std::thread::hardware_concurrency();
+}
+
+#elif defined( _WIN32 )
+
+#define WIN32_LEAN_AND_MEAN
+
+#include <windows.h>
+
+int getNumHardwareThreads()
+{
+    // caps out at 32
+    SYSTEM_INFO info;
+    GetSystemInfo( &info );
+    return info.dwNumberOfProcessors;
+}
+
+#else
+
+int getNumHardwareThreads()
+{
+    return 0;  // don't know
+}
+
+#endif
+
+
+struct WorkerThreadStatus
+{
+    enum Type
+    {
+        kInvalid,
+        kWaitingForWork,
+        kWorking,
+        kSleeping,
+    };
+};
+
+
+struct IJob
+{
+    virtual void executeJob() = 0;
+};
+
+class ParallelForJob : public IJob
+{
+    const btIParallelForBody* mBody;
+    int mBegin;
+    int mEnd;
+
+public:
+    ParallelForJob()
+    {
+        mBody = NULL;
+        mBegin = 0;
+        mEnd = 0;
+    }
+    void init( int iBegin, int iEnd, const btIParallelForBody& body )
+    {
+        mBody = &body;
+        mBegin = iBegin;
+        mEnd = iEnd;
+    }
+    virtual void executeJob() override
+    {
+        BT_PROFILE( "executeJob" );
+
+        // call the functor body to do the work
+        mBody->forLoop( mBegin, mEnd );
+    }
+};
+
+
+struct JobContext
+{
+    JobContext()
+    {
+        m_queueLock = NULL;
+        m_headIndex = 0;
+        m_tailIndex = 0;
+        m_workersShouldCheckQueue = false;
+        m_useSpinMutex = false;
+    }
+    b3CriticalSection* m_queueLock;
+    btSpinMutex m_mutex;
+    volatile bool m_workersShouldCheckQueue;
+
+    btAlignedObjectArray<IJob*> m_jobQueue;
+    bool m_queueIsEmpty;
+    int m_tailIndex;
+    int m_headIndex;
+    bool m_useSpinMutex;
+
+    void lockQueue()
+    {
+        if ( m_useSpinMutex )
+        {
+            m_mutex.lock();
+        }
+        else
+        {
+            m_queueLock->lock();
+        }
+    }
+    void unlockQueue()
+    {
+        if ( m_useSpinMutex )
+        {
+            m_mutex.unlock();
+        }
+        else
+        {
+            m_queueLock->unlock();
+        }
+    }
+    void clearQueue()
+    {
+        lockQueue();
+        m_headIndex = 0;
+        m_tailIndex = 0;
+        m_queueIsEmpty = true;
+        unlockQueue();
+        m_jobQueue.resizeNoInitialize( 0 );
+    }
+    void submitJob( IJob* job )
+    {
+        m_jobQueue.push_back( job );
+        lockQueue();
+        m_tailIndex++;
+        m_queueIsEmpty = false;
+        unlockQueue();
+    }
+    IJob* consumeJob()
+    {
+        if ( m_queueIsEmpty )
+        {
+            // lock free path. even if this is taken erroneously it isn't harmful
+            return NULL;
+        }
+        IJob* job = NULL;
+        lockQueue();
+        if ( !m_queueIsEmpty )
+        {
+            job = m_jobQueue[ m_headIndex++ ];
+            if ( m_headIndex == m_tailIndex )
+            {
+                m_queueIsEmpty = true;
+            }
+        }
+        unlockQueue();
+        return job;
+    }
+};
+
+
+struct WorkerThreadLocalStorage
+{
+    int threadId;
+    WorkerThreadStatus::Type status;
+};
+
+
+static void WorkerThreadFunc( void* userPtr, void* lsMemory )
+{
+    BT_PROFILE( "WorkerThreadFunc" );
+    WorkerThreadLocalStorage* localStorage = (WorkerThreadLocalStorage*) lsMemory;
+    localStorage->status = WorkerThreadStatus::kWaitingForWork;
+    //printf( "WorkerThreadFunc: worker %d start working\n", localStorage->threadId );
+
+    JobContext* jobContext = (JobContext*) userPtr;
+
+    while ( jobContext->m_workersShouldCheckQueue )
+    {
+        if ( IJob* job = jobContext->consumeJob() )
+        {
+            localStorage->status = WorkerThreadStatus::kWorking;
+            job->executeJob();
+            localStorage->status = WorkerThreadStatus::kWaitingForWork;
+        }
+        else
+        {
+            // todo: spin wait a bit to avoid hammering the empty queue
+        }
+    }
+
+    //printf( "WorkerThreadFunc stop working\n" );
+    localStorage->status = WorkerThreadStatus::kSleeping;
+    // go idle
+}
+
+
+static void* WorkerThreadAllocFunc()
+{
+    return new WorkerThreadLocalStorage;
+}
+
+
+
+class btTaskSchedulerDefault : public btITaskScheduler
+{
+    JobContext m_jobContext;
+    b3ThreadSupportInterface* m_threadSupport;
+    btAlignedObjectArray<ParallelForJob> m_jobs;
+    btSpinMutex m_antiNestingLock;  // prevent nested parallel-for
+    int m_numThreads;
+    int m_numWorkerThreads;
+    int m_numWorkersRunning;
+public:
+
+    btTaskSchedulerDefault() : btITaskScheduler("ThreadSupport")
+    {
+        m_threadSupport = NULL;
+        m_numThreads = getNumHardwareThreads();
+        // if can't detect number of cores,
+        if ( m_numThreads == 0 )
+        {
+            // take a guess
+            m_numThreads = 4;
+        }
+        m_numWorkerThreads = m_numThreads - 1;
+        m_numWorkersRunning = 0;
+    }
+
+    virtual ~btTaskSchedulerDefault()
+    {
+        shutdown();
+    }
+
+    void init()
+    {
+        int maxNumWorkerThreads = BT_MAX_THREAD_COUNT - 1;
+        m_threadSupport = createThreadSupport( maxNumWorkerThreads, WorkerThreadFunc, WorkerThreadAllocFunc, "TaskScheduler" );
+        m_jobContext.m_queueLock = m_threadSupport->createCriticalSection();
+        for ( int i = 0; i < maxNumWorkerThreads; i++ )
+        {
+            WorkerThreadLocalStorage* storage = (WorkerThreadLocalStorage*) m_threadSupport->getThreadLocalMemory( i );
+            btAssert( storage );
+            storage->threadId = i;
+            storage->status = WorkerThreadStatus::kSleeping;
+        }
+        setWorkersActive( false ); // no work for them yet
+    }
+
+    virtual void shutdown()
+    {
+        setWorkersActive( false );
+        waitForWorkersToSleep();
+        m_threadSupport->deleteCriticalSection( m_jobContext.m_queueLock );
+        m_jobContext.m_queueLock = NULL;
+
+        delete m_threadSupport;
+        m_threadSupport = NULL;
+    }
+
+    void setWorkersActive( bool active )
+    {
+        m_jobContext.m_workersShouldCheckQueue = active;
+    }
+
+    virtual int getMaxNumThreads() const BT_OVERRIDE
+    {
+        return BT_MAX_THREAD_COUNT;
+    }
+
+    virtual int getNumThreads() const BT_OVERRIDE
+    {
+        return m_numThreads;
+    }
+
+    virtual void setNumThreads( int numThreads ) BT_OVERRIDE
+    {
+        m_numThreads = btMax( btMin(numThreads, int(BT_MAX_THREAD_COUNT)), 1 );
+        m_numWorkerThreads = m_numThreads - 1;
+    }
+
+    void waitJobs()
+    {
+        BT_PROFILE( "waitJobs" );
+        // have the main thread work until the job queue is empty
+        for ( ;; )
+        {
+            if ( IJob* job = m_jobContext.consumeJob() )
+            {
+                job->executeJob();
+            }
+            else
+            {
+                break;
+            }
+        }
+        // done with jobs for now, tell workers to rest
+        setWorkersActive( false );
+        waitForWorkersToSleep();
+    }
+
+    void wakeWorkers()
+    {
+        BT_PROFILE( "wakeWorkers" );
+        btAssert( m_jobContext.m_workersShouldCheckQueue );
+        // tell each worker thread to start working
+        for ( int i = 0; i < m_numWorkerThreads; i++ )
+        {
+            m_threadSupport->runTask( B3_THREAD_SCHEDULE_TASK, &m_jobContext, i );
+            m_numWorkersRunning++;
+        }
+    }
+
+    void waitForWorkersToSleep()
+    {
+        BT_PROFILE( "waitForWorkersToSleep" );
+        while ( m_numWorkersRunning > 0 )
+        {
+            int iThread;
+            int threadStatus;
+            m_threadSupport->waitForResponse( &iThread, &threadStatus );  // wait for worker threads to finish working
+            m_numWorkersRunning--;
+        }
+        //m_threadSupport->waitForAllTasksToComplete();
+        for ( int i = 0; i < m_numWorkerThreads; i++ )
+        {
+            //m_threadSupport->waitForTaskCompleted( i );
+            WorkerThreadLocalStorage* storage = (WorkerThreadLocalStorage*) m_threadSupport->getThreadLocalMemory( i );
+            btAssert( storage );
+            btAssert( storage->status == WorkerThreadStatus::kSleeping );
+        }
+    }
+
+    virtual void parallelFor( int iBegin, int iEnd, int grainSize, const btIParallelForBody& body ) BT_OVERRIDE
+    {
+        BT_PROFILE( "parallelFor_ThreadSupport" );
+        btAssert( iEnd >= iBegin );
+        btAssert( grainSize >= 1 );
+        int iterationCount = iEnd - iBegin;
+        if ( iterationCount > grainSize && m_numWorkerThreads > 0 && m_antiNestingLock.tryLock() )
+        {
+            int jobCount = ( iterationCount + grainSize - 1 ) / grainSize;
+            btAssert( jobCount >= 2 );  // need more than one job for multithreading
+            if ( jobCount > m_jobs.size() )
+            {
+                m_jobs.resize( jobCount );
+            }
+            if ( jobCount > m_jobContext.m_jobQueue.capacity() )
+            {
+                m_jobContext.m_jobQueue.reserve( jobCount );
+            }
+
+            m_jobContext.clearQueue();
+            // prepare worker threads for incoming work
+            setWorkersActive( true );
+            wakeWorkers();
+            // submit all of the jobs
+            int iJob = 0;
+            for ( int i = iBegin; i < iEnd; i += grainSize )
+            {
+                btAssert( iJob < jobCount );
+                int iE = btMin( i + grainSize, iEnd );
+                ParallelForJob& job = m_jobs[ iJob ];
+                job.init( i, iE, body );
+                m_jobContext.submitJob( &job );
+                iJob++;
+            }
+
+            // put the main thread to work on emptying the job queue and then wait for all workers to finish
+            waitJobs();
+            m_antiNestingLock.unlock();
+        }
+        else
+        {
+            BT_PROFILE( "parallelFor_mainThread" );
+            // just run on main thread
+            body.forLoop( iBegin, iEnd );
+        }
+    }
+};
+
+
+
+btITaskScheduler* createDefaultTaskScheduler()
+{
+    btTaskSchedulerDefault* ts = new btTaskSchedulerDefault();
+    ts->init();
+    return ts;
+}
+
+#else // #if BT_THREADSAFE
+
+btITaskScheduler* createDefaultTaskScheduler()
+{
+    return NULL;
+}
+
+#endif // #else // #if BT_THREADSAFE

examples/MultiThreading/btTaskScheduler.h

@@ -0,0 +1,26 @@
+/*
+Copyright (c) 2003-2014 Erwin Coumans  http://bullet.googlecode.com
+
+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 BT_TASK_SCHEDULER_H
+#define BT_TASK_SCHEDULER_H
+
+
+class btITaskScheduler;
+
+btITaskScheduler* createDefaultTaskScheduler();
+
+
+#endif // BT_TASK_SCHEDULER_H

examples/OpenGLWindow/GLInstancingRenderer.cpp

@@ -129,6 +129,8 @@ struct b3GraphicsInstance
 	int m_instanceOffset;
 	int m_vertexArrayOffset;
 	int	m_primitiveType;
+	float m_materialShinyNess;
+	b3Vector3 m_materialSpecularColor;
 
 	b3GraphicsInstance()
 	:m_cube_vao(-1),
@@ -139,7 +141,9 @@ struct b3GraphicsInstance
 		m_numGraphicsInstances(0),
 		m_instanceOffset(0),
 		m_vertexArrayOffset(0),
-		m_primitiveType(B3_GL_TRIANGLES)
+		m_primitiveType(B3_GL_TRIANGLES),
+		m_materialShinyNess(41),
+		m_materialSpecularColor(b3MakeVector3(.5,.5,.5))
 	{
 	}
 
@@ -204,8 +208,10 @@ struct InternalDataRenderer : public GLInstanceRendererInternalData
     
 	GLfloat m_projectionMatrix[16];
 	GLfloat m_viewMatrix[16];
+	GLfloat m_viewMatrixInverse[16];
 
 	b3Vector3 m_lightPos;
+	b3Vector3 m_lightSpecularIntensity;
 
 	GLuint				m_defaultTexturehandle;
 	b3AlignedObjectArray<InternalTextureHandle>	m_textureHandles;
@@ -225,13 +231,15 @@ struct InternalDataRenderer : public GLInstanceRendererInternalData
 		m_shadowTexture(0),
 		m_renderFrameBuffer(0)
 	{
-		m_lightPos=b3MakeVector3(-50,50,50);
+		m_lightPos=b3MakeVector3(-50,30,40);
+		m_lightSpecularIntensity.setValue(1,1,1);
 
 		//clear to zero to make it obvious if the matrix is used uninitialized
 		for (int i=0;i<16;i++)
 		{
 			m_projectionMatrix[i]=0;
 			m_viewMatrix[i]=0;
+			m_viewMatrixInverse[i]=0;
 		}
 
 	}
@@ -280,9 +288,14 @@ GLuint linesVertexArrayObject=0;
 GLuint linesIndexVbo = 0;
 
 
+static GLint	useShadow_ViewMatrixInverse=0;
 static GLint	useShadow_ModelViewMatrix=0;
+static GLint	useShadow_lightSpecularIntensity = 0;
+static GLint	useShadow_materialSpecularColor = 0;
 static GLint	useShadow_MVP=0;
-static GLint  useShadow_lightDirIn=0;
+static GLint	useShadow_lightPosIn=0;
+static GLint	useShadow_cameraPositionIn = 0;
+static GLint	useShadow_materialShininessIn = 0;
 
 static GLint	useShadow_ProjectionMatrix=0;
 static GLint	useShadow_DepthBiasModelViewMatrix=0;
@@ -296,7 +309,7 @@ static GLint	ProjectionMatrix=0;
 static GLint	regularLightDirIn=0;
 
 
-static GLint                uniform_texture_diffuse = 0;
+static GLint	uniform_texture_diffuse = 0;
 
 static GLint	screenWidthPointSprite=0;
 static GLint	ModelViewMatrixPointSprite=0;
@@ -400,11 +413,15 @@ bool GLInstancingRenderer::readSingleInstanceTransformToCPU(float* position, flo
 	return false;
 }
 
-void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int bodyUniqueId)
+void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex2)
 {
 
-	b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
+	b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
 	b3Assert(pg);
+
+	if (pg==0)
+		return;
+
 	int srcIndex = pg->m_internalInstanceIndex;
 
 	b3Assert(srcIndex<m_data->m_totalNumInstances);
@@ -422,9 +439,9 @@ void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* positi
 }
 
 
-void GLInstancingRenderer::readSingleInstanceTransformFromCPU(int bodyUniqueId, float* position, float* orientation)
+void GLInstancingRenderer::readSingleInstanceTransformFromCPU(int srcIndex2, float* position, float* orientation)
 {
-	b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
+	b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
 	b3Assert(pg);
 	int srcIndex = pg->m_internalInstanceIndex;
 
@@ -440,9 +457,9 @@ void GLInstancingRenderer::readSingleInstanceTransformFromCPU(int bodyUniqueId,
 	orientation[2] = m_data->m_instance_quaternion_ptr[srcIndex*4+2];
 	orientation[3] = m_data->m_instance_quaternion_ptr[srcIndex*4+3];
 }
-void GLInstancingRenderer::writeSingleInstanceColorToCPU(const double* color, int bodyUniqueId)
+void GLInstancingRenderer::writeSingleInstanceColorToCPU(const double* color, int srcIndex2)
 {
-	b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
+	b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
 	b3Assert(pg);
 	int srcIndex = pg->m_internalInstanceIndex;
 
@@ -452,9 +469,9 @@ void GLInstancingRenderer::writeSingleInstanceColorToCPU(const double* color, in
 	m_data->m_instance_colors_ptr[srcIndex*4+3]=float(color[3]);
 }
 
-void GLInstancingRenderer::writeSingleInstanceColorToCPU(const float* color, int bodyUniqueId)
+void GLInstancingRenderer::writeSingleInstanceColorToCPU(const float* color, int srcIndex2)
 {
-	b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
+	b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
 	b3Assert(pg);
 	int srcIndex = pg->m_internalInstanceIndex;
 
@@ -464,9 +481,9 @@ void GLInstancingRenderer::writeSingleInstanceColorToCPU(const float* color, int
 	m_data->m_instance_colors_ptr[srcIndex*4+3]=color[3];
 }
 
-void GLInstancingRenderer::writeSingleInstanceScaleToCPU(const float* scale, int bodyUniqueId)
+void GLInstancingRenderer::writeSingleInstanceScaleToCPU(const float* scale, int srcIndex2)
 {
-	b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
+	b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
 	b3Assert(pg);
 	int srcIndex = pg->m_internalInstanceIndex;
 
@@ -475,9 +492,63 @@ void GLInstancingRenderer::writeSingleInstanceScaleToCPU(const float* scale, int
 	m_data->m_instance_scale_ptr[srcIndex*3+2]=scale[2];
 }
 
-void GLInstancingRenderer::writeSingleInstanceScaleToCPU(const double* scale, int bodyUniqueId)
+void GLInstancingRenderer::writeSingleInstanceSpecularColorToCPU(const double* specular, int srcIndex2)
 {
-	b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
+	b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
+	b3Assert(pg);
+	int graphicsIndex = pg->m_internalInstanceIndex;
+
+	int totalNumInstances = 0;
+
+	int gfxObjIndex = -1;
+
+	for (int i=0;i<m_graphicsInstances.size();i++)
+	{
+		totalNumInstances+=m_graphicsInstances[i]->m_numGraphicsInstances;
+		if (srcIndex2<totalNumInstances)
+		{
+			gfxObjIndex = i;
+			break;
+		}
+	}
+	if (gfxObjIndex>0)
+	{
+		m_graphicsInstances[gfxObjIndex]->m_materialSpecularColor[0] = specular[0];
+		m_graphicsInstances[gfxObjIndex]->m_materialSpecularColor[1] = specular[1];
+		m_graphicsInstances[gfxObjIndex]->m_materialSpecularColor[2] = specular[2];
+	}
+}
+void GLInstancingRenderer::writeSingleInstanceSpecularColorToCPU(const float* specular, int srcIndex2)
+{
+	b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
+	b3Assert(pg);
+	int srcIndex = pg->m_internalInstanceIndex;
+
+	int totalNumInstances = 0;
+
+	int gfxObjIndex = -1;
+
+	for (int i=0;i<m_graphicsInstances.size();i++)
+	{
+		totalNumInstances+=m_graphicsInstances[i]->m_numGraphicsInstances;
+		if (srcIndex2<totalNumInstances)
+		{
+			gfxObjIndex = i;
+			break;
+		}
+	}
+	if (gfxObjIndex>0)
+	{
+		m_graphicsInstances[gfxObjIndex]->m_materialSpecularColor[0] = specular[0];
+		m_graphicsInstances[gfxObjIndex]->m_materialSpecularColor[1] = specular[1];
+		m_graphicsInstances[gfxObjIndex]->m_materialSpecularColor[2] = specular[2];
+	}
+}
+
+
+void GLInstancingRenderer::writeSingleInstanceScaleToCPU(const double* scale, int srcIndex2)
+{
+	b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
 	b3Assert(pg);
 	int srcIndex = pg->m_internalInstanceIndex;
 
@@ -694,8 +765,8 @@ void GLInstancingRenderer::rebuildGraphicsInstances()
 
 	for (int i=0;i<usedObjects.size();i++)
 	{
-		int bodyUniqueId = usedObjects[i];
+		int srcIndex2 = usedObjects[i];
-		b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
+		b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
 		b3Assert(pg);
 		int srcIndex = pg->m_internalInstanceIndex;
 
@@ -722,9 +793,9 @@ void GLInstancingRenderer::rebuildGraphicsInstances()
 	}
 	for (int i=0;i<usedObjects.size();i++)
 	{
-		int bodyUniqueId = usedObjects[i];
+		int srcIndex2 = usedObjects[i];
-		b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(bodyUniqueId);
+		b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex2);
-		m_graphicsInstances[pg->m_shapeIndex]->m_tempObjectUids.push_back(bodyUniqueId);
+		m_graphicsInstances[pg->m_shapeIndex]->m_tempObjectUids.push_back(srcIndex2);
 	}
 
 	int curOffset = 0;
@@ -1099,13 +1170,18 @@ void GLInstancingRenderer::InitShaders()
 
 	glLinkProgram(useShadowMapInstancingShader);
 	glUseProgram(useShadowMapInstancingShader);
+	useShadow_ViewMatrixInverse = glGetUniformLocation(useShadowMapInstancingShader, "ViewMatrixInverse");
 	useShadow_ModelViewMatrix = glGetUniformLocation(useShadowMapInstancingShader, "ModelViewMatrix");
+	useShadow_lightSpecularIntensity = glGetUniformLocation(useShadowMapInstancingShader, "lightSpecularIntensityIn");
+	useShadow_materialSpecularColor = glGetUniformLocation(useShadowMapInstancingShader, "materialSpecularColorIn");
 	useShadow_MVP = 		glGetUniformLocation(useShadowMapInstancingShader, "MVP");
 	useShadow_ProjectionMatrix = glGetUniformLocation(useShadowMapInstancingShader, "ProjectionMatrix");
 	useShadow_DepthBiasModelViewMatrix = glGetUniformLocation(useShadowMapInstancingShader, "DepthBiasModelViewProjectionMatrix");
 	useShadow_uniform_texture_diffuse = glGetUniformLocation(useShadowMapInstancingShader, "Diffuse");
 	useShadow_shadowMap = glGetUniformLocation(useShadowMapInstancingShader,"shadowMap");
-	useShadow_lightDirIn = glGetUniformLocation(useShadowMapInstancingShader,"lightDirIn");
+	useShadow_lightPosIn = glGetUniformLocation(useShadowMapInstancingShader,"lightPosIn");
+	useShadow_cameraPositionIn = glGetUniformLocation(useShadowMapInstancingShader,"cameraPositionIn");
+	useShadow_materialShininessIn = glGetUniformLocation(useShadowMapInstancingShader,"materialShininessIn");
 
 	createShadowMapInstancingShader = gltLoadShaderPair(createShadowMapInstancingVertexShader,createShadowMapInstancingFragmentShader);
 	glLinkProgram(createShadowMapInstancingShader);
@@ -1272,6 +1348,15 @@ void GLInstancingRenderer::setActiveCamera(CommonCameraInterface* cam)
 	m_data->m_activeCamera = cam;
 }
 
+void GLInstancingRenderer::setLightSpecularIntensity(const float lightSpecularIntensity[3])
+{
+	m_data->m_lightSpecularIntensity[0] = lightSpecularIntensity[0];
+	m_data->m_lightSpecularIntensity[1] = lightSpecularIntensity[1];
+	m_data->m_lightSpecularIntensity[2] = lightSpecularIntensity[2];
+}
+
+
+
 void GLInstancingRenderer::setLightPosition(const float lightPos[3])
 {
 	m_data->m_lightPos[0] = lightPos[0];
@@ -1298,6 +1383,21 @@ void GLInstancingRenderer::updateCamera(int upAxis)
 	m_data->m_defaultCamera1.update();
     m_data->m_activeCamera->getCameraProjectionMatrix(m_data->m_projectionMatrix);
     m_data->m_activeCamera->getCameraViewMatrix(m_data->m_viewMatrix);
+	b3Scalar viewMat[16];
+	b3Scalar viewMatInverse[16];
+	
+	for (int i=0;i<16;i++)
+	{
+		viewMat[i] = m_data->m_viewMatrix[i];
+	}
+	b3Transform tr;
+	tr.setFromOpenGLMatrix(viewMat);
+	tr = tr.inverse();
+	tr.getOpenGLMatrix(viewMatInverse);
+	for (int i=0;i<16;i++)
+	{
+		m_data->m_viewMatrixInverse[i]=viewMatInverse[i];
+	}
 
 
 }
@@ -2127,12 +2227,24 @@ b3Assert(glGetError() ==GL_NO_ERROR);
 							glUseProgram(useShadowMapInstancingShader);
 							glUniformMatrix4fv(useShadow_ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]);
 							glUniformMatrix4fv(useShadow_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]);
+							glUniformMatrix4fv(useShadow_ViewMatrixInverse, 1, false, &m_data->m_viewMatrixInverse[0]);
+							glUniformMatrix4fv(useShadow_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]);
+
+							glUniform3f(useShadow_lightSpecularIntensity, m_data->m_lightSpecularIntensity[0],m_data->m_lightSpecularIntensity[1],m_data->m_lightSpecularIntensity[2]);
+							glUniform3f(useShadow_materialSpecularColor, gfxObj->m_materialSpecularColor[0],gfxObj->m_materialSpecularColor[1],gfxObj->m_materialSpecularColor[2]);
+
+							
+
 							float MVP[16];
 							b3Matrix4x4Mul16(m_data->m_projectionMatrix,m_data->m_viewMatrix,MVP);
 							glUniformMatrix4fv(useShadow_MVP, 1, false, &MVP[0]);
-							b3Vector3 gLightDir = m_data->m_lightPos;
+							//gLightDir.normalize();
-							gLightDir.normalize();
+							glUniform3f(useShadow_lightPosIn,m_data->m_lightPos[0],m_data->m_lightPos[1],m_data->m_lightPos[2]);
-							glUniform3f(useShadow_lightDirIn,gLightDir[0],gLightDir[1],gLightDir[2]);
+							float camPos[3];
+							m_data->m_activeCamera->getCameraPosition(camPos);
+							glUniform3f(useShadow_cameraPositionIn,camPos[0],camPos[1],camPos[2]);
+							glUniform1f(useShadow_materialShininessIn,gfxObj->m_materialShinyNess);
+						
 							glUniformMatrix4fv(useShadow_DepthBiasModelViewMatrix, 1, false, &depthBiasMVP[0][0]);
 							glActiveTexture(GL_TEXTURE1);
 							glBindTexture(GL_TEXTURE_2D, m_data->m_shadowTexture);

examples/OpenGLWindow/GLInstancingRenderer.h

@@ -95,6 +95,7 @@ public:
 
     }
 
+
 	virtual void readSingleInstanceTransformFromCPU(int srcIndex, float* position, float* orientation);
 
 	virtual void writeSingleInstanceTransformToGPU(float* position, float* orientation, int srcIndex);
@@ -102,6 +103,9 @@ public:
 	virtual void writeSingleInstanceColorToCPU(const float* color, int srcIndex);
 	virtual void writeSingleInstanceColorToCPU(const double* color, int srcIndex);
 
+	virtual void writeSingleInstanceSpecularColorToCPU(const double* specular, int srcIndex2);
+	virtual void writeSingleInstanceSpecularColorToCPU(const float* specular, int srcIndex2);
+
 	virtual void writeSingleInstanceScaleToCPU(const float* scale, int srcIndex);
 	virtual void writeSingleInstanceScaleToCPU(const double* scale, int srcIndex);
 
@@ -124,6 +128,7 @@ public:
 	
 	virtual void setLightPosition(const float lightPos[3]);
 	virtual void setLightPosition(const double lightPos[3]);
+	void setLightSpecularIntensity(const float lightSpecularIntensity[3]);
 
 	virtual void	resize(int width, int height);
 	virtual int	getScreenWidth()

examples/OpenGLWindow/GLPrimitiveRenderer.cpp

@@ -1,10 +1,10 @@
 #ifndef NO_OPENGL3
 #include "GLPrimitiveRenderer.h"
 #include "GLPrimInternalData.h"
-//#include "Bullet3Common/b3Logging.h"
+#include "Bullet3Common/b3Scalar.h"
 #include "LoadShader.h"
 
-#include <assert.h>
+
 
 static const char* vertexShader3D= \
 "#version 150   \n"
@@ -75,27 +75,27 @@ m_screenHeight(screenHeight)
 	
 	m_data->m_viewmatUniform = glGetUniformLocation(m_data->m_shaderProg,"viewMatrix");
 	if (m_data->m_viewmatUniform < 0) {
-		assert(0);
+		b3Assert(0);
 	}
 	m_data->m_projMatUniform = glGetUniformLocation(m_data->m_shaderProg,"projMatrix");
 	if (m_data->m_projMatUniform < 0) {
-		assert(0);
+		b3Assert(0);
 	}
     m_data->m_positionUniform = glGetUniformLocation(m_data->m_shaderProg, "p");
     if (m_data->m_positionUniform < 0) {
-		assert(0);
+		b3Assert(0);
 	}
 	m_data->m_colourAttribute = glGetAttribLocation(m_data->m_shaderProg, "colour");
 	if (m_data->m_colourAttribute < 0) {
-        assert(0);
+        b3Assert(0);
     }
 	m_data->m_positionAttribute = glGetAttribLocation(m_data->m_shaderProg, "position");
 	if (m_data->m_positionAttribute < 0) {
-		assert(0);
+		b3Assert(0);
   	}
 	m_data->m_textureAttribute = glGetAttribLocation(m_data->m_shaderProg,"texuv");
 	if (m_data->m_textureAttribute < 0) {
-		assert(0);
+		b3Assert(0);
 	}
 
     loadBufferData();
@@ -127,7 +127,7 @@ void GLPrimitiveRenderer::loadBufferData()
     glBufferData(GL_ARRAY_BUFFER,  MAX_VERTICES2 * sizeof(PrimVertex), 0, GL_DYNAMIC_DRAW);
    
 
-    assert(glGetError()==GL_NO_ERROR);
+    b3Assert(glGetError()==GL_NO_ERROR);
     
     
     
@@ -153,14 +153,14 @@ void GLPrimitiveRenderer::loadBufferData()
 
     glEnableVertexAttribArray(m_data->m_positionAttribute);
     glEnableVertexAttribArray(m_data->m_colourAttribute);
-	assert(glGetError()==GL_NO_ERROR);
+	b3Assert(glGetError()==GL_NO_ERROR);
     
 	glEnableVertexAttribArray(m_data->m_textureAttribute);
     
     glVertexAttribPointer(m_data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)0);
     glVertexAttribPointer(m_data->m_colourAttribute  , 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)sizeof(PrimVec4));
     glVertexAttribPointer(m_data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)(sizeof(PrimVec4)+sizeof(PrimVec4)));
-	assert(glGetError()==GL_NO_ERROR);
+	b3Assert(glGetError()==GL_NO_ERROR);
     
     
     
@@ -200,7 +200,7 @@ void GLPrimitiveRenderer::loadBufferData()
     glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, 256,256,0,GL_RGB,GL_UNSIGNED_BYTE,image);
     glGenerateMipmap(GL_TEXTURE_2D);
     
-    assert(glGetError()==GL_NO_ERROR);
+    b3Assert(glGetError()==GL_NO_ERROR);
     
     delete[] image;
     
@@ -226,14 +226,14 @@ void GLPrimitiveRenderer::drawLine()
 
 void GLPrimitiveRenderer::drawRect(float x0, float y0, float x1, float y1, float color[4])
 {
-	assert(glGetError()==GL_NO_ERROR);
+	b3Assert(glGetError()==GL_NO_ERROR);
 	glActiveTexture(GL_TEXTURE0);
-	assert(glGetError()==GL_NO_ERROR);
+	b3Assert(glGetError()==GL_NO_ERROR);
 	
 	 glBindTexture(GL_TEXTURE_2D,m_data->m_texturehandle);
-	assert(glGetError()==GL_NO_ERROR);
+	b3Assert(glGetError()==GL_NO_ERROR);
 	drawTexturedRect(x0,y0,x1,y1,color,0,0,1,1);
-	assert(glGetError()==GL_NO_ERROR);
+	b3Assert(glGetError()==GL_NO_ERROR);
 
 }
 
@@ -242,7 +242,7 @@ void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVert
 {
 	//B3_PROFILE("GLPrimitiveRenderer::drawTexturedRect3D");
 
-    assert(glGetError()==GL_NO_ERROR);
+    b3Assert(glGetError()==GL_NO_ERROR);
    
     
     glUseProgram(m_data->m_shaderProg);
@@ -250,7 +250,7 @@ void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVert
 	glUniformMatrix4fv(m_data->m_viewmatUniform, 1, false, viewMat);
 	glUniformMatrix4fv(m_data->m_projMatUniform, 1, false, projMat);
 
-    assert(glGetError()==GL_NO_ERROR);
+    b3Assert(glGetError()==GL_NO_ERROR);
     
     glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vertexBuffer);
     glBindVertexArray(m_data->m_vertexArrayObject);
@@ -277,7 +277,7 @@ void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVert
     
     
     
-    assert(glGetError()==GL_NO_ERROR);
+    b3Assert(glGetError()==GL_NO_ERROR);
     
     PrimVec2 p( 0.f,0.f);//?b?0.5f * sinf(timeValue), 0.5f * cosf(timeValue) );
 	if (useRGBA)
@@ -290,47 +290,47 @@ void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVert
     
 	glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
 
-    assert(glGetError()==GL_NO_ERROR);
+    b3Assert(glGetError()==GL_NO_ERROR);
     
     glEnableVertexAttribArray(m_data->m_positionAttribute);
-	assert(glGetError()==GL_NO_ERROR);
+	b3Assert(glGetError()==GL_NO_ERROR);
     
     glEnableVertexAttribArray(m_data->m_colourAttribute);
-	assert(glGetError()==GL_NO_ERROR);
+	b3Assert(glGetError()==GL_NO_ERROR);
     
 	glEnableVertexAttribArray(m_data->m_textureAttribute);
     
     glVertexAttribPointer(m_data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)0);
     glVertexAttribPointer(m_data->m_colourAttribute  , 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)sizeof(PrimVec4));
     glVertexAttribPointer(m_data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)(sizeof(PrimVec4)+sizeof(PrimVec4)));
-	assert(glGetError()==GL_NO_ERROR);
+	b3Assert(glGetError()==GL_NO_ERROR);
     
     glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_data->m_indexBuffer);
     
     //glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
     int indexCount = 6;
-    assert(glGetError()==GL_NO_ERROR);
+    b3Assert(glGetError()==GL_NO_ERROR);
     
 	
     glDrawElements(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, 0);
-    assert(glGetError()==GL_NO_ERROR);
+    b3Assert(glGetError()==GL_NO_ERROR);
 	
 	
     glBindVertexArray(0);
-    assert(glGetError()==GL_NO_ERROR);
+    b3Assert(glGetError()==GL_NO_ERROR);
 
 	glBindBuffer(GL_ARRAY_BUFFER, 0);
-    assert(glGetError()==GL_NO_ERROR);
+    b3Assert(glGetError()==GL_NO_ERROR);
 
 	glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
-    assert(glGetError()==GL_NO_ERROR);
+    b3Assert(glGetError()==GL_NO_ERROR);
 
 	//glDisableVertexAttribArray(m_data->m_textureAttribute);
-    assert(glGetError()==GL_NO_ERROR);
+    b3Assert(glGetError()==GL_NO_ERROR);
 
 	glUseProgram(0);
    
-    assert(glGetError()==GL_NO_ERROR);
+    b3Assert(glGetError()==GL_NO_ERROR);
 
 }
 
@@ -349,7 +349,7 @@ void GLPrimitiveRenderer::drawTexturedRect3D2( PrimVertex* vertices, int numVert
 	}
 	//B3_PROFILE("GLPrimitiveRenderer::drawTexturedRect3D");
 
-    assert(glGetError()==GL_NO_ERROR);
+    b3Assert(glGetError()==GL_NO_ERROR);
     float identity[16]={1,0,0,0,
 						0,1,0,0,
 						0,0,1,0,
@@ -360,7 +360,7 @@ void GLPrimitiveRenderer::drawTexturedRect3D2( PrimVertex* vertices, int numVert
 	glUniformMatrix4fv(m_data->m_viewmatUniform, 1, false, identity);
 	glUniformMatrix4fv(m_data->m_projMatUniform, 1, false, identity);
 
-    assert(glGetError()==GL_NO_ERROR);
+    b3Assert(glGetError()==GL_NO_ERROR);
     
     glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vertexBuffer2);
     glBindVertexArray(m_data->m_vertexArrayObject2);
@@ -388,7 +388,7 @@ void GLPrimitiveRenderer::drawTexturedRect3D2( PrimVertex* vertices, int numVert
     
     
     
-    assert(glGetError()==GL_NO_ERROR);
+    b3Assert(glGetError()==GL_NO_ERROR);
     
     PrimVec2 p( 0.f,0.f);//?b?0.5f * sinf(timeValue), 0.5f * cosf(timeValue) );
 	if (useRGBA)
@@ -401,47 +401,47 @@ void GLPrimitiveRenderer::drawTexturedRect3D2( PrimVertex* vertices, int numVert
     
 	glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
 
-    assert(glGetError()==GL_NO_ERROR);
+    b3Assert(glGetError()==GL_NO_ERROR);
     
     glEnableVertexAttribArray(m_data->m_positionAttribute);
-	assert(glGetError()==GL_NO_ERROR);
+	b3Assert(glGetError()==GL_NO_ERROR);
     
     glEnableVertexAttribArray(m_data->m_colourAttribute);
-	assert(glGetError()==GL_NO_ERROR);
+	b3Assert(glGetError()==GL_NO_ERROR);
     
 	glEnableVertexAttribArray(m_data->m_textureAttribute);
     
     glVertexAttribPointer(m_data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)0);
     glVertexAttribPointer(m_data->m_colourAttribute  , 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)sizeof(PrimVec4));
     glVertexAttribPointer(m_data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)(sizeof(PrimVec4)+sizeof(PrimVec4)));
-	assert(glGetError()==GL_NO_ERROR);
+	b3Assert(glGetError()==GL_NO_ERROR);
     
     glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_data->m_indexBuffer2);
     
     //glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
     int indexCount = (numVertices/4)*6;
-    assert(glGetError()==GL_NO_ERROR);
+    b3Assert(glGetError()==GL_NO_ERROR);
     
 	
     glDrawElements(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, 0);
-    assert(glGetError()==GL_NO_ERROR);
+    b3Assert(glGetError()==GL_NO_ERROR);
 	
 	
     glBindVertexArray(0);
-    assert(glGetError()==GL_NO_ERROR);
+    b3Assert(glGetError()==GL_NO_ERROR);
 
 	glBindBuffer(GL_ARRAY_BUFFER, 0);
-    assert(glGetError()==GL_NO_ERROR);
+    b3Assert(glGetError()==GL_NO_ERROR);
 
 	glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
-    assert(glGetError()==GL_NO_ERROR);
+    b3Assert(glGetError()==GL_NO_ERROR);
 
 	//glDisableVertexAttribArray(m_data->m_textureAttribute);
-    assert(glGetError()==GL_NO_ERROR);
+    b3Assert(glGetError()==GL_NO_ERROR);
 
 	glUseProgram(0);
    
-    assert(glGetError()==GL_NO_ERROR);
+    b3Assert(glGetError()==GL_NO_ERROR);
 	
 
 }
@@ -477,7 +477,7 @@ void GLPrimitiveRenderer::flushBatchedRects()
 		return;
 
 	glActiveTexture(GL_TEXTURE0);
-	assert(glGetError()==GL_NO_ERROR);
+	b3Assert(glGetError()==GL_NO_ERROR);
 	glBindTexture(GL_TEXTURE_2D,m_data->m_texturehandle);
 	drawTexturedRect3D2(m_data2->m_verticesRect, m_data2->m_numVerticesRect,0);
 	m_data2->m_numVerticesRect=0;

examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.glsl

@@ -13,9 +13,14 @@ in Vert
 
 uniform sampler2D Diffuse;
 uniform sampler2DShadow shadowMap;
+uniform mat4 ViewMatrixInverse;
 
-in vec3 lightDir,normal,ambient;
+in vec3 lightPos,cameraPosition, normal,ambient;
 in vec4 ShadowCoord;
+in vec4 vertexPos;
+in float materialShininess;
+in vec3 lightSpecularIntensity;
+in vec3 materialSpecularColor;
 
 out vec4 color;
 
@@ -28,8 +33,11 @@ void main(void)
 	float intensity,at,af;
 	if (fragment.color.w==0)
 		discard;
-
+	vec3 lightDir = normalize(lightPos);
-	intensity = 0.5+0.5*clamp( dot( normalize(normal),lightDir ), -1,1 );
+	
+	vec3 normalDir = normalize(normal);
+ 
+	intensity = 0.5+0.5*clamp( dot( normalDir,lightDir ), -1,1 );
 	
 	af = 1.0;
 		
@@ -38,7 +46,24 @@ void main(void)
 		
 	//float bias = 0.005f;
 	
+	vec3 specularReflection;
 	
+	if (dot(normalDir, lightDir) < 0.0) 
+	{
+		specularReflection = vec3(0.0, 0.0, 0.0);
+	}
+  else // light source on the right side
+	{
+		vec3 surfaceToLight = normalize(lightPos - vertexPos.xyz);
+    vec3 surfaceToCamera = normalize(cameraPosition - vertexPos.xyz);
+    
+    
+    float specularCoefficient = 0.0;
+		specularCoefficient = pow(max(0.0, dot(surfaceToCamera, reflect(-surfaceToLight, normalDir))), materialShininess);
+    specularReflection = specularCoefficient * materialSpecularColor * lightSpecularIntensity;
+  
+	}
+    
 
 	float visibility = texture(shadowMap, vec3(ShadowCoord.xy,(ShadowCoord.z)/ShadowCoord.w));
 	if (intensity<0.5)
@@ -46,6 +71,6 @@ void main(void)
 
 	intensity = 0.7*intensity  + 0.3*intensity*visibility;
 	
-	cf = intensity*(vec3(1.0,1.0,1.0)-ambient)+ambient;
+	cf = intensity*(vec3(1.0,1.0,1.0)-ambient)+ambient+specularReflection*visibility;
 	color  = vec4(ct * cf, fragment.color.w);
 }

examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.h

@@ -12,8 +12,13 @@ static const char* useShadowMapInstancingFragmentShader= \
 "} vert;\n"
 "uniform sampler2D Diffuse;\n"
 "uniform sampler2DShadow shadowMap;\n"
-"in vec3 lightDir,normal,ambient;\n"
+"uniform mat4 ViewMatrixInverse;\n"
+"in vec3 lightPos,cameraPosition, normal,ambient;\n"
 "in vec4 ShadowCoord;\n"
+"in vec4 vertexPos;\n"
+"in float materialShininess;\n"
+"in vec3 lightSpecularIntensity;\n"
+"in vec3 materialSpecularColor;\n"
 "out vec4 color;\n"
 "void main(void)\n"
 "{\n"
@@ -22,7 +27,11 @@ static const char* useShadowMapInstancingFragmentShader= \
 "	float intensity,at,af;\n"
 "	if (fragment.color.w==0)\n"
 "		discard;\n"
-"	intensity = 0.5+0.5*clamp( dot( normalize(normal),lightDir ), -1,1 );\n"
+"	vec3 lightDir = normalize(lightPos);\n"
+"	\n"
+"	vec3 normalDir = normalize(normal);\n"
+" \n"
+"	intensity = 0.5+0.5*clamp( dot( normalDir,lightDir ), -1,1 );\n"
 "	\n"
 "	af = 1.0;\n"
 "		\n"
@@ -31,13 +40,30 @@ static const char* useShadowMapInstancingFragmentShader= \
 "		\n"
 "	//float bias = 0.005f;\n"
 "	\n"
+"	vec3 specularReflection;\n"
 "	\n"
+"	if (dot(normalDir, lightDir) < 0.0) \n"
+"	{\n"
+"		specularReflection = vec3(0.0, 0.0, 0.0);\n"
+"	}\n"
+"  else // light source on the right side\n"
+"	{\n"
+"		vec3 surfaceToLight = normalize(lightPos - vertexPos.xyz);\n"
+"    vec3 surfaceToCamera = normalize(cameraPosition - vertexPos.xyz);\n"
+"    \n"
+"    \n"
+"    float specularCoefficient = 0.0;\n"
+"		specularCoefficient = pow(max(0.0, dot(surfaceToCamera, reflect(-surfaceToLight, normalDir))), materialShininess);\n"
+"    specularReflection = specularCoefficient * materialSpecularColor * lightSpecularIntensity;\n"
+"  \n"
+"	}\n"
+"    \n"
 "	float visibility = texture(shadowMap, vec3(ShadowCoord.xy,(ShadowCoord.z)/ShadowCoord.w));\n"
 "	if (intensity<0.5)\n"
 "		visibility = 0;\n"
 "	intensity = 0.7*intensity  + 0.3*intensity*visibility;\n"
 "	\n"
-"	cf = intensity*(vec3(1.0,1.0,1.0)-ambient)+ambient;\n"
+"	cf = intensity*(vec3(1.0,1.0,1.0)-ambient)+ambient+specularReflection*visibility;\n"
 "	color  = vec4(ct * cf, fragment.color.w);\n"
 "}\n"
 ;

examples/OpenGLWindow/Shaders/useShadowMapInstancingVS.glsl

@@ -15,7 +15,12 @@ uniform mat4 ModelViewMatrix;
 uniform mat4 ProjectionMatrix;
 uniform mat4 DepthBiasModelViewProjectionMatrix;
 uniform mat4 MVP;
-uniform vec3 lightDirIn;
+uniform vec3 lightPosIn;
+uniform vec3 cameraPositionIn;
+uniform mat4 ViewMatrixInverse;
+uniform float materialShininessIn;
+uniform vec3 lightSpecularIntensityIn;
+uniform vec3 materialSpecularColorIn;
 
 out vec4 ShadowCoord;
 
@@ -60,7 +65,13 @@ vec4 quatRotate ( in vec4 p, in vec4 q )
     return quatMul ( temp, vec4 ( -q.x, -q.y, -q.z, q.w ) );
 }
 
-out vec3 lightDir,normal,ambient;
+out vec3 lightPos,normal,ambient;
+out vec4 vertexPos;
+out vec3 cameraPosition;
+out float materialShininess;
+out vec3 lightSpecularIntensity;
+out vec3 materialSpecularColor;
+
 
 void main(void)
 {
@@ -69,15 +80,19 @@ void main(void)
 			
 	vec4 worldNormal = (quatRotate3( vertexnormal,q));
 	
-	normal = normalize(worldNormal).xyz;
+	normal = worldNormal.xyz;
 
-	lightDir = lightDirIn;
+	lightPos = lightPosIn;
-		
+	cameraPosition = cameraPositionIn;
+	materialShininess = materialShininessIn;
+	lightSpecularIntensity = lightSpecularIntensityIn;
+	materialSpecularColor = materialSpecularColorIn;
 	
 	vec4 localcoord = quatRotate3( position.xyz*instance_scale,q);
-	vec4 vertexPos = MVP* vec4((instance_position+localcoord).xyz,1);
+	vertexPos = vec4((instance_position+localcoord).xyz,1);
-
+	
-	gl_Position = vertexPos;
+	vec4 vertexLoc = MVP* vec4((instance_position+localcoord).xyz,1);
+	gl_Position = vertexLoc;
 	ShadowCoord = DepthBiasModelViewProjectionMatrix * vec4((instance_position+localcoord).xyz,1);
 
 	fragment.color = instance_color;

examples/OpenGLWindow/Shaders/useShadowMapInstancingVS.h

@@ -13,7 +13,12 @@ static const char* useShadowMapInstancingVertexShader= \
 "uniform mat4 ProjectionMatrix;\n"
 "uniform mat4 DepthBiasModelViewProjectionMatrix;\n"
 "uniform mat4 MVP;\n"
-"uniform vec3 lightDirIn;\n"
+"uniform vec3 lightPosIn;\n"
+"uniform vec3 cameraPositionIn;\n"
+"uniform mat4 ViewMatrixInverse;\n"
+"uniform float materialShininessIn;\n"
+"uniform vec3 lightSpecularIntensityIn;\n"
+"uniform vec3 materialSpecularColorIn;\n"
 "out vec4 ShadowCoord;\n"
 "out Fragment\n"
 "{\n"
@@ -51,7 +56,12 @@ static const char* useShadowMapInstancingVertexShader= \
 "    vec4 temp = quatMul ( q, p );\n"
 "    return quatMul ( temp, vec4 ( -q.x, -q.y, -q.z, q.w ) );\n"
 "}\n"
-"out vec3 lightDir,normal,ambient;\n"
+"out vec3 lightPos,normal,ambient;\n"
+"out vec4 vertexPos;\n"
+"out vec3 cameraPosition;\n"
+"out float materialShininess;\n"
+"out vec3 lightSpecularIntensity;\n"
+"out vec3 materialSpecularColor;\n"
 "void main(void)\n"
 "{\n"
 "	vec4 q = instance_quaternion;\n"
@@ -59,13 +69,18 @@ static const char* useShadowMapInstancingVertexShader= \
 "			\n"
 "	vec4 worldNormal = (quatRotate3( vertexnormal,q));\n"
 "	\n"
-"	normal = normalize(worldNormal).xyz;\n"
+"	normal = worldNormal.xyz;\n"
-"	lightDir = lightDirIn;\n"
+"	lightPos = lightPosIn;\n"
-"		\n"
+"	cameraPosition = cameraPositionIn;\n"
+"	materialShininess = materialShininessIn;\n"
+"	lightSpecularIntensity = lightSpecularIntensityIn;\n"
+"	materialSpecularColor = materialSpecularColorIn;\n"
 "	\n"
 "	vec4 localcoord = quatRotate3( position.xyz*instance_scale,q);\n"
-"	vec4 vertexPos = MVP* vec4((instance_position+localcoord).xyz,1);\n"
+"	vertexPos = vec4((instance_position+localcoord).xyz,1);\n"
-"	gl_Position = vertexPos;\n"
+"	\n"
+"	vec4 vertexLoc = MVP* vec4((instance_position+localcoord).xyz,1);\n"
+"	gl_Position = vertexLoc;\n"
 "	ShadowCoord = DepthBiasModelViewProjectionMatrix * vec4((instance_position+localcoord).xyz,1);\n"
 "	fragment.color = instance_color;\n"
 "	vert.texcoord = uvcoords;\n"

examples/OpenGLWindow/SimpleCamera.cpp

@@ -415,3 +415,13 @@ float SimpleCamera::getCameraFrustumNear() const
 {
     return m_data->m_frustumZNear;
 }
+
+void SimpleCamera::setCameraFrustumFar(float far)
+{
+	m_data->m_frustumZFar = far;
+}
+
+void SimpleCamera::setCameraFrustumNear(float near)
+{
+	m_data->m_frustumZNear = near;
+}

examples/OpenGLWindow/SimpleCamera.h

@@ -54,6 +54,9 @@ struct SimpleCamera : public CommonCameraInterface
     
     virtual float getCameraFrustumFar() const;
     virtual float getCameraFrustumNear() const;
+
+	virtual void setCameraFrustumFar(float far);
+    virtual void setCameraFrustumNear(float near);
 };
 
 #endif //SIMPLE_CAMERA_H

examples/OpenGLWindow/SimpleOpenGL2Renderer.h

@@ -39,6 +39,9 @@ public:
     virtual void writeSingleInstanceColorToCPU(const double* color, int srcIndex);
 	virtual void writeSingleInstanceScaleToCPU(const float* scale, int srcIndex);
     virtual void writeSingleInstanceScaleToCPU(const double* scale, int srcIndex);
+	virtual void writeSingleInstanceSpecularColorToCPU(const double* specular, int srcIndex){}
+	virtual void writeSingleInstanceSpecularColorToCPU(const float* specular, int srcIndex){}
+
     virtual void	getCameraViewMatrix(float viewMat[16]) const;
     virtual void	getCameraProjectionMatrix(float projMat[16]) const;
 	virtual void drawTexturedTriangleMesh(float worldPosition[3], float worldOrientation[4], const float* vertices, int numvertices, const unsigned int* indices, int numIndices, float color[4], int textureIndex=-1, int vertexLayout=0)

examples/RobotSimulator/CMakeLists.txt

@@ -32,6 +32,7 @@ SET(RobotSimulator_SRCS
 		../../examples/SharedMemory/PhysicsServer.cpp
 		../../examples/SharedMemory/PhysicsServer.h
 		../../examples/SharedMemory/PhysicsServerExample.cpp
+		../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp
 		../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
 		../../examples/SharedMemory/PhysicsServerSharedMemory.cpp
 		../../examples/SharedMemory/PhysicsServerSharedMemory.h

examples/RobotSimulator/premake4.lua

@@ -10,6 +10,7 @@ myfiles =
 			"../../examples/SharedMemory/PhysicsServer.cpp",
 			"../../examples/SharedMemory/PhysicsServer.h",
 			"../../examples/SharedMemory/PhysicsServerExample.cpp",
+			"../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp",
 			"../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp",
 			"../../examples/SharedMemory/PhysicsServerSharedMemory.cpp",
 			"../../examples/SharedMemory/PhysicsServerSharedMemory.h",

examples/SharedMemory/CMakeLists.txt

@@ -7,6 +7,7 @@ SET(SharedMemory_SRCS
 	PhysicsClientSharedMemory.cpp
 	PhysicsClientExample.cpp
 	PhysicsServerExample.cpp
+	PhysicsServerExampleBullet2.cpp
 	PhysicsServerSharedMemory.cpp
 	PhysicsServerSharedMemory.h
 	PhysicsServer.cpp

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -392,6 +392,18 @@ int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle
 
 }
 
+int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle, double restitutionVelocityThreshold)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
+
+	command->m_physSimParamArgs.m_restitutionVelocityThreshold = restitutionVelocityThreshold;
+	command->m_updateFlags |= SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD  ;
+	return 0;
+
+}
+
+
 int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
@@ -1297,6 +1309,42 @@ int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHa
 	return 0;
 }
 
+int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
+	command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
+	command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
+	command->m_changeDynamicsInfoArgs.m_spinningFriction = friction;
+	command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION;
+	return 0;
+
+}
+int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
+	command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
+	command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
+	command->m_changeDynamicsInfoArgs.m_rollingFriction = friction;
+	command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION;
+	return 0;
+
+}
+
+
+int b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double restitution)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
+	command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
+	command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
+	command->m_changeDynamicsInfoArgs.m_restitution = restitution;
+	command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_RESTITUTION;
+	return 0;
+
+}
+
 b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info)
 {
     PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -2394,6 +2442,21 @@ void b3UpdateVisualShapeRGBAColor(b3SharedMemoryCommandHandle commandHandle, dou
 	}
 }
 
+void b3UpdateVisualShapeSpecularColor(b3SharedMemoryCommandHandle commandHandle, double specularColor[3])
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command);
+	b3Assert(command->m_type == CMD_UPDATE_VISUAL_SHAPE);
+
+	if (command->m_type == CMD_UPDATE_VISUAL_SHAPE)
+	{
+		command->m_updateVisualShapeDataArguments.m_specularColor[0] = specularColor[0];
+		command->m_updateVisualShapeDataArguments.m_specularColor[1] = specularColor[1];
+		command->m_updateVisualShapeDataArguments.m_specularColor[2] = specularColor[2];
+		command->m_updateFlags |= CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR;
+	}
+}
+
 b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient)
 {
     PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -3101,3 +3164,38 @@ double b3GetTimeOut(b3PhysicsClientHandle physClient)
 	}
 	return -1;
 }
+
+void b3MultiplyTransforms(const double posA[3], const double ornA[4], const double posB[3], const double ornB[4], double outPos[3], double outOrn[4])
+{
+	b3Transform trA;
+	b3Transform trB;
+	trA.setOrigin(b3MakeVector3(posA[0],posA[1],posA[2]));
+	trA.setRotation(b3Quaternion(ornA[0],ornA[1],ornA[2],ornA[3]));
+	trB.setOrigin(b3MakeVector3(posB[0],posB[1],posB[2]));
+	trB.setRotation(b3Quaternion(ornB[0],ornB[1],ornB[2],ornB[3]));
+	b3Transform res = trA*trB;
+	outPos[0] = res.getOrigin()[0];
+	outPos[1] = res.getOrigin()[1];
+	outPos[2] = res.getOrigin()[2];
+	b3Quaternion orn = res.getRotation();
+	outOrn[0] = orn[0];
+	outOrn[1] = orn[1];
+	outOrn[2] = orn[2];
+	outOrn[3] = orn[3];
+}
+
+void b3InvertTransform(const double pos[3], const double orn[4], double outPos[3], double outOrn[4])
+{
+	b3Transform tr;
+	tr.setOrigin(b3MakeVector3(pos[0],pos[1],pos[2]));
+	tr.setRotation(b3Quaternion(orn[0],orn[1],orn[2],orn[3]));
+	b3Transform trInv = tr.inverse();
+	outPos[0] = trInv.getOrigin()[0];
+	outPos[1] = trInv.getOrigin()[1];
+	outPos[2] = trInv.getOrigin()[2];
+	b3Quaternion invOrn = trInv.getRotation();
+	outOrn[0] = invOrn[0];
+	outOrn[1] = invOrn[1];
+	outOrn[2] = invOrn[2];
+	outOrn[3] = invOrn[3];
+}

examples/SharedMemory/PhysicsClientC_API.h

@@ -84,7 +84,12 @@ int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3Dynamics
 b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient);
 int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass);
 int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction);
-    
+int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction);
+int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction);
+int b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double restitution);
+
+
+
 b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
 
 ///return a unique id for the user constraint, after successful creation, or -1 for an invalid constraint id
@@ -206,6 +211,8 @@ void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3Visu
 b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename);
 b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId);
 void b3UpdateVisualShapeRGBAColor(b3SharedMemoryCommandHandle commandHandle, double rgbaColor[4]);
+void b3UpdateVisualShapeSpecularColor(b3SharedMemoryCommandHandle commandHandle, double specularColor[3]);
+
 
 b3SharedMemoryCommandHandle	b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
 int	b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz);
@@ -223,6 +230,9 @@ int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandl
 int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryCommandHandle commandHandle, double contactBreakingThreshold);
 int b3PhysicsParamSetMaxNumCommandsPer1ms(b3SharedMemoryCommandHandle commandHandle, int maxNumCmdPer1ms);
 int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle, int enableFileCaching);
+int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle, double restitutionVelocityThreshold);
+
+
 
 //b3PhysicsParamSetInternalSimFlags is for internal/temporary/easter-egg/experimental demo purposes
 //Use at own risk: magic things may or my not happen when calling this API
@@ -409,6 +419,8 @@ void b3SetProfileTimingDuractionInMicroSeconds(b3SharedMemoryCommandHandle comma
 void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds);
 double b3GetTimeOut(b3PhysicsClientHandle physClient);
 
+void b3MultiplyTransforms(const double posA[3], const double ornA[4], const double posB[3], const double ornB[4], double outPos[3], double outOrn[4]);
+void b3InvertTransform(const double pos[3], const double orn[4], double outPos[3], double outOrn[4]);
 
 #ifdef __cplusplus
 }

examples/SharedMemory/PhysicsClientExample.cpp

@@ -5,7 +5,7 @@
 #include "../CommonInterfaces/Common2dCanvasInterface.h"
 #include "SharedMemoryCommon.h"
 #include "../CommonInterfaces/CommonParameterInterface.h"
-
+#include "PhysicsServerCommandProcessor.h"
 #include "PhysicsClientC_API.h"
 #include "PhysicsClient.h"
 //#include "SharedMemoryCommands.h"
@@ -37,6 +37,7 @@ class PhysicsClientExample : public SharedMemoryCommon
 {
 protected:
     b3PhysicsClientHandle m_physicsClientHandle;
+	
 
 	//this m_physicsServer is only used when option eCLIENTEXAMPLE_SERVER is enabled
 	PhysicsServerSharedMemory	m_physicsServer;
@@ -532,10 +533,26 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
 }
 
 
+struct Bullet2CommandProcessorCreation3 : public CommandProcessorCreationInterface
+{
+	virtual class CommandProcessorInterface* createCommandProcessor()
+	{
+		PhysicsServerCommandProcessor* proc = new PhysicsServerCommandProcessor;
+		return proc;
+	}
+
+	virtual void deleteCommandProcessor(CommandProcessorInterface* proc)
+	{
+		delete proc;
+	}
+};
+
+static Bullet2CommandProcessorCreation3 sB2PC2;
 
 PhysicsClientExample::PhysicsClientExample(GUIHelperInterface* helper, int options)
 :SharedMemoryCommon(helper),
 m_physicsClientHandle(0),
+m_physicsServer(&sB2PC2,0,0),
 m_wantsTermination(false),
 m_sharedMemoryKey(SHARED_MEMORY_KEY),
 m_selectedBody(-1),
@@ -577,6 +594,7 @@ PhysicsClientExample::~PhysicsClientExample()
             m_canvas->destroyCanvas(m_canvasSegMaskIndex);
 		
 	}
+	
     b3Printf("~PhysicsClientExample\n");
 }
 

examples/SharedMemory/PhysicsCommandProcessorInterface.h

@@ -1,7 +1,7 @@
 #ifndef PHYSICS_COMMAND_PROCESSOR_INTERFACE_H
 #define PHYSICS_COMMAND_PROCESSOR_INTERFACE_H
 
-enum PhysicsCOmmandRenderFlags
+enum PhysicsCommandRenderFlags
 {
 	COV_DISABLE_SYNC_RENDERING=1
 };
@@ -29,4 +29,34 @@ public:
 
 };
 
+
+class btVector3;
+class btQuaternion;
+
+class CommandProcessorInterface : public PhysicsCommandProcessorInterface
+{
+
+public:
+	virtual ~CommandProcessorInterface(){}
+
+	virtual void syncPhysicsToGraphics()=0;
+	virtual void stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents)=0;
+	virtual void enableRealTimeSimulation(bool enableRealTimeSim)=0;
+	virtual bool isRealTimeSimulationEnabled() const=0;
+
+	virtual void enableCommandLogging(bool enable, const char* fileName)=0;
+	virtual void replayFromLogFile(const char* fileName)=0;
+	virtual void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes )=0;
+
+	virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0;
+	virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0;
+	virtual void removePickingConstraint()=0;
+
+	virtual const btVector3& getVRTeleportPosition() const=0;
+	virtual void setVRTeleportPosition(const btVector3& vrReleportPos)=0;
+
+	virtual const btQuaternion& getVRTeleportOrientation() const=0;
+	virtual void setVRTeleportOrientation(const btQuaternion& vrReleportOrn)=0;
+};
+
 #endif //PHYSICS_COMMAND_PROCESSOR_INTERFACE_H

examples/SharedMemory/PhysicsLoopBack.cpp

@@ -2,25 +2,44 @@
 #include "PhysicsServerSharedMemory.h"
 #include "PhysicsClientSharedMemory.h"
 #include "../CommonInterfaces/CommonGUIHelperInterface.h"
-
+#include "PhysicsServerCommandProcessor.h"
+#include "../CommonInterfaces/CommonExampleInterface.h"
 struct PhysicsLoopBackInternalData
 {
+	CommandProcessorInterface* m_commandProcessor;
 	PhysicsClientSharedMemory* m_physicsClient;
 	PhysicsServerSharedMemory* m_physicsServer;
 	DummyGUIHelper m_noGfx;
 	
 
 	PhysicsLoopBackInternalData()
-		:m_physicsClient(0),
+		:m_commandProcessor(0),
+		m_physicsClient(0),
 		m_physicsServer(0)
 	{
 	}
 };
 
+struct Bullet2CommandProcessorCreation2 : public CommandProcessorCreationInterface
+{
+	virtual class CommandProcessorInterface* createCommandProcessor()
+	{
+		PhysicsServerCommandProcessor* proc = new PhysicsServerCommandProcessor;
+		return proc;
+	}
+
+	virtual void deleteCommandProcessor(CommandProcessorInterface* proc)
+	{
+		delete proc;
+	}
+};
+
+static Bullet2CommandProcessorCreation2 sB2Proc;
+
 PhysicsLoopBack::PhysicsLoopBack()
 {
 	m_data = new PhysicsLoopBackInternalData;
-	m_data->m_physicsServer = new PhysicsServerSharedMemory();
+	m_data->m_physicsServer = new PhysicsServerSharedMemory(&sB2Proc, 0,0);
 	m_data->m_physicsClient = new PhysicsClientSharedMemory();
 }
 
@@ -28,6 +47,7 @@ PhysicsLoopBack::~PhysicsLoopBack()
 {
 	delete m_data->m_physicsClient;
 	delete m_data->m_physicsServer;
+	delete m_data->m_commandProcessor;
 	delete m_data;
 }
 

examples/SharedMemory/PhysicsServer.h

@@ -25,18 +25,18 @@ public:
 
 	//@todo(erwincoumans) Should we have shared memory commands for picking objects?
 	///The pickBody method will try to pick the first body along a ray, return true if succeeds, false otherwise
-	virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0;
+	virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld){return false;}
-	virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0;
+	virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld){return false;}
-	virtual void removePickingConstraint()=0;
+	virtual void removePickingConstraint(){}
 
 	//for physicsDebugDraw and renderScene are mainly for debugging purposes
 	//and for physics visualization. The idea is that physicsDebugDraw can also send wireframe
 	//to a physics client, over shared memory
-	virtual void    physicsDebugDraw(int debugDrawFlags)=0;
+	virtual void    physicsDebugDraw(int debugDrawFlags){}
-	virtual void    renderScene(int renderFlags)=0;
+	virtual void    renderScene(int renderFlags){}
 
-	virtual void enableCommandLogging(bool enable, const char* fileName)=0;
+	virtual void enableCommandLogging(bool enable, const char* fileName){}
-	virtual void replayFromLogFile(const char* fileName)=0;
+	virtual void replayFromLogFile(const char* fileName){}
 	
 };
 

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -49,9 +49,8 @@
 
 //@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
 btVector3 gLastPickPos(0, 0, 0);
-bool gCloseToKuka=false;
+
-bool gEnableRealTimeSimVR=false;
+
-bool gCreateDefaultRobotAssets = false;
 int gInternalSimFlags = 0;
 bool gResetSimulation = 0;
 int gVRTrackingObjectUniqueId = -1;
@@ -59,9 +58,9 @@ int gVRTrackingObjectFlag = VR_CAMERA_TRACK_OBJECT_ORIENTATION;
 
 btTransform gVRTrackingObjectTr = btTransform::getIdentity();
 
-int gMaxNumCmdPer1ms = -1;//experiment: add some delay to avoid threads starving other threads
+
-int gCreateObjectSimVR = -1;
+
-int gEnableKukaControl = 0;
+
 btVector3 gVRTeleportPos1(0,0,0);
 btQuaternion gVRTeleportOrn(0, 0, 0,1);
 
@@ -1137,25 +1136,13 @@ struct PhysicsServerCommandProcessorInternalData
 
 
 	bool m_allowRealTimeSimulation;
-	bool m_hasGround;
+	
 
 	b3VRControllerEvents m_vrControllerEvents;
 
 
 	btAlignedObjectArray<b3KeyboardEvent> m_keyboardEvents;
 	
-	btMultiBodyFixedConstraint* m_gripperRigidbodyFixed;
-	btMultiBody* m_gripperMultiBody;
-	btMultiBodyFixedConstraint* m_kukaGripperFixed;
-	btMultiBody* m_kukaGripperMultiBody;
-	btMultiBodyPoint2Point* m_kukaGripperRevolute1;
-	btMultiBodyPoint2Point* m_kukaGripperRevolute2;
-	
-
-	int m_huskyId;
-	int m_KukaId;
-	int m_sphereId;
-	int m_gripperId;
 	CommandLogger* m_commandLogger;
 	CommandLogPlayback* m_logPlayback;
 
@@ -1231,17 +1218,6 @@ struct PhysicsServerCommandProcessorInternalData
 	PhysicsServerCommandProcessorInternalData()
 		:
 		m_allowRealTimeSimulation(false),
-	m_hasGround(false),	
-	m_gripperRigidbodyFixed(0),
-		m_gripperMultiBody(0),
-		m_kukaGripperFixed(0),
-		m_kukaGripperMultiBody(0),
-		m_kukaGripperRevolute1(0),
-		m_kukaGripperRevolute2(0),
-		m_huskyId(-1),
-		m_KukaId(-1),
-		m_sphereId(-1),
-		m_gripperId(-1),
 		m_commandLogger(0),
 		m_logPlayback(0),
 		m_physicsDeltaTime(1./240.),
@@ -3915,52 +3891,124 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 				{
 					BT_PROFILE("CMD_CHANGE_DYNAMICS_INFO");
 					
-					if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
+					int bodyUniqueId = clientCmd.m_changeDynamicsInfoArgs.m_bodyUniqueId;
-					{
+					int linkIndex = clientCmd.m_changeDynamicsInfoArgs.m_linkIndex;
-						int bodyUniqueId = clientCmd.m_changeDynamicsInfoArgs.m_bodyUniqueId;
+					double mass = clientCmd.m_changeDynamicsInfoArgs.m_mass;
-						int linkIndex = clientCmd.m_changeDynamicsInfoArgs.m_linkIndex;
+					double lateralFriction = clientCmd.m_changeDynamicsInfoArgs.m_lateralFriction;
-						double mass = clientCmd.m_changeDynamicsInfoArgs.m_mass;
+					double spinningFriction = clientCmd.m_changeDynamicsInfoArgs.m_spinningFriction;
-						btAssert(bodyUniqueId >= 0);
+					double rollingFriction = clientCmd.m_changeDynamicsInfoArgs.m_rollingFriction;
-						btAssert(linkIndex >= -1);
+					double restitution = clientCmd.m_changeDynamicsInfoArgs.m_restitution;
+					btAssert(bodyUniqueId >= 0);
+					btAssert(linkIndex >= -1);
 						
-						InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
+					InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
-						if (body && body->m_multiBody)
+
+					if (body && body->m_multiBody)
+					{
+						btMultiBody* mb = body->m_multiBody;
+						if (linkIndex == -1)
 						{
-							btMultiBody* mb = body->m_multiBody;
+							if (mb->getBaseCollider())
-							if (linkIndex == -1)
 							{
-								mb->setBaseMass(mass);
+								if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION)
+								{
+									mb->getBaseCollider()->setRestitution(restitution);
+								}
+								if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION)
+								{
+									mb->getBaseCollider()->setFriction(lateralFriction);
+								}
+								if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION)
+								{
+									mb->getBaseCollider()->setSpinningFriction(spinningFriction);
+								}
+								if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION)
+								{
+									mb->getBaseCollider()->setRollingFriction(rollingFriction);
+								}								
 							}
-							else
+							if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
 							{
-								mb->getLink(linkIndex).m_mass = mass;
+								mb->setBaseMass(mass);	
+								if (mb->getBaseCollider() && mb->getBaseCollider()->getCollisionShape())
+								{
+									btVector3 localInertia;
+									mb->getBaseCollider()->getCollisionShape()->calculateLocalInertia(mass,localInertia);
+									mb->setBaseInertia(localInertia);
+								}
+							}
+						}
+						else
+						{
+							if (linkIndex >= 0 && linkIndex < mb->getNumLinks())
+							{
+								if (mb->getLinkCollider(linkIndex))
+								{
+									if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION)
+									{
+										mb->getLinkCollider(linkIndex)->setRestitution(restitution);
+									}
+									if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION)
+									{
+										mb->getLinkCollider(linkIndex)->setSpinningFriction(spinningFriction);
+									}
+									if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION)
+									{
+										mb->getLinkCollider(linkIndex)->setRollingFriction(rollingFriction);
+									}
+
+									if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION)
+									{
+										mb->getLinkCollider(linkIndex)->setFriction(lateralFriction);
+									}
+								
+			
+								}
+								if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
+								{
+									mb->getLink(linkIndex).m_mass = mass;
+									if (mb->getLinkCollider(linkIndex) && mb->getLinkCollider(linkIndex)->getCollisionShape())
+									{
+										btVector3 localInertia;
+										mb->getLinkCollider(linkIndex)->getCollisionShape()->calculateLocalInertia(mass,localInertia);
+										mb->getLink(linkIndex).m_inertiaLocal = localInertia;
+									}
+								}
+							}
+						}
+					} else
+					{
+						if (body && body->m_rigidBody)
+						{
+							if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION)
+							{
+								body->m_rigidBody->setRestitution(restitution);
+							}
+							if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION)
+							{
+								body->m_rigidBody->setFriction(lateralFriction);
+							}
+							if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION)
+							{
+								body->m_rigidBody->setSpinningFriction(spinningFriction);
+							}
+							if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION)
+							{
+								body->m_rigidBody->setRollingFriction(rollingFriction);
+							}
+
+							if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
+							{
+								btVector3 localInertia;
+								if (body->m_rigidBody->getCollisionShape())
+								{
+									body->m_rigidBody->getCollisionShape()->calculateLocalInertia(mass,localInertia);
+								}
+								body->m_rigidBody->setMassProps(mass,localInertia);
 							}
 						}
 					}
 					
-					if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION)
-					{
-						int bodyUniqueId = clientCmd.m_changeDynamicsInfoArgs.m_bodyUniqueId;
-						int linkIndex = clientCmd.m_changeDynamicsInfoArgs.m_linkIndex;
-						double lateralFriction = clientCmd.m_changeDynamicsInfoArgs.m_lateralFriction;
-						btAssert(bodyUniqueId >= 0);
-						btAssert(linkIndex >= -1);
-						
-						InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
-						if (body && body->m_multiBody)
-						{
-							btMultiBody* mb = body->m_multiBody;
-							if (linkIndex == -1)
-							{
-								mb->getBaseCollider()->setFriction(lateralFriction);
-							}
-							else
-							{
-								mb->getLinkCollider(linkIndex)->setFriction(lateralFriction);
-							}
-						}
-					}
-
 					
 					SharedMemoryStatus& serverCmd =serverStatusOut;
 					serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
@@ -4017,7 +4065,6 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 					if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS)
 					{
 						//these flags are for internal/temporary/easter-egg/experimental demo purposes, use at own risk
-						gCreateDefaultRobotAssets = (clientCmd.m_physSimParamArgs.m_internalSimFlags & SIM_PARAM_INTERNAL_CREATE_ROBOT_ASSETS);
 						gInternalSimFlags = clientCmd.m_physSimParamArgs.m_internalSimFlags;
 					}
 
@@ -4041,11 +4088,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 					{
 						gContactBreakingThreshold = clientCmd.m_physSimParamArgs.m_contactBreakingThreshold;
 					}
-					if (clientCmd.m_updateFlags&SIM_PARAM_MAX_CMD_PER_1MS)
+					
-					{
-						gMaxNumCmdPer1ms = clientCmd.m_physSimParamArgs.m_maxNumCmdPer1ms;
-					}
-
 					if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_COLLISION_FILTER_MODE)
 					{
 						m_data->m_broadphaseCollisionFilterCallback->m_filterMode = clientCmd.m_physSimParamArgs.m_collisionFilterMode;
@@ -4070,6 +4113,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                     {
                         m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = clientCmd.m_physSimParamArgs.m_defaultContactERP;
                     }
+					if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD)
+                    {
+                        m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold = clientCmd.m_physSimParamArgs.m_restitutionVelocityThreshold;
+                    }
+
+					
 
 					if (clientCmd.m_updateFlags&SIM_PARAM_ENABLE_FILE_CACHING)
                     {
@@ -5621,7 +5670,6 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 						}
 					}                    
    
-					if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) 
 					{
 						int bodyUniqueId = clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId;
 						int linkIndex = clientCmd.m_updateVisualShapeDataArguments.m_jointIndex;
@@ -5637,7 +5685,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 									{
 										m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor);
 										int graphicsIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex();
-										m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor);
+										if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) 
+										{
+											m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor);
+										}
+										if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR) 
+										{
+											m_data->m_guiHelper->changeSpecularColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_specularColor);
+										}
+										
 									}
 								} else
 								{
@@ -5647,7 +5703,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 										{
 											m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor);
 											int graphicsIndex = bodyHandle->m_multiBody->getLink(linkIndex).m_collider->getUserIndex();
-											m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor);
+											if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) 
+											{
+												m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor);
+											}
+											if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR) 
+											{
+												m_data->m_guiHelper->changeSpecularColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_specularColor);
+											}
+
 										}
 									}
 								}
@@ -6210,7 +6274,7 @@ btQuaternion gVRController2Orn(0,0,0,1);
 btScalar gVRGripper2Analog = 0;
 btScalar gVRGripperAnalog = 0;
 
-bool gVRGripperClosed = false;
+
 
 
 int gDroppedSimulationSteps = 0;
@@ -6223,6 +6287,11 @@ void PhysicsServerCommandProcessor::enableRealTimeSimulation(bool enableRealTime
 	m_data->m_allowRealTimeSimulation = enableRealTimeSim;
 }
 
+bool PhysicsServerCommandProcessor::isRealTimeSimulationEnabled() const
+{
+	return 	m_data->m_allowRealTimeSimulation;
+}
+
 void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec,	const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents,const struct b3KeyboardEvent* keyEvents, int numKeyEvents)
 {
 	m_data->m_vrControllerEvents.addNewVREvents(vrControllerEvents,numVRControllerEvents);
@@ -6276,23 +6345,22 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec,	const
 			if (gVRTrackingObjectUniqueId>=0)
 			{
 				gVRTrackingObjectTr.setOrigin(bodyHandle->m_multiBody->getBaseWorldTransform().getOrigin());
+				gVRTeleportPos1 = gVRTrackingObjectTr.getOrigin();
 			}
 			if (gVRTrackingObjectFlag&VR_CAMERA_TRACK_OBJECT_ORIENTATION)
 			{
 				gVRTrackingObjectTr.setBasis(bodyHandle->m_multiBody->getBaseWorldTransform().getBasis());
+				gVRTeleportOrn = gVRTrackingObjectTr.getRotation();
+
 			}	
+
 		}
 	}
 
 	if ((m_data->m_allowRealTimeSimulation) && m_data->m_guiHelper)
 	{
 		
-		///this hardcoded C++ scene creation is temporary for demo purposes. It will be done in Python later...
+		
-		if (gCreateDefaultRobotAssets)
-		{
-			createDefaultRobotAssets();
-		}
-
 		int maxSteps = m_data->m_numSimulationSubSteps+3;
 		if (m_data->m_numSimulationSubSteps)
 		{
@@ -6341,553 +6409,29 @@ void PhysicsServerCommandProcessor::resetSimulation()
 	m_data->m_bodyHandles.exitHandles();
 	m_data->m_bodyHandles.initHandles();
 
-	m_data->m_hasGround = false;
+	
-	m_data->m_gripperRigidbodyFixed = 0;
+	
 
 }
 
-//todo: move this to Python/scripting (it is almost ready to be removed!)
-void PhysicsServerCommandProcessor::createDefaultRobotAssets()
-{
-	static btAlignedObjectArray<char> gBufferServerToClient;
-	gBufferServerToClient.resize(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
-	int bodyId = 0;
-
-	if (gCreateObjectSimVR >= 0)
-	{
-		gCreateObjectSimVR = -1;
-		btMatrix3x3 mat(gVRGripperOrn);
-		btScalar spawnDistance = 0.1;
-		btVector3 spawnDir = mat.getColumn(0);
-		btVector3 shiftPos = spawnDir*spawnDistance;
-		btVector3 spawnPos = gVRGripperPos + shiftPos;
-		loadUrdf("sphere_small.urdf", spawnPos, gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size(),0);
-		//loadUrdf("lego/lego.urdf", spawnPos, gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		m_data->m_sphereId = bodyId;
-		InteralBodyData* parentBody = m_data->m_bodyHandles.getHandle(bodyId);
-		if (parentBody->m_multiBody)
-		{
-			parentBody->m_multiBody->setBaseVel(spawnDir * 5);
-		}
-	}
-
-	if (!m_data->m_hasGround)
-	{
-		m_data->m_hasGround = true;
-
-		loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		loadUrdf("samurai.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-//		loadUrdf("quadruped/quadruped.urdf", btVector3(2, 2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-
-		if (m_data->m_gripperRigidbodyFixed == 0)
-		{
-			int bodyId = 0;
-
-			if (loadUrdf("pr2_gripper.urdf", btVector3(-0.2, 0.15, 0.9), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()))
-			{
-				InteralBodyData* parentBody = m_data->m_bodyHandles.getHandle(bodyId);
-				if (parentBody->m_multiBody)
-				{
-					parentBody->m_multiBody->setHasSelfCollision(0);
-					btVector3 pivotInParent(0.2, 0, 0);
-					btMatrix3x3 frameInParent;
-					//frameInParent.setRotation(btQuaternion(0, 0, 0, 1));
-					frameInParent.setIdentity();
-					btVector3 pivotInChild(0, 0, 0);
-					btMatrix3x3 frameInChild;
-					frameInChild.setIdentity();
-
-					m_data->m_gripperRigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody, -1, 0, pivotInParent, pivotInChild, frameInParent, frameInChild);
-					m_data->m_gripperMultiBody = parentBody->m_multiBody;
-					if (m_data->m_gripperMultiBody->getNumLinks() > 2)
-					{
-						m_data->m_gripperMultiBody->setJointPos(0, 0);
-						m_data->m_gripperMultiBody->setJointPos(2, 0);
-					}
-					m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(500);
-					btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
-					world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed);
-				}
-			}	
-		}
-
-		loadUrdf("kuka_iiwa/model_vr_limits.urdf", btVector3(1.4, -0.2, 0.6), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		m_data->m_KukaId = bodyId;
-		if (m_data->m_KukaId>=0)
-		{
-			InteralBodyData* kukaBody = m_data->m_bodyHandles.getHandle(m_data->m_KukaId);
-			if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs() == 7)
-			{
-				btScalar q[7];
-				q[0] = 0;// -SIMD_HALF_PI;
-				q[1] = 0;
-				q[2] = 0;
-				q[3] = SIMD_HALF_PI;
-				q[4] = 0;
-				q[5] = -SIMD_HALF_PI*0.66;
-				q[6] = 0;
-
-				for (int i = 0; i < 7; i++)
-				{
-					kukaBody->m_multiBody->setJointPos(i, q[i]);
-				}
-				btAlignedObjectArray<btQuaternion> scratch_q;
-				btAlignedObjectArray<btVector3> scratch_m;
-				kukaBody->m_multiBody->forwardKinematics(scratch_q, scratch_m);
-				int nLinks = kukaBody->m_multiBody->getNumLinks();
-				scratch_q.resize(nLinks + 1);
-				scratch_m.resize(nLinks + 1);
-				kukaBody->m_multiBody->updateCollisionObjectWorldTransforms(scratch_q, scratch_m);
-			}
-		}
-#if 1
-		loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .8), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .9), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-#endif
-
-		//		loadUrdf("r2d2.urdf", btVector3(-2, -4, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-
-#if 1
-		// Load one motor gripper for kuka
-		loadSdf("gripper/wsg50_one_motor_gripper_new_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true,CUF_USE_SDF);
-		m_data->m_gripperId = bodyId + 1;
-		{
-		InteralBodyData* gripperBody = m_data->m_bodyHandles.getHandle(m_data->m_gripperId);
-
-		// Reset the default gripper motor maximum torque for damping to 0
-		for (int i = 0; i < gripperBody->m_multiBody->getNumLinks(); i++)
-		{
-			if (supportsJointMotor(gripperBody->m_multiBody, i))
-			{
-				btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)gripperBody->m_multiBody->getLink(i).m_userPtr;
-				if (motor)
-				{
-					motor->setMaxAppliedImpulse(0);
-				}
-			}
-		}
-		}
-#endif
-#if 1
-		for (int i = 0; i < 6; i++)
-		{
-			loadUrdf("jenga/jenga.urdf", btVector3(1.3-0.1*i,-0.7,  .75), btQuaternion(btVector3(0,1,0),SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		}
-#endif
-		//loadUrdf("nao/nao.urdf", btVector3(2,5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-
-		// Add slider joint for fingers
-		btVector3 pivotInParent1(-0.055, 0, 0.02);
-		btVector3 pivotInChild1(0, 0, 0);
-		btMatrix3x3 frameInParent1(btQuaternion(0, 0, 0, 1.0));
-		btMatrix3x3 frameInChild1(btQuaternion(0, 0, 0, 1.0));
-		btVector3 jointAxis1(1.0, 0, 0);
-		btVector3 pivotInParent2(0.055, 0, 0.02);
-		btVector3 pivotInChild2(0, 0, 0);
-		btMatrix3x3 frameInParent2(btQuaternion(0, 0, 0, 1.0));
-		btMatrix3x3 frameInChild2(btQuaternion(0, 0, 1.0, 0));
-		btVector3 jointAxis2(1.0, 0, 0);
-
-		if (m_data->m_gripperId>=0)
-		{
-		InteralBodyData* gripperBody = m_data->m_bodyHandles.getHandle(m_data->m_gripperId);
-		m_data->m_kukaGripperRevolute1 = new btMultiBodyPoint2Point(gripperBody->m_multiBody, 2, gripperBody->m_multiBody, 4, pivotInParent1, pivotInChild1);
-		m_data->m_kukaGripperRevolute1->setMaxAppliedImpulse(5.0);
-		m_data->m_kukaGripperRevolute2 = new btMultiBodyPoint2Point(gripperBody->m_multiBody, 3, gripperBody->m_multiBody, 6, pivotInParent2, pivotInChild2);
-		m_data->m_kukaGripperRevolute2->setMaxAppliedImpulse(5.0);
-
-		m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute1);
-		m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute2);
-
-		}
-
-		if (m_data->m_KukaId>=0)
-		{
-		InteralBodyData* kukaBody = m_data->m_bodyHandles.getHandle(m_data->m_KukaId);
-		if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs()==7)
-		{
-			if (m_data->m_gripperId>=0)
-			{
-			InteralBodyData* gripperBody = m_data->m_bodyHandles.getHandle(m_data->m_gripperId);
-
-			gripperBody->m_multiBody->setHasSelfCollision(0);
-			btVector3 pivotInParent(0, 0, 0.05);
-			btMatrix3x3 frameInParent;
-			frameInParent.setIdentity();
-			btVector3 pivotInChild(0, 0, 0);
-			btMatrix3x3 frameInChild;
-			frameInChild.setIdentity();
-
-			m_data->m_kukaGripperFixed = new btMultiBodyFixedConstraint(kukaBody->m_multiBody, 6, gripperBody->m_multiBody, 0, pivotInParent, pivotInChild, frameInParent, frameInChild);
-			m_data->m_kukaGripperMultiBody = gripperBody->m_multiBody;
-			m_data->m_kukaGripperFixed->setMaxAppliedImpulse(500);
-			m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperFixed);
-			}
-		}
-		}
-#if 0
-
-		for (int i = 0; i < 10; i++)
-		{
-			loadUrdf("cube.urdf", btVector3(-4, -2, 0.5 + i), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		}
-
-		loadUrdf("sphere2.urdf", btVector3(-5, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		loadUrdf("sphere2.urdf", btVector3(-5, 0, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		loadUrdf("sphere2.urdf", btVector3(-5, 0, 3), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-#endif		
-		btTransform objectLocalTr[] = {
-			btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.0, 0.0, 0.0)),
-			btTransform(btQuaternion(btVector3(0,0,1),-SIMD_HALF_PI), btVector3(0.0, 0.15, 0.64)),
-			btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.1, 0.15, 0.85)),
-			btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.4, 0.05, 0.85)),
-			btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.3, -0.05, 0.7)),
-			btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.1, 0.05, 0.7)),
-			btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.2, 0.15, 0.7)),
-			btTransform(btQuaternion(0, 0, 0, 1), btVector3(-0.2, 0.15, 0.9)),
-			btTransform(btQuaternion(0, 0, 0, 1), btVector3(0.2, 0.05, 0.8))
-		};
-
-
-		btAlignedObjectArray<btTransform> objectWorldTr;
-		int numOb = sizeof(objectLocalTr) / sizeof(btTransform);
-		objectWorldTr.resize(numOb);
-
-		btTransform tr;
-		tr.setIdentity();
-		tr.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI));
-		tr.setOrigin(btVector3(1.0, -0.2, 0));
-
-		for (int i = 0; i < numOb; i++)
-		{
-			objectWorldTr[i] = tr*objectLocalTr[i];
-		}
-
-		// Table area
-		loadUrdf("table/table.urdf", objectWorldTr[0].getOrigin(), objectWorldTr[0].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		//loadUrdf("tray/tray_textured.urdf", objectWorldTr[1].getOrigin(), objectWorldTr[1].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		//loadUrdf("cup_small.urdf", objectWorldTr[2].getOrigin(), objectWorldTr[2].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		//loadUrdf("pitcher_small.urdf", objectWorldTr[3].getOrigin(), objectWorldTr[3].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		loadUrdf("teddy_vhacd.urdf", objectWorldTr[4].getOrigin(), objectWorldTr[4].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-
-		loadUrdf("cube_small.urdf", objectWorldTr[5].getOrigin(), objectWorldTr[5].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		loadUrdf("sphere_small.urdf", objectWorldTr[6].getOrigin(), objectWorldTr[6].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		loadUrdf("duck_vhacd.urdf", objectWorldTr[7].getOrigin(), objectWorldTr[7].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		//loadUrdf("Apple/apple.urdf", objectWorldTr[8].getOrigin(), objectWorldTr[8].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-
-		// Shelf area
-		loadSdf("kiva_shelf/model.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true, CUF_USE_SDF);
-		loadUrdf("teddy_vhacd.urdf", btVector3(-0.1, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());			
-		loadUrdf("sphere_small.urdf", btVector3(-0.1, 0.6, 1.25), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		loadUrdf("cube_small.urdf", btVector3(0.3, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-			
-		// Chess area
-		loadUrdf("table_square/table_square.urdf", btVector3(-1.0, 0, 0.0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		//loadUrdf("pawn.urdf", btVector3(-0.8, -0.1, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		//loadUrdf("queen.urdf", btVector3(-0.9, -0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		//loadUrdf("king.urdf", btVector3(-1.0, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		//loadUrdf("bishop.urdf", btVector3(-1.1, 0.1, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		//loadUrdf("rook.urdf", btVector3(-1.2, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		//loadUrdf("knight.urdf", btVector3(-1.2, 0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-
-		loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-		m_data->m_huskyId = bodyId;
-
-		m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
-
-	}
-
-	if (m_data->m_kukaGripperFixed && m_data->m_kukaGripperMultiBody)
-	{
-		InteralBodyData* childBody = m_data->m_bodyHandles.getHandle(m_data->m_gripperId);
-		// Add gripper controller
-		btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)childBody->m_multiBody->getLink(1).m_userPtr;
-		if (motor)
-		{
-			btScalar posTarget = (-0.048)*btMin(btScalar(0.75), gVRGripper2Analog) / 0.75;
-			motor->setPositionTarget(posTarget, .8);
-			motor->setVelocityTarget(0.0, .5);
-			motor->setMaxAppliedImpulse(1.0);
-
-		}
-	}
-
-	if (m_data->m_gripperRigidbodyFixed && m_data->m_gripperMultiBody)
-	{
-		m_data->m_gripperRigidbodyFixed->setFrameInB(btMatrix3x3(gVRGripperOrn));
-		m_data->m_gripperRigidbodyFixed->setPivotInB(gVRGripperPos);
-		btScalar avg = 0.f;
-
-		for (int i = 0; i < m_data->m_gripperMultiBody->getNumLinks(); i++)
-		{
-			if (supportsJointMotor(m_data->m_gripperMultiBody, i))
-			{
-				btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_data->m_gripperMultiBody->getLink(i ).m_userPtr;
-				if (motor)
-				{
-					motor->setErp(0.2);
-					btScalar posTarget = 0.1 + (1 - btMin(btScalar(0.75),gVRGripperAnalog)*btScalar(1.5))*SIMD_HALF_PI*0.29;
-					btScalar maxPosTarget = 0.55;
-					
-					btScalar correction = 0.f;
-
-					if (avg)
-					{
-						correction = m_data->m_gripperMultiBody->getJointPos(i) - avg;
-					}
-					
-					if (m_data->m_gripperMultiBody->getJointPos(i) < 0)
-					{
-						m_data->m_gripperMultiBody->setJointPos(i,0);
-					}
-					if (m_data->m_gripperMultiBody->getJointPos(i) > maxPosTarget)
-					{
-						m_data->m_gripperMultiBody->setJointPos(i, maxPosTarget);
-					}
-
-					if (avg)
-					{
-						motor->setPositionTarget(avg, 1);
-					}
-					else
-					{
-						motor->setPositionTarget(posTarget, 1);
-					}
-					motor->setVelocityTarget(0, 0.5);
-					btScalar maxImp = (1+0.1*i)*m_data->m_physicsDeltaTime;
-					motor->setMaxAppliedImpulse(maxImp);
-					avg = m_data->m_gripperMultiBody->getJointPos(i);
-
-					//motor->setRhsClamp(gRhsClamp);
-				}
-			}
-		}
-	}
-
-	// Inverse kinematics for KUKA
-	if (m_data->m_KukaId>=0)
-	{
-		InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(m_data->m_KukaId);
-		if (bodyHandle && bodyHandle->m_multiBody && bodyHandle->m_multiBody->getNumDofs()==7)
-		{
-			btMultiBody* mb = bodyHandle->m_multiBody;				
-			btScalar sqLen = (mb->getBaseWorldTransform().getOrigin() - gVRController2Pos).length2();
-			btScalar distanceThreshold = 1.3;
-			gCloseToKuka=(sqLen<(distanceThreshold*distanceThreshold));
-
-			int numDofs = bodyHandle->m_multiBody->getNumDofs();
-			btAlignedObjectArray<double> q_new;
-			btAlignedObjectArray<double> q_current;
-			q_current.resize(numDofs);
-			for (int i = 0; i < numDofs; i++)
-			{
-				q_current[i] = bodyHandle->m_multiBody->getJointPos(i);
-			}
-                       
-			q_new.resize(numDofs);
-			//sensible rest-pose
-			q_new[0] = 0;// -SIMD_HALF_PI;
-			q_new[1] = 0;
-			q_new[2] = 0;
-			q_new[3] = SIMD_HALF_PI;
-			q_new[4] = 0;
-			q_new[5] = -SIMD_HALF_PI*0.66;
-			q_new[6] = 0;
-
-			if (gCloseToKuka && gEnableKukaControl)
-			{
-				double dampIk[6] = {1.0, 1.0, 1.0, 1.0, 1.0, 0.0};
-
-				IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody);
-				IKTrajectoryHelper* ikHelperPtr = 0;
-
-				if (ikHelperPtrPtr)
-				{
-					ikHelperPtr = *ikHelperPtrPtr;
-				}
-				else
-				{
-					IKTrajectoryHelper* tmpHelper = new IKTrajectoryHelper;
-					m_data->m_inverseKinematicsHelpers.insert(bodyHandle->m_multiBody, tmpHelper);
-					ikHelperPtr = tmpHelper;
-				}
-
-				int endEffectorLinkIndex = 6;
-
-				if (ikHelperPtr && (endEffectorLinkIndex<bodyHandle->m_multiBody->getNumLinks()))
-				{					
-					b3AlignedObjectArray<double> jacobian_linear;
-					jacobian_linear.resize(3*numDofs);
-					b3AlignedObjectArray<double> jacobian_angular;
-					jacobian_angular.resize(3*numDofs);
-					int jacSize = 0;
-                                
-					btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
-                               
-					if (tree)
-					{
-						jacSize = jacobian_linear.size();
-						// Set jacobian value
-						int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
-                                                                      
-						btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs);
-						for (int i = 0; i < numDofs; i++)
-						{
-							q_current[i] = bodyHandle->m_multiBody->getJointPos(i);
-							q[i+baseDofs] = bodyHandle->m_multiBody->getJointPos(i);
-							qdot[i + baseDofs] = 0;
-							nu[i+baseDofs] = 0;
-						}
-						// Set the gravity to correspond to the world gravity
-						btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
-                                    
-						if (-1 != tree->setGravityInWorldFrame(id_grav) &&
-							-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
-						{
-							tree->calculateJacobians(q);
-							btInverseDynamics::mat3x jac_t(3,numDofs);
-							btInverseDynamics::mat3x jac_r(3,numDofs);
-							tree->getBodyJacobianTrans(endEffectorLinkIndex+1, &jac_t);
-							tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r);
-							for (int i = 0; i < 3; ++i)
-							{
-								for (int j = 0; j < numDofs; ++j)
-								{
-									jacobian_linear[i*numDofs+j] = jac_t(i,j);
-									jacobian_angular[i*numDofs+j] = jac_r(i,j);
-								}
-							}
-						}
-					}
-                       
-					int ikMethod= IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE;//IK2_VEL_DLS_WITH_ORIENTATION; //IK2_VEL_DLS;
-                               
-					btVector3DoubleData endEffectorWorldPosition;
-					btVector3DoubleData endEffectorWorldOrientation;
-					btVector3DoubleData targetWorldPosition;
-					btVector3DoubleData targetWorldOrientation;
-                                
-					btVector3 endEffectorPosWorld =  bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getOrigin();
-					btQuaternion endEffectorOriWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getRotation();
-					btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w());
-                        
-					// Prescribed position and orientation
-					static btScalar time=0.f;
-					time+=0.01;
-					btVector3 targetPos(0.4-0.4*b3Cos( time), 0, 0.8+0.4*b3Cos( time));
-					targetPos +=mb->getBasePos();
-					btVector4 downOrn(0,1,0,0);
-
-					// Controller orientation
-					btVector4 controllerOrn(gVRController2Orn.x(), gVRController2Orn.y(), gVRController2Orn.z(), gVRController2Orn.w());
-						
-					// Set position and orientation
-					endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);
-					endEffectorOri.serializeDouble(endEffectorWorldOrientation);
-					downOrn.serializeDouble(targetWorldOrientation);
-					//targetPos.serializeDouble(targetWorldPosition);
-						
-					gVRController2Pos.serializeDouble(targetWorldPosition);
-						
-					//controllerOrn.serializeDouble(targetWorldOrientation);
-
-					if (ikMethod == IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE)
-					{
-						btAlignedObjectArray<double> lower_limit;
-						btAlignedObjectArray<double> upper_limit;
-						btAlignedObjectArray<double> joint_range;
-						btAlignedObjectArray<double> rest_pose;
-						lower_limit.resize(numDofs);
-						upper_limit.resize(numDofs);
-						joint_range.resize(numDofs);
-						rest_pose.resize(numDofs);
-						lower_limit[0] = -.967;
-						lower_limit[1] = -2.0;
-						lower_limit[2] = -2.96;
-						lower_limit[3] = 0.19;
-						lower_limit[4] = -2.96;
-						lower_limit[5] = -2.09;
-						lower_limit[6] = -3.05;
-						upper_limit[0] = .96;
-						upper_limit[1] = 2.0;
-						upper_limit[2] = 2.96;
-						upper_limit[3] = 2.29;
-						upper_limit[4] = 2.96;
-						upper_limit[5] = 2.09;
-						upper_limit[6] = 3.05;
-						joint_range[0] = 5.8;
-						joint_range[1] = 4;
-						joint_range[2] = 5.8;
-						joint_range[3] = 4;
-						joint_range[4] = 5.8;
-						joint_range[5] = 4;
-						joint_range[6] = 6;
-						rest_pose[0] = 0;
-						rest_pose[1] = 0;
-						rest_pose[2] = 0;
-						rest_pose[3] = SIMD_HALF_PI;
-						rest_pose[4] = 0;
-						rest_pose[5] = -SIMD_HALF_PI*0.66;
-						rest_pose[6] = 0;
-						ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]);
-					}
-
-					ikHelperPtr->computeIK(targetWorldPosition.m_floats, targetWorldOrientation.m_floats,
-													endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
-													&q_current[0],
-													numDofs, endEffectorLinkIndex,
-								&q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, dampIk);
-				}
-			}
-
-			//directly set the position of the links, only for debugging IK, don't use this method!
-#if 0			
-			if (0)
-			{
-				for (int i=0;i<mb->getNumLinks();i++)
-				{
-					btScalar desiredPosition = q_new[i];
-					mb->setJointPosMultiDof(i,&desiredPosition);
-				}
-			} else
-#endif
-			{
-				int numMotors = 0;
-				//find the joint motors and apply the desired velocity and maximum force/torque
-				{
-					int velIndex = 6;//skip the 3 linear + 3 angular degree of freedom velocity entries of the base
-					int posIndex = 7;//skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base
-					for (int link=0;link<mb->getNumLinks();link++)
-					{
-						if (supportsJointMotor(mb,link))
-						{
-							btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr;
-
-							if (motor)
-							{
-								btScalar desiredVelocity = 0.f;
-								btScalar desiredPosition = q_new[link];
-								motor->setRhsClamp(gRhsClamp);
-								//printf("link %d: %f", link, q_new[link]);
-								motor->setVelocityTarget(desiredVelocity,1.0);
-								motor->setPositionTarget(desiredPosition,0.6);
-								btScalar maxImp = 1.0;
-									
-								motor->setMaxAppliedImpulse(maxImp);
-								numMotors++;
-							}
-						}
-						velIndex += mb->getLink(link).m_dofCount;
-						posIndex += mb->getLink(link).m_posVarCount;
-					}
-				}
-			}
-		} 
-			
-	}
-}
 
 void PhysicsServerCommandProcessor::setTimeOut(double /*timeOutInSeconds*/)
 {
 }
+
+const btVector3& PhysicsServerCommandProcessor::getVRTeleportPosition() const
+{
+	return gVRTeleportPos1;
+}
+void PhysicsServerCommandProcessor::setVRTeleportPosition(const btVector3& vrTeleportPos)
+{
+	gVRTeleportPos1 = vrTeleportPos;
+}
+const btQuaternion& PhysicsServerCommandProcessor::getVRTeleportOrientation() const
+{
+	return gVRTeleportOrn;
+}
+void PhysicsServerCommandProcessor::setVRTeleportOrientation(const btQuaternion& vrTeleportOrn)
+{
+	gVRTeleportOrn = vrTeleportOrn;
+}

examples/SharedMemory/PhysicsServerCommandProcessor.h

@@ -15,16 +15,11 @@ struct SharedMemLines
 
 
 ///todo: naming. Perhaps PhysicsSdkCommandprocessor?
-class PhysicsServerCommandProcessor : public PhysicsCommandProcessorInterface
+class PhysicsServerCommandProcessor : public CommandProcessorInterface
 {
 
 	struct PhysicsServerCommandProcessorInternalData* m_data;
 
-	
-
-	//todo: move this to physics client side / Python
-	void createDefaultRobotAssets();
-
 	void resetSimulation();
 
 protected:
@@ -91,19 +86,27 @@ public:
 	virtual void removePickingConstraint();
 
 	//logging /playback the shared memory commands
-	void enableCommandLogging(bool enable, const char* fileName);
+	virtual void enableCommandLogging(bool enable, const char* fileName);
-	void replayFromLogFile(const char* fileName);
+	virtual void replayFromLogFile(const char* fileName);
-	void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes );
+	virtual void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes );
 
 	//logging of object states (position etc)
 	void logObjectStates(btScalar timeStep);
 	void processCollisionForces(btScalar timeStep);
 
-	void stepSimulationRealTime(double dtInSec,	const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents);
+	virtual void stepSimulationRealTime(double dtInSec,	const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents);
-	void enableRealTimeSimulation(bool enableRealTimeSim);
+	virtual void enableRealTimeSimulation(bool enableRealTimeSim);
+	virtual bool isRealTimeSimulationEnabled() const;
+
 	void applyJointDamping(int bodyUniqueId);
 
 	virtual void setTimeOut(double timeOutInSeconds);
+
+	virtual const btVector3& getVRTeleportPosition() const;
+	virtual void setVRTeleportPosition(const btVector3& vrTeleportPos);
+
+	virtual const btQuaternion& getVRTeleportOrientation() const;
+	virtual void setVRTeleportOrientation(const btQuaternion& vrTeleportOrn);
 };
 
 #endif //PHYSICS_SERVER_COMMAND_PROCESSOR_H

examples/SharedMemory/PhysicsServerExample.cpp

@@ -4,9 +4,7 @@
 
 #include "PhysicsServerExample.h"
 
-#ifdef B3_USE_MIDI
+
-#include "RtMidi.h"
-#endif//B3_USE_MIDI
 
 #include "PhysicsServerSharedMemory.h"
 #include "Bullet3Common/b3CommandLineArgs.h"
@@ -24,37 +22,26 @@
 
 
 //@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
-extern btVector3 gLastPickPos;
 bool gEnablePicking=true;
 bool gEnableTeleporting=true;
 bool gEnableRendering= true;
 bool gEnableSyncPhysicsRendering= true;
 bool gEnableUpdateDebugDrawLines = true;
 
+
+//extern btVector3 gLastPickPos;
 btVector3 gVRTeleportPosLocal(0,0,0);
 btQuaternion gVRTeleportOrnLocal(0,0,0,1);
 
-extern btVector3 gVRTeleportPos1;
+
-extern btQuaternion gVRTeleportOrn;
 btScalar gVRTeleportRotZ = 0;
 
-extern btVector3 gVRGripperPos;
-extern btQuaternion gVRGripperOrn;
-extern btVector3 gVRController2Pos;
-extern btQuaternion gVRController2Orn;
-extern btScalar gVRGripperAnalog;
-extern btScalar gVRGripper2Analog;
-extern bool gCloseToKuka;
-extern bool gEnableRealTimeSimVR;
-extern bool gCreateDefaultRobotAssets;
 extern int gInternalSimFlags;
-extern int gCreateObjectSimVR;
 extern bool gResetSimulation;
-extern int gEnableKukaControl;
 int gGraspingController = -1;
 extern btScalar simTimeScalingFactor;
 bool gBatchUserDebugLines = true;
-extern bool gVRGripperClosed;
+
 
 const char* startFileNameVR = "0_VRDemoSettings.txt";
 
@@ -82,70 +69,20 @@ static void loadCurrentSettingsVR(b3CommandLineArgs& args)
 };
 
 //remember the settings (you don't want to re-tune again and again...)
-static void saveCurrentSettingsVR()
+
+
+static void saveCurrentSettingsVR(const btVector3& VRTeleportPos1)
 {
 	FILE* f = fopen(startFileNameVR, "w");
 	if (f)
 	{
-		fprintf(f, "--camPosX= %f\n", gVRTeleportPos1[0]);
+		fprintf(f, "--camPosX= %f\n", VRTeleportPos1[0]);
-		fprintf(f, "--camPosY= %f\n", gVRTeleportPos1[1]);
+		fprintf(f, "--camPosY= %f\n", VRTeleportPos1[1]);
-		fprintf(f, "--camPosZ= %f\n", gVRTeleportPos1[2]);
+		fprintf(f, "--camPosZ= %f\n", VRTeleportPos1[2]);
 		fprintf(f, "--camRotZ= %f\n", gVRTeleportRotZ);
 		fclose(f);
 	}
 };
-
-#if B3_USE_MIDI
-
-
-
-static float getParamf(float rangeMin, float rangeMax, int midiVal)
-{
-	float v = rangeMin + (rangeMax - rangeMin)* (float(midiVal / 127.));
-	return v;
-}
-void midiCallback(double deltatime, std::vector< unsigned char > *message, void *userData)
-{
-	unsigned int nBytes = message->size();
-	for (unsigned int i = 0; i<nBytes; i++)
-		std::cout << "Byte " << i << " = " << (int)message->at(i) << ", ";
-	if (nBytes > 0)
-		std::cout << "stamp = " << deltatime << std::endl;
-	
-	if (nBytes > 2)
-	{
-		
-		if (message->at(0) == 176)
-		{
-			if (message->at(1) == 16)
-			{
-				gVRTeleportRotZ= getParamf(-3.1415, 3.1415, message->at(2));
-				gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ);
-				saveCurrentSettingsVR();
-//				b3Printf("gVRTeleportOrnLocal rotZ = %f\n", gVRTeleportRotZ);
-			}
-
-			if (message->at(1) == 32)
-			{
-				gCreateDefaultRobotAssets = 1;
-			}
-
-			for (int i = 0; i < 3; i++)
-			{
-				if (message->at(1) == i)
-				{
-					gVRTeleportPos1[i] = getParamf(-2, 2, message->at(2));
-					saveCurrentSettingsVR();
-//					b3Printf("gVRTeleportPos[%d] =  %f\n", i,gVRTeleportPosLocal[i]);
-
-				}
-			}
-		}
-	}
-}
-
-#endif //B3_USE_MIDI
-
 bool gDebugRenderToggle  = false;
 void	MotionThreadFunc(void* userPtr,void* lsMemory);
 void*	MotionlsMemoryFunc();
@@ -186,6 +123,7 @@ enum MultiThreadedGUIHelperCommunicationEnums
 	eGUIDumpFramesToVideo,
 	eGUIHelperRemoveGraphicsInstance,
 	eGUIHelperChangeGraphicsInstanceRGBAColor,
+	eGUIHelperChangeGraphicsInstanceSpecularColor,
 	eGUIHelperSetVisualizerFlag,
 };
 
@@ -291,7 +229,6 @@ struct MotionThreadLocalStorage
 
 
 float clampedDeltaTime  = 0.2;
-extern int gMaxNumCmdPer1ms;
 
 
 void	MotionThreadFunc(void* userPtr,void* lsMemory)
@@ -325,16 +262,7 @@ void	MotionThreadFunc(void* userPtr,void* lsMemory)
 
 			{
 				
-				if (gMaxNumCmdPer1ms>0)
+
-				{
-					if (numCmdSinceSleep1ms>gMaxNumCmdPer1ms)
-					{
-						BT_PROFILE("usleep(10)");
-						b3Clock::usleep(10);
-						numCmdSinceSleep1ms = 0;
-						sleepClock.reset();
-					}
-				}
 				if (sleepClock.getTimeMilliseconds()>1)
 				{
 					sleepClock.reset();
@@ -600,7 +528,7 @@ struct ColorWidth
 
 ATTRIBUTE_ALIGNED16( class )MultithreadedDebugDrawer : public btIDebugDraw
 {
-	class GUIHelperInterface* m_guiHelper;
+	struct GUIHelperInterface* m_guiHelper;
 	int m_debugMode;
 
 	btAlignedObjectArray< btAlignedObjectArray<unsigned int> > m_sortedIndices;
@@ -991,6 +919,19 @@ public:
 		workerThreadWait();    
 	}
 
+	double m_specularColor[3];
+	int m_graphicsInstanceChangeSpecular;
+	virtual void changeSpecularColor(int instanceUid, const double specularColor[3])
+	{
+		m_graphicsInstanceChangeSpecular = instanceUid;
+		m_specularColor[0] = specularColor[0];
+		m_specularColor[1] = specularColor[1];
+		m_specularColor[2] = specularColor[2];
+		m_cs->lock();
+		m_cs->setSharedParam(1,eGUIHelperChangeGraphicsInstanceSpecularColor);
+		workerThreadWait(); 
+	}
+
 	virtual Common2dCanvasInterface* get2dCanvasInterface()
 	{
 		return 0;
@@ -1229,9 +1170,7 @@ class PhysicsServerExample : public SharedMemoryCommon
 	MotionArgs m_args[MAX_MOTION_NUM_THREADS];
 	MultiThreadedOpenGLGuiHelper* m_multiThreadedHelper;
     bool m_wantsShutdown;
-#ifdef B3_USE_MIDI
+
-	RtMidiIn* m_midi;
-#endif
     bool m_isConnected;
     btClock m_clock;
 	bool m_replay;
@@ -1243,7 +1182,7 @@ class PhysicsServerExample : public SharedMemoryCommon
 
 public:
 
-	PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper, SharedMemoryInterface* sharedMem=0, int options=0);
+	PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper, CommandProcessorCreationInterface* commandProcessorCreator, SharedMemoryInterface* sharedMem=0, int options=0);
 
 	virtual ~PhysicsServerExample();
 
@@ -1434,43 +1373,54 @@ public:
 		if (window->isModifierKeyPressed(B3G_SHIFT))
 			shift=0.01;
 
+		btVector3 VRTeleportPos =this->m_physicsServer.getVRTeleportPosition();
+
 		if (key=='w' && state)
 		{
-			gVRTeleportPos1[0]+=shift;
+			VRTeleportPos[0]+=shift;
-			saveCurrentSettingsVR();
+			m_physicsServer.setVRTeleportPosition(VRTeleportPos);
+			saveCurrentSettingsVR(VRTeleportPos);
 		}
 		if (key=='s' && state)
 		{
-			gVRTeleportPos1[0]-=shift;
+			VRTeleportPos[0]-=shift;
-			saveCurrentSettingsVR();
+			m_physicsServer.setVRTeleportPosition(VRTeleportPos);
+			saveCurrentSettingsVR(VRTeleportPos);
 		}
 		if (key=='a' && state)
 		{
-			gVRTeleportPos1[1]-=shift;
+			VRTeleportPos[1]-=shift;
-			saveCurrentSettingsVR();
+			m_physicsServer.setVRTeleportPosition(VRTeleportPos);
+			saveCurrentSettingsVR(VRTeleportPos);
 		}
 		if (key=='d' && state)
 		{
-			gVRTeleportPos1[1]+=shift;
+			VRTeleportPos[1]+=shift;
-			saveCurrentSettingsVR();
+			m_physicsServer.setVRTeleportPosition(VRTeleportPos);
+			saveCurrentSettingsVR(VRTeleportPos);
 		}
 		if (key=='q' && state)
 		{
-			gVRTeleportPos1[2]+=shift;
+			VRTeleportPos[2]+=shift;
-			saveCurrentSettingsVR();
+			m_physicsServer.setVRTeleportPosition(VRTeleportPos);
+			saveCurrentSettingsVR(VRTeleportPos);
 		}
 		if (key=='e' && state)
 		{
-			gVRTeleportPos1[2]-=shift;
+			VRTeleportPos[2]-=shift;
-			saveCurrentSettingsVR();
+			m_physicsServer.setVRTeleportPosition(VRTeleportPos);
+			saveCurrentSettingsVR(VRTeleportPos);
 		}
 		if (key=='z' && state)
 		{
 			gVRTeleportRotZ+=shift;
-			gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ);
+			btQuaternion VRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ);
-			saveCurrentSettingsVR();
+			m_physicsServer.setVRTeleportOrientation(VRTeleportOrn);
+			saveCurrentSettingsVR(VRTeleportPos);
 		}
 		
+		
+		
 		return false;
 	}
 
@@ -1490,33 +1440,33 @@ public:
 			setSharedMemoryKey(shmemKey);
 		}
 
-		if (args.GetCmdLineArgument("camPosX", gVRTeleportPos1[0]))
+		btVector3 vrTeleportPos = m_physicsServer.getVRTeleportPosition();
+
+		if (args.GetCmdLineArgument("camPosX", vrTeleportPos[0]))
 		{
-			printf("camPosX=%f\n", gVRTeleportPos1[0]);
+			printf("camPosX=%f\n", vrTeleportPos[0]);
 		}
 
-		if (args.GetCmdLineArgument("camPosY", gVRTeleportPos1[1]))
+		if (args.GetCmdLineArgument("camPosY", vrTeleportPos[1]))
 		{
-			printf("camPosY=%f\n", gVRTeleportPos1[1]);
+			printf("camPosY=%f\n", vrTeleportPos[1]);
 		}
 
-		if (args.GetCmdLineArgument("camPosZ", gVRTeleportPos1[2]))
+		if (args.GetCmdLineArgument("camPosZ", vrTeleportPos[2]))
 		{
-			printf("camPosZ=%f\n", gVRTeleportPos1[2]);
+			printf("camPosZ=%f\n", vrTeleportPos[2]);
 		}
 
+		m_physicsServer.setVRTeleportPosition(vrTeleportPos);
+
 		float camRotZ = 0.f;
 		if (args.GetCmdLineArgument("camRotZ", camRotZ))
 		{
 			printf("camRotZ = %f\n", camRotZ);
 			btQuaternion ornZ(btVector3(0, 0, 1), camRotZ);
-			gVRTeleportOrn = ornZ;
+			m_physicsServer.setVRTeleportOrientation(ornZ);
 		}
 
-		if (args.CheckCmdLineFlag("robotassets"))
-		{
-			gCreateDefaultRobotAssets = true;
-		}
 
 		if (args.CheckCmdLineFlag("realtimesimulation"))
 		{
@@ -1524,53 +1474,17 @@ public:
 			m_physicsServer.enableRealTimeSimulation(true);
 		}
 
-		if (args.CheckCmdLineFlag("norobotassets"))
+		
-		{
-			gCreateDefaultRobotAssets = false;
-		}
 
 
 	}
 
 };
 
-#ifdef B3_USE_MIDI
-static bool chooseMidiPort(RtMidiIn *rtmidi)
-{
-	/*
 
-	std::cout << "\nWould you like to open a virtual input port? [y/N] ";
+PhysicsServerExample::PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper,CommandProcessorCreationInterface* commandProcessorCreator, SharedMemoryInterface* sharedMem, int options)
-
-	std::string keyHit;
-	std::getline( std::cin, keyHit );
-	if ( keyHit == "y" ) {
-	rtmidi->openVirtualPort();
-	return true;
-	}
-	*/
-
-	std::string portName;
-	unsigned int i = 0, nPorts = rtmidi->getPortCount();
-	if (nPorts == 0) {
-		std::cout << "No midi input ports available!" << std::endl;
-		return false;
-	}
-
-	if (nPorts > 0) {
-		std::cout << "\nOpening midi input port " << rtmidi->getPortName() << std::endl;
-	}
-
-	//  std::getline( std::cin, keyHit );  // used to clear out stdin
-	rtmidi->openPort(i);
-
-	return true;
-}
-#endif //B3_USE_MIDI
-
-
-PhysicsServerExample::PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper, SharedMemoryInterface* sharedMem, int options)
 :SharedMemoryCommon(helper),
-m_physicsServer(sharedMem),
+m_physicsServer(commandProcessorCreator,sharedMem,0),
 m_wantsShutdown(false),
 m_isConnected(false),
 m_replay(false)
@@ -1579,14 +1493,7 @@ m_replay(false)
 ,m_tinyVrGui(0)
 #endif
 {
-#ifdef B3_USE_MIDI
-	m_midi = new   RtMidiIn();
-	chooseMidiPort(m_midi);
-	m_midi->setCallback(&midiCallback);
-	// Don't ignore sysex, timing, or active sensing messages.
-	m_midi->ignoreTypes(false, false, false);
 
-#endif
 	m_multiThreadedHelper = helper;
 //	b3Printf("Started PhysicsServer\n");
 }
@@ -1595,10 +1502,7 @@ m_replay(false)
 
 PhysicsServerExample::~PhysicsServerExample()
 {
-#ifdef B3_USE_MIDI
+
-	delete m_midi;
-	m_midi = 0;
-#endif
 #ifdef BT_ENABLE_VR
 	delete m_tinyVrGui;
 #endif
@@ -1776,22 +1680,22 @@ void	PhysicsServerExample::updateGraphics()
 
 		if (flag==COV_ENABLE_VR_TELEPORTING)
 		{
-			gEnableTeleporting = enable;
+			gEnableTeleporting = (enable!=0);
 		}
 
 		if (flag == COV_ENABLE_VR_PICKING)
 		{
-			gEnablePicking = enable;
+			gEnablePicking = (enable!=0);
 		}
     
 		if (flag ==COV_ENABLE_SYNC_RENDERING_INTERNAL)
 		{
-			gEnableSyncPhysicsRendering = enable;
+			gEnableSyncPhysicsRendering = (enable!=0);
 		}
 
 		if (flag == COV_ENABLE_RENDERING)
 		{
-			gEnableRendering = enable;
+			gEnableRendering = (enable!=0);
 		}
 
 		m_multiThreadedHelper->m_childGuiHelper->setVisualizerFlag(m_multiThreadedHelper->m_visualizerFlag,m_multiThreadedHelper->m_visualizerEnable);
@@ -1813,6 +1717,13 @@ void	PhysicsServerExample::updateGraphics()
 	}
 	case eGUIHelperRemoveAllGraphicsInstances:
         {
+#ifdef BT_ENABLE_VR
+			if (m_tinyVrGui)
+			{
+				delete m_tinyVrGui;
+				m_tinyVrGui=0;
+			}
+#endif //BT_ENABLE_VR
             m_multiThreadedHelper->m_childGuiHelper->removeAllGraphicsInstances();
 			if (m_multiThreadedHelper->m_childGuiHelper->getRenderInterface())
 			{
@@ -1837,6 +1748,13 @@ void	PhysicsServerExample::updateGraphics()
 		m_multiThreadedHelper->mainThreadRelease();
 		break;
 	}
+	case eGUIHelperChangeGraphicsInstanceSpecularColor:
+	{
+		m_multiThreadedHelper->m_childGuiHelper->changeSpecularColor(m_multiThreadedHelper->m_graphicsInstanceChangeSpecular,m_multiThreadedHelper->m_specularColor);
+		m_multiThreadedHelper->mainThreadRelease();
+		break;
+
+	}
 
     case eGUIHelperCopyCameraImageData:
         {
@@ -2000,7 +1918,6 @@ extern int gDroppedSimulationSteps;
 extern int gNumSteps;
 extern double gDtInSec;
 extern double gSubStep;
-extern int gVRTrackingObjectUniqueId;
 extern btTransform gVRTrackingObjectTr;
 
 
@@ -2199,33 +2116,23 @@ void PhysicsServerExample::drawUserDebugLines()
 void PhysicsServerExample::renderScene()
 {
 	btTransform vrTrans;
-	//gVRTeleportPos1 = gVRTeleportPosLocal;
-	//gVRTeleportOrn = gVRTeleportOrnLocal;
 
-	///little VR test to follow/drive Husky vehicle
-	if (gVRTrackingObjectUniqueId >= 0)
-	{
-		btTransform vrTrans;
-		vrTrans.setOrigin(gVRTeleportPosLocal);
-		vrTrans.setRotation(gVRTeleportOrnLocal);
-			
-		vrTrans = vrTrans * gVRTrackingObjectTr;
 
-		gVRTeleportPos1 = vrTrans.getOrigin();
-		gVRTeleportOrn = vrTrans.getRotation();
-	}
 		
 
 	B3_PROFILE("PhysicsServerExample::RenderScene");
 
 	drawUserDebugLines();
 
-	if (gEnableRealTimeSimVR)
+	if (m_physicsServer.isRealTimeSimulationEnabled())
 	{
 		
 		static int frameCount=0;
-		//static btScalar prevTime = m_clock.getTimeSeconds();
+		static btScalar prevTime = m_clock.getTimeSeconds();
 		frameCount++;
+		
+		static char line0[1024]={0};
+		static char line1[1024]={0};
 
 #if 0
 
@@ -2233,7 +2140,8 @@ void PhysicsServerExample::renderScene()
 		int numFrames = 200;
 		static int count = 0;
 		count++;
-
+		
+			
 		if (0 == (count & 1))
 		{
 			btScalar curTime = m_clock.getTimeSeconds();
@@ -2274,14 +2182,13 @@ void PhysicsServerExample::renderScene()
 		{
 
 			b3Transform tr;tr.setIdentity();
-			tr.setOrigin(b3MakeVector3(gVRController2Pos[0],gVRController2Pos[1],gVRController2Pos[2]));
+			btVector3 VRController2Pos = m_physicsServer.getVRTeleportPosition();
-			tr.setRotation(b3Quaternion(gVRController2Orn[0],gVRController2Orn[1],gVRController2Orn[2],gVRController2Orn[3]));
+			btQuaternion VRController2Orn = m_physicsServer.getVRTeleportOrientation();
+			tr.setOrigin(b3MakeVector3(VRController2Pos[0],VRController2Pos[1],VRController2Pos[2]));
+			tr.setRotation(b3Quaternion(VRController2Orn[0],VRController2Orn[1],VRController2Orn[2],VRController2Orn[3]));
 			tr = tr*b3Transform(b3Quaternion(0,0,-SIMD_HALF_PI),b3MakeVector3(0,0,0));
 			b3Scalar dt = 0.01;
 			m_tinyVrGui->clearTextArea();
-			static char line0[1024];
-			static char line1[1024];
-
 			m_tinyVrGui->grapicalPrintf(line0,0,0,0,0,0,255);
 			m_tinyVrGui->grapicalPrintf(line1,0,16,255,255,255,255);
 
@@ -2296,8 +2203,8 @@ void PhysicsServerExample::renderScene()
 	btTransform tr2a, tr2;
 	tr2a.setIdentity();
 	tr2.setIdentity();
-	tr2.setOrigin(gVRTeleportPos1);
+	tr2.setOrigin(m_physicsServer.getVRTeleportPosition());
-	tr2a.setRotation(gVRTeleportOrn);
+	tr2a.setRotation(m_physicsServer.getVRTeleportOrientation());
 	btTransform trTotal = tr2*tr2a;
 	btTransform trInv = trTotal.inverse();
 
@@ -2360,9 +2267,8 @@ void PhysicsServerExample::renderScene()
 
 	if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
 	{
-		if (!gEnableRealTimeSimVR)
+		if (!m_physicsServer.isRealTimeSimulationEnabled())
 		{
-			gEnableRealTimeSimVR = true;
 			m_physicsServer.enableRealTimeSimulation(1);
 		}
 	}
@@ -2462,35 +2368,6 @@ btVector3	PhysicsServerExample::getRayTo(int x,int y)
 }
 
 
-extern int gSharedMemoryKey;
-
-
-class CommonExampleInterface*    PhysicsServerCreateFunc(struct CommonExampleOptions& options)
-{
-
-	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);
-	}
-	if (options.m_option & PHYSICS_SERVER_ENABLE_COMMAND_LOGGING)
-	{
-		example->enableCommandLogging();
-	}
-	if (options.m_option & PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG)
-	{
-		example->replayFromLogFile();
-	}
-	return example;
-
-}
-
 
 
 void	PhysicsServerExample::vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orn[4])
@@ -2503,7 +2380,6 @@ void	PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
 	if (gGraspingController < 0)
 	{
 		gGraspingController = controllerId;
-		gEnableKukaControl = true;
 	}
 
 	btTransform trLocal;
@@ -2522,8 +2398,8 @@ void	PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
 
 
 
-	tr2.setOrigin(gVRTeleportPos1);
+	tr2.setOrigin(m_physicsServer.getVRTeleportPosition());
-	tr2a.setRotation(gVRTeleportOrn);
+	tr2a.setRotation(m_physicsServer.getVRTeleportOrientation());
 
 
 	btTransform trTotal = tr2*tr2a*trOrg*trLocal;
@@ -2533,8 +2409,7 @@ void	PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
 	{
 		if (button == 1 && state == 0)
 		{
-			//gResetSimulation = true;
+			
-			gVRTeleportPos1 = gLastPickPos;
 		}
 	} else
 	{
@@ -2575,7 +2450,6 @@ void	PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
 
 		if (controllerId == gGraspingController)
 		{
-			gCreateObjectSimVR = 1;
 		}
 		else
 		{
@@ -2591,7 +2465,7 @@ void	PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
 
 	if (controllerId == gGraspingController && (button == 33))
 	{
-		gVRGripperClosed =(state!=0);
+		
 	}
 	else
 	{
@@ -2661,25 +2535,18 @@ void	PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[
 
 	
 
-	tr2.setOrigin(gVRTeleportPos1);
+	tr2.setOrigin(m_physicsServer.getVRTeleportPosition());
-	tr2a.setRotation(gVRTeleportOrn);
+	tr2a.setRotation(m_physicsServer.getVRTeleportOrientation());
 
 
 	btTransform trTotal = tr2*tr2a*trOrg*trLocal;
 
 	if (controllerId == gGraspingController)
 	{
-		gVRGripperAnalog = analogAxis;
 
-		gVRGripperPos = trTotal.getOrigin();
-		gVRGripperOrn = trTotal.getRotation();
 	}
 	else
 	{
-		gVRGripper2Analog = analogAxis;
-		gVRController2Pos = trTotal.getOrigin();
-		gVRController2Orn = trTotal.getRotation();
-		
 		m_args[0].m_vrControllerPos[controllerId] = trTotal.getOrigin();
 		m_args[0].m_vrControllerOrn[controllerId] = trTotal.getRotation();
 	}
@@ -2722,8 +2589,8 @@ void	PhysicsServerExample::vrHMDMoveCallback(int controllerId, float pos[4], flo
 	tr2a.setIdentity();
 	btTransform tr2;
 	tr2.setIdentity();
-	tr2.setOrigin(gVRTeleportPos1);
+	tr2.setOrigin(m_physicsServer.getVRTeleportPosition());
-	tr2a.setRotation(gVRTeleportOrn);
+	tr2a.setRotation(m_physicsServer.getVRTeleportOrientation());
 	btTransform trTotal = tr2*tr2a*trOrg*trLocal;
 
 
@@ -2764,8 +2631,8 @@ void	PhysicsServerExample::vrGenericTrackerMoveCallback(int controllerId, float
 	tr2a.setIdentity();
 	btTransform tr2;
 	tr2.setIdentity();
-	tr2.setOrigin(gVRTeleportPos1);
+	tr2.setOrigin(m_physicsServer.getVRTeleportPosition());
-	tr2a.setRotation(gVRTeleportOrn);
+	tr2a.setRotation(m_physicsServer.getVRTeleportOrientation());
 	btTransform trTotal = tr2*tr2a*trOrg*trLocal;
 
 	m_args[0].m_csGUI->lock();
@@ -2782,4 +2649,36 @@ void	PhysicsServerExample::vrGenericTrackerMoveCallback(int controllerId, float
 	m_args[0].m_csGUI->unlock();
 }
 
-B3_STANDALONE_EXAMPLE(PhysicsServerCreateFunc)
+
+extern int gSharedMemoryKey;
+
+
+class CommonExampleInterface*    PhysicsServerCreateFuncInternal(struct CommonExampleOptions& options)
+{
+
+	MultiThreadedOpenGLGuiHelper* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper(options.m_guiHelper->getAppInterface(),options.m_guiHelper);
+	
+
+  	PhysicsServerExample* example = new PhysicsServerExample(guiHelperWrapper, 
+		options.m_commandProcessorCreation,
+		options.m_sharedMem, 
+		options.m_option);
+
+	if (gSharedMemoryKey>=0)
+	{
+		example->setSharedMemoryKey(gSharedMemoryKey);
+	}
+	if (options.m_option & PHYSICS_SERVER_ENABLE_COMMAND_LOGGING)
+	{
+		example->enableCommandLogging();
+	}
+	if (options.m_option & PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG)
+	{
+		example->replayFromLogFile();
+	}
+	return example;
+
+}
+
+
+

examples/SharedMemory/PhysicsServerExample.h

@@ -8,7 +8,9 @@ enum PhysicsServerOptions
 	PHYSICS_SERVER_USE_RTC_CLOCK = 4,
 };
 
-class CommonExampleInterface*    PhysicsServerCreateFunc(struct CommonExampleOptions& options);
+///Don't use PhysicsServerCreateFuncInternal directly
+///Use PhysicsServerCreateFuncBullet2 instead, or initialize options.m_commandProcessor
+class CommonExampleInterface*    PhysicsServerCreateFuncInternal(struct CommonExampleOptions& options);
 
 #endif //PHYSICS_SERVER_EXAMPLE_H
 

examples/SharedMemory/PhysicsServerExampleBullet2.cpp

@@ -0,0 +1,35 @@
+
+#include "PhysicsServerExampleBullet2.h"
+#include "PhysicsServerExample.h"
+#include "PhysicsServerCommandProcessor.h"
+#include "../CommonInterfaces/CommonExampleInterface.h"
+
+struct Bullet2CommandProcessorCreation : public CommandProcessorCreationInterface
+{
+	virtual class CommandProcessorInterface* createCommandProcessor()
+	{
+		PhysicsServerCommandProcessor* proc = new PhysicsServerCommandProcessor;
+		return proc;
+	}
+
+	virtual void deleteCommandProcessor(CommandProcessorInterface* proc)
+	{
+		delete proc;
+	}
+};
+
+
+static Bullet2CommandProcessorCreation sBullet2CommandCreator;
+
+CommonExampleInterface*    PhysicsServerCreateFuncBullet2(struct CommonExampleOptions& options)
+{
+	options.m_commandProcessorCreation = &sBullet2CommandCreator;
+	
+	CommonExampleInterface* example = PhysicsServerCreateFuncInternal(options);
+	return example;
+
+}
+
+B3_STANDALONE_EXAMPLE(PhysicsServerCreateFuncBullet2)
+
+

examples/SharedMemory/PhysicsServerExampleBullet2.h

@@ -0,0 +1,9 @@
+
+#ifndef PHYSICS_SERVER_EXAMPLE_BULLET_2_H
+#define PHYSICS_SERVER_EXAMPLE_BULLET_2_H
+
+
+
+class CommonExampleInterface*    PhysicsServerCreateFuncBullet2(struct CommonExampleOptions& options);
+
+#endif //PHYSICS_SERVER_EXAMPLE_BULLET_2_H

examples/SharedMemory/PhysicsServerSharedMemory.cpp

@@ -3,7 +3,7 @@
 #include "Win32SharedMemory.h"
 
 #include "../CommonInterfaces/CommonRenderInterface.h"
-
+#include "../CommonInterfaces/CommonExampleInterface.h"
 #include "btBulletDynamicsCommon.h"
 
 #include "LinearMath/btTransform.h"
@@ -30,8 +30,9 @@ struct PhysicsServerSharedMemoryInternalData
 	int m_sharedMemoryKey;
 	bool m_areConnected[MAX_SHARED_MEMORY_BLOCKS];
 	bool m_verboseOutput;
-	PhysicsServerCommandProcessor* m_commandProcessor;
+	CommandProcessorInterface* m_commandProcessor;
-	
+	CommandProcessorCreationInterface* m_commandProcessorCreator;
+
 	PhysicsServerSharedMemoryInternalData()
 		:m_sharedMemory(0),
 		m_ownsSharedMemory(false),
@@ -64,9 +65,10 @@ struct PhysicsServerSharedMemoryInternalData
 };
 
 
-PhysicsServerSharedMemory::PhysicsServerSharedMemory(SharedMemoryInterface* sharedMem)
+PhysicsServerSharedMemory::PhysicsServerSharedMemory(CommandProcessorCreationInterface* commandProcessorCreator, SharedMemoryInterface* sharedMem, int bla)
 {
 	m_data = new PhysicsServerSharedMemoryInternalData();
+	m_data->m_commandProcessorCreator = commandProcessorCreator;
 	if (sharedMem)
 	{
 		m_data->m_sharedMemory = sharedMem;
@@ -81,17 +83,13 @@ PhysicsServerSharedMemory::PhysicsServerSharedMemory(SharedMemoryInterface* shar
 	m_data->m_ownsSharedMemory = true;
 	}
 
-	m_data->m_commandProcessor = new PhysicsServerCommandProcessor;
 
+	m_data->m_commandProcessor = commandProcessorCreator->createCommandProcessor();
 
 }
 
 PhysicsServerSharedMemory::~PhysicsServerSharedMemory()
 {
-
-	m_data->m_commandProcessor->deleteDynamicsWorld();
-	delete m_data->m_commandProcessor;
-
     if (m_data->m_sharedMemory)
     {
         if (m_data->m_verboseOutput)
@@ -105,15 +103,16 @@ PhysicsServerSharedMemory::~PhysicsServerSharedMemory()
         m_data->m_sharedMemory = 0;
     }
 
-    
+	m_data->m_commandProcessorCreator->deleteCommandProcessor(m_data->m_commandProcessor);
     delete m_data;
 }
 
-void PhysicsServerSharedMemory::resetDynamicsWorld()
+/*void PhysicsServerSharedMemory::resetDynamicsWorld()
 {
 	m_data->m_commandProcessor->deleteDynamicsWorld();
 	m_data->m_commandProcessor ->createEmptyDynamicsWorld();
 }
+*/
 void PhysicsServerSharedMemory::setSharedMemoryKey(int key)
 {
 	m_data->m_sharedMemoryKey = key;
@@ -188,7 +187,7 @@ bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface*
 
 void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMemory)
 {
-	m_data->m_commandProcessor->deleteDynamicsWorld();
+	//m_data->m_commandProcessor->deleteDynamicsWorld();
 
 	m_data->m_commandProcessor->setGuiHelper(0);
 
@@ -237,6 +236,11 @@ void PhysicsServerSharedMemory::enableRealTimeSimulation(bool enableRealTimeSim)
 	m_data->m_commandProcessor->enableRealTimeSimulation(enableRealTimeSim);
 }
 
+bool PhysicsServerSharedMemory::isRealTimeSimulationEnabled() const
+{
+	return m_data->m_commandProcessor->isRealTimeSimulationEnabled();
+}
+
 
 
 void PhysicsServerSharedMemory::processClientCommands()
@@ -314,3 +318,24 @@ void PhysicsServerSharedMemory::replayFromLogFile(const char* fileName)
 {
 	m_data->m_commandProcessor->replayFromLogFile(fileName);
 }
+
+const btVector3& PhysicsServerSharedMemory::getVRTeleportPosition() const
+{
+	return m_data->m_commandProcessor->getVRTeleportPosition();
+}
+void PhysicsServerSharedMemory::setVRTeleportPosition(const btVector3& vrTeleportPos)
+{
+	m_data->m_commandProcessor->setVRTeleportPosition(vrTeleportPos);
+}
+
+const btQuaternion& PhysicsServerSharedMemory::getVRTeleportOrientation() const
+{
+	return m_data->m_commandProcessor->getVRTeleportOrientation();
+
+}
+void PhysicsServerSharedMemory::setVRTeleportOrientation(const btQuaternion& vrTeleportOrn)
+{
+	m_data->m_commandProcessor->setVRTeleportOrientation(vrTeleportOrn);
+}
+
+	

examples/SharedMemory/PhysicsServerSharedMemory.h

@@ -2,7 +2,8 @@
 #define PHYSICS_SERVER_SHARED_MEMORY_H
 
 #include "PhysicsServer.h"
- 
+#include "LinearMath/btQuaternion.h"
+
 class PhysicsServerSharedMemory : public PhysicsServer
 {
 	struct PhysicsServerSharedMemoryInternalData* m_data;
@@ -14,7 +15,7 @@ protected:
 	
 	
 public:
-	PhysicsServerSharedMemory(class SharedMemoryInterface* sharedMem=0);
+	PhysicsServerSharedMemory(struct CommandProcessorCreationInterface* commandProcessorCreator, class SharedMemoryInterface* sharedMem, int bla);
 	virtual ~PhysicsServerSharedMemory();
 
 	virtual void setSharedMemoryKey(int key);
@@ -29,6 +30,7 @@ public:
 	virtual void stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrEvents, int numVREvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents);
 
 	virtual void enableRealTimeSimulation(bool enableRealTimeSim);
+	virtual bool isRealTimeSimulationEnabled() const;
 
 	//bool	supportsJointMotor(class btMultiBody* body, int linkIndex);
 
@@ -38,6 +40,14 @@ public:
 	virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);
 	virtual void removePickingConstraint();
 
+	virtual const btVector3& getVRTeleportPosition() const;
+	virtual void setVRTeleportPosition(const btVector3& vrTeleportPos);
+
+	virtual const btQuaternion& getVRTeleportOrientation() const;
+	virtual void setVRTeleportOrientation(const btQuaternion& vrTeleportOrn);
+
+	
+
 	//for physicsDebugDraw and renderScene are mainly for debugging purposes
 	//and for physics visualization. The idea is that physicsDebugDraw can also send wireframe
 	//to a physics client, over shared memory
@@ -48,7 +58,6 @@ public:
 	void enableCommandLogging(bool enable, const char* fileName);
 	void replayFromLogFile(const char* fileName);
 	
-	void resetDynamicsWorld();
 
 };
 

examples/SharedMemory/SharedMemoryCommandProcessor.cpp

@@ -73,7 +73,15 @@ bool SharedMemoryCommandProcessor::connect()
 
 	if (m_data->m_testBlock1) {
 		if (m_data->m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER) {
-			b3Error("Error: please start server before client\n");
+
+			if ((m_data->m_testBlock1->m_magicId < 211705023) && 
+				(m_data->m_testBlock1->m_magicId >=201705023))
+			{
+				b3Error("Error: physics server version mismatch (expected %d got %d)\n",SHARED_MEMORY_MAGIC_NUMBER, m_data->m_testBlock1->m_magicId);
+			} else
+			{
+				b3Error("Error connecting to shared memory: please start server before client\n");
+			}
 			m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey,
 				SHARED_MEMORY_SIZE);
 			m_data->m_testBlock1 = 0;

examples/SharedMemory/SharedMemoryCommands.h

@@ -114,6 +114,9 @@ enum EnumChangeDynamicsInfoFlags
 	CHANGE_DYNAMICS_INFO_SET_MASS=1,
 	CHANGE_DYNAMICS_INFO_SET_COM=2,
 	CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION=4,
+	CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION=8,
+	CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION=16,
+	CHANGE_DYNAMICS_INFO_SET_RESTITUTION=32,
 };
 
 struct ChangeDynamicsInfoArgs
@@ -123,6 +126,9 @@ struct ChangeDynamicsInfoArgs
 	double m_mass;
 	double m_COM[3];
 	double m_lateralFriction;
+	double m_spinningFriction;
+	double m_rollingFriction;
+	double m_restitution;
 };
 
 struct GetDynamicsInfoArgs
@@ -249,6 +255,7 @@ enum EnumUpdateVisualShapeData
 {
 	CMD_UPDATE_VISUAL_SHAPE_TEXTURE=1,
 	CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR=2,
+	CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR=4,
 };
 
 struct UpdateVisualShapeDataArgs
@@ -258,6 +265,7 @@ struct UpdateVisualShapeDataArgs
     int m_shapeIndex;
     int m_textureUniqueId;
 	double m_rgbaColor[4];
+	double m_specularColor[3];
 };
 
 struct LoadTextureArgs
@@ -354,6 +362,8 @@ enum EnumSimParamUpdateFlags
 	SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD = 1024,
 	SIM_PARAM_MAX_CMD_PER_1MS = 2048,
 	SIM_PARAM_ENABLE_FILE_CACHING = 4096,
+	SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD = 8192,
+
 
 };
 
@@ -387,6 +397,7 @@ struct SendPhysicsSimulationParameters
 	double m_defaultContactERP;
 	int m_collisionFilterMode;
 	int m_enableFileCaching;
+	double m_restitutionVelocityThreshold;
 };
 
 struct LoadBunnyArgs

examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp

@@ -4,7 +4,9 @@
 
 #include "PhysicsClientSharedMemory.h"
 #include"../ExampleBrowser/InProcessExampleBrowser.h"
-#include "PhysicsServerExample.h"
+
+#include "PhysicsServerExampleBullet2.h"
+
 #include "../CommonInterfaces/CommonExampleInterface.h"
 #include "InProcessMemory.h"
 
@@ -142,7 +144,7 @@ public:
 		CommonExampleOptions options(guiHelper);
 		options.m_sharedMem = m_sharedMem;
 			
-		m_physicsServerExample = PhysicsServerCreateFunc(options);
+		m_physicsServerExample = PhysicsServerCreateFuncBullet2(options);
 		m_physicsServerExample ->initPhysics();
 		m_physicsServerExample ->resetCamera();
 		setSharedMemoryInterface(m_sharedMem);

examples/SharedMemory/SharedMemoryPublic.h

@@ -4,7 +4,7 @@
 #define SHARED_MEMORY_KEY 12347
 ///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures
 ///my convention is year/month/day/rev
-#define SHARED_MEMORY_MAGIC_NUMBER 201705025
+#define SHARED_MEMORY_MAGIC_NUMBER 201706001
 //#define SHARED_MEMORY_MAGIC_NUMBER 201703024
 
 enum EnumSharedMemoryClientCommand

examples/SharedMemory/TinyRendererVisualShapeConverter.cpp

@@ -179,10 +179,10 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
 	visualShapeOut.m_dimensions[2] = 0;
 	visualShapeOut.m_meshAssetFileName[0] = 0;
 	if (visual->m_geometry.m_hasLocalMaterial) {
-		visualShapeOut.m_rgbaColor[0] = visual->m_geometry.m_localMaterial.m_rgbaColor[0];
+		visualShapeOut.m_rgbaColor[0] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[0];
-		visualShapeOut.m_rgbaColor[1] = visual->m_geometry.m_localMaterial.m_rgbaColor[1];
+		visualShapeOut.m_rgbaColor[1] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[1];
-		visualShapeOut.m_rgbaColor[2] = visual->m_geometry.m_localMaterial.m_rgbaColor[2];
+		visualShapeOut.m_rgbaColor[2] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[2];
-		visualShapeOut.m_rgbaColor[3] = visual->m_geometry.m_localMaterial.m_rgbaColor[3];
+		visualShapeOut.m_rgbaColor[3] = visual->m_geometry.m_localMaterial.m_matColor.m_rgbaColor[3];
 	}
 	
 	GLInstanceGraphicsShape* glmesh = 0;
@@ -555,7 +555,7 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(
 				{
 					for (int i=0; i<4; i++)
 					{
-						rgbaColor[i] = (*matPtr)->m_rgbaColor[i];
+						rgbaColor[i] = (*matPtr)->m_matColor.m_rgbaColor[i];
 					}
 					//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
 					//m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor);
@@ -754,7 +754,12 @@ void TinyRendererVisualShapeConverter::render(const float viewMat[16], const flo
     clearColor.bgra[3] = 255;
     
     clearBuffers(clearColor);
+	float near = projMat[14]/(projMat[10]-1);
+	float far = projMat[14]/(projMat[10]+1);
 
+	m_data->m_camera.setCameraFrustumNear( near);
+	m_data->m_camera.setCameraFrustumFar(far);
+		
     
     ATTRIBUTE_ALIGNED16(btScalar modelMat[16]);
     

examples/SharedMemory/main.cpp

@@ -14,7 +14,8 @@ subject to the following restrictions:
 */
 
 
-#include "PhysicsServerExample.h"
+
+#include "PhysicsServerExampleBullet2.h"
 
 #include "Bullet3Common/b3CommandLineArgs.h"
 
@@ -77,7 +78,7 @@ int main(int argc, char* argv[])
 // options.m_option |= PHYSICS_SERVER_ENABLE_COMMAND_LOGGING;
 // options.m_option |= PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG;
         
-	example = (SharedMemoryCommon*)PhysicsServerCreateFunc(options);
+	example = (SharedMemoryCommon*)PhysicsServerCreateFuncBullet2(options);
 	
 	
 	example->initPhysics();

examples/SharedMemory/premake4.lua

@@ -23,6 +23,7 @@ myfiles =
 	"PhysicsClientSharedMemory.cpp",
 	"PhysicsClientExample.cpp",
 	"PhysicsServerExample.cpp",
+	"PhysicsServerExampleBullet2.cpp",
 	"PhysicsServerSharedMemory.cpp",
 	"PhysicsServerSharedMemory.h",
 	"PhysicsServer.cpp",

examples/TinyRenderer/TinyRenderer.cpp

@@ -38,6 +38,9 @@ struct DepthShader : public IShader {
 	m_lightModelView(lightModelView),
 	m_lightDistance(lightDistance)
     {
+	m_nearPlane = m_projectionMat.col(3)[2]/(m_projectionMat.col(2)[2]-1);
+	m_farPlane = m_projectionMat.col(3)[2]/(m_projectionMat.col(2)[2]+1);
+
         m_invModelMat = m_modelMat.invert_transpose();
     }
     virtual Vec4f vertex(int iface, int nthvert) {
@@ -91,7 +94,8 @@ struct Shader : public IShader {
     mat<4,3,float> varying_tri_light_view;
     mat<3,3,float> varying_nrm; // normal per vertex to be interpolated by FS
 	mat<4,3,float> world_tri; // model triangle coordinates in the world space used for backface culling, written by VS
-    
+   
+
     Shader(Model* model, Vec3f light_dir_local, Vec3f light_color, Matrix& modelView, Matrix& lightModelView, Matrix& projectionMat, Matrix& modelMat, Matrix& viewportMat, Vec3f localScaling, const Vec4f& colorRGBA, int width, int height, b3AlignedObjectArray<float>* shadowBuffer, float ambient_coefficient=0.6, float diffuse_coefficient=0.35, float specular_coefficient=0.05)
     :m_model(model),
     m_light_dir_local(light_dir_local),
@@ -112,6 +116,9 @@ struct Shader : public IShader {
     m_height(height)
    
     {
+		m_nearPlane = m_projectionMat.col(3)[2]/(m_projectionMat.col(2)[2]-1);
+		m_farPlane = m_projectionMat.col(3)[2]/(m_projectionMat.col(2)[2]+1);
+		//printf("near=%f, far=%f\n", m_nearPlane, m_farPlane);
         m_invModelMat = m_modelMat.invert_transpose();
 		m_projectionModelViewMat = m_projectionMat*m_modelView1;
 		m_projectionLightViewMat = m_projectionMat*m_lightModelView;
@@ -576,7 +583,6 @@ void TinyRenderer::renderObjectDepth(TinyRenderObjectData& renderData)
         Vec3f localScaling(renderData.m_localScaling[0],renderData.m_localScaling[1],renderData.m_localScaling[2]);
         
         DepthShader shader(model, lightModelViewMatrix, lightViewProjectionMatrix,renderData.m_modelMatrix, localScaling, light_distance);
-        
         for (int i=0; i<model->nfaces(); i++)
         {
             for (int j=0; j<3; j++) {

examples/TinyRenderer/our_gl.cpp

@@ -180,6 +180,11 @@ void triangle(mat<4,3,float> &clipc, IShader &shader, TGAImage &image, float *zb
 				zbuffer[P.x+P.y*image.get_width()]>frag_depth) 
 				continue;
             bool discard = shader.fragment(bc_clip, color);
+            if (frag_depth<-shader.m_farPlane)
+                discard=true;
+            if (frag_depth>-shader.m_nearPlane)
+                discard=true;
+
             if (!discard) {
                 zbuffer[P.x+P.y*image.get_width()] = frag_depth;
                 if (segmentationMaskBuffer)

examples/TinyRenderer/our_gl.h

@@ -11,6 +11,8 @@ Matrix projection(float coeff=0.f); // coeff = -1/c
 Matrix lookat(Vec3f eye, Vec3f center, Vec3f up);
 
 struct IShader {
+	 float m_nearPlane;
+	float m_farPlane;
     virtual ~IShader();
     virtual Vec4f vertex(int iface, int nthvert) = 0;
     virtual bool fragment(Vec3f bar, TGAColor &color) = 0;

examples/Utils/ChromeTraceUtil.cpp

@@ -184,7 +184,7 @@ void MyEnterProfileZoneFunc(const char* msg)
 		return;
 #ifndef BT_NO_PROFILE
 	int threadId = btQuickprofGetCurrentThreadIndex2();
-	if (threadId<0)
+	if (threadId<0 || threadId >= BT_QUICKPROF_MAX_THREAD_COUNT)
 		return;
 
 	if (gStackDepths[threadId] >= MAX_NESTING)
@@ -208,8 +208,8 @@ void MyLeaveProfileZoneFunc()
 		return;
 #ifndef BT_NO_PROFILE
 	int threadId = btQuickprofGetCurrentThreadIndex2();
-	if (threadId<0)
+    if (threadId<0 || threadId >= BT_QUICKPROF_MAX_THREAD_COUNT)
-		return;
+        return;
 
 	if (gStackDepths[threadId] <= 0)
 	{

examples/Utils/b3Clock.cpp

@@ -46,7 +46,6 @@ struct b3ClockData
 
 #ifdef B3_USE_WINDOWS_TIMERS
 	LARGE_INTEGER mClockFrequency;
-	DWORD mStartTick;
 	LARGE_INTEGER mStartTime;
 #else
 #ifdef __CELLOS_LV2__
@@ -88,11 +87,24 @@ b3Clock& b3Clock::operator=(const b3Clock& other)
 
 
 	/// Resets the initial reference time.
-void b3Clock::reset()
+void b3Clock::reset(bool zeroReference)
 {
+	if (zeroReference)
+	{
+#ifdef B3_USE_WINDOWS_TIMERS
+		m_data->mStartTime.QuadPart = 0;
+#else
+	#ifdef __CELLOS_LV2__
+			m_data->mStartTime = 0;
+	#else
+			m_data->mStartTime = (struct timeval){0};
+	#endif
+#endif
+
+	} else
+	{
 #ifdef B3_USE_WINDOWS_TIMERS
 	QueryPerformanceCounter(&m_data->mStartTime);
-	m_data->mStartTick = GetTickCount();
 #else
 #ifdef __CELLOS_LV2__
 
@@ -105,6 +117,7 @@ void b3Clock::reset()
 	gettimeofday(&m_data->mStartTime, 0);
 #endif
 #endif
+	}
 }
 
 /// Returns the time in ms since the last call to reset or since 

examples/Utils/b3Clock.h

@@ -13,8 +13,8 @@ public:
 
 	~b3Clock();
 
-	/// Resets the initial reference time.
+	/// Resets the initial reference time. If zeroReference is true, will set reference to absolute 0.
-	void reset();
+	void reset(bool zeroReference=false);
 
 	/// Returns the time in ms since the last call to reset or since 
 	/// the b3Clock was created.

examples/Utils/b3ResourcePath.cpp

@@ -53,11 +53,31 @@ int b3ResourcePath::getExePath(char* path, int maxPathLenInBytes)
 	return numBytes;
 }
 
-int b3ResourcePath::findResourcePath(const char* resourceName, char* resourcePath, int resourcePathMaxNumBytes)
+struct TempResourcePath
+{
+	char* m_path;
+	TempResourcePath(int len)
+	{
+		m_path = (char*)malloc(len);
+		memset(m_path,0,len);
+	}
+	virtual ~TempResourcePath()
+	{
+		free(m_path);
+	}
+};
+
+int b3ResourcePath::findResourcePath(const char* resourceName, char* resourcePathOut, int resourcePathMaxNumBytes)
 {
 	//first find in a resource/<exeName> location, then in various folders within 'data' using b3FileUtils
 	char exePath[B3_MAX_EXE_PATH_LEN];
 
+	bool res = b3FileUtils::findFile(resourceName, resourcePathOut, resourcePathMaxNumBytes);
+	if (res)
+    {
+            return strlen(resourcePathOut);
+    }
+
 	int l = b3ResourcePath::getExePath(exePath, B3_MAX_EXE_PATH_LEN);
 	if (l)
 	{
@@ -66,33 +86,31 @@ int b3ResourcePath::findResourcePath(const char* resourceName, char* resourcePat
         	int exeNamePos = b3FileUtils::extractPath(exePath,pathToExe,B3_MAX_EXE_PATH_LEN);
         	if (exeNamePos)
         	{
-				sprintf(resourcePath,"%s../data/%s",pathToExe,resourceName);
+				TempResourcePath tmpPath(resourcePathMaxNumBytes+1024);
+				char* resourcePathIn = tmpPath.m_path;
+				sprintf(resourcePathIn,"%s../data/%s",pathToExe,resourceName);
 				//printf("try resource at %s\n", resourcePath);	
-				if (b3FileUtils::findFile(resourcePath, resourcePath, resourcePathMaxNumBytes))
+				if (b3FileUtils::findFile(resourcePathIn, resourcePathOut, resourcePathMaxNumBytes))
 				{
-					return strlen(resourcePath);
+					return strlen(resourcePathOut);
 				}
 
-				sprintf(resourcePath,"%s../resources/%s/%s",pathToExe,&exePath[exeNamePos],resourceName);
+				sprintf(resourcePathIn,"%s../resources/%s/%s",pathToExe,&exePath[exeNamePos],resourceName);
 				//printf("try resource at %s\n", resourcePath);	
-				if (b3FileUtils::findFile(resourcePath, resourcePath, resourcePathMaxNumBytes))
+				if (b3FileUtils::findFile(resourcePathIn, resourcePathOut, resourcePathMaxNumBytes))
 				{
-					return strlen(resourcePath);
+					return strlen(resourcePathOut);
 				}
-         sprintf(resourcePath,"%s.runfiles/google3/third_party/bullet/data/%s",exePath,resourceName);
+				sprintf(resourcePathIn,"%s.runfiles/google3/third_party/bullet/data/%s",exePath,resourceName);
 				//printf("try resource at %s\n", resourcePath);	
-				if (b3FileUtils::findFile(resourcePath, resourcePath, resourcePathMaxNumBytes))
+				if (b3FileUtils::findFile(resourcePathIn, resourcePathOut, resourcePathMaxNumBytes))
 				{
-					return strlen(resourcePath);
+					return strlen(resourcePathOut);
 				}  
         	}
 	}
 
-	bool res = b3FileUtils::findFile(resourceName, resourcePath, resourcePathMaxNumBytes);
+
-	if (res)
-        {
-                return strlen(resourcePath);
-        }
 
 	return 0;
 }

examples/pybullet/CMakeLists.txt

@@ -33,6 +33,7 @@ SET(pybullet_SRCS
 		../../examples/SharedMemory/PhysicsServer.cpp
 		../../examples/SharedMemory/PhysicsServer.h
 		../../examples/SharedMemory/PhysicsServerExample.cpp
+		../../examples/SharedMemory/PhysicsServerExampleBullet2.cpp
 		../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
 		../../examples/SharedMemory/PhysicsServerSharedMemory.cpp
 		../../examples/SharedMemory/PhysicsServerSharedMemory.h