diff --git a/libraries/entities/src/EntityItem.cpp b/libraries/entities/src/EntityItem.cpp
index 431d638063..9fa58c9187 100644
--- a/libraries/entities/src/EntityItem.cpp
+++ b/libraries/entities/src/EntityItem.cpp
@@ -876,123 +876,112 @@ void EntityItem::simulate(const quint64& 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()) {
+    if (hasActions() || timeElapsed < 0.0f) {
         return;
     }
 
-    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 isMoving = 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);
+            transform.setRotation(rotation);
+            isMoving = true;
+        }
+    }
+    glm::vec3 position = transform.getTranslation();
 
-            setRotation(rotation);
+    const float MIN_KINEMATIC_LINEAR_SPEED_SQUARED = 1.0e-6f; // 1mm/sec ^2
+    bool hasLinearVelocity = (glm::length2(linearVelocity) > MIN_KINEMATIC_LINEAR_SPEED_SQUARED );
+    if (hasLinearVelocity) {
+        // linear damping
+        if (_damping > 0.0f) {
+            linearVelocity *= powf(1.0f - _damping, timeElapsed);
         }
 
-        setLocalAngularVelocity(localAngularVelocity);
+        // integrate position forward sans acceleration
+        position += linearVelocity * timeElapsed;
     }
 
-    if (hasLocalVelocity()) {
-
-        // acceleration is in the global frame, so transform it into the local frame.
-        // TODO: Move this into SpatiallyNestable.
+    const float MIN_KINEMATIC_GRAVITY_MOTION_SQUARED = 1.0e-6f; // 0.001 mm/sec^2
+    bool hasGravity = (glm::length2(_gravity) > MIN_KINEMATIC_GRAVITY_MOTION_SQUARED);
+    if (hasGravity) {
+        // acceleration is in world-frame but we need it in local-frame
+        glm::vec3 linearAcceleration = _gravity;
         bool success;
-        Transform transform = getParentTransform(success);
-        glm::vec3 localAcceleration(glm::vec3::_null);
+        Transform parentTransform = getParentTransform(success);
         if (success) {
-            localAcceleration = glm::inverse(transform.getRotation()) * getAcceleration();
-        } else {
-            localAcceleration = getAcceleration();
+            linearAcceleration = glm::inverse(parentTransform.getRotation()) * linearAcceleration;
         }
 
-        // 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
-        }
+        // integrate position's acceleration term
+        position += 0.5f * linearAcceleration * timeElapsed * timeElapsed;
 
-        // integrate position forward
-        glm::vec3 localPosition = getLocalPosition();
-        glm::vec3 newLocalPosition = localPosition + (localVelocity * timeElapsed) + 0.5f * localAcceleration * timeElapsed * timeElapsed;
+        // integrate linearVelocity
+        linearVelocity += 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
+    if (hasLinearVelocity || hasGravity) {
+        // We MUST eventually stop kinematic motion for slow entities otherwise they will take
+        // a looooong time to settle down, so we remeasure linear speed and zero the velocity
+        // if it is too small.
+        if (glm::length2(linearVelocity) < MIN_KINEMATIC_LINEAR_SPEED_SQUARED) {
+            linearVelocity = Vectors::ZERO;
+            if (!hasLinearVelocity) {
+                // Despite some gravity the final linear velocity is still too small to defeat the
+                // "effective resistance of free-space" which means we must reset position back to
+                // where it started since otherwise the entity may creep vveerrryy sslloowwllyy at
+                // a constant speed.
+                position = transform.getTranslation();
 
-        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;
+                // Ultimately what this means is that there is some minimum gravity we can
+                // fully support for kinematic motion.  It's exact value is a function of this
+                // entity's linearDamping and the hardcoded MIN_KINEMATIC_FOO parameters above,
+                // but the theoretical minimum gravity for zero damping at 90Hz is:
+                //
+                // minGravity = minSpeed / dt = 0.001 m/sec * 90 /sec = 0.09 m/sec^2
+                //
+                // In practice the true minimum is half that value, since if the frame rate is ever
+                // less than the expected then sometimes dt will be twice as long.
+                //
+                // Since we don't set isMoving true here this entity is destined to transition to
+                // STATIC unless it has some angular motion keeping it alive.
+            } else {
+                isMoving = true;
             }
         } else {
-            setLocalPosition(localPosition);
-            setLocalVelocity(localVelocity);
+            isMoving = true;
         }
+    }
+    transform.setTranslation(position);
+    setLocalTransformAndVelocities(transform, linearVelocity, angularVelocity);
 
-        #ifdef WANT_DEBUG
-            qCDebug(entities) << "    new position:" << position;
-            qCDebug(entities) << "    new velocity:" << velocity;
-            qCDebug(entities) << "    new AACube:" << getMaximumAACube();
-            qCDebug(entities) << "    old getAABox:" << getAABox();
-        #endif
+    if (setFlags && !isMoving) {
+        // flag this entity to transition from KINEMATIC to STATIC
+        _dirtyFlags |= Simulation::DIRTY_MOTION_TYPE;
     }
 }
 
