Merge branch 'master' of https://github.com/erwincoumans/bullet3

data/cube_small.urdf

@@ -3,7 +3,6 @@
   <link name="baseLink">
     <contact>
       <lateral_friction value="1.0"/>
-      <spinning_friction value="0.1"/>
       <inertia_scaling value="3.0"/>
     </contact>
     <inertial>

data/jenga/jenga.mtl

@@ -0,0 +1,16 @@
+newmtl jenga
+  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_Ka jenga.tga
+  map_Kd jenga.png
+  
+  
+

data/jenga/jenga.obj

@@ -0,0 +1,113 @@
+# jenga.obj
+#
+
+o jenga
+mtllib jenga.mtl
+
+v -0.5  -0.5  0.5
+v 0.5  -0.5  0.5
+v 0.5   0.5  0.5
+v -0.5   0.5  0.5
+
+v -0.5  -0.5  -0.5
+v 0.5  -0.5  -0.5
+v 0.5   0.5  -0.5
+v -0.5   0.5  -0.5
+
+v -0.5  -0.5  -0.5
+v -0.5  0.5  -0.5
+v -0.5   0.5  0.5
+v -0.5   -0.5  0.5
+
+v 0.5  -0.5  -0.5
+v 0.5  0.5  -0.5
+v 0.5   0.5  0.5
+v 0.5   -0.5  0.5
+v -0.5  -0.5   -0.5
+v -0.5  -0.5  0.5
+v 0.5  -0.5   0.5
+v 0.5 -0.5   -0.5
+v -0.5  0.5   -0.5
+v -0.5  0.5  0.5
+v 0.5  0.5   0.5
+v 0.5 0.5   -0.5
+
+vt 	0 1
+vt 	0 0.75
+vt 	0.25 0.75
+vt 	0.25 1
+
+vt 	0.25 0.5
+vt 	0.25 0.75
+vt 	0.5 0.75
+vt 	0.5 0.5
+
+vt 	1 0.75
+vt 	0.75 0.75
+vt 	0.75 1
+vt 	1 1
+
+vt 	0.25 0.75
+vt 	0.5 0.75
+vt 	0.5 1
+vt 	0.25 1
+
+vt 	0 0.5
+vt 	0 0.75
+vt 	0.25 0.75
+vt 	0.25 0.5
+
+
+vt 	0.75 0.75
+vt 	0.75 1
+vt 	0.5 1
+vt 	0.5 0.75
+
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn 0 0 -1
+vn -1 0 0
+vn -1 0 0
+vn -1 0 0
+vn -1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 1 0 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 -1 0
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+vn 0 1 0
+
+
+g jenga
+usemtl jenga
+s 1
+f 1/1/1 2/2/2 3/3/3
+f 1/1/1 3/3/3 4/4/4
+s 2
+f 7/7/7 6/6/6 5/5/5
+f 8/8/8 7/7/7 5/5/5 
+s 3
+f 11/11/11 10/10/10 9/9/9
+f 12/12/12 11/11/11 9/9/9
+s 4
+f 13/13/13 14/14/14 15/15/15
+f 13/13/13 15/15/15 16/16/16
+s 5
+f 19/19/19 18/18/18 17/17/17
+f 20/20/20 19/19/19 17/17/17
+s 6
+f 21/21/21 22/22/22 23/23/23
+f 21/21/21 23/23/23 24/24/24
+
+	

data/jenga/jenga.urdf

@@ -0,0 +1,31 @@
+<?xml version="0.0" ?>
+<robot name="jenga.urdf">
+  <link name="baseLink">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <spinning_friction value="0.5"/>
+      <inertia_scaling value="3.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value="1.0"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+				<mesh filename="jenga.obj" scale="0.15 0.05 0.03"/>
+      </geometry>
+       <material name="blue">
+        <color rgba="0.5 0.5 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+	 	<box size="0.15 0.05 0.03"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
+

data/plane.urdf

@@ -1,6 +1,6 @@
 <?xml version="0.0" ?>
 <robot name="cube.urdf">
-  <link name="baseLink">
+  <link name="planeLink">
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0"/>
        <mass value=".0"/>

data/pr2_gripper.urdf

@@ -17,7 +17,7 @@
       <origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
     </collision>
     <inertial>
-      <mass value="0.05"/>
+      <mass value="0.5"/>
       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
     </inertial>
   </link>
@@ -32,23 +32,23 @@
 
   <link name="left_gripper">
     <contact>
-      <lateral_friction value="50.0"/>
+      <lateral_friction value="1.0"/>
-      <spinning_friction value="0.1"/>
+      <spinning_friction value="1.5"/>
     </contact>
     <visual>
       <origin rpy="0.0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="l_finger.stl"/>
+        <mesh filename="l_finger_collision.stl"/>
       </geometry>
     </visual>
     <collision>
-      <geometry>
-        <mesh filename="l_finger.stl"/>
-      </geometry>
       <origin rpy="0.0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="l_finger_collision.stl"/>
+      </geometry>
     </collision>
     <inertial>
-      <mass value="0.05"/>
+      <mass value="0.1"/>
       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
     </inertial>
   </link>
@@ -60,7 +60,8 @@
 
   <link name="left_tip">
     <contact>
-      <lateral_friction value="50.0"/>
+      <lateral_friction value="1.0"/>
+      <spinning_friction value="1.5"/>
     </contact>
     <visual>
       <origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
@@ -72,10 +73,10 @@
       <geometry>
         <box size=".03 .03 .02"/>
       </geometry>
-      <origin rpy="0.0 0 0" xyz="0.109137 0.00495 0"/>
+      <origin rpy="0.0 0 0" xyz="0.105 0.00495 0"/>
     </collision>
     <inertial>
