diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index c61b424bce..a7c8c3e179 100755 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -122,7 +122,6 @@ MyAvatar::MyAvatar(QThread* thread) : _goToOrientation(), _prevShouldDrawHead(true), _audioListenerMode(FROM_HEAD), - _hmdAtRestDetector(glm::vec3(0), glm::quat()), _dominantHandSetting(QStringList() << AVATAR_SETTINGS_GROUP_NAME << "dominantHand", DOMINANT_RIGHT_HAND), _headPitchSetting(QStringList() << AVATAR_SETTINGS_GROUP_NAME << "", 0.0f), _scaleSetting(QStringList() << AVATAR_SETTINGS_GROUP_NAME << "scale", _targetScale), @@ -702,6 +701,46 @@ void MyAvatar::simulate(float deltaTime) { // before we perform rig animations and IK. updateSensorToWorldMatrix(); + // if we detect the hand controller is at rest, i.e. lying on the table, or the hand is too far away from the hmd + // disable the associated hand controller input. + { + // NOTE: all poses are in sensor space. + auto leftHandIter = _controllerPoseMap.find(controller::Action::LEFT_HAND); + if (leftHandIter != _controllerPoseMap.end() && leftHandIter->second.isValid()) { + _leftHandAtRestDetector.update(leftHandIter->second.getTranslation(), leftHandIter->second.getRotation()); + if (_leftHandAtRestDetector.isAtRest()) { + leftHandIter->second.valid = false; + } + } else { + _leftHandAtRestDetector.invalidate(); + } + + auto rightHandIter = _controllerPoseMap.find(controller::Action::RIGHT_HAND); + if (rightHandIter != _controllerPoseMap.end() && rightHandIter->second.isValid()) { + _rightHandAtRestDetector.update(rightHandIter->second.getTranslation(), rightHandIter->second.getRotation()); + if (_rightHandAtRestDetector.isAtRest()) { + rightHandIter->second.valid = false; + } + } else { + _rightHandAtRestDetector.invalidate(); + } + + auto headIter = _controllerPoseMap.find(controller::Action::HEAD); + + // The 99th percentile man has a spine to fingertip to height ratio of 0.45. Lets increase that by about 10% to 0.5 + // then measure the distance the center of the eyes to the finger tips. To come up with this ratio. + // From "The Measure of Man and Woman: Human Factors in Design, Revised Edition" by Alvin R. Tilley, Henry Dreyfuss Associates + const float MAX_HEAD_TO_HAND_DISTANCE_RATIO = 0.52f; + + float maxHeadHandDistance = getUserHeight() * MAX_HEAD_TO_HAND_DISTANCE_RATIO; + if (glm::length(headIter->second.getTranslation() - leftHandIter->second.getTranslation()) > maxHeadHandDistance) { + leftHandIter->second.valid = false; + } + if (glm::length(headIter->second.getTranslation() - rightHandIter->second.getTranslation()) > maxHeadHandDistance) { + rightHandIter->second.valid = false; + } + } + { PerformanceTimer perfTimer("skeleton"); diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 980aca59ce..485cab8f42 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -1764,8 +1764,8 @@ private: glm::vec3 _customListenPosition; glm::quat _customListenOrientation; - AtRestDetector _hmdAtRestDetector; - bool _lastIsMoving { false }; + AtRestDetector _leftHandAtRestDetector; + AtRestDetector _rightHandAtRestDetector; // all poses are in sensor-frame std::map _controllerPoseMap; diff --git a/libraries/shared/src/AtRestDetector.cpp b/libraries/shared/src/AtRestDetector.cpp index 4c08b30dcd..4b4356d641 100644 --- a/libraries/shared/src/AtRestDetector.cpp +++ b/libraries/shared/src/AtRestDetector.cpp @@ -30,32 +30,37 @@ void AtRestDetector::reset(const glm::vec3& startPosition, const glm::quat& star _isAtRest = false; } -bool AtRestDetector::update(const glm::vec3& position, const glm::quat& rotation) { - uint64_t now = usecTimestampNow(); - float dt = (float)(now - _lastUpdateTime) / (float)USECS_PER_SECOND; - _lastUpdateTime = now; - const float TAU = 1.0f; - float delta = glm::min(dt / TAU, 1.0f); +void AtRestDetector::update(const glm::vec3& position, const glm::quat& rotation) { + _lastIsAtRest = _isAtRest; + if (_isValid) { + uint64_t now = usecTimestampNow(); + float dt = (float)(now - _lastUpdateTime) / (float)USECS_PER_SECOND; + _lastUpdateTime = now; + 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. + _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 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 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); + // 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; + const float POSITION_VARIANCE_THRESHOLD = 0.001f; + const float QUAT_LOG_VARIANCE_THRESHOLD = 0.00002f; - _isAtRest = _positionVariance < POSITION_VARIANCE_THRESHOLD && _quatLogVariance < QUAT_LOG_VARIANCE_THRESHOLD; - return _isAtRest; + _isAtRest = _positionVariance < POSITION_VARIANCE_THRESHOLD && _quatLogVariance < QUAT_LOG_VARIANCE_THRESHOLD; + } else { + reset(position, rotation); + _isValid = true; + } } diff --git a/libraries/shared/src/AtRestDetector.h b/libraries/shared/src/AtRestDetector.h index 525156de65..eb8e5f53f9 100644 --- a/libraries/shared/src/AtRestDetector.h +++ b/libraries/shared/src/AtRestDetector.h @@ -17,21 +17,27 @@ class AtRestDetector { public: + AtRestDetector() {}; 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(const glm::vec3& position, const glm::quat& startRotation); + void update(const glm::vec3& position, const glm::quat& startRotation); + void invalidate() { _isValid = false; } bool isAtRest() const { return _isAtRest; } + bool onRest() const { return !_lastIsAtRest && _isAtRest; } + bool onWake() const { return _lastIsAtRest && !_isAtRest; } protected: + bool _isValid { false }; glm::vec3 _positionAverage; glm::vec3 _quatLogAverage; uint64_t _lastUpdateTime { 0 }; float _positionVariance { 0.0f }; float _quatLogVariance { 0.0f }; bool _isAtRest { false }; + bool _lastIsAtRest { false }; }; #endif