Merge branch 'master' into bullet-gym

data/plane_transparent.mtl

@@ -0,0 +1,14 @@
+newmtl Material
+  Ns 10.0000
+  Ni 1.5000
+  d 1.0000
+  Tr 0.0000
+  Tf 1.0000 1.0000 1.0000 
+  illum 2
+  Ka 0.0000 0.0000 0.0000
+  Kd 0.5880 0.5880 0.5880
+  Ks 0.0000 0.0000 0.0000
+  Ke 0.0000 0.0000 0.0000
+  map_Kd tex4x4.png
+  
+  

data/plane_transparent.obj

@@ -0,0 +1,18 @@
+# Blender v2.66 (sub 1) OBJ File: ''
+# www.blender.org
+mtllib plane_transparent.mtl
+o Plane
+v 15.000000 -15.000000 0.000000
+v 15.000000 15.000000 0.000000
+v -15.000000 15.000000 0.000000
+v -15.000000  -15.000000 0.000000
+
+vt 1.000000 0.000000
+vt 1.000000 1.000000
+vt 0.000000 1.000000
+vt 0.000000 0.000000
+
+usemtl Material
+s off
+f 1/1 2/2 3/3
+f 1/1 3/3 4/4

data/plane_transparent.urdf

@@ -0,0 +1,29 @@
+<?xml version="0.0" ?>
+<robot name="plane">
+  <link name="planeLink">
+  <contact>
+      <lateral_friction value="1"/>
+  </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_transparent.obj" scale="1 1 1"/>
+      </geometry>
+       <material name="white">
+        <color rgba="1 1 1 0.4"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -5"/>
+      <geometry>
+	 	<box size="30 30 10"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
+

data/racecar/racecar_differential.urdf

@@ -20,12 +20,12 @@
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="meshes/chassis_differential.STL"/>
+        <mesh scale="1 1 1 1" filename="meshes/chassis_differential.STL"/>
       </geometry>
       <material name="blue"/>
     </collision>
     <inertial>
-      <origin rpy="0 0 0" xyz="0.1477 0 0"/>
+      <origin rpy="0 0 0" xyz="0.2 0 0."/>
       <mass value="4.0"/>
       <inertia ixx="0.010609" ixy="0" ixz="0" iyy="0.050409" iyz="0" izz="0.05865"/>
     </inertial>
@@ -33,14 +33,14 @@
   <!-- Add the left rear wheel with its joints and tranmissions -->
   <link name="left_rear_wheel">
     <contact>
-      <lateral_friction value="1.0"/>
+      <lateral_friction value=".5"/>
       <rolling_friction value="0.0"/>
       <stiffness value="30000"/>
       <damping value="1000"/>
     </contact>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 -0.0225"/>
-      <mass value="0.034055"/>
+      <mass value="0.34055"/>
       <inertia ixx="0.00026046" ixy="0" ixz="0" iyy="0.00026046" iyz="0" izz="0.00041226"/>
     </inertial>
     <visual>
@@ -66,14 +66,14 @@
   <!-- Add the right rear wheel with its joints and tranmissions -->
   <link name="right_rear_wheel">
 <contact>
-      <lateral_friction value="1.0"/>
+      <lateral_friction value=".5"/>
       <rolling_friction value="0.0"/>
       <stiffness value="30000"/>
       <damping value="1000"/>
     </contact>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0.0225"/>
-      <mass value="0.034055"/>
+      <mass value="0.34055"/>
       <inertia ixx="0.00026046" ixy="0" ixz="0" iyy="0.00026046" iyz="0" izz="0.00041226"/>
     </inertial>
     <visual>
@@ -163,14 +163,14 @@
   <!-- Add the left front wheel with its joints and tranmissions -->
   <link name="left_front_wheel">
 <contact>
-      <lateral_friction value="1.0"/>
+      <lateral_friction value=".8"/>
       <rolling_friction value="0.0"/>
       <stiffness value="30000"/>
       <damping value="1000"/>
     </contact>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 -0.0225"/>
-      <mass value="0.034055"/>
+      <mass value="0.34055"/>
       <inertia ixx="0.00026046" ixy="0" ixz="0" iyy="0.00026046" iyz="0" izz="0.00041226"/>
     </inertial>
     <visual>
@@ -207,14 +207,14 @@
   <!-- Add the left front wheel with its joints and tranmissions -->
   <link name="right_front_wheel">
 <contact>
-      <lateral_friction value="1.0"/>
+      <lateral_friction value="0.8"/>
       <rolling_friction value="0.0"/>
       <stiffness value="30000"/>
       <damping value="1000"/>
     </contact>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0.0225"/>
-      <mass value="0.034055"/>
+      <mass value="0.34055"/>
       <inertia ixx="0.00026046" ixy="0" ixz="0" iyy="0.00026046" iyz="0" izz="0.00041226"/>
     </inertial>
     <visual>
@@ -533,7 +533,7 @@
     </contact>
     
     <inertial>
-      <mass value="0.5637"/>
+      <mass value="0.0637"/>
       <origin xyz="0 0 0"/>
       <inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
     </inertial>
@@ -573,7 +573,7 @@
     </contact>
     
     <inertial>
-      <mass value="0.556"/>
+      <mass value="0.056"/>
       <origin xyz="0 0 0"/>
       <inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
     </inertial>
@@ -612,7 +612,7 @@
     </contact>
     
     <inertial>
-      <mass value="0.556"/>
+      <mass value="0.056"/>
       <origin xyz="0 0 0"/>
       <inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
     </inertial>
@@ -650,7 +650,7 @@
     </contact>
     
     <inertial>
-      <mass value="0.556"/>
+      <mass value="0.056"/>
       <origin xyz="0 0 0"/>
       <inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
     </inertial>
@@ -686,7 +686,7 @@
     </contact>
     
     <inertial>
-      <mass value="0.556"/>
+      <mass value="0.056"/>
       <origin xyz="0 0 0"/>
       <inertia ixx="0.02467" ixy="0" ixz="0" iyy="0.02467" iyz="0" izz="0.04411"/>
     </inertial>

data/sphere_transparent.urdf

@@ -0,0 +1,32 @@
+<?xml version="0.0" ?>
+<robot name="urdf_robot">
+  <link name="baseLink">
+  	<contact>
+      <rolling_friction value="0.03"/>
+      <spinning_friction value="0.03"/>
+    </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="red_transparent">
+        <color rgba="1 0 0 0.5"/>
+        <specular rgb="11 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+	 <sphere radius="0.5"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
+

data/stone.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/stone.obj

@@ -0,0 +1,32 @@
+# Blender v2.78 (sub 0) OBJ File: ''
+# www.blender.org
+mtllib stone.mtl
+o Cube
+v -0.246350 -0.246483 -0.000624
+v -0.151407 -0.176325 0.172867
+v -0.246350 0.249205 -0.000624
+v -0.151407 0.129477 0.172867
+v 0.249338 -0.246483 -0.000624
+v 0.154395 -0.176325 0.172867
+v 0.249338 0.249205 -0.000624
+v 0.154395 0.129477 0.172867
+vn -0.8772 0.0000 0.4801
+vn 0.0000 0.8230 0.5680
+vn 0.8772 0.0000 0.4801
+vn 0.0000 -0.9271 0.3749
+vn 0.0000 0.0000 -1.0000
+vn 0.0000 0.0000 1.0000
+usemtl None
+s off
+f 1//1 4//1 3//1
+f 4//2 7//2 3//2
+f 8//3 5//3 7//3
+f 6//4 1//4 5//4
+f 7//5 1//5 3//5
+f 4//6 6//6 8//6
+f 1//1 2//1 4//1
+f 4//2 8//2 7//2
+f 8//3 6//3 5//3
+f 6//4 2//4 1//4
+f 7//5 5//5 1//5
+f 4//6 2//6 6//6

data/widowx/bsd_license.txt

@@ -0,0 +1,22 @@
+Converted from https://github.com/RobotnikAutomation/widowx_arm/blob/master/widowx_arm_description/package.xml
+
+Copyright 2017 robotnik
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
+3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+<package>
+  <name>widowx_arm_description</name>
+  <version>0.0.2</version>
+  <description>The widowx_arm_description package</description>
+
+	<maintainer email="jmrapado@robotnik.es">Jose Rapado Garc<72>a</maintainer>
+	<maintainer email="rnavarro@robotnik.es">Rom<6F>n Navarro</maintainer>
+	<maintainer email="cvillar@robotnik.es">Carlos Villar</maintainer>
+
+ 

data/widowx/widowx.urdf

@@ -0,0 +1,279 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<robot name="widowx" xmlns:xacro="http://ros.org/wiki/xacro">
+ 
+	<material name="yellow">
+		<color rgba="0.15 0.15 0.15 1.0"/>
+	</material>
+
+	<material name="white">
+		<color rgba="0.86 0.85 0.81 1.0"/>
+	</material>
+
+	<material name="black">
+		<color rgba="0.15 0.15 0.15 1.0"/>
+	</material>
+
+	<material name="grey">
+		<color rgba="0.34 0.35 0.36 1.0"/>
+	</material>
+
+
+	<material name="greyish">
+		<color rgba="0.75 0.75 0.75 1.0"/>
+	</material>
+	
+	
+  
+  <link name="arm_base_link">
+     <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+           <mesh filename="meshes/base_link.stl"/>
+        </geometry>
+        <material name="black"/>
+   </visual>
+     <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+           <mesh filename="meshes/base_link.stl"/>
+        </geometry>
+     </collision>
+     <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <mass value="13" />
+        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
+     </inertial>
+  </link>
+  <link name="shoulder_link">
+     <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+           <mesh filename="meshes/shoulder_link.stl" />
+        </geometry>
+        <material name="greyish"/>
+     </visual>
+     <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+           <mesh filename="meshes/shoulder_link.stl"/>
+        </geometry>
+     </collision>
+     <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <mass value=".1" />
+        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
+     </inertial>
+  </link>
+  <link name="biceps_link">
+     <visual>
+	      <origin xyz="0.04825 0 0.140" rpy="0 0 0" />
+        <!--origin xyz="0 0 0" rpy="0 0 0" /-->
+        <geometry>
+           <mesh filename="meshes/biceps_link.stl" />
+        </geometry>
+        <material name="black"/>
+     </visual>
+     <collision>
+        <origin xyz="0.04825 0 0.140" rpy="0 0 0" />
+        <geometry>
+           <mesh filename="meshes/biceps_link.stl" />
+        </geometry>
+     </collision>
+     <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <mass value=".1" />
+        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
+     </inertial>
+  </link>
+  <link name="forearm_link">
+     <visual>
+	      <origin xyz="0 0 0.14203" rpy="0 0 0" />
+	<!--origin xyz="0 0 0" rpy="0 0 0" /-->
+        <geometry>
+           <mesh filename="meshes/forearm_link.stl"/>
+        </geometry>
+       <material name="greyish"/>
+     </visual>
+     <collision>
+        <origin xyz="0 0 0.14203" rpy="0 0 0" />
+        <geometry>
+           <mesh filename="meshes/forearm_link.stl"/>
+        </geometry>
+     </collision>
+     <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <mass value=".1" />
+        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+     </inertial>
+  </link>
+  <link name="wrist_1_link">
+     <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+           <mesh filename="meshes/wrist_1_link.stl"/>
+        </geometry>
+        <material name="greyish"/>
+     </visual>
+     <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+           <mesh filename="meshes/wrist_1_link.stl"/>
+        </geometry>
+     </collision>
+     <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <mass value=".1" />
+        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
+     </inertial>
+  </link>
+  <link name="wrist_2_link">
+     <visual>
+        <origin xyz="0 0 0.043" rpy="0 0 0" />
+        <geometry>
+           <mesh filename="meshes/wrist_2_link.stl"/>
+        </geometry>
+        <material name="black"/>
+     </visual>
+     <collision>
+        <origin xyz="0 0 0.043" rpy="0 0 0" />
+        <geometry>
+           <mesh filename="meshes/wrist_2_link.stl"/>
+        </geometry>
+     </collision>
+     <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <mass value=".1" />
+        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
+     </inertial>
+  </link>
+  <link name="gripper_rail_link">
+     <visual>
+        <origin xyz="0 0 0" rpy="0 0 1.57" />
+        <geometry>
+           <mesh filename="meshes/gripper_rail_link.stl"/>
+        </geometry>
+        <material name="greyish"/>
+     </visual>
+     <collision>
+        <origin xyz="0 0 0" rpy="0 0 1.57" />
+        <geometry>
+           <mesh filename="meshes/gripper_rail_link.stl"/>
+        </geometry>
+     </collision>
+     <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <mass value=".1" />
+        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
+     </inertial>
+  </link>
+  <link name="gripper_aux_link">
+  </link>
+  <link name="gripper_1_link">
+     <visual>
+        <origin xyz="0 -0.0007 0" rpy="0 0 1.57" />
+        <geometry>
+           <mesh filename="meshes/gripper_hand_fixed_link.stl"/>
+        </geometry>
+        <material name="black"/>
+     </visual>
+     <collision>
+        <origin xyz="0 -0.0007 0" rpy="0 0 1.57" />
+        <geometry>
+           <mesh filename="meshes/gripper_hand_fixed_link.stl"/>
+        </geometry>
+     </collision>
+     <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <mass value=".1" />
+        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
+     </inertial>
+  </link>
+  <link name="gripper_2_link">
+     <visual>
+        <origin xyz="0 0.0007 0" rpy="0 0 -1.57" />
+        <geometry>
+           <mesh filename="meshes/gripper_hand_fixed_link.stl"/>
+        </geometry>
+        <material name="greyish"/>
+     </visual>
+     <collision>
+         <origin xyz="0 0.0007 0" rpy="0 0 -1.57" />
+        <geometry>
+           <mesh filename="meshes/gripper_hand_fixed_link.stl"/>
+        </geometry>
+     </collision>
+     <inertial>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <mass value=".1" />
+        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
+     </inertial>
+  </link>
+  <!-- joints -->
+ 
+  <joint name="joint_1" type="revolute">
+     <origin xyz="0 0 0.125" rpy="0 0 0" />
+     <parent link="arm_base_link" />
+     <child link="shoulder_link" />
+     <axis xyz="0 0 1" />
+     <limit lower="-2.617" upper="2.617" effort="0" velocity="0.785" />
+  </joint>
+  <joint name="joint_2" type="revolute">
+     <!--origin xyz="0.04825 0 0.14203" rpy="0 0 0" /-->
+     <origin xyz="0 0 0" rpy="0 0 0" />
+     <parent link="shoulder_link" />
+     <child link="biceps_link" />
+     <axis xyz="0 1 0" />
+     <limit lower="-1.571" upper="1.571" effort="0" velocity="1.571" />
+  </joint>
+  <joint name="joint_3" type="revolute">
+     <origin xyz="0.04825 0 0.14203" rpy="0 1.5707963268 0" />
+     <parent link="biceps_link" />
+     <child link="forearm_link" />
+     <axis xyz="0 1 0" />
+     <limit lower="-1.571" upper="1.571" effort="0" velocity="1.571" />
+  </joint>
+  <joint name="joint_4" type="revolute">
+     <origin xyz="0 0 0.14203" rpy="0 0 0" />
+     <parent link="forearm_link" />
+     <child link="wrist_1_link" />
+     <axis xyz="0 1 0" />
+     <limit lower="-1.745" upper="1.745" effort="0" velocity="1.571" />
+  </joint>
+  <joint name="joint_5" type="revolute">
+     <origin xyz="0 0 0.0715" rpy="0 0 0" />
+     <parent link="wrist_1_link" />
+     <child link="wrist_2_link" />
+     <axis xyz="0 0 1" />
+     <limit lower="-2.617" upper="2.617" effort="0" velocity="1.571" />
+  </joint>
+  <joint name="joint_6" type="fixed">
+     <origin xyz = "0 0 0.043" rpy="0 0 0" />
+     <parent link="wrist_2_link" />
+     <child link="gripper_rail_link" />
+     <axis xyz="0 0 1" />
+     <limit lower="-0.31" upper="0.1" effort="0" velocity="0.5" />
+  </joint>
+  <joint name="gripper_revolute_joint" type="revolute">
+     <origin xyz = "0 0 0" rpy="0 0 0" />
+     <parent link="gripper_rail_link" />
+     <child link="gripper_aux_link" />
+     <axis xyz="0 0 1" />
+     <limit lower="0" upper="2.6" effort="0" velocity="0.5" />
+  </joint>
+  <joint name="gripper_prismatic_joint_1" type="prismatic">
+     <origin xyz="0 0 0" rpy="0 0 0" />
+     <parent link="gripper_rail_link" />
+     <child link="gripper_1_link" />
+     <axis xyz="0 -1 0" />
+     <limit lower="0" upper="0.027" effort="0" velocity="0.5" />
+  </joint>
+  <joint name="gripper_prismatic_joint_2" type="prismatic">
+     <origin xyz="0 0 0" rpy="0 0 0" />
+     <parent link="gripper_rail_link" />
+     <child link="gripper_2_link" />
+     <mimic joint="gripper_prismatic_joint_1" />
+     <axis xyz="0 1 0" />
+     <limit lower="0" upper="0.027" effort="0" velocity="0.5" />
+  </joint>
+
+</robot>