-      <mass value="0.05"/>
+      <mass value="0.1"/>
       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
     </inertial>
   </link>
@@ -90,22 +91,23 @@
 
   <link name="right_gripper">
     <contact>
-      <lateral_friction value="50.0"/>
+      <lateral_friction value="1.0"/>
+      <spinning_friction value="1.5"/>
     </contact>
     <visual>
       <origin rpy="-3.1415 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="l_finger.stl"/>
+        <mesh filename="l_finger_collision.stl"/>
       </geometry>
     </visual>
          <collision>
-      <geometry>
-        <mesh filename="l_finger.stl"/>
-      </geometry>
       <origin rpy="-3.1415 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="l_finger_collision.stl"/>
+      </geometry>
     </collision>
     <inertial>
-      <mass value="0.05"/>
+      <mass value="0.1"/>
       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
     </inertial>
   </link>
@@ -117,7 +119,8 @@
 
   <link name="right_tip">
     <contact>
-      <lateral_friction value="50.0"/>
+      <lateral_friction value="1.0"/>
+      <spinning_friction value="1.5"/>
     </contact>
     <visual>
       <origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
@@ -129,10 +132,10 @@
       <geometry>
       	<box size=".03 .03 .02"/>
       </geometry>
-      <origin rpy="-3.1415 0 0" xyz="0.109137 0.00495 0"/>
+      <origin rpy="-3.1415 0 0" xyz="0.105 0.00495 0"/>
     </collision>
     <inertial>
-      <mass value="0.05"/>
+      <mass value="0.1"/>
       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
     </inertial>
   </link>

data/samurai.urdf

@@ -1,9 +1,6 @@
 <?xml version="0.0" ?>
 <robot name="cube.urdf">
   <link concave="yes" name="baseLink">
-    <contact>
-      <rolling_friction value="0.3"/>
-    </contact>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0"/>
        <mass value=".0"/>

data/samurai_monastry.obj

@@ -331416,11 +331416,3 @@ f 30335 30336
 f 1329 1330
 f 77715 77717
 f 20619 20622
-o Cube
-v -0.500000 -0.200000 -1.000000
-v 1.000000 -0.000000 0.000000
-v -0.500000 0.200000 1.000000
-usemtl Material
-s off
-f 86188 86189 86190
-f 86189 86188 86190

data/sphere2.urdf

@@ -1,6 +1,11 @@
 <?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"/>

data/teddy_vhacd.urdf

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

examples/ExampleBrowser/OpenGLGuiHelper.cpp

@@ -505,6 +505,8 @@ void OpenGLGuiHelper::autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWor
     
 void OpenGLGuiHelper::drawText3D( const char* txt, float posX, float posY, float posZ, float size)
 {
+	B3_PROFILE("OpenGLGuiHelper::drawText3D");
+
     btAssert(m_data->m_glApp);
     m_data->m_glApp->drawText3D(txt,posX,posY,posZ,size);
 }

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -14,7 +14,7 @@ subject to the following restrictions:
 
 #include "BulletUrdfImporter.h"
 #include "../../CommonInterfaces/CommonRenderInterface.h"
-
+#include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
 #include "URDFImporterInterface.h"
 #include "btBulletCollisionCommon.h"
 #include "../ImportObjDemo/LoadMeshFromObj.h"
@@ -28,6 +28,7 @@ subject to the following restrictions:
 
 #include "../ImportMeshUtility/b3ImportMeshUtility.h"
 
+static btScalar gUrdfDefaultCollisionMargin = 0.001;
 
 #include <iostream>
 #include <fstream>
@@ -295,19 +296,59 @@ void  BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVect
 	if (linkPtr)
 	{
 		UrdfLink* link = *linkPtr;
+		btMatrix3x3 linkInertiaBasis;
+		btScalar linkMass, principalInertiaX, principalInertiaY, principalInertiaZ;
 		if (link->m_parentJoint==0 && m_data->m_urdfParser.getModel().m_overrideFixedBase)
 		{
-			mass = 0.f;
+			linkMass = 0.f;
-			localInertiaDiagonal.setValue(0,0,0);
+			principalInertiaX = 0.f;
+			principalInertiaY = 0.f;
+			principalInertiaZ = 0.f;
+			linkInertiaBasis.setIdentity();
 		}
 		else
 		{
-			mass = link->m_inertia.m_mass;
+			linkMass = link->m_inertia.m_mass;
-			localInertiaDiagonal.setValue(link->m_inertia.m_ixx,link->m_inertia.m_iyy,
+			if (link->m_inertia.m_ixy == 0.0 &&
-										  link->m_inertia.m_izz);
+			    link->m_inertia.m_ixz == 0.0 &&
+			    link->m_inertia.m_iyz == 0.0)
+			{
+				principalInertiaX = link->m_inertia.m_ixx;
+				principalInertiaY = link->m_inertia.m_iyy;
+				principalInertiaZ = link->m_inertia.m_izz;
+				linkInertiaBasis.setIdentity();
 			}
-		inertialFrame = link->m_inertia.m_linkLocalFrame;
+			else
-		
+			{
+				principalInertiaX = link->m_inertia.m_ixx;
+				btMatrix3x3 inertiaTensor(link->m_inertia.m_ixx, link->m_inertia.m_ixy, link->m_inertia.m_ixz,
+							  link->m_inertia.m_ixy, link->m_inertia.m_iyy, link->m_inertia.m_iyz,
+							  link->m_inertia.m_ixz, link->m_inertia.m_iyz, link->m_inertia.m_izz);
+				btScalar threshold = 1.0e-6;
+				int numIterations = 30;
+				inertiaTensor.diagonalize(linkInertiaBasis, threshold, numIterations);
+				principalInertiaX = inertiaTensor[0][0];
+				principalInertiaY = inertiaTensor[1][1];
+				principalInertiaZ = inertiaTensor[2][2];
+			}
+		}
+		mass = linkMass;
+		if (principalInertiaX < 0 ||
+		    principalInertiaX > (principalInertiaY + principalInertiaZ) ||
+		    principalInertiaY < 0 ||
+		    principalInertiaY > (principalInertiaX + principalInertiaZ) ||
+		    principalInertiaZ < 0 ||
+		    principalInertiaZ > (principalInertiaX + principalInertiaY))
+		{
+			b3Warning("Bad inertia tensor properties, setting inertia to zero for link: %s\n", link->m_name.c_str());
+			principalInertiaX = 0.f;
+			principalInertiaY = 0.f;
+			principalInertiaZ = 0.f;
+			linkInertiaBasis.setIdentity();
+		}
+		localInertiaDiagonal.setValue(principalInertiaX, principalInertiaY, principalInertiaZ);
+		inertialFrame.setOrigin(link->m_inertia.m_linkLocalFrame.getOrigin());
+		inertialFrame.setBasis(link->m_inertia.m_linkLocalFrame.getBasis()*linkInertiaBasis);
 	}
 	else
 	{
@@ -360,7 +401,45 @@ bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWor
     return true;
 }
 
