Merge pull request #6011 from hyperlogic/tony/hmd-rest-detection

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

interface/src/avatar/MyAvatar.cpp

@@ -109,7 +109,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;
@@ -151,7 +152,7 @@ void MyAvatar::reset() {
     setEnableRigAnimations(true);
 
     // Reset dynamic state.
-    _wasPushing = _isPushing = _isBraking = _billboardValid = _goToPending = _straightingLean = false;
+    _wasPushing = _isPushing = _isBraking = _billboardValid = _goToPending = _straighteningLean = false;
     _skeletonModel.reset();
     getHead()->reset();
     _targetVelocity = glm::vec3(0.0f);
@@ -331,37 +332,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
@@ -369,13 +372,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;
 
@@ -360,10 +361,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