diff --git a/libraries/physics/src/EntityMotionState.cpp b/libraries/physics/src/EntityMotionState.cpp
index e3952ba1d6..5f168d2e33 100644
--- a/libraries/physics/src/EntityMotionState.cpp
+++ b/libraries/physics/src/EntityMotionState.cpp
@@ -94,7 +94,7 @@ void EntityMotionState::updateServerPhysicsVariables() {
 }
 
 // virtual
-bool EntityMotionState::handleEasyChanges(uint32_t& flags) {
+void EntityMotionState::handleEasyChanges(uint32_t& flags) {
     assert(entityTreeIsLocked());
     updateServerPhysicsVariables();
     ObjectMotionState::handleEasyChanges(flags);
@@ -137,8 +137,6 @@ bool EntityMotionState::handleEasyChanges(uint32_t& flags) {
     if ((flags & Simulation::DIRTY_PHYSICS_ACTIVATION) && !_body->isActive()) {
         _body->activate();
     }
-
-    return true;
 }
 
 
@@ -175,11 +173,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();
@@ -420,19 +420,18 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, const Q
         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();
+            if (_accelerationNearlyGravityCount < STEPS_TO_DECIDE_BALLISTIC) {
+                // only increment this if we haven't reached the threshold yet, to avoid overflowing the counter
+                ++_accelerationNearlyGravityCount;
             }
         } else {
-            // acceleration wasn't similar to this entities gravity, so reset the went-ballistic counter
-            resetAccelerationNearlyGravityCount();
+            // acceleration wasn't similar to this entity's gravity, reset the counter
+            _accelerationNearlyGravityCount = 0;
         }
 
         // 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) {
+        if (_accelerationNearlyGravityCount >= STEPS_TO_DECIDE_BALLISTIC) {
             _entity->setAcceleration(_entity->getGravity());
         } else {
             _entity->setAcceleration(glm::vec3(0.0f));
diff --git a/libraries/physics/src/EntityMotionState.h b/libraries/physics/src/EntityMotionState.h
index ac16ec6d5d..eb42d03b52 100644
--- a/libraries/physics/src/EntityMotionState.h
+++ b/libraries/physics/src/EntityMotionState.h
@@ -29,7 +29,7 @@ public:
     virtual ~EntityMotionState();
 
     void updateServerPhysicsVariables();
-    virtual bool handleEasyChanges(uint32_t& flags) override;
+    virtual void handleEasyChanges(uint32_t& flags) override;
     virtual bool handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine) override;
 
     /// \return PhysicsMotionType based on params set in EntityItem
@@ -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(); }
diff --git a/libraries/physics/src/ObjectMotionState.cpp b/libraries/physics/src/ObjectMotionState.cpp
index 482c3146f8..e902758461 100644
--- a/libraries/physics/src/ObjectMotionState.cpp
+++ b/libraries/physics/src/ObjectMotionState.cpp
@@ -38,8 +38,11 @@ const glm::vec3& ObjectMotionState::getWorldOffset() {
     return _worldOffset;
 }
 
+// We init worldSimulationStep to 1 instead of 0 because we initialize _lastKineticStep to (worldSimulationStep - 1)
+// so that the object starts moving on the first frame that it was set kinematic.
+static uint32_t worldSimulationStep { 1 };
+
 // static