+static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes)
+{
+	btCompoundShape* compound = new btCompoundShape();
+	btTransform identity;
+	identity.setIdentity();
 
+	for (int s = 0; s<(int)shapes.size(); s++)
+	{
+		btConvexHullShape* convexHull = new btConvexHullShape();
+		tinyobj::shape_t& shape = shapes[s];
+		int faceCount = shape.mesh.indices.size();
+
+		for (int f = 0; f<faceCount; f += 3)
+		{
+
+			btVector3 pt;
+			pt.setValue(shape.mesh.positions[shape.mesh.indices[f] * 3 + 0],
+				shape.mesh.positions[shape.mesh.indices[f] * 3 + 1],
+				shape.mesh.positions[shape.mesh.indices[f] * 3 + 2]);
+			convexHull->addPoint(pt,false);
+
+			pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 0],
+						shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 1],
+						shape.mesh.positions[shape.mesh.indices[f + 1] * 3 + 2]);
+			convexHull->addPoint(pt, false);
+
+			pt.setValue(shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 0],
+						shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 1],
+						shape.mesh.positions[shape.mesh.indices[f + 2] * 3 + 2]);
+			convexHull->addPoint(pt, false);
+		}
+
+		convexHull->recalcLocalAabb();
+		convexHull->optimizeConvexHull();
+		compound->addChildShape(identity,convexHull);
+	}
+
+	return compound;
+}
 
 btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix)
 {
@@ -386,7 +465,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
 
             }
             btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
-            cylZShape->setMargin(0.001);
+            cylZShape->setMargin(gUrdfDefaultCollisionMargin);
 			cylZShape->initializePolyhedralFeatures();
 			//btConvexShape* cylZShape = new btConeShapeZ(cyl->radius,cyl->length);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
             
@@ -403,7 +482,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
 			btBoxShape* boxShape = new btBoxShape(extents*0.5f);
 			//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
             shape = boxShape;
-			shape ->setMargin(0.001);
+			shape ->setMargin(gUrdfDefaultCollisionMargin);
             break;
         }
         case URDF_GEOM_SPHERE:
@@ -412,7 +491,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
 			btScalar radius = collision->m_geometry.m_sphereRadius;
 			btSphereShape* sphereShape = new btSphereShape(radius);
             shape = sphereShape;
-			shape ->setMargin(0.001);
+			shape ->setMargin(gUrdfDefaultCollisionMargin);
             break;
 
             break;
@@ -466,8 +545,19 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
 						switch (fileType)
 						{
                             case FILE_OBJ:
+                            {
+								if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
 								{
 									glmesh = LoadMeshFromObj(fullPath, collisionPathPrefix);
+								}
+								else
+								{
+									std::vector<tinyobj::shape_t> shapes;
+									std::string err = tinyobj::LoadObj(shapes, fullPath, collisionPathPrefix);
+									//create a convex hull for each shape, and store it in a btCompoundShape
+									shape = createConvexHullFromShapes(shapes);
+									return shape;
+								}
                                 break;
                             }
 						case FILE_STL:
@@ -600,7 +690,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
 								//cylZShape->initializePolyhedralFeatures();
 								//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
 								//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
-								cylZShape->setMargin(0.001);
+								cylZShape->setMargin(gUrdfDefaultCollisionMargin);
 								shape = cylZShape;
 							}
 						} else
@@ -659,7 +749,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
 			}
 
 			btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
-			cylZShape->setMargin(0.001);
+			cylZShape->setMargin(gUrdfDefaultCollisionMargin);
 			convexColShape = cylZShape;
 			break;
 		}
@@ -671,7 +761,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
 			btBoxShape* boxShape = new btBoxShape(extents*0.5f);
 			//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
 			convexColShape = boxShape;
-			convexColShape->setMargin(0.001);
+			convexColShape->setMargin(gUrdfDefaultCollisionMargin);
 			break;
 		}
 		case URDF_GEOM_SPHERE:
@@ -679,7 +769,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
 			btScalar radius = visual->m_geometry.m_sphereRadius;
 			btSphereShape* sphereShape = new btSphereShape(radius);
 			convexColShape = sphereShape;