examples/Collision/CollisionTutorialBullet2.cpp

@@ -79,7 +79,6 @@ public:
 		
 		gTotalPoints = 0;
 		m_app->setUpAxis(1);
-		m_app->m_renderer->enableBlend(true);
 		
 		switch (m_tutorialIndex)
 		{
@@ -250,7 +249,6 @@ public:
 
 		m_timeSeriesCanvas0 = 0;
 
-		m_app->m_renderer->enableBlend(false);
     }
     
     

examples/CommonInterfaces/CommonGUIHelperInterface.h

@@ -40,6 +40,10 @@ struct GUIHelperInterface
 	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 changeTexture(int textureUniqueId, const unsigned char* rgbTexels, int width, int height){}
+
+	virtual int getShapeIndexFromInstance(int instanceUid){return -1;}	
+	virtual void replaceTexture(int shapeIndex, int textureUid){}
 	
 	
 	virtual Common2dCanvasInterface* get2dCanvasInterface()=0;

examples/CommonInterfaces/CommonRenderInterface.h

@@ -70,6 +70,9 @@ struct CommonRenderInterface
     virtual int registerTexture(const unsigned char* texels, int width, int height, bool flipPixelsY=true)=0;
     virtual void updateTexture(int textureIndex, const unsigned char* texels, bool flipPixelsY=true)=0;
     virtual void activateTexture(int textureIndex)=0;
+	virtual void replaceTexture(int shapeIndex, int textureIndex){};
+
+	virtual int getShapeIndexFromInstance(int srcIndex) {return -1;}
 
 	virtual bool readSingleInstanceTransformToCPU(float* position, float* orientation, int srcIndex)=0;
 
@@ -86,7 +89,7 @@ struct CommonRenderInterface
     virtual int getTotalNumInstances() const = 0;
     
 	virtual void writeTransforms()=0;
-    virtual void enableBlend(bool blend)=0;
+    
 	virtual void clearZBuffer()=0;
 
 	//This is internal access to OpenGL3+ features, mainly used for OpenCL-OpenGL interop

examples/ExampleBrowser/InProcessExampleBrowser.cpp

@@ -230,6 +230,7 @@ static double gMinUpdateTimeMicroSecs = 4000.;
 
 void	ExampleBrowserThreadFunc(void* userPtr,void* lsMemory)
 {
+
 	printf("ExampleBrowserThreadFunc started\n");
 
 	ExampleBrowserThreadLocalStorage* localStorage = (ExampleBrowserThreadLocalStorage*) lsMemory;
@@ -257,7 +258,9 @@ void	ExampleBrowserThreadFunc(void* userPtr,void* lsMemory)
 
 		do
 		{
-			B3_PROFILE("ExampleBrowserThreadFunc");
+			clock.usleep(0);
+
+			//B3_PROFILE("ExampleBrowserThreadFunc");
 			float deltaTimeInSeconds = clock.getTimeMicroseconds()/1000000.f;
 			{
 				if (deltaTimeInSeconds > 0.1)
@@ -266,13 +269,13 @@ void	ExampleBrowserThreadFunc(void* userPtr,void* lsMemory)
 				}
 				if (deltaTimeInSeconds < (gMinUpdateTimeMicroSecs/1e6))
 				{
-					B3_PROFILE("clock.usleep");
+					//B3_PROFILE("clock.usleep");
-					clock.usleep(gMinUpdateTimeMicroSecs/10.);
 					exampleBrowser->updateGraphics();
 				} else
 				{
-					B3_PROFILE("exampleBrowser->update");
+					//B3_PROFILE("exampleBrowser->update");
 					clock.reset();
+					exampleBrowser->updateGraphics();
 					exampleBrowser->update(deltaTimeInSeconds);
 				}
 			}
@@ -429,6 +432,7 @@ void btUpdateInProcessExampleBrowserMainThread(btInProcessExampleBrowserMainThre
 {
     float deltaTimeInSeconds = data->m_clock.getTimeMicroseconds()/1000000.f;
     data->m_clock.reset();
+    data->m_exampleBrowser->updateGraphics();
     data->m_exampleBrowser->update(deltaTimeInSeconds);
 }
 void btShutDownExampleBrowserMainThread(btInProcessExampleBrowserMainThreadInternalData* data)

examples/ExampleBrowser/OpenGLExampleBrowser.cpp

@@ -127,6 +127,8 @@ extern bool useShadowMap;
 bool visualWireframe=false;
 static bool renderVisualGeometry=true;
 static bool renderGrid = true;
+static bool gEnableRenderLoop = true;
+
 bool renderGui = true;
 static bool enable_experimental_opencl = false;
 
@@ -370,6 +372,10 @@ void OpenGLExampleBrowser::registerFileImporter(const char* extension, CommonExa
 
 void OpenGLExampleBrowserVisualizerFlagCallback(int flag, bool enable)
 {
+	if (flag == COV_ENABLE_RENDERING)
+	{
+		gEnableRenderLoop = (enable!=0);
+	}
     if (flag == COV_ENABLE_SHADOWS)
     {
         useShadowMap = enable;
@@ -1185,7 +1191,7 @@ void OpenGLExampleBrowser::updateGraphics()
 	{
 			if (!pauseSimulation || singleStepSimulation)
 			{
-				B3_PROFILE("sCurrentDemo->updateGraphics");
+				//B3_PROFILE("sCurrentDemo->updateGraphics");
 				sCurrentDemo->updateGraphics();
 			}
 	}
@@ -1193,6 +1199,9 @@ void OpenGLExampleBrowser::updateGraphics()
 
 void OpenGLExampleBrowser::update(float deltaTime)
 {
+	if (!gEnableRenderLoop)
+		return;
+
 	b3ChromeUtilsEnableProfiling();
 	
 		B3_PROFILE("OpenGLExampleBrowser::update");

examples/ExampleBrowser/OpenGLGuiHelper.cpp

@@ -292,6 +292,12 @@ int	OpenGLGuiHelper::registerTexture(const unsigned char* texels, int width, int
 	return textureId;
 }
 
+void OpenGLGuiHelper::changeTexture(int textureUniqueId, const unsigned char* rgbTexels, int width, int height)
+{
+	bool flipPixelsY = true;
+	m_data->m_glApp->m_renderer->updateTexture(textureUniqueId, rgbTexels,flipPixelsY);
+}
+
 
 int OpenGLGuiHelper::registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId)
 {
@@ -318,6 +324,18 @@ void OpenGLGuiHelper::removeGraphicsInstance(int graphicsUid)
 	};
 }
 
+int OpenGLGuiHelper::getShapeIndexFromInstance(int instanceUid)
+{
+	return m_data->m_glApp->m_renderer->getShapeIndexFromInstance(instanceUid);
+}
+
+void OpenGLGuiHelper::replaceTexture(int shapeIndex, int textureUid)
+{
+	if (shapeIndex>=0)
+	{
+		m_data->m_glApp->m_renderer->replaceTexture(shapeIndex, textureUid);
+	};
+}
 void OpenGLGuiHelper::changeRGBAColor(int instanceUid, const double rgbaColor[4])
 {
 	if (instanceUid>=0)

examples/ExampleBrowser/OpenGLGuiHelper.h

@@ -28,6 +28,10 @@ struct OpenGLGuiHelper : public GUIHelperInterface
 	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 changeTexture(int textureUniqueId, const unsigned char* rgbTexels, int width, int height);
+
+	virtual int getShapeIndexFromInstance(int instanceUid);	
+	virtual void replaceTexture(int shapeIndex, int textureUid);
 
 	virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape);
 

examples/Importers/ImportURDFDemo/UrdfParser.h

@@ -156,7 +156,8 @@ struct UrdfLink
 
 	UrdfLink()
 		:m_parentLink(0),
-		m_parentJoint(0)
+		m_parentJoint(0),
+		m_linkIndex(-2)
 	{
 	}
 	

examples/InverseKinematics/InverseKinematicsExample.cpp

@@ -198,7 +198,6 @@ public:
     }
     virtual ~InverseKinematicsExample()
     {
-        m_app->m_renderer->enableBlend(false);
     }
 
     

examples/MultiThreading/MultiThreadingExample.cpp

@@ -176,14 +176,12 @@ public:
 		//int numBodies = 1;
 		
 		m_app->setUpAxis(1);
-		m_app->m_renderer->enableBlend(true);
 		
     }
     virtual ~MultiThreadingExample()
     {
 
 
-		m_app->m_renderer->enableBlend(false);
     }
     
    

examples/OpenGLWindow/GLInstancingRenderer.cpp

@@ -115,15 +115,21 @@ static InternalDataRenderer* sData2;
 
 GLint lineWidthRange[2]={1,1};
 
+enum
+{
+	eGfxTransparency=1,
+	eGfxHasTexture = 2,
+};
+
 struct b3GraphicsInstance
 {
 	GLuint	m_cube_vao;
 	GLuint	m_index_vbo;
-	GLuint				m_texturehandle;
+	GLuint	m_textureIndex;
-
 	int m_numIndices;
 	int m_numVertices;
 
+	
 	int m_numGraphicsInstances;
 	b3AlignedObjectArray<int> m_tempObjectUids;
 	int m_instanceOffset;
@@ -131,11 +137,12 @@ struct b3GraphicsInstance
 	int	m_primitiveType;
 	float m_materialShinyNess;
 	b3Vector3 m_materialSpecularColor;
+	int m_flags;//transparency etc
 
 	b3GraphicsInstance()
 	:m_cube_vao(-1),
 		m_index_vbo(-1),
-		m_texturehandle(0),
+		m_textureIndex(-1),
 		m_numIndices(-1),
 		m_numVertices(-1),
 		m_numGraphicsInstances(0),
@@ -143,7 +150,8 @@ struct b3GraphicsInstance
 		m_vertexArrayOffset(0),
 		m_primitiveType(B3_GL_TRIANGLES),
 		m_materialShinyNess(41),
-		m_materialSpecularColor(b3MakeVector3(.5,.5,.5))
+		m_materialSpecularColor(b3MakeVector3(.5,.5,.5)),
+		m_flags(0)
 	{
 	}
 
@@ -181,6 +189,7 @@ struct InternalTextureHandle
     GLuint  m_glTexture;
     int m_width;
     int m_height;
+	int m_enableFiltering;
 };
 
 struct b3PublicGraphicsInstanceData
@@ -324,8 +333,7 @@ GLInstancingRenderer::GLInstancingRenderer(int maxNumObjectCapacity, int maxShap
 	m_textureinitialized(false),
 	m_screenWidth(0),
 	m_screenHeight(0),
-	m_upAxis(1),
+	m_upAxis(1)
-    m_enableBlend(false)
 {
 
 	m_data = new InternalDataRenderer;
@@ -386,6 +394,15 @@ GLInstancingRenderer::~GLInstancingRenderer()
 
 
 
+int GLInstancingRenderer::getShapeIndexFromInstance(int srcIndex)
+{
+	b3PublicGraphicsInstance* pg = m_data->m_publicGraphicsInstances.getHandle(srcIndex);
+	if (pg)
+	{
+		return pg->m_shapeIndex;
+	}
+	return -1;
+}
 
 
 
@@ -865,6 +882,10 @@ int GLInstancingRenderer::registerGraphicsInstanceInternal(int newUid, const flo
 		m_data->m_instance_scale_ptr[index*3+1] = scaling[1];
 		m_data->m_instance_scale_ptr[index*3+2] = scaling[2];
 
+		if (color[3]<1 && color[3]>0)
+		{
+			gfxObj->m_flags |= eGfxTransparency;
+		}
 		gfxObj->m_numGraphicsInstances++;
 		m_data->m_totalNumInstances++;
 	} else
@@ -935,7 +956,7 @@ int	GLInstancingRenderer::registerTexture(const unsigned char* texels, int width
     h.m_glTexture = textureHandle;
     h.m_width = width;
     h.m_height = height;
-
+	h.m_enableFiltering = true;
 	m_data->m_textureHandles.push_back(h);
 	if (texels)
 	{
@@ -945,14 +966,24 @@ int	GLInstancingRenderer::registerTexture(const unsigned char* texels, int width
 }
 
 
+void    GLInstancingRenderer::replaceTexture(int shapeIndex, int textureId)
+{
+	if (shapeIndex >=0 && shapeIndex < m_data->m_textureHandles.size())
+	{
+		b3GraphicsInstance* gfxObj = m_graphicsInstances[shapeIndex];
+		if (textureId>=0)
+		{
+			gfxObj->m_textureIndex = textureId;
+			gfxObj->m_flags |= eGfxHasTexture;
+
+		}
+	}
+}
 
 void    GLInstancingRenderer::updateTexture(int textureIndex, const unsigned char* texels, bool flipPixelsY)
 {
     if (textureIndex>=0)
     {
-		
-		
-
         glActiveTexture(GL_TEXTURE0);
         b3Assert(glGetError() ==GL_NO_ERROR);
         InternalTextureHandle& h = m_data->m_textureHandles[textureIndex];
@@ -981,7 +1012,10 @@ void    GLInstancingRenderer::updateTexture(int textureIndex, const unsigned cha
 			glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, h.m_width,h.m_height,0,GL_RGB,GL_UNSIGNED_BYTE,&texels[0]);
 		}
         b3Assert(glGetError() ==GL_NO_ERROR);
+		if (h.m_enableFiltering)
+		{
 	        glGenerateMipmap(GL_TEXTURE_2D);
+		}
         b3Assert(glGetError() ==GL_NO_ERROR);
     }
 }
@@ -1018,7 +1052,8 @@ int GLInstancingRenderer::registerShape(const float* vertices, int numvertices,
 
 	if (textureId>=0)
 	{
-		gfxObj->m_texturehandle = m_data->m_textureHandles[textureId].m_glTexture;
+		gfxObj->m_textureIndex = textureId;
+		gfxObj->m_flags |= eGfxHasTexture;
 	}
 
 	gfxObj->m_primitiveType = primitiveType;
@@ -1898,7 +1933,24 @@ void GLInstancingRenderer::drawLine(const float from[4], const float to[4], cons
 	glUseProgram(0);
 }
 
+struct SortableTransparentInstance
+{
+	int m_shapeIndex;
+	int m_instanceId;
+	b3Vector3 m_centerPosition;
+};
 
+struct TransparentDistanceSortPredicate
+{
+	b3Vector3 m_camForwardVec;
+
+	inline bool operator() (const SortableTransparentInstance& a, const SortableTransparentInstance& b) const 
+	{
+		b3Scalar projA = a.m_centerPosition.dot(m_camForwardVec);
+		b3Scalar projB = b.m_centerPosition.dot(m_camForwardVec);
+		return (projA > projB);
+	}
+};
 
 void GLInstancingRenderer::renderSceneInternal(int renderMode)
 {
@@ -2019,9 +2071,9 @@ void GLInstancingRenderer::renderSceneInternal(int renderMode)
 	b3Vector3 center = b3MakeVector3(0,0,0);
 	//float upf[3];
 	//m_data->m_activeCamera->getCameraUpVector(upf);
-	b3Vector3 up, fwd;
+	b3Vector3 up, lightFwd;
 	b3Vector3 lightDir = m_data->m_lightPos.normalized();
-	b3PlaneSpace1(lightDir,up,fwd);
+	b3PlaneSpace1(lightDir,up,lightFwd);
 //	b3Vector3 up = b3MakeVector3(upf[0],upf[1],upf[2]);
 	b3CreateLookAt(m_data->m_lightPos,center,up,&depthViewMatrix[0][0]);
 	//b3CreateLookAt(lightPos,m_data->m_cameraTargetPosition,b3Vector3(0,1,0),(float*)depthModelViewMatrix2);
@@ -2082,37 +2134,113 @@ b3Assert(glGetError() ==GL_NO_ERROR);
 		totalNumInstances+=m_graphicsInstances[i]->m_numGraphicsInstances;
 	}
 	
+	b3AlignedObjectArray<SortableTransparentInstance> transparentInstances;
+	
+	{
 		int curOffset = 0;
 		//GLuint lastBindTexture = 0;
 
+		transparentInstances.reserve(totalNumInstances);
 
-	for (int i=0;i<m_graphicsInstances.size();i++)
+		for (int obj=0;obj<m_graphicsInstances.size();obj++)
+		{
+			b3GraphicsInstance* gfxObj = m_graphicsInstances[obj];
+			if (gfxObj->m_numGraphicsInstances)
+			{
+				SortableTransparentInstance inst;
+									
+				inst.m_shapeIndex = obj;
+			
+			
+				if ((gfxObj->m_flags&eGfxTransparency)==0)
+				{
+					inst.m_instanceId = curOffset;
+					inst.m_centerPosition.setValue(m_data->m_instance_positions_ptr[inst.m_instanceId*4+0],
+								m_data->m_instance_positions_ptr[inst.m_instanceId*4+1],
+								m_data->m_instance_positions_ptr[inst.m_instanceId*4+2]);
+					inst.m_centerPosition *= -1;//reverse sort opaque instances
+					transparentInstances.push_back(inst);
+				} else
+				{
+					for (int i=0;i<gfxObj->m_numGraphicsInstances;i++)
+					{
+						inst.m_instanceId = curOffset+i;
+						inst.m_centerPosition.setValue(m_data->m_instance_positions_ptr[inst.m_instanceId*4+0],
+										m_data->m_instance_positions_ptr[inst.m_instanceId*4+1],
+										m_data->m_instance_positions_ptr[inst.m_instanceId*4+2]);
+						transparentInstances.push_back(inst);
+					}
+				}
+				curOffset+=gfxObj->m_numGraphicsInstances;
+			}
+		}
+		TransparentDistanceSortPredicate sorter;
+		float fwd[3];
+		m_data->m_activeCamera->getCameraForwardVector(fwd);
+		sorter.m_camForwardVec.setValue(fwd[0],fwd[1],fwd[2]);
+		transparentInstances.quickSort(sorter);
+	}
+
+	
+	//two passes: first for opaque instances, second for transparent ones.
+	for (int pass = 0; pass<2;pass++)
+	{
+		for (int i=0;i<transparentInstances.size();i++)
 		{
 
-		b3GraphicsInstance* gfxObj = m_graphicsInstances[i];
+			int shapeIndex = transparentInstances[i].m_shapeIndex;
-		if (gfxObj->m_numGraphicsInstances)
+
+			b3GraphicsInstance* gfxObj = m_graphicsInstances[shapeIndex];
+			
+			//only draw stuff (opaque/transparent) if it is the right pass
+			int drawThisPass = (pass==0) == ((gfxObj->m_flags&eGfxTransparency)==0);
+
+			//transparent objects don't cast shadows (to simplify things)
+			if (gfxObj->m_flags&eGfxTransparency) 
+			{
+				if (renderMode==B3_CREATE_SHADOWMAP_RENDERMODE)
+					drawThisPass = 0;
+			}
+			
+			if (drawThisPass && gfxObj->m_numGraphicsInstances)
 			{
 				glActiveTexture(GL_TEXTURE0);
 				GLuint curBindTexture = 0;
-			if (gfxObj->m_texturehandle)
+				if (gfxObj->m_flags & eGfxHasTexture)
-				curBindTexture = gfxObj->m_texturehandle;
+				{
-			else
+					curBindTexture = m_data->m_textureHandles[gfxObj->m_textureIndex].m_glTexture;
-				curBindTexture = m_data->m_defaultTexturehandle;
 
-//disable lazy evaluation, it just leads to bugs
+					glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_BASE_LEVEL,0);
+
+					if (m_data->m_textureHandles[gfxObj->m_textureIndex].m_enableFiltering)
+					{
+						glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+						glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+					} else
+					{
+						glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+						glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+					}
+				}
+				else
+				{
+					curBindTexture = m_data->m_defaultTexturehandle;
+				}
+
+	//disable lazy evaluation, it just leads to bugs
 				//if (lastBindTexture != curBindTexture)
 				{
 					glBindTexture(GL_TEXTURE_2D,curBindTexture);
 				}
 				//lastBindTexture = curBindTexture;
 
-b3Assert(glGetError() ==GL_NO_ERROR);
+			b3Assert(glGetError() ==GL_NO_ERROR);
 		//	int myOffset = gfxObj->m_instanceOffset*4*sizeof(float);
 
 			int POSITION_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4);
 			int ORIENTATION_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4);
 			int COLOR_BUFFER_SIZE = (totalNumInstances*sizeof(float)*4);
-//		int SCALE_BUFFER_SIZE = (totalNumInstances*sizeof(float)*3);
+	//		int SCALE_BUFFER_SIZE = (totalNumInstances*sizeof(float)*3);
 
 			glBindVertexArray(gfxObj->m_cube_vao);
 
@@ -2123,8 +2251,8 @@ b3Assert(glGetError() ==GL_NO_ERROR);
 
 
 			glVertexAttribPointer(0, 4, GL_FLOAT, GL_FALSE, 9*sizeof(float), vertex.m_pointer);
-		glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)(curOffset*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes));
+			glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)(transparentInstances[i].m_instanceId*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes));
-		glVertexAttribPointer(2, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)(curOffset*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE));
+			glVertexAttribPointer(2, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)(transparentInstances[i].m_instanceId*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE));
 
 			PointerCaster uv;
 			uv.m_baseIndex = 7*sizeof(float)+vertex.m_baseIndex;
