fix for glitchy physics updates

The main problem was: ObjectMotionState::_sentPosition and friends
were not being updated when the EntityServer update arrives.

libraries/entities/src/EntityItem.cpp

@@ -1000,7 +1000,7 @@ void EntityItem::recalculateCollisionShape() {
 const float MIN_POSITION_DELTA = 0.0001f;
 const float MIN_ALIGNMENT_DOT = 0.9999f;
 const float MIN_MASS_DELTA = 0.001f;
-const float MIN_VELOCITY_DELTA = 0.025f;
+const float MIN_VELOCITY_DELTA = 0.01f;
 const float MIN_DAMPING_DELTA = 0.001f;
 const float MIN_GRAVITY_DELTA = 0.001f;
 const float MIN_SPIN_DELTA = 0.0003f;

libraries/physics/src/EntityMotionState.cpp

@@ -79,21 +79,59 @@ void EntityMotionState::setWorldTransform(const btTransform& worldTrans) {
 }
 #endif // USE_BULLET_PHYSICS
 
-void EntityMotionState::applyVelocities() const {
+void EntityMotionState::updateObjectEasy(uint32_t flags, uint32_t frame) {
 #ifdef USE_BULLET_PHYSICS
-    if (_body) {
+    if (flags & (EntityItem::DIRTY_POSITION | EntityItem::DIRTY_VELOCITY)) {
-        setVelocity(_entity->getVelocityInMeters());
+        if (flags & EntityItem::DIRTY_POSITION) {
-        // DANGER! EntityItem stores angularVelocity in degrees/sec!!!
+            _sentPosition = _entity->getPositionInMeters() - ObjectMotionState::getWorldOffset();
-        setAngularVelocity(glm::radians(_entity->getAngularVelocity()));
+            btTransform worldTrans;
-        _body->setActivationState(ACTIVE_TAG);
+            worldTrans.setOrigin(glmToBullet(_sentPosition));
-    }
-#endif // USE_BULLET_PHYSICS
-}
 
-void EntityMotionState::applyGravity() const {
+            _sentRotation = _entity->getRotation();
+            worldTrans.setRotation(glmToBullet(_sentRotation));
+
+            _body->setWorldTransform(worldTrans);
+        }
+        if (flags & EntityItem::DIRTY_VELOCITY) {
+            updateObjectVelocities();
+        }
+        _sentFrame = frame;
+    }
+
+    // TODO: entity support for friction and restitution
+    //_restitution = _entity->getRestitution();
+    _body->setRestitution(_restitution);
+    //_friction = _entity->getFriction();
+    _body->setFriction(_friction);
+
+    _linearDamping = _entity->getDamping();
+    _angularDamping = _entity->getAngularDamping();
+    _body->setDamping(_linearDamping, _angularDamping);
+
+    if (flags & EntityItem::DIRTY_MASS) {
+        float mass = getMass();
+        btVector3 inertia(0.0f, 0.0f, 0.0f);
+        _body->getCollisionShape()->calculateLocalInertia(mass, inertia);
+        _body->setMassProps(mass, inertia);
+        _body->updateInertiaTensor();
+    }
+    _body->activate();
+#endif // USE_BULLET_PHYSICS
+};
+
+void EntityMotionState::updateObjectVelocities() {
 #ifdef USE_BULLET_PHYSICS
     if (_body) {
-        setGravity(_entity->getGravityInMeters());
+        _sentVelocity = _entity->getVelocityInMeters();
+        setVelocity(_sentVelocity);
+
+        // DANGER! EntityItem stores angularVelocity in degrees/sec!!!
+        _sentAngularVelocity = glm::radians(_entity->getAngularVelocity());
+        setAngularVelocity(_sentAngularVelocity);
+
+        _sentAcceleration = _entity->getGravityInMeters();
+        setGravity(_sentAcceleration);
+
         _body->setActivationState(ACTIVE_TAG);
     }
 #endif // USE_BULLET_PHYSICS
@@ -123,7 +161,7 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_
                 _sentAngularVelocity = bulletToGLM(_body->getAngularVelocity());
 
                 // if the speeds are very small we zero them out
-                const float MINIMUM_EXTRAPOLATION_SPEED_SQUARED = 4.0e-6f; // 2mm/sec
+                const float MINIMUM_EXTRAPOLATION_SPEED_SQUARED = 1.0e-4f; // 1cm/sec
                 bool zeroSpeed = (glm::length2(_sentVelocity) < MINIMUM_EXTRAPOLATION_SPEED_SQUARED);
                 if (zeroSpeed) {
                     _sentVelocity = glm::vec3(0.0f);

libraries/physics/src/EntityMotionState.h

@@ -54,8 +54,8 @@ public:
 #endif // USE_BULLET_PHYSICS
 
     // these relay incoming values to the RigidBody
-    void applyVelocities() const;
+    void updateObjectEasy(uint32_t flags, uint32_t frame);
-    void applyGravity() const;
+    void updateObjectVelocities();
 
     void computeShapeInfo(ShapeInfo& info);
 

libraries/physics/src/ObjectMotionState.cpp

@@ -123,9 +123,9 @@ bool ObjectMotionState::doesNotNeedToSendUpdate() const {
 
 const float FIXED_SUBSTEP = 1.0f / 60.0f;
 
-bool ObjectMotionState::shouldSendUpdate(uint32_t simulationFrame, float subStepRemainder) {
+bool ObjectMotionState::shouldSendUpdate(uint32_t simulationFrame) {
     assert(_body);
-    float dt = (float)(simulationFrame - _sentFrame) * FIXED_SUBSTEP + subStepRemainder;
+    float dt = (float)(simulationFrame - _sentFrame) * FIXED_SUBSTEP;
     _sentFrame = simulationFrame;
     bool isActive = _body->isActive();
 
@@ -183,7 +183,7 @@ bool ObjectMotionState::shouldSendUpdate(uint32_t simulationFrame, float subStep
     }
     const float MIN_ROTATION_DOT = 0.98f;
     glm::quat actualRotation = bulletToGLM(worldTrans.getRotation());
-    return (glm::dot(actualRotation, _sentRotation) < MIN_ROTATION_DOT);
+    return (fabsf(glm::dot(actualRotation, _sentRotation)) < MIN_ROTATION_DOT);
 }
 
 #endif // USE_BULLET_PHYSICS

libraries/physics/src/ObjectMotionState.h

@@ -56,8 +56,9 @@ public:
     ObjectMotionState();
     ~ObjectMotionState();
 
-    virtual void applyVelocities() const = 0;
+    // An EASY update does not require the object to be removed and then reinserted into the PhysicsEngine
-    virtual void applyGravity() const = 0;
+    virtual void updateObjectEasy(uint32_t flags, uint32_t frame) = 0;
+    virtual void updateObjectVelocities() = 0;
 
     virtual void computeShapeInfo(ShapeInfo& info) = 0;
 
@@ -84,7 +85,7 @@ public:
     void clearOutgoingPacketFlags(uint32_t flags) { _outgoingPacketFlags &= ~flags; }
 
     bool doesNotNeedToSendUpdate() const;
-    virtual bool shouldSendUpdate(uint32_t simulationFrame, float subStepRemainder);
+    virtual bool shouldSendUpdate(uint32_t simulationFrame);
     virtual void sendUpdate(OctreeEditPacketSender* packetSender, uint32_t frame) = 0;
 
     virtual MotionType computeMotionType() const = 0;

libraries/physics/src/PhysicsEngine.cpp

@@ -41,14 +41,12 @@ void PhysicsEngine::updateEntitiesInternal(const quint64& now) {
 
     // this is step (4)
     QSet<ObjectMotionState*>::iterator stateItr = _outgoingPackets.begin();
-    uint32_t frame = getFrameCount();
-    float subStepRemainder = getSubStepRemainder();
     while (stateItr != _outgoingPackets.end()) {
         ObjectMotionState* state = *stateItr;
         if (state->doesNotNeedToSendUpdate()) {
             stateItr = _outgoingPackets.erase(stateItr);
-        } else if (state->shouldSendUpdate(frame, subStepRemainder)) {
+        } else if (state->shouldSendUpdate(_frameCount)) {
-            state->sendUpdate(_entityPacketSender, frame);
+            state->sendUpdate(_entityPacketSender, _frameCount);
             ++stateItr;
         } else {
             ++stateItr;
@@ -140,7 +138,8 @@ void PhysicsEngine::relayIncomingChangesToSimulation() {
                 updateObjectHard(body, motionState, flags);
             } else if (flags) {
                 // an EASY update does NOT require that the body be pulled out of physics engine
-                updateObjectEasy(body, motionState, flags);
+                // hence the MotionState has all the knowledge and authority to perform the update.
+                motionState->updateObjectEasy(flags, _frameCount);
             }
         }
 
@@ -269,8 +268,12 @@ bool PhysicsEngine::addObject(ObjectMotionState* motionState) {
                 body = new btRigidBody(mass, motionState, shape, inertia);
                 body->updateInertiaTensor();
                 motionState->_body = body;
-                motionState->applyVelocities();
+                motionState->updateObjectVelocities();
-                motionState->applyGravity();
+                // NOTE: Bullet will deactivate any object whose velocity is below these thresholds for longer than 2 seconds.
+                // (the 2 seconds is determined by: static btRigidBody::gDeactivationTime
+                const float LINEAR_VELOCITY_THRESHOLD = 0.05f;  // 5 cm/sec
+                const float ANGULAR_VELOCITY_THRESHOLD = 0.087266f;  // ~5 deg/sec
+                body->setSleepingThresholds(LINEAR_VELOCITY_THRESHOLD, ANGULAR_VELOCITY_THRESHOLD);
                 break;
             }
             case MOTION_TYPE_STATIC:
@@ -334,7 +337,7 @@ void PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motio
     }
     bool easyUpdate = flags & EASY_DIRTY_PHYSICS_FLAGS;
     if (easyUpdate) {
-        updateObjectEasy(body, motionState, flags);
+        motionState->updateObjectEasy(flags, _frameCount);
     }
 
     // update the motion parameters
@@ -385,31 +388,4 @@ void PhysicsEngine::updateObjectHard(btRigidBody* body, ObjectMotionState* motio
     body->activate();
 }
 
-// private
-void PhysicsEngine::updateObjectEasy(btRigidBody* body, ObjectMotionState* motionState, uint32_t flags) {
-    if (flags & EntityItem::DIRTY_POSITION) {
-        btTransform transform;
-        motionState->getWorldTransform(transform);
-        body->setWorldTransform(transform);
-    }
-    if (flags & EntityItem::DIRTY_VELOCITY) {
-        motionState->applyVelocities();
-        motionState->applyGravity();
-    }
-    body->setRestitution(motionState->_restitution);
-    body->setFriction(motionState->_friction);
-    body->setDamping(motionState->_linearDamping, motionState->_angularDamping);
-
-    if (flags & EntityItem::DIRTY_MASS) {
-        float mass = motionState->getMass();
-        btVector3 inertia(0.0f, 0.0f, 0.0f);
-        body->getCollisionShape()->calculateLocalInertia(mass, inertia);
-        body->setMassProps(mass, inertia);
-        body->updateInertiaTensor();
-    }
-    body->activate();
-
-    // TODO: support collision groups
-};
-
 #endif // USE_BULLET_PHYSICS

libraries/physics/src/PhysicsEngine.h

@@ -71,12 +71,6 @@ public:
     /// \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() { return _dynamicsWorld->getLocalTimeAccumulation(); }
-
 protected:
     void updateObjectHard(btRigidBody* body, ObjectMotionState* motionState, uint32_t flags);
     void updateObjectEasy(btRigidBody* body, ObjectMotionState* motionState, uint32_t flags);