From 771727890cb8af4e7a66fe006f7b3344135dd1b9 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 12 Apr 2016 09:56:01 -0700
Subject: [PATCH 01/14] more finely grained Bullet profiling

---
 libraries/physics/src/PhysicsEngine.cpp       |  1 +
 .../physics/src/ThreadSafeDynamicsWorld.cpp   | 26 ++++++++++++++++++-
 .../physics/src/ThreadSafeDynamicsWorld.h     |  1 +
 3 files changed, 27 insertions(+), 1 deletion(-)

diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp
index 54419a9c1a..8edbe67431 100644
--- a/libraries/physics/src/PhysicsEngine.cpp
+++ b/libraries/physics/src/PhysicsEngine.cpp
@@ -252,6 +252,7 @@ void PhysicsEngine::stepSimulation() {
     float timeStep = btMin(dt, MAX_TIMESTEP);
 
     if (_myAvatarController) {
+        BT_PROFILE("avatarController");
         // TODO: move this stuff outside and in front of stepSimulation, because
         // the updateShapeIfNecessary() call needs info from MyAvatar and should
         // be done on the main thread during the pre-simulation stuff
diff --git a/libraries/physics/src/ThreadSafeDynamicsWorld.cpp b/libraries/physics/src/ThreadSafeDynamicsWorld.cpp
index b6f3487f1a..5fe99f137c 100644
--- a/libraries/physics/src/ThreadSafeDynamicsWorld.cpp
+++ b/libraries/physics/src/ThreadSafeDynamicsWorld.cpp
@@ -67,7 +67,10 @@ int ThreadSafeDynamicsWorld::stepSimulationWithSubstepCallback(btScalar timeStep
 
         saveKinematicState(fixedTimeStep*clampedSimulationSteps);
 
-        applyGravity();
+        {
+            BT_PROFILE("applyGravity");
+            applyGravity();
+        }
 
         for (int i=0;i<clampedSimulationSteps;i++) {
             internalSingleStepSimulation(fixedTimeStep);
@@ -143,3 +146,24 @@ void ThreadSafeDynamicsWorld::synchronizeMotionStates() {
     }
 }
 
+void ThreadSafeDynamicsWorld::saveKinematicState(btScalar timeStep) {
+///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows
+///to switch status _after_ adding kinematic objects to the world
+///fix it for Bullet 3.x release
+    BT_PROFILE("saveKinematicState");
+    for (int i=0;i<m_collisionObjects.size();i++)
+    {
+        btCollisionObject* colObj = m_collisionObjects[i];
+        btRigidBody* body = btRigidBody::upcast(colObj);
+        if (body && body->getActivationState() != ISLAND_SLEEPING)
+        {
+            if (body->isKinematicObject())
+            {
+                //to calculate velocities next frame
+                body->saveKinematicState(timeStep);
+            }
+        }
+    }
+}
+
+
diff --git a/libraries/physics/src/ThreadSafeDynamicsWorld.h b/libraries/physics/src/ThreadSafeDynamicsWorld.h
index e9708149da..68062d8d29 100644
--- a/libraries/physics/src/ThreadSafeDynamicsWorld.h
+++ b/libraries/physics/src/ThreadSafeDynamicsWorld.h
@@ -41,6 +41,7 @@ public:
                                           btScalar fixedTimeStep = btScalar(1.)/btScalar(60.),
                                           SubStepCallback onSubStep = []() { });
     virtual void synchronizeMotionStates() override;
+    virtual void saveKinematicState(btScalar timeStep) override;
 
     // 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

From 905c5398c425af9172729b571ed2e4e2f7192ee7 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 12 Apr 2016 10:03:46 -0700
Subject: [PATCH 02/14] add EntityItem::set/getTransformAndVelocities()

---
 libraries/shared/src/SpatiallyNestable.cpp | 50 +++++++++++++++++-----
 libraries/shared/src/SpatiallyNestable.h   |  7 +++
 2 files changed, 47 insertions(+), 10 deletions(-)

diff --git a/libraries/shared/src/SpatiallyNestable.cpp b/libraries/shared/src/SpatiallyNestable.cpp
index c4cb4f94ba..6e66ac5bcb 100644
--- a/libraries/shared/src/SpatiallyNestable.cpp
+++ b/libraries/shared/src/SpatiallyNestable.cpp
@@ -90,11 +90,9 @@ SpatiallyNestablePointer SpatiallyNestable::getParentPointer(bool& success) cons
         return parent;
     }
 
-    SpatiallyNestablePointer thisPointer = getThisPointer();
-
     if (parent) {
         // we have a parent pointer but our _parentID doesn't indicate this parent.
-        parent->forgetChild(thisPointer);
+        parent->forgetChild(getThisPointer());
         _parentKnowsMe = false;
         _parent.reset();
     }
@@ -112,16 +110,11 @@ SpatiallyNestablePointer SpatiallyNestable::getParentPointer(bool& success) cons
 
     parent = _parent.lock();
     if (parent) {
-        parent->beParentOfChild(thisPointer);
+        parent->beParentOfChild(getThisPointer());
         _parentKnowsMe = true;
     }
 
-    if (parent || parentID.isNull()) {
-        success = true;
-    } else {
-        success = false;
-    }
-
+    success = (parent || parentID.isNull());
     return parent;
 }
 
@@ -873,3 +866,40 @@ bool SpatiallyNestable::hasAncestorOfType(NestableType nestableType) {
 
     return parent->hasAncestorOfType(nestableType);
 }
+
+void SpatiallyNestable::getLocalTransformAndVelocities(
+        Transform& transform,
+        glm::vec3& velocity,
+        glm::vec3& angularVelocity) const {
+    // transform
+    _transformLock.withReadLock([&] {
+        transform = _transform;
+    });
+    // linear velocity
+    _velocityLock.withReadLock([&] {
+        velocity = _velocity;
+    });
+    // angular velocity
+    _angularVelocityLock.withReadLock([&] {
+        angularVelocity = _angularVelocity;
+    });
+}
+
+void SpatiallyNestable::setLocalTransformAndVelocities(
+        const Transform& localTransform,
+        const glm::vec3& localVelocity,
+        const glm::vec3& localAngularVelocity) {
+    // transform
+    _transformLock.withWriteLock([&] {
+        _transform = localTransform;
+    });
+    // linear velocity
+    _velocityLock.withWriteLock([&] {
+        _velocity = localVelocity;
+    });
+    // angular velocity
+    _angularVelocityLock.withWriteLock([&] {
+        _angularVelocity = localAngularVelocity;
+    });
+    locationChanged();
+}
diff --git a/libraries/shared/src/SpatiallyNestable.h b/libraries/shared/src/SpatiallyNestable.h
index 6485c23b87..04bb57a688 100644
--- a/libraries/shared/src/SpatiallyNestable.h
+++ b/libraries/shared/src/SpatiallyNestable.h
@@ -151,6 +151,13 @@ protected:
     quint16 _parentJointIndex { 0 }; // which joint of the parent is this relative to?
     SpatiallyNestablePointer getParentPointer(bool& success) const;
 