@@ -2134,8 +2262,8 @@ b3Assert(glGetError() ==GL_NO_ERROR);
 
 			glVertexAttribPointer(3, 2, GL_FLOAT, GL_FALSE, 9*sizeof(float), uv.m_pointer);
 			glVertexAttribPointer(4, 3, GL_FLOAT, GL_FALSE, 9*sizeof(float), normal.m_pointer);
-		glVertexAttribPointer(5, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)(curOffset*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE));
+			glVertexAttribPointer(5, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)(transparentInstances[i].m_instanceId*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE));
-		glVertexAttribPointer(6, 3, GL_FLOAT, GL_FALSE, 0, (GLvoid *)(curOffset*3*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE));
+			glVertexAttribPointer(6, 3, GL_FLOAT, GL_FALSE, 0, (GLvoid *)(transparentInstances[i].m_instanceId*3*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE));
 
 			glEnableVertexAttribArray(0);
 			glEnableVertexAttribArray(1);
@@ -2175,10 +2303,10 @@ b3Assert(glGetError() ==GL_NO_ERROR);
 						  b3Assert(glGetError() ==GL_NO_ERROR);
 						  glPointSize(20);
 
-#ifndef __APPLE__
+	#ifndef __APPLE__
 						glEnable(GL_POINT_SPRITE_ARB);
-//					glTexEnvi(GL_POINT_SPRITE_ARB, GL_COORD_REPLACE_ARB, GL_TRUE);
+	//					glTexEnvi(GL_POINT_SPRITE_ARB, GL_COORD_REPLACE_ARB, GL_TRUE);
-#endif
+	#endif
 
 						glEnable(GL_VERTEX_PROGRAM_POINT_SIZE);
 						glDrawElementsInstanced(GL_POINTS, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances);
@@ -2189,6 +2317,13 @@ b3Assert(glGetError() ==GL_NO_ERROR);
 
 						case B3_DEFAULT_RENDERMODE:
 							{
+								if ( gfxObj->m_flags&eGfxTransparency)
+								{
+									glDepthMask(false);
+									glEnable (GL_BLEND);
+									glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+								}
+
 								glUseProgram(instancingShader);
 								glUniformMatrix4fv(ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]);
 								glUniformMatrix4fv(ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]);
@@ -2198,18 +2333,33 @@ b3Assert(glGetError() ==GL_NO_ERROR);
 								glUniform3f(regularLightDirIn,gLightDir[0],gLightDir[1],gLightDir[2]);
 
 								glUniform1i(uniform_texture_diffuse, 0);
+
+								if ( gfxObj->m_flags&eGfxTransparency)
+								{
+									
+									int instanceId = transparentInstances[i].m_instanceId;
+									glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)((instanceId)*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes));
+									glVertexAttribPointer(2, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)((instanceId)*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE));
+									glVertexAttribPointer(5, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)((instanceId)*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE));
+									glVertexAttribPointer(6, 3, GL_FLOAT, GL_FALSE, 0, (GLvoid *)((instanceId)*3*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE));
+
+									glDrawElements(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, 0);
+									
+								} else
+								{
 									glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances);
+								}
+                            
+								if ( gfxObj->m_flags&eGfxTransparency)
+								{
+									glDisable (GL_BLEND);
+									glDepthMask(true);
+								}
+								
 								break;
 							}
 							case B3_CREATE_SHADOWMAP_RENDERMODE:
 							{
-                            /*printf("createShadowMapInstancingShader=%d\n",createShadowMapInstancingShader);
-                            printf("createShadow_depthMVP=%d\n",createShadow_depthMVP);
-                            printf("indexOffset=%d\n",indexOffset);
-                            printf("gfxObj->m_numGraphicsInstances=%d\n",gfxObj->m_numGraphicsInstances);
-                            printf("indexCount=%d\n",indexCount);
-				*/
-
 								glUseProgram(createShadowMapInstancingShader);
 								glUniformMatrix4fv(createShadow_depthMVP, 1, false, &depthMVP[0][0]);
 								glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances);
@@ -2218,8 +2368,9 @@ b3Assert(glGetError() ==GL_NO_ERROR);
 
 						case B3_USE_SHADOWMAP_RENDERMODE:
 							{
-                            if (m_enableBlend)
+								if ( gfxObj->m_flags&eGfxTransparency)
 								{
+									glDepthMask(false);
 									glEnable (GL_BLEND);
 									glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
 								}
@@ -2249,10 +2400,28 @@ b3Assert(glGetError() ==GL_NO_ERROR);
 								glActiveTexture(GL_TEXTURE1);
 								glBindTexture(GL_TEXTURE_2D, m_data->m_shadowTexture);
 								glUniform1i(useShadow_shadowMap,1);
+							
+								//sort transparent objects
+							
+								//gfxObj->m_instanceOffset
+
+								if ( gfxObj->m_flags&eGfxTransparency)
+								{
+									int instanceId = transparentInstances[i].m_instanceId;
+									glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)((instanceId)*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes));
+									glVertexAttribPointer(2, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)((instanceId)*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE));
+									glVertexAttribPointer(5, 4, GL_FLOAT, GL_FALSE, 0, (GLvoid *)((instanceId)*4*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE));
+									glVertexAttribPointer(6, 3, GL_FLOAT, GL_FALSE, 0, (GLvoid *)((instanceId)*3*sizeof(float)+m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE));
+									glDrawElements(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, 0);
+								} else
+								{
 									glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances);
-                            if (m_enableBlend)
+								}
+                            
+								if ( gfxObj->m_flags&eGfxTransparency)
 								{
 									glDisable (GL_BLEND);
+									glDepthMask(true);
 								}
 								glActiveTexture(GL_TEXTURE1);
 								glBindTexture(GL_TEXTURE_2D,0);
@@ -2272,7 +2441,7 @@ b3Assert(glGetError() ==GL_NO_ERROR);
 					//glDrawElementsInstanced(GL_LINE_LOOP, indexCount, GL_UNSIGNED_INT, (void*)indexOffset, gfxObj->m_numGraphicsInstances);
 				}
 			}
-		curOffset+= gfxObj->m_numGraphicsInstances;
+		}
 	}
 
 	{

examples/OpenGLWindow/GLInstancingRenderer.h

@@ -39,7 +39,7 @@ class GLInstancingRenderer : public CommonRenderInterface
 	int m_screenHeight;
 	
 	int m_upAxis;
-    bool m_enableBlend;
+    
     
 	int registerGraphicsInstanceInternal(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling);
 	void rebuildGraphicsInstances();
@@ -67,7 +67,8 @@ public:
 	virtual int registerTexture(const unsigned char* texels, int width, int height, bool flipPixelsY=true);
     virtual void updateTexture(int textureIndex, const unsigned char* texels, bool flipPixelsY=true);
     virtual void activateTexture(int textureIndex);
-
+	virtual void replaceTexture(int shapeIndex, int textureId);
+	virtual int getShapeIndexFromInstance(int srcIndex);
 
 	///position x,y,z, quaternion x,y,z,w, color r,g,b,a, scaling x,y,z
 	virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling);
@@ -147,10 +148,7 @@ public:
 	virtual int getTotalNumInstances() const;
 	
 	virtual void enableShadowMap();
-    virtual void enableBlend(bool blend)
+    
-    {
-        m_enableBlend = blend;
-    }
 	virtual void clearZBuffer();
 
 	virtual void setRenderFrameBuffer(unsigned int renderFrameBuffer);

examples/OpenGLWindow/SimpleCamera.cpp

@@ -234,8 +234,15 @@ void SimpleCamera::update()
 
 	b3Vector3 eyePos = b3MakeVector3(0,0,0);
 	eyePos[forwardAxis] = -m_data->m_cameraDistance;
+	eyePos = b3Matrix3x3(eyeRot)*eyePos;
 
-	m_data->m_cameraForward = b3MakeVector3(eyePos[0],eyePos[1],eyePos[2]);
+	m_data->m_cameraPosition = eyePos;
+
+	
+
+	m_data->m_cameraPosition+= m_data->m_cameraTargetPosition;
+
+	m_data->m_cameraForward = m_data->m_cameraTargetPosition-m_data->m_cameraPosition;
 	if (m_data->m_cameraForward.length2() < B3_EPSILON)
 	{
 		m_data->m_cameraForward.setValue(1.f,0.f,0.f);
@@ -243,12 +250,6 @@ void SimpleCamera::update()
 	{
 		m_data->m_cameraForward.normalize();
 	}
-	
-	eyePos = b3Matrix3x3(eyeRot)*eyePos;
-
-	m_data->m_cameraPosition = eyePos;
-	m_data->m_cameraPosition+= m_data->m_cameraTargetPosition;
-
 }
 
 void SimpleCamera::getCameraProjectionMatrix(float projectionMatrix[16]) const

examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp

@@ -676,9 +676,6 @@ void SimpleOpenGL2Renderer::updateShape(int shapeIndex, const float* vertices)
 	}
 }
 
-void SimpleOpenGL2Renderer::enableBlend(bool blend)
-{
-}
 
 void SimpleOpenGL2Renderer::clearZBuffer()
 {

examples/OpenGLWindow/SimpleOpenGL2Renderer.h

@@ -83,7 +83,6 @@ public:
     
     virtual void updateShape(int shapeIndex, const float* vertices);
     
-    virtual void enableBlend(bool blend);
     
 	virtual void clearZBuffer();
 

examples/RenderingExamples/CoordinateSystemDemo.cpp

@@ -37,7 +37,6 @@ public:
     }
     virtual ~CoordinateSystemDemo()
     {
-        m_app->m_renderer->enableBlend(false);
     }
     
     

examples/RenderingExamples/DynamicTexturedCubeDemo.cpp

@@ -66,7 +66,6 @@ public:
     virtual ~DynamicTexturedCubeDemo()
     {
 		delete m_tinyVrGUI;
-        m_app->m_renderer->enableBlend(false);
     }
 
     

examples/RenderingExamples/RenderInstancingDemo.cpp

@@ -68,7 +68,6 @@ public:
     }
     virtual ~RenderInstancingDemo()
     {
-        m_app->m_renderer->enableBlend(false);
     }
 
     

examples/RenderingExamples/TinyRendererSetup.cpp

@@ -154,7 +154,6 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui)
 	m_app = gui->getAppInterface();
 	m_internalData = new TinyRendererSetupInternalData(gui->getAppInterface()->m_window->getWidth(),gui->getAppInterface()->m_window->getHeight());
 	
-	m_app->m_renderer->enableBlend(true);
     
 	const char* fileName = "textured_sphere_smooth.obj";
     fileName = "cube.obj";
@@ -225,7 +224,6 @@ TinyRendererSetup::TinyRendererSetup(struct GUIHelperInterface* gui)
 
 TinyRendererSetup::~TinyRendererSetup()
 {
-	m_app->m_renderer->enableBlend(false);
 	delete m_internalData;
 }
 

examples/RoboticsLearning/GripperGraspExample.cpp

@@ -45,7 +45,6 @@ public:
     }
     virtual ~GripperGraspExample()
     {
-        m_app->m_renderer->enableBlend(false);
     }
 
     

examples/RoboticsLearning/KukaGraspExample.cpp

@@ -54,7 +54,6 @@ public:
     }
     virtual ~KukaGraspExample()
     {
-        m_app->m_renderer->enableBlend(false);
     }
 
     

examples/RoboticsLearning/R2D2GraspExample.cpp

@@ -41,7 +41,6 @@ public:
     }
     virtual ~R2D2GraspExample()
     {
-        m_app->m_renderer->enableBlend(false);
     }
 
     

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -733,6 +733,7 @@ int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,do
 		if (shapeIndex <MAX_COMPOUND_COLLISION_SHAPES)
 		{
 			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_SPHERE;
+			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
 			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
 			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_sphereRadius = radius;
 			command->m_createCollisionShapeArgs.m_numCollisionShapes++;
@@ -753,6 +754,7 @@ int b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,doubl
 		if (shapeIndex <MAX_COMPOUND_COLLISION_SHAPES)
 		{
 			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_BOX;
+			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
 			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
 			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[0] = halfExtents[0];
 			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[1] = halfExtents[1];
@@ -775,6 +777,7 @@ int b3CreateCollisionShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle,d
 		if (shapeIndex <MAX_COMPOUND_COLLISION_SHAPES)
 		{
 			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_CAPSULE;
+			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
 			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
 			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_capsuleRadius = radius;
 			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_capsuleHeight = height;
@@ -796,6 +799,7 @@ int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle,
 		if (shapeIndex <MAX_COMPOUND_COLLISION_SHAPES)
 		{
 			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_CYLINDER;
+			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
 			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
 			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_capsuleRadius = radius;
 			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_capsuleHeight = height;
@@ -818,6 +822,7 @@ int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, do
 		if (shapeIndex <MAX_COMPOUND_COLLISION_SHAPES)
 		{
 			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_PLANE;
+			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
 			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
 			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_planeNormal[0] = planeNormal[0];
 			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_planeNormal[1] = planeNormal[1];
@@ -841,6 +846,7 @@ int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,cons
 		if (shapeIndex <MAX_COMPOUND_COLLISION_SHAPES && strlen(fileName)<VISUAL_SHAPE_MAX_PATH_LEN)
 		{
 			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_MESH;
+			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
 			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
 			strcpy(command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_meshFileName,fileName);
 			command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_meshScale[0] = meshScale[0];
@@ -856,6 +862,7 @@ int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,cons
 
 void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, int flags)
 {
+	
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
     b3Assert(command);
     b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE);
@@ -1942,6 +1949,20 @@ int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHa
 	return 0;
 }
 
+int b3InitChangeUserConstraintSetGearAuxLink(b3SharedMemoryCommandHandle commandHandle, int gearAuxLink)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command);
+	b3Assert(command->m_type == CMD_USER_CONSTRAINT);
+	b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT);
+	
+	command->m_updateFlags |=USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK;
+	command->m_userConstraintArguments.m_gearAuxLink = gearAuxLink;
+
+	return 0;
+}
+
+
 b3SharedMemoryCommandHandle  b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId)
 {
 	 PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -2897,6 +2918,25 @@ void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3Visu
 	}
 }
 
+b3SharedMemoryCommandHandle b3CreateChangeTextureCommandInit(b3PhysicsClientHandle physClient, int textureUniqueId, int width, int height, const char* rgbPixels)
+{
+    PhysicsClient* cl = (PhysicsClient* ) physClient;
+    b3Assert(cl);
+    b3Assert(cl->canSubmitCommand());
+    struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+    b3Assert(command);
+    command->m_type = CMD_CHANGE_TEXTURE;
+
+	command->m_changeTextureArgs.m_textureUniqueId = textureUniqueId;
+	command->m_changeTextureArgs.m_width = width;
+	command->m_changeTextureArgs.m_height = height;
+	int numPixels = width*height;
+	cl->uploadBulletFileToSharedMemory(rgbPixels,numPixels*3);
+	command->m_updateFlags = 0;
+    return (b3SharedMemoryCommandHandle) command;
+}
+
+
 b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename)
 {
     PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -2917,6 +2957,18 @@ b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient,
     return (b3SharedMemoryCommandHandle) command;
 }
 
+int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHandle)
+{
+	int uid = -1;
+	const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
+	btAssert(status->m_type == CMD_LOAD_TEXTURE_COMPLETED);	
+	if (status->m_type == CMD_LOAD_TEXTURE_COMPLETED)
+	{
+		uid = status->m_loadTextureResultArguments.m_textureUniqueId;
+	}
+	return uid;
+}
+
 b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId)
 {
     PhysicsClient* cl = (PhysicsClient* ) physClient;

examples/SharedMemory/PhysicsClientC_API.h

@@ -107,7 +107,7 @@ int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHan
 int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[4]);
 int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce);
 int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHandle, double gearRatio);
-
+int b3InitChangeUserConstraintSetGearAuxLink(b3SharedMemoryCommandHandle commandHandle, int gearAuxLink);
 
 b3SharedMemoryCommandHandle  b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
 
@@ -217,6 +217,10 @@ b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientH
 void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo);
 
 b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename);
+int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHandle);
+
+b3SharedMemoryCommandHandle b3CreateChangeTextureCommandInit(b3PhysicsClientHandle physClient, int textureUniqueId, int width, int height, const char* rgbPixels);
+
 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]);

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -1368,7 +1368,9 @@ void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data,
                   SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
     } else {
         for (int i = 0; i < len; i++) {
-            m_data->m_testBlock1->m_bulletStreamDataClientToServer[i] = data[i];
+            //m_data->m_testBlock1->m_bulletStreamDataClientToServer[i] = data[i];
+			m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[i] = data[i];
+
         }
     }
 }

examples/SharedMemory/PhysicsDirect.cpp

@@ -1095,6 +1095,14 @@ void PhysicsDirect::setSharedMemoryKey(int key)
 
 void PhysicsDirect::uploadBulletFileToSharedMemory(const char* data, int len)
 {
+	if (len>SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE)
+	{
+		len = SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE;
+	}
+	for (int i=0;i<len;i++)
+	{
+		m_data->m_bulletStreamDataServerToClient[i] = data[i];
+	}
 	//m_data->m_physicsClient->uploadBulletFileToSharedMemory(data,len);
 }
 

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -14,13 +14,13 @@
 #include "../Importers/ImportURDFDemo/UrdfParser.h"
 #include "../Utils/b3ResourcePath.h"
 #include "Bullet3Common/b3FileUtils.h"
-
+#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
 #include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
 #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
 #include "Bullet3Common/b3HashMap.h"
 #include "../Utils/ChromeTraceUtil.h"
-
+#include "stb_image/stb_image.h"
 #include "BulletInverseDynamics/MultiBodyTree.hpp"
 #include "IKTrajectoryHelper.h"
 #include "btBulletDynamicsCommon.h"
@@ -129,16 +129,17 @@ struct SharedMemoryDebugDrawer : public btIDebugDraw
 	}
 };
 
-struct InteralCollisionShapeData
+struct InternalCollisionShapeData
 {
 	btCollisionShape* m_collisionShape;
+	b3AlignedObjectArray<UrdfCollision> m_urdfCollisionObjects;
 	void clear()
 	{
 		m_collisionShape=0;
 	}
 };
 
-struct InteralBodyData
+struct InternalBodyData
 {
 	btMultiBody* m_multiBody;
 	btRigidBody* m_rigidBody;
@@ -151,7 +152,7 @@ struct InteralBodyData
 	b3HashMap<btHashInt, SDFAudioSource> m_audioSources;
 #endif //B3_ENABLE_TINY_AUDIO
 
-	InteralBodyData()		
+	InternalBodyData()		
 	{
 		clear();
 	}
@@ -182,8 +183,20 @@ struct InteralUserConstraintData
 	}
 };
 
-typedef b3PoolBodyHandle<InteralBodyData> InternalBodyHandle;
+struct InternalTextureData
-typedef b3PoolBodyHandle<InteralCollisionShapeData> InternalCollisionShapeHandle;
+{
+	int m_tinyRendererTextureId;
+	int m_openglTextureId;
+	void clear()
+	{
+		m_tinyRendererTextureId = -1;
+		m_openglTextureId = -1;
+	}
+};
+
+typedef b3PoolBodyHandle<InternalTextureData> InternalTextureHandle;
+typedef b3PoolBodyHandle<InternalBodyData> InternalBodyHandle;
+typedef b3PoolBodyHandle<InternalCollisionShapeData> InternalCollisionShapeHandle;
 
 class btCommandChunk
 {
@@ -1144,6 +1157,7 @@ struct ContactPointsStateLogger : public InternalStateLogger
 struct PhysicsServerCommandProcessorInternalData
 {
 	///handle management
+	b3ResizablePool< InternalTextureHandle > m_textureHandles;
 	b3ResizablePool< InternalBodyHandle > m_bodyHandles;
 	b3ResizablePool<InternalCollisionShapeHandle> m_userCollisionShapeHandles;
 
@@ -1268,7 +1282,7 @@ struct PhysicsServerCommandProcessorInternalData
 			int handle = allocHandle();
 			bla.push_back(handle);
 			InternalBodyHandle* body = getHandle(handle);
-			InteralBodyData* body2 = body;
+			InternalBodyData* body2 = body;
 		}
 		for (int i=0;i<bla.size();i++)
 		{
@@ -1282,7 +1296,7 @@ struct PhysicsServerCommandProcessorInternalData
 			int handle = allocHandle();
 			bla.push_back(handle);
 			InternalBodyHandle* body = getHandle(handle);
-			InteralBodyData* body2 = body;
+			InternalBodyData* body2 = body;
 		}
 		for (int i=0;i<bla.size();i++)
 		{
@@ -1295,7 +1309,7 @@ struct PhysicsServerCommandProcessorInternalData
 			int handle = allocHandle();
 			bla.push_back(handle);
 			InternalBodyHandle* body = getHandle(handle);
-			InteralBodyData* body2 = body;
+			InternalBodyData* body2 = body;
 		}
 		for (int i=0;i<bla.size();i++)
 		{
@@ -1739,21 +1753,28 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
 		return -1;
 	}
     
-    virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const  
+    virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const  
 	{
+		//if there is a visual, use it, otherwise convert collision shape back into UrdfCollision...
 
-		#if 0
+		UrdfModel model;// = m_data->m_urdfParser.getModel();
-  			if (m_data->m_customVisualShapesConverter)
+		UrdfLink link;
+		int colShapeUniqueId = m_createBodyArgs.m_linkCollisionShapeUniqueIds[urdfIndex];
+		if (colShapeUniqueId>=0)
 		{
-				const UrdfModel& model = m_data->m_urdfParser.getModel();
+			InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(colShapeUniqueId);
-				UrdfLink*const* linkPtr = model.m_links.getAtIndex(urdfIndex);
+			if (handle)
-				if (linkPtr)
 			{
-					m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, *linkPtr, &model, colObj, bodyUniqueId);
+				for (int i=0;i<handle->m_urdfCollisionObjects.size();i++)
+				{
+					link.m_collisionArray.push_back(handle->m_urdfCollisionObjects[i]);
 				}
 			}
 		}
-	#endif
+		//UrdfVisual vis;
+		//link.m_visualArray.push_back(vis);
+		//UrdfLink*const* linkPtr = model.m_links.getAtIndex(urdfIndex);
+		m_data->m_visualConverter.convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, &link, &model, colObj, bodyUniqueId);
 	}
     virtual void setBodyUniqueId(int bodyId) 
 	{
@@ -2671,7 +2692,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 							if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID)&& (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds>0))
 							{
 								int bodyUniqueId = clientCmd.m_stateLoggingArguments.m_bodyUniqueIds[0];
-								InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
+								InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
 								if (body)
 								{
 									if (body->m_multiBody)
@@ -3248,7 +3269,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 					for (int i=0;i<usedHandles.size();i++)
 					{
 						int usedHandle = usedHandles[i];
-						InteralBodyData* body = m_data->m_bodyHandles.getHandle(usedHandle);
+						InternalBodyData* body = m_data->m_bodyHandles.getHandle(usedHandle);
 						if (body && (body->m_multiBody || body->m_rigidBody))
 						{
 							serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[actualNumBodies++] = usedHandle;
@@ -3337,7 +3358,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 								{
 									{
 										int bodyUniqueId = sd.m_bodyUniqueIds[i];
-										InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
+										InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
 										if (body)
 										{
 											 if (body->m_multiBody)
@@ -3588,6 +3609,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 					btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld);
 
 					btCollisionShape* shape = 0;
+					b3AlignedObjectArray<UrdfCollision> urdfCollisionObjects;
 
 					btCompoundShape* compound = 0;
 
@@ -3597,6 +3619,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 					}
 					for (int i=0;i<clientCmd.m_createCollisionShapeArgs.m_numCollisionShapes;i++)
 					{
+						UrdfCollision urdfColObj;
+
 						btTransform childTransform;
 						childTransform.setIdentity();
 						if (clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_hasChildTransform)
@@ -3616,6 +3640,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 							}
 						}
 
+						urdfColObj.m_linkLocalFrame = childTransform;
+						urdfColObj.m_sourceFileLocation = "memory";
+						urdfColObj.m_name = "memory";
+						urdfColObj.m_geometry.m_type = URDF_GEOM_UNKNOWN;
+
 						switch (clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_type)
 						{
 							case GEOM_SPHERE:
@@ -3626,19 +3655,24 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 								{
 									compound->addChildShape(childTransform,shape);
 								}
+								urdfColObj.m_geometry.m_type = URDF_GEOM_SPHERE;
+								urdfColObj.m_geometry.m_sphereRadius = radius;
 								break;
 							}
 							case GEOM_BOX:
 							{
 								//double halfExtents[3] = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_sphereRadius;
-								shape = worldImporter->createBoxShape(btVector3(
+								btVector3 halfExtents(
 									clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[0],
 									clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[1],
-									clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[2]));
+									clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[2]);
+								shape = worldImporter->createBoxShape(halfExtents);
 								if (compound)
 								{
 									compound->addChildShape(childTransform,shape);
 								}
+								urdfColObj.m_geometry.m_type = URDF_GEOM_BOX;
+								urdfColObj.m_geometry.m_boxSize = 2.*halfExtents;
 								break;
 							}
 							case GEOM_CAPSULE:
@@ -3649,6 +3683,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 								{
 									compound->addChildShape(childTransform,shape);
 								}
+								urdfColObj.m_geometry.m_type = URDF_GEOM_CAPSULE;
+								urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleRadius;
+								urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleHeight;
+
 								break;
 							}
 							case GEOM_CYLINDER:
@@ -3659,6 +3697,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 								{
 									compound->addChildShape(childTransform,shape);
 								}
+								urdfColObj.m_geometry.m_type = URDF_GEOM_CYLINDER;
+								urdfColObj.m_geometry.m_capsuleRadius = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleRadius;
+								urdfColObj.m_geometry.m_capsuleHeight = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_capsuleHeight;
+
 								break;
 							}
 							case GEOM_PLANE:
@@ -3672,6 +3714,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 								{
 									compound->addChildShape(childTransform,shape);
 								}
+								urdfColObj.m_geometry.m_type = URDF_GEOM_PLANE;
+								urdfColObj.m_geometry.m_planeNormal.setValue(
+									clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_planeNormal[0],
+									clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_planeNormal[1],
+									clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_planeNormal[2]);
+									
 								break;
 							}
 							case GEOM_MESH:
@@ -3685,7 +3733,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 								const std::string& urdf_path="";
 
 								std::string fileName = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_meshFileName;
+								urdfColObj.m_geometry.m_type = URDF_GEOM_MESH;
+								urdfColObj.m_geometry.m_meshFileName = fileName;
 							
+								urdfColObj.m_geometry.m_meshScale = meshScale;
 								char relativeFileName[1024];
 								char pathPrefix[1024];
 								pathPrefix[0] = 0;
@@ -3702,11 +3753,61 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 								bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName,error_message_prefix,&out_found_filename, &out_type); 
 								if (foundFile)
 								{
+									urdfColObj.m_geometry.m_meshFileType = out_type;
+
 									if (out_type==UrdfGeometry::FILE_OBJ)
 									{
+										//create a convex hull for each shape, and store it in a btCompoundShape
+
+										if (clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_collisionFlags&GEOM_FORCE_CONCAVE_TRIMESH)
+										{
+											GLInstanceGraphicsShape* glmesh = LoadMeshFromObj(relativeFileName, pathPrefix);
+											
+											if (!glmesh || glmesh->m_numvertices<=0)
+											{
+												b3Warning("%s: cannot extract mesh from '%s'\n", pathPrefix, relativeFileName);
+												delete glmesh;
+												break;
+											}
+											btAlignedObjectArray<btVector3> convertedVerts;
+											convertedVerts.reserve(glmesh->m_numvertices);
+											
+											for (int i=0; i<glmesh->m_numvertices; i++)
+											{
+												convertedVerts.push_back(btVector3(
+													glmesh->m_vertices->at(i).xyzw[0]*meshScale[0],
+													glmesh->m_vertices->at(i).xyzw[1]*meshScale[1],
+													glmesh->m_vertices->at(i).xyzw[2]*meshScale[2]));
+											}
+
+											BT_PROFILE("convert trimesh");
+											btTriangleMesh* meshInterface = new btTriangleMesh();
+											{
+												BT_PROFILE("convert vertices");
+
+												for (int i=0; i<glmesh->m_numIndices/3; i++)
+												{
+													const btVector3& v0 = convertedVerts[glmesh->m_indices->at(i*3)];
+													const btVector3& v1 = convertedVerts[glmesh->m_indices->at(i*3+1)];
+													const btVector3& v2 = convertedVerts[glmesh->m_indices->at(i*3+2)];
+													meshInterface->addTriangle(v0,v1,v2);
+												}
+											}
+											{
+												BT_PROFILE("create btBvhTriangleMeshShape");
+												btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true);
+												//trimesh->setLocalScaling(collision->m_geometry.m_meshScale);
+												shape = trimesh;
+												if (compound)
+												{
+													compound->addChildShape(childTransform,shape);
+												}
+											}
+										} else
+										{
+
 											std::vector<tinyobj::shape_t> shapes;
 											std::string err = tinyobj::LoadObj(shapes,out_found_filename.c_str());
-										//create a convex hull for each shape, and store it in a btCompoundShape
 
 											//shape = createConvexHullFromShapes(shapes, collision->m_geometry.m_meshScale);
 											//static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale)
@@ -3751,13 +3852,20 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 											}
 										}
 									}
