From 2dabe69341e234aaba12904b72c7402390ea8100 Mon Sep 17 00:00:00 2001 From: "Anthony J. Thibault" Date: Tue, 6 Oct 2015 20:30:16 -0700 Subject: [PATCH] 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 | 35 +++++++++++---------- interface/src/avatar/MyAvatar.h | 6 ++-- libraries/shared/src/AtRestDetector.cpp | 41 +++++++++++++++++++++++++ libraries/shared/src/AtRestDetector.h | 34 ++++++++++++++++++++ 4 files changed, 98 insertions(+), 18 deletions(-) create mode 100644 libraries/shared/src/AtRestDetector.cpp create mode 100644 libraries/shared/src/AtRestDetector.h diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 0105d1b648..371ce549e6 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/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. diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 5d87737dd7..202079b405 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -17,6 +17,7 @@ #include #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); diff --git a/libraries/shared/src/AtRestDetector.cpp b/libraries/shared/src/AtRestDetector.cpp new file mode 100644 index 0000000000..5e623a005c --- /dev/null +++ b/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; +} diff --git a/libraries/shared/src/AtRestDetector.h b/libraries/shared/src/AtRestDetector.h new file mode 100644 index 0000000000..d82e54a692 --- /dev/null +++ b/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 +#include + +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