Merge pull request #9851 from AndrewMeadows/simulation-ownership-2

zero velocity of deactivated dynamic entities

interface/src/Application.cpp

@@ -4438,9 +4438,12 @@ void Application::update(float deltaTime) {
 
                 getEntities()->getTree()->withWriteLock([&] {
                     PerformanceTimer perfTimer("handleOutgoingChanges");
-                    const VectorOfMotionStates& outgoingChanges = _physicsEngine->getOutgoingChanges();
+                    const VectorOfMotionStates& deactivations = _physicsEngine->getDeactivatedMotionStates();
-                    _entitySimulation->handleOutgoingChanges(outgoingChanges);
+                    _entitySimulation->handleDeactivatedMotionStates(deactivations);
-                    avatarManager->handleOutgoingChanges(outgoingChanges);
+
+                    const VectorOfMotionStates& outgoingChanges = _physicsEngine->getChangedMotionStates();
+                    _entitySimulation->handleChangedMotionStates(outgoingChanges);
+                    avatarManager->handleChangedMotionStates(outgoingChanges);
                 });
 
                 if (!_aboutToQuit) {

interface/src/avatar/AvatarManager.cpp

@@ -424,7 +424,7 @@ void AvatarManager::getObjectsToChange(VectorOfMotionStates& result) {
     }
 }
 
-void AvatarManager::handleOutgoingChanges(const VectorOfMotionStates& motionStates) {
+void AvatarManager::handleChangedMotionStates(const VectorOfMotionStates& motionStates) {
     // TODO: extract the MyAvatar results once we use a MotionState for it.
 }
 

interface/src/avatar/AvatarManager.h

@@ -70,7 +70,7 @@ public:
     void getObjectsToRemoveFromPhysics(VectorOfMotionStates& motionStates);
     void getObjectsToAddToPhysics(VectorOfMotionStates& motionStates);
     void getObjectsToChange(VectorOfMotionStates& motionStates);
-    void handleOutgoingChanges(const VectorOfMotionStates& motionStates);
+    void handleChangedMotionStates(const VectorOfMotionStates& motionStates);
     void handleCollisionEvents(const CollisionEvents& collisionEvents);
 
     Q_INVOKABLE float getAvatarDataRate(const QUuid& sessionID, const QString& rateName = QString("")) const;

libraries/entities/src/EntityItem.cpp

@@ -655,13 +655,11 @@ int EntityItem::readEntityDataFromBuffer(const unsigned char* data, int bytesLef
 
 
     // pack SimulationOwner and terse update properties near each other
-
     // NOTE: the server is authoritative for changes to simOwnerID so we always unpack ownership data
     // even when we would otherwise ignore the rest of the packet.
 
     bool filterRejection = false;
     if (propertyFlags.getHasProperty(PROP_SIMULATION_OWNER)) {
-
         QByteArray simOwnerData;
         int bytes = OctreePacketData::unpackDataFromBytes(dataAt, simOwnerData);
         SimulationOwner newSimOwner;
@@ -1879,6 +1877,7 @@ void EntityItem::setSimulationOwner(const SimulationOwner& owner) {
 }
 
 void EntityItem::updateSimulationOwner(const SimulationOwner& owner) {
+    // NOTE: this method only used by EntityServer.  The Interface uses special code in readEntityDataFromBuffer().
     if (wantTerseEditLogging() && _simulationOwner != owner) {
         qCDebug(entities) << "sim ownership for" << getDebugName() << "is now" << owner;
     }
@@ -1894,8 +1893,9 @@ void EntityItem::clearSimulationOwnership() {
     }
 
     _simulationOwner.clear();
-    // don't bother setting the DIRTY_SIMULATOR_ID flag because clearSimulationOwnership()
+    // don't bother setting the DIRTY_SIMULATOR_ID flag because:
-    // is only ever called on the entity-server and the flags are only used client-side
+    // (a) when entity-server calls clearSimulationOwnership() the dirty-flags are meaningless (only used by interface)
+    // (b) the interface only calls clearSimulationOwnership() in a context that already knows best about dirty flags
     //_dirtyFlags |= Simulation::DIRTY_SIMULATOR_ID;
 
 }

libraries/physics/src/EntityMotionState.cpp

@@ -97,6 +97,21 @@ void EntityMotionState::updateServerPhysicsVariables() {
     _serverActionData = _entity->getActionData();
 }
 
+void EntityMotionState::handleDeactivation() {
+    // copy _server data to entity
+    bool success;
+    _entity->setPosition(_serverPosition, success, false);
+    _entity->setOrientation(_serverRotation, success, false);
+    _entity->setVelocity(ENTITY_ITEM_ZERO_VEC3);
+    _entity->setAngularVelocity(ENTITY_ITEM_ZERO_VEC3);
+    // and also to RigidBody
+    btTransform worldTrans;
+    worldTrans.setOrigin(glmToBullet(_serverPosition));
+    worldTrans.setRotation(glmToBullet(_serverRotation));
+    _body->setWorldTransform(worldTrans);
+    // no need to update velocities... should already be zero
+}
+
 // virtual
 void EntityMotionState::handleEasyChanges(uint32_t& flags) {
     assert(entityTreeIsLocked());
@@ -111,6 +126,8 @@ void EntityMotionState::handleEasyChanges(uint32_t& flags) {
                 flags &= ~Simulation::DIRTY_PHYSICS_ACTIVATION;
                 _body->setActivationState(WANTS_DEACTIVATION);
                 _outgoingPriority = 0;
+                const float ACTIVATION_EXPIRY = 3.0f; // something larger than the 2.0 hard coded in Bullet
+                _body->setDeactivationTime(ACTIVATION_EXPIRY);
             } else {
                 // disowned object is still moving --> start timer for ownership bid
                 // TODO? put a delay in here proportional to distance from object?
@@ -221,12 +238,9 @@ void EntityMotionState::getWorldTransform(btTransform& worldTrans) const {
 }
 
 // This callback is invoked by the physics simulation at the end of each simulation step...
-// iff the corresponding RigidBody is DYNAMIC and has moved.
+// iff the corresponding RigidBody is DYNAMIC and ACTIVE.
 void EntityMotionState::setWorldTransform(const btTransform& worldTrans) {
-    if (!_entity) {
+    assert(_entity);
-        return;
-    }
-
     assert(entityTreeIsLocked());
     measureBodyAcceleration();
     bool positionSuccess;

libraries/physics/src/EntityMotionState.h

@@ -29,6 +29,7 @@ public:
     virtual ~EntityMotionState();
 
     void updateServerPhysicsVariables();
+    void handleDeactivation();
     virtual void handleEasyChanges(uint32_t& flags) override;
     virtual bool handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine) override;
 

libraries/physics/src/PhysicalEntitySimulation.cpp

@@ -259,13 +259,27 @@ void PhysicalEntitySimulation::getObjectsToChange(VectorOfMotionStates& result)
     _pendingChanges.clear();
 }
 
-void PhysicalEntitySimulation::handleOutgoingChanges(const VectorOfMotionStates& motionStates) {
+void PhysicalEntitySimulation::handleDeactivatedMotionStates(const VectorOfMotionStates& motionStates) {
+    for (auto stateItr : motionStates) {
+        ObjectMotionState* state = &(*stateItr);
+        assert(state);
+        if (state->getType() == MOTIONSTATE_TYPE_ENTITY) {
+            EntityMotionState* entityState = static_cast<EntityMotionState*>(state);
+            entityState->handleDeactivation();
+            EntityItemPointer entity = entityState->getEntity();
+            _entitiesToSort.insert(entity);
+        }
+    }
+}
+
+void PhysicalEntitySimulation::handleChangedMotionStates(const VectorOfMotionStates& motionStates) {
     QMutexLocker lock(&_mutex);
 
     // walk the motionStates looking for those that correspond to entities
     for (auto stateItr : motionStates) {
         ObjectMotionState* state = &(*stateItr);
-        if (state && state->getType() == MOTIONSTATE_TYPE_ENTITY) {
+        assert(state);
+        if (state->getType() == MOTIONSTATE_TYPE_ENTITY) {
             EntityMotionState* entityState = static_cast<EntityMotionState*>(state);
             EntityItemPointer entity = entityState->getEntity();
             assert(entity.get());

libraries/physics/src/PhysicalEntitySimulation.h

@@ -56,7 +56,8 @@ public:
     void setObjectsToChange(const VectorOfMotionStates& objectsToChange);
     void getObjectsToChange(VectorOfMotionStates& result);
 
-    void handleOutgoingChanges(const VectorOfMotionStates& motionStates);
+    void handleDeactivatedMotionStates(const VectorOfMotionStates& motionStates);
+    void handleChangedMotionStates(const VectorOfMotionStates& motionStates);
     void handleCollisionEvents(const CollisionEvents& collisionEvents);
 
     EntityEditPacketSender* getPacketSender() { return _entityPacketSender; }
@@ -67,7 +68,7 @@ private:
     SetOfEntities _entitiesToAddToPhysics;
 
     SetOfEntityMotionStates _pendingChanges; // EntityMotionStates already in PhysicsEngine that need their physics changed
-    SetOfEntityMotionStates _outgoingChanges; // EntityMotionStates for which we need to send updates to entity-server
+    SetOfEntityMotionStates _outgoingChanges; // EntityMotionStates for which we may need to send updates to entity-server
 
     SetOfMotionStates _physicalObjects; // MotionStates of entities in PhysicsEngine
 

libraries/physics/src/PhysicsEngine.cpp

@@ -472,7 +472,7 @@ const CollisionEvents& PhysicsEngine::getCollisionEvents() {
     return _collisionEvents;
 }
 
-const VectorOfMotionStates& PhysicsEngine::getOutgoingChanges() {
+const VectorOfMotionStates& PhysicsEngine::getChangedMotionStates() {
     BT_PROFILE("copyOutgoingChanges");
     // Bullet will not deactivate static objects (it doesn't expect them to be active)
     // so we must deactivate them ourselves

libraries/physics/src/PhysicsEngine.h

@@ -65,7 +65,8 @@ public:
     bool hasOutgoingChanges() const { return _hasOutgoingChanges; }
 
     /// \return reference to list of changed MotionStates.  The list is only valid until beginning of next simulation loop.
-    const VectorOfMotionStates& getOutgoingChanges();
+    const VectorOfMotionStates& getChangedMotionStates();
+    const VectorOfMotionStates& getDeactivatedMotionStates() const { return _dynamicsWorld->getDeactivatedMotionStates(); }
 
     /// \return reference to list of Collision events.  The list is only valid until beginning of next simulation loop.
     const CollisionEvents& getCollisionEvents();

libraries/physics/src/ThreadSafeDynamicsWorld.cpp

@@ -120,30 +120,41 @@ void ThreadSafeDynamicsWorld::synchronizeMotionState(btRigidBody* body) {
 void ThreadSafeDynamicsWorld::synchronizeMotionStates() {
     BT_PROFILE("synchronizeMotionStates");
     _changedMotionStates.clear();
+
+    // NOTE: m_synchronizeAllMotionStates is 'false' by default for optimization.
+    // See PhysicsEngine::init() where we call _dynamicsWorld->setForceUpdateAllAabbs(false)
     if (m_synchronizeAllMotionStates) {
         //iterate  over all collision objects
         for (int i=0;i<m_collisionObjects.size();i++) {
             btCollisionObject* colObj = m_collisionObjects[i];
             btRigidBody* body = btRigidBody::upcast(colObj);
-            if (body) {
+            if (body && body->getMotionState()) {
-                if (body->getMotionState()) {
                 synchronizeMotionState(body);
                 _changedMotionStates.push_back(static_cast<ObjectMotionState*>(body->getMotionState()));
             }
         }
-        }
     } else  {
         //iterate over all active rigid bodies
+        // TODO? if this becomes a performance bottleneck we could derive our own SimulationIslandManager
+        // that remembers a list of objects deactivated last step
+        _activeStates.clear();
+        _deactivatedStates.clear();
         for (int i=0;i<m_nonStaticRigidBodies.size();i++) {
             btRigidBody* body = m_nonStaticRigidBodies[i];
+            ObjectMotionState* motionState = static_cast<ObjectMotionState*>(body->getMotionState());
+            if (motionState) {
                 if (body->isActive()) {
-                if (body->getMotionState()) {
                     synchronizeMotionState(body);
-                    _changedMotionStates.push_back(static_cast<ObjectMotionState*>(body->getMotionState()));
+                    _changedMotionStates.push_back(motionState);
+                    _activeStates.insert(motionState);
+                } else if (_lastActiveStates.find(motionState) != _lastActiveStates.end()) {
+                    // this object was active last frame but is no longer
+                    _deactivatedStates.push_back(motionState);
                 }
             }
         }
     }
+    _activeStates.swap(_lastActiveStates);
 }
 
 void ThreadSafeDynamicsWorld::saveKinematicState(btScalar timeStep) {

libraries/physics/src/ThreadSafeDynamicsWorld.h

@@ -49,12 +49,16 @@ public:
     float getLocalTimeAccumulation() const { return m_localTime; }
 
     const VectorOfMotionStates& getChangedMotionStates() const { return _changedMotionStates; }
+    const VectorOfMotionStates& getDeactivatedMotionStates() const { return _deactivatedStates; }
 
 private:
     // call this instead of non-virtual btDiscreteDynamicsWorld::synchronizeSingleMotionState()
     void synchronizeMotionState(btRigidBody* body);
 
     VectorOfMotionStates _changedMotionStates;
+    VectorOfMotionStates _deactivatedStates;
+    SetOfMotionStates _activeStates;
+    SetOfMotionStates _lastActiveStates;
 };
 
 #endif // hifi_ThreadSafeDynamicsWorld_h