overhaul of MotionState and shape creations

interface/src/Application.cpp

@@ -2782,21 +2782,15 @@ Application::~Application() {
     // remove avatars from physics engine
     auto avatarManager = DependencyManager::get<AvatarManager>();
     avatarManager->clearOtherAvatars();
+    auto myCharacterController = getMyAvatar()->getCharacterController();
+    myCharacterController->clearDetailedMotionStates();
 
     PhysicsEngine::Transaction transaction;
     avatarManager->buildPhysicsTransaction(transaction);
     _physicsEngine->processTransaction(transaction);
     avatarManager->handleProcessedPhysicsTransaction(transaction);
-
     avatarManager->deleteAllAvatars();
 
-    auto myCharacterController = getMyAvatar()->getCharacterController();
-    myCharacterController->clearDetailedMotionStates();
-
-    myCharacterController->buildPhysicsTransaction(transaction);
-    _physicsEngine->processTransaction(transaction);
-    myCharacterController->handleProcessedPhysicsTransaction(transaction);
-
     _physicsEngine->setCharacterController(nullptr);
 
     // the _shapeManager should have zero references
@@ -6412,32 +6406,11 @@ void Application::update(float deltaTime) {
                 PROFILE_RANGE(simulation_physics, "PrePhysics");
                 PerformanceTimer perfTimer("prePhysics)");
                 {
-                    PROFILE_RANGE(simulation_physics, "RemoveEntities");
+                    PROFILE_RANGE(simulation_physics, "Entities");
-                    const VectorOfMotionStates& motionStates = _entitySimulation->getObjectsToRemoveFromPhysics();
+                    PhysicsEngine::Transaction transaction;
-                    {
+                    _entitySimulation->buildPhysicsTransaction(transaction);
-                        PROFILE_RANGE_EX(simulation_physics, "NumObjs", 0xffff0000, (uint64_t)motionStates.size());
+                    _physicsEngine->processTransaction(transaction);
-                        _physicsEngine->removeObjects(motionStates);
+                    _entitySimulation->handleProcessedPhysicsTransaction(transaction);
-                    }
-                    _entitySimulation->deleteObjectsRemovedFromPhysics();
-                }
-
-                {
-                    PROFILE_RANGE(simulation_physics, "AddEntities");
-                    VectorOfMotionStates motionStates;
-                    getEntities()->getTree()->withReadLock([&] {
-                        _entitySimulation->getObjectsToAddToPhysics(motionStates);
-                        PROFILE_RANGE_EX(simulation_physics, "NumObjs", 0xffff0000, (uint64_t)motionStates.size());
-                        _physicsEngine->addObjects(motionStates);
-                    });
-                }
-                {
-                    VectorOfMotionStates motionStates;
-                    PROFILE_RANGE(simulation_physics, "ChangeEntities");
-                    getEntities()->getTree()->withReadLock([&] {
-                        _entitySimulation->getObjectsToChange(motionStates);
-                        VectorOfMotionStates stillNeedChange = _physicsEngine->changeObjects(motionStates);
-                        _entitySimulation->setObjectsToChange(stillNeedChange);
-                    });
                 }
 
                 _entitySimulation->applyDynamicChanges();
@@ -6450,9 +6423,6 @@ void Application::update(float deltaTime) {
                     avatarManager->buildPhysicsTransaction(transaction);
                     _physicsEngine->processTransaction(transaction);
                     avatarManager->handleProcessedPhysicsTransaction(transaction);
-                    myAvatar->getCharacterController()->buildPhysicsTransaction(transaction);
-                    _physicsEngine->processTransaction(transaction);
-                    myAvatar->getCharacterController()->handleProcessedPhysicsTransaction(transaction);
                     myAvatar->prepareForPhysicsSimulation();
                     _physicsEngine->enableGlobalContactAddedCallback(myAvatar->isFlying());
                 }

interface/src/avatar/AvatarManager.cpp

@@ -101,7 +101,7 @@ AvatarSharedPointer AvatarManager::addAvatar(const QUuid& sessionUUID, const QWe
 }
 
 AvatarManager::~AvatarManager() {
-    assert(_avatarsToChangeInPhysics.empty());
+    assert(_otherAvatarsToChangeInPhysics.empty());
 }
 
 void AvatarManager::init() {
@@ -314,7 +314,7 @@ void AvatarManager::updateOtherAvatars(float deltaTime) {
                 // remove the orb if it is there
                 avatar->removeOrb();
                 if (avatar->needsPhysicsUpdate()) {
-                    _avatarsToChangeInPhysics.insert(avatar);
+                    _otherAvatarsToChangeInPhysics.insert(avatar);
                 }
             } else {
                 avatar->updateOrbPosition();
@@ -419,69 +419,112 @@ AvatarSharedPointer AvatarManager::newSharedAvatar(const QUuid& sessionUUID) {
 }
 
 void AvatarManager::queuePhysicsChange(const OtherAvatarPointer& avatar) {
-    _avatarsToChangeInPhysics.insert(avatar);
+    _otherAvatarsToChangeInPhysics.insert(avatar);
+}
+
+DetailedMotionState* AvatarManager::createDetailedMotionState(OtherAvatarPointer avatar, int32_t jointIndex) {
+    bool isBound = false;
+    std::vector<int32_t> boundJoints;
+    const btCollisionShape* shape = avatar->createCollisionShape(jointIndex, isBound, boundJoints);
+    if (shape) {
+        //std::shared_ptr<OtherAvatar> avatar = shared_from_this();
+        //std::shared_ptr<Avatar> avatar = getThisPointer();
+        DetailedMotionState* motionState = new DetailedMotionState(avatar, shape, jointIndex);
+        motionState->setMass(0.0f); // DetailedMotionState has KINEMATIC MotionType, so zero mass is ok
+        motionState->setIsBound(isBound, boundJoints);
+        return motionState;
+    }
+    return nullptr;
+}
+
+void AvatarManager::rebuildAvatarPhysics(PhysicsEngine::Transaction& transaction, OtherAvatarPointer avatar) {
+    if (!avatar->_motionState) {
+        avatar->_motionState = new AvatarMotionState(avatar, nullptr);
+    }
+    AvatarMotionState* motionState = avatar->_motionState;
+    ShapeInfo shapeInfo;
+    avatar->computeShapeInfo(shapeInfo);
+    const btCollisionShape* shape = ObjectMotionState::getShapeManager()->getShape(shapeInfo);
+    assert(shape);
+    motionState->setShape(shape);
+    motionState->setMass(avatar->computeMass());
+    if (motionState->getRigidBody()) {
+        transaction.objectsToReinsert.push_back(motionState);
+    } else {
+        transaction.objectsToAdd.push_back(motionState);
+    }
+    motionState->clearIncomingDirtyFlags();
+
+    // Rather than reconcile numbers of joints after change to model or LOD
+    // we blow away old detailedMotionStates and create anew all around.
+
+    // delete old detailedMotionStates
+    auto& detailedMotionStates = avatar->getDetailedMotionStates();
+    if (detailedMotionStates.size() != 0) {
+        for (auto& detailedMotionState : detailedMotionStates) {
+            transaction.objectsToRemove.push_back(detailedMotionState);
+        }
+        avatar->resetDetailedMotionStates();
+    }
+
+    // build new detailedMotionStates
+    OtherAvatar::BodyLOD lod = avatar->getBodyLOD();
+    if (lod == OtherAvatar::BodyLOD::Sphere) {
+        auto dMotionState = createDetailedMotionState(avatar, -1);
+        detailedMotionStates.push_back(dMotionState);
+    } else {
+        int32_t numJoints = avatar->getJointCount();
+        for (int32_t i = 0; i < numJoints; i++) {
+            auto dMotionState = createDetailedMotionState(avatar, i);
+            if (dMotionState) {
+                detailedMotionStates.push_back(dMotionState);
+            }
+        }
+    }
+    avatar->_needsReinsertion = false;
 }
 
 void AvatarManager::buildPhysicsTransaction(PhysicsEngine::Transaction& transaction) {
-    SetOfOtherAvatars failedShapeBuilds;
+    _myAvatar->getCharacterController()->buildPhysicsTransaction(transaction);
-    for (auto avatar : _avatarsToChangeInPhysics) {
+    for (auto avatar : _otherAvatarsToChangeInPhysics) {
         bool isInPhysics = avatar->isInPhysicsSimulation();
         if (isInPhysics != avatar->shouldBeInPhysicsSimulation()) {
             if (isInPhysics) {
                 transaction.objectsToRemove.push_back(avatar->_motionState);
                 avatar->_motionState = nullptr;
                 auto& detailedMotionStates = avatar->getDetailedMotionStates();
-                for (auto& mState : detailedMotionStates) {
+                for (auto& motionState : detailedMotionStates) {
-                    transaction.objectsToRemove.push_back(mState);
+                    transaction.objectsToRemove.push_back(motionState);
                 }
                 avatar->resetDetailedMotionStates();
             } else {
-                if (avatar->getDetailedMotionStates().size() == 0) {
+                rebuildAvatarPhysics(transaction, avatar);
-                    avatar->createDetailedMotionStates(avatar);
-                    for (auto dMotionState : avatar->getDetailedMotionStates()) {
-                        transaction.objectsToAdd.push_back(dMotionState);
-                    }
-                }
-                if (avatar->getDetailedMotionStates().size() > 0) {
-                    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 {
-                    failedShapeBuilds.insert(avatar);
-                }
             }
         } else if (isInPhysics) {
-            transaction.objectsToChange.push_back(avatar->_motionState);
+            AvatarMotionState* motionState = avatar->_motionState;
-            
+            if (motionState->needsNewShape()) {
-            auto& detailedMotionStates = avatar->getDetailedMotionStates();
+                rebuildAvatarPhysics(transaction, avatar);
-            for (auto& mState : detailedMotionStates) {
+            } else {
-                if (mState) {
+                uint32_t flags = motionState->getIncomingDirtyFlags();
-                    transaction.objectsToChange.push_back(mState);
+                motionState->clearIncomingDirtyFlags();
+                if (flags | EASY_DIRTY_PHYSICS_FLAGS) {
+                    motionState->handleEasyChanges(flags);
+                }
+                // NOTE: we don't call detailedMotionState->handleEasyChanges() here is because they are KINEMATIC
+                // and Bullet will automagically call DetailedMotionState::getWorldTransform() on all that are active.
+
+                if (flags | (Simulation::DIRTY_MOTION_TYPE | Simulation::DIRTY_COLLISION_GROUP)) {
+                    transaction.objectsToReinsert.push_back(motionState);
+                    for (auto detailedMotionState : avatar->getDetailedMotionStates()) {
+                        transaction.objectsToReinsert.push_back(detailedMotionState);
+                    }
                 }
             }
-            
         }
     }
-    _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;
@@ -570,7 +613,7 @@ void AvatarManager::clearOtherAvatars() {
                 ++avatarIterator;
             }
         }
-    }    
+    }
 
     for (auto& av : removedAvatars) {
         handleRemovedAvatar(av);
@@ -578,7 +621,7 @@ void AvatarManager::clearOtherAvatars() {
 }
 
 void AvatarManager::deleteAllAvatars() {
-    assert(_avatarsToChangeInPhysics.empty());
+    assert(_otherAvatarsToChangeInPhysics.empty());
     QReadLocker locker(&_hashLock);
     AvatarHash::iterator avatarIterator = _avatarHash.begin();
     while (avatarIterator != _avatarHash.end()) {
@@ -588,7 +631,7 @@ void AvatarManager::deleteAllAvatars() {
         if (avatar != _myAvatar) {
             auto otherAvatar = std::static_pointer_cast<OtherAvatar>(avatar);
             assert(!otherAvatar->_motionState);
-            assert(otherAvatar->getDetailedMotionStates().size() == 0);            
+            assert(otherAvatar->getDetailedMotionStates().size() == 0);
         }
     }
 }

interface/src/avatar/AvatarManager.h

@@ -273,6 +273,8 @@ public slots:
 
 protected:
     AvatarSharedPointer addAvatar(const QUuid& sessionUUID, const QWeakPointer<Node>& mixerWeakPointer) override;
+    DetailedMotionState* createDetailedMotionState(OtherAvatarPointer avatar, int32_t jointIndex);
+    void rebuildAvatarPhysics(PhysicsEngine::Transaction& transaction, OtherAvatarPointer avatar);
 
 private:
     explicit AvatarManager(QObject* parent = 0);
@@ -288,7 +290,7 @@ private:
     void handleTransitAnimations(AvatarTransit::Status status);
 
     using SetOfOtherAvatars = std::set<OtherAvatarPointer>;
-    SetOfOtherAvatars _avatarsToChangeInPhysics;
+    SetOfOtherAvatars _otherAvatarsToChangeInPhysics;
 
     std::shared_ptr<MyAvatar> _myAvatar;
     quint64 _lastSendAvatarDataTime = 0; // Controls MyAvatar send data rate.

interface/src/avatar/AvatarMotionState.cpp

@@ -29,17 +29,13 @@ void AvatarMotionState::handleEasyChanges(uint32_t& flags) {
     }
 }
 
-bool AvatarMotionState::handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine) {
-    return ObjectMotionState::handleHardAndEasyChanges(flags, engine);
-}
-
 AvatarMotionState::~AvatarMotionState() {
     assert(_avatar);
     _avatar = nullptr;
 }
 
 // virtual
-uint32_t AvatarMotionState::getIncomingDirtyFlags() {
+uint32_t AvatarMotionState::getIncomingDirtyFlags() const {
     return _body ? _dirtyFlags : 0;
 }
 
@@ -54,13 +50,6 @@ PhysicsMotionType AvatarMotionState::computePhysicsMotionType() const {
     return MOTION_TYPE_DYNAMIC;
 }
 
-// virtual and protected
-const btCollisionShape* AvatarMotionState::computeNewShape() {
-    ShapeInfo shapeInfo;
-    _avatar->computeShapeInfo(shapeInfo);
-    return getShapeManager()->getShape(shapeInfo);
-}
-
 // virtual
 bool AvatarMotionState::isMoving() const {
     return false;

interface/src/avatar/AvatarMotionState.h

@@ -23,44 +23,44 @@ class AvatarMotionState : public ObjectMotionState {
 public:
     AvatarMotionState(OtherAvatarPointer avatar, const btCollisionShape* shape);
 
-    virtual void handleEasyChanges(uint32_t& flags) override;
+    void handleEasyChanges(uint32_t& flags) override;
-    virtual bool handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine) override;
 
-    virtual PhysicsMotionType getMotionType() const override { return _motionType; }
+    PhysicsMotionType getMotionType() const override { return _motionType; }
 
-    virtual uint32_t getIncomingDirtyFlags() override;
+    uint32_t getIncomingDirtyFlags() const override;
-    virtual void clearIncomingDirtyFlags() override;
+    void clearIncomingDirtyFlags() override;
 
-    virtual PhysicsMotionType computePhysicsMotionType() const override;
+    PhysicsMotionType computePhysicsMotionType() const override;
 
-    virtual bool isMoving() const override;
+    bool isMoving() const override;
 
     // this relays incoming position/rotation to the RigidBody
-    virtual void getWorldTransform(btTransform& worldTrans) const override;
+    void getWorldTransform(btTransform& worldTrans) const override;
 
     // this relays outgoing position/rotation to the EntityItem
-    virtual void setWorldTransform(const btTransform& worldTrans) override;
+    void setWorldTransform(const btTransform& worldTrans) override;
 
 
     // These pure virtual methods must be implemented for each MotionState type
     // and make it possible to implement more complicated methods in this base class.
 
     // pure virtual overrides from ObjectMotionState
-    virtual float getObjectRestitution() const override;
+    float getObjectRestitution() const override;
-    virtual float getObjectFriction() const override;
+    float getObjectFriction() const override;
-    virtual float getObjectLinearDamping() const override;
+    float getObjectLinearDamping() const override;
-    virtual float getObjectAngularDamping() const override;
+    float getObjectAngularDamping() const override;
 
-    virtual glm::vec3 getObjectPosition() const override;
+    glm::vec3 getObjectPosition() const override;
-    virtual glm::quat getObjectRotation() const override;
+    glm::quat getObjectRotation() const override;
-    virtual glm::vec3 getObjectLinearVelocity() const override;
+    glm::vec3 getObjectLinearVelocity() const override;
-    virtual glm::vec3 getObjectAngularVelocity() const override;
+    glm::vec3 getObjectAngularVelocity() const override;
-    virtual glm::vec3 getObjectGravity() const override;
+    glm::vec3 getObjectGravity() const override;
 
-    virtual const QUuid getObjectID() const override;
+    const QUuid getObjectID() const override;
 
-    virtual QString getName() const override;
+    QString getName() const override;
-    virtual QUuid getSimulatorID() const override;
+    ShapeType getShapeType() const override { return SHAPE_TYPE_CAPSULE_Y; }
+    QUuid getSimulatorID() const override;
 
     void setBoundingBox(const glm::vec3& corner, const glm::vec3& diagonal);
 
@@ -69,9 +69,9 @@ public:
     void setCollisionGroup(int32_t group) { _collisionGroup = group; }
     int32_t getCollisionGroup() { return _collisionGroup; }
 
-    virtual void computeCollisionGroupAndMask(int32_t& group, int32_t& mask) const override;
+    void computeCollisionGroupAndMask(int32_t& group, int32_t& mask) const override;
 
-    virtual float getMass() const override;
+    float getMass() const override;
 
     friend class AvatarManager;
     friend class Avatar;
@@ -85,9 +85,6 @@ protected:
     // ever called by the Avatar class dtor.
     ~AvatarMotionState();
 
-    virtual bool isReadyToComputeShape() const override { return true; }
-    virtual const btCollisionShape* computeNewShape() override;
-
     OtherAvatarPointer _avatar;
     float _diameter { 0.0f };
     int32_t _collisionGroup;

interface/src/avatar/DetailedMotionState.cpp

@@ -17,7 +17,7 @@
 #include "MyAvatar.h"
 
 
-DetailedMotionState::DetailedMotionState(AvatarPointer avatar, const btCollisionShape* shape, int jointIndex) : 
+DetailedMotionState::DetailedMotionState(AvatarPointer avatar, const btCollisionShape* shape, int32_t jointIndex) : 
     ObjectMotionState(shape), _avatar(avatar), _jointIndex(jointIndex) {
     assert(_avatar);
     if (!_avatar->isMyAvatar()) {
@@ -33,17 +33,13 @@ void DetailedMotionState::handleEasyChanges(uint32_t& flags) {
     }
 }
 
-bool DetailedMotionState::handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine) {
-    return ObjectMotionState::handleHardAndEasyChanges(flags, engine);
-}
-
 DetailedMotionState::~DetailedMotionState() {
     assert(_avatar);
     _avatar = nullptr;
 }
 
 // virtual
-uint32_t DetailedMotionState::getIncomingDirtyFlags() {
+uint32_t DetailedMotionState::getIncomingDirtyFlags() const {
     return _body ? _dirtyFlags : 0;
 }
 
@@ -54,26 +50,9 @@ void DetailedMotionState::clearIncomingDirtyFlags() {
 }
 
 PhysicsMotionType DetailedMotionState::computePhysicsMotionType() const {
-    // TODO?: support non-DYNAMIC motion for avatars? (e.g. when sitting)
     return MOTION_TYPE_KINEMATIC;
 }
 
-// virtual and protected
-const btCollisionShape* DetailedMotionState::computeNewShape() {
-    btCollisionShape* shape = nullptr;
-    if (!_avatar->isMyAvatar()) {
-        if (_otherAvatar != nullptr) {
-            shape =  _otherAvatar->createCollisionShape(_jointIndex, _isBound, _boundJoints);
-        }
-    } else {
-        std::shared_ptr<MyAvatar> myAvatar = std::static_pointer_cast<MyAvatar>(_avatar);
-        if (myAvatar) {
-            shape = myAvatar->getCharacterController()->createDetailedCollisionShapeForJoint(_jointIndex);
-        }
-    }
-    return shape;
-}
-
 // virtual
 bool DetailedMotionState::isMoving() const {
     return false;
@@ -185,4 +164,4 @@ void DetailedMotionState::forceActive() {
     if (_body && !_body->isActive()) {
         _body->setActivationState(ACTIVE_TAG);
     }
-}
+}

interface/src/avatar/DetailedMotionState.h

@@ -23,55 +23,55 @@ class DetailedMotionState : public ObjectMotionState {
 public:
     DetailedMotionState(AvatarPointer avatar, const btCollisionShape* shape, int jointIndex);
 
-    virtual void handleEasyChanges(uint32_t& flags) override;
+    void handleEasyChanges(uint32_t& flags) override;
-    virtual bool handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine) override;
 
-    virtual PhysicsMotionType getMotionType() const override { return _motionType; }
+    PhysicsMotionType getMotionType() const override { return _motionType; }
 
-    virtual uint32_t getIncomingDirtyFlags() override;
+    uint32_t getIncomingDirtyFlags() const override;
-    virtual void clearIncomingDirtyFlags() override;
+    void clearIncomingDirtyFlags() override;
 
-    virtual PhysicsMotionType computePhysicsMotionType() const override;
+    PhysicsMotionType computePhysicsMotionType() const override;
 
-    virtual bool isMoving() const override;
+    bool isMoving() const override;
 
     // this relays incoming position/rotation to the RigidBody
-    virtual void getWorldTransform(btTransform& worldTrans) const override;
+    void getWorldTransform(btTransform& worldTrans) const override;
 
     // this relays outgoing position/rotation to the EntityItem
-    virtual void setWorldTransform(const btTransform& worldTrans) override;
+    void setWorldTransform(const btTransform& worldTrans) override;
 
 
     // These pure virtual methods must be implemented for each MotionState type
     // and make it possible to implement more complicated methods in this base class.
 
     // pure virtual overrides from ObjectMotionState
-    virtual float getObjectRestitution() const override;
+    float getObjectRestitution() const override;
-    virtual float getObjectFriction() const override;
+    float getObjectFriction() const override;
-    virtual float getObjectLinearDamping() const override;
+    float getObjectLinearDamping() const override;
-    virtual float getObjectAngularDamping() const override;
+    float getObjectAngularDamping() const override;
 
-    virtual glm::vec3 getObjectPosition() const override;
+    glm::vec3 getObjectPosition() const override;
-    virtual glm::quat getObjectRotation() const override;
+    glm::quat getObjectRotation() const override;
-    virtual glm::vec3 getObjectLinearVelocity() const override;
+    glm::vec3 getObjectLinearVelocity() const override;
-    virtual glm::vec3 getObjectAngularVelocity() const override;
+    glm::vec3 getObjectAngularVelocity() const override;
-    virtual glm::vec3 getObjectGravity() const override;
+    glm::vec3 getObjectGravity() const override;
 
-    virtual const QUuid getObjectID() const override;
+    const QUuid getObjectID() const override;
 
-    virtual QString getName() const override;
+    QString getName() const override;
-    virtual QUuid getSimulatorID() const override;
+    ShapeType getShapeType() const override { return SHAPE_TYPE_HULL; }
+    QUuid getSimulatorID() const override;
 
     void addDirtyFlags(uint32_t flags) { _dirtyFlags |= flags; }
 
-    virtual void computeCollisionGroupAndMask(int32_t& group, int32_t& mask) const override;
+    void computeCollisionGroupAndMask(int32_t& group, int32_t& mask) const override;
 
-    virtual float getMass() const override;
+    float getMass() const override;
     void forceActive();
     QUuid getAvatarID() const { return _avatar->getID(); }
-    int getJointIndex() const { return _jointIndex; }
+    int32_t getJointIndex() const { return _jointIndex; }
-    void setIsBound(bool isBound, std::vector<int> boundJoints) { _isBound = isBound; _boundJoints = boundJoints; }
+    void setIsBound(bool isBound, const std::vector<int32_t>& boundJoints) { _isBound = isBound; _boundJoints = boundJoints; }
-    bool getIsBound(std::vector<int>& boundJoints) const { boundJoints = _boundJoints; return _isBound; }
+    bool getIsBound(std::vector<int32_t>& boundJoints) const { boundJoints = _boundJoints; return _isBound; }
 
     friend class AvatarManager;
     friend class Avatar;
@@ -84,17 +84,14 @@ protected:
     // ever called by the Avatar class dtor.
     ~DetailedMotionState();
 
-    virtual bool isReadyToComputeShape() const override { return true; }
-    virtual const btCollisionShape* computeNewShape() override;
-
     AvatarPointer _avatar;
     float _diameter { 0.0f };
 
     uint32_t _dirtyFlags;
-    int _jointIndex { -1 };
+    int32_t _jointIndex { -1 };
     OtherAvatarPointer _otherAvatar { nullptr };
     bool _isBound { false };
-    std::vector<int> _boundJoints;
+    std::vector<int32_t> _boundJoints;
 };
 
 #endif // hifi_DetailedMotionState_h

interface/src/avatar/MyCharacterController.cpp

@@ -377,21 +377,18 @@ void MyCharacterController::updateMassProperties() {
     _rigidBody->setMassProps(mass, inertia);
 }
 
-btCollisionShape* MyCharacterController::createDetailedCollisionShapeForJoint(int jointIndex) {
+const btCollisionShape* MyCharacterController::createDetailedCollisionShapeForJoint(int32_t jointIndex) {
     ShapeInfo shapeInfo;
     _avatar->computeDetailedShapeInfo(shapeInfo, jointIndex);
     if (shapeInfo.getType() != SHAPE_TYPE_NONE) {
-        btCollisionShape* shape = const_cast<btCollisionShape*>(ObjectMotionState::getShapeManager()->getShape(shapeInfo));
+        const btCollisionShape* shape = ObjectMotionState::getShapeManager()->getShape(shapeInfo);
-        if (shape) {
-            shape->setMargin(0.001f);
-        }
         return shape;
     }
     return nullptr;
 }
 
-DetailedMotionState* MyCharacterController::createDetailedMotionStateForJoint(int jointIndex) {
+DetailedMotionState* MyCharacterController::createDetailedMotionStateForJoint(int32_t jointIndex) {
-    auto shape = createDetailedCollisionShapeForJoint(jointIndex);
+    const btCollisionShape* shape = createDetailedCollisionShapeForJoint(jointIndex);
     if (shape) {
         DetailedMotionState* motionState = new DetailedMotionState(_avatar, shape, jointIndex);
         motionState->setMass(_avatar->computeMass());
@@ -423,25 +420,16 @@ void MyCharacterController::buildPhysicsTransaction(PhysicsEngine::Transaction&
     }
     if (_pendingFlags & PENDING_FLAG_ADD_DETAILED_TO_SIMULATION) {
         _pendingFlags &= ~PENDING_FLAG_ADD_DETAILED_TO_SIMULATION;
-        for (int i = 0; i < _avatar->getJointCount(); i++) {
+        for (int32_t i = 0; i < _avatar->getJointCount(); i++) {
             auto dMotionState = createDetailedMotionStateForJoint(i);
             if (dMotionState) {
                 _detailedMotionStates.push_back(dMotionState);
                 transaction.objectsToAdd.push_back(dMotionState);
             }
         }
-    } 
-}
-
-void MyCharacterController::handleProcessedPhysicsTransaction(PhysicsEngine::Transaction& transaction) {
-    // things on objectsToRemove are ready for delete
-    for (auto object : transaction.objectsToRemove) {
-        delete object;
     }
-    transaction.clear();
 }
 
-
 class DetailedRayResultCallback : public btCollisionWorld::AllHitsRayResultCallback {
 public:
     DetailedRayResultCallback()
@@ -467,7 +455,7 @@ std::vector<MyCharacterController::RayAvatarResult> MyCharacterController::rayTe
         _dynamicsWorld->rayTest(origin, end, rayCallback);
         if (rayCallback.m_hitFractions.size() > 0) {
             foundAvatars.reserve(rayCallback.m_hitFractions.size());
-            for (int i = 0; i < rayCallback.m_hitFractions.size(); i++) {
+            for (int32_t i = 0; i < rayCallback.m_hitFractions.size(); i++) {
                 auto object = rayCallback.m_collisionObjects[i];
                 ObjectMotionState* motionState = static_cast<ObjectMotionState*>(object->getUserPointer());
                 if (motionState && motionState->getType() == MOTIONSTATE_TYPE_DETAILED) {
@@ -493,4 +481,4 @@ std::vector<MyCharacterController::RayAvatarResult> MyCharacterController::rayTe
         }
     }
     return foundAvatars;
-}
+}

interface/src/avatar/MyCharacterController.h

@@ -44,27 +44,25 @@ public:
 
     void setDensity(btScalar density) { _density = density; }
 
-    btCollisionShape* createDetailedCollisionShapeForJoint(int jointIndex);
+    const btCollisionShape* createDetailedCollisionShapeForJoint(int32_t jointIndex);
-    DetailedMotionState* createDetailedMotionStateForJoint(int jointIndex);
+    DetailedMotionState* createDetailedMotionStateForJoint(int32_t jointIndex);
     std::vector<DetailedMotionState*>& getDetailedMotionStates() { return _detailedMotionStates; }
     void clearDetailedMotionStates();
     void resetDetailedMotionStates();
 
     void buildPhysicsTransaction(PhysicsEngine::Transaction& transaction);
-    void handleProcessedPhysicsTransaction(PhysicsEngine::Transaction& transaction);
-
 
     struct RayAvatarResult {
         bool _intersect { false };
         bool _isBound { false };
         QUuid _intersectWithAvatar;
-        int _intersectWithJoint { -1 };
+        int32_t _intersectWithJoint { -1 };
         float _distance { 0.0f };
         float _maxDistance { 0.0f };
         QVariantMap _extraInfo;
         glm::vec3 _intersectionPoint;
         glm::vec3 _intersectionNormal;
-        std::vector<int> _boundJoints;
+        std::vector<int32_t> _boundJoints;
     };
     std::vector<RayAvatarResult> rayTest(const btVector3& origin, const btVector3& direction, const btScalar& length,
                                          const QVector<uint>& jointsToExclude) const;

interface/src/avatar/OtherAvatar.cpp

@@ -116,6 +116,8 @@ void OtherAvatar::updateSpaceProxy(workload::Transaction& transaction) const {
 int OtherAvatar::parseDataFromBuffer(const QByteArray& buffer) {
     int32_t bytesRead = Avatar::parseDataFromBuffer(buffer);
     for (size_t i = 0; i < _detailedMotionStates.size(); i++) {
+        // NOTE: we activate _detailedMotionStates is because they are KINEMATIC
+        // and Bullet will automagically call DetailedMotionState::getWorldTransform() when active.
         _detailedMotionStates[i]->forceActive();
     }
     if (_moving && _motionState) {
@@ -124,11 +126,11 @@ int OtherAvatar::parseDataFromBuffer(const QByteArray& buffer) {
     return bytesRead;
 }
 
-btCollisionShape* OtherAvatar::createCollisionShape(int jointIndex, bool& isBound, std::vector<int>& boundJoints) {
+const btCollisionShape* OtherAvatar::createCollisionShape(int32_t jointIndex, bool& isBound, std::vector<int32_t>& boundJoints) {
     ShapeInfo shapeInfo;
     isBound = false;
-    QString jointName = ""; 
+    QString jointName = "";
-    if (jointIndex > -1 && jointIndex < (int)_multiSphereShapes.size()) {
+    if (jointIndex > -1 && jointIndex < (int32_t)_multiSphereShapes.size()) {
         jointName = _multiSphereShapes[jointIndex].getJointName();
     }
     switch (_bodyLOD) {
@@ -163,39 +165,21 @@ btCollisionShape* OtherAvatar::createCollisionShape(int jointIndex, bool& isBoun
             }
             break;
         }
+        // Note: MultiSphereLow case really means: "skip fingers and use spheres for hands,
+        // else fall through to MultiSphereHigh case"
     case BodyLOD::MultiSphereHigh:
         computeDetailedShapeInfo(shapeInfo, jointIndex);
         break;
     default:
+        assert(false); // should never reach here
         break;
     }
-    if (shapeInfo.getType() != SHAPE_TYPE_NONE) {
+    return ObjectMotionState::getShapeManager()->getShape(shapeInfo);
-        auto shape = const_cast<btCollisionShape*>(ObjectMotionState::getShapeManager()->getShape(shapeInfo));
-        if (shape) {
-            shape->setMargin(0.001f);
-        }
-        return shape;
-    }
-    return nullptr;
-}
-
-DetailedMotionState* OtherAvatar::createMotionState(std::shared_ptr<OtherAvatar> avatar, int jointIndex) {
-    bool isBound = false;
-    std::vector<int> boundJoints;
-    btCollisionShape* shape = createCollisionShape(jointIndex, isBound, boundJoints);
-    if (shape) {
-        DetailedMotionState* motionState = new DetailedMotionState(avatar, shape, jointIndex);
-        motionState->setMass(computeMass());
-        motionState->setIsBound(isBound, boundJoints);
-        return motionState;
-    }
-    return nullptr;
 }
 
 void OtherAvatar::resetDetailedMotionStates() {
-    for (size_t i = 0; i < _detailedMotionStates.size(); i++) {
+    // NOTE: the DetailedMotionStates are deleted after being added to PhysicsEngine::Transaction::_objectsToRemove
-        _detailedMotionStates[i] = nullptr;
+    // See AvatarManager::handleProcessedPhysicsTransaction()
-    }
     _detailedMotionStates.clear();
 }
 
@@ -231,11 +215,11 @@ void OtherAvatar::computeShapeLOD() {
 }
 
 bool OtherAvatar::isInPhysicsSimulation() const {
-    return _motionState != nullptr && _detailedMotionStates.size() > 0;
+    return _motionState && _motionState->getRigidBody();
 }
 
 bool OtherAvatar::shouldBeInPhysicsSimulation() const {
-    return !isDead() && !(isInPhysicsSimulation() && _needsReinsertion);
+    return !isDead() && _workloadRegion < workload::Region::R3;
 }
 
 bool OtherAvatar::needsPhysicsUpdate() const {
@@ -245,12 +229,9 @@ bool OtherAvatar::needsPhysicsUpdate() const {
 
 void OtherAvatar::rebuildCollisionShape() {
     if (_motionState) {
+        // do not actually rebuild here, instead flag for later
         _motionState->addDirtyFlags(Simulation::DIRTY_SHAPE | Simulation::DIRTY_MASS);
-    }
+        _needsReinsertion = true;
-    for (size_t i = 0; i < _detailedMotionStates.size(); i++) {
-        if (_detailedMotionStates[i]) {
-            _detailedMotionStates[i]->addDirtyFlags(Simulation::DIRTY_SHAPE | Simulation::DIRTY_MASS);
-        }
     }
 }
 
@@ -260,25 +241,6 @@ void OtherAvatar::setCollisionWithOtherAvatarsFlags() {
     }
 }
 
-void OtherAvatar::createDetailedMotionStates(const std::shared_ptr<OtherAvatar>& avatar) {
-    auto& detailedMotionStates = getDetailedMotionStates();
-    assert(detailedMotionStates.empty());
-    if (_bodyLOD == BodyLOD::Sphere) {
-        auto dMotionState = createMotionState(avatar, -1);
-        if (dMotionState) {
-            detailedMotionStates.push_back(dMotionState);
-        }
-    } else {
-        for (int i = 0; i < getJointCount(); i++) {
-            auto dMotionState = createMotionState(avatar, i);
-            if (dMotionState) {
-                detailedMotionStates.push_back(dMotionState);
-            }
-        }
-    }
-    _needsReinsertion = false;
-}
-
 void OtherAvatar::simulate(float deltaTime, bool inView) {
     PROFILE_RANGE(simulation, "simulate");
 

interface/src/avatar/OtherAvatar.h

@@ -52,9 +52,7 @@ public:
     bool shouldBeInPhysicsSimulation() const;
     bool needsPhysicsUpdate() const;
 
-    btCollisionShape* createCollisionShape(int jointIndex, bool& isBound, std::vector<int>& boundJoints);
+    const btCollisionShape* createCollisionShape(int32_t jointIndex, bool& isBound, std::vector<int32_t>& boundJoints);
-    DetailedMotionState* createMotionState(std::shared_ptr<OtherAvatar> avatar, int jointIndex);
-    void createDetailedMotionStates(const std::shared_ptr<OtherAvatar>& avatar);
     std::vector<DetailedMotionState*>& getDetailedMotionStates() { return _detailedMotionStates; }
     void resetDetailedMotionStates();
     BodyLOD getBodyLOD() { return _bodyLOD; }

libraries/physics/src/EntityMotionState.cpp

@@ -25,23 +25,6 @@
 #include "PhysicsHelpers.h"
 #include "PhysicsLogging.h"
 
-#ifdef WANT_DEBUG_ENTITY_TREE_LOCKS
-#include "EntityTree.h"
-
-bool EntityMotionState::entityTreeIsLocked() const {
-    EntityTreeElementPointer element = _entity->getElement();
-    EntityTreePointer tree = element ? element->getTree() : nullptr;
-    if (!tree) {
-        return true;
-    }
-    return true;
-}
-#else
-bool entityTreeIsLocked() {
-    return true;
-}
-#endif
-
 const uint8_t LOOPS_FOR_SIMULATION_ORPHAN = 50;
 const quint64 USECS_BETWEEN_OWNERSHIP_BIDS = USECS_PER_SECOND / 5;
 
@@ -74,7 +57,6 @@ EntityMotionState::EntityMotionState(btCollisionShape* shape, EntityItemPointer
 
     _type = MOTIONSTATE_TYPE_ENTITY;
     assert(_entity);
-    assert(entityTreeIsLocked());
     setMass(_entity->computeMass());
     // we need the side-effects of EntityMotionState::setShape() so we call it explicitly here
     // rather than pass the legit shape pointer to the ObjectMotionState ctor above.
@@ -143,7 +125,6 @@ void EntityMotionState::handleDeactivation() {
 
 // virtual
 void EntityMotionState::handleEasyChanges(uint32_t& flags) {
-    assert(entityTreeIsLocked());
     updateServerPhysicsVariables();
     ObjectMotionState::handleEasyChanges(flags);
 
@@ -191,17 +172,10 @@ void EntityMotionState::handleEasyChanges(uint32_t& flags) {
 }
 
 
-// virtual
-bool EntityMotionState::handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine) {
-    updateServerPhysicsVariables();
-    return ObjectMotionState::handleHardAndEasyChanges(flags, engine);
-}
-
 PhysicsMotionType EntityMotionState::computePhysicsMotionType() const {
     if (!_entity) {
         return MOTION_TYPE_STATIC;
     }
-    assert(entityTreeIsLocked());
 
     if (_entity->getLocked()) {
         if (_entity->isMoving()) {
@@ -226,7 +200,6 @@ PhysicsMotionType EntityMotionState::computePhysicsMotionType() const {
 }
 
 bool EntityMotionState::isMoving() const {
-    assert(entityTreeIsLocked());
     return _entity && _entity->isMovingRelativeToParent();
 }
 
@@ -240,7 +213,6 @@ void EntityMotionState::getWorldTransform(btTransform& worldTrans) const {
     if (!_entity) {
         return;
     }
-    assert(entityTreeIsLocked());
     if (_motionType == MOTION_TYPE_KINEMATIC) {
         BT_PROFILE("kinematicIntegration");
         uint32_t thisStep = ObjectMotionState::getWorldSimulationStep();
@@ -271,7 +243,6 @@ 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 ACTIVE.
 void EntityMotionState::setWorldTransform(const btTransform& worldTrans) {
-    assert(entityTreeIsLocked());
     measureBodyAcceleration();
 
     // If transform or velocities are flagged as dirty it means a network or scripted change
@@ -309,19 +280,6 @@ void EntityMotionState::setWorldTransform(const btTransform& worldTrans) {
 }
 
 
-// virtual and protected
-bool EntityMotionState::isReadyToComputeShape() const {
-    return _entity->isReadyToComputeShape();
-}
-
-// virtual and protected
-const btCollisionShape* EntityMotionState::computeNewShape() {
-    ShapeInfo shapeInfo;
-    assert(entityTreeIsLocked());
-    _entity->computeShapeInfo(shapeInfo);
-    return getShapeManager()->getShape(shapeInfo);
-}
-
 const uint8_t MAX_NUM_INACTIVE_UPDATES = 20;
 
 bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) {
@@ -439,7 +397,6 @@ bool EntityMotionState::shouldSendUpdate(uint32_t simulationStep) {
     DETAILED_PROFILE_RANGE(simulation_physics, "ShouldSend");
     // 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.
-    assert(entityTreeIsLocked());
 
     // this case is prevented by setting _ownershipState to UNOWNABLE in EntityMotionState::ctor
     assert(!(_entity->isAvatarEntity() && _entity->getOwningAvatarID() != Physics::getSessionUUID()));
@@ -505,7 +462,6 @@ void EntityMotionState::updateSendVelocities() {
 
 void EntityMotionState::sendBid(OctreeEditPacketSender* packetSender, uint32_t step) {
     DETAILED_PROFILE_RANGE(simulation_physics, "Bid");
-    assert(entityTreeIsLocked());
 
     updateSendVelocities();
 
@@ -546,7 +502,6 @@ void EntityMotionState::sendBid(OctreeEditPacketSender* packetSender, uint32_t s
 
 void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_t step) {
     DETAILED_PROFILE_RANGE(simulation_physics, "Send");
-    assert(entityTreeIsLocked());
     assert(isLocallyOwned());
 
     updateSendVelocities();
@@ -645,8 +600,7 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_
     _bumpedPriority = 0;
 }
 
-uint32_t EntityMotionState::getIncomingDirtyFlags() {
+uint32_t EntityMotionState::getIncomingDirtyFlags() const {
-    assert(entityTreeIsLocked());
     uint32_t dirtyFlags = 0;
     if (_body && _entity) {
         dirtyFlags = _entity->getDirtyFlags();
@@ -677,7 +631,6 @@ uint32_t EntityMotionState::getIncomingDirtyFlags() {
 }
 
 void EntityMotionState::clearIncomingDirtyFlags() {
-    assert(entityTreeIsLocked());
     if (_body && _entity) {
         _entity->clearDirtyFlags(DIRTY_PHYSICS_FLAGS);
     }
@@ -694,7 +647,6 @@ void EntityMotionState::slaveBidPriority() {
 
 // virtual
 QUuid EntityMotionState::getSimulatorID() const {
-    assert(entityTreeIsLocked());
     return _entity->getSimulatorID();
 }
 
@@ -762,6 +714,10 @@ glm::vec3 EntityMotionState::getObjectLinearVelocityChange() const {
     return _measuredAcceleration * _measuredDeltaTime;
 }
 
+bool EntityMotionState::shouldBeInPhysicsSimulation() const {
+    return _region < workload::Region::R3 && _entity->shouldBePhysical();
+}
+
 // virtual
 void EntityMotionState::setMotionType(PhysicsMotionType motionType) {
     ObjectMotionState::setMotionType(motionType);
@@ -770,7 +726,6 @@ void EntityMotionState::setMotionType(PhysicsMotionType motionType) {
 
 // virtual
 QString EntityMotionState::getName() const {
-    assert(entityTreeIsLocked());
     return _entity->getName();
 }
 

libraries/physics/src/EntityMotionState.h

@@ -39,7 +39,6 @@ public:
 
     void handleDeactivation();
     virtual void handleEasyChanges(uint32_t& flags) override;
-    virtual bool handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine) override;
 
     /// \return PhysicsMotionType based on params set in EntityItem
     virtual PhysicsMotionType computePhysicsMotionType() const override;
@@ -56,7 +55,7 @@ public:
     void sendBid(OctreeEditPacketSender* packetSender, uint32_t step);
     void sendUpdate(OctreeEditPacketSender* packetSender, uint32_t step);
 
-    virtual uint32_t getIncomingDirtyFlags() override;
+    virtual uint32_t getIncomingDirtyFlags() const override;
     virtual void clearIncomingDirtyFlags() override;
 
     virtual float getObjectRestitution() const override { return _entity->getRestitution(); }
@@ -85,6 +84,7 @@ public:
     void measureBodyAcceleration();
 
     virtual QString getName() const override;
+    ShapeType getShapeType() const override { return _entity->getShapeType(); }
 
     virtual void computeCollisionGroupAndMask(int32_t& group, int32_t& mask) const override;
 
@@ -113,12 +113,8 @@ protected:
 
     void clearObjectVelocities() const;
 
-    #ifdef WANT_DEBUG_ENTITY_TREE_LOCKS
+    bool isInPhysicsSimulation() const { return _body != nullptr; }
-    bool entityTreeIsLocked() const;
+    bool shouldBeInPhysicsSimulation() const;
-    #endif
-
-    bool isReadyToComputeShape() const override;
-    const btCollisionShape* computeNewShape() override;
     void setMotionType(PhysicsMotionType motionType) override;
 
     // EntityMotionState keeps a SharedPointer to its EntityItem which is only set in the CTOR

libraries/physics/src/ObjectMotionState.cpp

@@ -201,6 +201,9 @@ void ObjectMotionState::setShape(const btCollisionShape* shape) {
         if (_body && _type != MOTIONSTATE_TYPE_DETAILED) {
             updateCCDConfiguration();
         }
+    } else if (shape) {
+        // we need to release unused reference to shape
+        getShapeManager()->releaseShape(shape);
     }
 }
 
@@ -285,50 +288,6 @@ void ObjectMotionState::handleEasyChanges(uint32_t& flags) {
     }
 }
 
-bool ObjectMotionState::handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine) {
-    assert(_body && _shape);
-    if (flags & Simulation::DIRTY_SHAPE) {
-        // make sure the new shape is valid
-        if (!isReadyToComputeShape()) {
-            return false;
-        }
-        const btCollisionShape* newShape = computeNewShape();
-        if (!newShape) {
-            qCDebug(physics) << "Warning: failed to generate new shape!";
-            // failed to generate new shape! --> keep old shape and remove shape-change flag
-            flags &= ~Simulation::DIRTY_SHAPE;
-            // TODO: force this object out of PhysicsEngine rather than just use the old shape
-            if ((flags & HARD_DIRTY_PHYSICS_FLAGS) == 0) {
-                // no HARD flags remain, so do any EASY changes
-                if (flags & EASY_DIRTY_PHYSICS_FLAGS) {
-                    handleEasyChanges(flags);
-                }
-                return true;
-            }
-        } else {
-            if (_shape == newShape) {
-                // the shape didn't actually change, so we clear the DIRTY_SHAPE flag
-                flags &= ~Simulation::DIRTY_SHAPE;
-                // and clear the reference we just created
-                getShapeManager()->releaseShape(_shape);        
-            } else {
-                _body->setCollisionShape(const_cast<btCollisionShape*>(newShape));
-                setShape(newShape);
-            }
-        }
-    }
-    if (flags & EASY_DIRTY_PHYSICS_FLAGS) {
-        handleEasyChanges(flags);
-    }
-    // it is possible there are no HARD flags at this point (if DIRTY_SHAPE was removed)
-    // so we check again before we reinsert:
-    if (flags & HARD_DIRTY_PHYSICS_FLAGS) {
-        engine->reinsertObject(this);
-    }
-
-    return true;
-}
-
 void ObjectMotionState::updateBodyMaterialProperties() {
     _body->setRestitution(getObjectRestitution());
     _body->setFriction(getObjectFriction());

libraries/physics/src/ObjectMotionState.h

@@ -100,7 +100,6 @@ public:
     virtual ~ObjectMotionState();
 
     virtual void handleEasyChanges(uint32_t& flags);
-    virtual bool handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine);
 
     void updateBodyMaterialProperties();
     void updateBodyVelocities();
@@ -123,11 +122,12 @@ public:
     glm::vec3 getBodyAngularVelocity() const;
     virtual glm::vec3 getObjectLinearVelocityChange() const;
 
-    virtual uint32_t getIncomingDirtyFlags() = 0;
+    virtual uint32_t getIncomingDirtyFlags() const = 0;
     virtual void clearIncomingDirtyFlags() = 0;
 
     virtual PhysicsMotionType computePhysicsMotionType() const = 0;
 
+    virtual bool needsNewShape() const { return _shape == nullptr || getIncomingDirtyFlags() & Simulation::DIRTY_SHAPE; }
     const btCollisionShape* getShape() const { return _shape; }
     btRigidBody* getRigidBody() const { return _body; }
 
@@ -154,6 +154,7 @@ public:
     virtual void bump(uint8_t priority) {}
 
     virtual QString getName() const { return ""; }
+    virtual ShapeType getShapeType() const = 0;
 
     virtual void computeCollisionGroupAndMask(int32_t& group, int32_t& mask) const = 0;
 
@@ -172,8 +173,6 @@ public:
     friend class PhysicsEngine;
 
 protected:
-    virtual bool isReadyToComputeShape() const = 0;
-    virtual const btCollisionShape* computeNewShape() = 0;
     virtual void setMotionType(PhysicsMotionType motionType);
     void updateCCDConfiguration();
 
@@ -187,7 +186,7 @@ protected:
     btRigidBody* _body { nullptr };
     float _density { 1.0f };
 
-    // ACTION_CAN_CONTROL_KINEMATIC_OBJECT_HACK: These date members allow an Action
+    // ACTION_CAN_CONTROL_KINEMATIC_OBJECT_HACK: These data members allow an Action
     // to operate on a kinematic object without screwing up our default kinematic integration
     // which is done in the MotionState::getWorldTransform().
     mutable uint32_t _lastKinematicStep;

libraries/physics/src/PhysicalEntitySimulation.cpp

@@ -227,56 +227,16 @@ void PhysicalEntitySimulation::prepareEntityForDelete(EntityItemPointer entity)
 }
 // end EntitySimulation overrides
 
-const VectorOfMotionStates& PhysicalEntitySimulation::getObjectsToRemoveFromPhysics() {
+void PhysicalEntitySimulation::buildMotionStatesForEntitiesThatNeedThem() {
-    QMutexLocker lock(&_mutex);
-    for (auto entity: _entitiesToRemoveFromPhysics) {
-        _entitiesToAddToPhysics.remove(entity);
-        if (entity->isDead() && entity->getElement()) {
-            _deadEntities.insert(entity);
-        }
-
-        EntityMotionState* motionState = static_cast<EntityMotionState*>(entity->getPhysicsInfo());
-        if (motionState) {
-            _incomingChanges.remove(motionState);
-            removeOwnershipData(motionState);
-            _physicalObjects.remove(motionState);
-            // remember this motionState and delete it later (after removing its RigidBody from the PhysicsEngine)
-            _objectsToDelete.push_back(motionState);
-        }
-    }
-    _entitiesToRemoveFromPhysics.clear();
-    return _objectsToDelete;
-}
-
-void PhysicalEntitySimulation::deleteObjectsRemovedFromPhysics() {
-    QMutexLocker lock(&_mutex);
-    for (auto motionState : _objectsToDelete) {
-        // someday when we invert the entities/physics lib dependencies we can let EntityItem delete its own PhysicsInfo
-        // until then we must do it here
-        // NOTE: a reference to the EntityItemPointer is released in the EntityMotionState::dtor
-        delete motionState;
-    }
-    _objectsToDelete.clear();
-}
-
-void PhysicalEntitySimulation::getObjectsToAddToPhysics(VectorOfMotionStates& result) {
-    result.clear();
-
     // this lambda for when we decide to actually build the motionState
     auto buildMotionState = [&](btCollisionShape* shape, EntityItemPointer entity) {
         EntityMotionState* motionState = new EntityMotionState(shape, entity);
         entity->setPhysicsInfo(static_cast<void*>(motionState));
         motionState->setRegion(_space->getRegion(entity->getSpaceIndex()));
         _physicalObjects.insert(motionState);
-        result.push_back(motionState);
+        _incomingChanges.insert(motionState);
     };
 
-    // TODO:
-    // (1) make all changes to MotionState in "managers" (PhysicalEntitySimulation and AvatarManager)
-    // (2) store relevant change-flags on MotionState (maybe just EASY or HARD?)
-    // (3) remove knowledge of PhysicsEngine from ObjectMotionState
-    // (4) instead PhysicsEngine gets list of changed MotionStates, reads change-flags and applies changes accordingly
-
     QMutexLocker lock(&_mutex);
     uint32_t deliveryCount = ObjectMotionState::getShapeManager()->getWorkDeliveryCount();
     if (deliveryCount != _lastWorkDeliveryCount) {
@@ -319,7 +279,7 @@ void PhysicalEntitySimulation::getObjectsToAddToPhysics(VectorOfMotionStates& re
             } else {
                 // this is a CHANGE because motionState already exists
                 if (ObjectMotionState::getShapeManager()->hasShapeWithKey(requestItr->shapeHash)) {
-                    // TODO? reset DIRTY_SHAPE flag?
+                    entity->markDirtyFlags(Simulation::DIRTY_SHAPE);
                     _incomingChanges.insert(motionState);
                     requestItr = _shapeRequests.erase(requestItr);
                 } else {
@@ -338,7 +298,7 @@ void PhysicalEntitySimulation::getObjectsToAddToPhysics(VectorOfMotionStates& re
             prepareEntityForDelete(entity);
             entityItr = _entitiesToAddToPhysics.erase(entityItr);
         } else if (!entity->shouldBePhysical()) {
-            // this entity should no longer be on the internal _entitiesToAddToPhysics
+            // this entity should no longer be on _entitiesToAddToPhysics
             entityItr = _entitiesToAddToPhysics.erase(entityItr);
             if (entity->isMovingRelativeToParent()) {
                 SetOfEntities::iterator itr = _simpleKinematicEntities.find(entity);
@@ -347,16 +307,27 @@ void PhysicalEntitySimulation::getObjectsToAddToPhysics(VectorOfMotionStates& re
                 }
             }
         } else if (entity->isReadyToComputeShape()) {
-            // check to see if we're waiting for a shape
             ShapeRequest shapeRequest(entity);
             ShapeRequests::iterator  requestItr = _shapeRequests.find(shapeRequest);
             if (requestItr == _shapeRequests.end()) {
+                // not waiting for a shape (yet)
                 ShapeInfo shapeInfo;
                 entity->computeShapeInfo(shapeInfo);
                 uint32_t requestCount = ObjectMotionState::getShapeManager()->getWorkRequestCount();
                 btCollisionShape* shape = const_cast<btCollisionShape*>(ObjectMotionState::getShapeManager()->getShape(shapeInfo));
                 if (shape) {
-                    buildMotionState(shape, entity);
+                    EntityMotionState* motionState = static_cast<EntityMotionState*>(entity->getPhysicsInfo());
+                    if (!motionState) {
+                        buildMotionState(shape, entity);
+                    } else {
+                        // Is it possible to fall in here?
+                        // entity shouldn't be on _entitiesToAddToPhysics list if it already has a motionState.
+                        // but just in case...
+                        motionState->setShape(shape);
+                        motionState->setRegion(_space->getRegion(entity->getSpaceIndex()));
+                        _physicalObjects.insert(motionState);
+                        _incomingChanges.insert(motionState);
+                    }
                 } else if (requestCount != ObjectMotionState::getShapeManager()->getWorkRequestCount()) {
                     // shape doesn't exist but a new worker has been spawned to build it --> add to shapeRequests and wait
                     shapeRequest.shapeHash = shapeInfo.getHash();
@@ -389,6 +360,80 @@ void PhysicalEntitySimulation::getObjectsToChange(VectorOfMotionStates& result)
     _incomingChanges.clear();
 }
 
+void PhysicalEntitySimulation::buildPhysicsTransaction(PhysicsEngine::Transaction& transaction) {
+    buildMotionStatesForEntitiesThatNeedThem();
+    for (auto& object : _incomingChanges) {
+        uint32_t flags = object->getIncomingDirtyFlags();
+        object->clearIncomingDirtyFlags();
+
+        bool isInPhysicsSimulation = object->isInPhysicsSimulation();
+        if (isInPhysicsSimulation != object->shouldBeInPhysicsSimulation()) {
+            if (isInPhysicsSimulation) {
+                transaction.objectsToRemove.push_back(object);
+                continue;
+            } else {
+                transaction.objectsToAdd.push_back(object);
+            }
+        }
+
+        bool reinsert = false;
+        if (object->needsNewShape()) {
+            ShapeType shapeType = object->getShapeType();
+            if (shapeType == SHAPE_TYPE_STATIC_MESH) {
+                ShapeRequest shapeRequest(object->_entity);
+                ShapeRequests::iterator  requestItr = _shapeRequests.find(shapeRequest);
+                if (requestItr == _shapeRequests.end()) {
+                    ShapeInfo shapeInfo;
+                    object->_entity->computeShapeInfo(shapeInfo);
+                    uint32_t requestCount = ObjectMotionState::getShapeManager()->getWorkRequestCount();
+                    btCollisionShape* shape = const_cast<btCollisionShape*>(ObjectMotionState::getShapeManager()->getShape(shapeInfo));
+                    if (shape) {
+                        object->setShape(shape);
+                        reinsert = true;
+                    } else if (requestCount != ObjectMotionState::getShapeManager()->getWorkRequestCount()) {
+                        // shape doesn't exist but a new worker has been spawned to build it --> add to shapeRequests and wait
+                        shapeRequest.shapeHash = shapeInfo.getHash();
+                        _shapeRequests.insert(shapeRequest);
+                    } else {
+                        // failed to build shape --> will not be added/updated
+                    }
+                } else {
+                    // continue waiting for shape request
+                }
+            } else {
+                ShapeInfo shapeInfo;
+                object->_entity->computeShapeInfo(shapeInfo);
+                btCollisionShape* shape = const_cast<btCollisionShape*>(ObjectMotionState::getShapeManager()->getShape(shapeInfo));
+                if (shape) {
+                    object->setShape(shape);
+                    reinsert = true;
+                } else {
+                    // failed to build shape --> will not be added
+                }
+            }
+        }
+        if (!isInPhysicsSimulation) {
+            continue;
+        }
+        if (flags | EASY_DIRTY_PHYSICS_FLAGS) {
+            object->handleEasyChanges(flags);
+        }
+        if (flags | (Simulation::DIRTY_MOTION_TYPE | Simulation::DIRTY_COLLISION_GROUP) || reinsert) {
+            transaction.objectsToReinsert.push_back(object);
+        } else if (flags & Simulation::DIRTY_PHYSICS_ACTIVATION && object->getRigidBody()->isStaticObject()) {
+            transaction.activeStaticObjects.push_back(object);
+        }
+    }
+}
+
+void PhysicalEntitySimulation::handleProcessedPhysicsTransaction(PhysicsEngine::Transaction& transaction) {
+    // things on objectsToRemove are ready for delete
+    for (auto object : transaction.objectsToRemove) {
+        delete object;
+    }
+    transaction.clear();
+}
+
 void PhysicalEntitySimulation::handleDeactivatedMotionStates(const VectorOfMotionStates& motionStates) {
     bool serverlessMode = getEntityTree()->isServerlessMode();
     for (auto stateItr : motionStates) {

libraries/physics/src/PhysicalEntitySimulation.h

@@ -14,6 +14,7 @@
 
 #include <stdint.h>
 #include <map>
+#include <set>
 
 #include <btBulletDynamicsCommon.h>
 #include <BulletCollision/CollisionDispatch/btGhostObject.h>
@@ -80,13 +81,12 @@ protected: // only called by EntitySimulation
 public:
     virtual void prepareEntityForDelete(EntityItemPointer entity) override;
 
-    const VectorOfMotionStates& getObjectsToRemoveFromPhysics();
-    void deleteObjectsRemovedFromPhysics();
-
-    void getObjectsToAddToPhysics(VectorOfMotionStates& result);
     void setObjectsToChange(const VectorOfMotionStates& objectsToChange);
     void getObjectsToChange(VectorOfMotionStates& result);
 
+    void buildPhysicsTransaction(PhysicsEngine::Transaction& transaction);
+    void handleProcessedPhysicsTransaction(PhysicsEngine::Transaction& transaction);
+
     void handleDeactivatedMotionStates(const VectorOfMotionStates& motionStates);
     void handleChangedMotionStates(const VectorOfMotionStates& motionStates);
     void handleCollisionEvents(const CollisionEvents& collisionEvents);
@@ -99,23 +99,20 @@ public:
     void sendOwnedUpdates(uint32_t numSubsteps);
 
 private:
+    void buildMotionStatesForEntitiesThatNeedThem();
+
     class ShapeRequest {
     public:
-        ShapeRequest() : entity(), shapeHash(0) {}
+        ShapeRequest() { }
-        ShapeRequest(const EntityItemPointer& e) : entity(e), shapeHash(0) {}
+        ShapeRequest(const EntityItemPointer& e) : entity(e) { }
         bool operator<(const ShapeRequest& other) const { return entity.get() < other.entity.get(); }
         bool operator==(const ShapeRequest& other) const { return entity.get() == other.entity.get(); }
-        EntityItemPointer entity;
+        EntityItemPointer entity { nullptr };
-        mutable uint64_t shapeHash;
+        mutable uint64_t shapeHash { 0 };
     };
-    SetOfEntities _entitiesToAddToPhysics;
+    SetOfEntities _entitiesToAddToPhysics; // we could also call this: _entitiesThatNeedMotionStates
     SetOfEntities _entitiesToRemoveFromPhysics;
-
+    SetOfEntityMotionStates _incomingChanges; // EntityMotionStates changed by external events
-    VectorOfMotionStates _objectsToDelete;
-
-    SetOfEntityMotionStates _incomingChanges; // EntityMotionStates that have changed from external sources
-                                              // and need their RigidBodies updated
-
     SetOfMotionStates _physicalObjects; // MotionStates of entities in PhysicsEngine
 
     using ShapeRequests = std::set<ShapeRequest>;

libraries/physics/src/PhysicsEngine.cpp

@@ -260,35 +260,6 @@ void PhysicsEngine::addObjects(const VectorOfMotionStates& objects) {
     }
 }
 
-VectorOfMotionStates PhysicsEngine::changeObjects(const VectorOfMotionStates& objects) {
-    VectorOfMotionStates stillNeedChange;
-    for (auto object : objects) {
-        uint32_t flags = object->getIncomingDirtyFlags() & DIRTY_PHYSICS_FLAGS;
-        if (flags & HARD_DIRTY_PHYSICS_FLAGS) {
-            if (object->handleHardAndEasyChanges(flags, this)) {
-                object->clearIncomingDirtyFlags();
-            } else {
-                stillNeedChange.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());
-        }
-    }
-    // active static bodies 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;
-    }
-    return stillNeedChange;
-}
-
 void PhysicsEngine::reinsertObject(ObjectMotionState* object) {
     // remove object from DynamicsWorld
     bumpAndPruneContacts(object);
@@ -320,7 +291,6 @@ void PhysicsEngine::processTransaction(PhysicsEngine::Transaction& transaction)
             body->setMotionState(nullptr);
             delete body;
         }
-        object->clearIncomingDirtyFlags();
     }
 
     // adds
@@ -328,34 +298,16 @@ void PhysicsEngine::processTransaction(PhysicsEngine::Transaction& transaction)
         addObjectToDynamicsWorld(object);
     }
 
-    // changes
+    // reinserts
-    std::vector<ObjectMotionState*> failedChanges;
+    for (auto object : transaction.objectsToReinsert) {
-    for (auto object : transaction.objectsToChange) {
+        reinsertObject(object);
-        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)
+    for (auto object : transaction.activeStaticObjects) {
-    // so we must do it ourselves
+        btRigidBody* body = object->getRigidBody();
-    std::set<btRigidBody*>::const_iterator itr = _activeStaticBodies.begin();
+        _dynamicsWorld->updateSingleAabb(body);
-    while (itr != _activeStaticBodies.end()) {
+        _activeStaticBodies.insert(body);
-        _dynamicsWorld->updateSingleAabb(*itr);
-        ++itr;
     }
-    // we replace objectsToChange with any that failed
-    transaction.objectsToChange.swap(failedChanges);
 }
 
 void PhysicsEngine::removeContacts(ObjectMotionState* motionState) {

libraries/physics/src/PhysicsEngine.h

@@ -79,11 +79,13 @@ public:
         void clear() {
             objectsToRemove.clear();
             objectsToAdd.clear();
-            objectsToChange.clear();
+            objectsToReinsert.clear();
+            activeStaticObjects.clear();
         }
         std::vector<ObjectMotionState*> objectsToRemove;
         std::vector<ObjectMotionState*> objectsToAdd;
-        std::vector<ObjectMotionState*> objectsToChange;
+        std::vector<ObjectMotionState*> objectsToReinsert;
+        std::vector<ObjectMotionState*> activeStaticObjects;
     };
 
     PhysicsEngine(const glm::vec3& offset);
@@ -97,7 +99,7 @@ public:
     void removeSetOfObjects(const SetOfMotionStates& objects); // only called during teardown
 
     void addObjects(const VectorOfMotionStates& objects);
-    VectorOfMotionStates changeObjects(const VectorOfMotionStates& objects);
+    void changeObjects(const VectorOfMotionStates& objects);
     void reinsertObject(ObjectMotionState* object);
 
     void processTransaction(Transaction& transaction);

libraries/physics/src/ShapeFactory.cpp

@@ -293,6 +293,8 @@ const btCollisionShape* ShapeFactory::createShapeFromInfo(const ShapeInfo& info)
                 radiuses.push_back(sphereData.w);
             }
             shape = new btMultiSphereShape(positions.data(), radiuses.data(), (int)positions.size());
+            const float MULTI_SPHERE_MARGIN = 0.001f;
+            shape->setMargin(MULTI_SPHERE_MARGIN);
         }
         break;
         case SHAPE_TYPE_ELLIPSOID: {
@@ -459,6 +461,8 @@ const btCollisionShape* ShapeFactory::createShapeFromInfo(const ShapeInfo& info)
                 shape = compound;
             }
         }
+    } else {
+        // TODO: warn about this case
     }
     return shape;
 }