//// AtRestDetector.cpp// 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// #include "AtRestDetector.h" #include "NumericalConstants.h"#include "SharedLogging.h"#include "SharedUtil.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; _lastUpdateTime = usecTimestampNow(); _isAtRest = false;} 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 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; _isAtRest = _positionVariance < POSITION_VARIANCE_THRESHOLD && _quatLogVariance < QUAT_LOG_VARIANCE_THRESHOLD; } else { reset(position, rotation); _isValid = true; }}
//
// AtRestDetector.cpp
// 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
//
#include "AtRestDetector.h"
#include "NumericalConstants.h"
#include "SharedLogging.h"
#include "SharedUtil.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;
_lastUpdateTime = usecTimestampNow();
_isAtRest = false;
}
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 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;
_isAtRest = _positionVariance < POSITION_VARIANCE_THRESHOLD && _quatLogVariance < QUAT_LOG_VARIANCE_THRESHOLD;
} else {
reset(position, rotation);
_isValid = true;
}
}