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