+								}
 								break;
 							}
 							default:
 							{
 							}
 						}
+						if (urdfColObj.m_geometry.m_type != URDF_GEOM_UNKNOWN)
+						{
+							urdfCollisionObjects.push_back(urdfColObj);
 						}
+					}
+
+					
 					#if 0
 					shape = worldImporter->createCylinderShapeX(radius,height);
 					shape = worldImporter->createCylinderShapeY(radius,height);
@@ -3777,6 +3885,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 						int collisionShapeUid = m_data->m_userCollisionShapeHandles.allocHandle();
 						InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(collisionShapeUid);
 						handle->m_collisionShape = shape;
+						for (int i=0;i<urdfCollisionObjects.size();i++)
+						{
+							handle->m_urdfCollisionObjects.push_back(urdfCollisionObjects[i]);
+						}
 						serverStatusOut.m_createCollisionShapeResultArgs.m_collisionShapeUniqueId = collisionShapeUid;
 						m_data->m_worldImporters.push_back(worldImporter);
 						serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_COMPLETED;
@@ -3860,7 +3972,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 									serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize();
 								}
 								serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
-								InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
+								InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
 								strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());
 							}
 						}
@@ -3925,7 +4037,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 							serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize();
 						}
 						serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
-						InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
+						InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
 						strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());
 						hasStatus = true;
 
@@ -3997,7 +4109,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 						b3Printf("Processed CMD_CREATE_SENSOR");
 					}
 					int bodyUniqueId = clientCmd.m_createSensorArguments.m_bodyUniqueId;
-					InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
+					InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
                     if (body && body->m_multiBody)
                     {
                         btMultiBody* mb = body->m_multiBody;
@@ -4117,7 +4229,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 						}
 
 							int bodyUniqueId = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId;
-							InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
+							InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
 
                             if (body && body->m_multiBody)
                             {
@@ -4307,7 +4419,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 					serverStatusOut.m_type = CMD_REQUEST_COLLISION_INFO_FAILED;
 					hasStatus=true;
 					int bodyUniqueId = clientCmd.m_requestCollisionInfoArgs.m_bodyUniqueId;
-					InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
+					InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
 						
 
 					if (body && body->m_multiBody)
@@ -4408,7 +4520,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 							b3Printf("Sending the actual state (Q,U)");
 						}
 						int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId;
-						InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
+						InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
 						
 
 						if (body && body->m_multiBody)
@@ -4725,7 +4837,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 					btAssert(bodyUniqueId >= 0);
 					btAssert(linkIndex >= -1);
 						
-					InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
+					InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
 
 					if (body && body->m_multiBody)
 					{
@@ -4911,7 +5023,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 				{
 					int bodyUniqueId = clientCmd.m_getDynamicsInfoArgs.m_bodyUniqueId;
 					int linkIndex = clientCmd.m_getDynamicsInfoArgs.m_linkIndex;
-					InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
+					InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
 					if (body && body->m_multiBody)
 					{
 						SharedMemoryStatus& serverCmd = serverStatusOut;
@@ -5052,7 +5164,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 						b3Printf("Server Init Pose not implemented yet");
 					}
 					int bodyUniqueId = clientCmd.m_initPoseArgs.m_bodyUniqueId;
-					InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
+					InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
 
 					btVector3 baseLinVel(0, 0, 0);
 					btVector3 baseAngVel(0, 0, 0);
@@ -5646,7 +5758,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 								
 								if (bodyUniqueIdA >= 0)
 								{
-									InteralBodyData* bodyA = m_data->m_bodyHandles.getHandle(bodyUniqueIdA);
+									InternalBodyData* bodyA = m_data->m_bodyHandles.getHandle(bodyUniqueIdA);
 									if (bodyA)
 									{
 										if (bodyA->m_multiBody)
@@ -5680,7 +5792,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 								}
 								if (bodyUniqueIdB>=0)
 								{
-									InteralBodyData* bodyB = m_data->m_bodyHandles.getHandle(bodyUniqueIdB);
+									InternalBodyData* bodyB = m_data->m_bodyHandles.getHandle(bodyUniqueIdB);
 									if (bodyB)
 									{
 										if (bodyB->m_multiBody)
@@ -5949,7 +6061,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                     }
                     for (int i = 0; i < clientCmd.m_externalForceArguments.m_numForcesAndTorques; ++i)
                     {
-                        InteralBodyData* body = m_data->m_bodyHandles.getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]);
+                        InternalBodyData* body = m_data->m_bodyHandles.getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]);
 						bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME) != 0);
 
                         if (body && body->m_multiBody)
@@ -6155,12 +6267,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 					if (clientCmd.m_updateFlags & USER_CONSTRAINT_ADD_CONSTRAINT)
 					{
 						btScalar defaultMaxForce = 500.0;
-						InteralBodyData* parentBody = m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex);
+						InternalBodyData* parentBody = m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex);
 						if (parentBody && parentBody->m_multiBody)
 						{
 							if ((clientCmd.m_userConstraintArguments.m_parentJointIndex>=-1) && clientCmd.m_userConstraintArguments.m_parentJointIndex < parentBody->m_multiBody->getNumLinks())
 							{
-								InteralBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0;
+								InternalBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0;
 								//also create a constraint with just a single multibody/rigid body without child
 								//if (childBody)
 								{
@@ -6314,7 +6426,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 						}
 						else
 						{
-							InteralBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0;
+							InternalBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0;
 						
 							if (parentBody && childBody)
 							{
@@ -6398,6 +6510,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 								{
 									userConstraintPtr->m_mbConstraint->setGearRatio(clientCmd.m_userConstraintArguments.m_gearRatio);
 								}
+								if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK)
+								{
+									userConstraintPtr->m_mbConstraint->setGearAuxLink(clientCmd.m_userConstraintArguments.m_gearAuxLink);
+								}
 
 							}
 							if (userConstraintPtr->m_rbConstraint)
@@ -6659,12 +6775,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 					BT_PROFILE("CMD_UPDATE_VISUAL_SHAPE");
                     SharedMemoryStatus& serverCmd = serverStatusOut;
                     serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_FAILED;
+                    InternalTextureHandle* texHandle = 0;
 					
 					if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) 
 					{
+						texHandle = m_data->m_textureHandles.getHandle(clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId);
+
 						if (clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId>=0)
 						{
-		                    m_data->m_visualConverter.activateShapeTexture(clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId, clientCmd.m_updateVisualShapeDataArguments.m_jointIndex, clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex, clientCmd.m_updateVisualShapeDataArguments.m_textureUniqueId);
+							if (texHandle)
+							{
+			                    m_data->m_visualConverter.activateShapeTexture(clientCmd.m_updateVisualShapeDataArguments.m_bodyUniqueId, clientCmd.m_updateVisualShapeDataArguments.m_jointIndex, clientCmd.m_updateVisualShapeDataArguments.m_shapeIndex, texHandle->m_tinyRendererTextureId);
+							}
 						}
 					}                    
    
@@ -6681,10 +6803,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 								{
 									if (bodyHandle->m_multiBody->getBaseCollider())
 									{
-										m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor);
 										int graphicsIndex = bodyHandle->m_multiBody->getBaseCollider()->getUserIndex();
+										if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) 
+										{
+											if (texHandle)
+											{
+												int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex);
+												m_data->m_guiHelper->replaceTexture(shapeIndex,texHandle->m_openglTextureId);
+											}
+										}
 										if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) 
 										{
+											m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor);
 											m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor);
 										}
 										if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR) 
@@ -6699,10 +6829,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 									{
 										if (bodyHandle->m_multiBody->getLink(linkIndex).m_collider)
 										{
-											m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor);
 											int graphicsIndex = bodyHandle->m_multiBody->getLink(linkIndex).m_collider->getUserIndex();
+											if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_TEXTURE) 
+											{
+												if (texHandle)
+												{
+													int shapeIndex = m_data->m_guiHelper->getShapeIndexFromInstance(graphicsIndex);
+													m_data->m_guiHelper->replaceTexture(shapeIndex,texHandle->m_openglTextureId);
+												}
+											}
 											if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR) 
 											{
+												m_data->m_visualConverter.changeRGBAColor(bodyUniqueId,linkIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor);										
 												m_data->m_guiHelper->changeRGBAColor(graphicsIndex,clientCmd.m_updateVisualShapeDataArguments.m_rgbaColor);
 											}
 											if (clientCmd.m_updateFlags & CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR) 
@@ -6723,6 +6861,24 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 					serverCmd.m_type = CMD_VISUAL_SHAPE_UPDATE_COMPLETED;
                     hasStatus = true;
 
+                    break;
+                }
+
+				case CMD_CHANGE_TEXTURE:
+				{
+					SharedMemoryStatus& serverCmd = serverStatusOut;
+					serverCmd.m_type = CMD_CHANGE_TEXTURE_COMMAND_FAILED;
+					
+					InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(clientCmd.m_changeTextureArgs.m_textureUniqueId);
+					if(texH)
+					{
+						int gltex = texH->m_openglTextureId;
+						m_data->m_guiHelper->changeTexture(gltex,
+							(const unsigned char*)bufferServerToClient, clientCmd.m_changeTextureArgs.m_width,clientCmd.m_changeTextureArgs.m_height);
+
+						serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
+					}
+                    hasStatus = true;
                     break;
 				}
 				case CMD_LOAD_TEXTURE:
@@ -6731,12 +6887,46 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 					SharedMemoryStatus& serverCmd = serverStatusOut;
 					serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED;
 
-                    int uid = m_data->m_visualConverter.loadTextureFile(clientCmd.m_loadTextureArguments.m_textureFileName);
+					char relativeFileName[1024];
+					char pathPrefix[1024];
 
-                    if (uid>=0)
+					if(b3ResourcePath::findResourcePath(clientCmd.m_loadTextureArguments.m_textureFileName,relativeFileName,1024))
 					{
+						b3FileUtils::extractPath(relativeFileName,pathPrefix,1024);
+
+						int texHandle = m_data->m_textureHandles.allocHandle();
+						InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(texHandle);
+						if(texH)
+						{
+							texH->m_tinyRendererTextureId = -1;
+							texH->m_openglTextureId = -1;
+
+							int uid = m_data->m_visualConverter.loadTextureFile(relativeFileName);
+							if(uid>=0)
+							{
+								int m_tinyRendererTextureId;
+								texH->m_tinyRendererTextureId = uid;
+							}
+
+							{
+								int width,height,n;
+								unsigned char* imageData= stbi_load(relativeFileName,&width,&height,&n,3);
+
+								if(imageData)
+								{
+									texH->m_openglTextureId = m_data->m_guiHelper->registerTexture(imageData,width,height);
+									free(imageData);
+								}
+								else
+								{
+									b3Warning("Unsupported texture image format [%s]\n",relativeFileName);
+								}
+							}
+							serverCmd.m_loadTextureResultArguments.m_textureUniqueId = texHandle;
 							serverCmd.m_type = CMD_LOAD_TEXTURE_COMPLETED;
-                    } else
+						}
+					}
+					else
 					{
 						serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED;
 					}
