lock the entity tree during physics operations that access then entity tree. if the simulation and entity-tree are both going to be locked, be sure to lock the entity tree first (and unlock it last), because this is what the network-reading thread does

interface/src/Application.cpp

@@ -2496,24 +2496,45 @@ void Application::update(float deltaTime) {
 
         _entitySimulation.lock();
         _physicsEngine.deleteObjects(_entitySimulation.getObjectsToDelete());
+        _entitySimulation.unlock();
+
+        _entities.getTree()->lockForWrite();
+        _entitySimulation.lock();
         _physicsEngine.addObjects(_entitySimulation.getObjectsToAdd());
+        _entitySimulation.unlock();
+        _entities.getTree()->unlock();
+
+        _entities.getTree()->lockForWrite();
+        _entitySimulation.lock();
         _physicsEngine.changeObjects(_entitySimulation.getObjectsToChange());
+        _entitySimulation.unlock();
+        _entities.getTree()->unlock();
+
+        _entitySimulation.lock();
         _entitySimulation.applyActionChanges();
         _entitySimulation.unlock();
 
+
         AvatarManager* avatarManager = DependencyManager::get<AvatarManager>().data();
         _physicsEngine.deleteObjects(avatarManager->getObjectsToDelete());
         _physicsEngine.addObjects(avatarManager->getObjectsToAdd());
         _physicsEngine.changeObjects(avatarManager->getObjectsToChange());
 
+        _entities.getTree()->lockForWrite();
         _physicsEngine.stepSimulation();
+        _entities.getTree()->unlock();
 
         if (_physicsEngine.hasOutgoingChanges()) {
+            _entities.getTree()->lockForWrite();
             _entitySimulation.lock();
             _entitySimulation.handleOutgoingChanges(_physicsEngine.getOutgoingChanges(), _physicsEngine.getSessionID());
             _entitySimulation.unlock();
+            _entities.getTree()->unlock();
 
+            _entities.getTree()->lockForWrite();
             avatarManager->handleOutgoingChanges(_physicsEngine.getOutgoingChanges());
+            _entities.getTree()->unlock();
+
             auto collisionEvents = _physicsEngine.getCollisionEvents();
             avatarManager->handleCollisionEvents(collisionEvents);
 

libraries/physics/src/EntityMotionState.cpp

@@ -19,6 +19,10 @@
 #include "PhysicsHelpers.h"
 #include "PhysicsLogging.h"
 
+#ifdef WANT_DEBUG_ENTITY_TREE_LOCKS
+#include "EntityTree.h"
+#endif
+
 static const float ACCELERATION_EQUIVALENT_EPSILON_RATIO = 0.1f;
 static const quint8 STEPS_TO_DECIDE_BALLISTIC = 4;
 
@@ -42,6 +46,7 @@ EntityMotionState::EntityMotionState(btCollisionShape* shape, EntityItemPointer
 {
     _type = MOTIONSTATE_TYPE_ENTITY;
     assert(_entity != nullptr);
+    assert(entityTreeIsLocked());
     setMass(_entity->computeMass());
 }
 
@@ -50,7 +55,35 @@ EntityMotionState::~EntityMotionState() {
     assert(!_entity);
 }
 
+#ifdef WANT_DEBUG_ENTITY_TREE_LOCKS
+bool EntityMotionState::entityTreeIsLocked() const {
+    EntityTreeElement* element = _entity ? _entity->getElement() : nullptr;
+    EntityTree* tree = element ? element->getTree() : nullptr;
+    if (tree) {
+        bool readSuccess = tree->tryLockForRead();
+        if (readSuccess) {
+            tree->unlock();
+        }
+        bool writeSuccess = tree->tryLockForWrite();
+        if (writeSuccess) {
+            tree->unlock();
+        }
+        if (readSuccess && writeSuccess) {
+            return false;  // if we can take either kind of lock, there was no tree lock.
+        }
+        return true; // either read or write failed, so there is some lock in place.
+    } else {
+        return true;
+    }
+}
+#else
+bool EntityMotionState::entityTreeIsLocked() const {
+    return true;
+}
+#endif
+
 void EntityMotionState::updateServerPhysicsVariables() {
+    assert(entityTreeIsLocked());
     _serverPosition = _entity->getPosition();
     _serverRotation = _entity->getRotation();
     _serverVelocity = _entity->getVelocity();
@@ -60,6 +93,7 @@ void EntityMotionState::updateServerPhysicsVariables() {
 
 // virtual
 void EntityMotionState::handleEasyChanges(uint32_t flags) {
+    assert(entityTreeIsLocked());
     updateServerPhysicsVariables();
     ObjectMotionState::handleEasyChanges(flags);
     if (flags & EntityItem::DIRTY_SIMULATOR_ID) {
@@ -101,6 +135,7 @@ MotionType EntityMotionState::computeObjectMotionType() const {
     if (!_entity) {
         return MOTION_TYPE_STATIC;
     }
+    assert(entityTreeIsLocked());
     if (_entity->getCollisionsWillMove()) {
         return MOTION_TYPE_DYNAMIC;
     }
@@ -108,6 +143,7 @@ MotionType EntityMotionState::computeObjectMotionType() const {
 }
 
 bool EntityMotionState::isMoving() const {
+    assert(entityTreeIsLocked());
     return _entity && _entity->isMoving();
 }
 
@@ -120,6 +156,7 @@ void EntityMotionState::getWorldTransform(btTransform& worldTrans) const {
     if (!_entity) {
         return;
     }
+    assert(entityTreeIsLocked());
     if (_motionType == MOTION_TYPE_KINEMATIC) {
         // This is physical kinematic motion which steps strictly by the subframe count
         // of the physics simulation.
@@ -140,6 +177,7 @@ void EntityMotionState::setWorldTransform(const btTransform& worldTrans) {
     if (!_entity) {
         return;
     }
+    assert(entityTreeIsLocked());
     measureBodyAcceleration();
     _entity->setPosition(bulletToGLM(worldTrans.getOrigin()) + ObjectMotionState::getWorldOffset());
     _entity->setRotation(bulletToGLM(worldTrans.getRotation()));
@@ -164,9 +202,12 @@ void EntityMotionState::setWorldTransform(const btTransform& worldTrans) {
     #ifdef WANT_DEBUG
         quint64 now = usecTimestampNow();
         qCDebug(physics) << "EntityMotionState::setWorldTransform()... changed entity:" << _entity->getEntityItemID();
-        qCDebug(physics) << "       last edited:" << _entity->getLastEdited() << formatUsecTime(now - _entity->getLastEdited()) << "ago";
+        qCDebug(physics) << "       last edited:" << _entity->getLastEdited()
-        qCDebug(physics) << "    last simulated:" << _entity->getLastSimulated() << formatUsecTime(now - _entity->getLastSimulated()) << "ago";
+                         << formatUsecTime(now - _entity->getLastEdited()) << "ago";
-        qCDebug(physics) << "      last updated:" << _entity->getLastUpdated() << formatUsecTime(now - _entity->getLastUpdated()) << "ago";
+        qCDebug(physics) << "    last simulated:" << _entity->getLastSimulated()
+                         << formatUsecTime(now - _entity->getLastSimulated()) << "ago";
+        qCDebug(physics) << "      last updated:" << _entity->getLastUpdated()
+                         << formatUsecTime(now - _entity->getLastUpdated()) << "ago";
     #endif
 }
 
@@ -174,16 +215,18 @@ void EntityMotionState::setWorldTransform(const btTransform& worldTrans) {
 btCollisionShape* EntityMotionState::computeNewShape() {
     if (_entity) {
         ShapeInfo shapeInfo;
+        assert(entityTreeIsLocked());
         _entity->computeShapeInfo(shapeInfo);
         return getShapeManager()->getShape(shapeInfo);
     }
     return nullptr;
 }
 
-bool EntityMotionState::isCandidateForOwnership(const QUuid& sessionID) const { 
+bool EntityMotionState::isCandidateForOwnership(const QUuid& sessionID) const {
     if (!_body || !_entity) {
         return false;
     }
+    assert(entityTreeIsLocked());
     return _candidateForOwnership || sessionID == _entity->getSimulatorID();
 }
 
@@ -200,7 +243,7 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) {
         _sentActive = false;
         return false;
     }
-    
+
     #ifdef WANT_DEBUG
     glm::vec3 wasPosition = _serverPosition;
     glm::quat wasRotation = _serverRotation;
@@ -213,7 +256,7 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) {
     const float INACTIVE_UPDATE_PERIOD = 0.5f;
     if (!_sentActive) {
         // we resend the inactive update every INACTIVE_UPDATE_PERIOD
-        // until it is removed from the outgoing updates 
+        // until it is removed from the outgoing updates
         // (which happens when we don't own the simulation and it isn't touching our simulation)
         return (dt > INACTIVE_UPDATE_PERIOD);
     }
@@ -231,10 +274,10 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) {
         _serverPosition += dt * _serverVelocity;
     }
 
-    // Else we measure the error between current and extrapolated transform (according to expected behavior 
+    // Else we measure the error between current and extrapolated transform (according to expected behavior
     // of remote EntitySimulation) and return true if the error is significant.
 
-    // NOTE: math is done in the simulation-frame, which is NOT necessarily the same as the world-frame 
+    // NOTE: math is done in the simulation-frame, which is NOT necessarily the same as the world-frame
     // due to _worldOffset.
     // TODO: compensate for _worldOffset offset here
 
@@ -242,7 +285,7 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) {
 
     btTransform worldTrans = _body->getWorldTransform();
     glm::vec3 position = bulletToGLM(worldTrans.getOrigin());
-    
+
     float dx2 = glm::distance2(position, _serverPosition);
 
     const float MAX_POSITION_ERROR_SQUARED = 0.000004f; //  Sqrt() - corresponds to 2 millimeters
@@ -258,13 +301,13 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) {
 
         return true;
     }
-    
+
     if (glm::length2(_serverAngularVelocity) > 0.0f) {
         // compute rotation error
         float attenuation = powf(1.0f - _body->getAngularDamping(), dt);
         _serverAngularVelocity *= attenuation;
-   
+
-        // Bullet caps the effective rotation velocity inside its rotation integration step, therefore 
+        // Bullet caps the effective rotation velocity inside its rotation integration step, therefore
         // we must integrate with the same algorithm and timestep in order achieve similar results.
         for (int i = 0; i < numSteps; ++i) {
             _serverRotation = glm::normalize(computeBulletRotationStep(_serverAngularVelocity, PHYSICS_ENGINE_FIXED_SUBSTEP) * _serverRotation);
@@ -276,7 +319,7 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) {
     #ifdef WANT_DEBUG
         if ((fabsf(glm::dot(actualRotation, _serverRotation)) < MIN_ROTATION_DOT)) {
             qCDebug(physics) << ".... ((fabsf(glm::dot(actualRotation, _serverRotation)) < MIN_ROTATION_DOT)) ....";
-        
+
             qCDebug(physics) << "wasAngularVelocity:" << wasAngularVelocity;
             qCDebug(physics) << "_serverAngularVelocity:" << _serverAngularVelocity;
 
@@ -293,10 +336,11 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) {
 }
 
 bool EntityMotionState::shouldSendUpdate(uint32_t simulationStep, const QUuid& sessionID) {
-    // NOTE: we expect _entity and _body to be valid in this context, since shouldSendUpdate() is only called 
+    // NOTE: we expect _entity and _body to be valid in this context, since shouldSendUpdate() is only called
-    // after doesNotNeedToSendUpdate() returns false and that call should return 'true' if _entity or _body are NULL. 
+    // after doesNotNeedToSendUpdate() returns false and that call should return 'true' if _entity or _body are NULL.
     assert(_entity);
     assert(_body);
+    assert(entityTreeIsLocked());
 
     if (!remoteSimulationOutOfSync(simulationStep)) {
         _candidateForOwnership = false;
@@ -326,6 +370,7 @@ bool EntityMotionState::shouldSendUpdate(uint32_t simulationStep, const QUuid& s
 
 void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, const QUuid& sessionID, uint32_t step) {
     assert(_entity);
+    assert(entityTreeIsLocked());
 
     bool active = _body->isActive();
     if (!active) {
@@ -435,17 +480,18 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, const Q
     _lastStep = step;
 }
 
-uint32_t EntityMotionState::getAndClearIncomingDirtyFlags() { 
+uint32_t EntityMotionState::getAndClearIncomingDirtyFlags() {
+    assert(entityTreeIsLocked());
     uint32_t dirtyFlags = 0;
     if (_body && _entity) {
-        dirtyFlags = _entity->getDirtyFlags(); 
+        dirtyFlags = _entity->getDirtyFlags();
         _entity->clearDirtyFlags();
         // we add DIRTY_MOTION_TYPE if the body's motion type disagrees with entity velocity settings
         int bodyFlags = _body->getCollisionFlags();
         bool isMoving = _entity->isMoving();
         if (((bodyFlags & btCollisionObject::CF_STATIC_OBJECT) && isMoving) ||
                 (bodyFlags & btCollisionObject::CF_KINEMATIC_OBJECT && !isMoving)) {
-            dirtyFlags |= EntityItem::DIRTY_MOTION_TYPE; 
+            dirtyFlags |= EntityItem::DIRTY_MOTION_TYPE;
         }
     }
     return dirtyFlags;
@@ -455,6 +501,7 @@ uint32_t EntityMotionState::getAndClearIncomingDirtyFlags() {
 // virtual
 QUuid EntityMotionState::getSimulatorID() const {
     if (_entity) {
+        assert(entityTreeIsLocked());
         return _entity->getSimulatorID();
     }
     return QUuid();
@@ -469,12 +516,12 @@ void EntityMotionState::bump() {
 void EntityMotionState::resetMeasuredBodyAcceleration() {
     _lastMeasureStep = ObjectMotionState::getWorldSimulationStep();
     if (_body) {
-        _lastVelocity = bulletToGLM(_body->getLinearVelocity());                                                        
+        _lastVelocity = bulletToGLM(_body->getLinearVelocity());
     } else {
         _lastVelocity = glm::vec3(0.0f);
     }
     _measuredAcceleration = glm::vec3(0.0f);
-} 
+}
 
 void EntityMotionState::measureBodyAcceleration() {
     // try to manually measure the true acceleration of the object
@@ -504,7 +551,7 @@ glm::vec3 EntityMotionState::getObjectLinearVelocityChange() const {
     return _measuredAcceleration * _measuredDeltaTime;
 }
 
-// virtual 
+// virtual
 void EntityMotionState::setMotionType(MotionType motionType) {
     ObjectMotionState::setMotionType(motionType);
     resetMeasuredBodyAcceleration();
@@ -514,12 +561,13 @@ void EntityMotionState::setMotionType(MotionType motionType) {
 // virtual
 QString EntityMotionState::getName() {
     if (_entity) {
+        assert(entityTreeIsLocked());
         return _entity->getName();
     }
     return "";
 }
 
-// virtual 
+// virtual
 int16_t EntityMotionState::computeCollisionGroup() {
     switch (computeObjectMotionType()){
         case MOTION_TYPE_STATIC:

libraries/physics/src/EntityMotionState.h

@@ -83,6 +83,10 @@ public:
     friend class PhysicalEntitySimulation;
 
 protected:
+    #ifdef WANT_DEBUG_ENTITY_TREE_LOCKS
+    bool entityTreeIsLocked() const;
+    #endif
+
     virtual btCollisionShape* computeNewShape();
     virtual void clearObjectBackPointer();
     virtual void setMotionType(MotionType motionType);