From d74856e01b50737a44d900299974187b901ab925 Mon Sep 17 00:00:00 2001
From: Philip Rosedale <philip@highfidelity.io>
Date: Tue, 24 Feb 2015 11:21:04 -0800
Subject: [PATCH] add angularVelocity to controller

---
 interface/src/devices/SixenseManager.cpp          | 12 ++++++++++++
 .../scripting/ControllerScriptingInterface.cpp    | 15 +++++++++++++++
 .../src/scripting/ControllerScriptingInterface.h  |  1 +
 libraries/avatars/src/HandData.cpp                |  2 +-
 libraries/avatars/src/HandData.h                  |  4 +++-
 5 files changed, 32 insertions(+), 2 deletions(-)

diff --git a/interface/src/devices/SixenseManager.cpp b/interface/src/devices/SixenseManager.cpp
index ffaa5260db..609ac7b349 100644
--- a/interface/src/devices/SixenseManager.cpp
+++ b/interface/src/devices/SixenseManager.cpp
@@ -260,6 +260,18 @@ void SixenseManager::update(float deltaTime) {
             float sign = (i == 0) ? -1.0f : 1.0f;
             rotation *= glm::angleAxis(sign * PI/4.0f, glm::vec3(0.0f, 0.0f, 1.0f));
             
+            //  Angular Velocity of Palm
+            glm::quat deltaRotation = rotation * glm::inverse(palm->getRawRotation());
+            glm::vec3 angularVelocity(0.0f);
+            float rotationAngle = glm::angle(deltaRotation);
+            if ((rotationAngle > EPSILON) && (deltaTime > 0.0f)) {
+                angularVelocity = glm::normalize(glm::axis(deltaRotation));
+                angularVelocity *= (rotationAngle / deltaTime);
+                palm->setRawAngularVelocity(angularVelocity);
+            } else {
+                palm->setRawAngularVelocity(glm::vec3(0.0f));
+            }
+            
             if (_lowVelocityFilter) {
                 //  Use a velocity sensitive filter to damp small motions and preserve large ones with
                 //  no latency.
diff --git a/interface/src/scripting/ControllerScriptingInterface.cpp b/interface/src/scripting/ControllerScriptingInterface.cpp
index 929a763e70..2188f45374 100644
--- a/interface/src/scripting/ControllerScriptingInterface.cpp
+++ b/interface/src/scripting/ControllerScriptingInterface.cpp
@@ -208,6 +208,21 @@ glm::quat ControllerScriptingInterface::getSpatialControlRawRotation(int control
     }
     return glm::quat(); // bad index
 }
+    
+glm::vec3 ControllerScriptingInterface::getSpatialControlRawAngularVelocity(int controlIndex) const {
+    int palmIndex = controlIndex / NUMBER_OF_SPATIALCONTROLS_PER_PALM;
+    int controlOfPalm = controlIndex % NUMBER_OF_SPATIALCONTROLS_PER_PALM;
+    const PalmData* palmData = getActivePalm(palmIndex);
+    if (palmData) {
+        switch (controlOfPalm) {
+            case PALM_SPATIALCONTROL:
+                return palmData->getRawAngularVelocity();
+            case TIP_SPATIALCONTROL:
+                return palmData->getRawAngularVelocity();  //  Tip = palm angular velocity        
+        }
+    }
+    return glm::vec3(0); // bad index
+}
 
 glm::vec3 ControllerScriptingInterface::getSpatialControlNormal(int controlIndex) const {
     int palmIndex = controlIndex / NUMBER_OF_SPATIALCONTROLS_PER_PALM;
diff --git a/interface/src/scripting/ControllerScriptingInterface.h b/interface/src/scripting/ControllerScriptingInterface.h
index d56d159c06..c088dd6c9a 100644
--- a/interface/src/scripting/ControllerScriptingInterface.h
+++ b/interface/src/scripting/ControllerScriptingInterface.h
@@ -96,6 +96,7 @@ public slots:
     virtual glm::vec3 getSpatialControlVelocity(int controlIndex) const;
     virtual glm::vec3 getSpatialControlNormal(int controlIndex) const;
     virtual glm::quat getSpatialControlRawRotation(int controlIndex) const;
+    virtual glm::vec3 getSpatialControlRawAngularVelocity(int controlIndex) const;
     virtual void captureKeyEvents(const KeyEvent& event);
     virtual void releaseKeyEvents(const KeyEvent& event);
 
diff --git a/libraries/avatars/src/HandData.cpp b/libraries/avatars/src/HandData.cpp
index 5d850d06d0..9a9b51c1c8 100644
--- a/libraries/avatars/src/HandData.cpp
+++ b/libraries/avatars/src/HandData.cpp
@@ -67,7 +67,7 @@ PalmData::PalmData(HandData* owningHandData) :
 _rawRotation(0.0f, 0.0f, 0.0f, 1.0f),
 _rawPosition(0.0f),
 _rawVelocity(0.0f),
-_rotationalVelocity(0.0f),
+_rawAngularVelocity(0.0f),
 _totalPenetration(0.0f),
 _controllerButtons(0),
 _isActive(false),
diff --git a/libraries/avatars/src/HandData.h b/libraries/avatars/src/HandData.h
index 534ea67726..a5e2b2907e 100644
--- a/libraries/avatars/src/HandData.h
+++ b/libraries/avatars/src/HandData.h
@@ -98,6 +98,8 @@ public:
     void setRawPosition(const glm::vec3& pos)  { _rawPosition = pos; }
     void setRawVelocity(const glm::vec3& velocity) { _rawVelocity = velocity; }
     const glm::vec3& getRawVelocity()  const { return _rawVelocity; }
+    void setRawAngularVelocity(const glm::vec3& angularVelocity) { _rawAngularVelocity = angularVelocity; }
+    const glm::vec3& getRawAngularVelocity() const { return _rawAngularVelocity; }
     void addToPosition(const glm::vec3& delta);
 
     void addToPenetration(const glm::vec3& penetration) { _totalPenetration += penetration; }
@@ -148,7 +150,7 @@ private:
     glm::quat _rawRotation;
     glm::vec3 _rawPosition;
     glm::vec3 _rawVelocity;
-    glm::vec3 _rotationalVelocity;
+    glm::vec3 _rawAngularVelocity;
     glm::quat _lastRotation;
     
     glm::vec3 _tipPosition;