-uint32_t worldSimulationStep = 0;
 void ObjectMotionState::setWorldSimulationStep(uint32_t step) {
     assert(step > worldSimulationStep);
     worldSimulationStep = step;
@@ -164,7 +167,7 @@ void ObjectMotionState::setRigidBody(btRigidBody* body) {
     }
 }
 
-bool ObjectMotionState::handleEasyChanges(uint32_t& flags) {
+void ObjectMotionState::handleEasyChanges(uint32_t& flags) {
     if (flags & Simulation::DIRTY_POSITION) {
         btTransform worldTrans = _body->getWorldTransform();
         btVector3 newPosition = glmToBullet(getObjectPosition());
@@ -183,6 +186,10 @@ bool ObjectMotionState::handleEasyChanges(uint32_t& flags) {
             worldTrans.setRotation(newRotation);
         }
         _body->setWorldTransform(worldTrans);
+        if (!(flags & HARD_DIRTY_PHYSICS_FLAGS) && _body->isStaticObject()) {
+            // force activate static body so its Aabb is updated later
+            _body->activate(true);
+        }
     } else if (flags & Simulation::DIRTY_ROTATION) {
         btTransform worldTrans = _body->getWorldTransform();
         btQuaternion newRotation = glmToBullet(getObjectRotation());
@@ -192,6 +199,10 @@ bool ObjectMotionState::handleEasyChanges(uint32_t& flags) {
         }
         worldTrans.setRotation(newRotation);
         _body->setWorldTransform(worldTrans);
+        if (!(flags & HARD_DIRTY_PHYSICS_FLAGS) && _body->isStaticObject()) {
+            // force activate static body so its Aabb is updated later
+            _body->activate(true);
+        }
     }
 
     if (flags & Simulation::DIRTY_LINEAR_VELOCITY) {
@@ -232,8 +243,6 @@ bool ObjectMotionState::handleEasyChanges(uint32_t& flags) {
     if (flags & Simulation::DIRTY_MASS) {
         updateBodyMassProperties();
     }
-
-    return true;
 }
 
 bool ObjectMotionState::handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine) {
@@ -292,6 +301,10 @@ void ObjectMotionState::updateBodyVelocities() {
     _body->setActivationState(ACTIVE_TAG);
 }
 
+void ObjectMotionState::updateLastKinematicStep() {
+    _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 bb78eb12d6..44cb88721c 100644
--- a/libraries/physics/src/ObjectMotionState.h
+++ b/libraries/physics/src/ObjectMotionState.h
@@ -50,11 +50,12 @@ const uint32_t HARD_DIRTY_PHYSICS_FLAGS = (uint32_t)(Simulation::DIRTY_MOTION_TY
                                                      Simulation::DIRTY_COLLISION_GROUP);
 const uint32_t EASY_DIRTY_PHYSICS_FLAGS = (uint32_t)(Simulation::DIRTY_TRANSFORM | Simulation::DIRTY_VELOCITIES |
                                                      Simulation::DIRTY_MASS | Simulation::DIRTY_MATERIAL |
-                                                     Simulation::DIRTY_SIMULATOR_ID | Simulation::DIRTY_SIMULATION_OWNERSHIP_PRIORITY);
+                                                     Simulation::DIRTY_SIMULATOR_ID | Simulation::DIRTY_SIMULATION_OWNERSHIP_PRIORITY |
+                                                     Simulation::DIRTY_PHYSICS_ACTIVATION);
+
 
 // These are the set of incoming flags that the PhysicsEngine needs to hear about:
-const uint32_t DIRTY_PHYSICS_FLAGS = (uint32_t)(HARD_DIRTY_PHYSICS_FLAGS | EASY_DIRTY_PHYSICS_FLAGS |
-                                                Simulation::DIRTY_PHYSICS_ACTIVATION);
+const uint32_t DIRTY_PHYSICS_FLAGS = (uint32_t)(HARD_DIRTY_PHYSICS_FLAGS | EASY_DIRTY_PHYSICS_FLAGS);
 
 // These are the outgoing flags that the PhysicsEngine can affect:
 const uint32_t OUTGOING_DIRTY_PHYSICS_FLAGS = Simulation::DIRTY_TRANSFORM | Simulation::DIRTY_VELOCITIES;
@@ -80,11 +81,12 @@ public:
     ObjectMotionState(btCollisionShape* shape);
     ~ObjectMotionState();
 
-    virtual bool handleEasyChanges(uint32_t& flags);
+    virtual void handleEasyChanges(uint32_t& flags);
     virtual bool handleHardAndEasyChanges(uint32_t& flags, PhysicsEngine* engine);
 
     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 0040c19c3d..be80149c96 100644
--- a/libraries/physics/src/PhysicsEngine.cpp
+++ b/libraries/physics/src/PhysicsEngine.cpp
@@ -50,6 +50,13 @@ void PhysicsEngine::init() {
         // default gravity of the world is zero, so each object must specify its own gravity
         // TODO: set up gravity zones
         _dynamicsWorld->setGravity(btVector3(0.0f, 0.0f, 0.0f));
+
+        // By default Bullet will update the Aabb's of all objects every frame, even statics.
+        // This can waste CPU cycles so we configure Bullet to only update ACTIVE objects here.
+        // However, this means when a static object is moved we must manually update its Aabb
+        // in order for its broadphase collision queries to work correctly. Look at how we use
+        // _activeStaticBodies to track and update the Aabb's of moved static objects.
+        _dynamicsWorld->setForceUpdateAllAabbs(false);
     }
 }
 
@@ -80,6 +87,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);
@@ -189,12 +197,18 @@ VectorOfMotionStates PhysicsEngine::changeObjects(const VectorOfMotionStates& ob
                 stillNeedChange.push_back(object);
             }
         } else if (flags & EASY_DIRTY_PHYSICS_FLAGS) {
-            if (object->handleEasyChanges(flags)) {
-                object->clearIncomingDirtyFlags();
-            } else {
-                stillNeedChange.push_back(object);
-            }
+            object->handleEasyChanges(flags);
+            object->clearIncomingDirtyFlags();
         }
