From a3a87ef48b1960b929eafef3adde9ae9c82f7af5 Mon Sep 17 00:00:00 2001
From: Anthony Thibault <tony@highfidelity.io>
Date: Wed, 10 Oct 2018 17:52:26 -0700
Subject: [PATCH] Debug rendering of tracked objects, remove filters from body.

---
 interface/resources/controllers/vive.json     | 29 ++++----------
 interface/src/Application.cpp                 | 38 +++++++++++++++++++
 .../filters/ExponentialSmoothingFilter.cpp    |  2 +-
 3 files changed, 46 insertions(+), 23 deletions(-)

diff --git a/interface/resources/controllers/vive.json b/interface/resources/controllers/vive.json
index 8a7744efb3..42be6f3f04 100644
--- a/interface/resources/controllers/vive.json
+++ b/interface/resources/controllers/vive.json
@@ -51,30 +51,15 @@
         { "from": "Vive.RSCenter", "to": "Standard.RightPrimaryThumb" },
         { "from": "Vive.RightApplicationMenu", "to": "Standard.RightSecondaryThumb" },
 
-        { "from": "Vive.LeftHand", "to": "Standard.LeftHand"},
-        { "from": "Vive.RightHand", "to": "Standard.RightHand"},
+        { "from": "Vive.LeftHand", "to": "Standard.LeftHand" },
+        { "from": "Vive.RightHand", "to": "Standard.RightHand" },
 
-        {
-            "from": "Vive.LeftFoot", "to" : "Standard.LeftFoot",
-            "filters" : [{"type" :  "exponentialSmoothing", "rotation" : 0.15, "translation": 0.3}]
-        },
+        { "from": "Vive.LeftFoot", "to" : "Standard.LeftFoot" },
+        { "from": "Vive.RightFoot", "to" : "Standard.RightFoot" },
+        { "from": "Vive.Hips", "to" : "Standard.Hips" },
+        { "from": "Vive.Spine2", "to" : "Standard.Spine2" },
 
-        {
-            "from": "Vive.RightFoot", "to" : "Standard.RightFoot",
-            "filters" : [{"type" :  "exponentialSmoothing", "rotation" : 0.15, "translation": 0.3}]
-        },
-
-        {
-            "from": "Vive.Hips", "to" : "Standard.Hips",
-            "filters" : [{"type" :  "exponentialSmoothing", "rotation" : 0.15, "translation": 0.3}]
-        },
-
-        {
-            "from": "Vive.Spine2", "to" : "Standard.Spine2",
-            "filters" : [{"type" :  "exponentialSmoothing", "rotation" : 0.15, "translation": 0.3}]
-        },
-
-        { "from": "Vive.Head", "to" : "Standard.Head"},
+        { "from": "Vive.Head", "to" : "Standard.Head" },
         { "from": "Vive.RightArm", "to" : "Standard.RightArm" },
         { "from": "Vive.LeftArm", "to" : "Standard.LeftArm" },
 
diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index aa2b382c58..7457afe091 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -5801,6 +5801,44 @@ void Application::update(float deltaTime) {
             controller::Pose pose = userInputMapper->getPoseState(action);
             myAvatar->setControllerPoseInSensorFrame(action, pose.transform(avatarToSensorMatrix));
         }
+
+        // AJT: TODO put a nice menu around this.
+        // Make sure to remove all markers when menu is turned off.
+        {
+            static const std::vector<controller::Action> trackedObjectActions = {
+                controller::Action::TRACKED_OBJECT_00,
+                controller::Action::TRACKED_OBJECT_01,
+                controller::Action::TRACKED_OBJECT_02,
+                controller::Action::TRACKED_OBJECT_03,
+                controller::Action::TRACKED_OBJECT_04,
+                controller::Action::TRACKED_OBJECT_05,
+                controller::Action::TRACKED_OBJECT_06,
+                controller::Action::TRACKED_OBJECT_07,
+                controller::Action::TRACKED_OBJECT_08,
+                controller::Action::TRACKED_OBJECT_09,
+                controller::Action::TRACKED_OBJECT_10,
+                controller::Action::TRACKED_OBJECT_11,
+                controller::Action::TRACKED_OBJECT_12,
+                controller::Action::TRACKED_OBJECT_13,
+                controller::Action::TRACKED_OBJECT_14,
+                controller::Action::TRACKED_OBJECT_15
+            };
+
+            int i = 0;
+            glm::vec4 BLUE(0.0f, 0.0f, 1.0f, 1.0f);
+            for (auto& action : trackedObjectActions) {
+                QString key = QString("_TrackedObject%1").arg(i);
+                controller::Pose pose = userInputMapper->getPoseState(action);
+                if (pose.valid) {
+                    glm::vec3 pos = transformPoint(myAvatarMatrix, pose.translation);
+                    glm::quat rot = glmExtractRotation(myAvatarMatrix) * pose.rotation;
+                    DebugDraw::getInstance().addMarker(key, rot, pos, BLUE);
+                } else {
+                    DebugDraw::getInstance().removeMarker(key);
+                }
+                i++;
+            }
+        }
     }
 
     updateThreads(deltaTime); // If running non-threaded, then give the threads some time to process...
diff --git a/libraries/controllers/src/controllers/impl/filters/ExponentialSmoothingFilter.cpp b/libraries/controllers/src/controllers/impl/filters/ExponentialSmoothingFilter.cpp
index 9cf2673d55..0f204ce15f 100644
--- a/libraries/controllers/src/controllers/impl/filters/ExponentialSmoothingFilter.cpp
+++ b/libraries/controllers/src/controllers/impl/filters/ExponentialSmoothingFilter.cpp
@@ -35,7 +35,7 @@ namespace controller {
             if (_prevSensorValue.isValid()) {
                 // exponential smoothing filter
                 sensorValue.translation = _translationConstant * sensorValue.getTranslation() + (1.0f - _translationConstant) * _prevSensorValue.getTranslation();
-                sensorValue.rotation = safeMix(sensorValue.getRotation(), _prevSensorValue.getRotation(), _rotationConstant);
+                sensorValue.rotation = safeMix(sensorValue.getRotation(), _prevSensorValue.getRotation(), (1.0f - _rotationConstant));
 
                 // remember previous sensor space value.
                 _prevSensorValue = sensorValue;