+    void getLocalTransformAndVelocities(Transform& localTransform, glm::vec3& localVelocity, glm::vec3& localAngularVelocity) const;
+
+    void setLocalTransformAndVelocities(
+            const Transform& localTransform,
+            const glm::vec3& localVelocity,
+            const glm::vec3& localAngularVelocity);
+
     mutable SpatiallyNestableWeakPointer _parent;
 
     virtual void beParentOfChild(SpatiallyNestablePointer newChild) const;

From 3639ffe53e13bb2aad6e5b60472b37565e7cf17e Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Mon, 18 Apr 2016 11:39:06 -0700
Subject: [PATCH 03/14] optimize kinematic motion math

---
 libraries/entities/src/EntityItem.cpp       | 164 ++++++++------------
 libraries/entities/src/EntityItem.h         |   2 +-
 libraries/physics/src/EntityMotionState.cpp |   2 +-
 libraries/shared/src/SpatiallyNestable.cpp  |   2 +-
 4 files changed, 72 insertions(+), 98 deletions(-)

diff --git a/libraries/entities/src/EntityItem.cpp b/libraries/entities/src/EntityItem.cpp
index 65811c5c57..06afa2817c 100644
--- a/libraries/entities/src/EntityItem.cpp
+++ b/libraries/entities/src/EntityItem.cpp
@@ -731,7 +731,7 @@ int EntityItem::readEntityDataFromBuffer(const unsigned char* data, int bytesLef
 
         // we want to extrapolate the motion forward to compensate for packet travel time, but
         // we don't want the side effect of flag setting.
-        simulateKinematicMotion(skipTimeForward, false);
+        stepKinematicMotion(skipTimeForward);
     }
 
     if (overwriteLocalData) {
@@ -872,130 +872,104 @@ void EntityItem::simulate(const quint64& now) {
         qCDebug(entities) << "     ********** EntityItem::simulate() .... SETTING _lastSimulated=" << _lastSimulated;
     #endif
 
-    simulateKinematicMotion(timeElapsed);
+    if (!hasActions()) {
+        if (!stepKinematicMotion(timeElapsed)) {
+            // this entity is no longer moving
+            // flag it to transition from KINEMATIC to STATIC
+            _dirtyFlags |= Simulation::DIRTY_MOTION_TYPE;
+        }
+    }
     _lastSimulated = now;
 }
 
-void EntityItem::simulateKinematicMotion(float timeElapsed, bool setFlags) {
-#ifdef WANT_DEBUG
-    qCDebug(entities) << "EntityItem::simulateKinematicMotion timeElapsed" << timeElapsed;
-#endif
-
-    const float MIN_TIME_SKIP = 0.0f;
-    const float MAX_TIME_SKIP = 1.0f; // in seconds
-
-    timeElapsed = glm::clamp(timeElapsed, MIN_TIME_SKIP, MAX_TIME_SKIP);
-
-    if (hasActions()) {
-        return;
+bool EntityItem::stepKinematicMotion(float timeElapsed) {
+    if (timeElapsed < 0.0f) {
+        return true;
     }
 
-    if (hasLocalAngularVelocity()) {
-        glm::vec3 localAngularVelocity = getLocalAngularVelocity();
+    const float MAX_TIME_ELAPSED = 1.0f; // seconds
+    timeElapsed = glm::min(timeElapsed, MAX_TIME_ELAPSED);
 
+    Transform transform;
+    glm::vec3 linearVelocity;
+    glm::vec3 angularVelocity;
+    getLocalTransformAndVelocities(transform, linearVelocity, angularVelocity);
+
+    bool moving = false;
+    if (glm::length2(angularVelocity) > 0.0f) {
         // angular damping
         if (_angularDamping > 0.0f) {
-            localAngularVelocity *= powf(1.0f - _angularDamping, timeElapsed);
-            #ifdef WANT_DEBUG
-                qCDebug(entities) << "    angularDamping :" << _angularDamping;
-                qCDebug(entities) << "    newAngularVelocity:" << localAngularVelocity;
-            #endif
+            angularVelocity *= powf(1.0f - _angularDamping, timeElapsed);
         }
 
-        float angularSpeed = glm::length(localAngularVelocity);
-
-        const float EPSILON_ANGULAR_VELOCITY_LENGTH = 0.0017453f; // 0.0017453 rad/sec = 0.1f degrees/sec
-        if (angularSpeed < EPSILON_ANGULAR_VELOCITY_LENGTH) {
-            if (setFlags && angularSpeed > 0.0f) {
-                _dirtyFlags |= Simulation::DIRTY_MOTION_TYPE;
-            }
-            localAngularVelocity = ENTITY_ITEM_ZERO_VEC3;
+        const float MIN_KINEMATIC_ANGULAR_SPEED_SQUARED = 0.0017453f * 0.0017453f; // 0.0017453 rad/sec = 0.1f degrees/sec
+        if (glm::length2(angularVelocity) < MIN_KINEMATIC_ANGULAR_SPEED_SQUARED) {
+            angularVelocity = Vectors::ZERO;
         } else {
             // for improved agreement with the way Bullet integrates rotations we use an approximation
             // and break the integration into bullet-sized substeps
-            glm::quat rotation = getRotation();
+            glm::quat rotation = transform.getRotation();
             float dt = timeElapsed;
-            while (dt > PHYSICS_ENGINE_FIXED_SUBSTEP) {
-                glm::quat  dQ = computeBulletRotationStep(localAngularVelocity, PHYSICS_ENGINE_FIXED_SUBSTEP);
+            while (dt > 0.0f) {
+                glm::quat  dQ = computeBulletRotationStep(angularVelocity, glm::min(dt, PHYSICS_ENGINE_FIXED_SUBSTEP));
                 rotation = glm::normalize(dQ * rotation);
                 dt -= PHYSICS_ENGINE_FIXED_SUBSTEP;
             }
-            // NOTE: this final partial substep can drift away from a real Bullet simulation however
-            // it only becomes significant for rapidly rotating objects
-            // (e.g. around PI/4 radians per substep, or 7.5 rotations/sec at 60 substeps/sec).
-            glm::quat  dQ = computeBulletRotationStep(localAngularVelocity, dt);
-            rotation = glm::normalize(dQ * rotation);
-
-            bool success;
-            setOrientation(rotation, success, false);
+            transform.setRotation(rotation);
         }
-
-        setLocalAngularVelocity(localAngularVelocity);
+        moving = true;
     }
 
-    if (hasLocalVelocity()) {
-
-        // acceleration is in the global frame, so transform it into the local frame.
-        // TODO: Move this into SpatiallyNestable.
-        bool success;
-        Transform transform = getParentTransform(success);
-        glm::vec3 localAcceleration(glm::vec3::_null);
-        if (success) {
-            localAcceleration = glm::inverse(transform.getRotation()) * getAcceleration();
-        } else {
-            localAcceleration = getAcceleration();
-        }
+    glm::vec3 position = transform.getTranslation();
+    float linearSpeedSquared = glm::length2(linearVelocity);
+    if (linearSpeedSquared > 0.0f) {
+        glm::vec3 deltaVelocity = Vectors::ZERO;
 
         // linear damping
-        glm::vec3 localVelocity = getLocalVelocity();
         if (_damping > 0.0f) {
-            localVelocity *= powf(1.0f - _damping, timeElapsed);
-            #ifdef WANT_DEBUG
-                qCDebug(entities) << "    damping:" << _damping;
-                qCDebug(entities) << "    velocity AFTER dampingResistance:" << localVelocity;
-                qCDebug(entities) << "    glm::length(velocity):" << glm::length(localVelocity);
-            #endif
+            deltaVelocity = (powf(1.0f - _damping, timeElapsed) - 1.0f) * linearVelocity;
         }
 
-        // integrate position forward
-        glm::vec3 localPosition = getLocalPosition();
-        glm::vec3 newLocalPosition = localPosition + (localVelocity * timeElapsed) + 0.5f * localAcceleration * timeElapsed * timeElapsed;
+        const float MIN_KINEMATIC_LINEAR_ACCELERATION_SQUARED = 1.0e-4f; // 0.01 m/sec^2
+        const float MIN_KINEMATIC_LINEAR_SPEED_SQUARED = 1.0e-6f; // 0.001 m/sec^2
+        if (glm::length2(_gravity) > MIN_KINEMATIC_LINEAR_ACCELERATION_SQUARED) {
+            // yes gravity
+            // acceleration is in world-frame but we need it in local-frame
+            glm::vec3 linearAcceleration = _gravity;
+            bool success;
+            Transform parentTransform = getParentTransform(success);
+            if (success) {
+                linearAcceleration = glm::inverse(parentTransform.getRotation()) * linearAcceleration;
+            }
+            deltaVelocity += linearAcceleration * timeElapsed;
 
-        #ifdef WANT_DEBUG
-            qCDebug(entities) << "  EntityItem::simulate()....";
-            qCDebug(entities) << "    timeElapsed:" << timeElapsed;
-            qCDebug(entities) << "    old AACube:" << getMaximumAACube();
-            qCDebug(entities) << "    old position:" << localPosition;
-            qCDebug(entities) << "    old velocity:" << localVelocity;
-            qCDebug(entities) << "    old getAABox:" << getAABox();
-            qCDebug(entities) << "    newPosition:" << newPosition;
-            qCDebug(entities) << "    glm::distance(newPosition, position):" << glm::distance(newLocalPosition, localPosition);
-        #endif
-
-        localPosition = newLocalPosition;
-
-        // apply effective acceleration, which will be the same as gravity if the Entity isn't at rest.
-        localVelocity += localAcceleration * timeElapsed;
-
-        float speed = glm::length(localVelocity);
-        const float EPSILON_LINEAR_VELOCITY_LENGTH = 0.001f; // 1mm/sec
-        if (speed < EPSILON_LINEAR_VELOCITY_LENGTH) {
-            setVelocity(ENTITY_ITEM_ZERO_VEC3);
-            if (setFlags && speed > 0.0f) {
-                _dirtyFlags |= Simulation::DIRTY_MOTION_TYPE;
+            if (glm::length2(deltaVelocity) < MIN_KINEMATIC_LINEAR_SPEED_SQUARED
+                    && glm::length2(linearVelocity) < MIN_KINEMATIC_LINEAR_SPEED_SQUARED) {
+                linearVelocity = Vectors::ZERO;
+            } else {
+                // position's acceleration term uses deltaVelocity rather than raw gravity
+                // for more accuracy (includes damping effects)
+                position += timeElapsed * (linearVelocity + 0.5f * deltaVelocity);
+                linearVelocity += deltaVelocity;
             }
         } else {
-            setLocalPosition(localPosition);
-            setLocalVelocity(localVelocity);
+            // no gravity
+            if (linearSpeedSquared < MIN_KINEMATIC_LINEAR_SPEED_SQUARED) {
+                linearVelocity = Vectors::ZERO;
+            } else {
+                // NOTE: don't use acceleration term for linear displacement
+                position += timeElapsed * linearVelocity;
+                linearVelocity += deltaVelocity;
+            }
         }
-
-        #ifdef WANT_DEBUG
-            qCDebug(entities) << "    new position:" << position;
-            qCDebug(entities) << "    new velocity:" << velocity;
-            qCDebug(entities) << "    new AACube:" << getMaximumAACube();
-            qCDebug(entities) << "    old getAABox:" << getAABox();
-        #endif
+        moving = true;
     }
+
+    if (moving) {
+        transform.setTranslation(position);
+        setLocalTransformAndVelocities(transform, linearVelocity, angularVelocity);
+    }
+    return moving;
 }
 
 bool EntityItem::isMoving() const {
diff --git a/libraries/entities/src/EntityItem.h b/libraries/entities/src/EntityItem.h
index 08550c9ce5..c145334f80 100644
--- a/libraries/entities/src/EntityItem.h
+++ b/libraries/entities/src/EntityItem.h
@@ -152,7 +152,7 @@ public:
 
     // perform linear extrapolation for SimpleEntitySimulation
     void simulate(const quint64& now);
-    void simulateKinematicMotion(float timeElapsed, bool setFlags=true);
+    bool stepKinematicMotion(float timeElapsed); // return 'true' if moving
 
     virtual bool needsToCallUpdate() const { return false; }
 
diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp
index cd6c54675d..8ef4d7ac4b 100644
--- a/libraries/physics/src/EntityMotionState.cpp
+++ b/libraries/physics/src/EntityMotionState.cpp
@@ -187,7 +187,7 @@ void EntityMotionState::getWorldTransform(btTransform& worldTrans) const {
         // of the physics simulation.
         uint32_t thisStep = ObjectMotionState::getWorldSimulationStep();
         float dt = (thisStep - _lastKinematicStep) * PHYSICS_ENGINE_FIXED_SUBSTEP;
-        _entity->simulateKinematicMotion(dt);
+        _entity->stepKinematicMotion(dt);
 
         // bypass const-ness so we can remember the step
         const_cast<EntityMotionState*>(this)->_lastKinematicStep = thisStep;
diff --git a/libraries/shared/src/SpatiallyNestable.cpp b/libraries/shared/src/SpatiallyNestable.cpp
index 6e66ac5bcb..f0f130422c 100644
--- a/libraries/shared/src/SpatiallyNestable.cpp
+++ b/libraries/shared/src/SpatiallyNestable.cpp
@@ -419,7 +419,7 @@ void SpatiallyNestable::setVelocity(const glm::vec3& velocity, bool& success) {
         // _velocity is a vs parent value and any request for a world-frame velocity must
         // be computed), do this to avoid equipped (parenting-grabbed) things from drifting.
         // turning a zero velocity into a non-zero _velocity (because the avatar is moving)
-        // causes EntityItem::simulateKinematicMotion to have an effect on the equipped entity,
+        // causes EntityItem::stepKinematicMotion to have an effect on the equipped entity,
         // which causes it to drift from the hand.
         if (hasAncestorOfType(NestableType::Avatar)) {
             _velocity = velocity;

From c8f0b0501a76b7293c3435e1418b2cc6529eef89 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Mon, 18 Apr 2016 13:26:28 -0700
Subject: [PATCH 04/14] avoid active object update cycles

---
 libraries/shared/src/SpatiallyNestable.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/libraries/shared/src/SpatiallyNestable.cpp b/libraries/shared/src/SpatiallyNestable.cpp
index f0f130422c..2a3cb4af47 100644
--- a/libraries/shared/src/SpatiallyNestable.cpp
+++ b/libraries/shared/src/SpatiallyNestable.cpp
@@ -901,5 +901,5 @@ void SpatiallyNestable::setLocalTransformAndVelocities(
     _angularVelocityLock.withWriteLock([&] {
         _angularVelocity = localAngularVelocity;
     });
-    locationChanged();
+    locationChanged(false);
 }

From 8cbec06616186f5507d5434ec22eb0827a345b0e Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Mon, 18 Apr 2016 13:54:26 -0700
Subject: [PATCH 05/14] fix bug where kinematic objs pop on first step

---
 libraries/physics/src/ObjectMotionState.cpp | 7 +++++++
 libraries/physics/src/ObjectMotionState.h   | 2 ++
 libraries/physics/src/PhysicsEngine.cpp     | 1 +
 3 files changed, 10 insertions(+)

diff --git a/libraries/physics/src/ObjectMotionState.cpp b/libraries/physics/src/ObjectMotionState.cpp
index 0468e02cb8..de435e80da 100644
--- a/libraries/physics/src/ObjectMotionState.cpp
+++ b/libraries/physics/src/ObjectMotionState.cpp
@@ -45,6 +45,7 @@ void ObjectMotionState::setWorldSimulationStep(uint32_t step) {
     worldSimulationStep = step;
 }
 
+// static
 uint32_t ObjectMotionState::getWorldSimulationStep() {
     return worldSimulationStep;
 }
@@ -298,6 +299,12 @@ void ObjectMotionState::updateBodyVelocities() {
     _body->setActivationState(ACTIVE_TAG);
 }
 
+void ObjectMotionState::updateLastKinematicStep() {
+    // NOTE: we init to worldSimulationStep - 1 so that: when any object transitions to kinematic
+    // it will compute a non-zero dt on its first step.
+    _lastKinematicStep = ObjectMotionState::getWorldSimulationStep() - 1;
+}
+
 void ObjectMotionState::updateBodyMassProperties() {
     float mass = getMass();
     btVector3 inertia(0.0f, 0.0f, 0.0f);
diff --git a/libraries/physics/src/ObjectMotionState.h b/libraries/physics/src/ObjectMotionState.h
index 4293df6b3f..c0c10c6f71 100644
--- a/libraries/physics/src/ObjectMotionState.h
+++ b/libraries/physics/src/ObjectMotionState.h
@@ -86,6 +86,8 @@ public:
 
     void updateBodyMaterialProperties();
     void updateBodyVelocities();
+    void updateLastKinematicStep();
+
     virtual void updateBodyMassProperties();
 
     MotionStateType getType() const { return _type; }
diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp
index 8edbe67431..73d95f0c7f 100644
--- a/libraries/physics/src/PhysicsEngine.cpp
+++ b/libraries/physics/src/PhysicsEngine.cpp
@@ -86,6 +86,7 @@ void PhysicsEngine::addObjectToDynamicsWorld(ObjectMotionState* motionState) {
             body->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
             body->updateInertiaTensor();
             motionState->updateBodyVelocities();
+            motionState->updateLastKinematicStep();
             const float KINEMATIC_LINEAR_VELOCITY_THRESHOLD = 0.01f;  // 1 cm/sec
             const float KINEMATIC_ANGULAR_VELOCITY_THRESHOLD = 0.01f;  // ~1 deg/sec
             body->setSleepingThresholds(KINEMATIC_LINEAR_VELOCITY_THRESHOLD, KINEMATIC_ANGULAR_VELOCITY_THRESHOLD);

From ad045bc439ba950a15582457f6284bce9bb31b44 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Mon, 18 Apr 2016 14:19:44 -0700
Subject: [PATCH 06/14] remove second-order term from kinematic motion

---
 libraries/entities/src/EntityItem.cpp       | 7 ++++---
 libraries/physics/src/EntityMotionState.cpp | 2 ++
 2 files changed, 6 insertions(+), 3 deletions(-)

diff --git a/libraries/entities/src/EntityItem.cpp b/libraries/entities/src/EntityItem.cpp
index 06afa2817c..c1ea461d2e 100644
--- a/libraries/entities/src/EntityItem.cpp
+++ b/libraries/entities/src/EntityItem.cpp
@@ -947,9 +947,10 @@ bool EntityItem::stepKinematicMotion(float timeElapsed) {
                     && glm::length2(linearVelocity) < MIN_KINEMATIC_LINEAR_SPEED_SQUARED) {
                 linearVelocity = Vectors::ZERO;
             } else {
-                // position's acceleration term uses deltaVelocity rather than raw gravity
-                // for more accuracy (includes damping effects)
-                position += timeElapsed * (linearVelocity + 0.5f * deltaVelocity);
+                // NOTE: we do NOT include the second-order acceleration term (0.5 * a * dt^2)
+                // when computing the displacement because Bullet also ignores that term.  Yes,
+                // this is an approximation and it works best when dt is small.
+                position += timeElapsed * linearVelocity;
                 linearVelocity += deltaVelocity;
             }
         } else {
diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp
index 8ef4d7ac4b..f4e6b796e4 100644
--- a/libraries/physics/src/EntityMotionState.cpp
+++ b/libraries/physics/src/EntityMotionState.cpp
@@ -309,6 +309,8 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) {
     if (glm::length2(_serverVelocity) > 0.0f) {
         _serverVelocity += _serverAcceleration * dt;
         _serverVelocity *= powf(1.0f - _body->getLinearDamping(), dt);
+        // NOTE: we ignore the second-order acceleration term when integrating
+        // the position forward because Bullet also does this.
         _serverPosition += dt * _serverVelocity;
     }
 

From afcf6d68a79d665be4c3e6762900200d62c7ae5e Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 19 Apr 2016 11:15:09 -0700
Subject: [PATCH 07/14] move deactivation thresholds to shared header

---
 libraries/physics/src/PhysicsEngine.cpp | 8 ++------
 libraries/shared/src/PhysicsHelpers.h   | 5 +++++
 2 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/libraries/physics/src/PhysicsEngine.cpp b/libraries/physics/src/PhysicsEngine.cpp
index 73d95f0c7f..5523abf4e2 100644
--- a/libraries/physics/src/PhysicsEngine.cpp
+++ b/libraries/physics/src/PhysicsEngine.cpp
@@ -87,9 +87,7 @@ void PhysicsEngine::addObjectToDynamicsWorld(ObjectMotionState* motionState) {
             body->updateInertiaTensor();
             motionState->updateBodyVelocities();
             motionState->updateLastKinematicStep();
-            const float KINEMATIC_LINEAR_VELOCITY_THRESHOLD = 0.01f;  // 1 cm/sec
-            const float KINEMATIC_ANGULAR_VELOCITY_THRESHOLD = 0.01f;  // ~1 deg/sec
-            body->setSleepingThresholds(KINEMATIC_LINEAR_VELOCITY_THRESHOLD, KINEMATIC_ANGULAR_VELOCITY_THRESHOLD);
+            body->setSleepingThresholds(KINEMATIC_LINEAR_SPEED_THRESHOLD, KINEMATIC_ANGULAR_SPEED_THRESHOLD);
             break;
         }
         case MOTION_TYPE_DYNAMIC: {
@@ -110,9 +108,7 @@ void PhysicsEngine::addObjectToDynamicsWorld(ObjectMotionState* motionState) {
 
             // 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 DYNAMIC_LINEAR_VELOCITY_THRESHOLD = 0.05f;  // 5 cm/sec
-            const float DYNAMIC_ANGULAR_VELOCITY_THRESHOLD = 0.087266f;  // ~5 deg/sec
-            body->setSleepingThresholds(DYNAMIC_LINEAR_VELOCITY_THRESHOLD, DYNAMIC_ANGULAR_VELOCITY_THRESHOLD);
+            body->setSleepingThresholds(DYNAMIC_LINEAR_SPEED_THRESHOLD, DYNAMIC_ANGULAR_SPEED_THRESHOLD);
             if (!motionState->isMoving()) {
                 // try to initialize this object as inactive
                 body->forceActivationState(ISLAND_SLEEPING);
diff --git a/libraries/shared/src/PhysicsHelpers.h b/libraries/shared/src/PhysicsHelpers.h
index 28ded661f0..67442187a3 100644
--- a/libraries/shared/src/PhysicsHelpers.h
+++ b/libraries/shared/src/PhysicsHelpers.h
@@ -22,6 +22,11 @@
 const int PHYSICS_ENGINE_MAX_NUM_SUBSTEPS = 6; // Bullet will start to "lose time" at 10 FPS.
 const float PHYSICS_ENGINE_FIXED_SUBSTEP = 1.0f / 90.0f;
 
+const float DYNAMIC_LINEAR_SPEED_THRESHOLD = 0.05f;  // 5 cm/sec
+const float DYNAMIC_ANGULAR_SPEED_THRESHOLD = 0.087266f;  // ~5 deg/sec
+const float KINEMATIC_LINEAR_SPEED_THRESHOLD = 0.001f;  // 1 mm/sec
+const float KINEMATIC_ANGULAR_SPEED_THRESHOLD = 0.008f;  // ~0.5 deg/sec
+
 // return incremental rotation (Bullet-style) caused by angularVelocity over timeStep
 glm::quat computeBulletRotationStep(const glm::vec3& angularVelocity, float timeStep);
 

From a79f49a5cde125868adb5196752165cadcf53c7f Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 19 Apr 2016 11:26:26 -0700
Subject: [PATCH 08/14] don't stop slow kinematic objs when sending updates

---
 libraries/entities/src/EntityItem.cpp       | 16 ++++++++++------
 libraries/physics/src/EntityMotionState.cpp |  8 ++++++--
 2 files changed, 16 insertions(+), 8 deletions(-)

diff --git a/libraries/entities/src/EntityItem.cpp b/libraries/entities/src/EntityItem.cpp
index c1ea461d2e..4ea14a522d 100644
--- a/libraries/entities/src/EntityItem.cpp
+++ b/libraries/entities/src/EntityItem.cpp
@@ -883,7 +883,7 @@ void EntityItem::simulate(const quint64& now) {
 }
 
 bool EntityItem::stepKinematicMotion(float timeElapsed) {
-    if (timeElapsed < 0.0f) {
+    if (timeElapsed <= 0.0f) {
         return true;
     }
 
@@ -902,7 +902,8 @@ bool EntityItem::stepKinematicMotion(float timeElapsed) {
             angularVelocity *= powf(1.0f - _angularDamping, timeElapsed);
         }
 
-        const float MIN_KINEMATIC_ANGULAR_SPEED_SQUARED = 0.0017453f * 0.0017453f; // 0.0017453 rad/sec = 0.1f degrees/sec
+        const float MIN_KINEMATIC_ANGULAR_SPEED_SQUARED =
+            KINEMATIC_ANGULAR_SPEED_THRESHOLD * KINEMATIC_ANGULAR_SPEED_THRESHOLD;
         if (glm::length2(angularVelocity) < MIN_KINEMATIC_ANGULAR_SPEED_SQUARED) {
             angularVelocity = Vectors::ZERO;
         } else {
@@ -922,6 +923,8 @@ bool EntityItem::stepKinematicMotion(float timeElapsed) {
 
     glm::vec3 position = transform.getTranslation();
     float linearSpeedSquared = glm::length2(linearVelocity);
+    const float MIN_KINEMATIC_LINEAR_SPEED_SQUARED =
+        KINEMATIC_LINEAR_SPEED_THRESHOLD * KINEMATIC_LINEAR_SPEED_THRESHOLD;
     if (linearSpeedSquared > 0.0f) {
         glm::vec3 deltaVelocity = Vectors::ZERO;
 
@@ -931,7 +934,6 @@ bool EntityItem::stepKinematicMotion(float timeElapsed) {
         }
 
         const float MIN_KINEMATIC_LINEAR_ACCELERATION_SQUARED = 1.0e-4f; // 0.01 m/sec^2
-        const float MIN_KINEMATIC_LINEAR_SPEED_SQUARED = 1.0e-6f; // 0.001 m/sec^2
         if (glm::length2(_gravity) > MIN_KINEMATIC_LINEAR_ACCELERATION_SQUARED) {
             // yes gravity
             // acceleration is in world-frame but we need it in local-frame
@@ -943,8 +945,9 @@ bool EntityItem::stepKinematicMotion(float timeElapsed) {
             }
             deltaVelocity += linearAcceleration * timeElapsed;
 
-            if (glm::length2(deltaVelocity) < MIN_KINEMATIC_LINEAR_SPEED_SQUARED
-                    && glm::length2(linearVelocity) < MIN_KINEMATIC_LINEAR_SPEED_SQUARED) {
+            if (linearSpeedSquared < MIN_KINEMATIC_LINEAR_SPEED_SQUARED
+                    && glm::length2(deltaVelocity) < MIN_KINEMATIC_LINEAR_SPEED_SQUARED
+                    && glm::length2(linearVelocity + deltaVelocity) < MIN_KINEMATIC_LINEAR_SPEED_SQUARED) {
                 linearVelocity = Vectors::ZERO;
             } else {
                 // NOTE: we do NOT include the second-order acceleration term (0.5 * a * dt^2)
@@ -958,7 +961,8 @@ bool EntityItem::stepKinematicMotion(float timeElapsed) {
             if (linearSpeedSquared < MIN_KINEMATIC_LINEAR_SPEED_SQUARED) {
                 linearVelocity = Vectors::ZERO;
             } else {
-                // NOTE: don't use acceleration term for linear displacement
+                // NOTE: we don't use second-order acceleration term for linear displacement
+                // because Bullet doesn't use it.
                 position += timeElapsed * linearVelocity;
                 linearVelocity += deltaVelocity;
             }
diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp
index f4e6b796e4..4f86fbdf6b 100644
--- a/libraries/physics/src/EntityMotionState.cpp
+++ b/libraries/physics/src/EntityMotionState.cpp
@@ -402,7 +402,11 @@ bool EntityMotionState::shouldSendUpdate(uint32_t simulationStep) {
 
     if (_entity->getSimulatorID() != Physics::getSessionUUID()) {
         // we don't own the simulation
-        bool shouldBid = _outgoingPriority > 0 && // but we would like to own it and
+
+        // NOTE: we do not volunteer to own kinematic or static objects
+        uint8_t insufficientPriority = _body->isStaticOrKinematicObject() ? VOLUNTEER_SIMULATION_PRIORITY : 0;
+
+        bool shouldBid = _outgoingPriority > insufficientPriority && // but we would like to own it AND
                 usecTimestampNow() > _nextOwnershipBid; // it is time to bid again
         if (shouldBid && _outgoingPriority < _entity->getSimulationPriority()) {
             // we are insufficiently interested so clear our interest
@@ -461,7 +465,7 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_
                 (DYNAMIC_ANGULAR_VELOCITY_THRESHOLD * DYNAMIC_ANGULAR_VELOCITY_THRESHOLD);
         bool movingSlowly = movingSlowlyLinear && movingSlowlyAngular && _entity->getAcceleration() == glm::vec3(0.0f);
 
-        if (movingSlowly) {
+        if (!_body->isStaticOrKinematicObject() && movingSlowly) {
             // velocities might not be zero, but we'll fake them as such, which will hopefully help convince
             // other simulating observers to deactivate their own copies
             glm::vec3 zero(0.0f);

From a3c66d8c912754d1b0713489cf9bb2a3e7e3591e Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 19 Apr 2016 13:18:29 -0700
Subject: [PATCH 09/14] reduce number of updates for fast-moving objects

---
 libraries/physics/src/EntityMotionState.cpp | 31 +++++++++++++--------
 1 file changed, 19 insertions(+), 12 deletions(-)

diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp
index 4f86fbdf6b..079589f2ae 100644
--- a/libraries/physics/src/EntityMotionState.cpp
+++ b/libraries/physics/src/EntityMotionState.cpp
@@ -178,11 +178,13 @@ 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 {
+    BT_PROFILE("getWorldTransform");
     if (!_entity) {
         return;
     }
     assert(entityTreeIsLocked());
     if (_motionType == MOTION_TYPE_KINEMATIC) {
+        BT_PROFILE("kinematicIntegration");
         // This is physical kinematic motion which steps strictly by the subframe count
         // of the physics simulation.
         uint32_t thisStep = ObjectMotionState::getWorldSimulationStep();
@@ -270,6 +272,7 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) {
         _serverPosition = bulletToGLM(xform.getOrigin());
         _serverRotation = bulletToGLM(xform.getRotation());
         _serverVelocity = getBodyLinearVelocityGTSigma();
+        _serverAcceleration = Vectors::ZERO;
         _serverAngularVelocity = bulletToGLM(_body->getAngularVelocity());
         _lastStep = simulationStep;
         _serverActionData = _entity->getActionData();
@@ -336,19 +339,23 @@ bool EntityMotionState::remoteSimulationOutOfSync(uint32_t simulationStep) {
     glm::vec3 position = bulletToGLM(worldTrans.getOrigin());
 
     float dx2 = glm::distance2(position, _serverPosition);
-
-    const float MAX_POSITION_ERROR_SQUARED = 0.000004f; //  Sqrt() - corresponds to 2 millimeters
+    const float MAX_POSITION_ERROR_SQUARED = 0.000004f; // corresponds to 2mm
     if (dx2 > MAX_POSITION_ERROR_SQUARED) {
-
-        #ifdef WANT_DEBUG
-            qCDebug(physics) << ".... (dx2 > MAX_POSITION_ERROR_SQUARED) ....";
-            qCDebug(physics) << "wasPosition:" << wasPosition;
-            qCDebug(physics) << "bullet position:" << position;
-            qCDebug(physics) << "_serverPosition:" << _serverPosition;
-            qCDebug(physics) << "dx2:" << dx2;
-        #endif
-
-        return true;
+        // we don't mind larger position error when the object has high speed
+        // so we divide by speed and check again
+        float speed2 = glm::length2(_serverVelocity);
+        const float MIN_ERROR_RATIO_SQUARED = 0.0025f; // corresponds to 5% error in 1 second
+        const float MIN_SPEED_SQUARED = 1.0e-6f; // corresponds to 1mm/sec
+        if (speed2 < MIN_SPEED_SQUARED || dx2 / speed2 > MIN_ERROR_RATIO_SQUARED) {
+            #ifdef WANT_DEBUG
+                qCDebug(physics) << ".... (dx2 > MAX_POSITION_ERROR_SQUARED) ....";
+                qCDebug(physics) << "wasPosition:" << wasPosition;
+                qCDebug(physics) << "bullet position:" << position;
+                qCDebug(physics) << "_serverPosition:" << _serverPosition;
+                qCDebug(physics) << "dx2:" << dx2;
+            #endif
+            return true;
+        }
     }
 
     if (glm::length2(_serverAngularVelocity) > 0.0f) {

From 8efb07cfd8c81739615fce2665b3233b1552bf7e Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 19 Apr 2016 15:41:07 -0700
Subject: [PATCH 10/14] correctg measured acceleration for kinematics

---
 libraries/entities/src/EntityItem.cpp | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/libraries/entities/src/EntityItem.cpp b/libraries/entities/src/EntityItem.cpp
index 4ea14a522d..673d5cf172 100644
--- a/libraries/entities/src/EntityItem.cpp
+++ b/libraries/entities/src/EntityItem.cpp
@@ -877,6 +877,10 @@ void EntityItem::simulate(const quint64& now) {
             // this entity is no longer moving
             // flag it to transition from KINEMATIC to STATIC
             _dirtyFlags |= Simulation::DIRTY_MOTION_TYPE;
+            setAcceleration(Vectors::ZERO);
+        } else {
+            // this object is moving kinematically, so make sure its "measured acceleration" is "gravity"
+            setAcceleration(_gravity);
         }
     }
     _lastSimulated = now;

From 14abb152169dec092efe5d0319a8fcefdf70b424 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 19 Apr 2016 15:46:00 -0700
Subject: [PATCH 11/14] set kinematic accel before stepping

---
 libraries/entities/src/EntityItem.cpp | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

diff --git a/libraries/entities/src/EntityItem.cpp b/libraries/entities/src/EntityItem.cpp
index 673d5cf172..a012e2b514 100644
--- a/libraries/entities/src/EntityItem.cpp
+++ b/libraries/entities/src/EntityItem.cpp
@@ -873,14 +873,12 @@ void EntityItem::simulate(const quint64& now) {
     #endif
 
     if (!hasActions()) {
+        setAcceleration(_gravity);
         if (!stepKinematicMotion(timeElapsed)) {
             // this entity is no longer moving
             // flag it to transition from KINEMATIC to STATIC
             _dirtyFlags |= Simulation::DIRTY_MOTION_TYPE;
             setAcceleration(Vectors::ZERO);
-        } else {
-            // this object is moving kinematically, so make sure its "measured acceleration" is "gravity"
-            setAcceleration(_gravity);
         }
     }
     _lastSimulated = now;
@@ -938,10 +936,10 @@ bool EntityItem::stepKinematicMotion(float timeElapsed) {
         }
 
         const float MIN_KINEMATIC_LINEAR_ACCELERATION_SQUARED = 1.0e-4f; // 0.01 m/sec^2
-        if (glm::length2(_gravity) > MIN_KINEMATIC_LINEAR_ACCELERATION_SQUARED) {
+        if (glm::length2(_acceleration) > MIN_KINEMATIC_LINEAR_ACCELERATION_SQUARED) {
             // yes gravity
             // acceleration is in world-frame but we need it in local-frame
-            glm::vec3 linearAcceleration = _gravity;
+            glm::vec3 linearAcceleration = _acceleration;
             bool success;
             Transform parentTransform = getParentTransform(success);
             if (success) {

From 872622c6f75dad8a5053e071ba74f6f950930fa9 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Wed, 20 Apr 2016 11:26:38 -0700
Subject: [PATCH 12/14] fix acceleration of for server-side kinematics

---
 libraries/entities/src/EntityItem.cpp       |  5 +-
 libraries/physics/src/EntityMotionState.cpp | 91 ++++++++++++---------
 libraries/physics/src/EntityMotionState.h   |  6 +-
 3 files changed, 54 insertions(+), 48 deletions(-)

diff --git a/libraries/entities/src/EntityItem.cpp b/libraries/entities/src/EntityItem.cpp
index a012e2b514..a51e5164df 100644
--- a/libraries/entities/src/EntityItem.cpp
+++ b/libraries/entities/src/EntityItem.cpp
@@ -873,7 +873,6 @@ void EntityItem::simulate(const quint64& now) {
     #endif
 
     if (!hasActions()) {
-        setAcceleration(_gravity);
         if (!stepKinematicMotion(timeElapsed)) {
             // this entity is no longer moving
             // flag it to transition from KINEMATIC to STATIC
@@ -937,7 +936,7 @@ bool EntityItem::stepKinematicMotion(float timeElapsed) {
 
         const float MIN_KINEMATIC_LINEAR_ACCELERATION_SQUARED = 1.0e-4f; // 0.01 m/sec^2
         if (glm::length2(_acceleration) > MIN_KINEMATIC_LINEAR_ACCELERATION_SQUARED) {
-            // yes gravity
+            // yes acceleration
             // acceleration is in world-frame but we need it in local-frame
             glm::vec3 linearAcceleration = _acceleration;
             bool success;
@@ -959,7 +958,7 @@ bool EntityItem::stepKinematicMotion(float timeElapsed) {
                 linearVelocity += deltaVelocity;
             }
         } else {
-            // no gravity
+            // no acceleration
             if (linearSpeedSquared < MIN_KINEMATIC_LINEAR_SPEED_SQUARED) {
                 linearVelocity = Vectors::ZERO;
             } else {
diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp
index 079589f2ae..d5e86814f9 100644
--- a/libraries/physics/src/EntityMotionState.cpp
+++ b/libraries/physics/src/EntityMotionState.cpp
@@ -56,8 +56,8 @@ EntityMotionState::EntityMotionState(btCollisionShape* shape, EntityItemPointer
     _serverGravity(0.0f),
     _serverAcceleration(0.0f),
     _serverActionData(QByteArray()),
-    _lastVelocity(glm::vec3(0.0f)),
-    _measuredAcceleration(glm::vec3(0.0f)),
+    _lastVelocity(0.0f),
+    _measuredAcceleration(0.0f),
     _nextOwnershipBid(0),
     _measuredDeltaTime(0.0f),
     _lastMeasureStep(0),
@@ -186,13 +186,18 @@ void EntityMotionState::getWorldTransform(btTransform& worldTrans) const {
     if (_motionType == MOTION_TYPE_KINEMATIC) {
         BT_PROFILE("kinematicIntegration");
         // This is physical kinematic motion which steps strictly by the subframe count
-        // of the physics simulation.
+        // of the physics simulation and uses full gravity for acceleration.
+        _entity->setAcceleration(_entity->getGravity());
         uint32_t thisStep = ObjectMotionState::getWorldSimulationStep();
         float dt = (thisStep - _lastKinematicStep) * PHYSICS_ENGINE_FIXED_SUBSTEP;
         _entity->stepKinematicMotion(dt);
 
         // bypass const-ness so we can remember the step
         const_cast<EntityMotionState*>(this)->_lastKinematicStep = thisStep;
+
+        // and set the acceleration-matches-gravity count high so that if we send an update
+        // it will use the correct acceleration for remote simulations
+        _accelerationNearlyGravityCount = (uint8_t)(-1);
     }
     worldTrans.setOrigin(glmToBullet(getObjectPosition()));
     worldTrans.setRotation(glmToBullet(_entity->getRotation()));
@@ -433,51 +438,42 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_
 
     if (!_body->isActive()) {
         // make sure all derivatives are zero
-        glm::vec3 zero(0.0f);
-        _entity->setVelocity(zero);
-        _entity->setAngularVelocity(zero);
-        _entity->setAcceleration(zero);
+        _entity->setVelocity(Vectors::ZERO);
+        _entity->setAngularVelocity(Vectors::ZERO);
+        _entity->setAcceleration(Vectors::ZERO);
         _numInactiveUpdates++;
     } else {
-        const uint8_t STEPS_TO_DECIDE_BALLISTIC = 4;
-        float gravityLength = glm::length(_entity->getGravity());
-        float accVsGravity = glm::abs(glm::length(_measuredAcceleration) - gravityLength);
-        const float ACCELERATION_EQUIVALENT_EPSILON_RATIO = 0.1f;
-        if (accVsGravity < ACCELERATION_EQUIVALENT_EPSILON_RATIO * gravityLength) {
-            // acceleration measured during the most recent simulation step was close to gravity.
-            if (getAccelerationNearlyGravityCount() < STEPS_TO_DECIDE_BALLISTIC) {
-                // only increment this if we haven't reached the threshold yet.  this is to avoid
-                // overflowing the counter.
-                incrementAccelerationNearlyGravityCount();
-            }
-        } else {
-            // acceleration wasn't similar to this entities gravity, so reset the went-ballistic counter
-            resetAccelerationNearlyGravityCount();
-        }
+        glm::vec3 gravity = _entity->getGravity();
+        glm::vec3 error = _measuredAcceleration - gravity;
+        float errorLength = glm::length(error);
+        int numSteps = (int)(step - _lastStep);
 
         // if this entity has been accelerated at close to gravity for a certain number of simulation-steps, let
         // the entity server's estimates include gravity.
-        if (getAccelerationNearlyGravityCount() >= STEPS_TO_DECIDE_BALLISTIC) {
-            _entity->setAcceleration(_entity->getGravity());
+        const uint8_t STEPS_TO_DECIDE_BALLISTIC = 4;
+        if (_accelerationNearlyGravityCount >= STEPS_TO_DECIDE_BALLISTIC) {
+            _entity->setAcceleration(gravity);
         } else {
-            _entity->setAcceleration(glm::vec3(0.0f));
+            _entity->setAcceleration(Vectors::ZERO);
         }
 
-        const float DYNAMIC_LINEAR_VELOCITY_THRESHOLD = 0.05f;  // 5 cm/sec
-        const float DYNAMIC_ANGULAR_VELOCITY_THRESHOLD = 0.087266f;  // ~5 deg/sec
+        if (!_body->isStaticOrKinematicObject()) {
+            const float DYNAMIC_LINEAR_VELOCITY_THRESHOLD = 0.05f;  // 5 cm/sec
+            const float DYNAMIC_ANGULAR_VELOCITY_THRESHOLD = 0.087266f;  // ~5 deg/sec
 
-        bool movingSlowlyLinear =
-            glm::length2(_entity->getVelocity()) < (DYNAMIC_LINEAR_VELOCITY_THRESHOLD * DYNAMIC_LINEAR_VELOCITY_THRESHOLD);
-        bool movingSlowlyAngular = glm::length2(_entity->getAngularVelocity()) <
-                (DYNAMIC_ANGULAR_VELOCITY_THRESHOLD * DYNAMIC_ANGULAR_VELOCITY_THRESHOLD);
-        bool movingSlowly = movingSlowlyLinear && movingSlowlyAngular && _entity->getAcceleration() == glm::vec3(0.0f);
+            bool movingSlowlyLinear =
+                glm::length2(_entity->getVelocity()) < (DYNAMIC_LINEAR_VELOCITY_THRESHOLD * DYNAMIC_LINEAR_VELOCITY_THRESHOLD);
+            bool movingSlowlyAngular = glm::length2(_entity->getAngularVelocity()) <
+                    (DYNAMIC_ANGULAR_VELOCITY_THRESHOLD * DYNAMIC_ANGULAR_VELOCITY_THRESHOLD);
+            bool movingSlowly = movingSlowlyLinear && movingSlowlyAngular && _entity->getAcceleration() == Vectors::ZERO;
 
-        if (!_body->isStaticOrKinematicObject() && movingSlowly) {
-            // velocities might not be zero, but we'll fake them as such, which will hopefully help convince
-            // other simulating observers to deactivate their own copies
-            glm::vec3 zero(0.0f);
-            _entity->setVelocity(zero);
-            _entity->setAngularVelocity(zero);
+            if (movingSlowly) {
+                // velocities might not be zero, but we'll fake them as such, which will hopefully help convince
+                // other simulating observers to deactivate their own copies
+                glm::vec3 zero(0.0f);
+                _entity->setVelocity(zero);
+                _entity->setAngularVelocity(zero);
+            }
         }
         _numInactiveUpdates = 0;
     }
@@ -625,9 +621,9 @@ void EntityMotionState::resetMeasuredBodyAcceleration() {
     if (_body) {
         _lastVelocity = getBodyLinearVelocityGTSigma();
     } else {
-        _lastVelocity = glm::vec3(0.0f);
+        _lastVelocity = Vectors::ZERO;
     }
-    _measuredAcceleration = glm::vec3(0.0f);
+    _measuredAcceleration = Vectors::ZERO;
 }
 
 void EntityMotionState::measureBodyAcceleration() {
@@ -651,8 +647,23 @@ void EntityMotionState::measureBodyAcceleration() {
             _lastStep = ObjectMotionState::getWorldSimulationStep();
             _numInactiveUpdates = 0;
         }
+
+        glm::vec3 gravity = _entity->getGravity();
+        float gravityLength = glm::length(gravity);
+        float accVsGravity = glm::abs(glm::length(_measuredAcceleration) - gravityLength);
+        const float ACCELERATION_EQUIVALENT_EPSILON_RATIO = 0.1f;
+        if (accVsGravity < ACCELERATION_EQUIVALENT_EPSILON_RATIO * gravityLength) {
+            // acceleration measured during the most recent simulation step was close to gravity.
+            if (_accelerationNearlyGravityCount < (uint8_t)(-2)) {
+                ++_accelerationNearlyGravityCount;
+            }
+        } else {
+            // acceleration wasn't similar to this entities gravity, so reset the went-ballistic counter
+            _accelerationNearlyGravityCount = 0;
+        }
     }
 }
+
 glm::vec3 EntityMotionState::getObjectLinearVelocityChange() const {
     // This is the dampened change in linear velocity, as calculated in measureBodyAcceleration: dv = a * dt
     // It is generally only meaningful during the lifespan of collision. In particular, it is not meaningful
diff --git a/libraries/physics/src/EntityMotionState.h b/libraries/physics/src/EntityMotionState.h
index 938f3e84c1..8f1532bf8f 100644
--- a/libraries/physics/src/EntityMotionState.h
+++ b/libraries/physics/src/EntityMotionState.h
@@ -51,10 +51,6 @@ public:
     virtual uint32_t getIncomingDirtyFlags() override;
     virtual void clearIncomingDirtyFlags() override;
 
-    void incrementAccelerationNearlyGravityCount() { _accelerationNearlyGravityCount++; }
-    void resetAccelerationNearlyGravityCount() { _accelerationNearlyGravityCount = 0; }
-    uint8_t getAccelerationNearlyGravityCount() { return _accelerationNearlyGravityCount; }
-
     virtual float getObjectRestitution() const override { return _entity->getRestitution(); }
     virtual float getObjectFriction() const override { return _entity->getFriction(); }
     virtual float getObjectLinearDamping() const override { return _entity->getDamping(); }
@@ -123,7 +119,7 @@ protected:
     uint32_t _lastStep; // last step of server extrapolation
 
     uint8_t _loopsWithoutOwner;
-    uint8_t _accelerationNearlyGravityCount;
+    mutable uint8_t _accelerationNearlyGravityCount;
     uint8_t _numInactiveUpdates { 1 };
     uint8_t _outgoingPriority { 0 };
 };

From 2bd83764e784c1a8b383d57b9374d1b31c979c38 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Wed, 20 Apr 2016 11:38:01 -0700
Subject: [PATCH 13/14] remove some debug cruft

---
 libraries/physics/src/EntityMotionState.cpp | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp
index d5e86814f9..fbec7cd3e0 100644
--- a/libraries/physics/src/EntityMotionState.cpp
+++ b/libraries/physics/src/EntityMotionState.cpp
@@ -444,9 +444,6 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_
         _numInactiveUpdates++;
     } else {
         glm::vec3 gravity = _entity->getGravity();
-        glm::vec3 error = _measuredAcceleration - gravity;
-        float errorLength = glm::length(error);
-        int numSteps = (int)(step - _lastStep);
 
         // if this entity has been accelerated at close to gravity for a certain number of simulation-steps, let
         // the entity server's estimates include gravity.

From 34d622e49840e0af01bbbc8f08f23b6efc827b78 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Fri, 22 Apr 2016 13:10:23 -0700
Subject: [PATCH 14/14] add warning about truncated kinematic timestep

---
 libraries/entities/src/EntityItem.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/libraries/entities/src/EntityItem.cpp b/libraries/entities/src/EntityItem.cpp
index a51e5164df..7260e5e782 100644
--- a/libraries/entities/src/EntityItem.cpp
+++ b/libraries/entities/src/EntityItem.cpp
@@ -889,6 +889,9 @@ bool EntityItem::stepKinematicMotion(float timeElapsed) {
     }
 
     const float MAX_TIME_ELAPSED = 1.0f; // seconds
+    if (timeElapsed > MAX_TIME_ELAPSED) {
+        qCWarning(entities) << "kinematic timestep = " << timeElapsed << " truncated to " << MAX_TIME_ELAPSED;
+    }
     timeElapsed = glm::min(timeElapsed, MAX_TIME_ELAPSED);
 
     Transform transform;