+        if (object->getMotionType() == MOTION_TYPE_STATIC && object->isActive()) {
+            _activeStaticBodies.push_back(object->getRigidBody());
+        }
+    }
+    // active static bodies have changed (in an Easy way) and need their Aabbs updated
+    // but we've configured Bullet to NOT update them automatically (for improved performance)
+    // so we must do it ourselves
+    for (size_t i = 0; i < _activeStaticBodies.size(); ++i) {
+        _dynamicsWorld->updateSingleAabb(_activeStaticBodies[i]);
     }
     return stillNeedChange;
 }
@@ -240,6 +254,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
@@ -388,6 +403,12 @@ const CollisionEvents& PhysicsEngine::getCollisionEvents() {
 
 const VectorOfMotionStates& PhysicsEngine::getOutgoingChanges() {
     BT_PROFILE("copyOutgoingChanges");
+    // Bullet will not deactivate static objects (it doesn't expect them to be active)
+    // so we must deactivate them ourselves
+    for (size_t i = 0; i < _activeStaticBodies.size(); ++i) {
+        _activeStaticBodies[i]->forceActivationState(ISLAND_SLEEPING);
+    }
+    _activeStaticBodies.clear();
     _dynamicsWorld->synchronizeMotionStates();
     _hasOutgoingChanges = false;
     return _dynamicsWorld->getChangedMotionStates();
diff --git a/libraries/physics/src/PhysicsEngine.h b/libraries/physics/src/PhysicsEngine.h
index f644d6f5b2..18e4016fec 100644
--- a/libraries/physics/src/PhysicsEngine.h
+++ b/libraries/physics/src/PhysicsEngine.h
@@ -13,9 +13,9 @@
 #define hifi_PhysicsEngine_h
 
 #include <stdint.h>
+#include <vector>
 
 #include <QUuid>
-#include <QVector>
 #include <btBulletDynamicsCommon.h>
 #include <BulletCollision/CollisionDispatch/btGhostObject.h>
 
@@ -41,7 +41,7 @@ public:
 };
 
 typedef std::map<ContactKey, ContactInfo> ContactMap;
-typedef QVector<Collision> CollisionEvents;
+typedef std::vector<Collision> CollisionEvents;
 
 class PhysicsEngine {
 public:
@@ -110,6 +110,7 @@ private:
     ContactMap _contactMap;
     CollisionEvents _collisionEvents;
     QHash<QUuid, EntityActionPointer> _objectActions;
+    std::vector<btRigidBody*> _activeStaticBodies;
 
     glm::vec3 _originOffset;
     QUuid _sessionID;
@@ -121,7 +122,6 @@ private:
 
     bool _dumpNextStats = false;
     bool _hasOutgoingChanges = false;
-
 };
 
 typedef std::shared_ptr<PhysicsEngine> PhysicsEnginePointer;
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
diff --git a/libraries/shared/src/GLMHelpers.cpp b/libraries/shared/src/GLMHelpers.cpp
index d21d88d212..53abb3827d 100644
--- a/libraries/shared/src/GLMHelpers.cpp
+++ b/libraries/shared/src/GLMHelpers.cpp
@@ -463,14 +463,6 @@ glm::vec2 getFacingDir2D(const glm::mat4& m) {
     }
 }
 
