When HMD is at rest, re-center the body under the avatar.

This should help the case when a user avatar is stuck in an
uncomfortable pose for a long period of time.  If they stop
moving their head, the body should recenter itself and
appear more natural.

Added an AtRestDetector class.  That tracks the average and variance
of both position and rotation (quaternion logarithms), then
detects when the variance falls under a threshold.

Also, renamed variables with the straighting prefix to straightening.

interface/src/avatar/MyAvatar.cpp

@@ -111,7 +111,8 @@ MyAvatar::MyAvatar(RigPointer rig) :
     _goToOrientation(),
     _rig(rig),
     _prevShouldDrawHead(true),
-    _audioListenerMode(FROM_HEAD)
+    _audioListenerMode(FROM_HEAD),
+    _hmdAtRestDetector(glm::vec3(0), glm::quat())
 {
     for (int i = 0; i < MAX_DRIVE_KEYS; i++) {
         _driveKeys[i] = 0.0f;
@@ -311,37 +312,39 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) {
     _hmdSensorPosition = extractTranslation(hmdSensorMatrix);
     _hmdSensorOrientation = glm::quat_cast(hmdSensorMatrix);
 
-    const float STRAIGHTING_LEAN_DURATION = 0.5f;  // seconds
+    bool hmdIsAtRest = _hmdAtRestDetector.update(deltaTime, _hmdSensorPosition, _hmdSensorOrientation);
+
+    const float STRAIGHTENING_LEAN_DURATION = 0.5f;  // seconds
 
     // define a vertical capsule
-    const float STRAIGHTING_LEAN_CAPSULE_RADIUS = 0.2f;  // meters
-    const float STRAIGHTING_LEAN_CAPSULE_LENGTH = 0.05f;  // length of the cylinder part of the capsule in meters.
+    const float STRAIGHTENING_LEAN_CAPSULE_RADIUS = 0.2f;  // meters
+    const float STRAIGHTENING_LEAN_CAPSULE_LENGTH = 0.05f;  // length of the cylinder part of the capsule in meters.
 
     auto newBodySensorMatrix = deriveBodyFromHMDSensor();
     glm::vec3 diff = extractTranslation(newBodySensorMatrix) - extractTranslation(_bodySensorMatrix);
-    if (!_straightingLean && capsuleCheck(diff, STRAIGHTING_LEAN_CAPSULE_LENGTH, STRAIGHTING_LEAN_CAPSULE_RADIUS)) {
+    if (!_straighteningLean && (capsuleCheck(diff, STRAIGHTENING_LEAN_CAPSULE_LENGTH, STRAIGHTENING_LEAN_CAPSULE_RADIUS) || hmdIsAtRest)) {
 
         // begin homing toward derived body position.
-        _straightingLean = true;
-        _straightingLeanAlpha = 0.0f;
+        _straighteningLean = true;
+        _straighteningLeanAlpha = 0.0f;
 
-    } else if (_straightingLean) {
+    } else if (_straighteningLean) {
 
         auto newBodySensorMatrix = deriveBodyFromHMDSensor();
         auto worldBodyMatrix = _sensorToWorldMatrix * newBodySensorMatrix;
         glm::vec3 worldBodyPos = extractTranslation(worldBodyMatrix);
         glm::quat worldBodyRot = glm::normalize(glm::quat_cast(worldBodyMatrix));
 
-        _straightingLeanAlpha += (1.0f / STRAIGHTING_LEAN_DURATION) * deltaTime;
+        _straighteningLeanAlpha += (1.0f / STRAIGHTENING_LEAN_DURATION) * deltaTime;
 
-        if (_straightingLeanAlpha >= 1.0f) {
-            _straightingLean = false;
+        if (_straighteningLeanAlpha >= 1.0f) {
+            _straighteningLean = false;
             nextAttitude(worldBodyPos, worldBodyRot);
             _bodySensorMatrix = newBodySensorMatrix;
         } else {
             // interp position toward the desired pos
-            glm::vec3 pos = lerp(getPosition(), worldBodyPos, _straightingLeanAlpha);
-            glm::quat rot = glm::normalize(safeMix(getOrientation(), worldBodyRot, _straightingLeanAlpha));
+            glm::vec3 pos = lerp(getPosition(), worldBodyPos, _straighteningLeanAlpha);
+            glm::quat rot = glm::normalize(safeMix(getOrientation(), worldBodyRot, _straighteningLeanAlpha));
             nextAttitude(pos, rot);
 
             // interp sensor matrix toward desired
@@ -349,13 +352,13 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) {
             glm::quat nextBodyRot = glm::normalize(glm::quat_cast(newBodySensorMatrix));
             glm::vec3 prevBodyPos = extractTranslation(_bodySensorMatrix);
             glm::quat prevBodyRot = glm::normalize(glm::quat_cast(_bodySensorMatrix));
-            pos = lerp(prevBodyPos, nextBodyPos, _straightingLeanAlpha);
-            rot = glm::normalize(safeMix(prevBodyRot, nextBodyRot, _straightingLeanAlpha));
+            pos = lerp(prevBodyPos, nextBodyPos, _straighteningLeanAlpha);
+            rot = glm::normalize(safeMix(prevBodyRot, nextBodyRot, _straighteningLeanAlpha));
             _bodySensorMatrix = createMatFromQuatAndPos(rot, pos);
         }
     }
 }
-// 
+
 // best called at end of main loop, just before rendering.
 // update sensor to world matrix from current body position and hmd sensor.
 // This is so the correct camera can be used for rendering.

interface/src/avatar/MyAvatar.h

@@ -17,6 +17,7 @@
 #include <Rig.h>
 
 #include "Avatar.h"
+#include "AtRestDetector.h"
 
 class ModelItemID;
 
@@ -363,10 +364,11 @@ private:
     glm::vec3 _customListenPosition;
     glm::quat _customListenOrientation;
 
-    bool _straightingLean = false;
-    float _straightingLeanAlpha = 0.0f;
+    bool _straighteningLean = false;
+    float _straighteningLeanAlpha = 0.0f;
 
     quint64 _lastUpdateFromHMDTime = usecTimestampNow();
+    AtRestDetector _hmdAtRestDetector;
 };
 
 QScriptValue audioListenModeToScriptValue(QScriptEngine* engine, const AudioListenerMode& audioListenerMode);

libraries/shared/src/AtRestDetector.cpp

@@ -0,0 +1,41 @@
+#include "AtRestDetector.h"
+#include "SharedLogging.h"
+
+AtRestDetector::AtRestDetector(const glm::vec3& startPosition, const glm::quat& startRotation) {
+    reset(startPosition, startRotation);
+}
+
+void AtRestDetector::reset(const glm::vec3& startPosition, const glm::quat& startRotation) {
+    _positionAverage = startPosition;
+    _positionVariance = 0.0f;
+
+    glm::quat ql = glm::log(startRotation);
+    _quatLogAverage = glm::vec3(ql.x, ql.y, ql.z);
+    _quatLogVariance = 0.0f;
+}
+
+bool AtRestDetector::update(float dt, const glm::vec3& position, const glm::quat& rotation) {
+    const float TAU = 1.0f;
+    float delta = glm::min(dt / TAU, 1.0f);
+
+    // keep a running average of position.
+    _positionAverage = position * delta + _positionAverage * (1 - delta);
+
+    // keep a running average of position variances.
+    glm::vec3 dx = position - _positionAverage;
+    _positionVariance = glm::dot(dx, dx) * delta + _positionVariance * (1 - delta);
+
+    // keep a running average of quaternion logarithms.
+    glm::quat quatLogAsQuat = glm::log(rotation);
+    glm::vec3 quatLog(quatLogAsQuat.x, quatLogAsQuat.y, quatLogAsQuat.z);
+    _quatLogAverage = quatLog * delta + _quatLogAverage * (1 - delta);
+
+    // keep a running average of quatLog variances.
+    glm::vec3 dql = quatLog - _quatLogAverage;
+    _quatLogVariance = glm::dot(dql, dql) * delta + _quatLogVariance * (1 - delta);
+
+    const float POSITION_VARIANCE_THRESHOLD = 0.001f;
+    const float QUAT_LOG_VARIANCE_THRESHOLD = 0.00002f;
+
+    return _positionVariance < POSITION_VARIANCE_THRESHOLD && _quatLogVariance < QUAT_LOG_VARIANCE_THRESHOLD;
+}

libraries/shared/src/AtRestDetector.h

@@ -0,0 +1,34 @@
+//
+//  AtRestDetector.h
+//  libraries/shared/src
+//
+//  Created by Anthony Thibault on 10/6/2015
+//  Copyright 2015 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#ifndef hifi_AtRestDetector_h
+#define hifi_AtRestDetector_h
+
+#include <glm/glm.hpp>
+#include <glm/gtx/quaternion.hpp>
+
+class AtRestDetector {
+public:
+    AtRestDetector(const glm::vec3& startPosition, const glm::quat& startRotation);
+    void reset(const glm::vec3& startPosition, const glm::quat& startRotation);
+
+    // returns true if object is at rest, dt in assumed to be seconds.
+    bool update(float dt, const glm::vec3& position, const glm::quat& startRotation);
+
+protected:
+    glm::vec3 _positionAverage;
+    float _positionVariance;
+
+    glm::vec3 _quatLogAverage;
+    float _quatLogVariance;
+};
+
+#endif