@@ -6938,7 +7128,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 						if ((clientCmd.m_updateFlags & USER_DEBUG_SET_CUSTOM_OBJECT_COLOR) || (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR))
 						{
 							int bodyUniqueId = clientCmd.m_userDebugDrawArgs.m_objectUniqueId;
-							InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
+							InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
 							if (body)
 							{
 								btCollisionObject* destColObj = 0;

examples/SharedMemory/PhysicsServerExample.cpp

@@ -131,6 +131,9 @@ enum MultiThreadedGUIHelperCommunicationEnums
 	eGUIHelperChangeGraphicsInstanceRGBAColor,
 	eGUIHelperChangeGraphicsInstanceSpecularColor,
 	eGUIHelperSetVisualizerFlag,
+	eGUIHelperChangeGraphicsInstanceTextureId,
+	eGUIHelperGetShapeIndexFromInstance,
+	eGUIHelperChangeTexture,
 };
 
 
@@ -933,6 +936,46 @@ public:
 		workerThreadWait();    
 	}
 	
+	int m_getShapeIndex_instance;
+	int getShapeIndex_shapeIndex;
+		
+	virtual int getShapeIndexFromInstance(int instance) 
+	{
+		m_getShapeIndex_instance = instance;
+		m_cs->lock();
+		m_cs->setSharedParam(1,eGUIHelperGetShapeIndexFromInstance);
+		workerThreadWait();		
+		return getShapeIndex_shapeIndex;
+	}
+
+	int m_graphicsInstanceChangeTextureId;
+	int m_graphicsInstanceChangeTextureShapeIndex;
+	virtual void replaceTexture(int shapeIndex, int textureUid)
+	{
+		m_graphicsInstanceChangeTextureShapeIndex = shapeIndex;
+		m_graphicsInstanceChangeTextureId = textureUid;
+		m_cs->lock();
+		m_cs->setSharedParam(1,eGUIHelperChangeGraphicsInstanceTextureId);
+		workerThreadWait();   
+	}
+
+
+	int m_changeTextureUniqueId;
+	const unsigned char* m_changeTextureRgbTexels;
+	int m_changeTextureWidth;
+	int m_changeTextureHeight;
+
+	virtual void changeTexture(int textureUniqueId, const unsigned char* rgbTexels, int width, int height)
+	{
+		m_changeTextureUniqueId = textureUniqueId;
+		m_changeTextureRgbTexels = rgbTexels;
+		m_changeTextureWidth = width;
+		m_changeTextureHeight = height;
+		m_cs->lock();
+		m_cs->setSharedParam(1,eGUIHelperChangeTexture);
+		workerThreadWait();   
+	}
+
 	double m_rgbaColor[4];
 	int m_graphicsInstanceChangeColor;
 	virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4])
@@ -1913,6 +1956,34 @@ void	PhysicsServerExample::updateGraphics()
 		break;
 	}
 
+	case eGUIHelperGetShapeIndexFromInstance:
+	{
+		m_multiThreadedHelper->getShapeIndex_shapeIndex = m_multiThreadedHelper->m_childGuiHelper->getShapeIndexFromInstance(m_multiThreadedHelper->m_getShapeIndex_instance);
+		m_multiThreadedHelper->mainThreadRelease();
+		break;
+	}
+
+	case eGUIHelperChangeGraphicsInstanceTextureId:
+	{
+		m_multiThreadedHelper->m_childGuiHelper->replaceTexture(
+			m_multiThreadedHelper->m_graphicsInstanceChangeTextureShapeIndex,
+			m_multiThreadedHelper->m_graphicsInstanceChangeTextureId);
+		m_multiThreadedHelper->mainThreadRelease();
+		break;
+	}
+
+	
+	case eGUIHelperChangeTexture:
+	{
+		m_multiThreadedHelper->m_childGuiHelper->changeTexture(
+			m_multiThreadedHelper->m_changeTextureUniqueId,
+			m_multiThreadedHelper->m_changeTextureRgbTexels,
+			m_multiThreadedHelper->m_changeTextureWidth,
+			m_multiThreadedHelper->m_changeTextureHeight);
+		m_multiThreadedHelper->mainThreadRelease();
+		break;
+	}
+
 	case eGUIHelperChangeGraphicsInstanceRGBAColor:
 	{
 		m_multiThreadedHelper->m_childGuiHelper->changeRGBAColor(m_multiThreadedHelper->m_graphicsInstanceChangeColor,m_multiThreadedHelper->m_rgbaColor);

examples/SharedMemory/SharedMemoryCommands.h

@@ -283,6 +283,11 @@ struct LoadTextureArgs
     char m_textureFileName[MAX_FILENAME_LENGTH];
 };
 
+struct b3LoadTextureResultArgs
+{
+	int m_textureUniqueId;
+};
+
 struct SendVisualShapeDataArgs
 {
 	int m_bodyUniqueId;
@@ -645,6 +650,8 @@ enum EnumUserConstraintFlags
 	USER_CONSTRAINT_CHANGE_MAX_FORCE=32,
 	USER_CONSTRAINT_REQUEST_INFO=64,
 	USER_CONSTRAINT_CHANGE_GEAR_RATIO=128,	
+	USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK=256,	
+
 };
 
 enum EnumBodyChangeFlags
@@ -891,6 +898,12 @@ struct b3CreateMultiBodyResultArgs
 	int m_bodyUniqueId;
 };
 
+struct b3ChangeTextureArgs
+{
+	int m_textureUniqueId;
+	int m_width;
+	int m_height;
+};
 
 struct SharedMemoryCommand
 {
@@ -943,6 +956,7 @@ struct SharedMemoryCommand
 		struct b3CreateVisualShapeArgs m_createVisualShapeArgs;
 		struct b3CreateMultiBodyArgs m_createMultiBodyArgs;
 		struct b3RequestCollisionInfoArgs m_requestCollisionInfoArgs;
+		struct b3ChangeTextureArgs m_changeTextureArgs;
     };
 };
 
@@ -1013,6 +1027,7 @@ struct SharedMemoryStatus
 		struct b3CreateMultiBodyResultArgs m_createMultiBodyResultArgs;
 		struct b3SendCollisionInfoArgs m_sendCollisionInfoArgs;
 		struct SendMouseEvents m_sendMouseEvents;
+		struct b3LoadTextureResultArgs m_loadTextureResultArguments;
 
 	};
 };

examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp

@@ -4,6 +4,7 @@
 
 #include "PhysicsClientSharedMemory.h"
 #include"../ExampleBrowser/InProcessExampleBrowser.h"
+#include <stdio.h>
 
 #include "PhysicsServerExampleBullet2.h"
 
@@ -52,12 +53,12 @@ public:
 			}
 		}
 			{	
-	   		unsigned long int ms = m_clock.getTimeMilliseconds();
+	   		//unsigned long int ms = m_clock.getTimeMilliseconds();
-			if (ms>20)
+			//if (ms>2)
 			{ 
-				B3_PROFILE("m_clock.reset()");
+			//	B3_PROFILE("m_clock.reset()");
 
-				m_clock.reset(); 
+			//	m_clock.reset(); 
         			btUpdateInProcessExampleBrowserMainThread(m_data);
 			}
 		}
@@ -165,7 +166,8 @@ public:
 	 // return non-null if there is a status, nullptr otherwise
     virtual const struct SharedMemoryStatus* processServerStatus()
     {
-		//m_physicsServerExample->updateGraphics();
+		printf("updating graphics!\n");
+		m_physicsServerExample->updateGraphics();
 
 		unsigned long long int curTime = m_clock.getTimeMicroseconds();
 		unsigned long long int dtMicro = curTime - m_prevTime;

examples/SharedMemory/SharedMemoryPublic.h

@@ -4,7 +4,8 @@
 #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 201706015
+#define SHARED_MEMORY_MAGIC_NUMBER 201707140
+//#define SHARED_MEMORY_MAGIC_NUMBER 201706015
 //#define SHARED_MEMORY_MAGIC_NUMBER 201706001
 //#define SHARED_MEMORY_MAGIC_NUMBER 201703024
 
@@ -67,6 +68,7 @@ enum EnumSharedMemoryClientCommand
 	CMD_CREATE_MULTI_BODY,
 	CMD_REQUEST_COLLISION_INFO,
 	CMD_REQUEST_MOUSE_EVENTS_DATA,
+	CMD_CHANGE_TEXTURE,
     //don't go beyond this command!
     CMD_MAX_CLIENT_COMMANDS,
     
@@ -161,6 +163,7 @@ enum EnumSharedMemoryServerStatus
 		CMD_REQUEST_COLLISION_INFO_COMPLETED,
 		CMD_REQUEST_COLLISION_INFO_FAILED,
 		CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED,
+		CMD_CHANGE_TEXTURE_COMMAND_FAILED,
         //don't go beyond 'CMD_MAX_SERVER_COMMANDS!
         CMD_MAX_SERVER_COMMANDS
 };
@@ -235,6 +238,7 @@ struct b3UserConstraint
     double m_maxAppliedForce;
     int m_userConstraintUniqueId;
 	double m_gearRatio;
+	int m_gearAuxLink;
 
 };
 

examples/SharedMemory/TinyRendererVisualShapeConverter.cpp

@@ -506,6 +506,15 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
     
 }
 
+static btVector4 sColors[4] =
+{
+	btVector4(60./256.,186./256.,84./256.,1),
+	btVector4(244./256.,194./256.,13./256.,1),
+	btVector4(219./256.,50./256.,54./256.,1),
+	btVector4(72./256.,133./256.,237./256.,1),
+
+	//btVector4(1,1,0,1),
+};
 
 
 void TinyRendererVisualShapeConverter::convertVisualShapes(
@@ -546,7 +555,17 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(
 			}
 			btTransform childTrans = vis->m_linkLocalFrame;
 
-			float rgbaColor[4] = {1,1,1,1};
+			
+
+			int colorIndex = colObj? colObj->getBroadphaseHandle()->getUid() & 3 : 0;
+
+			btVector4 color;
+			color = sColors[colorIndex];
+			float rgbaColor[4] = {color[0],color[1],color[2],color[3]};
+			if (colObj->getCollisionShape()->getShapeType()==STATIC_PLANE_PROXYTYPE)
+			{
+				color.setValue(1,1,1,1);
+			}
 			if (model && useVisual)
 			{
 				btHashString matName(linkPtr->m_visualArray[v1].m_materialName.c_str());
@@ -1051,12 +1070,15 @@ void TinyRendererVisualShapeConverter::resetAll()
 void TinyRendererVisualShapeConverter::activateShapeTexture(int shapeUniqueId, int textureUniqueId)
 {
     btAssert(textureUniqueId < m_data->m_textures.size());
+	if (textureUniqueId>=0 && textureUniqueId<m_data->m_textures.size())
+	{
 		TinyRendererObjectArray** ptrptr = m_data->m_swRenderInstances.getAtIndex(shapeUniqueId);
 		if (ptrptr && *ptrptr)
 		{
 			TinyRendererObjectArray* ptr = *ptrptr;
 			ptr->m_renderObjects[0]->m_model->setDiffuseTextureFromData(m_data->m_textures[textureUniqueId].textureData,m_data->m_textures[textureUniqueId].m_width,m_data->m_textures[textureUniqueId].m_height);
 		}
+	}
 }
 
 void TinyRendererVisualShapeConverter::activateShapeTexture(int objectUniqueId, int jointIndex, int shapeIndex, int textureUniqueId)
@@ -1065,12 +1087,19 @@ void TinyRendererVisualShapeConverter::activateShapeTexture(int objectUniqueId,
     for (int i = 0; i < m_data->m_visualShapes.size(); i++)
     {
         if (m_data->m_visualShapes[i].m_objectUniqueId == objectUniqueId && m_data->m_visualShapes[i].m_linkIndex == jointIndex)
+        {
+			if (shapeIndex<0)
+			{
+				activateShapeTexture(i, textureUniqueId);		
+			} else
 			{
 	            start = i;
 		        break;
 			}
         }
-    
+    }
+    if (shapeIndex>=0)
+	{
 		if (start >= 0)
 		{
 			if (start + shapeIndex < m_data->m_visualShapes.size())
@@ -1078,6 +1107,7 @@ void TinyRendererVisualShapeConverter::activateShapeTexture(int objectUniqueId,
 				activateShapeTexture(start + shapeIndex, textureUniqueId);
 			}
 		}
+	}
 }
 
 int TinyRendererVisualShapeConverter::registerTexture(unsigned char* texels, int width, int height)

examples/StandaloneMain/hellovr_opengl_main.cpp

@@ -373,8 +373,8 @@ void MyKeyboardCallback(int key, int state)
 
 #include "../SharedMemory/SharedMemoryPublic.h"
 extern bool useShadowMap;
-bool gEnableVRRenderControllers=true;
+static bool gEnableVRRenderControllers=true;
-
+static bool gEnableVRRendering = true;
 
 
 void VRPhysicsServerVisualizerFlagCallback(int flag, bool enable)
@@ -391,7 +391,10 @@ void VRPhysicsServerVisualizerFlagCallback(int flag, bool enable)
 	{
 		gEnableVRRenderControllers = enable;
 	}
-	
+	if (flag == COV_ENABLE_RENDERING)
+	{
+		gEnableVRRendering = enable;
+	}
 
 
     if (flag == COV_ENABLE_WIREFRAME)
@@ -887,12 +890,17 @@ void CMainApplication::RunMainLoop()
 	while ( !bQuit && !m_app->m_window->requestedExit())
 	{
 		b3ChromeUtilsEnableProfiling();
+		if (gEnableVRRendering)
 		{
 			B3_PROFILE("main");
 
 			bQuit = HandleInput();
 
 			RenderFrame();
+		} else
+		{
+			b3Clock::usleep(0);
+			sExample->updateGraphics();
 		}
 	}
 

examples/Tutorial/Tutorial.cpp

@@ -300,7 +300,6 @@ public:
 		int numBodies = 1;
 		
 		m_app->setUpAxis(1);
-		m_app->m_renderer->enableBlend(true);
 		
 		switch (m_tutorialIndex)
 		{
@@ -405,7 +404,7 @@ public:
 			{
 				int width,height,n;
 				
-				const char* filename = "data/cube.png";
+				const char* filename = "data/checker_huge.gif";
 				const unsigned char* image=0;
 				
 				const char* prefix[]={"./","../","../../","../../../","../../../../"};
@@ -426,15 +425,23 @@ public:
 			}
 			
 			//            int boxId = m_app->registerCubeShape(1,1,1,textureIndex);
-			int boxId = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_HIGH, textureIndex);
+			int sphereTransparent = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_HIGH, textureIndex);
-			b3Vector4 color = b3MakeVector4(1,1,1,0.8);
+			int sphereOpaque= m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_HIGH, textureIndex);
+			
 			b3Vector3 scaling = b3MakeVector3(SPHERE_RADIUS,SPHERE_RADIUS,SPHERE_RADIUS);
 			for (int i=0;i<m_bodies.size();i++)
 			{
+				int gfxShape = sphereOpaque;
+				b3Vector4 color = b3MakeVector4(.1,.1,1,1);
+				if (i%2)
+				{
+					color.setValue(1,.1,.1,0.1);
+					gfxShape = sphereTransparent;
+				}
 				m_bodies[i]->m_collisionShape.m_sphere.m_radius = SPHERE_RADIUS;
 				m_bodies[i]->m_collisionShape.m_type = LW_SPHERE_TYPE;
 				
-				m_bodies[i]->m_graphicsIndex = m_app->m_renderer->registerGraphicsInstance(boxId,m_bodies[i]->m_worldPose.m_position, m_bodies[i]->m_worldPose.m_orientation,color,scaling);
+				m_bodies[i]->m_graphicsIndex = m_app->m_renderer->registerGraphicsInstance(gfxShape,m_bodies[i]->m_worldPose.m_position, m_bodies[i]->m_worldPose.m_orientation,color,scaling);
 				m_app->m_renderer->writeSingleInstanceTransformToCPU(m_bodies[i]->m_worldPose.m_position, m_bodies[i]->m_worldPose.m_orientation, m_bodies[i]->m_graphicsIndex);
 			}
 		}
