Merge pull request #7711 from AndrewMeadows/faster-kinematics

faster hifi kinematic motion calculations take-4

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,113 @@ 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;
+            setAcceleration(Vectors::ZERO);
+        }
+    }
     _lastSimulated = now;
 }
 
-void EntityItem::simulateKinematicMotion(float timeElapsed, bool setFlags) {
+bool EntityItem::stepKinematicMotion(float timeElapsed) {
-#ifdef WANT_DEBUG
+    if (timeElapsed <= 0.0f) {
-    qCDebug(entities) << "EntityItem::simulateKinematicMotion timeElapsed" << timeElapsed;
+        return true;
-#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;
     }
 
-    if (hasLocalAngularVelocity()) {
+    const float MAX_TIME_ELAPSED = 1.0f; // seconds
-        glm::vec3 localAngularVelocity = getLocalAngularVelocity();
+    if (timeElapsed > MAX_TIME_ELAPSED) {
+        qCWarning(entities) << "kinematic timestep = " << timeElapsed << " truncated to " << MAX_TIME_ELAPSED;
+    }
+    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);
+            angularVelocity *= powf(1.0f - _angularDamping, timeElapsed);
-            #ifdef WANT_DEBUG
-                qCDebug(entities) << "    angularDamping :" << _angularDamping;
-                qCDebug(entities) << "    newAngularVelocity:" << localAngularVelocity;
-            #endif
         }
 
-        float angularSpeed = glm::length(localAngularVelocity);
+        const float MIN_KINEMATIC_ANGULAR_SPEED_SQUARED =
-
+            KINEMATIC_ANGULAR_SPEED_THRESHOLD * KINEMATIC_ANGULAR_SPEED_THRESHOLD;
-        const float EPSILON_ANGULAR_VELOCITY_LENGTH = 0.0017453f; // 0.0017453 rad/sec = 0.1f degrees/sec
+        if (glm::length2(angularVelocity) < MIN_KINEMATIC_ANGULAR_SPEED_SQUARED) {
-        if (angularSpeed < EPSILON_ANGULAR_VELOCITY_LENGTH) {
+            angularVelocity = Vectors::ZERO;
-            if (setFlags && angularSpeed > 0.0f) {
-                _dirtyFlags |= Simulation::DIRTY_MOTION_TYPE;
-            }
-            localAngularVelocity = ENTITY_ITEM_ZERO_VEC3;
         } 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) {
+            while (dt > 0.0f) {
-                glm::quat  dQ = computeBulletRotationStep(localAngularVelocity, PHYSICS_ENGINE_FIXED_SUBSTEP);
+                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
+            transform.setRotation(rotation);
-            // 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);
         }
-
+        moving = true;
-        setLocalAngularVelocity(localAngularVelocity);
     }
 
-    if (hasLocalVelocity()) {
+    glm::vec3 position = transform.getTranslation();
-
+    float linearSpeedSquared = glm::length2(linearVelocity);
-        // acceleration is in the global frame, so transform it into the local frame.
+    const float MIN_KINEMATIC_LINEAR_SPEED_SQUARED =
-        // TODO: Move this into SpatiallyNestable.
+        KINEMATIC_LINEAR_SPEED_THRESHOLD * KINEMATIC_LINEAR_SPEED_THRESHOLD;
-        bool success;
+    if (linearSpeedSquared > 0.0f) {
-        Transform transform = getParentTransform(success);
+        glm::vec3 deltaVelocity = Vectors::ZERO;
-        glm::vec3 localAcceleration(glm::vec3::_null);
-        if (success) {
-            localAcceleration = glm::inverse(transform.getRotation()) * getAcceleration();
-        } else {
-            localAcceleration = getAcceleration();
-        }
 
         // linear damping
-        glm::vec3 localVelocity = getLocalVelocity();
         if (_damping > 0.0f) {
-            localVelocity *= powf(1.0f - _damping, timeElapsed);
+            deltaVelocity = (powf(1.0f - _damping, timeElapsed) - 1.0f) * linearVelocity;
-            #ifdef WANT_DEBUG
-                qCDebug(entities) << "    damping:" << _damping;
-                qCDebug(entities) << "    velocity AFTER dampingResistance:" << localVelocity;
-                qCDebug(entities) << "    glm::length(velocity):" << glm::length(localVelocity);
-            #endif
         }
 
-        // integrate position forward
+        const float MIN_KINEMATIC_LINEAR_ACCELERATION_SQUARED = 1.0e-4f; // 0.01 m/sec^2
-        glm::vec3 localPosition = getLocalPosition();
+        if (glm::length2(_acceleration) > MIN_KINEMATIC_LINEAR_ACCELERATION_SQUARED) {
-        glm::vec3 newLocalPosition = localPosition + (localVelocity * timeElapsed) + 0.5f * localAcceleration * timeElapsed * timeElapsed;
+            // yes acceleration
+            // acceleration is in world-frame but we need it in local-frame
+            glm::vec3 linearAcceleration = _acceleration;
+            bool success;
+            Transform parentTransform = getParentTransform(success);
+            if (success) {
+                linearAcceleration = glm::inverse(parentTransform.getRotation()) * linearAcceleration;
+            }
+            deltaVelocity += linearAcceleration * timeElapsed;
 
-        #ifdef WANT_DEBUG
+            if (linearSpeedSquared < MIN_KINEMATIC_LINEAR_SPEED_SQUARED
-            qCDebug(entities) << "  EntityItem::simulate()....";
+                    && glm::length2(deltaVelocity) < MIN_KINEMATIC_LINEAR_SPEED_SQUARED
-            qCDebug(entities) << "    timeElapsed:" << timeElapsed;
+                    && glm::length2(linearVelocity + deltaVelocity) < MIN_KINEMATIC_LINEAR_SPEED_SQUARED) {
-            qCDebug(entities) << "    old AACube:" << getMaximumAACube();
+                linearVelocity = Vectors::ZERO;
-            qCDebug(entities) << "    old position:" << localPosition;
+            } else {
-            qCDebug(entities) << "    old velocity:" << localVelocity;
+                // NOTE: we do NOT include the second-order acceleration term (0.5 * a * dt^2)
-            qCDebug(entities) << "    old getAABox:" << getAABox();
+                // when computing the displacement because Bullet also ignores that term.  Yes,
-            qCDebug(entities) << "    newPosition:" << newPosition;
+                // this is an approximation and it works best when dt is small.
-            qCDebug(entities) << "    glm::distance(newPosition, position):" << glm::distance(newLocalPosition, localPosition);
+                position += timeElapsed * linearVelocity;
-        #endif
+                linearVelocity += deltaVelocity;
-
-        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;
             }
         } else {
-            setLocalPosition(localPosition);
+            // no acceleration
-            setLocalVelocity(localVelocity);
+            if (linearSpeedSquared < MIN_KINEMATIC_LINEAR_SPEED_SQUARED) {
+                linearVelocity = Vectors::ZERO;
+            } else {
+                // NOTE: we don't use second-order acceleration term for linear displacement
+                // because Bullet doesn't use it.
+                position += timeElapsed * linearVelocity;
+                linearVelocity += deltaVelocity;
+            }
         }
-
+        moving = true;
-        #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 (moving) {
+        transform.setTranslation(position);
+        setLocalTransformAndVelocities(transform, linearVelocity, angularVelocity);
+    }
+    return moving;
 }
 
 bool EntityItem::isMoving() const {

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; }
 

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)),
+    _lastVelocity(0.0f),
-    _measuredAcceleration(glm::vec3(0.0f)),
+    _measuredAcceleration(0.0f),
     _nextOwnershipBid(0),
     _measuredDeltaTime(0.0f),
     _lastMeasureStep(0),
