add/remove avatars according to workload region

interface/src/Application.cpp

@@ -2540,11 +2540,15 @@ void Application::cleanupBeforeQuit() {
 
 Application::~Application() {
     // remove avatars from physics engine
-    DependencyManager::get<AvatarManager>()->clearOtherAvatars();
-    VectorOfMotionStates motionStates;
-    DependencyManager::get<AvatarManager>()->getObjectsToRemoveFromPhysics(motionStates);
-    _physicsEngine->removeObjects(motionStates);
-    DependencyManager::get<AvatarManager>()->deleteAllAvatars();
+    auto avatarManager = DependencyManager::get<AvatarManager>();
+    avatarManager->clearOtherAvatars();
+
+    PhysicsEngine::Transaction transaction;
+    avatarManager->buildPhysicsTransaction(transaction);
+    _physicsEngine->processTransaction(transaction);
+    avatarManager->handleProcessedPhysicsTransaction(transaction);
+
+    avatarManager->deleteAllAvatars();
 
     _physicsEngine->setCharacterController(nullptr);
 
@@ -5706,12 +5710,10 @@ void Application::update(float deltaTime) {
 
                 t1 = std::chrono::high_resolution_clock::now();
 
-                avatarManager->getObjectsToRemoveFromPhysics(motionStates);
-                _physicsEngine->removeObjects(motionStates);
-                avatarManager->getObjectsToAddToPhysics(motionStates);
-                _physicsEngine->addObjects(motionStates);
-                avatarManager->getObjectsToChange(motionStates);
-                _physicsEngine->changeObjects(motionStates);
+                PhysicsEngine::Transaction transaction;
+                avatarManager->buildPhysicsTransaction(transaction);
+                _physicsEngine->processTransaction(transaction);
+                avatarManager->handleProcessedPhysicsTransaction(transaction);
 
                 myAvatar->prepareForPhysicsSimulation();
                 _physicsEngine->forEachDynamic([&](EntityDynamicPointer dynamic) {

interface/src/avatar/AvatarManager.cpp

@@ -98,7 +98,7 @@ AvatarSharedPointer AvatarManager::addAvatar(const QUuid& sessionUUID, const QWe
 }
 
 AvatarManager::~AvatarManager() {
-    assert(_motionStates.empty());
+    assert(_avatarsToChangeInPhysics.empty());
 }
 
 void AvatarManager::init() {
@@ -213,7 +213,6 @@ void AvatarManager::updateOtherAvatars(float deltaTime) {
     uint64_t updateExpiry = startTime + UPDATE_BUDGET;
     int numAvatarsUpdated = 0;
     int numAVatarsNotUpdated = 0;
-    bool physicsEnabled = qApp->isPhysicsEnabled();
 
     render::Transaction renderTransaction;
     workload::Transaction workloadTransaction;
@@ -240,18 +239,6 @@ void AvatarManager::updateOtherAvatars(float deltaTime) {
         if (_shouldRender) {
             avatar->ensureInScene(avatar, qApp->getMain3DScene());
         }
-        if (physicsEnabled && !avatar->isInPhysicsSimulation()) {
-            ShapeInfo shapeInfo;
-            avatar->computeShapeInfo(shapeInfo);
-            btCollisionShape* shape = const_cast<btCollisionShape*>(ObjectMotionState::getShapeManager()->getShape(shapeInfo));
-            if (shape) {
-                AvatarMotionState* motionState = new AvatarMotionState(avatar, shape);
-                motionState->setMass(avatar->computeMass());
-                avatar->setMotionState(motionState);
-                _motionStates.insert(avatar.get(), motionState);
-                _motionStatesToAddToPhysics.insert(motionState);
-            }
-        }
         avatar->animateScaleChanges(deltaTime);
 
         const float OUT_OF_VIEW_THRESHOLD = 0.5f * AvatarData::OUT_OF_VIEW_PENALTY;
@@ -393,8 +380,52 @@ AvatarSharedPointer AvatarManager::newSharedAvatar() {
     return AvatarSharedPointer(new OtherAvatar(qApp->thread()), [](OtherAvatar* ptr) { ptr->deleteLater(); });
 }
 
-void AvatarManager::handleSpaceChange(OtherAvatarPointer avatar) {
-    // WORKLOAD_AVATARS_BOOKMARK: implement this
+void AvatarManager::queuePhysicsChange(const OtherAvatarPointer& avatar) {
+    _avatarsToChangeInPhysics.insert(avatar);
+}
+
+void AvatarManager::buildPhysicsTransaction(PhysicsEngine::Transaction& transaction) {
+    SetOfOtherAvatars failedShapeBuilds;
+    for (auto avatar : _avatarsToChangeInPhysics) {
+        bool isInPhysics = avatar->isInPhysicsSimulation();
+        if (isInPhysics != avatar->shouldBeInPhysicsSimulation()) {
+            if (isInPhysics) {
+                transaction.objectsToRemove.push_back(avatar->_motionState);
+                avatar->_motionState = nullptr;
+            } else {
+                ShapeInfo shapeInfo;
+                avatar->computeShapeInfo(shapeInfo);
+                btCollisionShape* shape = const_cast<btCollisionShape*>(ObjectMotionState::getShapeManager()->getShape(shapeInfo));
+                if (shape) {
+                    AvatarMotionState* motionState = new AvatarMotionState(avatar, shape);
+                    motionState->setMass(avatar->computeMass());
+                    avatar->_motionState = motionState;
+                    transaction.objectsToAdd.push_back(motionState);
+                } else {
+                    failedShapeBuilds.insert(avatar);
+                }
+            }
+        } else if (isInPhysics) {
+            transaction.objectsToChange.push_back(avatar->_motionState);
+        }
+    }
+    _avatarsToChangeInPhysics.swap(failedShapeBuilds);
+}
+
+void AvatarManager::handleProcessedPhysicsTransaction(PhysicsEngine::Transaction& transaction) {
+    // things on objectsToChange correspond to failed changes
+    // so we push them back onto _avatarsToChangeInPhysics
+    for (auto object : transaction.objectsToChange) {
+        AvatarMotionState* motionState = static_cast<AvatarMotionState*>(object);
+        assert(motionState);
+        assert(motionState->_avatar);
+        _avatarsToChangeInPhysics.insert(motionState->_avatar);
+    }
+    // things on objectsToRemove are ready for delete
+    for (auto object : transaction.objectsToRemove) {
+        delete object;
+    }
+    transaction.clear();
 }
 
 void AvatarManager::handleRemovedAvatar(const AvatarSharedPointer& removedAvatar, KillAvatarReason removalReason) {
@@ -405,15 +436,8 @@ void AvatarManager::handleRemovedAvatar(const AvatarSharedPointer& removedAvatar
     }
     AvatarHashMap::handleRemovedAvatar(avatar, removalReason);
 
-    // remove from physics
-    avatar->setMotionState(nullptr);
-    AvatarMotionStateMap::iterator itr = _motionStates.find(avatar.get());
-    if (itr != _motionStates.end()) {
-        AvatarMotionState* motionState = *itr;
-        _motionStatesToAddToPhysics.remove(motionState);
-        _motionStatesToRemoveFromPhysics.push_back(motionState);
-        _motionStates.erase(itr);
-    }
+    avatar->die();
+    queuePhysicsChange(avatar);
 
     if (removalReason == KillAvatarReason::TheirAvatarEnteredYourBubble) {
         emit DependencyManager::get<UsersScriptingInterface>()->enteredIgnoreRadius();
@@ -449,8 +473,7 @@ void AvatarManager::clearOtherAvatars() {
 }
 
 void AvatarManager::deleteAllAvatars() {
-    assert(_motionStates.empty()); // should have called clearOtherAvatars() before getting here
-    deleteMotionStates();
+    assert(_avatarsToChangeInPhysics.empty());
 
     QReadLocker locker(&_hashLock);
     AvatarHash::iterator avatarIterator =  _avatarHash.begin();
@@ -458,39 +481,7 @@ void AvatarManager::deleteAllAvatars() {
         auto avatar = std::static_pointer_cast<Avatar>(avatarIterator.value());
         avatarIterator = _avatarHash.erase(avatarIterator);
         avatar->die();
-    }
-}
-
-void AvatarManager::deleteMotionStates() {
-    // delete motionstates that were removed from physics last frame
-    for (auto state : _motionStatesToDelete) {
-        delete state;
-    }
-    _motionStatesToDelete.clear();
-}
-
-void AvatarManager::getObjectsToRemoveFromPhysics(VectorOfMotionStates& result) {
-    deleteMotionStates();
-    result = _motionStatesToRemoveFromPhysics;
-    _motionStatesToDelete.swap(_motionStatesToRemoveFromPhysics);
-}
-
-void AvatarManager::getObjectsToAddToPhysics(VectorOfMotionStates& result) {
-    result.clear();
-    for (auto motionState : _motionStatesToAddToPhysics) {
-        result.push_back(motionState);
-    }
-    _motionStatesToAddToPhysics.clear();
-}
-
-void AvatarManager::getObjectsToChange(VectorOfMotionStates& result) {
-    result.clear();
-    AvatarMotionStateMap::iterator motionStateItr = _motionStates.begin();
-    while (motionStateItr != _motionStates.end()) {
-        if ((*motionStateItr)->getIncomingDirtyFlags() != 0) {
-            result.push_back(*motionStateItr);
-        }
-        ++motionStateItr;
+        assert(!avatar->_motionState);
     }
 }
 

interface/src/avatar/AvatarManager.h

@@ -12,6 +12,8 @@
 #ifndef hifi_AvatarManager_h
 #define hifi_AvatarManager_h
 
+#include <set>
+
 #include <QtCore/QHash>
 #include <QtCore/QObject>
 #include <QtCore/QSharedPointer>
@@ -177,16 +179,17 @@ public:
     float getMyAvatarSendRate() const { return _myAvatarSendRate.rate(); }
     int getIdentityRequestsSent() const { return _identityRequestsSent; }
 
-public slots:
+    void queuePhysicsChange(const OtherAvatarPointer& avatar);
+    void buildPhysicsTransaction(PhysicsEngine::Transaction& transaction);
+    void handleProcessedPhysicsTransaction(PhysicsEngine::Transaction& transaction);
 
+public slots:
     /**jsdoc
      * @function AvatarManager.updateAvatarRenderStatus
      * @param {boolean} shouldRenderAvatars
      */
     void updateAvatarRenderStatus(bool shouldRenderAvatars);
 
-    void handleSpaceChange(OtherAvatarPointer avatar);
-
 protected:
     AvatarSharedPointer addAvatar(const QUuid& sessionUUID, const QWeakPointer<Node>& mixerWeakPointer) override;
 
@@ -197,16 +200,12 @@ private:
     void simulateAvatarFades(float deltaTime);
 
     AvatarSharedPointer newSharedAvatar() override;
-    void deleteMotionStates();
     void handleRemovedAvatar(const AvatarSharedPointer& removedAvatar, KillAvatarReason removalReason = KillAvatarReason::NoReason) override;
 
     QVector<AvatarSharedPointer> _avatarsToFade;
 
-    using AvatarMotionStateMap = QMap<Avatar*, AvatarMotionState*>;
-    AvatarMotionStateMap _motionStates;
-    VectorOfMotionStates _motionStatesToRemoveFromPhysics;
-    VectorOfMotionStates _motionStatesToDelete;
-    SetOfMotionStates _motionStatesToAddToPhysics;
+    using SetOfOtherAvatars = std::set<OtherAvatarPointer>;
+    SetOfOtherAvatars _avatarsToChangeInPhysics;
 
     std::shared_ptr<MyAvatar> _myAvatar;
     quint64 _lastSendAvatarDataTime = 0; // Controls MyAvatar send data rate.

interface/src/avatar/OtherAvatar.cpp

@@ -82,12 +82,16 @@ int OtherAvatar::parseDataFromBuffer(const QByteArray& buffer) {
     return bytesRead;
 }
 
+void OtherAvatar::setWorkloadRegion(uint8_t region) {
+    _workloadRegion = region;
+}
+
+bool OtherAvatar::shouldBeInPhysicsSimulation() const {
+    return (_workloadRegion < workload::Region::R3 && !isDead());
+}
+
 void OtherAvatar::rebuildCollisionShape() {
     if (_motionState) {
         _motionState->addDirtyFlags(Simulation::DIRTY_SHAPE | Simulation::DIRTY_MASS);
     }
 }
-
-void OtherAvatar::setMotionState(AvatarMotionState* motionState) {
-    _motionState = motionState;
-}

interface/src/avatar/OtherAvatar.h

@@ -18,6 +18,7 @@
 #include "ui/overlays/Overlays.h"
 #include "ui/overlays/Sphere3DOverlay.h"
 
+class AvatarManager;
 class AvatarMotionState;
 
 class OtherAvatar : public Avatar {
@@ -36,15 +37,20 @@ public:
 
     int parseDataFromBuffer(const QByteArray& buffer) override;
 
-    void setMotionState(AvatarMotionState* motionState);
     bool isInPhysicsSimulation() const { return _motionState != nullptr; }
     void rebuildCollisionShape() override;
 
+    void setWorkloadRegion(uint8_t region);
+    bool shouldBeInPhysicsSimulation() const;
+
+    friend AvatarManager;
+
 protected:
     std::shared_ptr<Sphere3DOverlay> _otherAvatarOrbMeshPlaceholder { nullptr };
     OverlayID _otherAvatarOrbMeshPlaceholderID { UNKNOWN_OVERLAY_ID };
     AvatarMotionState* _motionState { nullptr };
     int32_t _spaceIndex { -1 };
+    uint8_t _workloadRegion { workload::Region::INVALID };
 };
 
 using OtherAvatarPointer = std::shared_ptr<OtherAvatar>;

interface/src/workload/PhysicsBoundary.cpp

@@ -25,8 +25,6 @@ void PhysicsBoundary::run(const workload::WorkloadContextPointer& context, const
         return;
     }
     GameWorkloadContext* gameContext = static_cast<GameWorkloadContext*>(context.get());
-    PhysicalEntitySimulationPointer simulation = gameContext->_simulation;
-    auto avatarManager = DependencyManager::get<AvatarManager>();
     const auto& regionChanges = inputs.get0();
     for (uint32_t i = 0; i < (uint32_t)regionChanges.size(); ++i) {
         const workload::Space::Change& change = regionChanges[i];
@@ -34,13 +32,15 @@ void PhysicsBoundary::run(const workload::WorkloadContextPointer& context, const
         if (nestable) {
             switch (nestable->getNestableType()) {
                 case NestableType::Entity: {
-                    EntityItemPointer entity = std::static_pointer_cast<EntityItem>(nestable);
-                    simulation->changeEntity(entity);
+                    gameContext->_simulation->changeEntity(std::static_pointer_cast<EntityItem>(nestable));
                 }
                 break;
                 case NestableType::Avatar: {
                     auto avatar = std::static_pointer_cast<OtherAvatar>(nestable);
-                    avatarManager->handleSpaceChange(avatar);
+                    avatar->setWorkloadRegion(change.region);
+                    if (avatar->isInPhysicsSimulation() != avatar->shouldBeInPhysicsSimulation()) {
+                        DependencyManager::get<AvatarManager>()->queuePhysicsChange(avatar);
+                    }
                 }
                 break;
                 default:

libraries/physics/src/PhysicsEngine.cpp

@@ -279,6 +279,57 @@ void PhysicsEngine::reinsertObject(ObjectMotionState* object) {
     }
 }
 
+void PhysicsEngine::processTransaction(PhysicsEngine::Transaction& transaction) {
+    // removes
+    for (auto object : transaction.objectsToRemove) {
+        btRigidBody* body = object->getRigidBody();
+        if (body) {
+            removeDynamicsForBody(body);
+            _dynamicsWorld->removeRigidBody(body);
+
+            // NOTE: setRigidBody() modifies body->m_userPointer so we should clear the MotionState's body BEFORE deleting it.
+            object->setRigidBody(nullptr);
+            body->setMotionState(nullptr);
+            delete body;
+        }
+        object->clearIncomingDirtyFlags();
+    }
+
+    // adds
+    for (auto object : transaction.objectsToAdd) {
+        addObjectToDynamicsWorld(object);
+    }
+
+    // changes
+    std::vector<ObjectMotionState*> failedChanges;
+    for (auto object : transaction.objectsToChange) {
+        uint32_t flags = object->getIncomingDirtyFlags() & DIRTY_PHYSICS_FLAGS;
+        if (flags & HARD_DIRTY_PHYSICS_FLAGS) {
+            if (object->handleHardAndEasyChanges(flags, this)) {
+                object->clearIncomingDirtyFlags();
+            } else {
+                failedChanges.push_back(object);
+            }
+        } else if (flags & EASY_DIRTY_PHYSICS_FLAGS) {
+            object->handleEasyChanges(flags);
+            object->clearIncomingDirtyFlags();
+        }
+        if (object->getMotionType() == MOTION_TYPE_STATIC && object->isActive()) {
+            _activeStaticBodies.insert(object->getRigidBody());
+        }
+    }
+    // activeStaticBodies have changed (in an Easy way) and need their Aabbs updated
+    // but we've configured Bullet to NOT update them automatically (for improved performance)
+    // so we must do it ourselves
+    std::set<btRigidBody*>::const_iterator itr = _activeStaticBodies.begin();
+    while (itr != _activeStaticBodies.end()) {
+        _dynamicsWorld->updateSingleAabb(*itr);
+        ++itr;
+    }
+    // we replace objectsToChange with any that failed
+    transaction.objectsToChange.swap(failedChanges);
+}
+
 void PhysicsEngine::removeContacts(ObjectMotionState* motionState) {
     // trigger events for new/existing/old contacts
     ContactMap::iterator contactItr = _contactMap.begin();

libraries/physics/src/PhysicsEngine.h

@@ -70,6 +70,18 @@ using CollisionEvents = std::vector<Collision>;
 
 class PhysicsEngine {
 public:
+    class Transaction {
+    public:
+        void clear() {
+            objectsToRemove.clear();
+            objectsToAdd.clear();
+            objectsToChange.clear();
+        }
+        std::vector<ObjectMotionState*> objectsToRemove;
+        std::vector<ObjectMotionState*> objectsToAdd;
+        std::vector<ObjectMotionState*> objectsToChange;
+    };
+
     PhysicsEngine(const glm::vec3& offset);
     ~PhysicsEngine();
     void init();
@@ -83,6 +95,8 @@ public:
     VectorOfMotionStates changeObjects(const VectorOfMotionStates& objects);
     void reinsertObject(ObjectMotionState* object);
 
+    void processTransaction(Transaction& transaction);
+
     void stepSimulation();
     void harvestPerformanceStats();
     void printPerformanceStatsToFile(const QString& filename);