-bool isNaN(glm::vec3 value) {
-    return isNaN(value.x) || isNaN(value.y) || isNaN(value.z);
-}
-
-bool isNaN(glm::quat value) {
-    return isNaN(value.w) || isNaN(value.x) || isNaN(value.y) || isNaN(value.z);
-}
-
 glm::mat4 orthoInverse(const glm::mat4& m) {
     glm::mat4 r = m;
     r[3] = glm::vec4(0.0f, 0.0f, 0.0f, 1.0f);
diff --git a/libraries/shared/src/GLMHelpers.h b/libraries/shared/src/GLMHelpers.h
index 469ca1fc81..8b1446d4e5 100644
--- a/libraries/shared/src/GLMHelpers.h
+++ b/libraries/shared/src/GLMHelpers.h
@@ -229,8 +229,8 @@ void generateBasisVectors(const glm::vec3& primaryAxis, const glm::vec3& seconda
 glm::vec2 getFacingDir2D(const glm::quat& rot);
 glm::vec2 getFacingDir2D(const glm::mat4& m);
 
-bool isNaN(glm::vec3 value);
-bool isNaN(glm::quat value);
+inline bool isNaN(const glm::vec3& value) { return isNaN(value.x) || isNaN(value.y) || isNaN(value.z); }
+inline bool isNaN(const glm::quat& value) { return isNaN(value.w) || isNaN(value.x) || isNaN(value.y) || isNaN(value.z); }
 
 glm::mat4 orthoInverse(const glm::mat4& m);
 
diff --git a/libraries/shared/src/PhysicsCollisionGroups.h b/libraries/shared/src/PhysicsCollisionGroups.h
index 6d320e69cb..794f338dc5 100644
--- a/libraries/shared/src/PhysicsCollisionGroups.h
+++ b/libraries/shared/src/PhysicsCollisionGroups.h
@@ -51,10 +51,10 @@ const int16_t BULLET_COLLISION_GROUP_COLLISIONLESS = 1 << 14;
 const int16_t BULLET_COLLISION_MASK_DEFAULT = ~ BULLET_COLLISION_GROUP_COLLISIONLESS;
 
 // STATIC does not collide with itself (as optimization of physics simulation)
-const int16_t BULLET_COLLISION_MASK_STATIC = ~ (BULLET_COLLISION_GROUP_COLLISIONLESS | BULLET_COLLISION_GROUP_STATIC);
+const int16_t BULLET_COLLISION_MASK_STATIC = ~ (BULLET_COLLISION_GROUP_COLLISIONLESS | BULLET_COLLISION_GROUP_KINEMATIC | BULLET_COLLISION_GROUP_STATIC);
 
 const int16_t BULLET_COLLISION_MASK_DYNAMIC = BULLET_COLLISION_MASK_DEFAULT;
-const int16_t BULLET_COLLISION_MASK_KINEMATIC = BULLET_COLLISION_MASK_DEFAULT;
+const int16_t BULLET_COLLISION_MASK_KINEMATIC = BULLET_COLLISION_MASK_STATIC;
 
 // MY_AVATAR does not collide with itself
 const int16_t BULLET_COLLISION_MASK_MY_AVATAR = ~(BULLET_COLLISION_GROUP_COLLISIONLESS | BULLET_COLLISION_GROUP_MY_AVATAR);
diff --git a/libraries/shared/src/SharedUtil.cpp b/libraries/shared/src/SharedUtil.cpp
index 30d4726bcc..d48bddbd88 100644
--- a/libraries/shared/src/SharedUtil.cpp
+++ b/libraries/shared/src/SharedUtil.cpp
@@ -247,12 +247,6 @@ int getNthBit(unsigned char byte, int ordinal) {
     return ERROR_RESULT;
 }
 
-bool isBetween(int64_t value, int64_t max, int64_t min) {
-    return ((value <= max) && (value >= min));
-}
-
-
-
 void setSemiNibbleAt(unsigned char& byte, int bitIndex, int value) {
     //assert(value <= 3 && value >= 0);
     byte |= ((value & 3) << (6 - bitIndex)); // semi-nibbles store 00, 01, 10, or 11
@@ -260,12 +254,7 @@ void setSemiNibbleAt(unsigned char& byte, int bitIndex, int value) {
 
 bool isInEnvironment(const char* environment) {
     char* environmentString = getenv("HIFI_ENVIRONMENT");
-
-    if (environmentString && strcmp(environmentString, environment) == 0) {
-        return true;
-    } else {
-        return false;
-    }
+    return (environmentString && strcmp(environmentString, environment) == 0);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////
@@ -632,10 +621,6 @@ void debug::checkDeadBeef(void* memoryVoid, int size) {
     assert(memcmp((unsigned char*)memoryVoid, DEADBEEF, std::min(size, DEADBEEF_SIZE)) != 0);
 }
 
-bool isNaN(float value) {
-    return value != value;
-}
-
 QString formatUsecTime(float usecs, int prec) {
     static const quint64 SECONDS_PER_MINUTE = 60;
     static const quint64 USECS_PER_MINUTE = USECS_PER_SECOND * SECONDS_PER_MINUTE;
diff --git a/libraries/shared/src/SharedUtil.h b/libraries/shared/src/SharedUtil.h
index 8fb65a5247..e9201b4a92 100644
--- a/libraries/shared/src/SharedUtil.h
+++ b/libraries/shared/src/SharedUtil.h
@@ -180,11 +180,11 @@ private:
     static int DEADBEEF_SIZE;
 };
 
-bool isBetween(int64_t value, int64_t max, int64_t min);
-
+/// \return true when value is between max and min
+inline bool isBetween(int64_t value, int64_t max, int64_t min) { return ((value <= max) && (value >= min)); }
 
 /// \return bool is the float NaN
-bool isNaN(float value);
+inline bool isNaN(float value) { return value != value; }
 
 QString formatUsecTime(float usecs, int prec = 3);
 QString formatSecondsElapsed(float seconds);
diff --git a/libraries/shared/src/SpatiallyNestable.cpp b/libraries/shared/src/SpatiallyNestable.cpp
index 13bf5d9054..3cbd3c5ac9 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;
 }
 
@@ -849,3 +842,40 @@ AACube SpatiallyNestable::getQueryAACube() const {
     }
     return result;
 }
+
+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 379f2facd7..ef70d0231b 100644
--- a/libraries/shared/src/SpatiallyNestable.h
+++ b/libraries/shared/src/SpatiallyNestable.h
@@ -149,6 +149,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;
diff --git a/libraries/shared/src/Transform.cpp b/libraries/shared/src/Transform.cpp
index a3a3c05731..c51b3dae4b 100644
--- a/libraries/shared/src/Transform.cpp
+++ b/libraries/shared/src/Transform.cpp
@@ -150,7 +150,3 @@ QJsonObject Transform::toJson(const Transform& transform) {
     }
     return result;
 }
-
-bool Transform::containsNaN() const {
-    return isNaN(_rotation) || isNaN(_scale) || isNaN(_translation);
-}
diff --git a/libraries/shared/src/Transform.h b/libraries/shared/src/Transform.h
index 1024173cbd..1e1d10c54b 100644
--- a/libraries/shared/src/Transform.h
+++ b/libraries/shared/src/Transform.h
@@ -145,7 +145,7 @@ public:
     Vec4 transform(const Vec4& pos) const;
     Vec3 transform(const Vec3& pos) const;
 
-    bool containsNaN() const;
+    bool containsNaN() const { return isNaN(_rotation) || isNaN(glm::dot(_scale, _translation)); }
 
 protected: