From 771727890cb8af4e7a66fe006f7b3344135dd1b9 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 12 Apr 2016 09:56:01 -0700
Subject: [PATCH] more finely grained Bullet profiling

---
 libraries/physics/src/PhysicsEngine.cpp       |  1 +
 .../physics/src/ThreadSafeDynamicsWorld.cpp   | 26 ++++++++++++++++++-
 .../physics/src/ThreadSafeDynamicsWorld.h     |  1 +
 3 files changed, 27 insertions(+), 1 deletion(-)

diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp
index 54419a9c1a..8edbe67431 100644
--- a/libraries/physics/src/PhysicsEngine.cpp
+++ b/libraries/physics/src/PhysicsEngine.cpp
@@ -252,6 +252,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