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] 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);