From 78f6d2e850e18415534eded1780b733960ee6f5a Mon Sep 17 00:00:00 2001
From: Dante Ruiz <dante@highfidelity.io>
Date: Thu, 18 May 2017 21:22:42 +0100
Subject: [PATCH] fix low velocity filter

---
 interface/resources/controllers/vive.json         |  6 ++++--
 .../impl/filters/LowVelocityFilter.cpp            | 15 ++++++++-------
 2 files changed, 12 insertions(+), 9 deletions(-)

diff --git a/interface/resources/controllers/vive.json b/interface/resources/controllers/vive.json
index 9ad7969217..07c4047e0e 100644
--- a/interface/resources/controllers/vive.json
+++ b/interface/resources/controllers/vive.json
@@ -45,17 +45,19 @@
         
         {
             "from": "Vive.RightFoot", "to" : "Standard.RightFoot",
-            "filters" : [{"type" :  "lowVelocity", "rotation" : 1.0, "translation": 1.0}]
-            ,"when": [ "Application.InHMD"]
+            "filters" : [{"type" :  "lowVelocity", "rotation" : 1.0, "translation": 1.0}],
+            "when": [ "Application.InHMD"]
         },
         
         {
             "from": "Vive.Hips", "to" : "Standard.Hips",
+            "filters" : [{"type" :  "lowVelocity", "rotation" : 0.01, "translation": 0.01}],
             "when": [ "Application.InHMD"]
         },
         
         {
             "from": "Vive.Spine2", "to" : "Standard.Spine2",
+            "filters" : [{"type" :  "lowVelocity", "rotation" : 0.01, "translation": 0.01}],
             "when": [ "Application.InHMD"]
         },
         
diff --git a/libraries/controllers/src/controllers/impl/filters/LowVelocityFilter.cpp b/libraries/controllers/src/controllers/impl/filters/LowVelocityFilter.cpp
index edb706482c..22db24274e 100644
--- a/libraries/controllers/src/controllers/impl/filters/LowVelocityFilter.cpp
+++ b/libraries/controllers/src/controllers/impl/filters/LowVelocityFilter.cpp
@@ -22,20 +22,21 @@ namespace controller {
     Pose LowVelocityFilter::apply(Pose newPose) const {
         auto userInputMapper = DependencyManager::get<UserInputMapper>();
         const InputCalibrationData calibrationData = userInputMapper->getInputCalibrationData();
+        glm::mat4 sensorToAvatarMat = glm::inverse(calibrationData.avatarMat) * calibrationData.sensorToWorldMat;
+        glm::mat4 avatarToSensorMat = glm::inverse(calibrationData.sensorToWorldMat) * calibrationData.avatarMat;
         Pose finalPose = newPose;
         if (finalPose.isValid() && _oldPose.isValid()) {
-            Pose sensorPose = finalPose.transform(calibrationData.avatarMat);
-            Pose lastPose = _oldPose.transform(calibrationData.avatarMat);
-            
+            Pose sensorPose = finalPose.transform(avatarToSensorMat);
+            Pose lastPose = _oldPose;
+
             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;
+            finalPose = sensorPose.transform(sensorToAvatarMat);
         }
-        _oldPose = finalPose;
+        _oldPose = finalPose.transform(avatarToSensorMat);
         return finalPose;
     }
 
@@ -51,5 +52,5 @@ namespace controller {
         }
         return false;
     }
-    
+
 }