From d544b7a645a779d95caed858b36b1440a5f9dc5a Mon Sep 17 00:00:00 2001
From: "U-GAPOS\\andrew" <andrew@highfidelity.io>
Date: Tue, 3 Nov 2015 13:59:15 -0800
Subject: [PATCH] fix bug: avatar can't walking when HMD at rest

---
 interface/src/avatar/MyAvatar.cpp | 37 +++++++++++++++----------------
 1 file changed, 18 insertions(+), 19 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index a5b39ef6e9..741a82fdb8 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -347,23 +347,6 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) {
 }
 
 void MyAvatar::updateHMDFollowVelocity() {
-    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);
-    if (hmdIsAtRest || justStartedMoving) {
-        _isFollowingHMD = true;
-    }
-
     // compute offset to body's target position (in sensor-frame)
     auto sensorBodyMatrix = deriveBodyFromHMDSensor();
     _hmdFollowOffset = extractTranslation(sensorBodyMatrix) - extractTranslation(_bodySensorMatrix);
@@ -372,13 +355,29 @@ void MyAvatar::updateHMDFollowVelocity() {
         // don't pull the body DOWN to match the target (allow animation system to squat)
         truncatedOffset.y = 0.0f;
     }
+    float truncatedOffsetDistance = glm::length(truncatedOffset);
+
+    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;
+    if (justStartedMoving || (hmdIsAtRest && truncatedOffsetDistance > MIN_HMD_HIP_SHIFT)) {
+        _isFollowingHMD = true;
+    }
 
     bool needNewFollowSpeed = (_isFollowingHMD && _hmdFollowSpeed == 0.0f);
     if (!needNewFollowSpeed) {
         // check to see if offset has exceeded its threshold
-        float distance = glm::length(truncatedOffset);
         const float MAX_HMD_HIP_SHIFT = 0.2f;
-        if (distance > MAX_HMD_HIP_SHIFT) {
+        if (truncatedOffsetDistance > MAX_HMD_HIP_SHIFT) {
             _isFollowingHMD = true;
             needNewFollowSpeed = true;
         }