@@ -178,19 +178,26 @@ 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.
+        // 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->simulateKinematicMotion(dt);
+        _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()));
@@ -270,6 +277,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();
@@ -309,6 +317,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;
     }
 
@@ -334,19 +344,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; // corresponds to 2mm
-    const float MAX_POSITION_ERROR_SQUARED = 0.000004f; //  Sqrt() - corresponds to 2 millimeters
     if (dx2 > MAX_POSITION_ERROR_SQUARED) {
-
+        // we don't mind larger position error when the object has high speed
-        #ifdef WANT_DEBUG
+        // so we divide by speed and check again
-            qCDebug(physics) << ".... (dx2 > MAX_POSITION_ERROR_SQUARED) ....";
+        float speed2 = glm::length2(_serverVelocity);
-            qCDebug(physics) << "wasPosition:" << wasPosition;
+        const float MIN_ERROR_RATIO_SQUARED = 0.0025f; // corresponds to 5% error in 1 second
-            qCDebug(physics) << "bullet position:" << position;
+        const float MIN_SPEED_SQUARED = 1.0e-6f; // corresponds to 1mm/sec
-            qCDebug(physics) << "_serverPosition:" << _serverPosition;
+        if (speed2 < MIN_SPEED_SQUARED || dx2 / speed2 > MIN_ERROR_RATIO_SQUARED) {
-            qCDebug(physics) << "dx2:" << dx2;
+            #ifdef WANT_DEBUG
-        #endif
+                qCDebug(physics) << ".... (dx2 > MAX_POSITION_ERROR_SQUARED) ....";
-
+                qCDebug(physics) << "wasPosition:" << wasPosition;
-        return true;
+                qCDebug(physics) << "bullet position:" << position;
+                qCDebug(physics) << "_serverPosition:" << _serverPosition;
+                qCDebug(physics) << "dx2:" << dx2;
+            #endif
+            return true;
+        }
     }
 
     if (glm::length2(_serverAngularVelocity) > 0.0f) {
@@ -400,7 +414,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
@@ -420,51 +438,39 @@ void EntityMotionState::sendUpdate(OctreeEditPacketSender* packetSender, uint32_
 
     if (!_body->isActive()) {
         // make sure all derivatives are zero
-        glm::vec3 zero(0.0f);
+        _entity->setVelocity(Vectors::ZERO);
-        _entity->setVelocity(zero);
+        _entity->setAngularVelocity(Vectors::ZERO);
-        _entity->setAngularVelocity(zero);
+        _entity->setAcceleration(Vectors::ZERO);
-        _entity->setAcceleration(zero);
         _numInactiveUpdates++;
     } else {
-        const uint8_t STEPS_TO_DECIDE_BALLISTIC = 4;
+        glm::vec3 gravity = _entity->getGravity();
-        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();
-        }
 
         // 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) {
+        const uint8_t STEPS_TO_DECIDE_BALLISTIC = 4;
-            _entity->setAcceleration(_entity->getGravity());
+        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
+        if (!_body->isStaticOrKinematicObject()) {
-        const float DYNAMIC_ANGULAR_VELOCITY_THRESHOLD = 0.087266f;  // ~5 deg/sec
+            const float DYNAMIC_LINEAR_VELOCITY_THRESHOLD = 0.05f;  // 5 cm/sec
+            const float DYNAMIC_ANGULAR_VELOCITY_THRESHOLD = 0.087266f;  // ~5 deg/sec
 
-        bool movingSlowlyLinear =
+            bool movingSlowlyLinear =
-            glm::length2(_entity->getVelocity()) < (DYNAMIC_LINEAR_VELOCITY_THRESHOLD * DYNAMIC_LINEAR_VELOCITY_THRESHOLD);
+                glm::length2(_entity->getVelocity()) < (DYNAMIC_LINEAR_VELOCITY_THRESHOLD * DYNAMIC_LINEAR_VELOCITY_THRESHOLD);
-        bool movingSlowlyAngular = glm::length2(_entity->getAngularVelocity()) <
+            bool movingSlowlyAngular = glm::length2(_entity->getAngularVelocity()) <
-                (DYNAMIC_ANGULAR_VELOCITY_THRESHOLD * DYNAMIC_ANGULAR_VELOCITY_THRESHOLD);
+                    (DYNAMIC_ANGULAR_VELOCITY_THRESHOLD * DYNAMIC_ANGULAR_VELOCITY_THRESHOLD);
-        bool movingSlowly = movingSlowlyLinear && movingSlowlyAngular && _entity->getAcceleration() == glm::vec3(0.0f);
+            bool movingSlowly = movingSlowlyLinear && movingSlowlyAngular && _entity->getAcceleration() == Vectors::ZERO;
 
-        if (movingSlowly) {
+            if (movingSlowly) {
-            // velocities might not be zero, but we'll fake them as such, which will hopefully help convince
+                // 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
+                // other simulating observers to deactivate their own copies
-            glm::vec3 zero(0.0f);
+                glm::vec3 zero(0.0f);
-            _entity->setVelocity(zero);
+                _entity->setVelocity(zero);
-            _entity->setAngularVelocity(zero);
+                _entity->setAngularVelocity(zero);
+            }
         }
         _numInactiveUpdates = 0;
     }
@@ -612,9 +618,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() {
@@ -638,8 +644,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

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 };
 };

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

libraries/physics/src/ObjectMotionState.h

@@ -86,6 +86,8 @@ public:
 
     void updateBodyMaterialProperties();
     void updateBodyVelocities();
+    void updateLastKinematicStep();
+
     virtual void updateBodyMassProperties();
 
     MotionStateType getType() const { return _type; }

libraries/physics/src/PhysicsEngine.cpp

@@ -86,9 +86,8 @@ void PhysicsEngine::addObjectToDynamicsWorld(ObjectMotionState* motionState) {
             body->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
             body->updateInertiaTensor();
             motionState->updateBodyVelocities();
-            const float KINEMATIC_LINEAR_VELOCITY_THRESHOLD = 0.01f;  // 1 cm/sec
+            motionState->updateLastKinematicStep();
-            const float KINEMATIC_ANGULAR_VELOCITY_THRESHOLD = 0.01f;  // ~1 deg/sec
+            body->setSleepingThresholds(KINEMATIC_LINEAR_SPEED_THRESHOLD, KINEMATIC_ANGULAR_SPEED_THRESHOLD);
-            body->setSleepingThresholds(KINEMATIC_LINEAR_VELOCITY_THRESHOLD, KINEMATIC_ANGULAR_VELOCITY_THRESHOLD);
             break;
         }
         case MOTION_TYPE_DYNAMIC: {
@@ -109,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
+            body->setSleepingThresholds(DYNAMIC_LINEAR_SPEED_THRESHOLD, DYNAMIC_ANGULAR_SPEED_THRESHOLD);
-            const float DYNAMIC_ANGULAR_VELOCITY_THRESHOLD = 0.087266f;  // ~5 deg/sec
-            body->setSleepingThresholds(DYNAMIC_LINEAR_VELOCITY_THRESHOLD, DYNAMIC_ANGULAR_VELOCITY_THRESHOLD);
             if (!motionState->isMoving()) {
                 // try to initialize this object as inactive
                 body->forceActivationState(ISLAND_SLEEPING);
@@ -252,6 +249,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

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);
+            }
+        }
+    }
+}
+
+

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

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

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 = (parent || parentID.isNull());
-        success = true;
-    } else {
-        success = false;
-    }
-
     return parent;
 }
 
@@ -426,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;
@@ -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(false);
+}

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;