From 74380260b3c65d8de3f4ca19278045e6fb92174d Mon Sep 17 00:00:00 2001
From: Philip Rosedale <philip@highfidelity.io>
Date: Thu, 26 Dec 2013 19:07:02 -0800
Subject: [PATCH] Grab includes rotation

---
 interface/src/avatar/Hand.cpp     | 15 ++++++++++++++-
 interface/src/avatar/Hand.h       |  3 +++
 interface/src/avatar/MyAvatar.cpp | 10 ++++++++--
 3 files changed, 25 insertions(+), 3 deletions(-)

diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp
index 136d59bc37..20f034989b 100644
--- a/interface/src/avatar/Hand.cpp
+++ b/interface/src/avatar/Hand.cpp
@@ -52,6 +52,8 @@ Hand::Hand(Avatar* owningAvatar) :
     _pitchUpdate(0),
     _grabDelta(0, 0, 0),
     _grabDeltaVelocity(0, 0, 0),
+    _grabStartRotation(0, 0, 0, 1),
+    _grabCurrentRotation(0, 0, 0, 1),
     _throwInjector(QUrl("https://dl.dropboxusercontent.com/u/1864924/hifi-sounds/throw.raw")),
     _catchInjector(QUrl("https://dl.dropboxusercontent.com/u/1864924/hifi-sounds/catch.raw"))
 {
@@ -227,7 +229,7 @@ void Hand::simulateToyBall(PalmData& palm, const glm::vec3& fingerTipPosition, f
 }
 
 glm::vec3 Hand::getAndResetGrabDelta() {
-    const float HAND_GRAB_SCALE_DISTANCE = 5.f;
+    const float HAND_GRAB_SCALE_DISTANCE = 2.f;
     glm::vec3 delta = _grabDelta * _owningAvatar->getScale() * HAND_GRAB_SCALE_DISTANCE;
     _grabDelta = glm::vec3(0,0,0);
     glm::quat avatarRotation = _owningAvatar->getOrientation();
@@ -242,6 +244,11 @@ glm::vec3 Hand::getAndResetGrabDeltaVelocity() {
     return avatarRotation * -delta;
     
 }
+glm::quat Hand::getAndResetGrabRotation() {
+    glm::quat diff = _grabCurrentRotation * glm::inverse(_grabStartRotation);
+    _grabStartRotation = _grabCurrentRotation;
+    return diff;
+}
 
 void Hand::simulate(float deltaTime, bool isMine) {
     
@@ -275,10 +282,16 @@ void Hand::simulate(float deltaTime, bool isMine) {
                 
                 if (palm.getControllerButtons() & BUTTON_4) {
                     _grabDelta +=  palm.getRawVelocity() * deltaTime;
+                    _grabCurrentRotation = palm.getRawRotation();
                 }
                 if ((palm.getLastControllerButtons() & BUTTON_4) && !(palm.getControllerButtons() & BUTTON_4)) {
+                    // Just ending grab, capture velocity
                     _grabDeltaVelocity = palm.getRawVelocity();
                 }
+                if (!(palm.getLastControllerButtons() & BUTTON_4) && (palm.getControllerButtons() & BUTTON_4)) {
+                    // Just starting grab, capture starting rotation
+                    _grabStartRotation = palm.getRawRotation();
+                }
                 
                 if (palm.getControllerButtons() & BUTTON_1) {
                     if (glm::length(fingerTipPosition - _lastFingerAddVoxel) > (FINGERTIP_VOXEL_SIZE / 2.f)) {
diff --git a/interface/src/avatar/Hand.h b/interface/src/avatar/Hand.h
index 428132ba13..e938135a29 100755
--- a/interface/src/avatar/Hand.h
+++ b/interface/src/avatar/Hand.h
@@ -64,6 +64,7 @@ public:
     // Get the drag distance to move
     glm::vec3 getAndResetGrabDelta();
     glm::vec3 getAndResetGrabDeltaVelocity();
+    glm::quat getAndResetGrabRotation();
 
 private:
     // disallow copies of the Hand, copy of owning Avatar is disallowed too
@@ -110,6 +111,8 @@ private:
     
     glm::vec3 _grabDelta;
     glm::vec3 _grabDeltaVelocity;
+    glm::quat _grabStartRotation;
+    glm::quat _grabCurrentRotation;
     
     AudioInjector _throwInjector;
     AudioInjector _catchInjector;
diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 28529cd00b..42337f304e 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -258,14 +258,20 @@ void MyAvatar::simulate(float deltaTime, Transmitter* transmitter) {
     }
     
     updateChatCircle(deltaTime);
-    
-    //  Get any position or velocity update from Grab controller
+        
+    //  Get any position, velocity, or rotation update from Grab Drag controller
     glm::vec3 moveFromGrab = _hand.getAndResetGrabDelta();
     if (glm::length(moveFromGrab) > EPSILON) {
         _position += moveFromGrab;
         _velocity = glm::vec3(0, 0, 0);
     }
     _velocity += _hand.getAndResetGrabDeltaVelocity();
+    glm::quat deltaRotation = _hand.getAndResetGrabRotation();
+    glm::vec3 euler = safeEulerAngles(deltaRotation);
+    //  Adjust body yaw by yaw from controller
+    setOrientation(glm::angleAxis(euler.y, glm::vec3(0, 1, 0)) * getOrientation());
+    //  Adjust head pitch from controller
+    getHead().setMousePitch(getHead().getMousePitch() + euler.x);
     
     _position += _velocity * deltaTime;