From e929e3f789f94b4305e3801c1a3fb74755b6d3c0 Mon Sep 17 00:00:00 2001
From: Brad Hefta-Gaub <brad@highfidelity.io>
Date: Fri, 18 Dec 2015 19:18:39 -0800
Subject: [PATCH] more work on angular velocity tests

---
 ...t.js => reticleHandAngularVelocityTest.js} | 60 +++++++++++++------
 libraries/script-engine/src/Quat.cpp          |  4 ++
 libraries/script-engine/src/Quat.h            |  1 +
 3 files changed, 48 insertions(+), 17 deletions(-)
 rename examples/controllers/{reticleHandTest.js => reticleHandAngularVelocityTest.js} (57%)

diff --git a/examples/controllers/reticleHandTest.js b/examples/controllers/reticleHandAngularVelocityTest.js
similarity index 57%
rename from examples/controllers/reticleHandTest.js
rename to examples/controllers/reticleHandAngularVelocityTest.js
index 4132c80d85..94288b7bfb 100644
--- a/examples/controllers/reticleHandTest.js
+++ b/examples/controllers/reticleHandAngularVelocityTest.js
@@ -1,5 +1,5 @@
 //
-//  reticleTest.js
+//  reticleHandAngularVelocityTest.js
 //  examples/controllers
 //
 //  Created by Brad Hefta-Gaub on 2015/12/15
@@ -9,6 +9,14 @@
 //  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
 //
 
+// If you set this to true, you will get the raw instantaneous angular velocity. 
+// note: there is a LOT of noise in the hydra rotation, you will probably be very
+// frustrated with the level of jitter.
+var USE_INSTANTANEOUS_ANGULAR_VELOCITY = false;
+var whichHand = Controller.Standard.RightHand;
+var whichTrigger = Controller.Standard.RT;
+
+
 
 function msecTimestampNow() {
     var d = new Date();
@@ -30,7 +38,7 @@ function moveReticle(dX, dY) {
     // some debugging to see if position is jumping around on us...
     var distanceSinceLastMove = length(lastPos, globalPos);
     if (distanceSinceLastMove > EXPECTED_CHANGE) {
-        print("distanceSinceLastMove:" + distanceSinceLastMove + "----------------------------");
+        print("------------------ distanceSinceLastMove:" + distanceSinceLastMove + "----------------------------");
     }
 
     if (Math.abs(dX) > EXPECTED_CHANGE) {
@@ -48,12 +56,25 @@ function moveReticle(dX, dY) {
     lastPos = globalPos;
 }
 
+var firstTime = true;
 var lastTime = msecTimestampNow();
+var previousRotation;
 
 var MAPPING_NAME = "com.highfidelity.testing.reticleWithHand";
 var mapping = Controller.newMapping(MAPPING_NAME);
-mapping.from(Controller.Standard.RightHand).peek().to(function(pose) {
+mapping.from(whichTrigger).peek().constrainToInteger().to(Controller.Actions.ReticleClick);
+mapping.from(whichHand).peek().to(function(pose) {
 
+    var MSECS_PER_SECOND = 1000;
+    var now = msecTimestampNow();
+    var deltaMsecs =  (now - lastTime);
+    var deltaTime = deltaMsecs / MSECS_PER_SECOND;
+
+    if (firstTime) {
+        previousRotation = pose.rotation;
+        lastTime = msecTimestampNow();
+        firstTime = false;
+    }
 
     // pose.angularVelocity - is the angularVelocity in a "physics" sense, that
     // means the direction of the vector is the axis of symetry of rotation
@@ -67,29 +88,34 @@ mapping.from(Controller.Standard.RightHand).peek().to(function(pose) {
     var xPart = -pose.angularVelocity.y;
     var yPart = -pose.angularVelocity.x;
 
+    // pose.angularVelocity is "smoothed", we can calculate our own instantaneous
+    // angular velocity as such:
+    if (USE_INSTANTANEOUS_ANGULAR_VELOCITY) {
+        var previousConjugate = Quat.conjugate(previousRotation);
+        var deltaRotation = Quat.multiply(pose.rotation, previousConjugate);
+        var normalizedDeltaRotation = Quat.normalize(deltaRotation);
+        var axis = Quat.axis(normalizedDeltaRotation);
+        var speed = Quat.angle(normalizedDeltaRotation) / deltaTime;
+        var instantaneousAngularVelocity = Vec3.multiply(speed, axis);
+
+        xPart = -instantaneousAngularVelocity.y;
+        yPart = -instantaneousAngularVelocity.x;
+
+        previousRotation = pose.rotation;
+    }
 
     var MOVE_SCALE = 1;
-    var MSECS_PER_SECOND = 1000;
-    var now = msecTimestampNow();
-    var secondsSinceLast = (now - lastTime) / MSECS_PER_SECOND;
     lastTime = now;
 
-    //print("secondsSinceLast:" + secondsSinceLast);
-
-    //print("x part:" + xPart);
-    //print("y part:" + yPart);
-
-    var dX = (xPart * MOVE_SCALE) / secondsSinceLast;
-    var dY = (yPart * MOVE_SCALE) / secondsSinceLast;
-
-    //print("dX:" + dX);
-    //print("dY:" + dY);
+    var dX = (xPart * MOVE_SCALE) / deltaTime;
+    var dY = (yPart * MOVE_SCALE) / deltaTime;
 
     moveReticle(dX, dY);
 });
 mapping.enable();
 
-
 Script.scriptEnding.connect(function(){
     mapping.disable();
 });
+
+
diff --git a/libraries/script-engine/src/Quat.cpp b/libraries/script-engine/src/Quat.cpp
index bb74b20be0..f6b1726770 100644
--- a/libraries/script-engine/src/Quat.cpp
+++ b/libraries/script-engine/src/Quat.cpp
@@ -22,6 +22,10 @@ quat Quat::normalize(const glm::quat& q) {
     return glm::normalize(q);
 }
 
+quat Quat::conjugate(const glm::quat& q) {
+    return glm::conjugate(q);
+}
+
 glm::quat Quat::rotationBetween(const glm::vec3& v1, const glm::vec3& v2) {
     return ::rotationBetween(v1, v2);
 }
diff --git a/libraries/script-engine/src/Quat.h b/libraries/script-engine/src/Quat.h
index 543c93401f..bc7f11ab01 100644
--- a/libraries/script-engine/src/Quat.h
+++ b/libraries/script-engine/src/Quat.h
@@ -26,6 +26,7 @@ class Quat : public QObject {
 public slots:
     glm::quat multiply(const glm::quat& q1, const glm::quat& q2);
     glm::quat normalize(const glm::quat& q);
+    glm::quat conjugate(const glm::quat& q);
     glm::quat lookAt(const glm::vec3& eye, const glm::vec3& center, const glm::vec3& up);
     glm::quat lookAtSimple(const glm::vec3& eye, const glm::vec3& center);
     glm::quat rotationBetween(const glm::vec3& v1, const glm::vec3& v2);