remove that odd triangle in the origin of samurai castle (VR)

add rolling/spinning friction to cube, remove it from plane/samurai.urdf
URDF2Bullet: support joint limits for revolute and prismatic, only if defined (if upper < lower, disable limit)
add some profiling markers to improve performance

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/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="100"/>
       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
     </inertial>
   </link>
@@ -41,12 +41,6 @@
         <mesh filename="l_finger.stl"/>
       </geometry>
     </visual>
-    <collision>
-      <geometry>
-        <mesh filename="l_finger.stl"/>
-      </geometry>
-      <origin rpy="0.0 0 0" xyz="0 0 0"/>
-    </collision>
     <inertial>
       <mass value="0.05"/>
       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
@@ -61,6 +55,7 @@
   <link name="left_tip">
     <contact>
       <lateral_friction value="50.0"/>
+      <spinning_friction value="0.1"/>
     </contact>
     <visual>
       <origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
@@ -70,9 +65,9 @@
     </visual>
     <collision>
       <geometry>
-        <box size=".03 .03 .02"/>
+        <box size=".13 .03 .02"/>
       </geometry>
-      <origin rpy="0.0 0 0" xyz="0.109137 0.00495 0"/>
+      <origin rpy="0.0 0 0" xyz="0.055 0.00495 0"/>
     </collision>
     <inertial>
       <mass value="0.05"/>
@@ -91,6 +86,7 @@
   <link name="right_gripper">
     <contact>
       <lateral_friction value="50.0"/>
+      <spinning_friction value="0.1"/>
     </contact>
     <visual>
       <origin rpy="-3.1415 0 0" xyz="0 0 0"/>
@@ -98,12 +94,6 @@
         <mesh filename="l_finger.stl"/>
       </geometry>
     </visual>
-    <collision>
-      <geometry>
-        <mesh filename="l_finger.stl"/>
-      </geometry>
-      <origin rpy="-3.1415 0 0" xyz="0 0 0"/>
-    </collision>
     <inertial>
       <mass value="0.05"/>
       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
@@ -118,6 +108,7 @@
   <link name="right_tip">
     <contact>
       <lateral_friction value="50.0"/>
+      <spinning_friction value="0.1"/>
     </contact>
     <visual>
       <origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
@@ -127,9 +118,9 @@
     </visual>
     <collision>
       <geometry>
-        <box size=".03 .03 .02"/>
+        <box size=".13 .03 .02"/>
       </geometry>
-      <origin rpy="-3.1415 0 0" xyz="0.109137 0.00495 0"/>
+      <origin rpy="-3.1415 0 0" xyz="0.055 0.00495 0"/>
     </collision>
     <inertial>
       <mass value="0.05"/>

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"
@@ -360,7 +360,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)
 {
@@ -467,7 +505,18 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
 						{
                             case FILE_OBJ:
                             {
-                                glmesh = LoadMeshFromObj(fullPath,collisionPathPrefix);
+								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:

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);
-                        btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody,mbLinkIndex,jointLowerLimit, jointUpperLimit);
+						if (jointLowerLimit <= jointUpperLimit)
-                        world1->addMultiBodyConstraint(con);
+						{
+							//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
 {
@@ -596,7 +597,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
 
 
 	m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
-	m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.05;
+	m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08;
 }
 
 void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies()
@@ -2850,10 +2851,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 +2866,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 +2877,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 +2893,109 @@ 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.2, 0.2, 1), 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
+//#define JENGA 1
+#ifdef JENGA
+				int jengaHeight = 17;
+				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, 0.025*i, .0151*0.5 + .015*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.075  + 0.025*i, +1 / 3.*0.075,0.0151*0.5 + .015*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());
+
 		}
 
 		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;
+					if (motor)
 					{
-						btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_data->m_gripperMultiBody->getLink(i * 2).m_userPtr;
+						motor->setErp(0.1);
-						if (motor)
+						btScalar posTarget = 0.1 + (1 - btMin(0.75,gVRGripperAnalog)*1.5)*SIMD_HALF_PI*0.29;
+						btScalar maxPosTarget = 0.55;
+						
+						if (m_data->m_gripperMultiBody->getJointPos(i) < 0)
 						{
-							motor->setErp(0.01);
+							m_data->m_gripperMultiBody->setJointPos(i,0);
-
-							motor->setPositionTarget(0.1+(1-gVRGripperAnalog)*SIMD_HALF_PI*0.5, 1);
-							
-							
-							motor->setVelocityTarget(0, 0.1);
-							btScalar maxImp = 1550.*m_data->m_physicsDeltaTime;
-							motor->setMaxAppliedImpulse(maxImp);
 						}
+						if (m_data->m_gripperMultiBody->getJointPos(i) > maxPosTarget)
+						{
+							m_data->m_gripperMultiBody->setJointPos(i, maxPosTarget);
+						}
+
+						motor->setPositionTarget(posTarget, 1);
+						motor->setVelocityTarget(0, 0.5);
+						btScalar maxImp = 500*m_data->m_physicsDeltaTime;
+						motor->setMaxAppliedImpulse(maxImp);
 					}
 				}
 			}
 		}
 
-		int maxSteps = 3;
+		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 numSteps = m_data->m_dynamicsWorld->stepSimulation(dtInSec,maxSteps,m_data->m_physicsDeltaTime);
+		int numSteps = m_data->m_dynamicsWorld->stepSimulation(dtInSec,maxSteps, gSubStep);
 		gDroppedSimulationSteps += numSteps > maxSteps ? numSteps - maxSteps : 0;
 
 		if (numSteps)
 		{
 			gNumSteps = numSteps;
 			gDtInSec = dtInSec;
-			gSubStep = m_data->m_physicsDeltaTime;
 		}
 	}
 }