@@ -467,7 +474,6 @@ public:
 		m_timeSeriesCanvas0 = 0;
 		m_timeSeriesCanvas1 = 0;
 
-		m_app->m_renderer->enableBlend(false);
     }
     
     

examples/Utils/ChromeTraceUtil.cpp

@@ -250,6 +250,8 @@ void b3ChromeUtilsStopTimingsAndWriteJsonFile(const char* fileNamePrefix)
 			static int fileCounter = 0;
 			sprintf(fileName,"%s_%d.json",fileNamePrefix, fileCounter++);
 			gTimingFile = fopen(fileName,"w");
+			if (gTimingFile)
+			{
 				fprintf(gTimingFile,"{\"traceEvents\":[\n");
 				//dump the content to file
 				for (int i=0;i<BT_QUICKPROF_MAX_THREAD_COUNT;i++)
@@ -262,6 +264,11 @@ void b3ChromeUtilsStopTimingsAndWriteJsonFile(const char* fileNamePrefix)
 				}
 				fprintf(gTimingFile,"\n],\n\"displayTimeUnit\": \"ns\"}");
 				fclose(gTimingFile);
+			} else
+			{
+				b3Printf("Error opening file");
+				b3Printf(fileName);
+			}
 			gTimingFile = 0;
 }
 

examples/pybullet/examples/changeTexture.py

@@ -0,0 +1,43 @@
+import pybullet as p
+import time
+p.connect(p.GUI)
+planeUidA = p.loadURDF("plane_transparent.urdf",[0,0,0])
+planeUid = p.loadURDF("plane_transparent.urdf",[0,0,-1])
+
+texUid = p.loadTexture("tex256.png")
+
+p.changeVisualShape(planeUidA,-1,rgbaColor=[1,1,1,0.5])
+p.changeVisualShape(planeUid,-1,rgbaColor=[1,1,1,0.5])
+p.changeVisualShape(planeUid,-1, textureUniqueId = texUid)
+
+width = 256
+height = 256
+pixels = [255]*width*height*3
+colorR = 0
+colorG = 0
+colorB = 0
+
+
+#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
+#p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
+
+blue=0
+logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "renderbench.json")
+for i in range (100000):
+	p.stepSimulation()
+	for i in range (width):
+		for j in range(height):
+			pixels[(i+j*width)*3+0]=i
+			pixels[(i+j*width)*3+1]=(j+blue)%256
+			pixels[(i+j*width)*3+2]=blue
+	blue=blue+1
+	p.changeTexture(texUid, pixels,width,height)
+	start = time.time()
+	p.getCameraImage(300,300,renderer=p.ER_BULLET_HARDWARE_OPENGL)
+	end = time.time()
+	print("rendering duraction")
+	print(end-start)
+p.stopStateLogging(logId)
+#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
+#p.configureDebugVisualizer(p.COV_ENABLE_GUI,1)
+

examples/pybullet/examples/createMultiBodyLinks.py

@@ -49,5 +49,8 @@ for i in range (p.getNumJoints(sphereUid)):
 	p.getJointInfo(sphereUid,i)
 	
 while (1):
+	keys = p.getKeyboardEvents()
+	print(keys)
+
 	time.sleep(0.01)
 	

examples/pybullet/examples/createObstacleCourse.py

@@ -0,0 +1,93 @@
+import pybullet as p
+import time
+import math
+
+p.connect(p.GUI)
+#don't create a ground plane, to allow for gaps etc
+p.resetSimulation()
+#p.createCollisionShape(p.GEOM_PLANE)
+#p.createMultiBody(0,0)
+#p.resetDebugVisualizerCamera(5,75,-26,[0,0,1]);
+p.resetDebugVisualizerCamera(15,-346,-16,[-15,0,1]);
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
+
+sphereRadius = 0.05
+colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
+stoneId = p.createCollisionShape(p.GEOM_MESH,fileName="stone.obj")
+
+boxHalfLength = 0.5
+boxHalfWidth = 2.5
+boxHalfHeight = 0.1
+segmentLength = 5
+
+colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[boxHalfLength,boxHalfWidth,boxHalfHeight])
+
+mass = 1
+visualShapeId = -1
+
+segmentStart = 0
+
+for i in range (segmentLength):
+	p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1])
+	segmentStart=segmentStart-1
+	
+for i in range (segmentLength):
+	height = 0
+	if (i%2):
+		height=1
+	p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1+height])
+	segmentStart=segmentStart-1
+
+baseOrientation = p.getQuaternionFromEuler([math.pi/2.,0,math.pi/2.])
+
+for i in range (segmentLength):
+	p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1])
+	segmentStart=segmentStart-1
+	if (i%2):
+		p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,i%3,-0.1+height+boxHalfWidth],baseOrientation=baseOrientation)
+
+for i in range (segmentLength):
+	p.createMultiBody(baseMass=0,baseCollisionShapeIndex = colBoxId,basePosition = [segmentStart,0,-0.1])
+	width=4
+	for j in range (width):
+		p.createMultiBody(baseMass=0,baseCollisionShapeIndex = stoneId,basePosition = [segmentStart,0.5*(i%2)+j-width/2.,0])
+	segmentStart=segmentStart-1
+
+
+link_Masses=[1]
+linkCollisionShapeIndices=[colBoxId]
+linkVisualShapeIndices=[-1]
+linkPositions=[[0,0,0]]
+linkOrientations=[[0,0,0,1]]
+linkInertialFramePositions=[[0,0,0]]
+linkInertialFrameOrientations=[[0,0,0,1]]
+indices=[0]
+jointTypes=[p.JOINT_REVOLUTE]
+axis=[[1,0,0]]
+
+baseOrientation = [0,0,0,1]
+for i in range (segmentLength):
+	boxId = p.createMultiBody(0,colSphereId,-1,[segmentStart,0,-0.1],baseOrientation,linkMasses=link_Masses,linkCollisionShapeIndices=linkCollisionShapeIndices,linkVisualShapeIndices=linkVisualShapeIndices,linkPositions=linkPositions,linkOrientations=linkOrientations,linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations,linkParentIndices=indices,linkJointTypes=jointTypes,linkJointAxis=axis)			
+	p.changeDynamics(boxId,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0)
+	print(p.getNumJoints(boxId))
+	for joint in range (p.getNumJoints(boxId)):
+		targetVelocity = 1
+		if (i%2):
+			targetVelocity =-1
+		p.setJointMotorControl2(boxId,joint,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=10)
+	segmentStart=segmentStart-1.1
+
+
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
+while (1):
+	camData = p.getDebugVisualizerCamera()
+	viewMat = camData[2]
+	projMat = camData[3]
+	p.getCameraImage(256,256,viewMatrix=viewMat, projectionMatrix=projMat, renderer=p.ER_BULLET_HARDWARE_OPENGL)
+	keys = p.getKeyboardEvents()
+	p.stepSimulation()	
+	#print(keys)
+	time.sleep(0.01)
+	

examples/pybullet/examples/createSphereMultiBodies.py

@@ -4,7 +4,11 @@ import time
 useMaximalCoordinates = 0
 
 p.connect(p.GUI)
-p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates)
+#p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates)
+monastryId = concaveEnv =p.createCollisionShape(p.GEOM_MESH,fileName="samurai_monastry.obj",flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
+orn = p.getQuaternionFromEuler([1.5707963,0,0])
+p.createMultiBody (0,monastryId, baseOrientation=orn)
+
 sphereRadius = 0.05
 colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
 colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])
@@ -28,5 +32,7 @@ p.setGravity(0,0,-10)
 p.setRealTimeSimulation(1)
 
 while (1):
+	keys = p.getKeyboardEvents()
+	#print(keys)
 	time.sleep(0.01)
 	

examples/pybullet/examples/loadingBench.py

@@ -0,0 +1,28 @@
+import pybullet as p
+import time
+p.connect(p.GUI)
+
+p.resetSimulation()
+timinglog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "loadingBenchVR.json")
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
+print("load plane.urdf")
+p.loadURDF("plane.urdf")
+print("load r2d2.urdf")
+
+p.loadURDF("r2d2.urdf",0,0,1)
+print("load kitchen/1.sdf")
+p.loadSDF("kitchens/1.sdf")
+print("load 100 times plate.urdf")
+for i in range (100):
+	p.loadURDF("dinnerware/plate.urdf",0,i,1)
+
+p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
+
+p.stopStateLogging(timinglog)
+print("stopped state logging")
+p.getCameraImage(320,200)
+
+while (1):
+	p.stepSimulation()
+
+

examples/pybullet/examples/racecar_differential.py

@@ -7,16 +7,15 @@ if (cid<0):
 	
 p.resetSimulation()
 p.setGravity(0,0,-10)
-p.setPhysicsEngineParameter(numSolverIterations=1000)
 useRealTimeSim = 1
 
 #for video recording (works best on Mac and Linux, not well on Windows)
 #p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4")
 p.setRealTimeSimulation(useRealTimeSim) # either this
-#p.loadURDF("plane.urdf")
+p.loadURDF("plane.urdf")
-p.loadSDF("stadium.sdf")
+#p.loadSDF("stadium.sdf")
 
-car = p.loadURDF("racecar/racecar_differential.urdf")#, [0,0,2],useFixedBase=True)
+car = p.loadURDF("racecar/racecar_differential.urdf") #, [0,0,2],useFixedBase=True)
 for i in range (p.getNumJoints(car)):
 	print (p.getJointInfo(car,i))
 for wheel in range(p.getNumJoints(car)):
@@ -27,7 +26,6 @@ wheels = [8,15]
 print("----------------")
 
 #p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10)
