implement EntitySimulation API in PhysicsEngine

libraries/physics/src/PhysicsEngine.cpp

@@ -8,6 +8,10 @@
 //  Distributed under the Apache License, Version 2.0.
 //  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
 //
+// TODO: make _incomingEntityChanges to be list of MotionState*'s.
+// TODO: make MotionState able to clear incoming flags
+// TODO: make MotionState::setWorldTransform() put itself on _incomingChanges list
+// TODO: give PhysicsEngine instance an _entityPacketSender
 
 #include "PhysicsEngine.h"
 #ifdef USE_BULLET_PHYSICS
@@ -32,104 +36,32 @@ PhysicsEngine::PhysicsEngine(const glm::vec3& offset)
 PhysicsEngine::~PhysicsEngine() {
 }
 
-/// \param tree pointer to EntityTree which is stored internally
+// begin EntitySimulation overrides
-void PhysicsEngine::setEntityTree(EntityTree* tree) {
+void PhysicsEngine::updateEntitiesInternal(const quint64& now) {
-    assert(_entityTree == NULL);
+    // relay outgoing changes: from physics engine to EntityItem's
-    assert(tree);
-    _entityTree = tree;
-}
 
-void PhysicsEngine::relayIncomingChangesToSimulation() {
+    QSet<ObjectMotionState*>::iterator stateItr = _outgoingPhysics.begin();
-    // process incoming changes
+    uint32_t frame = getFrameCount();
-    QSet<EntityItem*>::iterator item_itr = _incomingPhysics.begin();
-    while (item_itr != _incomingPhysics.end()) {
-        EntityItem* entity = *item_itr;
-        void* physicsInfo = entity->getPhysicsInfo();
-        if (physicsInfo) {
-            ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
-            uint32_t preOutgoingFlags = motionState->getOutgoingUpdateFlags();
-            updateObject(motionState, entity->getUpdateFlags());
-            uint32_t postOutgoingFlags = motionState->getOutgoingUpdateFlags();
-            if (preOutgoingFlags && !postOutgoingFlags) {
-                _outgoingPhysics.remove(entity);
-            }
-        }
-        entity->clearUpdateFlags();
-        ++item_itr;
-    } 
-    _incomingPhysics.clear();
-}
-
-/// \param[out] entitiesToDelete list of entities removed from simulation and should be deleted.
-void PhysicsEngine::updateEntities(QSet<EntityItem*>& entitiesToDelete) {
-    // TODO: Andrew to make this work.
-    EnitySimulation::updateEntities(entitiesToDelete);
-
-    item_itr = _outgoingPhysics.begin();
-    uint32_t simulationFrame = getSimulationFrame();
     float subStepRemainder = getSubStepRemainder();
-    while (item_itr != _outgoingPhysics.end()) {
+    while (stateItr != _outgoingPhysics.end()) {
-        EntityItem* entity = *item_itr;
+        ObjectMotionState* state = *stateItr;
-        void* physicsInfo = entity->getPhysicsInfo();
+        if (state->shouldSendUpdate(frame, subStepRemainder)) {
-        if (physicsInfo) {
+            state->sendUpdate(_entityPacketSender);
-            ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
+            ++stateItr;
-            if (motionState->shouldSendUpdate(simulationFrame, subStepRemainder)) {
+        } else if (state->isAtRest()) {
-                EntityItemProperties properties = entity->getProperties();
+            stateItr = _outgoingPhysics.erase(stateItr);
-                motionState->pushToProperties(properties);
+        } else {
-
+            ++stateItr;
-                /*
-                properties.setVelocity(newVelocity);
-                properties.setPosition(newPosition);
-                properties.setRotation(newRotation);
-                properties.setAngularVelocity(newAngularVelocity);
-                properties.setLastEdited(now);
-                */
-        
-                EntityItemID id(entity->getID());
-                _packetSender->queueEditEntityMessage(PacketTypeEntityAddOrEdit, id, properties);
-                ++itemItr;
-            } else if (motionState->shouldRemoveFromOutgoingPhysics()) {
-                itemItr = _outgoingPhysics.erase(itemItr);
-            } else {
-                ++itemItr;
-            }
         }
     }
-
-    item_itr = _movedEntities.begin();
-    AACube domainBounds(glm::vec3(0.0f,0.0f,0.0f), 1.0f);
-    while (item_itr != _movedEntities.end()) {
-        EntityItem* entity = *item_itr;
-        void* physicsInfo = entity->getPhysicsInfo();
-        if (physicsInfo) {
-            ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
-    
-            AACube oldCube, newCube;
-            motionState->getBoundingCubes(oldCube, newCube);
-    
-            // check to see if this movement has sent the entity outside of the domain.
-            if (!domainBounds.touches(newCube)) {
-                //qDebug() << "Entity " << entity->getEntityItemID() << " moved out of domain bounds.";
-                entitiesToDelete << entity->getEntityItemID();
-                clearEntityState(entity);
-            } else if (newCube != oldCube) {
-                moveOperator.addEntityToMoveList(entity, oldCube, newCube);
-            }
-        }
-    }
-
-    // TODO: check for entities that have exited the world boundaries
 }
 
-/// \param entity pointer to EntityItem to add to the simulation
+void PhysicsEngine::addEntityInternal(EntityItem* entity) {
-/// \sideeffect the EntityItem::_simulationState member may be updated to indicate membership to internal list
-void PhysicsEngine::addEntity(EntityItem* entity) {
     assert(entity);
     void* physicsInfo = entity->getPhysicsInfo();
     if (!physicsInfo) {
-        assert(!_entities.contains(entity));
-        _entities.insert(entity);
         EntityMotionState* motionState = new EntityMotionState(entity);
+        _entityMotionStates.insert(motionState);
         if (addObject(motionState)) {
             entity->setPhysicsInfo(static_cast<void*>(motionState));
         } else {
@@ -139,36 +71,52 @@ void PhysicsEngine::addEntity(EntityItem* entity) {
     }
 }
 
-/// \param entity pointer to EntityItem to removed from the simulation
+void PhysicsEngine::removeEntityInternal(EntityItem* entity) {
-/// \sideeffect the EntityItem::_simulationState member may be updated to indicate non-membership to internal list
-void PhysicsEngine::removeEntity(EntityItem* entity) {
     assert(entity);
     void* physicsInfo = entity->getPhysicsInfo();
     if (physicsInfo) {
         ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
         removeObject(motionState);
         entity->setPhysicsInfo(NULL);
+        _entityMotionStates.remove(motionState);
     }
-    _entities.remove(entity);
 }
 
-/// \param entity pointer to EntityItem to that may have changed in a way that would affect its simulation
+void PhysicsEngine::entityChangedInternal(EntityItem* entity) {
-void PhysicsEngine::entityChanged(EntityItem* entity) {
+    // queue incoming changes: from external sources (script, EntityServer, etc) to physics engine
-    _incomingPhysics.insert(entity);
+    _incomingEntityChanges.insert(entity);
 }
 
-void PhysicsEngine::clearEntities() {
+void PhysicsEngine::clearEntitiesInternal() {
     // For now we assume this would only be called on shutdown in which case we can just let the memory get lost.
-    QSet<EntityItem*>::const_iterator entityItr = _entities.begin();
+    QSet<ObjectMotionState*>::const_iterator stateItr = _entityMotionStates.begin();
-    for (entityItr = _entities.begin(); entityItr != _entities.end(); ++entityItr) {
+    for (stateItr = _entityMotionStates.begin(); stateItr != _entityMotionStates.end(); ++stateItr) {
-        void* physicsInfo = (*entityItr)->getPhysicsInfo();
+        removeObject(*stateItr);
+    }
+    _entityMotionStates.clear();
+    _incomingEntityChanges.clear();
+}
+// end EntitySimulation overrides
+
+void PhysicsEngine::relayIncomingChangesToSimulation() {
+    // process incoming changes
+    QSet<EntityItem*>::iterator itemItr = _incomingEntityChanges.begin();
+    while (itemItr != _incomingEntityChanges.end()) {
+        EntityItem* entity = *itemItr;
+        void* physicsInfo = entity->getPhysicsInfo();
         if (physicsInfo) {
             ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
-            removeObject(motionState);
+            uint32_t flags = entity->getDirtyFlags();
+            if (flags & DIRTY_PHYSICS_FLAGS) {
+                updateObject(motionState, flags);
+                // this incoming change will override any outgoing changes to the same parameters
+                motionState->clearOutgoingDirtyFlags(flags);
+            }
         }
-    }
+        entity->clearDirtyFlags();
-    _entities.clear();
+        ++itemItr;
-    _incomingPhysics.clear();
+    } 
+    _incomingEntityChanges.clear();
 }
 
 // virtual
@@ -353,10 +301,10 @@ bool PhysicsEngine::updateObject(ObjectMotionState* motionState, uint32_t flags)
         return false;
     }
 
-    if (flags & PHYSICS_UPDATE_HARD) {
+    if (flags & HARD_DIRTY_PHYSICS_FLAGS) {
         // a hard update requires the body be pulled out of physics engine, changed, then reinserted
         updateObjectHard(body, motionState, flags);
-    } else if (flags & PHYSICS_UPDATE_EASY) {
+    } else if (flags & EASY_DIRTY_PHYSICS_FLAGS) {
         // an easy update does not require that the body be pulled out of physics engine
         updateObjectEasy(body, motionState, flags);
     }
@@ -370,7 +318,7 @@ void PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motio
     // pull body out of physics engine
     _dynamicsWorld->removeRigidBody(body);
 
-    if (flags & PHYSICS_UPDATE_SHAPE) {
+    if (flags & EntityItem::DIRTY_SHAPE) {
         btCollisionShape* oldShape = body->getCollisionShape();
         ShapeInfo info;
         motionState->computeShapeInfo(info);
@@ -384,9 +332,9 @@ void PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motio
             _shapeManager.releaseShape(newShape);
         }
         // MASS bit should be set whenever SHAPE is set
-        assert(flags & PHYSICS_UPDATE_MASS);
+        assert(flags & EntityItem::DIRTY_MASS);
     }
-    bool easyUpdate = flags & PHYSICS_UPDATE_EASY;
+    bool easyUpdate = flags & EASY_DIRTY_PHYSICS_FLAGS;
     if (easyUpdate) {
         updateObjectEasy(body, motionState, flags);
     }
@@ -406,7 +354,7 @@ void PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motio
         case MOTION_TYPE_DYNAMIC: {
             int collisionFlags = body->getCollisionFlags() & ~(btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_STATIC_OBJECT);
             body->setCollisionFlags(collisionFlags);
-            if (! (flags & PHYSICS_UPDATE_MASS)) {
+            if (! (flags & EntityItem::DIRTY_MASS)) {
                 // always update mass properties when going dynamic (unless it's already been done)
                 btVector3 inertia(0.0f, 0.0f, 0.0f);
                 float mass = motionState->getMass();
@@ -441,19 +389,19 @@ void PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motio
 
 // private
 void PhysicsEngine::updateObjectEasy(btRigidBody* body, ObjectMotionState* motionState, uint32_t flags) {
-    if (flags & PHYSICS_UPDATE_POSITION) {
+    if (flags & EntityItem::DIRTY_POSITION) {
         btTransform transform;
         motionState->getWorldTransform(transform);
         body->setWorldTransform(transform);
     }
-    if (flags & PHYSICS_UPDATE_VELOCITY) {
+    if (flags & EntityItem::DIRTY_VELOCITY) {
         motionState->applyVelocities();
         motionState->applyGravity();
     }
     body->setRestitution(motionState->_restitution);
     body->setFriction(motionState->_friction);
 
-    if (flags & PHYSICS_UPDATE_MASS) {
+    if (flags & EntityItem::DIRTY_MASS) {
         float mass = motionState->getMass();
         btVector3 inertia(0.0f, 0.0f, 0.0f);
         body->getCollisionShape()->calculateLocalInertia(mass, inertia);

libraries/physics/src/PhysicsEngine.h

@@ -12,24 +12,8 @@
 #ifndef hifi_PhysicsEngine_h
 #define hifi_PhysicsEngine_h
 
-enum PhysicsUpdateFlag {
-    PHYSICS_UPDATE_POSITION = 0x0001,
-    PHYSICS_UPDATE_VELOCITY = 0x0002,
-    PHYSICS_UPDATE_GRAVITY = 0x0004,
-    PHYSICS_UPDATE_MASS = 0x0008,
-    PHYSICS_UPDATE_COLLISION_GROUP = 0x0010,
-    PHYSICS_UPDATE_MOTION_TYPE = 0x0020,
-    PHYSICS_UPDATE_SHAPE = 0x0040
-};
-
 typedef unsigned int uint32_t;
 
-// The update flags trigger two varieties of updates: "hard" which require the body to be pulled 
-// and re-added to the physics engine and "easy" which just updates the body properties.
-const uint32_t PHYSICS_UPDATE_HARD = PHYSICS_UPDATE_MOTION_TYPE | PHYSICS_UPDATE_SHAPE;
-const uint32_t PHYSICS_UPDATE_EASY = PHYSICS_UPDATE_POSITION | PHYSICS_UPDATE_VELOCITY | 
-        PHYSICS_UPDATE_GRAVITY | PHYSICS_UPDATE_MASS | PHYSICS_UPDATE_COLLISION_GROUP;
-
 #ifdef USE_BULLET_PHYSICS
 
 #include <QSet>
@@ -40,6 +24,7 @@ const uint32_t PHYSICS_UPDATE_EASY = PHYSICS_UPDATE_POSITION | PHYSICS_UPDATE_VE
 
 #include "BulletUtil.h"
 #include "ObjectMotionState.h"
+#include "ObjectMotionState.h"
 #include "PositionHashKey.h"
 #include "ShapeManager.h"
 #include "ThreadSafeDynamicsWorld.h"
@@ -54,30 +39,12 @@ public:
 
     ~PhysicsEngine();
 
-    // override from EntitySimulation
+    // overrides from EntitySimulation
-    /// \param tree pointer to EntityTree which is stored internally
+    void updateEntitiesInternal(const quint64& now);
-    void setEntityTree(EntityTree* tree);
+    void addEntityInternal(EntityItem* entity);
-
+    void removeEntityInternal(EntityItem* entity);
-    // override from EntitySimulation
+    void entityChangedInternal(EntityItem* entity);
-    /// \param[out] entitiesToDelete list of entities removed from simulation and should be deleted.
+    void clearEntitiesInternal();
-    void updateEntities(QSet<EntityItem*>& entitiesToDelete);
-
-    // override from EntitySimulation
-    /// \param entity pointer to EntityItem to add to the simulation
-    /// \sideeffect the EntityItem::_simulationState member may be updated to indicate membership to internal list
-    void addEntity(EntityItem* entity);
-
-    // override from EntitySimulation
-    /// \param entity pointer to EntityItem to removed from the simulation
-    /// \sideeffect the EntityItem::_simulationState member may be updated to indicate non-membership to internal list
-    void removeEntity(EntityItem* entity);
-
-    // override from EntitySimulation
-    /// \param entity pointer to EntityItem to that may have changed in a way that would affect its simulation
-    void entityChanged(EntityItem* entity);
-
-    // override from EntitySimulation
-    void clearEntities();
 
     virtual void init();
 
@@ -112,6 +79,9 @@ public:
     /// \return true if entity updated
     bool updateObject(ObjectMotionState* motionState, uint32_t flags);
 
+    /// process queue of changed from external sources
+    void relayIncomingChangesToSimulation();
+
     /// \return duration of fixed simulation substep
     float getFixedSubStep() const;
 
@@ -122,7 +92,7 @@ public:
     // Bullet will extrapolate the positions provided to MotionState::setWorldTransform() in an effort to provide 
     // smoother visible motion when the render frame rate does not match that of the simulation loop.  We provide 
     // access to this fraction for improved filtering of update packets to interested parties.
-    float getSubStepRemainder() { _dynamicsWorld->getLocalTimeAccumulation(); }
+    float getSubStepRemainder() { return _dynamicsWorld->getLocalTimeAccumulation(); }
 
 protected:
     void updateObjectHard(btRigidBody* body, ObjectMotionState* motionState, uint32_t flags);
@@ -141,9 +111,11 @@ private:
     btHashMap<PositionHashKey, VoxelObject> _voxels;
 
     // EntitySimulation stuff
-    QSet<EntityItem*> _entities; // all entities that we track
+    QSet<ObjectMotionState*> _entityMotionStates; // all entities that we track
-    QSet<EntityItem*> _incomingPhysics; // entities with pending physics changes by script or packet
+    QSet<EntityItem*> _incomingEntityChanges; // entities with pending physics changes by script or packet
-    QSet<EntityItem*> _outgoingPhysics; // entites with pending transform changes by physics simulation
+    QSet<ObjectMotionState*> _outgoingPhysics; // MotionStates with pending transform changes from physics simulation
+
+    EntityEditPacketSender* _entityPacketSender;
 
     uint32_t _frameCount;
 };