add logic for when to update moving entity

libraries/physics/src/EntityMotionState.cpp

@@ -16,21 +16,33 @@
 #endif // USE_BULLET_PHYSICS
 #include "EntityMotionState.h"
 
-// TODO: store _cachedWorldOffset in a more central location -- VoxelTree and others also need to know about it
+// TODO: store _worldOffset in a more central location -- VoxelTree and others also need to know about it
 // origin of physics simulation in world frame
-glm::vec3 _cachedWorldOffset(0.0f);
+glm::vec3 _worldOffset(0.0f);
 
 // static 
 void EntityMotionState::setWorldOffset(const glm::vec3& offset) {
-    _cachedWorldOffset = offset;
+    _worldOffset = offset;
 }
 
 // static 
 const glm::vec3& getWorldOffset() {
-    return _cachedWorldOffset;
+    return _worldOffset;
 }
 
-EntityMotionState::EntityMotionState(EntityItem* entity) : _entity(entity) {
+EntityMotionState::EntityMotionState(EntityItem* entity) 
+    :   _entity(entity),
+        _sentMoving(false),
+        _notMoving(true),
+        _recievedNotMoving(false),
+        _sentFrame(0),
+        _sentPosition(0.0f),
+        _sentRotation(),
+        _sentVelocity(0.0f),
+        _sentVelocity(0.0f),
+        _sentAngularVelocity(0.0f),
+        _sentGravity(0.0f)
+{
     assert(entity != NULL);
     _oldBoundingCube = _entity->getMaximumAACube();
 }
@@ -38,7 +50,7 @@ EntityMotionState::EntityMotionState(EntityItem* entity) : _entity(entity) {
 EntityMotionState::~EntityMotionState() {
 }
 
-MotionType EntityMotionState::getMotionType() const {
+MotionType EntityMotionState::computeMotionType() const {
     // HACK: According to EntityTree the meaning of "static" is "not moving" whereas
     // to Bullet it means "can't move".  For demo purposes we temporarily interpret
     // Entity::weightless to mean Bullet::static.
@@ -53,7 +65,7 @@ MotionType EntityMotionState::getMotionType() const {
 //     it is an opportunity for outside code to update the object's simulation position
 void EntityMotionState::getWorldTransform (btTransform &worldTrans) const {
     btVector3 pos;
-    glmToBullet(_entity->getPositionInMeters() - _cachedWorldOffset, pos);
+    glmToBullet(_entity->getPositionInMeters() - _worldOffset, pos);
     worldTrans.setOrigin(pos);
 
     btQuaternion rot;
@@ -70,7 +82,7 @@ void EntityMotionState::setWorldTransform (const btTransform &worldTrans) {
     if (! (updateFlags &  EntityItem::UPDATE_POSITION)) {
         glm::vec3 pos;
         bulletToGLM(worldTrans.getOrigin(), pos);
-        _entity->setPositionInMeters(pos + _cachedWorldOffset);
+        _entity->setPositionInMeters(pos + _worldOffset);
     
         glm::quat rot;
         bulletToGLM(worldTrans.getRotation(), rot);
@@ -116,3 +128,42 @@ void EntityMotionState::getBoundingCubes(AACube& oldCube, AACube& newCube) {
     _oldBoundingCube = newCube;
 }
 
+const float FIXED_SUBSTEP = 1.0f / 60.0f;
+bool EntityMotionState::shouldSendUpdate(uint32_t simulationFrame, float subStepRemainder) const {
+    // TODO: Andrew to test this and make sure it works as expected
+    assert(_body);
+    float dt = (float)(simulationFrame - _sentFrame) * FIXED_SUBSTEP + subStepRemainder;
+    const float DEFAULT_UPDATE_PERIOD = 10.0f;j
+    if (dt > DEFAULT_UPDATE_PERIOD) {
+        return ! (_notMoving && _recievedNotMoving);
+    }
+    if (_sentMoving && _notMoving) {
+        return true;
+    }
+
+    // compute position error
+    glm::vec3 expectedPosition = _sentPosition + dt * (_sentVelocity + (0.5f * dt) * _sentGravity);
+
+    glm::vec3 actualPos;
+    btTransform worldTrans = _body->getWorldTransform();
+    bulletToGLM(worldTrans.getOrigin(), actualPos);
+    
+    float dx2 = glm::length2(actualPosition - expectedPosition);
+    const MAX_POSITION_ERROR_SQUARED = 0.001; // 0.001 m^2 ~~> 0.03 m
+    if (dx2 > MAX_POSITION_ERROR_SQUARED) {
+        return true;
+    }
+
+    // compute rotation error
+    float spin = glm::length(_sentAngularVelocity);
+    glm::quat expectedRotation = _sentRotation;
+    const float MIN_SPIN = 1.0e-4f;
+    if (spin > MIN_SPIN) {
+        glm::vec3 axis = _sentAngularVelocity / spin;
+        expectedRotation = glm::angleAxis(dt * spin, axis) * _sentRotation;
+    }
+    const float MIN_ROTATION_DOT = 0.98f;
+    glm::quat actualRotation;
+    bulletToGLM(worldTrans.getRotation(), actualRotation);
+    return (glm::dot(actualRotation, expectedRotation) < MIN_ROTATION_DOT);
+}

libraries/physics/src/EntityMotionState.h

@@ -14,6 +14,8 @@
 
 #include <AACube.h>
 
+#include "PhysicsEngineParams.h"
+
 #ifdef USE_BULLET_PHYSICS
 #include "ObjectMotionState.h"
 #else // USE_BULLET_PHYSICS
@@ -26,20 +28,35 @@ public:
 
 class EntityItem;
 
+// From the MotionState's perspective:
+//      Inside = physics simulation
+//      Outside = external agents (scripts, user interaction, other simulations)
+
 class EntityMotionState : public ObjectMotionState {
 public:
+    // The WorldOffset is used to keep the positions of objects in the simulation near the origin, to
+    // reduce numerical error when computing vector differences.  In other words: The EntityMotionState 
+    // class translates between the simulation-frame and the world-frame as known by the render pipeline, 
+    // various object trees, etc.  The EntityMotionState class uses a static "worldOffset" to help in
+    // the translations.
     static void setWorldOffset(const glm::vec3& offset);
     static const glm::vec3& getWorldOffset();
 
     EntityMotionState(EntityItem* item);
     virtual ~EntityMotionState();
 
-    MotionType getMotionType() const;
+    /// \return MOTION_TYPE_DYNAMIC or MOTION_TYPE_STATIC based on params set in EntityItem
+    MotionType computeMotionType() const;
 
 #ifdef USE_BULLET_PHYSICS
+    // this relays incoming position/rotation to the RigidBody
     void getWorldTransform (btTransform &worldTrans) const;
+
+    // this relays outgoing position/rotation to the EntityItem
     void setWorldTransform (const btTransform &worldTrans);
 #endif // USE_BULLET_PHYSICS
+
+    // these relay incoming values to the RigidBody
     void applyVelocities() const;
     void applyGravity() const;
 
@@ -47,9 +64,23 @@ public:
 
     void getBoundingCubes(AACube& oldCube, AACube& newCube);
 
+    bool shouldSendUpdate(uint32_t simulationFrame, float subStepRemainder) const;
+
 protected:
     EntityItem* _entity;
     AACube _oldBoundingCube;
+
+    bool _sentMoving;
+    bool _notMoving;
+    bool _receivedNotMoving;
+    // TODO: Andrew to talk to Brad about what to do about lost packets ^^^
+
+    uint32_t _sentFrame;
+    glm::vec3 _sentPosition;
+    glm::quat _sentRotation;;
+    glm::vec3 _sentVelocity;
+    glm::vec3 _sentAngularVelocity;
+    glm::vec3 _sentGravity;
 };
 
 #endif // hifi_EntityMotionState_h

libraries/physics/src/PhysicsEngine.cpp

@@ -25,7 +25,8 @@ PhysicsEngine::PhysicsEngine(const glm::vec3& offset)
         _constraintSolver(NULL), 
         _dynamicsWorld(NULL),
         _originOffset(offset),
-        _voxels() {
+        _voxels(),
+        _frameCount(0) {
 }
 
 PhysicsEngine::~PhysicsEngine() {
@@ -38,41 +39,85 @@ void PhysicsEngine::setEntityTree(EntityTree* tree) {
     _entityTree = tree;
 }
 
-/// \param[out] entitiesToDelete list of entities removed from simulation and should be deleted.
-void PhysicsEngine::updateEntities(QSet<EntityItem*>& entitiesToDelete) {
-    // relay changes
-    QSet<EntityItem*>::iterator item_itr = _changedEntities.begin();
-    while (item_itr != _changedEntities.end()) {
+void PhysicsEngine::relayIncomingChangesToSimulation() {
+    // process incoming changes
+    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;
     } 
-    _changedEntities.clear();
+    _incomingPhysics.clear();
+}
 
-    // hunt for entities who have expired
-    // TODO: make EntityItems use an expiry to make this work faster.
-    item_itr = _mortalEntities.begin();
-    while (item_itr != _mortalEntities.end()) {
+/// \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()) {
         EntityItem* entity = *item_itr;
-        // always check to see if the lifetime has expired, for immortal entities this is always false
-        if (entity->lifetimeHasExpired()) {
-            qDebug() << "Lifetime has expired for entity:" << entity->getEntityItemID();
-            entitiesToDelete.insert(entity);
-            // remove entity from the list
-            item_itr = _mortalEntities.erase(item_itr);
-            _entities.remove(entity);
-        } else if (entity->isImmortal()) {
-            // remove entity from the list
-            item_itr = _mortalEntities.erase(item_itr);
-        } else {
-            ++item_itr;
+        void* physicsInfo = entity->getPhysicsInfo();
+        if (physicsInfo) {
+            ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
+            if (motionState->shouldSendUpdate(simulationFrame, subStepRemainder)) {
+                EntityItemProperties properties = entity->getProperties();
+                motionState->pushToProperties(properties);
+
+                /*
+                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
 }
 
@@ -84,9 +129,6 @@ void PhysicsEngine::addEntity(EntityItem* entity) {
     if (!physicsInfo) {
         assert(!_entities.contains(entity));
         _entities.insert(entity);
-        if (entity->isMortal()) {
-            _mortalEntities.insert(entity);
-        }
         EntityMotionState* motionState = new EntityMotionState(entity);
         if (addObject(motionState)) {
             entity->setPhysicsInfo(static_cast<void*>(motionState));
@@ -108,12 +150,11 @@ void PhysicsEngine::removeEntity(EntityItem* entity) {
         entity->setPhysicsInfo(NULL);
     }
     _entities.remove(entity);
-    _mortalEntities.remove(entity);
 }
 
 /// \param entity pointer to EntityItem to that may have changed in a way that would affect its simulation
 void PhysicsEngine::entityChanged(EntityItem* entity) {
-    _changedEntities.insert(entity);
+    _incomingPhysics.insert(entity);
 }
 
 void PhysicsEngine::clearEntities() {
@@ -127,8 +168,7 @@ void PhysicsEngine::clearEntities() {
         }
     }
     _entities.clear();
-    _changedEntities.clear();
-    _mortalEntities.clear();
+    _incomingPhysics.clear();
 }
 
 // virtual
@@ -165,15 +205,18 @@ void PhysicsEngine::init() {
     }
 }
 
+const float FIXED_SUBSTEP = 1.0f / 60.0f;
+
 void PhysicsEngine::stepSimulation() {
-    const float MAX_TIMESTEP = 1.0f / 30.0f;
-    const int MAX_NUM_SUBSTEPS = 2;
-    const float FIXED_SUBSTEP = 1.0f / 60.0f;
+    const int MAX_NUM_SUBSTEPS = 4;
+    const float MAX_TIMESTEP = (float)MAX_NUM_SUBSTEPS * FIXED_SUBSTEP;
 
     float dt = 1.0e-6f * (float)(_clock.getTimeMicroseconds());
     _clock.reset();
     float timeStep = btMin(dt, MAX_TIMESTEP);
-    _dynamicsWorld->stepSimulation(timeStep, MAX_NUM_SUBSTEPS, FIXED_SUBSTEP);
+    // TODO: Andrew to build a list of outgoingChanges when motionStates are synched, then send the updates out
+    int numSubSteps = _dynamicsWorld->stepSimulation(timeStep, MAX_NUM_SUBSTEPS, FIXED_SUBSTEP);
+    _frameCount += (uint32_t)numSubSteps;
 }
 
 bool PhysicsEngine::addVoxel(const glm::vec3& position, float scale) {
@@ -250,7 +293,7 @@ bool PhysicsEngine::addObject(ObjectMotionState* motionState) {
         btVector3 inertia(0.0f, 0.0f, 0.0f);
         float mass = 0.0f;
         btRigidBody* body = NULL;
-        switch(motionState->getMotionType()) {
+        switch(motionState->computeMotionType()) {
             case MOTION_TYPE_KINEMATIC: {
                 body = new btRigidBody(mass, motionState, shape, inertia);
                 body->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
@@ -322,7 +365,7 @@ bool PhysicsEngine::updateObject(ObjectMotionState* motionState, uint32_t flags)
 
 // private
 void PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motionState, uint32_t flags) {
-    MotionType newType = motionState->getMotionType();
+    MotionType newType = motionState->computeMotionType();
 
     // pull body out of physics engine
     _dynamicsWorld->removeRigidBody(body);

libraries/physics/src/PhysicsEngine.h

@@ -112,6 +112,18 @@ public:
     /// \return true if entity updated
     bool updateObject(ObjectMotionState* motionState, uint32_t flags);
 
+    /// \return duration of fixed simulation substep
+    float getFixedSubStep() const;
+
+    /// \return number of simulation frames the physics engine has taken
+    uint32_t getFrameCount() const { return _frameCount; }
+
+    /// \return substep remainder used for Bullet MotionState extrapolation
+    // 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(); }
+
 protected:
     void updateObjectHard(btRigidBody* body, ObjectMotionState* motionState, uint32_t flags);
     void updateObjectEasy(btRigidBody* body, ObjectMotionState* motionState, uint32_t flags);
@@ -129,9 +141,11 @@ private:
     btHashMap<PositionHashKey, VoxelObject> _voxels;
 
     // EntitySimulation stuff
-    QSet<EntityItem*> _entities;
-    QSet<EntityItem*> _changedEntities;
-    QSet<EntityItem*> _mortalEntities;
+    QSet<EntityItem*> _entities; // all entities that we track
+    QSet<EntityItem*> _incomingPhysics; // entities with pending physics changes by script or packet
+    QSet<EntityItem*> _outgoingPhysics; // entites with pending transform changes by physics simulation
+
+    uint32_t _frameCount;
 };
 
 #else // USE_BULLET_PHYSICS

libraries/physics/src/ThreadSafeDynamicsWorld.h

@@ -38,6 +38,11 @@ public:
     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; }
+
 private:
     EntityTree* _entities;
 };