-			convexColShape->setMargin(0.001);
+			convexColShape->setMargin(gUrdfDefaultCollisionMargin);
 			break;
 
 			break;
@@ -1077,7 +1167,7 @@ btCollisionShape* BulletURDFImporter::getAllocatedCollisionShape(int index)
     btCompoundShape* compoundShape = new btCompoundShape();
     m_data->m_allocatedCollisionShapes.push_back(compoundShape);
     
-    compoundShape->setMargin(0.001);
+    compoundShape->setMargin(gUrdfDefaultCollisionMargin);
 	UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
 	btAssert(linkPtr);
 	if (linkPtr)

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -162,8 +162,6 @@ void ConvertURDF2BulletInternal(
     int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex);
     btRigidBody* parentRigidBody = 0;
 
-    //std::string name = u2b.getLinkName(urdfLinkIndex);
-    //b3Printf("link name=%s urdf link index=%d\n",name.c_str(),urdfLinkIndex);
     //b3Printf("mb link index = %d\n",mbLinkIndex);
 
 	btTransform parentLocalInertialFrame;
@@ -322,6 +320,14 @@ void ConvertURDF2BulletInternal(
                         cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping;
                         cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction= jointFriction;
                         creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
+						if (jointLowerLimit <= jointUpperLimit)
+						{
+							//std::string name = u2b.getLinkName(urdfLinkIndex);
+							//printf("create btMultiBodyJointLimitConstraint for revolute link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit, jointUpperLimit);
+
+							btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit);
+							world1->addMultiBodyConstraint(con);
+						}
 
                     } else
                     {
@@ -343,8 +349,14 @@ void ConvertURDF2BulletInternal(
                                                                    -offsetInB.getOrigin(),
                                                                    disableParentCollision);
                         creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
+						if (jointLowerLimit <= jointUpperLimit)
+						{
+							//std::string name = u2b.getLinkName(urdfLinkIndex);
+							//printf("create btMultiBodyJointLimitConstraint for prismatic link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit,jointUpperLimit);
+
 							btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit);
 							world1->addMultiBodyConstraint(con);
+						}
                         //printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit);
 
                     } else
