cleanup start/stop logic for HMD follow

interface/src/avatar/MyAvatar.cpp

@@ -177,9 +177,8 @@ void MyAvatar::reset(bool andReload) {
 
     // Reset dynamic state.
     _wasPushing = _isPushing = _isBraking = _billboardValid = false;
-    _isFollowingHMD = false;
+    _followVelocity = Vectors::ZERO;
-    _hmdFollowVelocity = Vectors::ZERO;
+    _followSpeed = 0.0f;
-    _hmdFollowSpeed = 0.0f;
     _skeletonModel.reset();
     getHead()->reset();
     _targetVelocity = glm::vec3(0.0f);
@@ -352,52 +351,39 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) {
 void MyAvatar::updateHMDFollowVelocity() {
     // compute offset to body's target position (in sensor-frame)
     auto sensorBodyMatrix = deriveBodyFromHMDSensor();
-    _hmdFollowOffset = extractTranslation(sensorBodyMatrix) - extractTranslation(_bodySensorMatrix);
+    glm::vec3 offset = extractTranslation(sensorBodyMatrix) - extractTranslation(_bodySensorMatrix);
-    glm::vec3 truncatedOffset = _hmdFollowOffset;
+    _followOffsetDistance = glm::length(offset);
-    if (truncatedOffset.y < 0.0f) {
+
-        // don't pull the body DOWN to match the target (allow animation system to squat)
+    const float FOLLOW_TIMESCALE = 0.5f;
-        truncatedOffset.y = 0.0f;
+    const float FOLLOW_THRESHOLD_SPEED = 0.2f;
-    }
+    const float FOLLOW_MIN_DISTANCE = 0.02f;
-    float truncatedOffsetDistance = glm::length(truncatedOffset);
+    const float FOLLOW_THRESHOLD_DISTANCE = 0.2f;
 
-    bool isMoving;
-    if (_lastIsMoving) {
-        const float MOVE_EXIT_SPEED_THRESHOLD = 0.07f;  // m/sec
-        isMoving = glm::length(_velocity) >= MOVE_EXIT_SPEED_THRESHOLD;
-    } else {
-        const float MOVE_ENTER_SPEED_THRESHOLD = 0.2f; // m/sec
-        isMoving = glm::length(_velocity) > MOVE_ENTER_SPEED_THRESHOLD;
-    }
-    bool justStartedMoving = (_lastIsMoving != isMoving) && isMoving;
-    _lastIsMoving = isMoving;
     bool hmdIsAtRest = _hmdAtRestDetector.update(_hmdSensorPosition, _hmdSensorOrientation);
-    const float MIN_HMD_HIP_SHIFT = 0.05f;
+    bool avatarIsMoving = glm::length(_velocity - _followVelocity) > FOLLOW_THRESHOLD_SPEED;
-    if (justStartedMoving || (hmdIsAtRest && truncatedOffsetDistance > MIN_HMD_HIP_SHIFT)) {
+    bool shouldFollow = hmdIsAtRest || avatarIsMoving;
-        _isFollowingHMD = true;
-    }
 
-    bool needNewFollowSpeed = (_isFollowingHMD && _hmdFollowSpeed == 0.0f);
+    // linear part
-    if (!needNewFollowSpeed) {
+    _followOffsetDistance = glm::length(offset);
-        // check to see if offset has exceeded its threshold
+    if (_followOffsetDistance < FOLLOW_MIN_DISTANCE) {
-        const float MAX_HMD_HIP_SHIFT = 0.2f;
+        // close enough
-        if (truncatedOffsetDistance > MAX_HMD_HIP_SHIFT) {
+        _followOffsetDistance = 0.0f;
-            _isFollowingHMD = true;
+    } else {
-            needNewFollowSpeed = true;
+        glm::vec3 truncatedOffset = offset;
+        if (truncatedOffset.y < 0.0f) {
+            truncatedOffset.y = 0.0f;
+        }
+        float truncatedDistance = glm::length(truncatedOffset);
+        bool needsNewSpeed = truncatedDistance > FOLLOW_THRESHOLD_DISTANCE;
+        if (needsNewSpeed || (shouldFollow && _followSpeed == 0.0f)) {
+            // compute new speed
+            _followSpeed = _followOffsetDistance / FOLLOW_TIMESCALE;
+        }
+        if (_followSpeed > 0.0f) {
+            // to compute new velocity we must rotate offset into the world-frame
+            glm::quat sensorToWorldRotation = extractRotation(_sensorToWorldMatrix);
+            _followVelocity = _followSpeed * glm::normalize(sensorToWorldRotation * offset);
         }
-    }
-    if (_isFollowingHMD) {
-        // only bother to rotate into world frame if we're following
-        glm::quat sensorToWorldRotation = extractRotation(_sensorToWorldMatrix);
-        _hmdFollowOffset = sensorToWorldRotation * _hmdFollowOffset;
-    }
-    if (needNewFollowSpeed) {
-        // compute new velocity that will be used to resolve offset of hips from body
-        const float FOLLOW_HMD_DURATION = 0.5f;  // seconds
-        _hmdFollowVelocity = (_hmdFollowOffset / FOLLOW_HMD_DURATION);
-        _hmdFollowSpeed = glm::length(_hmdFollowVelocity);
-    } else if (_isFollowingHMD) {
-        // compute new velocity (but not new speed)
-        _hmdFollowVelocity = _hmdFollowSpeed * glm::normalize(_hmdFollowOffset);
     }
 }
 
@@ -1300,11 +1286,11 @@ void MyAvatar::prepareForPhysicsSimulation() {
     _characterController.setAvatarPositionAndOrientation(getPosition(), getOrientation());
     if (qApp->isHMDMode()) {
         updateHMDFollowVelocity();
-    } else if (_isFollowingHMD) {
+    } else if (_followSpeed > 0.0f) {
-        _isFollowingHMD = false;
+        _followVelocity = Vectors::ZERO;
-        _hmdFollowVelocity = Vectors::ZERO;
+        _followSpeed = 0.0f;
     }
-    _characterController.setHMDVelocity(_hmdFollowVelocity);
+    _characterController.setFollowVelocity(_followVelocity);
 }
 
 void MyAvatar::harvestResultsFromPhysicsSimulation() {
@@ -1312,35 +1298,27 @@ void MyAvatar::harvestResultsFromPhysicsSimulation() {
     glm::quat orientation = getOrientation();
     _characterController.getAvatarPositionAndOrientation(position, orientation);
     nextAttitude(position, orientation);
-    if (_isFollowingHMD) {
+    if (_followSpeed > 0.0f) {
-        setVelocity(_characterController.getLinearVelocity() + _hmdFollowVelocity);
+        adjustSensorTransform();
-        glm::vec3 hmdShift = _characterController.getHMDShift();
+        setVelocity(_characterController.getLinearVelocity() + _followVelocity);
-        adjustSensorTransform(hmdShift);
     } else {
         setVelocity(_characterController.getLinearVelocity());
     }
 }
 
-void MyAvatar::adjustSensorTransform(glm::vec3 hmdShift) {
+void MyAvatar::adjustSensorTransform() {
     // compute blendFactor of latest hmdShift
     // which we'll use to blend the rotation part
-    float blendFactor = 1.0f;
+    float linearDistance = _characterController.getFollowTime() * _followSpeed;
-    float shiftLength = glm::length(hmdShift);
+    float blendFactor = linearDistance < _followOffsetDistance ? linearDistance / _followOffsetDistance : 1.0f;
-    if (shiftLength > 1.0e-5f) {
-        float offsetLength = glm::length(_hmdFollowOffset);
-        if (offsetLength > shiftLength) {
-            blendFactor = shiftLength / offsetLength;
-        }
-    }
 
     auto newBodySensorMatrix = deriveBodyFromHMDSensor();
     auto worldBodyMatrix = _sensorToWorldMatrix * newBodySensorMatrix;
     glm::quat finalBodyRotation = glm::normalize(glm::quat_cast(worldBodyMatrix));
     if (blendFactor >= 0.99f) {
         // the "adjustment" is more or less complete so stop following
-        _isFollowingHMD = false;
+        _followVelocity = Vectors::ZERO;
-        _hmdFollowSpeed = 0.0f;
+        _followSpeed = 0.0f;
-        _hmdFollowVelocity = Vectors::ZERO;
         // and slam the body's transform anyway to eliminate any slight errors
         glm::vec3 finalBodyPosition = extractTranslation(worldBodyMatrix);
         nextAttitude(finalBodyPosition, finalBodyRotation);
@@ -1520,6 +1498,9 @@ void MyAvatar::initAnimGraph() {
                          QUrl::fromLocalFile(PathUtils::resourcesPath() + "meshes/defaultAvatar_full/avatar-animation.json") :
                          _animGraphUrl);
     _rig->initAnimGraph(graphUrl, _skeletonModel.getGeometry()->getFBXGeometry());
+
+    _bodySensorMatrix = deriveBodyFromHMDSensor(); // Based on current cached HMD position/rotation..
+    updateSensorToWorldMatrix(); // Uses updated position/orientation and _bodySensorMatrix changes
 }
 
 void MyAvatar::destroyAnimGraph() {

interface/src/avatar/MyAvatar.h

@@ -208,7 +208,7 @@ public:
 
     void prepareForPhysicsSimulation();
     void harvestResultsFromPhysicsSimulation();
-    void adjustSensorTransform(glm::vec3 hmdShift);
+    void adjustSensorTransform();
 
     const QString& getCollisionSoundURL() { return _collisionSoundURL; }
     void setCollisionSoundURL(const QString& url);
@@ -394,9 +394,10 @@ private:
 
     // used to transform any sensor into world space, including the _hmdSensorMat, or hand controllers.
     glm::mat4 _sensorToWorldMatrix;
-    glm::vec3 _hmdFollowOffset { Vectors::ZERO };
+
-    glm::vec3 _hmdFollowVelocity { Vectors::ZERO };
+    glm::vec3 _followVelocity { Vectors::ZERO };
-    float _hmdFollowSpeed { 0.0f };
+    float _followSpeed { 0.0f };
+    float _followOffsetDistance { 0.0f };
 
     bool _goToPending;
     glm::vec3 _goToPosition;
@@ -414,9 +415,6 @@ private:
     glm::vec3 _customListenPosition;
     glm::quat _customListenOrientation;
 
-    bool _isFollowingHMD { false };
-    float _followHMDAlpha { 0.0f };
-
     AtRestDetector _hmdAtRestDetector;
     bool _lastIsMoving { false };
 };

interface/src/avatar/MyCharacterController.cpp

@@ -60,7 +60,7 @@ MyCharacterController::MyCharacterController(MyAvatar* avatar) {
     _floorDistance = MAX_FALL_HEIGHT;
 
     _walkVelocity.setValue(0.0f, 0.0f, 0.0f);
-    _hmdVelocity.setValue(0.0f, 0.0f, 0.0f);
+    _followVelocity.setValue(0.0f, 0.0f, 0.0f);
     _jumpSpeed = JUMP_SPEED;
     _isOnGround = false;
     _isJumping = false;
@@ -68,7 +68,7 @@ MyCharacterController::MyCharacterController(MyAvatar* avatar) {
     _isHovering = true;
     _isPushingUp = false;
     _jumpToHoverStart = 0;
-    _lastStepDuration = 0.0f;
+    _followTime = 0.0f;
 
     _pendingFlags = PENDING_FLAG_UPDATE_SHAPE;
     updateShapeIfNecessary();
@@ -161,16 +161,14 @@ void MyCharacterController::playerStep(btCollisionWorld* dynaWorld, btScalar dt)
         }
     }
 
-    // Rather than add _hmdVelocity to the velocity of the RigidBody, we explicitly teleport 
+    // Rather than add _followVelocity to the velocity of the RigidBody, we explicitly teleport 
     // the RigidBody forward according to the formula: distance = rate * time
-    if (_hmdVelocity.length2() > 0.0f) {
+    if (_followVelocity.length2() > 0.0f) {
         btTransform bodyTransform = _rigidBody->getWorldTransform();
-        bodyTransform.setOrigin(bodyTransform.getOrigin() + dt * _hmdVelocity);
+        bodyTransform.setOrigin(bodyTransform.getOrigin() + dt * _followVelocity);
         _rigidBody->setWorldTransform(bodyTransform);
     }
-    // MyAvatar will ask us how far we stepped for HMD motion, which will depend on how 
+    _followTime += dt;
-    // much time has accumulated in _lastStepDuration.
-    _lastStepDuration += dt;
 }
 
 void MyCharacterController::jump() {
@@ -346,8 +344,8 @@ void MyCharacterController::setTargetVelocity(const glm::vec3& velocity) {
     _walkVelocity = glmToBullet(velocity);
 }
 
-void MyCharacterController::setHMDVelocity(const glm::vec3& velocity) {
+void MyCharacterController::setFollowVelocity(const glm::vec3& velocity) {
-    _hmdVelocity = glmToBullet(velocity);
+    _followVelocity = glmToBullet(velocity);
 }
 
 glm::vec3 MyCharacterController::getLinearVelocity() const {
@@ -400,7 +398,7 @@ void MyCharacterController::preSimulation() {
             }
         }
     }
-    _lastStepDuration = 0.0f;
+    _followTime = 0.0f;
 }
 
 void MyCharacterController::postSimulation() {

interface/src/avatar/MyCharacterController.h

@@ -64,8 +64,8 @@ public:
     void getAvatarPositionAndOrientation(glm::vec3& position, glm::quat& rotation) const;
 
     void setTargetVelocity(const glm::vec3& velocity);
-    void setHMDVelocity(const glm::vec3& velocity);
+    void setFollowVelocity(const glm::vec3& velocity);
-    glm::vec3 getHMDShift() const { return _lastStepDuration * bulletToGLM(_hmdVelocity); }
+    float getFollowTime() const { return _followTime; }
 
     glm::vec3 getLinearVelocity() const;
 
@@ -75,7 +75,7 @@ protected:
 protected:
     btVector3 _currentUp;
     btVector3 _walkVelocity;
-    btVector3 _hmdVelocity;
+    btVector3 _followVelocity;
     btTransform _avatarBodyTransform;
 
     glm::vec3 _shapeLocalOffset;
@@ -93,7 +93,7 @@ protected:
     btScalar _gravity;
 
     btScalar _jumpSpeed;
-    btScalar _lastStepDuration;
+    btScalar _followTime;
 
     bool _enabled;
     bool _isOnGround;