diff --git a/interface/resources/controllers/vive.json b/interface/resources/controllers/vive.json index 1757c30ae3..9ad7969217 100644 --- a/interface/resources/controllers/vive.json +++ b/interface/resources/controllers/vive.json @@ -39,25 +39,23 @@ { "from": "Vive.LeftFoot", "to" : "Standard.LeftFoot", - "filters" : [{"type" : "lowVelocity", "rotation" : 9.0, "translation": 9.0 }], + "filters" : [{"type" : "lowVelocity", "rotation" : 1.0, "translation": 1.0}], "when": [ "Application.InHMD"] }, { "from": "Vive.RightFoot", "to" : "Standard.RightFoot", - "filters" : [{"type" : "lowVelocity", "rotation" : 9.0, "translation": 9.0}] + "filters" : [{"type" : "lowVelocity", "rotation" : 1.0, "translation": 1.0}] ,"when": [ "Application.InHMD"] }, { "from": "Vive.Hips", "to" : "Standard.Hips", - "filters" : [{"type" : "lowVelocity", "rotation" : 10.0, "translation": 10.0}], "when": [ "Application.InHMD"] }, { "from": "Vive.Spine2", "to" : "Standard.Spine2", - "filters" : [{"type" : "lowVelocity", "rotation" : 10.0, "translation": 10.0}], "when": [ "Application.InHMD"] }, diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 7904447db0..ba962cef76 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -4376,6 +4376,7 @@ void Application::update(float deltaTime) { } } + userInputMapper->setInputCalibrationData(calibrationData); userInputMapper->update(deltaTime); if (keyboardMousePlugin && keyboardMousePlugin->isActive()) { diff --git a/libraries/controllers/src/controllers/UserInputMapper.h b/libraries/controllers/src/controllers/UserInputMapper.h index baa05f2f9f..05a286cc10 100644 --- a/libraries/controllers/src/controllers/UserInputMapper.h +++ b/libraries/controllers/src/controllers/UserInputMapper.h @@ -114,6 +114,9 @@ namespace controller { void loadDefaultMapping(uint16 deviceID); void enableMapping(const QString& mappingName, bool enable = true); + void setInputCalibrationData(const InputCalibrationData& data) { inputCalibrationData = data; } + const InputCalibrationData& getInputCalibrationData() { return inputCalibrationData; } + void unloadMappings(const QStringList& jsonFiles); void unloadMapping(const QString& jsonFile); @@ -187,6 +190,8 @@ namespace controller { QSet _loadedRouteJsonFiles; + InputCalibrationData inputCalibrationData; + mutable std::recursive_mutex _lock; }; diff --git a/libraries/controllers/src/controllers/impl/filters/LowVelocityFilter.cpp b/libraries/controllers/src/controllers/impl/filters/LowVelocityFilter.cpp index 594f538369..edb706482c 100644 --- a/libraries/controllers/src/controllers/impl/filters/LowVelocityFilter.cpp +++ b/libraries/controllers/src/controllers/impl/filters/LowVelocityFilter.cpp @@ -11,18 +11,29 @@ #include #include +#include "../../UserInputMapper.h" +#include "../../Input.h" +#include static const QString JSON_ROTATION = QStringLiteral("rotation"); static const QString JSON_TRANSLATION = QStringLiteral("translation"); namespace controller { Pose LowVelocityFilter::apply(Pose newPose) const { + auto userInputMapper = DependencyManager::get(); + const InputCalibrationData calibrationData = userInputMapper->getInputCalibrationData(); Pose finalPose = newPose; if (finalPose.isValid() && _oldPose.isValid()) { - float rotationFilter = glm::clamp(1.0f - (glm::length(_oldPose.getVelocity() / _rotationConstant)), 0.0f, 1.0f); - float translationFilter = glm::clamp(1.0f - (glm::length(_oldPose.getVelocity() / _translationConstant)), 0.0f, 1.0f); - finalPose.translation = _oldPose.getTranslation() * translationFilter + newPose.getTranslation() * (1.0f - translationFilter); - finalPose.rotation = safeMix(_oldPose.getRotation(), newPose.getRotation(), 1.0f - rotationFilter); + Pose sensorPose = finalPose.transform(calibrationData.avatarMat); + Pose lastPose = _oldPose.transform(calibrationData.avatarMat); + + float rotationFilter = glm::clamp(1.0f - (glm::length(lastPose.getVelocity() / _rotationConstant)), 0.0f, 1.0f); + float translationFilter = glm::clamp(1.0f - (glm::length(lastPose.getVelocity() / _translationConstant)), 0.0f, 1.0f); + sensorPose.translation = lastPose.getTranslation() * translationFilter + sensorPose.getTranslation() * (1.0f - translationFilter); + sensorPose.rotation = safeMix(lastPose.getRotation(), sensorPose.getRotation(), 1.0f - rotationFilter); + + finalPose = sensorPose.transform(glm::inverse(calibrationData.avatarMat)); + finalPose.valid = true; } _oldPose = finalPose; return finalPose;