examples/SharedMemory/PhysicsServerExample.cpp

@@ -893,6 +893,8 @@ extern double gSubStep;
 
 void PhysicsServerExample::renderScene()
 {
+	B3_PROFILE("PhysicsServerExample::RenderScene");
+
 	///debug rendering
 	//m_args[0].m_cs->lock();
 	
@@ -934,6 +936,7 @@ void PhysicsServerExample::renderScene()
 	if (gDebugRenderToggle)
 	if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
 	{
+		B3_PROFILE("Draw Debug HUD");
 		//some little experiment to add text/HUD to a VR camera (HTC Vive/Oculus Rift)
 
 		static int frameCount=0;
@@ -1157,13 +1160,13 @@ void	PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
 extern btVector3 gVRGripperPos;
 extern btQuaternion gVRGripperOrn;
 extern btScalar gVRGripperAnalog;
-
+extern bool gEnableRealTimeSimVR;
 
 
 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_DrawConstraintLimits+
-									//btIDebugDraw::DBG_DrawConstraints
+								//	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 )
 	{
-		DrawControllers();
+		{
+			B3_PROFILE("DrawControllers");
+			DrawControllers();
+		}
 		RenderStereoTargets();
-		RenderDistortion();
+		{
+			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,13 +871,18 @@ 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 );
 	}
 
-	UpdateHMDMatrixPose();
+	{
+		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);
 
@@ -1866,38 +1890,47 @@ Matrix4 CMainApplication::GetCurrentViewProjectionMatrix( vr::Hmd_Eye nEye )
 //-----------------------------------------------------------------------------
 void CMainApplication::UpdateHMDMatrixPose()
 {
-	if ( !m_pHMD )
+	if (!m_pHMD)
 		return;
-
+	{
-	vr::VRCompositor()->WaitGetPoses(m_rTrackedDevicePose, vr::k_unMaxTrackedDeviceCount, NULL, 0 );
+		B3_PROFILE("WaitGetPoses");
+		vr::VRCompositor()->WaitGetPoses(m_rTrackedDevicePose, vr::k_unMaxTrackedDeviceCount, NULL, 0);
+	}
 
 	m_iValidPoseCount = 0;
 	m_strPoseClasses = "";
-	for ( int nDevice = 0; nDevice < vr::k_unMaxTrackedDeviceCount; ++nDevice )
 	{
-		if ( m_rTrackedDevicePose[nDevice].bPoseIsValid )
+		B3_PROFILE("for loop");
+
+		for (int nDevice = 0; nDevice < vr::k_unMaxTrackedDeviceCount; ++nDevice)
 		{
-			m_iValidPoseCount++;
+			if (m_rTrackedDevicePose[nDevice].bPoseIsValid)
-			m_rmat4DevicePose[nDevice] = ConvertSteamVRMatrixToMatrix4( m_rTrackedDevicePose[nDevice].mDeviceToAbsoluteTracking );
-			if (m_rDevClassChar[nDevice]==0)
 			{
-				switch (m_pHMD->GetTrackedDeviceClass(nDevice))
+				m_iValidPoseCount++;
+				m_rmat4DevicePose[nDevice] = ConvertSteamVRMatrixToMatrix4(m_rTrackedDevicePose[nDevice].mDeviceToAbsoluteTracking);
+				if (m_rDevClassChar[nDevice] == 0)
 				{
-				case vr::TrackedDeviceClass_Controller:        m_rDevClassChar[nDevice] = 'C'; break;
+					switch (m_pHMD->GetTrackedDeviceClass(nDevice))
-				case vr::TrackedDeviceClass_HMD:               m_rDevClassChar[nDevice] = 'H'; break;
+					{
-				case vr::TrackedDeviceClass_Invalid:           m_rDevClassChar[nDevice] = 'I'; break;
+					case vr::TrackedDeviceClass_Controller:        m_rDevClassChar[nDevice] = 'C'; break;
-				case vr::TrackedDeviceClass_Other:             m_rDevClassChar[nDevice] = 'O'; break;
+					case vr::TrackedDeviceClass_HMD:               m_rDevClassChar[nDevice] = 'H'; break;
-				case vr::TrackedDeviceClass_TrackingReference: m_rDevClassChar[nDevice] = 'T'; break;
+					case vr::TrackedDeviceClass_Invalid:           m_rDevClassChar[nDevice] = 'I'; break;
-				default:                                       m_rDevClassChar[nDevice] = '?'; break;
+					case vr::TrackedDeviceClass_Other:             m_rDevClassChar[nDevice] = 'O'; break;
+					case vr::TrackedDeviceClass_TrackingReference: m_rDevClassChar[nDevice] = 'T'; break;
+					default:                                       m_rDevClassChar[nDevice] = '?'; break;
+					}
 				}
+				m_strPoseClasses += m_rDevClassChar[nDevice];
 			}
-			m_strPoseClasses += m_rDevClassChar[nDevice];
 		}
 	}
-
-	if ( m_rTrackedDevicePose[vr::k_unTrackedDeviceIndex_Hmd].bPoseIsValid )
 	{
-		m_mat4HMDPose = m_rmat4DevicePose[vr::k_unTrackedDeviceIndex_Hmd].invert();
+		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