@@ -387,7 +399,8 @@ void ConvertURDF2BulletInternal(
 
                 col->setWorldTransform(tr);
 				
-                bool isDynamic = true;
+				//base and fixed? -> static, otherwise flag as dynamic
+                bool isDynamic = (mbLinkIndex<0 && cache.m_bulletMultiBody->hasFixedBase())? false : true;
                 short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
                 short collisionFilterMask = isDynamic? 	short(btBroadphaseProxy::AllFilter) : 	short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
 

examples/Importers/ImportURDFDemo/UrdfParser.cpp

@@ -816,7 +816,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
 bool UrdfParser::parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorLogger* logger)
 {
 	joint.m_lowerLimit = 0.f;
-	joint.m_upperLimit = 0.f;
+	joint.m_upperLimit = -1.f;
 	joint.m_effortLimit = 0.f;
 	joint.m_velocityLimit = 0.f;
 	joint.m_jointDamping = 0.f;

examples/Importers/ImportURDFDemo/UrdfParser.h

@@ -138,7 +138,7 @@ struct UrdfJoint
 	double m_jointFriction;
 	UrdfJoint()
 		:m_lowerLimit(0),
-		m_upperLimit(0),
+		m_upperLimit(-1),
 		m_effortLimit(0),
 		m_velocityLimit(0),
 		m_jointDamping(0),

examples/OpenGLWindow/GLPrimitiveRenderer.cpp

@@ -1,7 +1,7 @@
 #ifndef NO_OPENGL3
 #include "GLPrimitiveRenderer.h"
 #include "GLPrimInternalData.h"
-
+//#include "Bullet3Common/b3Logging.h"
 #include "LoadShader.h"
 
 #include <assert.h>
@@ -205,6 +205,7 @@ void GLPrimitiveRenderer::drawRect(float x0, float y0, float x1, float y1, float
 
 void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVertex& v1,const PrimVertex& v2,const PrimVertex& v3,float viewMat[16],float projMat[16], bool useRGBA)
 {
+	//B3_PROFILE("GLPrimitiveRenderer::drawTexturedRect3D");
 
     assert(glGetError()==GL_NO_ERROR);
    

examples/OpenGLWindow/SimpleOpenGL3App.cpp

@@ -230,7 +230,7 @@ struct sth_stash* SimpleOpenGL3App::getFontStash()
 
 void SimpleOpenGL3App::drawText3D( const char* txt, float worldPosX, float worldPosY, float worldPosZ, float size1)
 {
-
+	B3_PROFILE("SimpleOpenGL3App::drawText3D");
 	float viewMat[16];
 	float projMat[16];
 	CommonCameraInterface* cam = m_instancingRenderer->getActiveCamera();
@@ -273,6 +273,7 @@ void SimpleOpenGL3App::drawText3D( const char* txt, float worldPosX, float world
 		bool measureOnly = false;
 
 		float fontSize= 32;//64;//512;//128;
+		
 		sth_draw_text(m_data->m_fontStash,
                     m_data->m_droidRegular,fontSize,posX,posY,
 					txt,&dx, this->m_instancingRenderer->getScreenWidth(),this->m_instancingRenderer->getScreenHeight(),measureOnly,m_window->getRetinaScale());

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -28,6 +28,7 @@
 #include "SharedMemoryCommands.h"
 
 btVector3 gLastPickPos(0, 0, 0);
+bool gEnableRealTimeSimVR=false;
 
 struct UrdfLinkNameMapUtil
 {
@@ -556,7 +557,7 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor()
 	m_data = new PhysicsServerCommandProcessorInternalData();
 
 	createEmptyDynamicsWorld();
-	m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.0;
+	m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.0001;
 
 }
 
@@ -597,6 +598,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 
 	m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
 	m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.05;
+
 }
 
 void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies()
@@ -2838,7 +2840,7 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
 	m_data->m_logPlayback = pb;
 }
 
-btVector3 gVRGripperPos(0,0,0);
+btVector3 gVRGripperPos(0,0,0.2);
 btQuaternion gVRGripperOrn(0,0,0,1);
 btScalar gVRGripperAnalog = 0;
 bool gVRGripperClosed = false;
@@ -2850,10 +2852,11 @@ double gDtInSec = 0.f;
 double gSubStep = 0.f;
 void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
 {
-	if (m_data->m_allowRealTimeSimulation && m_data->m_guiHelper)
+	if ((gEnableRealTimeSimVR || m_data->m_allowRealTimeSimulation) && m_data->m_guiHelper)
 	{
 		static btAlignedObjectArray<char> gBufferServerToClient;
-		gBufferServerToClient.resize(32768);
+		gBufferServerToClient.resize(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+
 
 
 		if (!m_data->m_hasGround)
@@ -2864,6 +2867,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
 			
 
 			loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+			loadUrdf("samurai.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 
 			if (m_data->m_gripperRigidbodyFixed == 0)
 			{
@@ -2874,11 +2878,11 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
 					InteralBodyData* parentBody = m_data->getHandle(bodyId);
 					if (parentBody->m_multiBody)
 					{
-						parentBody->m_multiBody->setHasSelfCollision(1);
+						parentBody->m_multiBody->setHasSelfCollision(0);
-						btVector3 pivotInParent(0, 0, 0);
+						btVector3 pivotInParent(0.2, 0, 0);
 						btMatrix3x3 frameInParent;
-						frameInParent.setRotation(btQuaternion(0, 0, 0, 1));
+						//frameInParent.setRotation(btQuaternion(0, 0, 0, 1));
-
+						frameInParent.setIdentity();
 						btVector3 pivotInChild(0, 0, 0);
 						btMatrix3x3 frameInChild;
 						frameInChild.setIdentity();
@@ -2890,62 +2894,110 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
 							m_data->m_gripperMultiBody->setJointPos(0, 0);
 							m_data->m_gripperMultiBody->setJointPos(2, 0);
 						}
-						m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(1.);
+						m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(10000);
 						btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
 						world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed);
 					}
 				}
+#if 1
+				for (int i = 0; i < 10; i++)
+				{
+					loadUrdf("cube.urdf", btVector3(3, -2, 0.5+i), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+				}
 
 				loadUrdf("sphere2.urdf", btVector3(-2, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 				loadUrdf("sphere2.urdf", btVector3(-2, 0, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 				loadUrdf("sphere2.urdf", btVector3(-2, 0, 3), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+				loadUrdf("r2d2.urdf", btVector3(2, -2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+				
 
-				loadUrdf("samurai.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 				loadUrdf("kuka_iiwa/model.urdf", btVector3(3, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-				loadUrdf("cube_small.urdf", btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+				loadUrdf("cube_small.urdf", btVector3(0.3, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
-				loadUrdf("husky/husky.urdf", btVector3(1, 1, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+				
+				
+				//				loadUrdf("cube_small.urdf", btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+#endif
+#if 0
+				int jengaHeight = 10;
+				for (int j = 0; j < jengaHeight; j++)
+				{
+					for (int i = 0; i < 3; i++)
+					{
+						if (j & 1)
+						{
+							loadUrdf("jenga/jenga.urdf", btVector3(-0.5, 0.05*i, .03*0.5 + .03*j), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+						}
+						else
+						{
+							btQuaternion orn(btVector3(0, 0, 1), SIMD_HALF_PI);
+							loadUrdf("jenga/jenga.urdf", btVector3(-0.5 -1 / 3.*0.15  + 0.05*i, +1 / 3.*0.15,0.03*0.5 + .03*j), orn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+						}
+					}
+				}
+#endif
+			
+				//loadUrdf("nao/nao.urdf", btVector3(2,5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+				loadUrdf("husky/husky.urdf", btVector3(5, 2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 				m_data->m_huskyId = bodyId;
 				
 			}
+			loadSdf("kiva_shelf/model.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true);
+			loadUrdf("teddy_vhacd.urdf", btVector3(1, 1, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+
+			m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
+
 		}
 
 		if (m_data->m_gripperRigidbodyFixed && m_data->m_gripperMultiBody)
 		{
 			m_data->m_gripperRigidbodyFixed->setFrameInB(btMatrix3x3(gVRGripperOrn));
 			m_data->m_gripperRigidbodyFixed->setPivotInB(gVRGripperPos);
-			if (m_data->m_gripperMultiBody->getNumLinks() > 2)
+			for (int i = 0; i < m_data->m_gripperMultiBody->getNumLinks(); i++)
 			{
-				for (int i = 0; i < 2; i++)
+				if (supportsJointMotor(m_data->m_gripperMultiBody, i))
 				{
-					if (supportsJointMotor(m_data->m_gripperMultiBody, i * 2))
+					btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_data->m_gripperMultiBody->getLink(i ).m_userPtr;
-					{
-						btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_data->m_gripperMultiBody->getLink(i * 2).m_userPtr;
 					if (motor)
 					{
-							motor->setErp(0.01);
+						motor->setErp(0.2);
+						btScalar posTarget = 0.1 + (1 - btMin(btScalar(0.75),gVRGripperAnalog)*btScalar(1.5))*SIMD_HALF_PI*0.29;
+						btScalar maxPosTarget = 0.55;
 						
-							motor->setPositionTarget(0.1+(1-gVRGripperAnalog)*SIMD_HALF_PI*0.5, 1);
+						if (m_data->m_gripperMultiBody->getJointPos(i) < 0)
+						{
+							m_data->m_gripperMultiBody->setJointPos(i,0);
+						}
+						if (m_data->m_gripperMultiBody->getJointPos(i) > maxPosTarget)
+						{
+							m_data->m_gripperMultiBody->setJointPos(i, maxPosTarget);
+						}
 
-							
+						motor->setPositionTarget(posTarget, 1);
-							motor->setVelocityTarget(0, 0.1);
+						motor->setVelocityTarget(0, 0.5);
-							btScalar maxImp = 1550.*m_data->m_physicsDeltaTime;
+						btScalar maxImp = 1*m_data->m_physicsDeltaTime;
 						motor->setMaxAppliedImpulse(maxImp);
 					}
 				}
 			}
 		}
+
+		int maxSteps = m_data->m_numSimulationSubSteps+3;
+		if (m_data->m_numSimulationSubSteps)
+		{
+			gSubStep = m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps;
+		}
+		else
+		{
+			gSubStep = m_data->m_physicsDeltaTime;
 		}
 		
-		int maxSteps = 3;
+		int numSteps = m_data->m_dynamicsWorld->stepSimulation(dtInSec,maxSteps, gSubStep);
-
-		int numSteps = m_data->m_dynamicsWorld->stepSimulation(dtInSec,maxSteps,m_data->m_physicsDeltaTime);
 		gDroppedSimulationSteps += numSteps > maxSteps ? numSteps - maxSteps : 0;
 
 		if (numSteps)
 		{
 			gNumSteps = numSteps;
 			gDtInSec = dtInSec;
-			gSubStep = m_data->m_physicsDeltaTime;
 		}
 	}
 }

examples/SharedMemory/PhysicsServerExample.cpp

@@ -19,6 +19,10 @@
 extern btVector3 gLastPickPos;
 btVector3 gVRTeleportPos(0,0,0);
 btQuaternion gVRTeleportOrn(0, 0, 0,1);
+extern btVector3 gVRGripperPos;
+extern btQuaternion gVRGripperOrn;
+extern btScalar gVRGripperAnalog;
+extern bool gEnableRealTimeSimVR;
 
 extern bool gVRGripperClosed;
 
@@ -893,6 +897,8 @@ extern double gSubStep;
 
 void PhysicsServerExample::renderScene()
 {
+	B3_PROFILE("PhysicsServerExample::RenderScene");
+
 	///debug rendering
 	//m_args[0].m_cs->lock();
 	
@@ -934,6 +940,8 @@ void PhysicsServerExample::renderScene()
 	if (gDebugRenderToggle)
 	if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
 	{
+		gEnableRealTimeSimVR = true;
+		B3_PROFILE("Draw Debug HUD");
 		//some little experiment to add text/HUD to a VR camera (HTC Vive/Oculus Rift)
 
 		static int frameCount=0;
@@ -1154,16 +1162,12 @@ void	PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
 	}
 }
 
-extern btVector3 gVRGripperPos;
-extern btQuaternion gVRGripperOrn;
-extern btScalar gVRGripperAnalog;
-
 
 
 void	PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4], float analogAxis)
 {
 
-	
+	gEnableRealTimeSimVR = true;
 
 	if (controllerId <= 0 || controllerId >= MAX_VR_CONTROLLERS)
 	{

examples/StandaloneMain/hellovr_opengl_main.cpp

@@ -1,4 +1,5 @@
 #ifdef BT_ENABLE_VR
+//#define BT_USE_CUSTOM_PROFILER
 //========= Copyright Valve Corporation ============//
 
 #include "../OpenGLWindow/SimpleOpenGL3App.h"
@@ -701,11 +702,12 @@ bool CMainApplication::HandleInput()
 							{
 								glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
 								///todo(erwincoumans) can't use reguar debug drawer, because physics/graphics are not in sync
+								//so it can (and likely will) cause crashes
 								//add a special debug drawer that deals with this
-								//gDebugDrawFlags = btIDebugDraw::DBG_DrawWireframe;// :DBG_DrawContactPoints
+									//gDebugDrawFlags = btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints+
 									//btIDebugDraw::DBG_DrawConstraintLimits+
 									//btIDebugDraw::DBG_DrawConstraints
-									;
+									//;
 							}
 
 							sExample->vrControllerButtonCallback(unDevice, button, 1, pos, orn);
@@ -751,7 +753,7 @@ bool CMainApplication::HandleInput()
 				}
 			} 
 
-			m_rbShowTrackedDevice[ unDevice ] = state.ulButtonPressed == 0;
+//			m_rbShowTrackedDevice[ unDevice ] = state.ulButtonPressed == 0;
 		}
 		sPrevStates[unDevice] = state;
 	}
@@ -769,6 +771,8 @@ void CMainApplication::RunMainLoop()
 
 	while ( !bQuit && !m_app->m_window->requestedExit())
 	{
+		B3_PROFILE("main");
+
 		bQuit = HandleInput();
 
 		RenderFrame();
@@ -812,9 +816,15 @@ void CMainApplication::RenderFrame()
 	// for now as fast as possible
 	if ( m_pHMD )
 	{
+		{
+			B3_PROFILE("DrawControllers");
 			DrawControllers();
+		}
 		RenderStereoTargets();
+		{
+			B3_PROFILE("RenderDistortion");
 			RenderDistortion();
+		}
 
 		vr::Texture_t leftEyeTexture = {(void*)leftEyeDesc.m_nResolveTextureId, vr::API_OpenGL, vr::ColorSpace_Gamma };
 		vr::VRCompositor()->Submit(vr::Eye_Left, &leftEyeTexture );
@@ -824,6 +834,7 @@ void CMainApplication::RenderFrame()
 
 	if ( m_bVblank && m_bGlFinishHack )
 	{
+		B3_PROFILE("bGlFinishHack");
 		//$ HACKHACK. From gpuview profiling, it looks like there is a bug where two renders and a present
 		// happen right before and after the vsync causing all kinds of jittering issues. This glFinish()
 		// appears to clear that up. Temporary fix while I try to get nvidia to investigate this problem.
@@ -833,6 +844,7 @@ void CMainApplication::RenderFrame()
 
 	// SwapWindow
 	{
+		B3_PROFILE("m_app->swapBuffer");
 		m_app->swapBuffer();
 		//SDL_GL_SwapWindow( m_pWindow );
 		
@@ -840,6 +852,7 @@ void CMainApplication::RenderFrame()
 
 	// Clear
 	{
+		B3_PROFILE("glClearColor");
 		// We want to make sure the glFinish waits for the entire present to complete, not just the submission
 		// of the command. So, we do a clear here right here so the glFinish will wait fully for the swap.
 		glClearColor( 0, 0, 0, 1 );
@@ -849,6 +862,8 @@ void CMainApplication::RenderFrame()
 	// Flush and wait for swap.
 	if ( m_bVblank )
 	{
+		B3_PROFILE("glFlushglFinish");
+
 		glFlush();
 		glFinish();
 	}
@@ -856,14 +871,19 @@ void CMainApplication::RenderFrame()
 	// Spew out the controller and pose count whenever they change.
 	if ( m_iTrackedControllerCount != m_iTrackedControllerCount_Last || m_iValidPoseCount != m_iValidPoseCount_Last )
 	{
+		B3_PROFILE("debug pose");
+
 		m_iValidPoseCount_Last = m_iValidPoseCount;
 		m_iTrackedControllerCount_Last = m_iTrackedControllerCount;
 		
 		b3Printf( "PoseCount:%d(%s) Controllers:%d\n", m_iValidPoseCount, m_strPoseClasses.c_str(), m_iTrackedControllerCount );
 	}
 
+	{
+		B3_PROFILE("UpdateHMDMatrixPose");
 		UpdateHMDMatrixPose();
 	}
+}
 
 
 //-----------------------------------------------------------------------------
@@ -1565,6 +1585,8 @@ void CMainApplication::SetupDistortion()
 //-----------------------------------------------------------------------------
 void CMainApplication::RenderStereoTargets()
 {
+	B3_PROFILE("CMainApplication::RenderStereoTargets");
+
 	sExample->stepSimulation(1./60.);
 
 	glClearColor( 0.15f, 0.15f, 0.18f, 1.0f ); // nice background color, but not black
@@ -1641,7 +1663,7 @@ void CMainApplication::RenderStereoTargets()
 	{
 		sExample->physicsDebugDraw(gDebugDrawFlags);
 	} 
-	
+	else
 	{
 		sExample->renderScene();
 	}
@@ -1692,7 +1714,7 @@ void CMainApplication::RenderStereoTargets()
 	{
 		sExample->physicsDebugDraw(gDebugDrawFlags);
 	} 
-
+	else
 	{
 		sExample->renderScene();
 	}
@@ -1720,6 +1742,8 @@ void CMainApplication::RenderStereoTargets()
 //-----------------------------------------------------------------------------
 void CMainApplication::RenderScene( vr::Hmd_Eye nEye )
 {
+	B3_PROFILE("RenderScene");
+
 	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 	glEnable(GL_DEPTH_TEST);
 
@@ -1868,11 +1892,16 @@ void CMainApplication::UpdateHMDMatrixPose()
 {
 	if (!m_pHMD)
 		return;
-
+	{
+		B3_PROFILE("WaitGetPoses");
 		vr::VRCompositor()->WaitGetPoses(m_rTrackedDevicePose, vr::k_unMaxTrackedDeviceCount, NULL, 0);
+	}
 
 	m_iValidPoseCount = 0;
 	m_strPoseClasses = "";
+	{
+		B3_PROFILE("for loop");
+
 		for (int nDevice = 0; nDevice < vr::k_unMaxTrackedDeviceCount; ++nDevice)
 		{
 			if (m_rTrackedDevicePose[nDevice].bPoseIsValid)
@@ -1894,12 +1923,16 @@ void CMainApplication::UpdateHMDMatrixPose()
 				m_strPoseClasses += m_rDevClassChar[nDevice];
 			}
 		}
+	}
+	{
+		B3_PROFILE("m_mat4HMDPose invert");
 
 		if (m_rTrackedDevicePose[vr::k_unTrackedDeviceIndex_Hmd].bPoseIsValid)
 		{
 			m_mat4HMDPose = m_rmat4DevicePose[vr::k_unTrackedDeviceIndex_Hmd].invert();
 		}
 	}
+}
 
 
 //-----------------------------------------------------------------------------
@@ -2145,6 +2178,11 @@ void CGLRenderModel::Draw()
 //-----------------------------------------------------------------------------
 int main(int argc, char *argv[])
 {
+#ifdef BT_USE_CUSTOM_PROFILER
+	//b3SetCustomEnterProfileZoneFunc(...);
+	//b3SetCustomLeaveProfileZoneFunc(...);
+#endif
+
 	CMainApplication *pMainApplication = new CMainApplication( argc, argv );
 
 	if (!pMainApplication->BInit())
@@ -2157,6 +2195,10 @@ int main(int argc, char *argv[])
 
 	pMainApplication->Shutdown();
 
+#ifdef BT_USE_CUSTOM_PROFILER
+//...
+#endif
+
 	return 0;
 }
 #endif //BT_ENABLE_VR

src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp

@@ -372,10 +372,10 @@ void* btHashedOverlappingPairCache::removeOverlappingPair(btBroadphaseProxy* pro
 	return userData;
 }
 //#include <stdio.h>
-
+#include "LinearMath/btQuickprof.h"
 void	btHashedOverlappingPairCache::processAllOverlappingPairs(btOverlapCallback* callback,btDispatcher* dispatcher)
 {
-
+	BT_PROFILE("btHashedOverlappingPairCache::processAllOverlappingPairs");
 	int i;
 
 //	printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());

src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp

@@ -16,7 +16,7 @@ subject to the following restrictions:
 
 
 #include "btCollisionDispatcher.h"
-
+#include "LinearMath/btQuickprof.h"
 
 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
 
@@ -227,6 +227,8 @@ public:
 
 	virtual bool	processOverlap(btBroadphasePair& pair)
 	{
+		BT_PROFILE("btCollisionDispatcher::processOverlap");
+
 		(*m_dispatcher->getNearCallback())(pair,*m_dispatcher,m_dispatchInfo);
 
 		return false;
@@ -249,7 +251,6 @@ void	btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pa
 
 
 
-
 //by default, Bullet will use this near callback
 void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo)
 {

src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp

@@ -15,6 +15,7 @@ subject to the following restrictions:
 */
 
 #include "btCompoundCompoundCollisionAlgorithm.h"
+#include "LinearMath/btQuickprof.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
 #include "BulletCollision/BroadphaseCollision/btDbvt.h"
@@ -124,6 +125,7 @@ struct	btCompoundCompoundLeafCallback : btDbvt::ICollide
 	
 	void		Process(const btDbvtNode* leaf0,const btDbvtNode* leaf1)
 	{
+		BT_PROFILE("btCompoundCompoundLeafCallback::Process");
 		m_numOverlapPairs++;
 
 

src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp

@@ -15,6 +15,7 @@ subject to the following restrictions:
 
 
 #include "btConvexConcaveCollisionAlgorithm.h"
+#include "LinearMath/btQuickprof.h"
 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
@@ -79,6 +80,7 @@ void	btConvexTriangleCallback::clearCache()
 void btConvexTriangleCallback::processTriangle(btVector3* triangle,int
 partId, int triangleIndex)
 {
+	BT_PROFILE("btConvexTriangleCallback::processTriangle");
 
 	if (!TestTriangleAgainstAabb2(triangle, m_aabbMin, m_aabbMax))
 	{
@@ -184,7 +186,7 @@ void btConvexConcaveCollisionAlgorithm::clearCache()
 
 void btConvexConcaveCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
 {
-	
+	BT_PROFILE("btConvexConcaveCollisionAlgorithm::processCollision");
 	
 	const btCollisionObjectWrapper* convexBodyWrap = m_isSwapped ? body1Wrap : body0Wrap;
 	const btCollisionObjectWrapper* triBodyWrap = m_isSwapped ? body0Wrap : body1Wrap;
@@ -265,6 +267,7 @@ btScalar btConvexConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionObj
 		
 		virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex)
 		{
+			BT_PROFILE("processTriangle");
 			(void)partId;
 			(void)triangleIndex;
 			//do a swept sphere for now

src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp

@@ -245,16 +245,18 @@ void	btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co
 		btStridingMeshInterface*	m_meshInterface;
 		btTriangleCallback*		m_callback;
 		btVector3				m_triangle[3];
-
+		int m_numOverlap;
 
 		MyNodeOverlapCallback(btTriangleCallback* callback,btStridingMeshInterface* meshInterface)
 			:m_meshInterface(meshInterface),
-			m_callback(callback)
+			m_callback(callback),
+			m_numOverlap(0)
 		{
 		}
 				
 		virtual void processNode(int nodeSubPart, int nodeTriangleIndex)
 		{
+			m_numOverlap++;
 			const unsigned char *vertexbase;
 			int numverts;
 			PHY_ScalarType type;
@@ -322,7 +324,6 @@ void	btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,co
 
 	m_bvh->reportAabbOverlappingNodex(&myNodeCallback,aabbMin,aabbMax);
 	
-
 #endif//DISABLE_BVH
 
 

src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.cpp

@@ -39,7 +39,7 @@ subject to the following restrictions:
 //temp globals, to improve GJK/EPA/penetration calculations
 int gNumDeepPenetrationChecks = 0;
 int gNumGjkChecks = 0;
-
+btScalar gGjkEpaPenetrationTolerance = 0.001;
 
 btGjkPairDetector::btGjkPairDetector(const btConvexShape* objectA,const btConvexShape* objectB,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver*	penetrationDepthSolver)
 :m_cachedSeparatingAxis(btScalar(0.),btScalar(1.),btScalar(0.)),
@@ -304,7 +304,7 @@ void btGjkPairDetector::getClosestPointsNonVirtual(const ClosestPointInput& inpu
 		}
 
 		bool catchDegeneratePenetrationCase = 
-			(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < 0.01));
+			(m_catchDegeneracies && m_penetrationDepthSolver && m_degenerateSimplex && ((distance+margin) < gGjkEpaPenetrationTolerance));
 
 		//if (checkPenetration && !isValid)
 		if (checkPenetration && (!isValid || catchDegeneratePenetrationCase ))