From 0d0f98f669f428508951a32de252e19389d32a44 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 11 Nov 2014 12:09:59 -0800
Subject: [PATCH] init and step physics simulation

---
 interface/src/Application.cpp          | 11 +++++++++++
 libraries/physics/src/PhysicsWorld.cpp | 11 +++++++++++
 libraries/physics/src/PhysicsWorld.h   |  3 +++
 3 files changed, 25 insertions(+)

diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index 5abb826f45..f0b4afe90a 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -2009,6 +2009,10 @@ void Application::init() {
 
     // save settings when avatar changes
     connect(_myAvatar, &MyAvatar::transformChanged, this, &Application::updateMyAvatarTransform);
+
+#ifdef USE_BULLET_PHYSICS
+    _physicsWorld.init();
+#endif // USE_BULLET_PHYSICS
 }
 
 void Application::closeMirrorView() {
@@ -2314,6 +2318,13 @@ void Application::update(float deltaTime) {
     updateDialogs(deltaTime); // update various stats dialogs if present
     updateCursor(deltaTime); // Handle cursor updates
 
+#ifdef USE_BULLET_PHYSICS
+    {
+        PerformanceTimer perfTimer("physics");
+        _physicsWorld.stepSimulation();
+    }
+#endif // USE_BULLET_PHYSICS
+
     {
         PerformanceTimer perfTimer("entities");
         _entities.update(); // update the models...
diff --git a/libraries/physics/src/PhysicsWorld.cpp b/libraries/physics/src/PhysicsWorld.cpp
index 22f7b6c0f0..72d2786350 100644
--- a/libraries/physics/src/PhysicsWorld.cpp
+++ b/libraries/physics/src/PhysicsWorld.cpp
@@ -22,6 +22,17 @@ void PhysicsWorld::init() {
     _dynamicsWorld = new btDiscreteDynamicsWorld(_collisionDispatcher, _broadphaseFilter, _constraintSolver, _collisionConfig);
 }
 
+void PhysicsWorld::stepSimulation() {
+    const float MAX_TIMESTEP = 1.0f / 30.0f;
+    const int MAX_NUM_SUBSTEPS = 2;
+    const float FIXED_SUBSTEP = 1.0f / 60.0f;
+
+    float dt = 1.0e-6f * (float)(_clock.getTimeMicroseconds());
+    _clock.reset();
+    float timeStep = btMin(dt, MAX_TIMESTEP);
+    _dynamicsWorld->stepSimulation(timeStep, MAX_NUM_SUBSTEPS, FIXED_SUBSTEP);
+}
+
 bool PhysicsWorld::addVoxel(const glm::vec3& position, float scale) {
     glm::vec3 halfExtents = glm::vec3(0.5f * scale);
     glm::vec3 trueCenter = position + halfExtents;
diff --git a/libraries/physics/src/PhysicsWorld.h b/libraries/physics/src/PhysicsWorld.h
index e599b4f8a8..e18b87c91c 100644
--- a/libraries/physics/src/PhysicsWorld.h
+++ b/libraries/physics/src/PhysicsWorld.h
@@ -41,6 +41,8 @@ public:
 
     void init();
 
+    void stepSimulation();
+
     /// \param offset position of simulation origin in domain-frame
     void setOriginOffset(const glm::vec3& offset) { _originOffset = offset; }
 
@@ -70,6 +72,7 @@ public:
     bool updateEntityMassProperties(CustomMotionState* motionState, float mass, const glm::vec3& inertiaEigenValues);
 
 protected:
+    btClock _clock;
     btDefaultCollisionConfiguration* _collisionConfig;
     btCollisionDispatcher* _collisionDispatcher;
     btBroadphaseInterface* _broadphaseFilter;