From 8acb6d13d57efb1d685d9b402b26dcc09cc405f4 Mon Sep 17 00:00:00 2001
From: ZappoMan <bradh@konamoxt.com>
Date: Mon, 10 Feb 2014 15:25:09 -0800
Subject: [PATCH 01/13] move hydra thrust and view code into JS example

---
 examples/cameraExample.js         |  7 +++
 examples/hydraThrustAndView.js    | 86 +++++++++++++++++++++++++++++++
 interface/src/Application.cpp     |  2 +-
 interface/src/avatar/Hand.cpp     |  1 -
 interface/src/avatar/Hand.h       |  6 ---
 interface/src/avatar/MyAvatar.cpp | 34 +-----------
 interface/src/avatar/MyAvatar.h   | 10 ++--
 7 files changed, 100 insertions(+), 46 deletions(-)
 create mode 100644 examples/hydraThrustAndView.js

diff --git a/examples/cameraExample.js b/examples/cameraExample.js
index d42c3c1b0e..65cffb17dd 100644
--- a/examples/cameraExample.js
+++ b/examples/cameraExample.js
@@ -69,6 +69,13 @@ function checkCamera() {
         var viewJoystickPosition = Controller.getJoystickPosition(VIEW_CONTROLLER);
         yaw -= viewJoystickPosition.x * JOYSTICK_YAW_MAG * deltaTime;
         pitch += viewJoystickPosition.y * JOYSTICK_PITCH_MAG * deltaTime;
+        if (yaw > 360) {
+            yaw -= 360;
+        }
+        if (yaw < -360) {
+            yaw += 360;
+        }
+        print("pitch="+ pitch + " yaw="+ yaw + " roll=" + roll);
         var orientation = Quat.fromPitchYawRoll(pitch, yaw, roll);
         Camera.setOrientation(orientation);
     }
