From 9ecc1cc4853bc70e868847a66dd57abb381d3f46 Mon Sep 17 00:00:00 2001
From: erwincoumans <erwin.coumans@gmail.com>
Date: Fri, 22 Feb 2019 09:17:55 -0800
Subject: [PATCH] Implement CustomProfilerCallback, hooking up to Bullet
 profiling test dominoes

---
 .../physx/PhysXServerCommandProcessor.cpp     | 21 +++++++
 .../pybullet/examples/otherPhysicsEngine.py   | 57 +++++++++++++++----
 2 files changed, 67 insertions(+), 11 deletions(-)

diff --git a/examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp b/examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp
index aa2687a23..89c24e8a4 100644
--- a/examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp
+++ b/examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp
@@ -50,7 +50,25 @@ public:
 	}
 };
 
+class CustomProfilerCallback : public physx::PxProfilerCallback
+{
+public:
+	virtual ~CustomProfilerCallback() {}
 
+	virtual void* zoneStart(const char* eventName, bool detached, uint64_t contextId)
+	{
+		b3EnterProfileZone(eventName);
+		return 0;
+	}
+
+	virtual void zoneEnd(void* profilerData, const char* eventName, bool detached, uint64_t contextId)
+	{
+		b3LeaveProfileZone();
+	}
+
+};
+
+static CustomProfilerCallback gCustomProfilerCallback;
 
 
 struct InternalPhysXBodyData
@@ -292,6 +310,9 @@ bool PhysXServerCommandProcessor::connect()
 	{
 		
 		m_data->m_foundation = PxCreateFoundation(PX_PHYSICS_VERSION, m_data->m_allocator, m_data->m_errorCallback);
+		// This call should be performed after PVD is initialized, otherwise it will have no effect.
+		PxSetProfilerCallback(&gCustomProfilerCallback);
+
 		m_data->m_physics = PxCreatePhysics(PX_PHYSICS_VERSION, *m_data->m_foundation, physx::PxTolerancesScale(), true, 0);
 		m_data->m_cooking = PxCreateCooking(PX_PHYSICS_VERSION, *m_data->m_foundation, physx::PxCookingParams(physx::PxTolerancesScale()));
 
diff --git a/examples/pybullet/examples/otherPhysicsEngine.py b/examples/pybullet/examples/otherPhysicsEngine.py
index 4bff90a05..93f9fbe44 100644
--- a/examples/pybullet/examples/otherPhysicsEngine.py
+++ b/examples/pybullet/examples/otherPhysicsEngine.py
@@ -4,27 +4,61 @@ import time
 import math
 
 usePhysX = True
+useMaximalCoordinates = True
 if usePhysX:
-  p.connect(p.PhysX,options="--numCores=1 --solver=pgs")
+  p.connect(p.PhysX,options="--numCores=8 --solver=pgs")
   p.loadPlugin("eglRendererPlugin")
 else:
   p.connect(p.GUI)
 
-useMaximalCoordinates = False
+p.setPhysicsEngineParameter(fixedTimeStep=1./240.,numSolverIterations=4, minimumSolverIslandSize=1024)
+p.setPhysicsEngineParameter(contactBreakingThreshold=0.01)
+
 p.setAdditionalSearchPath(pd.getDataPath())
-p.loadURDF("plane.urdf", useMaximalCoordinates=useMaximalCoordinates)
+#Always make ground plane maximal coordinates, to avoid performance drop in PhysX
+#See https://github.com/NVIDIAGameWorks/PhysX/issues/71
+
+p.loadURDF("plane.urdf", useMaximalCoordinates=True)#useMaximalCoordinates)
 p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
 p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
 logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"physx_create_dominoes.json")
-for j in range (50):
-    for i in range (100):
-        sphere = p.loadURDF("domino/domino.urdf",[i*0.04,1+j*.25,0.03], useMaximalCoordinates=useMaximalCoordinates)
+jran = 50
+iran = 100
+
+num=64
+radius=0.1
+numDominoes=0
+
+for i in range (int(num*50)):
+  num=(radius*2*math.pi)/0.08
+  radius += 0.05/float(num)
+  orn = p.getQuaternionFromEuler([0,0,0.5*math.pi+math.pi*2*i/float(num)])
+  pos = [radius*math.cos(2*math.pi*(i/float(num))),radius*math.sin(2*math.pi*(i/float(num))), 0.03]
+  sphere = p.loadURDF("domino/domino.urdf",pos, orn, useMaximalCoordinates=useMaximalCoordinates)
+  numDominoes+=1
+  
+pos=[pos[0],pos[1],pos[2]+0.3]
+orn = p.getQuaternionFromEuler([0,0,-math.pi/4.])
+sphere = p.loadURDF("domino/domino.urdf",pos, orn, useMaximalCoordinates=useMaximalCoordinates)
+
+print("numDominoes=",numDominoes)
+
+
+#for j in range (20):
+#    for i in range (100):
+#        if (i<99):
+#          sphere = p.loadURDF("domino/domino.urdf",[i*0.04,1+j*.25,0.03], useMaximalCoordinates=useMaximalCoordinates)
+#        else:
+#          orn = p.getQuaternionFromEuler([0,-3.14*0.24,0])
+#          sphere = p.loadURDF("domino/domino.urdf",[(i-1)*0.04,1+j*.25,0.03], orn, useMaximalCoordinates=useMaximalCoordinates)
+    
+    
 print("loaded!")
-p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
 
-p.changeDynamics(sphere ,-1, mass=1000)
 
-door = p.loadURDF("door.urdf",[0,0,1])
+#p.changeDynamics(sphere ,-1, mass=1000)
+
+door = p.loadURDF("door.urdf",[0,0,-11])
 p.changeDynamics(door ,1, linearDamping=0, angularDamping=0, jointDamping=0, mass=1)
 print("numJoints = ", p.getNumJoints(door))
 
@@ -43,8 +77,9 @@ angle = math.pi*0.5
 count=0
 while (1):
   count+=1
-  if (count==10):
+  if (count==12):
       p.stopStateLogging(logId)
+      p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
 
   curTime = time.time()
   
@@ -61,4 +96,4 @@ while (1):
   #contacts = p.getContactPoints()
   #print("contacts=",contacts)
   p.stepSimulation()
-  time.sleep(1./240.)
+  #time.sleep(1./240.)