-
 c = p.createConstraint(car,9,car,11,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
 p.changeConstraint(c,gearRatio=1, maxForce=10000)
 
@@ -40,12 +38,17 @@ p.changeConstraint(c,gearRatio=-1, maxForce=10000)
 c = p.createConstraint(car,16,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
 p.changeConstraint(c,gearRatio=1, maxForce=10000)
 
-c = p.createConstraint(car,17,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
-p.changeConstraint(c,gearRatio=-1, maxForce=10000)
 
 c = p.createConstraint(car,16,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
 p.changeConstraint(c,gearRatio=-1, maxForce=10000)
 
+c = p.createConstraint(car,17,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+p.changeConstraint(c,gearRatio=-1, maxForce=10000)
+
+c = p.createConstraint(car,1,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15, maxForce=10000)
+c = p.createConstraint(car,3,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15,maxForce=10000)
 
 
 steering = [0,2]

examples/pybullet/examples/transparent.py

@@ -0,0 +1,18 @@
+import pybullet as p
+import time
+p.connect(p.GUI)
+p.loadURDF("plane.urdf")
+sphereUid = p.loadURDF("sphere_transparent.urdf",[0,0,2])
+
+redSlider = p.addUserDebugParameter("red",0,1,1)
+greenSlider = p.addUserDebugParameter("green",0,1,0)
+blueSlider = p.addUserDebugParameter("blue",0,1,0)
+alphaSlider = p.addUserDebugParameter("alpha",0,1,0.5)
+
+while (1):
+	red = p.readUserDebugParameter(redSlider)
+	green = p.readUserDebugParameter(greenSlider)
+	blue = p.readUserDebugParameter(blueSlider)
+	alpha = p.readUserDebugParameter(alphaSlider)
+	p.changeVisualShape(sphereUid,-1,rgbaColor=[red,green,blue,alpha])
+	time.sleep(0.01)

examples/pybullet/examples/vr_racecar_differential.py

@@ -0,0 +1,93 @@
+import pybullet as p
+import time
+CONTROLLER_ID = 0
+POSITION=1
+ORIENTATION=2
+BUTTONS=6
+
+
+cid = p.connect(p.SHARED_MEMORY)
+if (cid<0):
+	p.connect(p.GUI)
+	
+p.resetSimulation()
+p.setGravity(0,0,-10)
+useRealTimeSim = 1
+
+#for video recording (works best on Mac and Linux, not well on Windows)
+#p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4")
+p.setRealTimeSimulation(useRealTimeSim) # either this
+p.loadURDF("plane.urdf")
+#p.loadSDF("stadium.sdf")
+
+car = p.loadURDF("racecar/racecar_differential.urdf") #, [0,0,2],useFixedBase=True)
+for i in range (p.getNumJoints(car)):
+	print (p.getJointInfo(car,i))
+for wheel in range(p.getNumJoints(car)):
+		p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
+		p.getJointInfo(car,wheel)	
+
+wheels = [8,15]
+print("----------------")
+
+#p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10)
+c = p.createConstraint(car,9,car,11,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+p.changeConstraint(c,gearRatio=1, maxForce=10000)
+
+c = p.createConstraint(car,10,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+p.changeConstraint(c,gearRatio=-1, maxForce=10000)
+
+c = p.createConstraint(car,9,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+p.changeConstraint(c,gearRatio=-1, maxForce=10000)
+
+c = p.createConstraint(car,16,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+p.changeConstraint(c,gearRatio=1, maxForce=10000)
+
+
+c = p.createConstraint(car,16,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+p.changeConstraint(c,gearRatio=-1, maxForce=10000)
+
+c = p.createConstraint(car,17,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+p.changeConstraint(c,gearRatio=-1, maxForce=10000)
+
+c = p.createConstraint(car,1,car,18,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15, maxForce=10000)
+c = p.createConstraint(car,3,car,19,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
+p.changeConstraint(c,gearRatio=-1, gearAuxLink = 15,maxForce=10000)
+
+
+steering = [0,2]
+
+targetVelocitySlider = p.addUserDebugParameter("wheelVelocity",-50,50,0)
+maxForceSlider = p.addUserDebugParameter("maxForce",0,50,20)
+steeringSlider = p.addUserDebugParameter("steering",-1,1,0)
+activeController = -1
+
+while (True):
+			
+	
+	maxForce = p.readUserDebugParameter(maxForceSlider)
+	targetVelocity = p.readUserDebugParameter(targetVelocitySlider)
+	steeringAngle = p.readUserDebugParameter(steeringSlider)
+	#print(targetVelocity)
+	events = p.getVREvents()
+	for e in events:
+		if (e[BUTTONS][33]&p.VR_BUTTON_WAS_TRIGGERED):
+			activeController = e[CONTROLLER_ID]
+		if (activeController == e[CONTROLLER_ID]):
+			orn = e[2]
+			eul = p.getEulerFromQuaternion(orn)
+			steeringAngle=eul[0]
+			
+			targetVelocity = 20.0*e[3]
+		
+	for wheel in wheels:
+		p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=targetVelocity,force=maxForce)
+		
+	for steer in steering:
+		p.setJointMotorControl2(car,steer,p.POSITION_CONTROL,targetPosition=-steeringAngle)
+		
+	steering
+	if (useRealTimeSim==0):
+		p.stepSimulation()
+	time.sleep(0.01)

examples/pybullet/examples/widows.py

@@ -0,0 +1,21 @@
+import pybullet as p
+import time
+
+p.connect(p.GUI)
+p.loadURDF("table/table.urdf", 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
+p.setGravity(0,0,-10)
+arm = p.loadURDF("widowx/widowx.urdf",useFixedBase=True)
+
+p.resetBasePositionAndOrientation(arm,[-0.098612,-0.000726,-0.194018],[0.000000,0.000000,0.000000,1.000000])
+  
+	
+while (1):
+	p.stepSimulation()
+	time.sleep(0.01)
+	#p.saveWorld("test.py")
+	viewMat = p.getDebugVisualizerCamera()[2]
+	#projMatrix = [0.7499999403953552, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0000200271606445, -1.0, 0.0, 0.0, -0.02000020071864128, 0.0]
+	projMatrix = p.getDebugVisualizerCamera()[3]
+	width=640
+	height=480
+	img_arr = p.getCameraImage(width=width,height=height,viewMatrix=viewMat,projectionMatrix=projMatrix)

examples/pybullet/gym/envs/bullet/cartpole_bullet.py

@@ -22,10 +22,14 @@ class CartPoleBulletEnv(gym.Env):
     'video.frames_per_second' : 50
   }
 
-  def __init__(self):
+  def __init__(self, renders=True):
     # start the bullet physics server
+    self._renders = renders
+    if (renders):
 	    p.connect(p.GUI)
-    #p.connect(p.DIRECT)
+    else:
+    	p.connect(p.DIRECT)
+
     observation_high = np.array([
           np.finfo(np.float32).max,
           np.finfo(np.float32).max,
@@ -33,7 +37,7 @@ class CartPoleBulletEnv(gym.Env):
           np.finfo(np.float32).max])
     action_high = np.array([0.1])
 
-    self.action_space = spaces.Discrete(5)
+    self.action_space = spaces.Discrete(9)
     self.observation_space = spaces.Box(-observation_high, observation_high)
 
     self.theta_threshold_radians = 1
@@ -56,8 +60,8 @@ class CartPoleBulletEnv(gym.Env):
     self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
     theta, theta_dot, x, x_dot = self.state
     
-    dv = 0.4
+    dv = 0.1
-    deltav = [-2.*dv, -dv, 0, dv, 2.*dv][action]
+    deltav = [-10.*dv,-5.*dv, -2.*dv, -0.1*dv, 0, 0.1*dv, 2.*dv,5.*dv, 10.*dv][action]
     
     p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(deltav + self.state[3]))
 

examples/pybullet/gym/train_pybullet_cartpole.py

@@ -12,7 +12,7 @@ def callback(lcl, glb):
 
 def main():
 	
-    env = gym.make('CartPoleBulletEnv-v0')
+    env = CartPoleBulletEnv(renders=False)
     model = deepq.models.mlp([64])
     act = deepq.learn(
         env,

examples/pybullet/pybullet.c

@@ -1433,7 +1433,7 @@ static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* ar
 			int i;
 			for (i = 0; i < numControlledDofs; i++)
 			{
-				int jointIndex = pybullet_internalGetFloatFromSequence(jointIndicesSeq, i);
+				int jointIndex = pybullet_internalGetIntFromSequence(jointIndicesSeq, i);
 				if ((jointIndex >= numJoints) || (jointIndex < 0))
 				{
 					Py_DECREF(jointIndicesSeq);
@@ -4551,6 +4551,75 @@ static PyObject* pybullet_changeVisualShape(PyObject* self, PyObject* args, PyOb
 	return Py_None;
 }
 
+
+static PyObject* pybullet_changeTexture(PyObject* self, PyObject* args, PyObject* keywds)
+{
+	b3SharedMemoryCommandHandle commandHandle = 0;
+	b3SharedMemoryStatusHandle statusHandle=0;
+	int statusType = -1;
+	int textureUniqueId = -1;
+	int physicsClientId = 0;
+	int width=-1;
+	int height=-1;
+	
+	PyObject* pixelsObj = 0;
+
+	b3PhysicsClientHandle sm = 0;
+	static char* kwlist[] = {"textureUniqueId", "pixels", "width", "height", "physicsClientId", NULL};
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOii|i", kwlist, &textureUniqueId, &pixelsObj, &width, &height, &physicsClientId))
+	{
+		return NULL;
+	}
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0)
+	{
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+
+	if (textureUniqueId>=0 && width>=0 && height>=0 && pixelsObj)
+	{
+		PyObject* seqPixels = PySequence_Fast(pixelsObj, "expected a sequence");
+		PyObject* item;
+		int i;
+		int numPixels = width*height;
+		unsigned char* pixelBuffer = (unsigned char*) malloc (numPixels*3);
+		if (PyList_Check(seqPixels))
+		{
+			for (i=0;i<numPixels*3;i++)
+			{
+				item = PyList_GET_ITEM(seqPixels, i);
+				pixelBuffer[i] = PyLong_AsLong(item);
+			}
+		} else
+		{
+			for (i=0;i<numPixels*3;i++)
+			{
+				item = PyTuple_GET_ITEM(seqPixels, i);
+				pixelBuffer[i] = PyLong_AsLong(item);
+			}
+		}		
+
+		commandHandle = b3CreateChangeTextureCommandInit(sm,textureUniqueId, width,height,(const char*) pixelBuffer);
+		free(pixelBuffer);
+		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+		statusType = b3GetStatusType(statusHandle);
+		if (statusType == CMD_CLIENT_COMMAND_COMPLETED)
+		{
+			Py_INCREF(Py_None);
+			return Py_None;
+		} else
+		{
+			PyErr_SetString(SpamError, "Error processing changeTexture.");
+			return NULL;
+		}
+	}
+
+	PyErr_SetString(SpamError, "Error: invalid arguments in changeTexture.");
+	return NULL;
+}
+
+
 static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args, PyObject* keywds)
 {
 	const char* filename = 0;
@@ -4578,16 +4647,14 @@ static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args, PyObject*
 		statusType = b3GetStatusType(statusHandle);
 		if (statusType == CMD_LOAD_TEXTURE_COMPLETED)
 		{
-		}
+			PyObject* item;
-		else
+			item = PyInt_FromLong(b3GetStatusTextureUniqueId(statusHandle));
-		{
+			return item;
-			PyErr_SetString(SpamError, "Error loading texture");
-			return NULL;
 		}
 	}
 
-	Py_INCREF(Py_None);
+	PyErr_SetString(SpamError, "Error loading texture");
-	return Py_None;
+	return NULL;
 }
 
 static PyObject* MyConvertContactPoint(struct b3ContactInformation* contactPointPtr)
@@ -4804,11 +4871,12 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
 
 static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, PyObject* keywds)
 {
-	static char* kwlist[] = {"userConstraintUniqueId", "jointChildPivot", "jointChildFrameOrientation", "maxForce", "gearRatio", "physicsClientId", NULL};
+	static char* kwlist[] = {"userConstraintUniqueId", "jointChildPivot", "jointChildFrameOrientation", "maxForce", "gearRatio", "gearAuxLink", "physicsClientId", NULL};
 	int userConstraintUniqueId = -1;
 	b3SharedMemoryCommandHandle commandHandle;
 	b3SharedMemoryStatusHandle statusHandle;
 	int statusType;
+	int gearAuxLink = -1;
 	int physicsClientId = 0;
 	b3PhysicsClientHandle sm = 0;
 	PyObject* jointChildPivotObj = 0;
@@ -4817,7 +4885,7 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P
 	double jointChildFrameOrn[4];
 	double maxForce = -1;
 	double gearRatio = 0;
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOddi", kwlist, &userConstraintUniqueId, &jointChildPivotObj, &jointChildFrameOrnObj, &maxForce, &gearRatio, &physicsClientId))
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOddii", kwlist, &userConstraintUniqueId, &jointChildPivotObj, &jointChildFrameOrnObj, &maxForce, &gearRatio, &gearAuxLink, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -4847,6 +4915,10 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P
 	{
 		b3InitChangeUserConstraintSetGearRatio(commandHandle,gearRatio);
 	}
+	if (gearAuxLink>=0)
+	{
+		b3InitChangeUserConstraintSetGearAuxLink(commandHandle,gearAuxLink);
+	}
 	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
 	statusType = b3GetStatusType(statusHandle);
 	Py_INCREF(Py_None);
@@ -5008,7 +5080,7 @@ static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, P
 			pybullet_internalSetVectord(planeNormalObj,planeNormal);
 			shapeIndex = b3CreateCollisionShapeAddPlane(commandHandle, planeNormal, planeConstant);
 		}
-		if (shapeIndex && flags)
+		if (shapeIndex>=0 && flags)
 		{
 			b3CreateCollisionSetFlag(commandHandle,shapeIndex,flags);
 		}
@@ -5180,14 +5252,16 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje
 				double linkJointAxis[3];
 				double linkInertialFramePosition[3];
 				double linkInertialFrameOrientation[4];
+				int linkParentIndex;
+				int linkJointType;
 
 				pybullet_internalGetVector3FromSequence(seqLinkInertialFramePositions,i,linkInertialFramePosition);
 				pybullet_internalGetVector4FromSequence(linkInertialFrameOrientationObj,i,linkInertialFrameOrientation);
 				pybullet_internalGetVector3FromSequence(seqLinkPositions,i,linkPosition);
 				pybullet_internalGetVector4FromSequence(seqLinkOrientations,i,linkOrientation);
 				pybullet_internalGetVector3FromSequence(seqLinkJoinAxis,i,linkJointAxis);
-				int linkParentIndex = pybullet_internalGetIntFromSequence(seqLinkParentIndices,i);
+				linkParentIndex = pybullet_internalGetIntFromSequence(seqLinkParentIndices,i);
-				int linkJointType = pybullet_internalGetIntFromSequence(seqLinkJointTypes,i);
+				linkJointType = pybullet_internalGetIntFromSequence(seqLinkJointTypes,i);
 
 				b3CreateMultiBodyLink(commandHandle, 
 									linkMass, 
@@ -7019,6 +7093,9 @@ static PyMethodDef SpamMethods[] = {
 	{"loadTexture", (PyCFunction)pybullet_loadTexture, METH_VARARGS | METH_KEYWORDS,
 	 "Load texture file."},
 
+	{"changeTexture", (PyCFunction)pybullet_changeTexture, METH_VARARGS | METH_KEYWORDS,
+	 "Change a texture file."},
+
 	{"getQuaternionFromEuler", pybullet_getQuaternionFromEuler, METH_VARARGS,
 	 "Convert Euler [roll, pitch, yaw] as in URDF/SDF convention, to "
 	 "quaternion [x,y,z,w]"},
@@ -7147,14 +7224,22 @@ PyMODINIT_FUNC
 #if PY_MAJOR_VERSION >= 3
 PyInit_pybullet(void)
 #else
+#ifdef BT_USE_EGL
+initpybullet_egl(void)
+#else
 initpybullet(void)
+#endif //BT_USE_EGL
 #endif
 {
 	PyObject* m;
 #if PY_MAJOR_VERSION >= 3
 	m = PyModule_Create(&moduledef);
+#else
+#ifdef BT_USE_EGL
+	m = Py_InitModule3("pybullet_egl", SpamMethods, "Python bindings for Bullet");
 #else
 	m = Py_InitModule3("pybullet", SpamMethods, "Python bindings for Bullet");
+#endif //BT_USE_EGL
 #endif
 
 #if PY_MAJOR_VERSION >= 3
@@ -7275,6 +7360,8 @@ initpybullet(void)
 	PyModule_AddIntConstant(m, "GEOM_PLANE", GEOM_PLANE);
 	PyModule_AddIntConstant(m, "GEOM_CAPSULE", GEOM_CAPSULE);
 
+	PyModule_AddIntConstant(m, "GEOM_FORCE_CONCAVE_TRIMESH", GEOM_FORCE_CONCAVE_TRIMESH);
+
 
 
 	SpamError = PyErr_NewException("pybullet.error", NULL, NULL);

setup.py

@@ -386,6 +386,7 @@ if _platform == "linux" or _platform == "linux2":
     sources = sources + ["examples/ThirdPartyLibs/enet/unix.c"]\
     +["examples/OpenGLWindow/X11OpenGLWindow.cpp"]\
     +["examples/ThirdPartyLibs/Glew/glew.c"]
+    include_dirs += ["examples/ThirdPartyLibs/optionalX11"]
 elif _platform == "win32":
     print("win32!")
     libraries = ['Ws2_32','Winmm','User32','Opengl32','kernel32','glu32','Gdi32','Comdlg32']
@@ -419,7 +420,7 @@ else:
 
 setup(
 	name = 'pybullet',
-	version='1.1.8',
+	version='1.2.2',
 	description='Official Python Interface for the Bullet Physics SDK Robotics Simulator',
 	long_description='pybullet is an easy to use Python module for physics simulation, robotics and machine learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
 	url='https://github.com/bulletphysics/bullet3',

src/BulletDynamics/Featherstone/btMultiBodyConstraint.h

@@ -184,6 +184,9 @@ public:
 	virtual void debugDraw(class btIDebugDraw* drawer)=0;
 
 	virtual void setGearRatio(btScalar ratio) {}
+	virtual void setGearAuxLink(int gearAuxLink) {}
+
+	
 };
 
 #endif //BT_MULTIBODY_CONSTRAINT_H

src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp

@@ -22,7 +22,8 @@ subject to the following restrictions:
 
 btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
 	:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,1,false),
-	m_gearRatio(1)
+	m_gearRatio(1),
+	m_gearAuxLink(-1)
 {
    
 }
@@ -121,11 +122,18 @@ void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray&
         int dof = 0;
         btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
         btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
+		btScalar auxVel = 0;
+		
+		if (m_gearAuxLink>=0)
+		{
+			auxVel = m_bodyA->getJointVelMultiDof(m_gearAuxLink)[dof];
+		}
+		currentVelocity += auxVel;
 			
 		//btScalar positionStabiliationTerm = erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
         //btScalar velocityError = (m_desiredVelocity - currentVelocity);
 
-        btScalar desiredRelativeVelocity =   0;
+        btScalar desiredRelativeVelocity =   auxVel;
     
 		fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,desiredRelativeVelocity);
 

src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h

@@ -31,10 +31,11 @@ protected:
   btMatrix3x3     m_frameInA;
   btMatrix3x3     m_frameInB;
 	btScalar			m_gearRatio;
+	int			m_gearAuxLink;
 	
 public:
 
-	btMultiBodyGearConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
+	//btMultiBodyGearConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
 	btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
 
 	virtual ~btMultiBodyGearConstraint();
@@ -97,6 +98,10 @@ public:
 	{
 		m_gearRatio = gearRatio;
 	}
+	virtual void setGearAuxLink(int gearAuxLink)
+	{
+		m_gearAuxLink = gearAuxLink;
+	}
 
 };
 

src/LinearMath/btHashMap.h

@@ -389,26 +389,36 @@ protected:
 	const Value* getAtIndex(int index) const
 	{
 		btAssert(index < m_valueArray.size());
-
+		btAssert(index>=0);
+		if (index>=0 && index < m_valueArray.size())
+		{
 			return &m_valueArray[index];
 		}
+		return 0;
+	}
 
 	Value* getAtIndex(int index)
 	{
 		btAssert(index < m_valueArray.size());
-
+		btAssert(index>=0);
+		if (index>=0 && index < m_valueArray.size())
+		{
 			return &m_valueArray[index];
 		}
+		return 0;
+	}
 
     Key getKeyAtIndex(int index)
     {
         btAssert(index < m_keyArray.size());
+		btAssert(index>=0);
 		return m_keyArray[index];
     }
     
     const Key getKeyAtIndex(int index) const
     {
         btAssert(index < m_keyArray.size());
+		btAssert(index>=0);
 		return m_keyArray[index];
    }