From 2681312b54757fd2ef0b938b81d34149ce53cb3f Mon Sep 17 00:00:00 2001
From: Philip Rosedale <philip@highfidelity.io>
Date: Fri, 23 May 2014 00:09:15 -0700
Subject: [PATCH] Velocity damped hand positions

---
 examples/hydraMove.js                    |  2 +-
 interface/src/devices/SixenseManager.cpp | 10 +++++++---
 2 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/examples/hydraMove.js b/examples/hydraMove.js
index 60823ffb36..bd462f5e38 100644
--- a/examples/hydraMove.js
+++ b/examples/hydraMove.js
@@ -50,7 +50,7 @@ var JOYSTICK_PITCH_MAG = PITCH_MAG * 0.5;
 
 
 var LEFT_PALM = 0;
-var LEFT_BUTTON_4 = 5;
+var LEFT_BUTTON_4 = 4;
 var LEFT_BUTTON_FWD = 5;
 var RIGHT_PALM = 2;
 var RIGHT_BUTTON_4 = 10;
diff --git a/interface/src/devices/SixenseManager.cpp b/interface/src/devices/SixenseManager.cpp
index 547642a1be..d2784961bd 100644
--- a/interface/src/devices/SixenseManager.cpp
+++ b/interface/src/devices/SixenseManager.cpp
@@ -120,7 +120,6 @@ void SixenseManager::update(float deltaTime) {
         //  Rotation of Palm
         glm::quat rotation(data->rot_quat[3], -data->rot_quat[0], data->rot_quat[1], -data->rot_quat[2]);
         rotation = glm::angleAxis(PI, glm::vec3(0.f, 1.f, 0.f)) * _orbRotation * rotation;
-        palm->setRawRotation(rotation);
         
         //  Compute current velocity from position change
         glm::vec3 rawVelocity;
@@ -130,8 +129,13 @@ void SixenseManager::update(float deltaTime) {
             rawVelocity = glm::vec3(0.0f);
         }
         palm->setRawVelocity(rawVelocity);   //  meters/sec
-        palm->setRawPosition(position);
-        
+    
+        //  Use a velocity sensitive filter to damp small motions and preserve large ones with
+        //  no latency.
+        float velocityFilter = glm::clamp(1.0f - glm::length(rawVelocity), 0.0f, 1.0f);
+        palm->setRawPosition(palm->getRawPosition() * velocityFilter + position * (1.0f - velocityFilter));
+        palm->setRawRotation(safeMix(palm->getRawRotation(), rotation, 1.0f - velocityFilter));
+ 
         // use the velocity to determine whether there's any movement (if the hand isn't new)
         const float MOVEMENT_DISTANCE_THRESHOLD = 0.003f;
         _amountMoved += rawVelocity * deltaTime;