init and step physics simulation

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...

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;

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;