make some things compile after reorganization

libraries/entities/src/EntityItem.h

@@ -299,6 +299,7 @@ public:
 
     void* getPhysicsInfo() const { return _physicsInfo; }
     
+    void setPhysicsInfo(void* data) { _physicsInfo = data; }
     EntityTreeElement* getElement() const { return _element; }
     EntitySimulation* getSimulation() const { return _simulation; }
 
@@ -311,7 +312,6 @@ public:
     glm::vec3 entityToWorld(const glm::vec3& point) const;
     
 protected:
-    void setPhysicsInfo(void* data) { _physicsInfo = data; }
 
     static bool _sendPhysicsUpdates;
     EntityTypes::EntityType _type;

libraries/entities/src/EntitySimulation.cpp

@@ -56,7 +56,7 @@ void EntitySimulation::getEntitiesToDelete(SetOfEntities& entitiesToDelete) {
     }
 }
 
-// private
+// protected
 void EntitySimulation::expireMortalEntities(const quint64& now) {
     if (now > _nextExpiry) {
         // only search for expired entities if we expect to find one
@@ -82,7 +82,7 @@ void EntitySimulation::expireMortalEntities(const quint64& now) {
     }
 }
 
-// private
+// protected
 void EntitySimulation::callUpdateOnEntitiesThatNeedIt(const quint64& now) {
     PerformanceTimer perfTimer("updatingEntities");
     SetOfEntities::iterator itemItr = _entitiesToUpdate.begin();
@@ -99,7 +99,7 @@ void EntitySimulation::callUpdateOnEntitiesThatNeedIt(const quint64& now) {
     }
 }
 
-// private
+// protected
 void EntitySimulation::sortEntitiesThatMoved() {
     // NOTE: this is only for entities that have been moved by THIS EntitySimulation.
     // External changes to entity position/shape are expected to be sorted outside of the EntitySimulation.

libraries/entities/src/EntitySimulation.h

@@ -77,6 +77,8 @@ signals:
 
 protected:
 
+    void clearEntitySimulation(EntityItem* entity) { entity->_simulation = nullptr; }
+
     // These pure virtual methods are protected because they are not to be called will-nilly. The base class
     // calls them in the right places.
 

libraries/entities/src/SimpleEntitySimulation.cpp

@@ -65,15 +65,15 @@ void SimpleEntitySimulation::removeEntityInternal(EntityItem* entity) {
     _movingEntities.remove(entity);
     _movableButStoppedEntities.remove(entity);
     _hasSimulationOwnerEntities.remove(entity);
-    entity->_simulation = nullptr;
+    clearEntitySimulation(entity);
 }
 
 void SimpleEntitySimulation::deleteEntityInternal(EntityItem* entity) {
     _movingEntities.remove(entity);
     _movableButStoppedEntities.remove(entity);
     _hasSimulationOwnerEntities.remove(entity);
-    entity->_simulation = nullptr;
+    clearEntitySimulation(entity);
-    if (!entity->_tree) {
+    if (!entity->getElement()) {
         // we held the last reference to this entity, so delete it
         delete entity;
     }

libraries/physics/src/EntityMotionState.cpp

@@ -54,21 +54,6 @@ MotionType EntityMotionState::computeObjectMotionType() const {
     return _entity->isMoving() ?  MOTION_TYPE_KINEMATIC : MOTION_TYPE_STATIC;
 }
 
-void EntityMotionState::updateKinematicState(uint32_t substep) {
-    setKinematic(_entity->isMoving(), substep);
-}
-
-void EntityMotionState::stepKinematicSimulation(quint64 now) {
-    assert(_isKinematic);
-    // NOTE: this is non-physical kinematic motion which steps to real run-time (now)
-    // which is different from physical kinematic motion (inside getWorldTransform())
-    // which steps in physics simulation time.
-    _entity->simulate(now);
-    // TODO: we can't use measureBodyAcceleration() here because the entity
-    // has no RigidBody and the timestep is a little bit out of sync with the physics simulation anyway.
-    // Hence we must manually measure kinematic velocity and acceleration.
-}
-
 bool EntityMotionState::isMoving() const {
     return _entity->isMoving();
 }
@@ -79,18 +64,18 @@ bool EntityMotionState::isMoving() const {
 // (2) at the beginning of each simulation step for KINEMATIC RigidBody's --
 //     it is an opportunity for outside code to update the object's simulation position
 void EntityMotionState::getWorldTransform(btTransform& worldTrans) const {
-    if (_isKinematic) {
+    if (_motionType == MOTION_TYPE_KINEMATIC) {
         // This is physical kinematic motion which steps strictly by the subframe count
         // of the physics simulation.
-        uint32_t substep = PhysicsEngine::getNumSubsteps();
+        uint32_t thisStep = ObjectMotionState::getWorldSimulationStep();
-        float dt = (substep - _lastKinematicSubstep) * PHYSICS_ENGINE_FIXED_SUBSTEP;
+        float dt = (thisStep - _lastKinematicStep) * PHYSICS_ENGINE_FIXED_SUBSTEP;
         _entity->simulateKinematicMotion(dt);
         _entity->setLastSimulated(usecTimestampNow());
 
-        // bypass const-ness so we can remember the substep
+        // bypass const-ness so we can remember the step
-        const_cast<EntityMotionState*>(this)->_lastKinematicSubstep = substep;
+        const_cast<EntityMotionState*>(this)->_lastKinematicStep = thisStep;
     }
-    worldTrans.setOrigin(glmToBullet(_entity->getPosition() - ObjectMotionState::getWorldOffset()));
+    worldTrans.setOrigin(glmToBullet(getObjectPosition()));
     worldTrans.setRotation(glmToBullet(_entity->getRotation()));
 }
 
@@ -101,13 +86,8 @@ void EntityMotionState::setWorldTransform(const btTransform& worldTrans) {
     _entity->setPosition(bulletToGLM(worldTrans.getOrigin()) + ObjectMotionState::getWorldOffset());
     _entity->setRotation(bulletToGLM(worldTrans.getRotation()));
 
-    glm::vec3 v;
+    _entity->setVelocity(getBodyLinearVelocity());
-    getVelocity(v);
+    _entity->setAngularVelocity(getBodyAngularVelocity());
-    _entity->setVelocity(v);
-
-    glm::vec3 av;
-    getAngularVelocity(av);
-    _entity->setAngularVelocity(av);
 
     _entity->setLastSimulated(usecTimestampNow());
 
@@ -405,3 +385,32 @@ uint32_t EntityMotionState::getIncomingDirtyFlags() const {
     return dirtyFlags;
 */
 }
+
+void EntityMotionState::resetMeasuredBodyAcceleration() {
+    _lastMeasureStep = ObjectMotionState::getWorldSimulationStep();
+    _lastVelocity = bulletToGLM(_body->getLinearVelocity());                                                        
+    _measuredAcceleration = glm::vec3(0.0f);
+} 
+
+void EntityMotionState::measureBodyAcceleration() {
+    // try to manually measure the true acceleration of the object
+    uint32_t thisStep = ObjectMotionState::getWorldSimulationStep();
+    uint32_t numSubsteps = thisStep - _lastMeasureStep;
+    if (numSubsteps > 0) {
+        float dt = ((float)numSubsteps * PHYSICS_ENGINE_FIXED_SUBSTEP);
+        float invDt = 1.0f / dt;
+        _lastMeasureStep = thisStep;
+
+        // Note: the integration equation for velocity uses damping:   v1 = (v0 + a * dt) * (1 - D)^dt
+        // hence the equation for acceleration is: a = (v1 / (1 - D)^dt - v0) / dt
+        glm::vec3 velocity = bulletToGLM(_body->getLinearVelocity());
+        _measuredAcceleration = (velocity / powf(1.0f - _body->getLinearDamping(), dt) - _lastVelocity) * invDt;
+        _lastVelocity = velocity;
+    }
+}
+
+// virtual 
+void EntityMotionState::setMotionType(MotionType motionType) {
+    ObjectMotionState::setMotionType(motionType);
+    resetMeasuredBodyAcceleration();
+}

libraries/physics/src/EntityMotionState.h

@@ -31,9 +31,6 @@ public:
     /// \return MOTION_TYPE_DYNAMIC or MOTION_TYPE_STATIC based on params set in EntityItem
     virtual MotionType computeObjectMotionType() const;
 
-    virtual void updateKinematicState(uint32_t substep);
-    virtual void stepKinematicSimulation(quint64 now);
-
     virtual bool isMoving() const;
 
     // this relays incoming position/rotation to the RigidBody
@@ -65,15 +62,20 @@ public:
     virtual float getObjectLinearDamping() const { return _entity->getDamping(); }
     virtual float getObjectAngularDamping() const { return _entity->getAngularDamping(); }
 
-    virtual const glm::vec3& getObjectPosition() const { return _entity->getPosition(); }
+    virtual glm::vec3 getObjectPosition() const { return _entity->getPosition() - ObjectMotionState::getWorldOffset(); }
     virtual const glm::quat& getObjectRotation() const { return _entity->getRotation(); }
     virtual const glm::vec3& getObjectLinearVelocity() const { return _entity->getVelocity(); }
     virtual const glm::vec3& getObjectAngularVelocity() const { return _entity->getAngularVelocity(); }
     virtual const glm::vec3& getObjectGravity() const { return _entity->getGravity(); }
 
-    EntityItem* getEntityItem() const { return _entityItem; }
+    EntityItem* getEntity() const { return _entity; }
+
+    void resetMeasuredBodyAcceleration();
+    void measureBodyAcceleration();
 
 protected:
+    virtual void setMotionType(MotionType motionType);
+
     EntityItem* _entity;
 
     bool _sentMoving;   // true if last update was moving
@@ -87,6 +89,10 @@ protected:
     glm::vec3 _sentGravity;
     glm::vec3 _sentAcceleration;
 
+    uint32_t _lastMeasureStep;
+    glm::vec3 _lastVelocity;
+    glm::vec3 _measuredAcceleration;
+
     quint8 _accelerationNearlyGravityCount;
     bool _shouldClaimSimulationOwnership;
     quint32 _movingStepsWithoutSimulationOwner;

libraries/physics/src/ObjectMotionState.cpp

@@ -36,22 +36,26 @@ const glm::vec3& ObjectMotionState::getWorldOffset() {
 }
 
 // static 
-uint32_t _worldSimulationStep = 0;
+uint32_t worldSimulationStep = 0;
 void ObjectMotionState::setWorldSimulationStep(uint32_t step) {
-    assert(step > _worldSimulationStep);
+    assert(step > worldSimulationStep);
-    _worldSimulationStep = step;
+    worldSimulationStep = step;
+}
+
+uint32_t ObjectMotionState::getWorldSimulationStep() {
+    return worldSimulationStep;
 }
 
 // static 
-ShapeManager* _shapeManager = nullptr;
+ShapeManager* shapeManager = nullptr;
-void ObjectMotionState::setShapeManager(ShapeManager* shapeManager) {
+void ObjectMotionState::setShapeManager(ShapeManager* manager) {
-    assert(shapeManager);
+    assert(manager);
-    _shapeManager = shapeManager;
+    shapeManager = manager;
 }
 
 ShapeManager* ObjectMotionState::getShapeManager() {
-    assert(_shapeManager); // you must properly set _shapeManager before calling getShapeManager()
+    assert(shapeManager); // you must properly set shapeManager before calling getShapeManager()
-    return _shapeManager;
+    return shapeManager;
 }
 
 ObjectMotionState::ObjectMotionState(btCollisionShape* shape) :
@@ -59,43 +63,8 @@ ObjectMotionState::ObjectMotionState(btCollisionShape* shape) :
     _shape(shape),
     _body(nullptr),
     _mass(0.0f),
-    _lastSimulationStep(0),
+    _lastKinematicStep(worldSimulationStep)
-    _lastVelocity(0.0f),
-    _measuredAcceleration(0.0f)
 {
-    assert(_shape);
-}
-
-ObjectMotionState::~ObjectMotionState() {
-    // NOTE: you MUST remove this MotionState from the world before you call the dtor.
-    assert(_body == nullptr);
-    if (_shape) {
-        // the ObjectMotionState owns a reference to its shape in the ShapeManager
-        // se we must release it
-        getShapeManager()->releaseShape(_shape);
-        _shape = nullptr;
-    }
-}
-
-void ObjectMotionState::measureBodyAcceleration() {
-    // try to manually measure the true acceleration of the object
-    uint32_t numSubsteps = _worldSimulationStep - _lastSimulationStep;
-    if (numSubsteps > 0) {
-        float dt = ((float)numSubsteps * PHYSICS_ENGINE_FIXED_SUBSTEP);
-        float invDt = 1.0f / dt;
-        _lastSimulationStep = _worldSimulationStep;
-
-        // Note: the integration equation for velocity uses damping:   v1 = (v0 + a * dt) * (1 - D)^dt
-        // hence the equation for acceleration is: a = (v1 / (1 - D)^dt - v0) / dt
-        glm::vec3 velocity = bulletToGLM(_body->getLinearVelocity());
-        _measuredAcceleration = (velocity / powf(1.0f - _body->getLinearDamping(), dt) - _lastVelocity) * invDt;
-        _lastVelocity = velocity;
-    }
-}
-
-void ObjectMotionState::resetMeasuredBodyAcceleration() {
-    _lastSimulationStep = _worldSimulationStep;
-    _lastVelocity = bulletToGLM(_body->getLinearVelocity());
 }
 
 void ObjectMotionState::setBodyLinearVelocity(const glm::vec3& velocity) const {
@@ -110,7 +79,7 @@ void ObjectMotionState::setBodyGravity(const glm::vec3& gravity) const {
     _body->setGravity(glmToBullet(gravity));
 }
 
-glm::vec3 ObjectMotionState::getBodyVelocity() const {
+glm::vec3 ObjectMotionState::getBodyLinearVelocity() const {
     return bulletToGLM(_body->getLinearVelocity());
 }
 
@@ -118,6 +87,17 @@ glm::vec3 ObjectMotionState::getBodyAngularVelocity() const {
     return bulletToGLM(_body->getAngularVelocity());
 }
 
+void ObjectMotionState::releaseShape() {
+    if (_shape) {
+        shapeManager->releaseShape(_shape);
+        _shape = nullptr;
+    }
+}
+
+void ObjectMotionState::setMotionType(MotionType motionType) {
+    _motionType = motionType;
+}
+
 void ObjectMotionState::setRigidBody(btRigidBody* body) {
     // give the body a (void*) back-pointer to this ObjectMotionState
     if (_body != body) {

libraries/physics/src/ObjectMotionState.h

@@ -18,7 +18,7 @@
 #include <EntityItem.h>
 
 #include "ContactInfo.h"
-#include "ShapeInfo.h"
+#include "ShapeManager.h"
 
 enum MotionType {
     MOTION_TYPE_STATIC,     // no motion
@@ -46,6 +46,7 @@ const uint32_t OUTGOING_DIRTY_PHYSICS_FLAGS = EntityItem::DIRTY_POSITION | Entit
 
 
 class OctreeEditPacketSender;
+class PhysicsEngine;
 
 extern const int MAX_NUM_NON_MOVING_UPDATES;
 
@@ -56,18 +57,18 @@ public:
     // ObjectMotionState context.
     static void setWorldOffset(const glm::vec3& offset);
     static const glm::vec3& getWorldOffset();
+
     static void setWorldSimulationStep(uint32_t step);
-    static void setShapeManager(ShapeManager* shapeManager);
+    static uint32_t getWorldSimulationStep();
+
+    static void setShapeManager(ShapeManager* manager);
     static ShapeManager* getShapeManager();
 
     ObjectMotionState(btCollisionShape* shape);
     ~ObjectMotionState();
 
-    void measureBodyAcceleration();
+    void handleEasyChanges(uint32_t flags);
-    void resetMeasuredBodyAcceleration();
+    void handleHardAndEasyChanges(uint32_t flags, PhysicsEngine* engine);
-
-    void handleEasyChanges();
-    void handleHardAndEasyChanges();
 
     virtual void updateBodyMaterialProperties();
     virtual void updateBodyVelocities();
@@ -82,7 +83,7 @@ public:
     void setBodyAngularVelocity(const glm::vec3& velocity) const;
     void setBodyGravity(const glm::vec3& gravity) const;
 
-    glm::vec3 getBodyVelocity() const;
+    glm::vec3 getBodyLinearVelocity() const;
     glm::vec3 getBodyAngularVelocity() const;
 
     virtual uint32_t getIncomingDirtyFlags() const = 0;
@@ -93,6 +94,8 @@ public:
     btCollisionShape* getShape() const { return _shape; }
     btRigidBody* getRigidBody() const { return _body; }
 
+    void releaseShape();
+
     virtual bool isMoving() const = 0;
 
     // These pure virtual methods must be implemented for each MotionState type
@@ -103,7 +106,7 @@ public:
     virtual float getObjectLinearDamping() const = 0;
     virtual float getObjectAngularDamping() const = 0;
     
-    virtual const glm::vec3& getObjectPosition() const = 0;
+    virtual glm::vec3 getObjectPosition() const = 0;
     virtual const glm::quat& getObjectRotation() const = 0;
     virtual const glm::vec3& getObjectLinearVelocity() const = 0;
     virtual const glm::vec3& getObjectAngularVelocity() const = 0;
@@ -112,6 +115,7 @@ public:
     friend class PhysicsEngine;
 
 protected:
+    virtual void setMotionType(MotionType motionType);
     void setRigidBody(btRigidBody* body);
 
     MotionStateType _type = MOTION_STATE_TYPE_UNKNOWN; // type of MotionState
@@ -121,9 +125,7 @@ protected:
     btRigidBody* _body;
     float _mass;
 
-    uint32_t _lastSimulationStep;
+    uint32_t _lastKinematicStep;
-    glm::vec3 _lastVelocity;
-    glm::vec3 _measuredAcceleration;
 };
 
 #endif // hifi_ObjectMotionState_h

libraries/physics/src/PhysicalEntitySimulation.cpp

@@ -9,13 +9,15 @@
 //  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
 //
 
+#include "EntityMotionState.h"
 #include "PhysicalEntitySimulation.h"
 #include "PhysicsEngine.h"
 #include "PhysicsHelpers.h"
 #include "PhysicsLogging.h"
 #include "ShapeInfoUtil.h"
+#include "ShapeManager.h"
 
-PhysicalEntitySimulation::PhysicalEntitySimulation() :
+PhysicalEntitySimulation::PhysicalEntitySimulation() {
 }
 
 PhysicalEntitySimulation::~PhysicalEntitySimulation() {
@@ -29,10 +31,10 @@ void PhysicalEntitySimulation::init(
     assert(tree);
     setEntityTree(tree);
 
-    assert(*physicsEngine);
+    assert(physicsEngine);
     _physicsEngine = physicsEngine;
 
-    assert(*shapeManager);
+    assert(shapeManager);
     _shapeManager = shapeManager;
 
     assert(packetSender);
@@ -44,26 +46,6 @@ void PhysicalEntitySimulation::updateEntitiesInternal(const quint64& now) {
     // TODO: add back non-physical kinematic objects and step them forward here
 }
 
-void PhysicalEntitySimulation::sendOutgoingPackets() {
-    uint32_t numSubsteps = _physicsEngine->getNumSubsteps();
-    if (_lastNumSubstepsAtUpdateInternal != numSubsteps) {
-        _lastNumSubstepsAtUpdateInternal = numSubsteps;
-
-        SetOfMotionStates::iterator stateItr = _outgoingChanges.begin();
-        while (stateItr != _outgoingChanges.end()) {
-            EntityMotionState* state = *stateItr;
-            if (state->doesNotNeedToSendUpdate()) {
-                stateItr = _outgoingChanges.erase(stateItr);
-            } else if (state->shouldSendUpdate(numSubsteps)) {
-                state->sendUpdate(_entityPacketSender, numSubsteps);
-                ++stateItr;
-            } else {
-                ++stateItr;
-            }
-        }
-    }
-}
-
 void PhysicalEntitySimulation::addEntityInternal(EntityItem* entity) {
     assert(entity);
     void* physicsInfo = entity->getPhysicsInfo();
@@ -76,10 +58,9 @@ void PhysicalEntitySimulation::removeEntityInternal(EntityItem* entity) {
     assert(entity);
     void* physicsInfo = entity->getPhysicsInfo();
     if (physicsInfo) {
-        ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
+        _pendingRemoves.insert(entity);
-        _pendingRemoves.insert(motionState);
     } else {
-        entity->_simulation = nullptr;
+        clearEntitySimulation(entity);
     }
 }
 
@@ -87,10 +68,9 @@ void PhysicalEntitySimulation::deleteEntityInternal(EntityItem* entity) {
     assert(entity);
     void* physicsInfo = entity->getPhysicsInfo();
     if (physicsInfo) {
-        ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
+        _pendingRemoves.insert(entity);
-        _pendingRemoves.insert(motionState);
     } else {
-        entity->_simulation = nullptr;
+        clearEntitySimulation(entity);
         delete entity;
     }
 }
@@ -100,10 +80,9 @@ void PhysicalEntitySimulation::entityChangedInternal(EntityItem* entity) {
     assert(entity);
     void* physicsInfo = entity->getPhysicsInfo();
     if (physicsInfo) {
-        ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
+        _pendingChanges.insert(entity);
-        _pendingChanges.insert(motionState);
     } else { 
-        if (!entity->ignoreForCollisions()) {
+        if (!entity->getIgnoreForCollisions()) {
             // The intent is for this object to be in the PhysicsEngine.  
             // Perhaps it's shape has changed and it can now be added?
             _pendingAdds.insert(entity);
@@ -130,14 +109,16 @@ void PhysicalEntitySimulation::clearEntitiesInternal() {
     // TODO: we should probably wait to lock the _physicsEngine so we don't mess up data structures
     // while it is in the middle of a simulation step.  As it is, we're probably in shutdown mode
     // anyway, so maybe the simulation was already properly shutdown?  Cross our fingers...
-    SetOfMotionStates::const_iterator stateItr = _physicalEntities.begin();
+
+    _physicsEngine->deleteObjects(_physicalEntities);
+
     for (auto stateItr : _physicalEntities) {
-        EntityMotionState motionState = static_cast<EntityMotionState*>(*stateItr);
+        EntityMotionState* motionState = static_cast<EntityMotionState*>(&(*stateItr));
-        _physicsEngine->removeObjectFromBullet(motionState);
+        EntityItem* entity = motionState->getEntity();
-        EntityItem* entity = motionState->_entity;
+        entity->setPhysicsInfo(nullptr);
-        _entity->setPhysicsInfo(nullptr);
         delete motionState;
     }
+
     _physicalEntities.clear();
     _pendingRemoves.clear();
     _pendingAdds.clear();
@@ -149,13 +130,13 @@ void PhysicalEntitySimulation::clearEntitiesInternal() {
 VectorOfMotionStates& PhysicalEntitySimulation::getObjectsToRemove() {
     _tempVector.clear();
     for (auto entityItr : _pendingRemoves) {
-        EntityItem* entity = *entityItr;
+        EntityItem* entity = &(*entityItr);
-        _physicalEntities.remove(entity);
         _pendingAdds.remove(entity);
         _pendingChanges.remove(entity);
-        ObjectMotionState* motionState = static_cast<ObjectMotionState*>(entity->getPhysicsInfo());
+        EntityMotionState* motionState = static_cast<EntityMotionState*>(entity->getPhysicsInfo());
         if (motionState) {
             _tempVector.push_back(motionState);
+            _physicalEntities.remove(motionState);
         }
     }
     _pendingRemoves.clear();
@@ -176,15 +157,14 @@ VectorOfMotionStates& PhysicalEntitySimulation::getObjectsToAdd() {
             if (shape) {
                 EntityMotionState* motionState = new EntityMotionState(shape, entity);
                 entity->setPhysicsInfo(static_cast<void*>(motionState));
+                motionState->setMass(entity->computeMass());
                 _physicalEntities.insert(motionState);
+                _tempVector.push_back(motionState);
                 entityItr = _pendingAdds.erase(entityItr);
-                // TODO: figure out how to get shapeInfo (or relevant info) to PhysicsEngine during add
-                //_physicsEngine->addObject(shapeInfo, motionState);
             } else {
                 ++entityItr;
             }
         }
-        _tempVector.push_back(motionState);
     }
     return _tempVector;
 }
@@ -192,7 +172,7 @@ VectorOfMotionStates& PhysicalEntitySimulation::getObjectsToAdd() {
 VectorOfMotionStates& PhysicalEntitySimulation::getObjectsToChange() {
     _tempVector.clear();
     for (auto entityItr : _pendingChanges) {
-        EntityItem* entity = *entityItr;
+        EntityItem* entity = &(*entityItr);
         ObjectMotionState* motionState = static_cast<ObjectMotionState*>(entity->getPhysicsInfo());
         if (motionState) {
             _tempVector.push_back(motionState);
@@ -205,14 +185,32 @@ VectorOfMotionStates& PhysicalEntitySimulation::getObjectsToChange() {
 void PhysicalEntitySimulation::handleOutgoingChanges(VectorOfMotionStates& motionStates) {
     // walk the motionStates looking for those that correspond to entities
     for (auto stateItr : motionStates) {
-        ObjectMotionState* state = *stateItr;
+        ObjectMotionState* state = &(*stateItr);
         if (state->getType() == MOTION_STATE_TYPE_ENTITY) {
             EntityMotionState* entityState = static_cast<EntityMotionState*>(state);
             _outgoingChanges.insert(entityState);
-            _entitiesToSort.insert(entityState->getEntityItem());
+            _entitiesToSort.insert(entityState->getEntity());
+        }
+    }
+   
+    // send outgoing packets
+    uint32_t numSubsteps = _physicsEngine->getNumSubsteps();
+    if (_lastStepSendPackets != numSubsteps) {
+        _lastStepSendPackets = numSubsteps;
+
+        QSet<EntityMotionState*>::iterator stateItr = _outgoingChanges.begin();
+        while (stateItr != _outgoingChanges.end()) {
+            EntityMotionState* state = *stateItr;
+            if (state->doesNotNeedToSendUpdate()) {
+                stateItr = _outgoingChanges.erase(stateItr);
+            } else if (state->shouldSendUpdate(numSubsteps)) {
+                state->sendUpdate(_entityPacketSender, numSubsteps);
+                ++stateItr;
+            } else {
+                ++stateItr;
+            }
         }
     }
-    sendOutgoingPackets();
 }
 
 void PhysicalEntitySimulation::bump(EntityItem* bumpEntity) {

libraries/physics/src/PhysicalEntitySimulation.h

@@ -22,14 +22,16 @@
 
 #include "PhysicsTypedefs.h"
 
+class EntityMotionState;
 class PhysicsEngine;
+class ShapeManager;
 
 class PhysicalEntitySimulation :public EntitySimulation {
 public:
     PhysicalEntitySimulation();
     ~PhysicalEntitySimulation();
 
-    void init(EntityTree* tree, PhysicsEngine* engine, EntityEditPacketSender* packetSender);
+    void init(EntityTree* tree, PhysicsEngine* engine, ShapeManager* shapeManager, EntityEditPacketSender* packetSender);
 
     // overrides for EntitySimulation
     void updateEntitiesInternal(const quint64& now);
@@ -64,7 +66,7 @@ private:
     PhysicsEngine* _physicsEngine = nullptr;
     EntityEditPacketSender* _entityPacketSender = nullptr;
 
-    uint32_t _lastNumSubstepsAtUpdateInternal = 0;
+    uint32_t _lastStepSendPackets = 0;
 };
 
 #endif // hifi_PhysicalEntitySimulation_h

libraries/physics/src/PhysicsEngine.cpp

@@ -68,11 +68,26 @@ void PhysicsEngine::removeObjects(VectorOfMotionStates& objects) {
 
         btRigidBody* body = object->getRigidBody();
         _dynamicsWorld->removeRigidBody(body);
+        removeContacts(object);
 
         // NOTE: setRigidBody() modifies body->m_userPointer so we should clear the MotionState's body BEFORE deleting it.
         object->setRigidBody(nullptr);
         delete body;
+        object->releaseShape();
+    }
+}
+
+void PhysicsEngine::deleteObjects(SetOfMotionStates& objects) {
+    for (auto object : objects) {
+        assert(object);
+    
+        btRigidBody* body = object->getRigidBody();
+        _dynamicsWorld->removeRigidBody(body);
         removeContacts(object);
+
+        // NOTE: setRigidBody() modifies body->m_userPointer so we should clear the MotionState's body BEFORE deleting it.
+        object->setRigidBody(nullptr);
+        delete body;
         object->releaseShape();
     }
 }
@@ -151,6 +166,7 @@ void PhysicsEngine::stepSimulation() {
 }
 
 void PhysicsEngine::doOwnershipInfection(const btCollisionObject* objectA, const btCollisionObject* objectB) {
+    /* TODO: Andrew to make this work for ObjectMotionState
     BT_PROFILE("ownershipInfection");
     assert(objectA);
     assert(objectB);
@@ -184,6 +200,7 @@ void PhysicsEngine::doOwnershipInfection(const btCollisionObject* objectA, const
             a->setShouldClaimSimulationOwnership(true);
         }
     }
+    */
 }
 
 void PhysicsEngine::computeCollisionEvents() {
@@ -215,7 +232,13 @@ void PhysicsEngine::computeCollisionEvents() {
             doOwnershipInfection(objectA, objectB);
         }
     }
-    
+
+    fireCollisionEvents();
+    ++_numContactFrames;
+}
+
+void PhysicsEngine::fireCollisionEvents() {
+    /* TODO: Andrew to make this work for ObjectMotionStates
     const uint32_t CONTINUE_EVENT_FILTER_FREQUENCY = 10;
 
     // scan known contacts and trigger events
@@ -253,14 +276,14 @@ void PhysicsEngine::computeCollisionEvents() {
             ++contactItr;
         }
     }
-    ++_numContactFrames;
+    */
 }
 
 VectorOfMotionStates& PhysicsEngine::getOutgoingChanges() {
     BT_PROFILE("copyOutgoingChanges");
-    _dynamicsWorld.synchronizeMotionStates();
+    _dynamicsWorld->synchronizeMotionStates();
     _hasOutgoingChanges = false;
-    return _dynamicsWorld.getChangedMotionStates();
+    return _dynamicsWorld->getChangedMotionStates();
 }
 
 void PhysicsEngine::dumpStatsIfNecessary() {
@@ -287,7 +310,9 @@ void PhysicsEngine::addObject(ObjectMotionState* motionState) {
     btVector3 inertia(0.0f, 0.0f, 0.0f);
     float mass = 0.0f;
     btRigidBody* body = nullptr;
-    switch(motionState->computeObjectMotionType()) {
+    MotionType motionType = motionState->computeObjectMotionType();
+    motionState->setMotionType(motionType);
+    switch(motionType) {
         case MOTION_TYPE_KINEMATIC: {
             body = new btRigidBody(mass, motionState, shape, inertia);
             body->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
@@ -330,11 +355,50 @@ void PhysicsEngine::addObject(ObjectMotionState* motionState) {
     motionState->updateBodyMaterialProperties();
 
     _dynamicsWorld->addRigidBody(body);
-    motionState->resetMeasuredBodyAcceleration();
 }
 
-/* TODO: convert bump() to take an ObjectMotionState.
+void PhysicsEngine::bump(ObjectMotionState* object) {
-* Expose SimulationID to ObjectMotionState*/
+    /* TODO: Andrew to implement this
+    // If this node is doing something like deleting an entity, scan for contacts involving the
+    // entity.  For each found, flag the other entity involved as being simulated by this node.
+    int numManifolds = _collisionDispatcher->getNumManifolds();
+    for (int i = 0; i < numManifolds; ++i) {
+        btPersistentManifold* contactManifold =  _collisionDispatcher->getManifoldByIndexInternal(i);
+        if (contactManifold->getNumContacts() > 0) {
+            const btCollisionObject* objectA = static_cast<const btCollisionObject*>(contactManifold->getBody0());
+            const btCollisionObject* objectB = static_cast<const btCollisionObject*>(contactManifold->getBody1());
+            if (objectA && objectB) {
+                void* a = objectA->getUserPointer();
+                void* b = objectB->getUserPointer();
+                if (a && b) {
+                    EntityMotionState* entityMotionStateA = static_cast<EntityMotionState*>(a);
+                    EntityMotionState* entityMotionStateB = static_cast<EntityMotionState*>(b);
+                    EntityItem* entityA = entityMotionStateA ? entityMotionStateA->getEntity() : nullptr;
+                    EntityItem* entityB = entityMotionStateB ? entityMotionStateB->getEntity() : nullptr;
+                    if (entityA && entityB) {
+                        if (entityA == bumpEntity) {
+                            entityMotionStateB->setShouldClaimSimulationOwnership(true);
+                            if (!objectB->isActive()) {
+                                objectB->setActivationState(ACTIVE_TAG);
+                            }
+                        }
+                        if (entityB == bumpEntity) {
+                            entityMotionStateA->setShouldClaimSimulationOwnership(true);
+                            if (!objectA->isActive()) {
+                                objectA->setActivationState(ACTIVE_TAG);
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+    */
+}
+
+/*
+// TODO: convert bump() to take an ObjectMotionState.
+// Expose SimulationID to ObjectMotionState
 void PhysicsEngine::bump(EntityItem* bumpEntity) {
     // If this node is doing something like deleting an entity, scan for contacts involving the
     // entity.  For each found, flag the other entity involved as being simulated by this node.
@@ -371,58 +435,13 @@ void PhysicsEngine::bump(EntityItem* bumpEntity) {
         }
     }
 }
+*/
 
 void PhysicsEngine::removeRigidBody(btRigidBody* body) {
     // pull body out of physics engine
     _dynamicsWorld->removeRigidBody(body);
 }
 
-void PhysicsEngine::addRigidBody(btRigidBody* body, MotionType motionType, float mass) {
-    switch (motionType) {
-        case MOTION_TYPE_KINEMATIC: {
-            int collisionFlags = body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT;
-            collisionFlags &= ~(btCollisionObject::CF_STATIC_OBJECT);
-            body->setCollisionFlags(collisionFlags);
-            body->forceActivationState(DISABLE_DEACTIVATION);
-
-            body->setMassProps(0.0f, btVector3(0.0f, 0.0f, 0.0f));
-            body->updateInertiaTensor();
-            break;
-        }
-        case MOTION_TYPE_DYNAMIC: {
-            int collisionFlags = body->getCollisionFlags() & ~(btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_STATIC_OBJECT);
-            body->setCollisionFlags(collisionFlags);
-            if (! (flags & EntityItem::DIRTY_MASS)) {
-                // always update mass properties when going dynamic (unless it's already been done above)
-                btVector3 inertia(0.0f, 0.0f, 0.0f);
-                body->getCollisionShape()->calculateLocalInertia(mass, inertia);
-                body->setMassProps(mass, inertia);
-                body->updateInertiaTensor();
-            }
-            body->forceActivationState(ACTIVE_TAG);
-            break;
-        }
-        default: {
-            // MOTION_TYPE_STATIC
-            int collisionFlags = body->getCollisionFlags() | btCollisionObject::CF_STATIC_OBJECT;
-            collisionFlags &= ~(btCollisionObject::CF_KINEMATIC_OBJECT);
-            body->setCollisionFlags(collisionFlags);
-            body->forceActivationState(DISABLE_SIMULATION);
-
-            body->setMassProps(0.0f, btVector3(0.0f, 0.0f, 0.0f));
-            body->updateInertiaTensor();
-
-            body->setLinearVelocity(btVector3(0.0f, 0.0f, 0.0f));
-            body->setAngularVelocity(btVector3(0.0f, 0.0f, 0.0f));
-            break;
-        }
-    }
-
-    _dynamicsWorld->addRigidBody(body);
-
-    body->activate();
-}
-
 void PhysicsEngine::setCharacterController(DynamicCharacterController* character) {
     if (_characterController != character) {
         if (_characterController) {

libraries/physics/src/PhysicsEngine.h

@@ -53,11 +53,13 @@ public:
 
     /// process queue of changed from external sources
     void removeObjects(VectorOfMotionStates& objects);
+    void deleteObjects(SetOfMotionStates& objects);
     void addObjects(VectorOfMotionStates& objects);
     void changeObjects(VectorOfMotionStates& objects);
 
     void stepSimulation();
     void computeCollisionEvents();
+    void fireCollisionEvents();
 
     bool hasOutgoingChanges() const { return _hasOutgoingChanges; }
     VectorOfMotionStates& getOutgoingChanges();
@@ -71,7 +73,11 @@ public:
 
     /// \param motionState pointer to Object's MotionState
     /// \return true if Object added
-    void addObject(const ShapeInfo& shapeInfo, btCollisionShape* shape, ObjectMotionState* motionState);
+    void addObject(ObjectMotionState* motionState);
+
+    void bump(ObjectMotionState* motionState);
+
+    void removeRigidBody(btRigidBody* body);
 
     void setCharacterController(DynamicCharacterController* character);
 

libraries/physics/src/ThreadSafeDynamicsWorld.cpp

@@ -17,6 +17,7 @@
 
 #include <LinearMath/btQuickprof.h>
 
+#include "ObjectMotionState.h"
 #include "ThreadSafeDynamicsWorld.h"
 
 ThreadSafeDynamicsWorld::ThreadSafeDynamicsWorld(
@@ -95,7 +96,7 @@ void ThreadSafeDynamicsWorld::synchronizeMotionStates() {
             btRigidBody* body = btRigidBody::upcast(colObj);
             if (body) {
                 synchronizeSingleMotionState(body);
-                _changedMotionStates.push_back(body->getMotionState());
+                _changedMotionStates.push_back(static_cast<ObjectMotionState*>(body->getMotionState()));
             }
         }
     } else  {       
@@ -104,7 +105,7 @@ void ThreadSafeDynamicsWorld::synchronizeMotionStates() {
             btRigidBody* body = m_nonStaticRigidBodies[i];
             if (body->isActive()) {
                 synchronizeSingleMotionState(body);
-                _changedMotionStates.push_back(body->getMotionState());
+                _changedMotionStates.push_back(static_cast<ObjectMotionState*>(body->getMotionState()));
             }
         }
     }   

libraries/physics/src/ThreadSafeDynamicsWorld.h

@@ -18,6 +18,7 @@
 #ifndef hifi_ThreadSafeDynamicsWorld_h
 #define hifi_ThreadSafeDynamicsWorld_h
 
+#include <BulletDynamics/Dynamics/btRigidBody.h>
 #include <BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h>
 
 #include "PhysicsTypedefs.h"
@@ -34,13 +35,14 @@ public:
 
     // virtual overrides from btDiscreteDynamicsWorld
     int stepSimulation( btScalar timeStep, int maxSubSteps=1, btScalar fixedTimeStep=btScalar(1.)/btScalar(60.));
+    void synchronizeMotionStates();
 
     // btDiscreteDynamicsWorld::m_localTime is the portion of real-time that has not yet been simulated
     // but is used for MotionState::setWorldTransform() extrapolation (a feature that Bullet uses to provide 
     // smoother rendering of objects when the physics simulation loop is ansynchronous to the render loop).
     float getLocalTimeAccumulation() const { return m_localTime; }
 
-    VectorOfMotionStates& getChangedMotionStates() const { return _changedMotionStates; }
+    VectorOfMotionStates& getChangedMotionStates() { return _changedMotionStates; }
 
 private:
     VectorOfMotionStates _changedMotionStates;