minor DynamicsWorld optimizations and track global steps count therein

libraries/physics/src/EntityMotionState.cpp

@@ -252,11 +252,9 @@ void EntityMotionState::getWorldTransform(btTransform& worldTrans) const {
 
         uint32_t thisStep = ObjectMotionState::getWorldSimulationStep();
         float dt = (thisStep - _lastKinematicStep) * PHYSICS_ENGINE_FIXED_SUBSTEP;
+        _lastKinematicStep = thisStep;
         _entity->stepKinematicMotion(dt);
 
-        // bypass const-ness so we can remember the step
-        const_cast<EntityMotionState*>(this)->_lastKinematicStep = thisStep;
-
         // and set the acceleration-matches-gravity count high so that if we send an update
         // it will use the correct acceleration for remote simulations
         _accelerationNearlyGravityCount = (uint8_t)(-1);

libraries/physics/src/PhysicsEngine.cpp

@@ -71,8 +71,8 @@ void PhysicsEngine::init() {
     }
 }
 
-uint32_t PhysicsEngine::getNumSubsteps() {
-    return _numSubsteps;
+uint32_t PhysicsEngine::getNumSubsteps() const {
+    return _dynamicsWorld->getNumSubsteps();
 }
 
 // private
@@ -329,13 +329,9 @@ void PhysicsEngine::stepSimulation() {
                                                                         PHYSICS_ENGINE_FIXED_SUBSTEP, onSubStep);
     if (numSubsteps > 0) {
         BT_PROFILE("postSimulation");
-        _numSubsteps += (uint32_t)numSubsteps;
-        ObjectMotionState::setWorldSimulationStep(_numSubsteps);
-
         if (_myAvatarController) {
             _myAvatarController->postSimulation();
         }
-
         _hasOutgoingChanges = true;
     }
 

libraries/physics/src/PhysicsEngine.h

@@ -52,7 +52,7 @@ public:
     ~PhysicsEngine();
     void init();
 
-    uint32_t getNumSubsteps();
+    uint32_t getNumSubsteps() const;
 
     void removeObjects(const VectorOfMotionStates& objects);
     void removeSetOfObjects(const SetOfMotionStates& objects); // only called during teardown
@@ -135,7 +135,6 @@ private:
     CharacterController* _myAvatarController;
 
     uint32_t _numContactFrames = 0;
-    uint32_t _numSubsteps;
 
     bool _dumpNextStats { false };
     bool _saveNextStats { false };

libraries/physics/src/ThreadSafeDynamicsWorld.cpp

@@ -59,14 +59,11 @@ int ThreadSafeDynamicsWorld::stepSimulationWithSubstepCallback(btScalar timeStep
         }
     }
 
-    /*//process some debugging flags
-    if (getDebugDrawer()) {
-        btIDebugDraw* debugDrawer = getDebugDrawer();
-        gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
-    }*/
     if (subSteps) {
         //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
         int clampedSimulationSteps = (subSteps > maxSubSteps)? maxSubSteps : subSteps;
+        _numSubsteps += clampedSimulationSteps;
+        ObjectMotionState::setWorldSimulationStep(_numSubsteps);
 
         saveKinematicState(fixedTimeStep*clampedSimulationSteps);
 
@@ -98,28 +95,24 @@ int ThreadSafeDynamicsWorld::stepSimulationWithSubstepCallback(btScalar timeStep
 // call this instead of non-virtual btDiscreteDynamicsWorld::synchronizeSingleMotionState()
 void ThreadSafeDynamicsWorld::synchronizeMotionState(btRigidBody* body) {
     btAssert(body);
-    if (body->getMotionState() && !body->isStaticObject()) {
-        //we need to call the update at least once, even for sleeping objects
-        //otherwise the 'graphics' transform never updates properly
-        ///@todo: add 'dirty' flag
-        //if (body->getActivationState() != ISLAND_SLEEPING)
-        {
-            if (body->isKinematicObject()) {
-                ObjectMotionState* objectMotionState = static_cast<ObjectMotionState*>(body->getMotionState());
-                if (objectMotionState->hasInternalKinematicChanges()) {
-                    objectMotionState->clearInternalKinematicChanges();
-                    body->getMotionState()->setWorldTransform(body->getWorldTransform());
-                }
-                return;
-            }
-            btTransform interpolatedTransform;
-            btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
-                body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),
-                (m_latencyMotionStateInterpolation && m_fixedTimeStep) ? m_localTime - m_fixedTimeStep : m_localTime*body->getHitFraction(),
-                interpolatedTransform);
-            body->getMotionState()->setWorldTransform(interpolatedTransform);
+    btAssert(body->getMotionState());
+
+    if (body->isKinematicObject()) {
+        ObjectMotionState* objectMotionState = static_cast<ObjectMotionState*>(body->getMotionState());
+        if (objectMotionState->hasInternalKinematicChanges()) {
+            // this is a special case where the kinematic motion has been updated by an Action
+            // so we supply the body's current transform to the MotionState
+            objectMotionState->clearInternalKinematicChanges();
+            body->getMotionState()->setWorldTransform(body->getWorldTransform());
         }
+        return;
     }
+    btTransform interpolatedTransform;
+    btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
+        body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),
+        (m_latencyMotionStateInterpolation && m_fixedTimeStep) ? m_localTime - m_fixedTimeStep : m_localTime*body->getHitFraction(),
+        interpolatedTransform);
+    body->getMotionState()->setWorldTransform(interpolatedTransform);
 }
 
 void ThreadSafeDynamicsWorld::synchronizeMotionStates() {
@@ -164,24 +157,12 @@ 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
     DETAILED_PROFILE_RANGE(simulation_physics, "saveKinematicState");
     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);
-            }
+    for (int i=0;i<m_nonStaticRigidBodies.size();i++) {
+        btRigidBody* body = m_nonStaticRigidBodies[i];
+        if (body && body->isKinematicObject() && body->getActivationState() != ISLAND_SLEEPING) {
+            body->saveKinematicState(timeStep);
         }
     }
 }
-
-

libraries/physics/src/ThreadSafeDynamicsWorld.h

@@ -37,6 +37,7 @@ public:
             btConstraintSolver* constraintSolver,
             btCollisionConfiguration* collisionConfiguration);
 
+    int getNumSubsteps() const { return _numSubsteps; }
     int stepSimulationWithSubstepCallback(btScalar timeStep, int maxSubSteps = 1,
                                           btScalar fixedTimeStep = btScalar(1.)/btScalar(60.),
                                           SubStepCallback onSubStep = []() { });
@@ -61,6 +62,7 @@ private:
     VectorOfMotionStates _deactivatedStates;
     SetOfMotionStates _activeStates;
     SetOfMotionStates _lastActiveStates;
+    int _numSubsteps { 0 };
 };
 
 #endif // hifi_ThreadSafeDynamicsWorld_h