From efb3edf4006de207c23a9ad65de4355eccb024c1 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Mon, 12 May 2014 11:39:21 -0700
Subject: [PATCH] Store sixense data in meters not millimeters

---
 interface/src/devices/SixenseManager.cpp | 22 ++++++++++++++--------
 libraries/avatars/src/HandData.cpp       |  2 +-
 libraries/avatars/src/HandData.h         |  4 +---
 3 files changed, 16 insertions(+), 12 deletions(-)

diff --git a/interface/src/devices/SixenseManager.cpp b/interface/src/devices/SixenseManager.cpp
index 461b414b2b..1698cebcf9 100644
--- a/interface/src/devices/SixenseManager.cpp
+++ b/interface/src/devices/SixenseManager.cpp
@@ -22,9 +22,9 @@ const int CALIBRATION_STATE_Z = 3;
 const int CALIBRATION_STATE_COMPLETE = 4;
 
 // default (expected) location of neck in sixense space
-const float NECK_X = 250.f;   // millimeters
-const float NECK_Y = 300.f;   // millimeters
-const float NECK_Z = 300.f;   // millimeters
+const float NECK_X = 0.25f; // meters
+const float NECK_Y = 0.3f;  // meters
+const float NECK_Z = 0.3f;  // meters
 #endif
 
 SixenseManager::SixenseManager() {
@@ -107,7 +107,10 @@ void SixenseManager::update(float deltaTime) {
         palm->setTrigger(data->trigger);
         palm->setJoystick(data->joystick_x, data->joystick_y);
  
+        // NOTE: Sixense API returns pos data in millimeters but we IMMEDIATELY convert to meters.
         glm::vec3 position(data->pos[0], data->pos[1], data->pos[2]);
+        position *= METERS_PER_MILLIMETER;
+
         // Transform the measured position into body frame.  
         glm::vec3 neck = _neckBase;
         // Zeroing y component of the "neck" effectively raises the measured position a little bit.
@@ -122,7 +125,7 @@ void SixenseManager::update(float deltaTime) {
         //  Compute current velocity from position change
         glm::vec3 rawVelocity;
         if (deltaTime > 0.f) {
-            rawVelocity = (position - palm->getRawPosition()) * (METERS_PER_MILLIMETER / deltaTime); 
+            rawVelocity = (position - palm->getRawPosition()) / deltaTime; 
         } else {
             rawVelocity = glm::vec3(0.0f);
         }
@@ -138,12 +141,12 @@ void SixenseManager::update(float deltaTime) {
         }
         
         // Store the one fingertip in the palm structure so we can track velocity
-        const float FINGER_LENGTH = 300.0f;   //  meters
+        const float FINGER_LENGTH = 0.3f;   //  meters
         const glm::vec3 FINGER_VECTOR(0.0f, 0.0f, FINGER_LENGTH);
         const glm::vec3 newTipPosition = position + rotation * FINGER_VECTOR;
         glm::vec3 oldTipPosition = palm->getTipRawPosition();
         if (deltaTime > 0.f) {
-            palm->setTipVelocity((newTipPosition - oldTipPosition) * (METERS_PER_MILLIMETER / deltaTime));
+            palm->setTipVelocity((newTipPosition - oldTipPosition) / deltaTime);
         } else {
             palm->setTipVelocity(glm::vec3(0.f));
         }
@@ -173,8 +176,8 @@ void SixenseManager::update(float deltaTime) {
 // (4) move arms a bit forward (Z)
 // (5) release BUTTON_FWD on both hands
 
-const float MINIMUM_ARM_REACH = 300.f;   // millimeters
-const float MAXIMUM_NOISE_LEVEL = 50.f;  // millimeters
+const float MINIMUM_ARM_REACH = 0.3f; // meters
+const float MAXIMUM_NOISE_LEVEL = 0.05f; // meters
 const quint64 LOCK_DURATION = USECS_PER_SECOND / 4;     // time for lock to be acquired
 
 void SixenseManager::updateCalibration(const sixenseControllerData* controllers) {
@@ -214,10 +217,13 @@ void SixenseManager::updateCalibration(const sixenseControllerData* controllers)
         return;
     }
 
+    // NOTE: Sixense API returns pos data in millimeters but we IMMEDIATELY convert to meters.
     const float* pos = dataLeft->pos;
     glm::vec3 positionLeft(pos[0], pos[1], pos[2]);
+    positionLeft *= METERS_PER_MILLIMETER;
     pos = dataRight->pos;
     glm::vec3 positionRight(pos[0], pos[1], pos[2]);
+    positionRight *= METERS_PER_MILLIMETER;
 
     if (_calibrationState == CALIBRATION_STATE_IDLE) {
         float reach = glm::distance(positionLeft, positionRight);
diff --git a/libraries/avatars/src/HandData.cpp b/libraries/avatars/src/HandData.cpp
index 109eab5c5d..4dad06fa84 100644
--- a/libraries/avatars/src/HandData.cpp
+++ b/libraries/avatars/src/HandData.cpp
@@ -27,7 +27,7 @@ HandData::HandData(AvatarData* owningAvatar) :
 }
 
 glm::vec3 HandData::worldToLocalVector(const glm::vec3& worldVector) const {
-    return glm::inverse(getBaseOrientation()) * worldVector / LEAP_UNIT_SCALE;
+    return glm::inverse(getBaseOrientation()) * worldVector;
 }
 
 PalmData& HandData::addNewPalm()  {
diff --git a/libraries/avatars/src/HandData.h b/libraries/avatars/src/HandData.h
index 968d7bbe38..d1e11aed8e 100755
--- a/libraries/avatars/src/HandData.h
+++ b/libraries/avatars/src/HandData.h
@@ -30,8 +30,6 @@ const int NUM_FINGERS = NUM_HANDS * NUM_FINGERS_PER_HAND;
 const int LEAPID_INVALID = -1;
 const int SIXENSEID_INVALID = -1;
 
-const float LEAP_UNIT_SCALE = 0.001f; ///< convert mm to meters
-
 const int SIXENSE_CONTROLLER_ID_LEFT_HAND = 0;
 const int SIXENSE_CONTROLLER_ID_RIGHT_HAND = 1;
 
@@ -42,7 +40,7 @@ public:
     
     // position conversion
     glm::vec3 localToWorldPosition(const glm::vec3& localPosition) {
-        return getBasePosition() + getBaseOrientation() * localPosition * LEAP_UNIT_SCALE;
+        return getBasePosition() + getBaseOrientation() * localPosition;
     }
 
     glm::vec3 localToWorldDirection(const glm::vec3& localVector) {