From 3f52d237edbf2cc7d810782f8b4c623d6d5e200f Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Thu, 31 Mar 2016 10:27:17 -0700
Subject: [PATCH] adding Bullet profiler instances for better stats

---
 libraries/physics/src/EntityMotionState.cpp   |  2 ++
 libraries/physics/src/PhysicsEngine.cpp       |  1 +
 .../physics/src/ThreadSafeDynamicsWorld.cpp   | 26 ++++++++++++++++++-
 .../physics/src/ThreadSafeDynamicsWorld.h     |  1 +
 4 files changed, 29 insertions(+), 1 deletion(-)

diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp
index e3952ba1d6..7b83c2e2ce 100644
--- a/libraries/physics/src/EntityMotionState.cpp
+++ b/libraries/physics/src/EntityMotionState.cpp
@@ -175,11 +175,13 @@ bool EntityMotionState::isMoving() const {
 // (2) at the beginning of each simulation step for KINEMATIC RigidBody's --
 //     it is an opportunity for outside code to update the object's simulation position
 void EntityMotionState::getWorldTransform(btTransform& worldTrans) const {
+    BT_PROFILE("getWorldTransform");
     if (!_entity) {
         return;
     }
     assert(entityTreeIsLocked());
     if (_motionType == MOTION_TYPE_KINEMATIC) {
+        BT_PROFILE("kinematicIntegration");
         // This is physical kinematic motion which steps strictly by the subframe count
         // of the physics simulation.
         uint32_t thisStep = ObjectMotionState::getWorldSimulationStep();
diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp
index 0040c19c3d..74f6f90900 100644
--- a/libraries/physics/src/PhysicsEngine.cpp
+++ b/libraries/physics/src/PhysicsEngine.cpp
@@ -240,6 +240,7 @@ void PhysicsEngine::stepSimulation() {
     float timeStep = btMin(dt, MAX_TIMESTEP);
 
     if (_myAvatarController) {
+        BT_PROFILE("avatarController");
         // TODO: move this stuff outside and in front of stepSimulation, because
         // the updateShapeIfNecessary() call needs info from MyAvatar and should
         // be done on the main thread during the pre-simulation stuff
diff --git a/libraries/physics/src/ThreadSafeDynamicsWorld.cpp b/libraries/physics/src/ThreadSafeDynamicsWorld.cpp
index b6f3487f1a..5fe99f137c 100644
--- a/libraries/physics/src/ThreadSafeDynamicsWorld.cpp
+++ b/libraries/physics/src/ThreadSafeDynamicsWorld.cpp
@@ -67,7 +67,10 @@ int ThreadSafeDynamicsWorld::stepSimulationWithSubstepCallback(btScalar timeStep
 
         saveKinematicState(fixedTimeStep*clampedSimulationSteps);
 
-        applyGravity();
+        {
+            BT_PROFILE("applyGravity");
+            applyGravity();
+        }
 
         for (int i=0;i<clampedSimulationSteps;i++) {
             internalSingleStepSimulation(fixedTimeStep);
@@ -143,3 +146,24 @@ void ThreadSafeDynamicsWorld::synchronizeMotionStates() {
     }
 }
 
+void ThreadSafeDynamicsWorld::saveKinematicState(btScalar timeStep) {
+///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows
+///to switch status _after_ adding kinematic objects to the world
+///fix it for Bullet 3.x release
+    BT_PROFILE("saveKinematicState");
+    for (int i=0;i<m_collisionObjects.size();i++)
+    {
+        btCollisionObject* colObj = m_collisionObjects[i];
+        btRigidBody* body = btRigidBody::upcast(colObj);
+        if (body && body->getActivationState() != ISLAND_SLEEPING)
+        {
+            if (body->isKinematicObject())
+            {
+                //to calculate velocities next frame
+                body->saveKinematicState(timeStep);
+            }
+        }
+    }
+}
+
+
diff --git a/libraries/physics/src/ThreadSafeDynamicsWorld.h b/libraries/physics/src/ThreadSafeDynamicsWorld.h
index e9708149da..68062d8d29 100644
--- a/libraries/physics/src/ThreadSafeDynamicsWorld.h
+++ b/libraries/physics/src/ThreadSafeDynamicsWorld.h
@@ -41,6 +41,7 @@ public:
                                           btScalar fixedTimeStep = btScalar(1.)/btScalar(60.),
                                           SubStepCallback onSubStep = []() { });
     virtual void synchronizeMotionStates() override;
+    virtual void saveKinematicState(btScalar timeStep) override;
 
     // btDiscreteDynamicsWorld::m_localTime is the portion of real-time that has not yet been simulated
     // but is used for MotionState::setWorldTransform() extrapolation (a feature that Bullet uses to provide