diff --git a/examples/hydraThrustAndView.js b/examples/hydraThrustAndView.js
new file mode 100644
index 0000000000..249aa5948c
--- /dev/null
+++ b/examples/hydraThrustAndView.js
@@ -0,0 +1,86 @@
+//
+//  hydraThrustAndView.js
+//  hifi
+//
+//  Created by Brad Hefta-Gaub on 2/6/14.
+//  Copyright (c) 2014 HighFidelity, Inc. All rights reserved.
+//
+//  This is an example script that demonstrates use of the Controller and MyAvatar classes to implement
+//  avatar flying through the hydra/controller joysticks
+//
+//
+
+var damping = 0.9;
+var position = { x: MyAvatar.position.x, y: MyAvatar.position.y, z: MyAvatar.position.z };
+var joysticksCaptured = false;
+var THRUST_CONTROLLER = 0;
+var VIEW_CONTROLLER = 1;
+var INITIAL_THRUST_MULTPLIER = 1.0;
+var THRUST_INCREASE_RATE = 1.05;
+var MAX_THRUST_MULTIPLIER = 75.0;
+var thrustMultiplier = INITIAL_THRUST_MULTPLIER;
+
+function flyWithHydra() {
+    var deltaTime = 1/60; // approximately our FPS - maybe better to be elapsed time since last call
+    var THRUST_MAG_UP = 800.0;
+    var THRUST_MAG_DOWN = 300.0;
+    var THRUST_MAG_FWD = 500.0;
+    var THRUST_MAG_BACK = 300.0;
+    var THRUST_MAG_LATERAL = 250.0;
+    var THRUST_JUMP = 120.0;
+    var scale = 1.0;
+
+    var YAW_MAG = 500.0;
+    var PITCH_MAG = 100.0;
+    var THRUST_MAG_HAND_JETS = THRUST_MAG_FWD;
+    var JOYSTICK_YAW_MAG = YAW_MAG;
+    var JOYSTICK_PITCH_MAG = PITCH_MAG * 0.5;
+
+    var thrustJoystickPosition = Controller.getJoystickPosition(THRUST_CONTROLLER);
+    
+    if (thrustJoystickPosition.x != 0 || thrustJoystickPosition.y != 0) {
+        if (thrustMultiplier < MAX_THRUST_MULTIPLIER) {
+            thrustMultiplier *= 1 + (deltaTime * THRUST_INCREASE_RATE);
+        }
+        var currentOrientation = MyAvatar.orientation;
+
+        var front = Quat.getFront(currentOrientation);
+        var right = Quat.getRight(currentOrientation);
+        var up = Quat.getUp(currentOrientation);
+    
+        var thrustFront = Vec3.multiply(front, scale * THRUST_MAG_HAND_JETS * thrustJoystickPosition.y * thrustMultiplier * deltaTime);
+        MyAvatar.addThrust(thrustFront);
+        var thrustRight = Vec3.multiply(right, scale * THRUST_MAG_HAND_JETS * thrustJoystickPosition.x * thrustMultiplier * deltaTime);
+        MyAvatar.addThrust(thrustRight);
+    } else {
+        thrustMultiplier = INITIAL_THRUST_MULTPLIER;
+    }
+
+    // View Controller
+    var viewJoystickPosition = Controller.getJoystickPosition(VIEW_CONTROLLER);
+    if (viewJoystickPosition.x != 0 || viewJoystickPosition.y != 0) {
+
+        // change the body yaw based on our x controller
+        var orientation = MyAvatar.orientation;
+        var deltaOrientation = Quat.fromPitchYawRoll(0, (-1 * viewJoystickPosition.x * JOYSTICK_YAW_MAG * deltaTime), 0);
+        MyAvatar.orientation = Quat.multiply(orientation, deltaOrientation);
+
+        // change the headPitch based on our x controller
+        //pitch += viewJoystickPosition.y * JOYSTICK_PITCH_MAG * deltaTime;
+        var newPitch = MyAvatar.headPitch + (viewJoystickPosition.y * JOYSTICK_PITCH_MAG * deltaTime);
+        MyAvatar.headPitch = newPitch;
+    }
+}
+
+Script.willSendVisualDataCallback.connect(flyWithHydra);
+Controller.captureJoystick(THRUST_CONTROLLER);
+Controller.captureJoystick(VIEW_CONTROLLER);
+
+// Map keyPress and mouse move events to our callbacks
+function scriptEnding() {
+    // re-enabled the standard application for touch events
+    Controller.releaseJoystick(THRUST_CONTROLLER);
+    Controller.releaseJoystick(VIEW_CONTROLLER);
+}
+Script.scriptEnding.connect(scriptEnding);
+
diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp
index 160d6b7c2c..004b71074b 100644
--- a/interface/src/Application.cpp
+++ b/interface/src/Application.cpp
@@ -4042,7 +4042,7 @@ void Application::loadScript(const QString& fileNameString) {
     scriptEngine->getParticlesScriptingInterface()->setParticleTree(_particles.getTree());
     
     // hook our avatar object into this script engine
-    scriptEngine->setAvatarData( static_cast<Avatar*>(_myAvatar), "MyAvatar");
+    scriptEngine->setAvatarData(_myAvatar, "MyAvatar"); // leave it as a MyAvatar class to expose thrust features
 
     CameraScriptableObject* cameraScriptable = new CameraScriptableObject(&_myCamera, &_viewFrustum);
     scriptEngine->registerGlobalObject("Camera", cameraScriptable);
diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp
index b441000cc1..302475df8a 100644
--- a/interface/src/avatar/Hand.cpp
+++ b/interface/src/avatar/Hand.cpp
@@ -33,7 +33,6 @@ Hand::Hand(Avatar* owningAvatar) :
     _collisionCenter(0,0,0),
     _collisionAge(0),
     _collisionDuration(0),
-    _pitchUpdate(0),
     _grabDelta(0, 0, 0),
     _grabDeltaVelocity(0, 0, 0),
     _grabStartRotation(0, 0, 0, 1),
diff --git a/interface/src/avatar/Hand.h b/interface/src/avatar/Hand.h
index 3c8ec2d562..aec5cdb921 100755
--- a/interface/src/avatar/Hand.h
+++ b/interface/src/avatar/Hand.h
@@ -58,10 +58,6 @@ public:
     const glm::vec3& getLeapFingerTipBallPosition (int ball) const { return _leapFingerTipBalls [ball].position;}
     const glm::vec3& getLeapFingerRootBallPosition(int ball) const { return _leapFingerRootBalls[ball].position;}
     
-    // Pitch from controller input to view
-    const float getPitchUpdate() const { return _pitchUpdate; }
-    void setPitchUpdate(float pitchUpdate) { _pitchUpdate = pitchUpdate; }
-    
     // Get the drag distance to move
     glm::vec3 getAndResetGrabDelta();
     glm::vec3 getAndResetGrabDeltaVelocity();
@@ -101,8 +97,6 @@ private:
     
     void handleVoxelCollision(PalmData* palm, const glm::vec3& fingerTipPosition, VoxelTreeElement* voxel, float deltaTime);
     
-    float _pitchUpdate;
-    
     glm::vec3 _grabDelta;
     glm::vec3 _grabDeltaVelocity;
     glm::quat _grabStartRotation;
diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 98eb9a4431..4eec6e70f8 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -323,6 +323,7 @@ void MyAvatar::simulate(float deltaTime) {
 
     updateChatCircle(deltaTime);
 
+    // TODO: this should be removed and turned into JS instead
     //  Get any position, velocity, or rotation update from Grab Drag controller
     glm::vec3 moveFromGrab = _hand.getAndResetGrabDelta();
     if (glm::length(moveFromGrab) > EPSILON) {
@@ -797,39 +798,6 @@ void MyAvatar::updateThrust(float deltaTime) {
             up;
         }
     }
-    //  Add thrust and rotation from hand controllers
-    const float THRUST_MAG_HAND_JETS = THRUST_MAG_FWD;
-    const float JOYSTICK_YAW_MAG = YAW_MAG;
-    const float JOYSTICK_PITCH_MAG = PITCH_MAG * 0.5f;
-    const int THRUST_CONTROLLER = 0;
-    const int VIEW_CONTROLLER = 1;
-    for (size_t i = 0; i < getHand().getPalms().size(); ++i) {
-        PalmData& palm = getHand().getPalms()[i];
-
-        // If the script hasn't captured this joystick, then let the default behavior work
-        if (!Application::getInstance()->getControllerScriptingInterface()->isJoystickCaptured(palm.getSixenseID())) {
-            if (palm.isActive() && (palm.getSixenseID() == THRUST_CONTROLLER)) {
-                if (palm.getJoystickY() != 0.f) {
-                    FingerData& finger = palm.getFingers()[0];
-                    if (finger.isActive()) {
-                    }
-                    _thrust += front * _scale * THRUST_MAG_HAND_JETS * palm.getJoystickY() * _thrustMultiplier * deltaTime;
-                }
-                if (palm.getJoystickX() != 0.f) {
-                    _thrust += right * _scale * THRUST_MAG_HAND_JETS * palm.getJoystickX() * _thrustMultiplier * deltaTime;
-                }
-            } else if (palm.isActive() && (palm.getSixenseID() == VIEW_CONTROLLER)) {
-                if (palm.getJoystickX() != 0.f) {
-                    _bodyYawDelta -= palm.getJoystickX() * JOYSTICK_YAW_MAG * deltaTime;
-                }
-                if (palm.getJoystickY() != 0.f) {
-                    getHand().setPitchUpdate(getHand().getPitchUpdate() +
-                                             (palm.getJoystickY() * JOYSTICK_PITCH_MAG * deltaTime));
-                }
-            }
-        }
-
-    }
 
     //  Update speed brake status
     const float MIN_SPEED_BRAKE_VELOCITY = _scale * 0.4f;
diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h
index 7dfb8812dd..4e381509b9 100644
--- a/interface/src/avatar/MyAvatar.h
+++ b/interface/src/avatar/MyAvatar.h
@@ -45,7 +45,6 @@ public:
 
     // setters
     void setMousePressed(bool mousePressed) { _mousePressed = mousePressed; }
-    void setThrust(glm::vec3 newThrust) { _thrust = newThrust; }
     void setVelocity(const glm::vec3 velocity) { _velocity = velocity; }
     void setLeanScale(float scale) { _leanScale = scale; }
     void setGravity(glm::vec3 gravity);
@@ -78,9 +77,6 @@ public:
     
     static void sendKillAvatar();
 
-    //  Set/Get update the thrust that will move the avatar around
-    void addThrust(glm::vec3 newThrust) { _thrust += newThrust; };
-    glm::vec3 getThrust() { return _thrust; };
 
     void orbit(const glm::vec3& position, int deltaX, int deltaY);
 
@@ -94,9 +90,13 @@ public slots:
     void increaseSize();
     void decreaseSize();
     void resetSize();
-    
     void sendIdentityPacket();
 
+    //  Set/Get update the thrust that will move the avatar around
+    void addThrust(glm::vec3 newThrust) { _thrust += newThrust; };
+    glm::vec3 getThrust() { return _thrust; };
+    void setThrust(glm::vec3 newThrust) { _thrust = newThrust; }
+
 private:
     bool _mousePressed;
     float _bodyPitchDelta;

From c68abb3499aecf9c255abc1396107eb592861589 Mon Sep 17 00:00:00 2001
From: ZappoMan <bradh@konamoxt.com>
Date: Mon, 10 Feb 2014 15:33:07 -0800
Subject: [PATCH 02/13] nodelist bug

---
 libraries/shared/src/NodeList.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/libraries/shared/src/NodeList.cpp b/libraries/shared/src/NodeList.cpp
index 92f6f961d5..3d4e86decf 100644
--- a/libraries/shared/src/NodeList.cpp
+++ b/libraries/shared/src/NodeList.cpp
@@ -87,7 +87,7 @@ bool NodeList::packetVersionAndHashMatch(const QByteArray& packet) {
     if (packet[1] != versionForPacketType(packetTypeForPacket(packet))
         && packetTypeForPacket(packet) != PacketTypeStunResponse) {
         PacketType mismatchType = packetTypeForPacket(packet);
-        int numPacketTypeBytes = arithmeticCodingValueFromBuffer(packet.data());
+        int numPacketTypeBytes = numBytesArithmeticCodingFromBuffer(packet.data());
         
         qDebug() << "Packet version mismatch on" << packetTypeForPacket(packet) << "- Sender"
             << uuidFromPacketHeader(packet) << "sent" << qPrintable(QString::number(packet[numPacketTypeBytes])) << "but"

From 0c547a8160f7b874caeb68171fad9ad4daacc4eb Mon Sep 17 00:00:00 2001
From: ZappoMan <bradh@konamoxt.com>
Date: Mon, 10 Feb 2014 15:37:49 -0800
Subject: [PATCH 03/13] added Quat.safeEulerAngles() for JS

---
 libraries/script-engine/src/Quat.cpp | 5 +++++
 libraries/script-engine/src/Quat.h   | 1 +
 2 files changed, 6 insertions(+)

diff --git a/libraries/script-engine/src/Quat.cpp b/libraries/script-engine/src/Quat.cpp
index a197d59aeb..dceb1e40df 100644
--- a/libraries/script-engine/src/Quat.cpp
+++ b/libraries/script-engine/src/Quat.cpp
@@ -10,6 +10,7 @@
 //
 
 #include <OctreeConstants.h>
+#include <SharedUtil.h>
 #include "Quat.h"
 
 glm::quat Quat::multiply(const glm::quat& q1, const glm::quat& q2) { 
@@ -35,3 +36,7 @@ glm::vec3 Quat::getRight(const glm::quat& orientation) {
 glm::vec3 Quat::getUp(const glm::quat& orientation) {
     return orientation * IDENTITY_UP;
 }
+
+glm::vec3 Quat::safeEulerAngles(const glm::quat& orientation) {
+    return safeEulerAngles(orientation);
+}
diff --git a/libraries/script-engine/src/Quat.h b/libraries/script-engine/src/Quat.h
index 72fec6d6dc..7e29c41833 100644
--- a/libraries/script-engine/src/Quat.h
+++ b/libraries/script-engine/src/Quat.h
@@ -26,6 +26,7 @@ public slots:
     glm::vec3 getFront(const glm::quat& orientation);
     glm::vec3 getRight(const glm::quat& orientation);
     glm::vec3 getUp(const glm::quat& orientation);
+    glm::vec3 safeEulerAngles(const glm::quat& orientation);
 };
 
 

From 3f73b61a76a11554f4dfd85038738c040551eff3 Mon Sep 17 00:00:00 2001
From: ZappoMan <bradh@konamoxt.com>
Date: Mon, 10 Feb 2014 15:38:19 -0800
Subject: [PATCH 04/13] renamed hydraMove.js

---
 examples/{hydraThrustAndView.js => hydraMove.js} | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 rename examples/{hydraThrustAndView.js => hydraMove.js} (100%)

diff --git a/examples/hydraThrustAndView.js b/examples/hydraMove.js
similarity index 100%
rename from examples/hydraThrustAndView.js
rename to examples/hydraMove.js

From 991c72006c9ffc0567f0ec48541cd9f91ca69d88 Mon Sep 17 00:00:00 2001
From: ZappoMan <bradh@konamoxt.com>
Date: Mon, 10 Feb 2014 17:25:14 -0800
Subject: [PATCH 05/13] moving the hydra grab behavior to JS

---
 examples/hydraMove.js                         | 88 +++++++++++++++++++
 .../src/ControllerScriptingInterface.cpp      | 15 ++++
 interface/src/ControllerScriptingInterface.h  |  2 +-
 interface/src/avatar/Hand.cpp                 | 41 +--------
 interface/src/avatar/Hand.h                   | 10 ---
 interface/src/avatar/MyAvatar.cpp             | 16 ----
 .../AbstractControllerScriptingInterface.h    |  2 +
 libraries/script-engine/src/Quat.cpp          | 14 ++-
 libraries/script-engine/src/Quat.h            |  2 +
 libraries/script-engine/src/Vec3.cpp          |  8 ++
 libraries/script-engine/src/Vec3.h            |  2 +
 11 files changed, 132 insertions(+), 68 deletions(-)

diff --git a/examples/hydraMove.js b/examples/hydraMove.js
index 249aa5948c..252d3c7756 100644
--- a/examples/hydraMove.js
+++ b/examples/hydraMove.js
@@ -19,6 +19,46 @@ var INITIAL_THRUST_MULTPLIER = 1.0;
 var THRUST_INCREASE_RATE = 1.05;
 var MAX_THRUST_MULTIPLIER = 75.0;
 var thrustMultiplier = INITIAL_THRUST_MULTPLIER;
+var grabDelta = { x: 0, y: 0, z: 0};
+var grabDeltaVelocity = { x: 0, y: 0, z: 0};
+var grabStartRotation = { x: 0, y: 0, z: 0, w: 1};
+var grabCurrentRotation = { x: 0, y: 0, z: 0, w: 1};
+var grabbingWithRightHand = false;
+var wasGrabbingWithRightHand = false;
+var grabbingWithLeftHand = false;
+var wasGrabbingWithLeftHand = false;
+var EPSILON = 0.000001; //smallish positive number - used as margin of error for some computations
+var velocity = { x: 0, y: 0, z: 0};
+
+
+var LEFT_PALM = 0;
+var LEFT_BUTTON_4 = 4;
+var RIGHT_PALM = 2;
+var RIGHT_BUTTON_4 = 10;
+
+function getAndResetGrabDelta() {
+    var HAND_GRAB_SCALE_DISTANCE = 2.0;
+    var delta = Vec3.multiply(grabDelta, (MyAvatar.scale * HAND_GRAB_SCALE_DISTANCE));
+    grabDelta = { x: 0, y: 0, z: 0};
+    var avatarRotation = MyAvatar.orientation;
+    var result = Vec3.multiplyQbyV(avatarRotation, Vec3.multiply(delta, -1));
+    return result;
+}
+
+function getAndResetGrabDeltaVelocity() {
+    var HAND_GRAB_SCALE_VELOCITY = 5.0;
+    var delta = Vec3.multiply(grabDeltaVelocity, (MyAvatar.scale * HAND_GRAB_SCALE_VELOCITY));
+    grabDeltaVelocity = { x: 0, y: 0, z: 0};
+    var avatarRotation = MyAvatar.orientation;
+    var result = Quat.multiply(avatarRotation, Vec3.multiply(delta, -1));
+    return result;
+}
+
+function getAndResetGrabRotation() {
+    var quatDiff = Quat.multiply(grabCurrentRotation, Quat.inverse(grabStartRotation));
+    grabStartRotation = grabCurrentRotation;
+    return quatDiff;
+}
 
 function flyWithHydra() {
     var deltaTime = 1/60; // approximately our FPS - maybe better to be elapsed time since last call
@@ -70,6 +110,54 @@ function flyWithHydra() {
         var newPitch = MyAvatar.headPitch + (viewJoystickPosition.y * JOYSTICK_PITCH_MAG * deltaTime);
         MyAvatar.headPitch = newPitch;
     }
+    
+    // check for and handle grab behaviors
+    grabbingWithRightHand = Controller.isButtonPressed(RIGHT_BUTTON_4);
+    grabbingWithLeftHand = Controller.isButtonPressed(LEFT_BUTTON_4);
+
+    if (grabbingWithRightHand) {
+        grabDelta = Vec3.sum(grabDelta, Vec3.multiply(Controller.getSpatialControlVelocity(RIGHT_PALM), deltaTime));
+        grabCurrentRotation = Controller.getSpatialControlRawRotation(RIGHT_PALM);
+    }
+    if (!grabbingWithRightHand && wasGrabbingWithRightHand) {
+        // Just ending grab, capture velocity
+        grabDeltaVelocity = Controller.getSpatialControlVelocity(RIGHT_PALM);
+    }
+    if (grabbingWithRightHand && !wasGrabbingWithRightHand) {
+        // Just starting grab, capture starting rotation
+        grabStartRotation = Controller.getSpatialControlRawRotation(RIGHT_PALM);
+    }
+
+    grabbing = grabbingWithRightHand || grabbingWithLeftHand;
+
+    if (grabbing) {
+    
+        // move position
+        var moveFromGrab = getAndResetGrabDelta();
+        if (Vec3.length(moveFromGrab) > EPSILON) {
+            position = Vec3.sum(position, moveFromGrab);
+            velocity = { x: 0, y: 0, z: 0};
+        }
+        
+        // add some velocity...
+        velocity = Vec3.sum(velocity, getAndResetGrabDeltaVelocity());
+        
+        // add some rotation...
+        var deltaRotation = getAndResetGrabRotation();
+        var GRAB_CONTROLLER_TURN_SCALING = 0.5;
+        var euler = Vec3.multiply(Quat.safeEulerAngles(deltaRotation), GRAB_CONTROLLER_TURN_SCALING);
+    
+        //  Adjust body yaw by yaw from controller
+        var orientation = Quat.multiply(Quat.angleAxis(-euler.y, {x:0, y: 1, z:0}), MyAvatar.orientation);
+        MyAvatar.orientation = orientation;
+
+        //  Adjust head pitch from controller
+        //getHead().setPitch(getHead().getPitch() - euler.x);
+        MyAvatar.headPitch = MyAvatar.headPitch - euler.x;
+    }
+    
+    wasGrabbingWithRightHand = grabbingWithRightHand;
+    wasGrabbingWithLeftHand = grabbingWithLeftHand;
 }
 
 Script.willSendVisualDataCallback.connect(flyWithHydra);
diff --git a/interface/src/ControllerScriptingInterface.cpp b/interface/src/ControllerScriptingInterface.cpp
index 8716116877..b3d6170bff 100644
--- a/interface/src/ControllerScriptingInterface.cpp
+++ b/interface/src/ControllerScriptingInterface.cpp
@@ -172,6 +172,21 @@ glm::vec3 ControllerScriptingInterface::getSpatialControlVelocity(int controlInd
     return glm::vec3(0); // bad index
 }
 
+glm::quat ControllerScriptingInterface::getSpatialControlRawRotation(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->getRawRotation();
+            case TIP_SPATIALCONTROL:
+                return palmData->getRawRotation(); // currently the tip doesn't have a unique rotation, use the palm rotation
+        }
+    }
+    return glm::quat(); // bad index
+}
+
 glm::vec3 ControllerScriptingInterface::getSpatialControlNormal(int controlIndex) const {
     int palmIndex = controlIndex / NUMBER_OF_SPATIALCONTROLS_PER_PALM;
     int controlOfPalm = controlIndex % NUMBER_OF_SPATIALCONTROLS_PER_PALM;
diff --git a/interface/src/ControllerScriptingInterface.h b/interface/src/ControllerScriptingInterface.h
index e9fbd2d6d3..f0a50559f9 100644
--- a/interface/src/ControllerScriptingInterface.h
+++ b/interface/src/ControllerScriptingInterface.h
@@ -58,7 +58,7 @@ public slots:
     virtual glm::vec3 getSpatialControlPosition(int controlIndex) const;
     virtual glm::vec3 getSpatialControlVelocity(int controlIndex) const;
     virtual glm::vec3 getSpatialControlNormal(int controlIndex) const;
-
+    virtual glm::quat getSpatialControlRawRotation(int controlIndex) const;
     virtual void captureKeyEvents(const KeyEvent& event);
     virtual void releaseKeyEvents(const KeyEvent& event);
 
diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp
index 302475df8a..291ed034ec 100644
--- a/interface/src/avatar/Hand.cpp
+++ b/interface/src/avatar/Hand.cpp
@@ -32,11 +32,7 @@ Hand::Hand(Avatar* owningAvatar) :
     _ballColor(0.0, 0.0, 0.4),
     _collisionCenter(0,0,0),
     _collisionAge(0),
-    _collisionDuration(0),
-    _grabDelta(0, 0, 0),
-    _grabDeltaVelocity(0, 0, 0),
-    _grabStartRotation(0, 0, 0, 1),
-    _grabCurrentRotation(0, 0, 0, 1)
+    _collisionDuration(0)
 {
 }
 
@@ -53,28 +49,6 @@ void Hand::init() {
 void Hand::reset() {
 }
 
-glm::vec3 Hand::getAndResetGrabDelta() {
-    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();
-    return avatarRotation * -delta;
-}
-
-glm::vec3 Hand::getAndResetGrabDeltaVelocity() {
-    const float HAND_GRAB_SCALE_VELOCITY = 5.f;
-    glm::vec3 delta = _grabDeltaVelocity * _owningAvatar->getScale() * HAND_GRAB_SCALE_VELOCITY;
-    _grabDeltaVelocity = glm::vec3(0,0,0);
-    glm::quat avatarRotation = _owningAvatar->getOrientation();
-    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) {
     
     if (_collisionAge > 0.f) {
@@ -100,19 +74,6 @@ void Hand::simulate(float deltaTime, bool isMine) {
                 
                 _buckyBalls.grab(palm, fingerTipPosition, _owningAvatar->getOrientation(), deltaTime);
                 
-                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)) {
                         QColor paintColor = Menu::getInstance()->getActionForOption(MenuOption::VoxelPaintColor)->data().value<QColor>();
diff --git a/interface/src/avatar/Hand.h b/interface/src/avatar/Hand.h
index aec5cdb921..c2f49a15e5 100755
--- a/interface/src/avatar/Hand.h
+++ b/interface/src/avatar/Hand.h
@@ -58,11 +58,6 @@ public:
     const glm::vec3& getLeapFingerTipBallPosition (int ball) const { return _leapFingerTipBalls [ball].position;}
     const glm::vec3& getLeapFingerRootBallPosition(int ball) const { return _leapFingerRootBalls[ball].position;}
     
-    // 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
     Hand(const Hand&);
@@ -96,11 +91,6 @@ private:
     void calculateGeometry();
     
     void handleVoxelCollision(PalmData* palm, const glm::vec3& fingerTipPosition, VoxelTreeElement* voxel, float deltaTime);
-    
-    glm::vec3 _grabDelta;
-    glm::vec3 _grabDeltaVelocity;
-    glm::quat _grabStartRotation;
-    glm::quat _grabCurrentRotation;
 };
 
 #endif
diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 4eec6e70f8..eab2207bd9 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -323,22 +323,6 @@ void MyAvatar::simulate(float deltaTime) {
 
     updateChatCircle(deltaTime);
 
-    // TODO: this should be removed and turned into JS instead
-    //  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();
-    const float GRAB_CONTROLLER_TURN_SCALING = 0.5f;
-    glm::vec3 euler = safeEulerAngles(deltaRotation) * GRAB_CONTROLLER_TURN_SCALING;
-    //  Adjust body yaw by yaw from controller
-    setOrientation(glm::angleAxis(-euler.y, glm::vec3(0, 1, 0)) * getOrientation());
-    //  Adjust head pitch from controller
-    getHead().setPitch(getHead().getPitch() - euler.x);
-
     _position += _velocity * deltaTime;
 
     // update avatar skeleton and simulate hand and head
diff --git a/libraries/script-engine/src/AbstractControllerScriptingInterface.h b/libraries/script-engine/src/AbstractControllerScriptingInterface.h
index 4fad5f6edc..d9878d0b71 100644
--- a/libraries/script-engine/src/AbstractControllerScriptingInterface.h
+++ b/libraries/script-engine/src/AbstractControllerScriptingInterface.h
@@ -12,6 +12,7 @@
 #include <QtCore/QObject>
 
 #include <glm/glm.hpp>
+#include <glm/gtc/quaternion.hpp>
 
 #include "EventTypes.h"
 
@@ -37,6 +38,7 @@ public slots:
     virtual glm::vec3 getSpatialControlPosition(int controlIndex) const = 0;
     virtual glm::vec3 getSpatialControlVelocity(int controlIndex) const = 0;
     virtual glm::vec3 getSpatialControlNormal(int controlIndex) const = 0;
+    virtual glm::quat getSpatialControlRawRotation(int controlIndex) const = 0;
 
     virtual void captureKeyEvents(const KeyEvent& event) = 0;
     virtual void releaseKeyEvents(const KeyEvent& event) = 0;
diff --git a/libraries/script-engine/src/Quat.cpp b/libraries/script-engine/src/Quat.cpp
index dceb1e40df..2f1c39f9e3 100644
--- a/libraries/script-engine/src/Quat.cpp
+++ b/libraries/script-engine/src/Quat.cpp
@@ -9,6 +9,8 @@
 //
 //
 
+#include <glm/gtx/vector_angle.hpp>
+
 #include <OctreeConstants.h>
 #include <SharedUtil.h>
 #include "Quat.h"
@@ -25,6 +27,11 @@ glm::quat Quat::fromPitchYawRoll(float pitch, float yaw, float roll) {
     return glm::quat(glm::radians(glm::vec3(pitch, yaw, roll)));
 }
 
+glm::quat Quat::inverse(const glm::quat& q) {
+    return glm::inverse(q);
+}
+
+
 glm::vec3 Quat::getFront(const glm::quat& orientation) {
     return orientation * IDENTITY_FRONT;
 }
@@ -38,5 +45,10 @@ glm::vec3 Quat::getUp(const glm::quat& orientation) {
 }
 
 glm::vec3 Quat::safeEulerAngles(const glm::quat& orientation) {
-    return safeEulerAngles(orientation);
+    return ::safeEulerAngles(orientation);
 }
+
+glm::quat Quat::angleAxis(float angle, const glm::vec3& v) {
+    return glm::angleAxis(angle, v);
+}
+
diff --git a/libraries/script-engine/src/Quat.h b/libraries/script-engine/src/Quat.h
index 7e29c41833..985dd0a00f 100644
--- a/libraries/script-engine/src/Quat.h
+++ b/libraries/script-engine/src/Quat.h
@@ -23,10 +23,12 @@ public slots:
     glm::quat multiply(const glm::quat& q1, const glm::quat& q2);
     glm::quat fromVec3(const glm::vec3& vec3);
     glm::quat fromPitchYawRoll(float pitch, float yaw, float roll);
+    glm::quat inverse(const glm::quat& q);
     glm::vec3 getFront(const glm::quat& orientation);
     glm::vec3 getRight(const glm::quat& orientation);
     glm::vec3 getUp(const glm::quat& orientation);
     glm::vec3 safeEulerAngles(const glm::quat& orientation);
+    glm::quat angleAxis(float angle, const glm::vec3& v);
 };
 
 
diff --git a/libraries/script-engine/src/Vec3.cpp b/libraries/script-engine/src/Vec3.cpp
index 87b1b510a4..2d6a5b2316 100644
--- a/libraries/script-engine/src/Vec3.cpp
+++ b/libraries/script-engine/src/Vec3.cpp
@@ -19,6 +19,14 @@ glm::vec3 Vec3::multiply(const glm::vec3& v1, float f) {
     return v1 * f;
 }
 
+glm::vec3 Vec3::multiplyQbyV(const glm::quat& q, const glm::vec3& v) {
+    return q * v;
+}
+
 glm::vec3 Vec3::sum(const glm::vec3& v1, const glm::vec3& v2) {
     return v1 + v2;
 }
+
+float Vec3::length(const glm::vec3& v) {
+    return glm::length(v);
+}
diff --git a/libraries/script-engine/src/Vec3.h b/libraries/script-engine/src/Vec3.h
index 1cc44f3061..bf883f3e5a 100644
--- a/libraries/script-engine/src/Vec3.h
+++ b/libraries/script-engine/src/Vec3.h
@@ -23,7 +23,9 @@ class Vec3 : public QObject {
 public slots:
     glm::vec3 multiply(const glm::vec3& v1, const glm::vec3& v2);
     glm::vec3 multiply(const glm::vec3& v1, float f);
+    glm::vec3 multiplyQbyV(const glm::quat& q, const glm::vec3& v);
     glm::vec3 sum(const glm::vec3& v1, const glm::vec3& v2);
+    float length(const glm::vec3& v);
 };
 
 

From 4b5e633258bd293ad8b0b936b275639fa6522690 Mon Sep 17 00:00:00 2001
From: ZappoMan <bradh@konamoxt.com>
Date: Mon, 10 Feb 2014 19:48:51 -0800
Subject: [PATCH 06/13] tweaks to hydraMove.js, added scale to MyAvatar JS

---
 examples/hydraMove.js              | 178 +++++++++++++++++------------
 libraries/avatars/src/AvatarData.h |   1 +
 2 files changed, 109 insertions(+), 70 deletions(-)

diff --git a/examples/hydraMove.js b/examples/hydraMove.js
index 252d3c7756..6223e6fe2c 100644
--- a/examples/hydraMove.js
+++ b/examples/hydraMove.js
@@ -1,8 +1,8 @@
 //
-//  hydraThrustAndView.js
+//  hydraMove.js
 //  hifi
 //
-//  Created by Brad Hefta-Gaub on 2/6/14.
+//  Created by Brad Hefta-Gaub on 2/10/14.
 //  Copyright (c) 2014 HighFidelity, Inc. All rights reserved.
 //
 //  This is an example script that demonstrates use of the Controller and MyAvatar classes to implement
@@ -27,8 +27,21 @@ var grabbingWithRightHand = false;
 var wasGrabbingWithRightHand = false;
 var grabbingWithLeftHand = false;
 var wasGrabbingWithLeftHand = false;
-var EPSILON = 0.000001; //smallish positive number - used as margin of error for some computations
+var EPSILON = 0.000001;
 var velocity = { x: 0, y: 0, z: 0};
+var deltaTime = 1/60; // approximately our FPS - maybe better to be elapsed time since last call
+var THRUST_MAG_UP = 800.0;
+var THRUST_MAG_DOWN = 300.0;
+var THRUST_MAG_FWD = 500.0;
+var THRUST_MAG_BACK = 300.0;
+var THRUST_MAG_LATERAL = 250.0;
+var THRUST_JUMP = 120.0;
+
+var YAW_MAG = 500.0;
+var PITCH_MAG = 100.0;
+var THRUST_MAG_HAND_JETS = THRUST_MAG_FWD;
+var JOYSTICK_YAW_MAG = YAW_MAG;
+var JOYSTICK_PITCH_MAG = PITCH_MAG * 0.5;
 
 
 var LEFT_PALM = 0;
@@ -36,6 +49,7 @@ var LEFT_BUTTON_4 = 4;
 var RIGHT_PALM = 2;
 var RIGHT_BUTTON_4 = 10;
 
+// Used by handleGrabBehavior() for managing the grab position changes
 function getAndResetGrabDelta() {
     var HAND_GRAB_SCALE_DISTANCE = 2.0;
     var delta = Vec3.multiply(grabDelta, (MyAvatar.scale * HAND_GRAB_SCALE_DISTANCE));
@@ -45,8 +59,9 @@ function getAndResetGrabDelta() {
     return result;
 }
 
+// Used by handleGrabBehavior() for managing the grab velocity feature
 function getAndResetGrabDeltaVelocity() {
-    var HAND_GRAB_SCALE_VELOCITY = 5.0;
+    var HAND_GRAB_SCALE_VELOCITY = 50.0;
     var delta = Vec3.multiply(grabDeltaVelocity, (MyAvatar.scale * HAND_GRAB_SCALE_VELOCITY));
     grabDeltaVelocity = { x: 0, y: 0, z: 0};
     var avatarRotation = MyAvatar.orientation;
@@ -54,28 +69,96 @@ function getAndResetGrabDeltaVelocity() {
     return result;
 }
 
+// Used by handleGrabBehavior() for managing the grab rotation feature
 function getAndResetGrabRotation() {
     var quatDiff = Quat.multiply(grabCurrentRotation, Quat.inverse(grabStartRotation));
     grabStartRotation = grabCurrentRotation;
     return quatDiff;
 }
 
+// handles all the grab related behavior: position (crawl), velocity (flick), and rotate (twist)
+function handleGrabBehavior() {
+    // check for and handle grab behaviors
+    grabbingWithRightHand = Controller.isButtonPressed(RIGHT_BUTTON_4);
+    grabbingWithLeftHand = Controller.isButtonPressed(LEFT_BUTTON_4);
+    stoppedGrabbingWithLeftHand = false;
+    stoppedGrabbingWithRightHand = false;
+    
+    if (grabbingWithRightHand && !wasGrabbingWithRightHand) {
+        // Just starting grab, capture starting rotation
+        grabStartRotation = Controller.getSpatialControlRawRotation(RIGHT_PALM);
+    }
+    if (grabbingWithRightHand) {
+        grabDelta = Vec3.sum(grabDelta, Vec3.multiply(Controller.getSpatialControlVelocity(RIGHT_PALM), deltaTime));
+        grabCurrentRotation = Controller.getSpatialControlRawRotation(RIGHT_PALM);
+    }
+    if (!grabbingWithRightHand && wasGrabbingWithRightHand) {
+        // Just ending grab, capture velocity
+        grabDeltaVelocity = Controller.getSpatialControlVelocity(RIGHT_PALM);
+        stoppedGrabbingWithRightHand = true;
+    }
+
+    if (grabbingWithLeftHand && !wasGrabbingWithLeftHand) {
+        // Just starting grab, capture starting rotation
+        grabStartRotation = Controller.getSpatialControlRawRotation(LEFT_PALM);
+    }
+
+    if (grabbingWithLeftHand) {
+        grabDelta = Vec3.sum(grabDelta, Vec3.multiply(Controller.getSpatialControlVelocity(LEFT_PALM), deltaTime));
+        grabCurrentRotation = Controller.getSpatialControlRawRotation(LEFT_PALM);
+    }
+    if (!grabbingWithLeftHand && wasGrabbingWithLeftHand) {
+        // Just ending grab, capture velocity
+        grabDeltaVelocity = Controller.getSpatialControlVelocity(LEFT_PALM);
+        stoppedGrabbingWithLeftHand = true;
+    }
+
+    grabbing = grabbingWithRightHand || grabbingWithLeftHand;
+    stoppedGrabbing = stoppedGrabbingWithRightHand || stoppedGrabbingWithLeftHand;
+
+    if (grabbing) {
+    
+        // move position
+        var moveFromGrab = getAndResetGrabDelta();
+        if (Vec3.length(moveFromGrab) > EPSILON) {
+            MyAvatar.position = Vec3.sum(MyAvatar.position, moveFromGrab);
+            velocity = { x: 0, y: 0, z: 0};
+        }
+        
+        // add some rotation...
+        var deltaRotation = getAndResetGrabRotation();
+        var GRAB_CONTROLLER_TURN_SCALING = 0.5;
+        var euler = Vec3.multiply(Quat.safeEulerAngles(deltaRotation), GRAB_CONTROLLER_TURN_SCALING);
+    
+        //  Adjust body yaw by yaw from controller
+        var orientation = Quat.multiply(Quat.angleAxis(-euler.y, {x:0, y: 1, z:0}), MyAvatar.orientation);
+        MyAvatar.orientation = orientation;
+
+        //  Adjust head pitch from controller
+        MyAvatar.headPitch = MyAvatar.headPitch - euler.x;
+    }
+
+    //print("velocity=" +velocity.x +"," +velocity.y +"," +velocity.z + " length=" + Vec3.length(velocity));
+
+    // add some velocity...
+    if (stoppedGrabbing) {
+        velocity = Vec3.sum(velocity, getAndResetGrabDeltaVelocity());
+    }
+
+    // handle residual velocity
+    if(Vec3.length(velocity) > EPSILON) {
+        MyAvatar.position = Vec3.sum(MyAvatar.position, Vec3.multiply(velocity, deltaTime));
+        // damp velocity
+        velocity = Vec3.multiply(velocity, damping);
+    }
+    
+    
+    wasGrabbingWithRightHand = grabbingWithRightHand;
+    wasGrabbingWithLeftHand = grabbingWithLeftHand;
+}
+
+// Main update function that handles flying and grabbing behaviort
 function flyWithHydra() {
-    var deltaTime = 1/60; // approximately our FPS - maybe better to be elapsed time since last call
-    var THRUST_MAG_UP = 800.0;
-    var THRUST_MAG_DOWN = 300.0;
-    var THRUST_MAG_FWD = 500.0;
-    var THRUST_MAG_BACK = 300.0;
-    var THRUST_MAG_LATERAL = 250.0;
-    var THRUST_JUMP = 120.0;
-    var scale = 1.0;
-
-    var YAW_MAG = 500.0;
-    var PITCH_MAG = 100.0;
-    var THRUST_MAG_HAND_JETS = THRUST_MAG_FWD;
-    var JOYSTICK_YAW_MAG = YAW_MAG;
-    var JOYSTICK_PITCH_MAG = PITCH_MAG * 0.5;
-
     var thrustJoystickPosition = Controller.getJoystickPosition(THRUST_CONTROLLER);
     
     if (thrustJoystickPosition.x != 0 || thrustJoystickPosition.y != 0) {
@@ -88,9 +171,11 @@ function flyWithHydra() {
         var right = Quat.getRight(currentOrientation);
         var up = Quat.getUp(currentOrientation);
     
-        var thrustFront = Vec3.multiply(front, scale * THRUST_MAG_HAND_JETS * thrustJoystickPosition.y * thrustMultiplier * deltaTime);
+        var thrustFront = Vec3.multiply(front, MyAvatar.scale * THRUST_MAG_HAND_JETS * 
+                                        thrustJoystickPosition.y * thrustMultiplier * deltaTime);
         MyAvatar.addThrust(thrustFront);
-        var thrustRight = Vec3.multiply(right, scale * THRUST_MAG_HAND_JETS * thrustJoystickPosition.x * thrustMultiplier * deltaTime);
+        var thrustRight = Vec3.multiply(right, MyAvatar.scale * THRUST_MAG_HAND_JETS * 
+                                        thrustJoystickPosition.x * thrustMultiplier * deltaTime);
         MyAvatar.addThrust(thrustRight);
     } else {
         thrustMultiplier = INITIAL_THRUST_MULTPLIER;
@@ -110,56 +195,9 @@ function flyWithHydra() {
         var newPitch = MyAvatar.headPitch + (viewJoystickPosition.y * JOYSTICK_PITCH_MAG * deltaTime);
         MyAvatar.headPitch = newPitch;
     }
-    
-    // check for and handle grab behaviors
-    grabbingWithRightHand = Controller.isButtonPressed(RIGHT_BUTTON_4);
-    grabbingWithLeftHand = Controller.isButtonPressed(LEFT_BUTTON_4);
-
-    if (grabbingWithRightHand) {
-        grabDelta = Vec3.sum(grabDelta, Vec3.multiply(Controller.getSpatialControlVelocity(RIGHT_PALM), deltaTime));
-        grabCurrentRotation = Controller.getSpatialControlRawRotation(RIGHT_PALM);
-    }
-    if (!grabbingWithRightHand && wasGrabbingWithRightHand) {
-        // Just ending grab, capture velocity
-        grabDeltaVelocity = Controller.getSpatialControlVelocity(RIGHT_PALM);
-    }
-    if (grabbingWithRightHand && !wasGrabbingWithRightHand) {
-        // Just starting grab, capture starting rotation
-        grabStartRotation = Controller.getSpatialControlRawRotation(RIGHT_PALM);
-    }
-
-    grabbing = grabbingWithRightHand || grabbingWithLeftHand;
-
-    if (grabbing) {
-    
-        // move position
-        var moveFromGrab = getAndResetGrabDelta();
-        if (Vec3.length(moveFromGrab) > EPSILON) {
-            position = Vec3.sum(position, moveFromGrab);
-            velocity = { x: 0, y: 0, z: 0};
-        }
-        
-        // add some velocity...
-        velocity = Vec3.sum(velocity, getAndResetGrabDeltaVelocity());
-        
-        // add some rotation...
-        var deltaRotation = getAndResetGrabRotation();
-        var GRAB_CONTROLLER_TURN_SCALING = 0.5;
-        var euler = Vec3.multiply(Quat.safeEulerAngles(deltaRotation), GRAB_CONTROLLER_TURN_SCALING);
-    
-        //  Adjust body yaw by yaw from controller
-        var orientation = Quat.multiply(Quat.angleAxis(-euler.y, {x:0, y: 1, z:0}), MyAvatar.orientation);
-        MyAvatar.orientation = orientation;
-
-        //  Adjust head pitch from controller
-        //getHead().setPitch(getHead().getPitch() - euler.x);
-        MyAvatar.headPitch = MyAvatar.headPitch - euler.x;
-    }
-    
-    wasGrabbingWithRightHand = grabbingWithRightHand;
-    wasGrabbingWithLeftHand = grabbingWithLeftHand;
+    handleGrabBehavior();
 }
-
+    
 Script.willSendVisualDataCallback.connect(flyWithHydra);
 Controller.captureJoystick(THRUST_CONTROLLER);
 Controller.captureJoystick(VIEW_CONTROLLER);
diff --git a/libraries/avatars/src/AvatarData.h b/libraries/avatars/src/AvatarData.h
index 46d92c0f2e..08604a95f1 100755
--- a/libraries/avatars/src/AvatarData.h
+++ b/libraries/avatars/src/AvatarData.h
@@ -67,6 +67,7 @@ class AvatarData : public NodeData {
     Q_OBJECT
 
     Q_PROPERTY(glm::vec3 position READ getPosition WRITE setPosition)
+    Q_PROPERTY(float scale READ getTargetScale WRITE setTargetScale)
     Q_PROPERTY(glm::vec3 handPosition READ getHandPosition WRITE setHandPosition)
     Q_PROPERTY(float bodyYaw READ getBodyYaw WRITE setBodyYaw)
     Q_PROPERTY(float bodyPitch READ getBodyPitch WRITE setBodyPitch)

From 4b2ddc90214b2400f2871b9ac3cbd0e338ab0282 Mon Sep 17 00:00:00 2001
From: ZappoMan <bradh@konamoxt.com>
Date: Mon, 10 Feb 2014 19:53:34 -0800
Subject: [PATCH 07/13] remove dead comment

---
 examples/hydraMove.js | 2 --
 1 file changed, 2 deletions(-)

diff --git a/examples/hydraMove.js b/examples/hydraMove.js
index 6223e6fe2c..6ce0ad884e 100644
--- a/examples/hydraMove.js
+++ b/examples/hydraMove.js
@@ -138,8 +138,6 @@ function handleGrabBehavior() {
         MyAvatar.headPitch = MyAvatar.headPitch - euler.x;
     }
 
-    //print("velocity=" +velocity.x +"," +velocity.y +"," +velocity.z + " length=" + Vec3.length(velocity));
-
     // add some velocity...
     if (stoppedGrabbing) {
         velocity = Vec3.sum(velocity, getAndResetGrabDeltaVelocity());

From 9244180cb45ca69175ad7f99e53971c4e85a7a2c Mon Sep 17 00:00:00 2001
From: ZappoMan <bradh@konamoxt.com>
Date: Mon, 10 Feb 2014 20:27:39 -0800
Subject: [PATCH 08/13] removed debug

---
 examples/cameraExample.js | 1 -
 1 file changed, 1 deletion(-)

diff --git a/examples/cameraExample.js b/examples/cameraExample.js
index 65cffb17dd..d55f376b76 100644
--- a/examples/cameraExample.js
+++ b/examples/cameraExample.js
@@ -75,7 +75,6 @@ function checkCamera() {
         if (yaw < -360) {
             yaw += 360;
         }
-        print("pitch="+ pitch + " yaw="+ yaw + " roll=" + roll);
         var orientation = Quat.fromPitchYawRoll(pitch, yaw, roll);
         Camera.setOrientation(orientation);
     }

From 861778347f615b843c13b4040270e6cfb2990dd4 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 11 Feb 2014 08:54:40 -0800
Subject: [PATCH 09/13] Fixing windows compile warnings about signed vs
 unsigned compare.

---
 interface/src/avatar/Hand.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp
index c2ea3f8f31..53f023f370 100644
--- a/interface/src/avatar/Hand.cpp
+++ b/interface/src/avatar/Hand.cpp
@@ -223,7 +223,7 @@ void Hand::updateCollisions() {
                     }
                 }
                 if (avatar->findSphereCollisions(palm.getPosition(), scaledPalmRadius, collisions)) {
-                    for (size_t j = 0; j < collisions.size(); ++j) {
+                    for (int j = 0; j < collisions.size(); ++j) {
                         if (!avatar->poke(collisions[j])) {
                             totalPenetration = addPenetrations(totalPenetration, collisions[j]._penetration);
                         }
@@ -240,7 +240,7 @@ void Hand::updateCollisions() {
                 skeletonModel.getLastFreeJointIndex((i == leftPalmIndex) ? skeletonModel.getLeftHandJointIndex() :
                     (i == rightPalmIndex) ? skeletonModel.getRightHandJointIndex() : -1)));
             if (_owningAvatar->findSphereCollisions(palm.getPosition(), scaledPalmRadius, collisions, skipIndex)) {
-                for (size_t j = 0; j < collisions.size(); ++j) {
+                for (int j = 0; j < collisions.size(); ++j) {
                     totalPenetration = addPenetrations(totalPenetration, collisions[j]._penetration);
                 }
             }

From 887fa0c93888bf054365b138ead8ab95070a3d21 Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 11 Feb 2014 09:23:40 -0800
Subject: [PATCH 10/13] Only resolve our hand collisions that would not move
 the other avatar.

This helps us only penetrate the moveable parts of other avatars.
---
 interface/src/avatar/Avatar.cpp  | 11 +++++++++--
 interface/src/avatar/Avatar.h    |  3 +++
 interface/src/avatar/Hand.cpp    |  3 ++-
 interface/src/renderer/Model.cpp | 14 ++++++++++++++
 interface/src/renderer/Model.h   |  3 +++
 5 files changed, 31 insertions(+), 3 deletions(-)

diff --git a/interface/src/avatar/Avatar.cpp b/interface/src/avatar/Avatar.cpp
index c299c0c617..9cb69e170e 100644
--- a/interface/src/avatar/Avatar.cpp
+++ b/interface/src/avatar/Avatar.cpp
@@ -443,9 +443,16 @@ float Avatar::getHeight() const {
     return extents.maximum.y - extents.minimum.y;
 }
 
-bool Avatar::poke(ModelCollisionInfo& collision) {
-    // ATM poke() can only affect the Skeleton (not the head)
+bool Avatar::isPokeable(ModelCollisionInfo& collision) const {
+    // ATM only the Skeleton is pokeable
     // TODO: make poke affect head
+    if (collision._model == &_skeletonModel && collision._jointIndex != -1) {
+        return _skeletonModel.isPokeable(collision);
+    }
+    return false;
+}
+
+bool Avatar::poke(ModelCollisionInfo& collision) {
     if (collision._model == &_skeletonModel && collision._jointIndex != -1) {
         return _skeletonModel.poke(collision);
     }
diff --git a/interface/src/avatar/Avatar.h b/interface/src/avatar/Avatar.h
index 8290115240..7e8a1d8f64 100755
--- a/interface/src/avatar/Avatar.h
+++ b/interface/src/avatar/Avatar.h
@@ -128,6 +128,9 @@ public:
 
     float getHeight() const;
 
+    /// \return true if we expect the avatar would move as a result of the collision
+    bool isPokeable(ModelCollisionInfo& collision) const;
+
     /// \param collision a data structure for storing info about collisions against Models
     /// \return true if the collision affects the Avatar models
     bool poke(ModelCollisionInfo& collision);
diff --git a/interface/src/avatar/Hand.cpp b/interface/src/avatar/Hand.cpp
index 53f023f370..4dac42a02e 100644
--- a/interface/src/avatar/Hand.cpp
+++ b/interface/src/avatar/Hand.cpp
@@ -224,7 +224,8 @@ void Hand::updateCollisions() {
                 }
                 if (avatar->findSphereCollisions(palm.getPosition(), scaledPalmRadius, collisions)) {
                     for (int j = 0; j < collisions.size(); ++j) {
-                        if (!avatar->poke(collisions[j])) {
+                        // we don't resolve penetrations that would poke the other avatar
+                        if (!avatar->isPokeable(collisions[j])) {
                             totalPenetration = addPenetrations(totalPenetration, collisions[j]._penetration);
                         }
                     }
diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index e1652b1237..b74882de5f 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -713,6 +713,20 @@ void Model::renderCollisionProxies(float alpha) {
     glPopMatrix();
 }
 
+bool Model::isPokeable(ModelCollisionInfo& collision) const {
+    // the joint is pokable by a collision if it exists and is free to move
+    const FBXJoint& joint = _geometry->getFBXGeometry().joints[collision._jointIndex];
+    if (joint.parentIndex == -1  || 
+        _jointStates.isEmpty())
+    {
+        return false;
+    }
+    // an empty freeLineage means the joint can't move
+    const FBXGeometry& geometry = _geometry->getFBXGeometry();
+    const QVector<int>& freeLineage = geometry.joints.at(collision._jointIndex).freeLineage;
+    return !freeLineage.isEmpty();
+}
+
 bool Model::poke(ModelCollisionInfo& collision) {
     // This needs work.  At the moment it can wiggle joints that are free to move (such as arms)
     // but unmovable joints (such as torso) cannot be influenced at all.
diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h
index f99e46c5f4..9ef9625b01 100644
--- a/interface/src/renderer/Model.h
+++ b/interface/src/renderer/Model.h
@@ -164,6 +164,9 @@ public:
     
     void renderCollisionProxies(float alpha);
 
+    /// \return true if the collision would move the model
+    bool isPokeable(ModelCollisionInfo& collision) const;
+
     /// \param collisionInfo info about the collision
     /// \return true if collision affects the Model
     bool poke(ModelCollisionInfo& collisionInfo);

From 1fca763b9d5ebca51913fe754d67d891678b14b3 Mon Sep 17 00:00:00 2001
From: ZappoMan <bradh@konamoxt.com>
Date: Tue, 11 Feb 2014 12:43:48 -0800
Subject: [PATCH 11/13] removed whitespace to get build to run

---
 libraries/script-engine/src/Quat.h | 2 --
 1 file changed, 2 deletions(-)

diff --git a/libraries/script-engine/src/Quat.h b/libraries/script-engine/src/Quat.h
index 985dd0a00f..867069d6d6 100644
--- a/libraries/script-engine/src/Quat.h
+++ b/libraries/script-engine/src/Quat.h
@@ -31,6 +31,4 @@ public slots:
     glm::quat angleAxis(float angle, const glm::vec3& v);
 };
 
-
-
 #endif /* defined(__hifi__Quat__) */

From d0f9b7871062be9ea6590b2893987c8d87dc574e Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 11 Feb 2014 13:48:48 -0800
Subject: [PATCH 12/13] Adding avatar body-body collisions to prevent
 near-clipping.

---
 interface/src/avatar/MyAvatar.cpp    | 66 +++++++++++++++++++++++++---
 interface/src/renderer/FBXReader.cpp |  9 ++++
 interface/src/renderer/FBXReader.h   |  1 +
 interface/src/renderer/Model.cpp     |  9 ++++
 interface/src/renderer/Model.h       |  3 ++
 5 files changed, 81 insertions(+), 7 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 5628740770..5a4512419d 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -225,8 +225,6 @@ void MyAvatar::simulate(float deltaTime) {
             updateCollisionWithVoxels(deltaTime, radius);
         }
         if (_collisionFlags & COLLISION_GROUP_AVATARS) {
-            // Note, hand-vs-avatar collisions are done elsewhere
-            // This is where we avatar-vs-avatar bounding capsule
             updateCollisionWithAvatars(deltaTime);
         }
     }
@@ -974,7 +972,43 @@ void MyAvatar::updateCollisionSound(const glm::vec3 &penetration, float deltaTim
     }
 }
 
-const float DEFAULT_HAND_RADIUS = 0.1f;
+bool findAvatarAvatarPenetration(const glm::vec3 positionA, float radiusA, float heightA,
+        const glm::vec3 positionB, float radiusB, float heightB, glm::vec3& penetration) {
+    glm::vec3 positionBA = positionB - positionA;
+    float xzDistance = sqrt(positionBA.x * positionBA.x + positionBA.z * positionBA.z);
+    if (xzDistance < (radiusA + radiusB)) {
+        float yDistance = fabs(positionBA.y);
+        float halfHeights = 0.5 * (heightA + heightB);
+        if (yDistance < halfHeights) {
+            // cylinders collide
+            if (xzDistance > 0.f) {
+                positionBA.y = 0.f;
+                // note, penetration should point from A into B
+                penetration = positionBA * ((radiusA + radiusB - xzDistance) / xzDistance);
+                return true;
+            } else {
+                // exactly coaxial -- we'll return false for this case
+                return false;
+            }
+        } else if (yDistance < halfHeights + radiusA + radiusB) {
+            // caps collide
+            if (positionBA.y < 0.f) {
+                // A is above B
+                positionBA.y += halfHeights;
+                float BA = glm::length(positionBA);
+                penetration = positionBA * (radiusA + radiusB - BA) / BA;
+                return true;
+            } else {
+                // A is below B
+                positionBA.y -= halfHeights;
+                float BA = glm::length(positionBA);
+                penetration = positionBA * (radiusA + radiusB - BA) / BA;
+                return true;
+            }
+        }
+    }
+    return false;
+}
 
 void MyAvatar::updateCollisionWithAvatars(float deltaTime) {
     //  Reset detector for nearest avatar
@@ -984,7 +1018,14 @@ void MyAvatar::updateCollisionWithAvatars(float deltaTime) {
         // no need to compute a bunch of stuff if we have one or fewer avatars
         return;
     }
-    float myRadius = getHeight();
+    float myBoundingRadius = 0.5f * getHeight();
+
+    // HACK: body-body collision uses two coaxial capsules with axes parallel to y-axis
+    // TODO: make the collision work without assuming avatar orientation
+    Extents myStaticExtents = _skeletonModel.getStaticExtents();
+    glm::vec3 staticScale = myStaticExtents.maximum - myStaticExtents.minimum;
+    float myCapsuleRadius = 0.25f * (staticScale.x + staticScale.z);
+    float myCapsuleHeight = staticScale.y;
 
     CollisionInfo collisionInfo;
     foreach (const AvatarSharedPointer& avatarPointer, avatars) {
@@ -997,9 +1038,20 @@ void MyAvatar::updateCollisionWithAvatars(float deltaTime) {
         if (_distanceToNearestAvatar > distance) {
             _distanceToNearestAvatar = distance;
         }
-        float theirRadius = avatar->getHeight();
-        if (distance < myRadius + theirRadius) {
-            // TODO: Andrew to make avatar-avatar capsule collisions work here
+        float theirBoundingRadius = 0.5f * avatar->getHeight();
+        if (distance < myBoundingRadius + theirBoundingRadius) {
+            Extents theirStaticExtents = _skeletonModel.getStaticExtents();
+            glm::vec3 staticScale = theirStaticExtents.maximum - theirStaticExtents.minimum;
+            float theirCapsuleRadius = 0.25f * (staticScale.x + staticScale.z);
+            float theirCapsuleHeight = staticScale.y;
+
+            glm::vec3 penetration(0.f);
+            if (findAvatarAvatarPenetration(_position, myCapsuleRadius, myCapsuleHeight,
+                avatar->getPosition(), theirCapsuleRadius, theirCapsuleHeight, penetration)) {
+                // move the avatar out by half the penetration
+                setPosition(_position - 0.5f * penetration);
+                glm::vec3 pushOut = 0.5f * penetration;
+            }
         }
     }
 }
diff --git a/interface/src/renderer/FBXReader.cpp b/interface/src/renderer/FBXReader.cpp
index 237ba7196d..b0bdf420f2 100644
--- a/interface/src/renderer/FBXReader.cpp
+++ b/interface/src/renderer/FBXReader.cpp
@@ -1268,6 +1268,8 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
 
     geometry.bindExtents.minimum = glm::vec3(FLT_MAX, FLT_MAX, FLT_MAX);
     geometry.bindExtents.maximum = glm::vec3(-FLT_MAX, -FLT_MAX, -FLT_MAX);
+    geometry.staticExtents.minimum = glm::vec3(FLT_MAX, FLT_MAX, FLT_MAX);
+    geometry.staticExtents.maximum = glm::vec3(-FLT_MAX, -FLT_MAX, -FLT_MAX);
     
     QVariantHash springs = mapping.value("spring").toHash();
     QVariant defaultSpring = springs.value("default");
@@ -1424,6 +1426,8 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
                         boneDirection /= boneLength;
                     }
                 }
+                bool jointIsStatic = joint.freeLineage.isEmpty();
+                glm::vec3 jointTranslation = extractTranslation(geometry.offset * joint.bindTransform);
                 float radiusScale = extractUniformScale(joint.transform * fbxCluster.inverseBindMatrix);
                 float totalWeight = 0.0f;
                 for (int j = 0; j < cluster.indices.size(); j++) {
@@ -1441,6 +1445,11 @@ FBXGeometry extractFBXGeometry(const FBXNode& node, const QVariantHash& mapping)
                                 joint.boneRadius = glm::max(joint.boneRadius, radiusScale * glm::distance(
                                     vertex, boneEnd + boneDirection * proj));
                             }
+                            if (jointIsStatic) {
+                                // expand the extents of static (nonmovable) joints
+                                geometry.staticExtents.minimum = glm::min(geometry.staticExtents.minimum, vertex + jointTranslation);
+                                geometry.staticExtents.maximum = glm::max(geometry.staticExtents.maximum, vertex + jointTranslation);
+                            }
                         }
 
                         // look for an unused slot in the weights vector
diff --git a/interface/src/renderer/FBXReader.h b/interface/src/renderer/FBXReader.h
index d700439460..eb4c5bac41 100644
--- a/interface/src/renderer/FBXReader.h
+++ b/interface/src/renderer/FBXReader.h
@@ -159,6 +159,7 @@ public:
     glm::vec3 neckPivot;
     
     Extents bindExtents;
+    Extents staticExtents;
     
     QVector<FBXAttachment> attachments;
 };
diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index b74882de5f..ae1fb203d1 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -305,6 +305,15 @@ Extents Model::getBindExtents() const {
     return scaledExtents;
 }
 
+Extents Model::getStaticExtents() const {
+    if (!isActive()) {
+        return Extents();
+    }
+    const Extents& staticExtents = _geometry->getFBXGeometry().staticExtents;
+    Extents scaledExtents = { staticExtents.minimum * _scale, staticExtents.maximum * _scale };
+    return scaledExtents;
+}
+
 int Model::getParentJointIndex(int jointIndex) const {
     return (isActive() && jointIndex != -1) ? _geometry->getFBXGeometry().joints.at(jointIndex).parentIndex : -1;
 }
diff --git a/interface/src/renderer/Model.h b/interface/src/renderer/Model.h
index 9ef9625b01..ddf80b8b21 100644
--- a/interface/src/renderer/Model.h
+++ b/interface/src/renderer/Model.h
@@ -66,6 +66,9 @@ public:
     
     /// Returns the extents of the model in its bind pose.
     Extents getBindExtents() const;
+
+    /// Returns the extents of the unmovable joints of the model.
+    Extents getStaticExtents() const;
     
     /// Returns a reference to the shared geometry.
     const QSharedPointer<NetworkGeometry>& getGeometry() const { return _geometry; }

From 66dc4e17ad5b60ea1728dc6fd95e02731c315e6a Mon Sep 17 00:00:00 2001
From: Andrew Meadows <andrew@highfidelity.io>
Date: Tue, 11 Feb 2014 15:59:35 -0800
Subject: [PATCH 13/13] Fixing formatting to be KR

---
 interface/src/renderer/Model.cpp | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/interface/src/renderer/Model.cpp b/interface/src/renderer/Model.cpp
index ae1fb203d1..32963cb703 100644
--- a/interface/src/renderer/Model.cpp
+++ b/interface/src/renderer/Model.cpp
@@ -725,9 +725,7 @@ void Model::renderCollisionProxies(float alpha) {
 bool Model::isPokeable(ModelCollisionInfo& collision) const {
     // the joint is pokable by a collision if it exists and is free to move
     const FBXJoint& joint = _geometry->getFBXGeometry().joints[collision._jointIndex];
-    if (joint.parentIndex == -1  || 
-        _jointStates.isEmpty())
-    {
+    if (joint.parentIndex == -1 || _jointStates.isEmpty()) {
         return false;
     }
     // an empty freeLineage means the joint can't move