From dbe65df90b7dd4268aeb0d61535f62351b40fea1 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Tue, 23 Aug 2016 16:42:02 -0700
Subject: [PATCH 01/44] merge from upstream

---
 .../system/controllers/handControllerGrab.js  | 261 ++++++++++--------
 1 file changed, 149 insertions(+), 112 deletions(-)

diff --git a/scripts/system/controllers/handControllerGrab.js b/scripts/system/controllers/handControllerGrab.js
index ab1fe76a91..664ba53258 100644
--- a/scripts/system/controllers/handControllerGrab.js
+++ b/scripts/system/controllers/handControllerGrab.js
@@ -10,7 +10,7 @@
 //
 //  Distributed under the Apache License, Version 2.0.
 //  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
-/* global setEntityCustomData, getEntityCustomData, vec3toStr, flatten, Xform */
+/* global setEntityCustomData, getEntityCustomData, vec3toStr, flatten, Xform, Script, Quat, Vec3, MyAvatar, Entities, Overlays, Settings, Reticle, Controller, Camera, Messages, Mat4 */
 
 Script.include("/~/system/libraries/utils.js");
 Script.include("/~/system/libraries/Xform.js");
@@ -22,12 +22,12 @@ var WANT_DEBUG = false;
 var WANT_DEBUG_STATE = false;
 var WANT_DEBUG_SEARCH_NAME = null;
 
+var FORCE_IGNORE_IK = false;
+
 //
 // these tune time-averaging and "on" value for analog trigger
 //
 
-var SPARK_MODEL_SCALE_FACTOR = 0.75;
-
 var TRIGGER_SMOOTH_RATIO = 0.1; //  Time averaging of trigger - 0.0 disables smoothing
 var TRIGGER_OFF_VALUE = 0.1;
 var TRIGGER_ON_VALUE = TRIGGER_OFF_VALUE + 0.05; //  Squeezed just enough to activate search or near grab
@@ -59,6 +59,13 @@ var EQUIP_SPHERE_COLOR = {
 var EQUIP_SPHERE_ALPHA = 0.15;
 var EQUIP_SPHERE_SCALE_FACTOR = 0.65;
 
+
+var GRAB_POINT_SPHERE_RADIUS = 0.08;
+var GRAB_POINT_SPHERE_COLOR = { red: 20, green: 90, blue: 238 };
+var GRAB_POINT_SPHERE_ALPHA = 0.85;
+
+
+
 //
 // distant manipulation
 //
@@ -87,8 +94,6 @@ var COLORS_GRAB_DISTANCE_HOLD = {
     blue: 214
 };
 
-
-var LINE_LENGTH = 500;
 var PICK_MAX_DISTANCE = 500; // max length of pick-ray
 
 //
@@ -129,7 +134,6 @@ var ZERO_VEC = {
 var NULL_UUID = "{00000000-0000-0000-0000-000000000000}";
 
 // these control how long an abandoned pointer line or action will hang around
-var LIFETIME = 10;
 var ACTION_TTL = 15; // seconds
 var ACTION_TTL_REFRESH = 5;
 var PICKS_PER_SECOND_PER_HAND = 60;
@@ -164,7 +168,7 @@ var USE_BLACKLIST = true;
 var blacklist = [];
 
 var FORBIDDEN_GRAB_NAMES = ["Grab Debug Entity", "grab pointer"];
-var FORBIDDEN_GRAB_TYPES = ['Unknown', 'Light', 'PolyLine', 'Zone'];
+var FORBIDDEN_GRAB_TYPES = ["Unknown", "Light", "PolyLine", "Zone"];
 
 // states for the state machine
 var STATE_OFF = 0;
@@ -246,11 +250,9 @@ function projectOntoEntityXYPlane(entityID, worldPos) {
              y: (1 - normalizedPos.y) * props.dimensions.y }; // flip y-axis
 }
 
-function handLaserIntersectEntity(entityID, hand) {
-    var standardControllerValue = (hand === RIGHT_HAND) ? Controller.Standard.RightHand : Controller.Standard.LeftHand;
-    var pose = Controller.getPoseValue(standardControllerValue);
-    var worldHandPosition = Vec3.sum(Vec3.multiplyQbyV(MyAvatar.orientation, pose.translation), MyAvatar.position);
-    var worldHandRotation = Quat.multiply(MyAvatar.orientation, pose.rotation);
+function handLaserIntersectEntity(entityID, start) {
+    var worldHandPosition = start.position;
+    var worldHandRotation = start.orientation;
 
     var props = entityPropertiesCache.getProps(entityID);
 
@@ -328,7 +330,7 @@ function entityIsGrabbedByOther(entityID) {
     for (var actionIndex = 0; actionIndex < actionIDs.length; actionIndex++) {
         var actionID = actionIDs[actionIndex];
         var actionArguments = Entities.getActionArguments(entityID, actionID);
-        var tag = actionArguments["tag"];
+        var tag = actionArguments.tag;
         if (tag == getTag()) {
             // we see a grab-*uuid* shaped tag, but it's our tag, so that's okay.
             continue;
@@ -653,9 +655,7 @@ EquipHotspotBuddy.prototype.update = function(deltaTime, timestamp) {
 
         if (overlayInfoSet.timestamp != timestamp && overlayInfoSet.currentSize <= 0.05) {
             // this is an old overlay, that has finished fading out, delete it!
-            overlayInfoSet.overlays.forEach(function(overlay) {
-                Overlays.deleteOverlay(overlay);
-            });
+            overlayInfoSet.overlays.forEach(Overlays.deleteOverlay);
             delete this.map[keys[i]];
         } else {
             // update overlay position, rotation to follow the object it's attached to.
@@ -687,16 +687,28 @@ var equipHotspotBuddy = new EquipHotspotBuddy();
 
 function MyController(hand) {
     this.hand = hand;
-    if (this.hand === RIGHT_HAND) {
-        this.getHandPosition = MyAvatar.getRightPalmPosition;
-        // this.getHandRotation = MyAvatar.getRightPalmRotation;
-    } else {
-        this.getHandPosition = MyAvatar.getLeftPalmPosition;
-        // this.getHandRotation = MyAvatar.getLeftPalmRotation;
-    }
-    this.getHandRotation = function() {
-        var controllerHandInput = (this.hand === RIGHT_HAND) ? Controller.Standard.RightHand : Controller.Standard.LeftHand;
-        return Quat.multiply(MyAvatar.orientation, Controller.getPoseValue(controllerHandInput).rotation);
+
+    // handPosition is where the avatar's hand appears to be, in-world.
+    this.getHandPosition = function () {
+        if (this.hand === RIGHT_HAND) {
+            return MyAvatar.getRightPalmPosition();
+        } else {
+            return MyAvatar.getLeftPalmPosition();
+        }
+    };
+    this.getHandRotation = function () {
+        if (this.hand === RIGHT_HAND) {
+            return MyAvatar.getRightPalmRotation();
+        } else {
+            return MyAvatar.getLeftPalmRotation();
+        }
+    };
+    // controllerPosition is where the controller would be, in-world.
+    this.getControllerLocation = function () {
+        var standardControllerValue = (hand === RIGHT_HAND) ? Controller.Standard.RightHand : Controller.Standard.LeftHand;
+        var pose = Controller.getPoseValue(standardControllerValue);
+        return {position: Vec3.sum(Vec3.multiplyQbyV(MyAvatar.orientation, pose.translation), MyAvatar.position),
+                orientation: Quat.multiply(MyAvatar.orientation, pose.rotation)};
     };
 
     this.actionID = null; // action this script created...
@@ -803,6 +815,35 @@ function MyController(hand) {
         }
     };
 
+
+    this.grabPointSphereOn = function() {
+        var controllerLocation = this.getControllerLocation();
+        if (this.grabPointSphere) {
+            Overlays.editOverlay(this.grabPointSphere, {
+                position: controllerLocation.position
+            });            
+        } else {
+            this.grabPointSphere = Overlays.addOverlay("sphere", {
+                position: controllerLocation.position,
+                rotation: { x: 0, y: 0, z: 0, w: 1 },
+                dimensions: GRAB_POINT_SPHERE_RADIUS,
+                color: GRAB_POINT_SPHERE_COLOR,
+                alpha: GRAB_POINT_SPHERE_ALPHA,
+                solid: true,
+                visible: true,
+                ignoreRayIntersection: true,
+                drawInFront: false
+            });
+        }
+    };
+
+    this.grabPointSphereOff = function() {
+        if (this.grabPointSphere) {
+            Overlays.deleteOverlay(this.grabPointSphere);
+            this.grabPointSphere = null;
+        }
+    };
+
     this.searchSphereOn = function(location, size, color) {
 
         var rotation = Quat.lookAt(location, Camera.getPosition(), Vec3.UP);
@@ -1002,14 +1043,15 @@ function MyController(hand) {
         }
         if (!this.waitForTriggerRelease && this.triggerSmoothedSqueezed()) {
             this.lastPickTime = 0;
-            var controllerHandInput = (this.hand === RIGHT_HAND) ? Controller.Standard.RightHand : Controller.Standard.LeftHand;
-            this.startingHandRotation = Controller.getPoseValue(controllerHandInput).rotation;
+            this.startingHandRotation = this.getControllerLocation().orientation;
             if (this.triggerSmoothedSqueezed()) {
                 this.setState(STATE_SEARCHING, "trigger squeeze detected");
                 return;
             }
         }
 
+        this.grabPointSphereOn();
+
         var candidateEntities = Entities.findEntities(this.getHandPosition(), EQUIP_HOTSPOT_RENDER_RADIUS);
         entityPropertiesCache.addEntities(candidateEntities);
         var potentialEquipHotspot = this.chooseBestEquipHotspot(candidateEntities);
@@ -1033,7 +1075,8 @@ function MyController(hand) {
             !potentialEquipHotspot && this.prevPotentialEquipHotspot) {
             Controller.triggerHapticPulse(HAPTIC_TEXTURE_STRENGTH, HAPTIC_TEXTURE_DURATION, this.hand);
             this.lastHapticPulseLocation = currentLocation;
-        } else if (potentialEquipHotspot && Vec3.distance(this.lastHapticPulseLocation, currentLocation) > HAPTIC_TEXTURE_DISTANCE) {
+        } else if (potentialEquipHotspot &&
+                   Vec3.distance(this.lastHapticPulseLocation, currentLocation) > HAPTIC_TEXTURE_DISTANCE) {
             Controller.triggerHapticPulse(HAPTIC_TEXTURE_STRENGTH, HAPTIC_TEXTURE_DURATION, this.hand);
             this.lastHapticPulseLocation = currentLocation;
         }
@@ -1045,11 +1088,9 @@ function MyController(hand) {
     // @returns {object} returns object with two keys entityID and distance
     //
     this.calcRayPickInfo = function(hand) {
-
-        var standardControllerValue = (hand === RIGHT_HAND) ? Controller.Standard.RightHand : Controller.Standard.LeftHand;
-        var pose = Controller.getPoseValue(standardControllerValue);
-        var worldHandPosition = Vec3.sum(Vec3.multiplyQbyV(MyAvatar.orientation, pose.translation), MyAvatar.position);
-        var worldHandRotation = Quat.multiply(MyAvatar.orientation, pose.rotation);
+        var controllerLocation = this.getControllerLocation();
+        var worldHandPosition = controllerLocation.position;
+        var worldHandRotation = controllerLocation.orientation;
 
         var pickRay = {
             origin: PICK_WITH_HAND_RAY ? worldHandPosition : Camera.position,
@@ -1522,16 +1563,11 @@ function MyController(hand) {
     this.distanceHoldingEnter = function() {
         Messages.sendLocalMessage('Hifi-Teleport-Disabler','both');
         this.clearEquipHaptics();
+        this.grabPointSphereOff();
 
-        // controller pose is in avatar frame
-        var device = (this.hand === RIGHT_HAND) ? Controller.Standard.RightHand : Controller.Standard.LeftHand;
-        var avatarControllerPose = Controller.getPoseValue(device);
+        var worldControllerPosition = this.getControllerLocation().position;
 
-        // transform it into world frame
-        var worldControllerPosition = Vec3.sum(MyAvatar.position,
-            Vec3.multiplyQbyV(MyAvatar.orientation, avatarControllerPose.translation));
-
-        // also transform the position into room space
+        // transform the position into room space
         var worldToSensorMat = Mat4.inverse(MyAvatar.getSensorToWorldMatrix());
         var roomControllerPosition = Mat4.transformPoint(worldToSensorMat, worldControllerPosition);
 
@@ -1594,14 +1630,10 @@ function MyController(hand) {
 
         this.heartBeat(this.grabbedEntity);
 
-        // controller pose is in avatar frame
-        var device = (this.hand === RIGHT_HAND) ? Controller.Standard.RightHand : Controller.Standard.LeftHand;
-        var avatarControllerPose = Controller.getPoseValue(device);
 
-        // transform it into world frame
-        var worldControllerPosition = Vec3.sum(MyAvatar.position,
-            Vec3.multiplyQbyV(MyAvatar.orientation, avatarControllerPose.translation));
-        var worldControllerRotation = Quat.multiply(MyAvatar.orientation, avatarControllerPose.rotation);
+        var controllerLocation = this.getControllerLocation();
+        var worldControllerPosition = controllerLocation.position;
+        var worldControllerRotation = controllerLocation.orientation;
 
         // also transform the position into room space
         var worldToSensorMat = Mat4.inverse(MyAvatar.getSensorToWorldMatrix());
@@ -1672,8 +1704,6 @@ function MyController(hand) {
             }
         }
 
-        var handPosition = this.getHandPosition();
-
         // visualizations
 
         var rayPickInfo = this.calcRayPickInfo(this.hand);
@@ -1738,10 +1768,7 @@ function MyController(hand) {
     };
 
     this.dropGestureProcess = function(deltaTime) {
-        var standardControllerValue = (this.hand === RIGHT_HAND) ? Controller.Standard.RightHand : Controller.Standard.LeftHand;
-        var pose = Controller.getPoseValue(standardControllerValue);
-        var worldHandRotation = Quat.multiply(MyAvatar.orientation, pose.rotation);
-
+        var worldHandRotation = this.getControllerLocation().orientation;
         var localHandUpAxis = this.hand === RIGHT_HAND ? {
             x: 1,
             y: 0,
@@ -1780,12 +1807,11 @@ function MyController(hand) {
     this.nearGrabbingEnter = function() {
         if (this.hand === 0) {
             Messages.sendLocalMessage('Hifi-Teleport-Disabler', 'left');
-
         }
         if (this.hand === 1) {
             Messages.sendLocalMessage('Hifi-Teleport-Disabler', 'right');
-
         }
+        this.grabPointSphereOff();
         this.lineOff();
         this.overlayLineOff();
 
@@ -1809,12 +1835,23 @@ function MyController(hand) {
         var grabbedProperties = Entities.getEntityProperties(this.grabbedEntity, GRABBABLE_PROPERTIES);
         this.activateEntity(this.grabbedEntity, grabbedProperties, false);
 
-        // var handRotation = this.getHandRotation();
-        var handRotation = (this.hand === RIGHT_HAND) ? MyAvatar.getRightPalmRotation() : MyAvatar.getLeftPalmRotation();
-        var handPosition = this.getHandPosition();
-
         var grabbableData = getEntityCustomData(GRABBABLE_DATA_KEY, this.grabbedEntity, DEFAULT_GRABBABLE_DATA);
-        this.ignoreIK = grabbableData.ignoreIK ? grabbableData.ignoreIK : false;
+        if (FORCE_IGNORE_IK) {
+            this.ignoreIK = true;
+        } else {
+            this.ignoreIK = grabbableData.ignoreIK ? grabbableData.ignoreIK : false;
+        }
+
+        var handRotation;
+        var handPosition;
+        if (this.ignoreIK) {
+            var controllerLocation = this.getControllerLocation();
+            handRotation = controllerLocation.orientation;
+            handPosition = controllerLocation.position;
+        } else {
+            handRotation = this.getHandRotation();
+            handPosition = this.getHandPosition();
+        }
 
         var hasPresetPosition = false;
         if (this.state == STATE_HOLD && this.grabbedHotspot) {
@@ -1866,8 +1903,8 @@ function MyController(hand) {
                 angularVelocity: {x: 0, y: 0, z: 0}
             };
             if (hasPresetPosition) {
-                reparentProps["localPosition"] = this.offsetPosition;
-                reparentProps["localRotation"] = this.offsetRotation;
+                reparentProps.localPosition = this.offsetPosition;
+                reparentProps.localRotation = this.offsetRotation;
             }
             Entities.editEntity(this.grabbedEntity, reparentProps);
 
@@ -1941,9 +1978,10 @@ function MyController(hand) {
 
                 // store the offset attach points into preferences.
                 if (USE_ATTACH_POINT_SETTINGS && this.grabbedHotspot && this.grabbedEntity) {
-                    var props = Entities.getEntityProperties(this.grabbedEntity, ["localPosition", "localRotation"]);
-                    if (props && props.localPosition && props.localRotation) {
-                        storeAttachPointForHotspotInSettings(this.grabbedHotspot, this.hand, props.localPosition, props.localRotation);
+                    var prefprops = Entities.getEntityProperties(this.grabbedEntity, ["localPosition", "localRotation"]);
+                    if (prefprops && prefprops.localPosition && prefprops.localRotation) {
+                        storeAttachPointForHotspotInSettings(this.grabbedHotspot, this.hand,
+                                                             prefprops.localPosition, prefprops.localRotation);
                     }
                 }
 
@@ -2050,16 +2088,15 @@ function MyController(hand) {
     };
 
     this.nearTriggerEnter = function() {
-
         this.clearEquipHaptics();
-
+        this.grabPointSphereOff();
         Controller.triggerShortHapticPulse(1.0, this.hand);
         this.callEntityMethodOnGrabbed("startNearTrigger");
     };
 
     this.farTriggerEnter = function() {
         this.clearEquipHaptics();
-
+        this.grabPointSphereOff();
         this.callEntityMethodOnGrabbed("startFarTrigger");
     };
 
@@ -2082,7 +2119,7 @@ function MyController(hand) {
         var handPosition = this.getHandPosition();
         var pickRay = {
             origin: handPosition,
-            direction: Quat.getUp(this.getHandRotation())
+            direction: Quat.getUp(this.getControllerRotation())
         };
 
         var now = Date.now();
@@ -2111,7 +2148,7 @@ function MyController(hand) {
 
     this.entityTouchingEnter = function() {
         // test for intersection between controller laser and web entity plane.
-        var intersectInfo = handLaserIntersectEntity(this.grabbedEntity, this.hand);
+        var intersectInfo = handLaserIntersectEntity(this.grabbedEntity, this.getControllerLocation());
         if (intersectInfo) {
             var pointerEvent = {
                 type: "Press",
@@ -2133,7 +2170,7 @@ function MyController(hand) {
 
     this.entityTouchingExit = function() {
         // test for intersection between controller laser and web entity plane.
-        var intersectInfo = handLaserIntersectEntity(this.grabbedEntity, this.hand);
+        var intersectInfo = handLaserIntersectEntity(this.grabbedEntity, this.getControllerLocation());
         if (intersectInfo) {
             var pointerEvent = {
                 type: "Release",
@@ -2164,7 +2201,7 @@ function MyController(hand) {
         }
 
         // test for intersection between controller laser and web entity plane.
-        var intersectInfo = handLaserIntersectEntity(this.grabbedEntity, this.hand);
+        var intersectInfo = handLaserIntersectEntity(this.grabbedEntity, this.getControllerLocation());
         if (intersectInfo) {
 
             if (Entities.keyboardFocusEntity != this.grabbedEntity) {
@@ -2259,7 +2296,7 @@ function MyController(hand) {
         var now = Date.now();
         if (now - this.lastHeartBeat > HEART_BEAT_INTERVAL) {
             var data = getEntityCustomData(GRAB_USER_DATA_KEY, entityID, {});
-            data["heartBeat"] = now;
+            data.heartBeat = now;
             setEntityCustomData(GRAB_USER_DATA_KEY, entityID, data);
             this.lastHeartBeat = now;
         }
@@ -2268,7 +2305,7 @@ function MyController(hand) {
     this.resetAbandonedGrab = function(entityID) {
         print("cleaning up abandoned grab on " + entityID);
         var data = getEntityCustomData(GRAB_USER_DATA_KEY, entityID, {});
-        data["refCount"] = 1;
+        data.refCount = 1;
         setEntityCustomData(GRAB_USER_DATA_KEY, entityID, data);
         this.deactivateEntity(entityID, false);
     };
@@ -2287,29 +2324,29 @@ function MyController(hand) {
             // get re-instated after all the grabs have been released) be correct.
             Script.clearTimeout(delayedDeactivateTimeout);
             delayedDeactivateTimeout = null;
-            grabbedProperties["collidesWith"] = delayedDeactivateFunc();
+            grabbedProperties.collidesWith = delayedDeactivateFunc();
         }
 
         var data = getEntityCustomData(GRAB_USER_DATA_KEY, entityID, {});
         var now = Date.now();
 
         if (wasLoaded) {
-            data["refCount"] = 1;
+            data.refCount = 1;
         } else {
-            data["refCount"] = data["refCount"] ? data["refCount"] + 1 : 1;
+            data.refCount = data.refCount ? data.refCount + 1 : 1;
 
             // zero gravity and set ignoreForCollisions in a way that lets us put them back, after all grabs are done
-            if (data["refCount"] == 1) {
-                data["heartBeat"] = now;
+            if (data.refCount == 1) {
+                data.heartBeat = now;
                 this.lastHeartBeat = now;
 
                 this.isInitialGrab = true;
-                data["gravity"] = grabbedProperties.gravity;
-                data["collidesWith"] = grabbedProperties.collidesWith;
-                data["collisionless"] = grabbedProperties.collisionless;
-                data["dynamic"] = grabbedProperties.dynamic;
-                data["parentID"] = wasLoaded ? NULL_UUID : grabbedProperties.parentID;
-                data["parentJointIndex"] = grabbedProperties.parentJointIndex;
+                data.gravity = grabbedProperties.gravity;
+                data.collidesWith = grabbedProperties.collidesWith;
+                data.collisionless = grabbedProperties.collisionless;
+                data.dynamic = grabbedProperties.dynamic;
+                data.parentID = wasLoaded ? NULL_UUID : grabbedProperties.parentID;
+                data.parentJointIndex = grabbedProperties.parentJointIndex;
 
                 var whileHeldProperties = {
                     gravity: {
@@ -2323,9 +2360,9 @@ function MyController(hand) {
                     "collidesWith": COLLIDES_WITH_WHILE_GRABBED
                 };
                 Entities.editEntity(entityID, whileHeldProperties);
-            } else if (data["refCount"] > 1) {
-                if (data["heartBeat"] === undefined ||
-                    now - data["heartBeat"] > HEART_BEAT_TIMEOUT) {
+            } else if (data.refCount > 1) {
+                if (data.heartBeat === undefined ||
+                    now - data.heartBeat > HEART_BEAT_TIMEOUT) {
                     // this entity has userData suggesting it is grabbed, but nobody is updating the hearbeat.
                     // deactivate it before grabbing.
                     this.resetAbandonedGrab(entityID);
@@ -2371,9 +2408,9 @@ function MyController(hand) {
             collidesWith: collidesWith
         });
         var data = getEntityCustomData(GRAB_USER_DATA_KEY, entityID, {});
-        if (data && data["refCount"]) {
-            data["refCount"] = data["refCount"] - 1;
-            if (data["refCount"] < 1) {
+        if (data && data.refCount) {
+            data.refCount = data.refCount - 1;
+            if (data.refCount < 1) {
                 data = null;
             }
         } else {
@@ -2393,24 +2430,24 @@ function MyController(hand) {
 
         var data = getEntityCustomData(GRAB_USER_DATA_KEY, entityID, {});
         var doDelayedDeactivate = false;
-        if (data && data["refCount"]) {
-            data["refCount"] = data["refCount"] - 1;
-            if (data["refCount"] < 1) {
+        if (data && data.refCount) {
+            data.refCount = data.refCount - 1;
+            if (data.refCount < 1) {
                 deactiveProps = {
-                    gravity: data["gravity"],
+                    gravity: data.gravity,
                     // don't set collidesWith myAvatar back right away, because thrown things tend to bounce off the
                     // avatar's capsule.
-                    collidesWith: removeMyAvatarFromCollidesWith(data["collidesWith"]),
-                    collisionless: data["collisionless"],
-                    dynamic: data["dynamic"],
-                    parentID: data["parentID"],
-                    parentJointIndex: data["parentJointIndex"]
+                    collidesWith: removeMyAvatarFromCollidesWith(data.collidesWith),
+                    collisionless: data.collisionless,
+                    dynamic: data.dynamic,
+                    parentID: data.parentID,
+                    parentJointIndex: data.parentJointIndex
                 };
 
-                doDelayedDeactivate = (data["collidesWith"].indexOf("myAvatar") >= 0);
+                doDelayedDeactivate = (data.collidesWith.indexOf("myAvatar") >= 0);
 
                 if (doDelayedDeactivate) {
-                    var delayedCollidesWith = data["collidesWith"];
+                    var delayedCollidesWith = data.collidesWith;
                     var delayedEntityID = entityID;
                     delayedDeactivateFunc = function() {
                         // set collidesWith back to original value a bit later than the rest
@@ -2430,19 +2467,19 @@ function MyController(hand) {
 
                 if (!noVelocity &&
                     parentID == MyAvatar.sessionUUID &&
-                    Vec3.length(data["gravity"]) > 0.0 &&
-                    data["dynamic"] &&
-                    data["parentID"] == NULL_UUID &&
-                    !data["collisionless"]) {
-                    deactiveProps["velocity"] = this.currentVelocity;
+                    Vec3.length(data.gravity) > 0.0 &&
+                    data.dynamic &&
+                    data.parentID == NULL_UUID &&
+                    !data.collisionless) {
+                    deactiveProps.velocity = this.currentVelocity;
                 }
                 if (noVelocity) {
-                    deactiveProps["velocity"] = {
+                    deactiveProps.velocity = {
                         x: 0.0,
                         y: 0.0,
                         z: 0.0
                     };
-                    deactiveProps["angularVelocity"] = {
+                    deactiveProps.angularVelocity = {
                         x: 0.0,
                         y: 0.0,
                         z: 0.0
@@ -2480,7 +2517,7 @@ function MyController(hand) {
                         y: 0.0,
                         z: 0.0
                     },
-                    dynamic: data["dynamic"]
+                    dynamic: data.dynamic
                 });
             }
         } else {

From 18852137b5b2c347662d6a6f3003756c0aee7bf7 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Tue, 23 Aug 2016 17:06:53 -0700
Subject: [PATCH 02/44] cleanups, avoid pop when grabbing

---
 .../system/controllers/handControllerGrab.js  | 32 ++++++++++++-------
 1 file changed, 20 insertions(+), 12 deletions(-)

diff --git a/scripts/system/controllers/handControllerGrab.js b/scripts/system/controllers/handControllerGrab.js
index 664ba53258..60fc2e019b 100644
--- a/scripts/system/controllers/handControllerGrab.js
+++ b/scripts/system/controllers/handControllerGrab.js
@@ -22,7 +22,7 @@ var WANT_DEBUG = false;
 var WANT_DEBUG_STATE = false;
 var WANT_DEBUG_SEARCH_NAME = null;
 
-var FORCE_IGNORE_IK = false;
+var FORCE_IGNORE_IK = true;
 
 //
 // these tune time-averaging and "on" value for analog trigger
@@ -60,12 +60,6 @@ var EQUIP_SPHERE_ALPHA = 0.15;
 var EQUIP_SPHERE_SCALE_FACTOR = 0.65;
 
 
-var GRAB_POINT_SPHERE_RADIUS = 0.08;
-var GRAB_POINT_SPHERE_COLOR = { red: 20, green: 90, blue: 238 };
-var GRAB_POINT_SPHERE_ALPHA = 0.85;
-
-
-
 //
 // distant manipulation
 //
@@ -107,7 +101,7 @@ var EQUIP_HOTSPOT_RENDER_RADIUS = 0.0; // radius used for palm vs equip-hotspot
 
 var NEAR_GRABBING_ACTION_TIMEFRAME = 0.05; // how quickly objects move to their new position
 
-var NEAR_GRAB_RADIUS = 0.15; // radius used for palm vs object for near grabbing.
+var NEAR_GRAB_RADIUS = 0.07; // radius used for palm vs object for near grabbing.
 var NEAR_GRAB_MAX_DISTANCE = 1.0; // you cannot grab objects that are this far away from your hand
 
 var NEAR_GRAB_PICK_RADIUS = 0.25; // radius used for search ray vs object for near grabbing.
@@ -118,6 +112,13 @@ var NEAR_GRABBING_KINEMATIC = true; // force objects to be kinematic when near-g
 // if an equipped item is "adjusted" to be too far from the hand it's in, it will be unequipped.
 var CHECK_TOO_FAR_UNEQUIP_TIME = 0.3; // seconds, duration between checks
 
+
+var GRAB_POINT_SPHERE_OFFSET = {x: 0, y: 0.2, z:0};
+var GRAB_POINT_SPHERE_RADIUS = NEAR_GRAB_RADIUS;
+var GRAB_POINT_SPHERE_COLOR = { red: 20, green: 90, blue: 238 };
+var GRAB_POINT_SPHERE_ALPHA = 0.85;
+
+
 //
 // other constants
 //
@@ -703,12 +704,16 @@ function MyController(hand) {
             return MyAvatar.getLeftPalmRotation();
         }
     };
-    // controllerPosition is where the controller would be, in-world.
+    // controllerLocation is where the controller would be, in-world.
     this.getControllerLocation = function () {
         var standardControllerValue = (hand === RIGHT_HAND) ? Controller.Standard.RightHand : Controller.Standard.LeftHand;
         var pose = Controller.getPoseValue(standardControllerValue);
-        return {position: Vec3.sum(Vec3.multiplyQbyV(MyAvatar.orientation, pose.translation), MyAvatar.position),
-                orientation: Quat.multiply(MyAvatar.orientation, pose.rotation)};
+
+        var orientation = Quat.multiply(MyAvatar.orientation, pose.rotation)
+        var position = Vec3.sum(Vec3.multiplyQbyV(MyAvatar.orientation, pose.translation), MyAvatar.position);
+        position = Vec3.sum(position, Vec3.multiplyQbyV(orientation, GRAB_POINT_SPHERE_OFFSET));
+
+        return {position: position, orientation: orientation};
     };
 
     this.actionID = null; // action this script created...
@@ -1368,6 +1373,8 @@ function MyController(hand) {
         this.isInitialGrab = false;
         this.shouldResetParentOnRelease = false;
 
+        this.grabPointSphereOn();
+
         this.checkForStrayChildren();
 
         if (this.triggerSmoothedReleased()) {
@@ -1847,7 +1854,8 @@ function MyController(hand) {
         if (this.ignoreIK) {
             var controllerLocation = this.getControllerLocation();
             handRotation = controllerLocation.orientation;
-            handPosition = controllerLocation.position;
+            handPosition = Vec3.subtract(controllerLocation.position,
+                                         Vec3.multiplyQbyV(handRotation, GRAB_POINT_SPHERE_OFFSET));
         } else {
             handRotation = this.getHandRotation();
             handPosition = this.getHandPosition();

From 39f52b36824e5e151ea30124de3c63e50d89c49e Mon Sep 17 00:00:00 2001
From: Seth Alves <seth.alves@gmail.com>
Date: Thu, 25 Aug 2016 10:58:23 -0700
Subject: [PATCH 03/44] start on faux avatar joint that represent hand
 controllers

---
 interface/src/avatar/Avatar.cpp               | 72 ++++++++++++-------
 interface/src/avatar/MyAvatar.cpp             | 22 +++++-
 interface/src/avatar/MyAvatar.h               | 10 ++-
 libraries/avatars/src/AvatarData.cpp          | 28 ++++++++
 libraries/avatars/src/AvatarData.h            | 13 ++++
 .../src/controllers/UserInputMapper.cpp       | 13 +++-
 .../src/controllers/UserInputMapper.h         |  2 +-
 7 files changed, 130 insertions(+), 30 deletions(-)

diff --git a/interface/src/avatar/Avatar.cpp b/interface/src/avatar/Avatar.cpp
index 31fb2abe53..717a0eeac7 100644
--- a/interface/src/avatar/Avatar.cpp
+++ b/interface/src/avatar/Avatar.cpp
@@ -59,8 +59,6 @@ const float DISPLAYNAME_ALPHA = 1.0f;
 const float DISPLAYNAME_BACKGROUND_ALPHA = 0.4f;
 const glm::vec3 HAND_TO_PALM_OFFSET(0.0f, 0.12f, 0.08f);
 
-const int SENSOR_TO_WORLD_MATRIX_INDEX = 65534;
-
 namespace render {
     template <> const ItemKey payloadGetKey(const AvatarSharedPointer& avatar) {
         return ItemKey::Builder::opaqueShape();
@@ -853,32 +851,54 @@ glm::vec3 Avatar::getDefaultJointTranslation(int index) const {
 }
 
 glm::quat Avatar::getAbsoluteJointRotationInObjectFrame(int index) const {
-    if (index == SENSOR_TO_WORLD_MATRIX_INDEX) {
-        glm::mat4 sensorToWorldMatrix = getSensorToWorldMatrix();
-        bool success;
-        Transform avatarTransform;
-        Transform::mult(avatarTransform, getParentTransform(success), getLocalTransform());
-        glm::mat4 invAvatarMat = avatarTransform.getInverseMatrix();
-        return glmExtractRotation(invAvatarMat * sensorToWorldMatrix);
-    } else {
-        glm::quat rotation;
-        _skeletonModel->getAbsoluteJointRotationInRigFrame(index, rotation);
-        return Quaternions::Y_180 * rotation;
+    switch(index) {
+        case SENSOR_TO_WORLD_MATRIX_INDEX: {
+            glm::mat4 sensorToWorldMatrix = getSensorToWorldMatrix();
+            bool success;
+            Transform avatarTransform;
+            Transform::mult(avatarTransform, getParentTransform(success), getLocalTransform());
+            glm::mat4 invAvatarMat = avatarTransform.getInverseMatrix();
+            return glmExtractRotation(invAvatarMat * sensorToWorldMatrix);
+        }
+        case CONTROLLER_LEFTHAND_INDEX: {
+            Transform controllerLeftHandTransform = Transform(getControllerLeftHandMatrix());
+            return controllerLeftHandTransform.getRotation();
+        }
+        case CONTROLLER_RIGHTHAND_INDEX: {
+            Transform controllerRightHandTransform = Transform(getControllerRightHandMatrix());
+            return controllerRightHandTransform.getRotation();
+        }
+        default: {
+            glm::quat rotation;
+            _skeletonModel->getAbsoluteJointRotationInRigFrame(index, rotation);
+            return Quaternions::Y_180 * rotation;
+        }
     }
 }
 
 glm::vec3 Avatar::getAbsoluteJointTranslationInObjectFrame(int index) const {
-    if (index == SENSOR_TO_WORLD_MATRIX_INDEX) {
-        glm::mat4 sensorToWorldMatrix = getSensorToWorldMatrix();
-        bool success;
-        Transform avatarTransform;
-        Transform::mult(avatarTransform, getParentTransform(success), getLocalTransform());
-        glm::mat4 invAvatarMat = avatarTransform.getInverseMatrix();
-        return extractTranslation(invAvatarMat * sensorToWorldMatrix);
-    } else {
-        glm::vec3 translation;
-        _skeletonModel->getAbsoluteJointTranslationInRigFrame(index, translation);
-        return Quaternions::Y_180 * translation;
+    switch(index) {
+        case SENSOR_TO_WORLD_MATRIX_INDEX: {
+            glm::mat4 sensorToWorldMatrix = getSensorToWorldMatrix();
+            bool success;
+            Transform avatarTransform;
+            Transform::mult(avatarTransform, getParentTransform(success), getLocalTransform());
+            glm::mat4 invAvatarMat = avatarTransform.getInverseMatrix();
+            return extractTranslation(invAvatarMat * sensorToWorldMatrix);
+        }
+        case CONTROLLER_LEFTHAND_INDEX: {
+            Transform controllerLeftHandTransform = Transform(getControllerLeftHandMatrix());
+            return controllerLeftHandTransform.getTranslation();
+        }
+        case CONTROLLER_RIGHTHAND_INDEX: {
+            Transform controllerRightHandTransform = Transform(getControllerRightHandMatrix());
+            return controllerRightHandTransform.getTranslation();
+        }
+        default: {
+            glm::vec3 translation;
+            _skeletonModel->getAbsoluteJointTranslationInRigFrame(index, translation);
+            return Quaternions::Y_180 * translation;
+        }
     }
 }
 
@@ -889,6 +909,10 @@ int Avatar::getJointIndex(const QString& name) const {
             Q_RETURN_ARG(int, result), Q_ARG(const QString&, name));
         return result;
     }
+    int result = getFauxJointIndex(name);
+    if (result != -1) {
+        return result;
+    }
     return _skeletonModel->isActive() ? _skeletonModel->getFBXGeometry().getJointIndex(name) : -1;
 }
 
diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 5687b5025c..b021b9d5b2 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -532,6 +532,23 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) {
     _hmdSensorFacing = getFacingDir2D(_hmdSensorOrientation);
 }
 
+void MyAvatar::updateJointsFromControllers() {
+    if (QThread::currentThread() != thread()) { abort(); } // XXX
+    auto userInputMapper = DependencyManager::get<UserInputMapper>();
+
+    controller::Pose leftControllerPose = userInputMapper->getPoseState(controller::Action::LEFT_HAND);
+    Transform controllerLeftHandTransform;
+    controllerLeftHandTransform.setTranslation(leftControllerPose.getTranslation());
+    controllerLeftHandTransform.setRotation(leftControllerPose.getRotation());
+    _controllerLeftHandMatrixCache.set(controllerLeftHandTransform.getMatrix());
+
+    controller::Pose rightControllerPose = userInputMapper->getPoseState(controller::Action::RIGHT_HAND);
+    Transform controllerRightHandTransform;
+    controllerRightHandTransform.setTranslation(rightControllerPose.getTranslation());
+    controllerRightHandTransform.setRotation(rightControllerPose.getRotation());
+    _controllerRightHandMatrixCache.set(controllerRightHandTransform.getMatrix());
+}
+
 // best called at end of main loop, after physics.
 // update sensor to world matrix from current body position and hmd sensor.
 // This is so the correct camera can be used for rendering.
@@ -545,10 +562,13 @@ void MyAvatar::updateSensorToWorldMatrix() {
     lateUpdatePalms();
 
     if (_enableDebugDrawSensorToWorldMatrix) {
-        DebugDraw::getInstance().addMarker("sensorToWorldMatrix", glmExtractRotation(_sensorToWorldMatrix), extractTranslation(_sensorToWorldMatrix), glm::vec4(1));
+        DebugDraw::getInstance().addMarker("sensorToWorldMatrix", glmExtractRotation(_sensorToWorldMatrix),
+                                           extractTranslation(_sensorToWorldMatrix), glm::vec4(1));
     }
 
     _sensorToWorldMatrixCache.set(_sensorToWorldMatrix);
+
+    updateJointsFromControllers();
 }
 
 //  Update avatar head rotation with sensor data
diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h
index 1f212a1fec..cbd325f7cf 100644
--- a/interface/src/avatar/MyAvatar.h
+++ b/interface/src/avatar/MyAvatar.h
@@ -117,6 +117,9 @@ public:
     // as it moves through the world.
     void updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix);
 
+    // read the locations of hand controllers and save the values
+    void updateJointsFromControllers();
+
     // best called at end of main loop, just before rendering.
     // update sensor to world matrix from current body position and hmd sensor.
     // This is so the correct camera can be used for rendering.
@@ -410,9 +413,10 @@ private:
     bool _useSnapTurn { true };
     bool _clearOverlayWhenMoving { true };
 
-    // working copy of sensorToWorldMatrix.
-    // See AvatarData for thread-safe _sensorToWorldMatrixCache, used for outward facing access
-    glm::mat4 _sensorToWorldMatrix;
+    // working copies -- see AvatarData for thread-safe _sensorToWorldMatrixCache, used for outward facing access
+    glm::mat4 _sensorToWorldMatrix { glm::mat4() };
+    glm::mat4 _controllerRightHandMatrix { glm::mat4() };
+    glm::mat4 _controllerLeftHandMatrix { glm::mat4() };
 
     // cache of the current HMD sensor position and orientation
     // in sensor space.
diff --git a/libraries/avatars/src/AvatarData.cpp b/libraries/avatars/src/AvatarData.cpp
index 9088ee0577..7aeedb8c84 100644
--- a/libraries/avatars/src/AvatarData.cpp
+++ b/libraries/avatars/src/AvatarData.cpp
@@ -915,7 +915,24 @@ void AvatarData::clearJointsData() {
     }
 }
 
+int AvatarData::getFauxJointIndex(const QString& name) const {
+    if (name == "sensorToWorld") {
+        return SENSOR_TO_WORLD_MATRIX_INDEX;
+    }
+    if (name == "Controller.Standard.LeftHand") {
+        return CONTROLLER_LEFTHAND_INDEX;
+    }
+    if (name == "Controller.Standard.RightHand") {
+        return CONTROLLER_RIGHTHAND_INDEX;
+    }
+    return -1;
+}
+
 int AvatarData::getJointIndex(const QString& name) const {
+    int result = getFauxJointIndex(name);
+    if (result != -1) {
+        return result;
+    }
     QReadLocker readLock(&_jointDataLock);
     return _jointIndices.value(name) - 1;
 }
@@ -1743,6 +1760,17 @@ glm::mat4 AvatarData::getSensorToWorldMatrix() const {
     return _sensorToWorldMatrixCache.get();
 }
 
+// thread-safe
+glm::mat4 AvatarData::getControllerLeftHandMatrix() const {
+    return _controllerLeftHandMatrixCache.get();
+}
+
+// thread-safe
+glm::mat4 AvatarData::getControllerRightHandMatrix() const {
+    return _controllerRightHandMatrixCache.get();
+}
+
+
 QScriptValue RayToAvatarIntersectionResultToScriptValue(QScriptEngine* engine, const RayToAvatarIntersectionResult& value) {
     QScriptValue obj = engine->newObject();
     obj.setProperty("intersects", value.intersects);
diff --git a/libraries/avatars/src/AvatarData.h b/libraries/avatars/src/AvatarData.h
index 572657e921..9c49e11956 100644
--- a/libraries/avatars/src/AvatarData.h
+++ b/libraries/avatars/src/AvatarData.h
@@ -173,6 +173,8 @@ class AvatarData : public QObject, public SpatiallyNestable {
     Q_PROPERTY(QUuid sessionUUID READ getSessionUUID)
 
     Q_PROPERTY(glm::mat4 sensorToWorldMatrix READ getSensorToWorldMatrix)
+    Q_PROPERTY(glm::mat4 controllerLeftHandMatrix READ getControllerLeftHandMatrix)
+    Q_PROPERTY(glm::mat4 controllerRightHandMatrix READ getControllerRightHandMatrix)
 
 public:
 
@@ -356,6 +358,8 @@ public:
 
     // thread safe
     Q_INVOKABLE glm::mat4 getSensorToWorldMatrix() const;
+    Q_INVOKABLE glm::mat4 getControllerLeftHandMatrix() const;
+    Q_INVOKABLE glm::mat4 getControllerRightHandMatrix() const;
 
 public slots:
     void sendAvatarDataPacket();
@@ -433,6 +437,10 @@ protected:
 
     // used to transform any sensor into world space, including the _hmdSensorMat, or hand controllers.
     ThreadSafeValueCache<glm::mat4> _sensorToWorldMatrixCache { glm::mat4() };
+    ThreadSafeValueCache<glm::mat4> _controllerLeftHandMatrixCache { glm::mat4() };
+    ThreadSafeValueCache<glm::mat4> _controllerRightHandMatrixCache { glm::mat4() };
+
+    int getFauxJointIndex(const QString& name) const;
 
 private:
     friend void avatarStateFromFrame(const QByteArray& frameData, AvatarData* _avatar);
@@ -519,5 +527,10 @@ Q_DECLARE_METATYPE(RayToAvatarIntersectionResult)
 QScriptValue RayToAvatarIntersectionResultToScriptValue(QScriptEngine* engine, const RayToAvatarIntersectionResult& results);
 void RayToAvatarIntersectionResultFromScriptValue(const QScriptValue& object, RayToAvatarIntersectionResult& results);
 
+// faux joint indexes (-1 means invalid)
+const int SENSOR_TO_WORLD_MATRIX_INDEX = 65534; // -2
+const int CONTROLLER_RIGHTHAND_INDEX = 65533; // -3
+const int CONTROLLER_LEFTHAND_INDEX = 65532; // -4
+
 
 #endif // hifi_AvatarData_h
diff --git a/libraries/controllers/src/controllers/UserInputMapper.cpp b/libraries/controllers/src/controllers/UserInputMapper.cpp
index 7490a44c11..f5343c0c9a 100755
--- a/libraries/controllers/src/controllers/UserInputMapper.cpp
+++ b/libraries/controllers/src/controllers/UserInputMapper.cpp
@@ -325,7 +325,6 @@ QString UserInputMapper::getActionName(Action action) const {
     return QString();
 }
 
-
 QVector<QString> UserInputMapper::getActionNames() const {
     Locker locker(_lock);
     QVector<QString> result;
@@ -335,6 +334,18 @@ QVector<QString> UserInputMapper::getActionNames() const {
     return result;
 }
 
+Pose UserInputMapper::getPoseState(Action action) const {
+    if (QThread::currentThread() != thread()) {
+        Pose result;
+        QMetaObject::invokeMethod(const_cast<UserInputMapper*>(this), "getPoseState", Qt::BlockingQueuedConnection,
+                                  Q_RETURN_ARG(Pose, result), Q_ARG(Action, action));
+        return result;
+    }
+
+    return _poseStates[toInt(action)];
+}
+
+
 bool UserInputMapper::triggerHapticPulse(float strength, float duration, controller::Hand hand) {
     Locker locker(_lock);
     bool toReturn = false;
diff --git a/libraries/controllers/src/controllers/UserInputMapper.h b/libraries/controllers/src/controllers/UserInputMapper.h
index 874e5054ea..baa05f2f9f 100644
--- a/libraries/controllers/src/controllers/UserInputMapper.h
+++ b/libraries/controllers/src/controllers/UserInputMapper.h
@@ -81,7 +81,7 @@ namespace controller {
         QVector<Action> getAllActions() const;
         QString getActionName(Action action) const;
         float getActionState(Action action) const { return _actionStates[toInt(action)]; }
-        Pose getPoseState(Action action) const { return _poseStates[toInt(action)]; }
+        Pose getPoseState(Action action) const;
         int findAction(const QString& actionName) const;
         QVector<QString> getActionNames() const;
         Input inputFromAction(Action action) const { return getActionInputs()[toInt(action)].first; }

From 0783629cde165d0731bb72fdb5c03ca73c91f292 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Thu, 25 Aug 2016 13:31:07 -0700
Subject: [PATCH 04/44] grab-point sphere is a child of controller joint, so it
 doesn't jitter

---
 .../system/controllers/handControllerGrab.js   | 18 +++++++++---------
 1 file changed, 9 insertions(+), 9 deletions(-)

diff --git a/scripts/system/controllers/handControllerGrab.js b/scripts/system/controllers/handControllerGrab.js
index eb1d5002bb..347e45f879 100644
--- a/scripts/system/controllers/handControllerGrab.js
+++ b/scripts/system/controllers/handControllerGrab.js
@@ -25,8 +25,8 @@ var WANT_DEBUG = false;
 var WANT_DEBUG_STATE = false;
 var WANT_DEBUG_SEARCH_NAME = null;
 
-var FORCE_IGNORE_IK = false;
-var SHOW_GRAB_POINT_SPHERE = false;
+var FORCE_IGNORE_IK = true;
+var SHOW_GRAB_POINT_SPHERE = true;
 
 //
 // these tune time-averaging and "on" value for analog trigger
@@ -833,12 +833,8 @@ function MyController(hand) {
         if (!SHOW_GRAB_POINT_SPHERE) {
             return;
         }
-        var controllerLocation = this.getControllerLocation();
-        if (this.grabPointSphere) {
-            Overlays.editOverlay(this.grabPointSphere, {
-                position: controllerLocation.position
-            });
-        } else {
+        if (!this.grabPointSphere) {
+            var controllerLocation = this.getControllerLocation();
             this.grabPointSphere = Overlays.addOverlay("sphere", {
                 position: controllerLocation.position,
                 rotation: { x: 0, y: 0, z: 0, w: 1 },
@@ -848,7 +844,11 @@ function MyController(hand) {
                 solid: true,
                 visible: true,
                 ignoreRayIntersection: true,
-                drawInFront: false
+                drawInFront: false,
+                parentID: MyAvatar.sessionUUID,
+                parentJointIndex: MyAvatar.getJointIndex(this.hand === RIGHT_HAND ?
+                                                         "Controller.Standard.RightHand" :
+                                                         "Controller.Standard.LeftHand")
             });
         }
     };

From 948b4b7a15d70115c58a4def92704a7ce7e9b220 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Thu, 25 Aug 2016 13:46:52 -0700
Subject: [PATCH 05/44] in search, consider distance from grab-point rather
 than from avatar-hand when selecting the closest entity

---
 scripts/system/controllers/handControllerGrab.js | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/scripts/system/controllers/handControllerGrab.js b/scripts/system/controllers/handControllerGrab.js
index 347e45f879..aabca934c1 100644
--- a/scripts/system/controllers/handControllerGrab.js
+++ b/scripts/system/controllers/handControllerGrab.js
@@ -1393,7 +1393,8 @@ function MyController(hand) {
             return;
         }
 
-        var handPosition = this.getHandPosition();
+        // var handPosition = this.getHandPosition();
+        var handPosition = this.getControllerLocation().position;
 
         var rayPickInfo = this.calcRayPickInfo(this.hand);
 

From 7c5b8cb75daddb86381db3776a996cb26b263ffd Mon Sep 17 00:00:00 2001
From: Seth Alves <seth.alves@gmail.com>
Date: Thu, 25 Aug 2016 15:21:29 -0700
Subject: [PATCH 06/44] send faux joints to the avatar-mixer

---
 interface/src/avatar/MyAvatar.h               |  2 --
 libraries/avatars/src/AvatarData.cpp          | 34 +++++++++++++++++--
 .../src/controllers/UserInputMapper.cpp       |  8 +----
 .../networking/src/udt/PacketHeaders.cpp      |  2 +-
 libraries/networking/src/udt/PacketHeaders.h  |  3 +-
 .../system/controllers/handControllerGrab.js  |  8 +++--
 6 files changed, 40 insertions(+), 17 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h
index cbd325f7cf..eb37f9976c 100644
--- a/interface/src/avatar/MyAvatar.h
+++ b/interface/src/avatar/MyAvatar.h
@@ -415,8 +415,6 @@ private:
 
     // working copies -- see AvatarData for thread-safe _sensorToWorldMatrixCache, used for outward facing access
     glm::mat4 _sensorToWorldMatrix { glm::mat4() };
-    glm::mat4 _controllerRightHandMatrix { glm::mat4() };
-    glm::mat4 _controllerLeftHandMatrix { glm::mat4() };
 
     // cache of the current HMD sensor position and orientation
     // in sensor space.
diff --git a/libraries/avatars/src/AvatarData.cpp b/libraries/avatars/src/AvatarData.cpp
index 7aeedb8c84..037f12d2fa 100644
--- a/libraries/avatars/src/AvatarData.cpp
+++ b/libraries/avatars/src/AvatarData.cpp
@@ -374,6 +374,16 @@ QByteArray AvatarData::toByteArray(bool cullSmallChanges, bool sendAll) {
         }
     }
 
+    // faux joints
+    Transform controllerLeftHandTransform = Transform(getControllerLeftHandMatrix());
+    destinationBuffer += packOrientationQuatToSixBytes(destinationBuffer, controllerLeftHandTransform.getRotation());
+    destinationBuffer += packFloatVec3ToSignedTwoByteFixed(destinationBuffer, controllerLeftHandTransform.getTranslation(),
+                                                           TRANSLATION_COMPRESSION_RADIX);
+    Transform controllerRightHandTransform = Transform(getControllerRightHandMatrix());
+    destinationBuffer += packOrientationQuatToSixBytes(destinationBuffer, controllerRightHandTransform.getRotation());
+    destinationBuffer += packFloatVec3ToSignedTwoByteFixed(destinationBuffer, controllerRightHandTransform.getTranslation(),
+                                                           TRANSLATION_COMPRESSION_RADIX);
+
     #ifdef WANT_DEBUG
     if (sendAll) {
         qDebug() << "AvatarData::toByteArray" << cullSmallChanges << sendAll
@@ -429,6 +439,20 @@ bool AvatarData::shouldLogError(const quint64& now) {
     return false;
 }
 
+
+const unsigned char* unpackFauxJoint(const unsigned char* sourceBuffer, ThreadSafeValueCache<glm::mat4>& matrixCache) {
+    glm::quat orientation;
+    glm::vec3 position;
+    Transform transform;
+    sourceBuffer += unpackOrientationQuatFromSixBytes(sourceBuffer, orientation);
+    sourceBuffer += unpackFloatVec3FromSignedTwoByteFixed(sourceBuffer, position, TRANSLATION_COMPRESSION_RADIX);
+    transform.setTranslation(position);
+    transform.setRotation(orientation);
+    matrixCache.set(transform.getMatrix());
+    return sourceBuffer;
+}
+
+
 #define PACKET_READ_CHECK(ITEM_NAME, SIZE_TO_READ)                                        \
     if ((endPosition - sourceBuffer) < (int)SIZE_TO_READ) {                               \
         if (shouldLogError(now)) {                                                        \
@@ -655,6 +679,10 @@ int AvatarData::parseDataFromBuffer(const QByteArray& buffer) {
     }
     #endif
 
+    // faux joints
+    sourceBuffer = unpackFauxJoint(sourceBuffer, _controllerLeftHandMatrixCache);
+    sourceBuffer = unpackFauxJoint(sourceBuffer, _controllerRightHandMatrixCache);
+
     int numBytesRead = sourceBuffer - startPosition;
     _averageBytesReceived.updateAverage(numBytesRead);
     return numBytesRead;
@@ -916,13 +944,13 @@ void AvatarData::clearJointsData() {
 }
 
 int AvatarData::getFauxJointIndex(const QString& name) const {
-    if (name == "sensorToWorld") {
+    if (name == "_SENSOR_TO_WORLD_MATRIX") {
         return SENSOR_TO_WORLD_MATRIX_INDEX;
     }
-    if (name == "Controller.Standard.LeftHand") {
+    if (name == "_CONTROLLER_LEFTHAND") {
         return CONTROLLER_LEFTHAND_INDEX;
     }
-    if (name == "Controller.Standard.RightHand") {
+    if (name == "_CONTROLLER_RIGHTHAND") {
         return CONTROLLER_RIGHTHAND_INDEX;
     }
     return -1;
diff --git a/libraries/controllers/src/controllers/UserInputMapper.cpp b/libraries/controllers/src/controllers/UserInputMapper.cpp
index f5343c0c9a..ec32e6b8c9 100755
--- a/libraries/controllers/src/controllers/UserInputMapper.cpp
+++ b/libraries/controllers/src/controllers/UserInputMapper.cpp
@@ -335,13 +335,7 @@ QVector<QString> UserInputMapper::getActionNames() const {
 }
 
 Pose UserInputMapper::getPoseState(Action action) const {
-    if (QThread::currentThread() != thread()) {
-        Pose result;
-        QMetaObject::invokeMethod(const_cast<UserInputMapper*>(this), "getPoseState", Qt::BlockingQueuedConnection,
-                                  Q_RETURN_ARG(Pose, result), Q_ARG(Action, action));
-        return result;
-    }
-
+    if (QThread::currentThread() != thread()) { abort(); } // XXX
     return _poseStates[toInt(action)];
 }
 
diff --git a/libraries/networking/src/udt/PacketHeaders.cpp b/libraries/networking/src/udt/PacketHeaders.cpp
index 0d25d4f1be..0ef5dcfce3 100644
--- a/libraries/networking/src/udt/PacketHeaders.cpp
+++ b/libraries/networking/src/udt/PacketHeaders.cpp
@@ -52,7 +52,7 @@ PacketVersion versionForPacketType(PacketType packetType) {
         case PacketType::AvatarData:
         case PacketType::BulkAvatarData:
         case PacketType::KillAvatar:
-            return static_cast<PacketVersion>(AvatarMixerPacketVersion::SensorToWorldMat);
+            return static_cast<PacketVersion>(AvatarMixerPacketVersion::HandControllerJoints);
         case PacketType::ICEServerHeartbeat:
             return 18; // ICE Server Heartbeat signing
         case PacketType::AssetGetInfo:
diff --git a/libraries/networking/src/udt/PacketHeaders.h b/libraries/networking/src/udt/PacketHeaders.h
index 3ecdb75a18..2718f4c4a0 100644
--- a/libraries/networking/src/udt/PacketHeaders.h
+++ b/libraries/networking/src/udt/PacketHeaders.h
@@ -193,7 +193,8 @@ enum class AvatarMixerPacketVersion : PacketVersion {
     SoftAttachmentSupport,
     AvatarEntities,
     AbsoluteSixByteRotations,
-    SensorToWorldMat
+    SensorToWorldMat,
+    HandControllerJoints
 };
 
 enum class DomainConnectRequestVersion : PacketVersion {
diff --git a/scripts/system/controllers/handControllerGrab.js b/scripts/system/controllers/handControllerGrab.js
index aabca934c1..4834d533f5 100644
--- a/scripts/system/controllers/handControllerGrab.js
+++ b/scripts/system/controllers/handControllerGrab.js
@@ -117,7 +117,7 @@ var NEAR_GRABBING_KINEMATIC = true; // force objects to be kinematic when near-g
 var CHECK_TOO_FAR_UNEQUIP_TIME = 0.3; // seconds, duration between checks
 
 
-var GRAB_POINT_SPHERE_OFFSET = {x: 0, y: 0.2, z:0};
+var GRAB_POINT_SPHERE_OFFSET = { x: 0.0, y: 0.2, z: 0.0 };
 var GRAB_POINT_SPHERE_RADIUS = NEAR_GRAB_RADIUS;
 var GRAB_POINT_SPHERE_COLOR = { red: 20, green: 90, blue: 238 };
 var GRAB_POINT_SPHERE_ALPHA = 0.85;
@@ -719,6 +719,7 @@ function MyController(hand) {
 
         var orientation = Quat.multiply(MyAvatar.orientation, pose.rotation)
         var position = Vec3.sum(Vec3.multiplyQbyV(MyAvatar.orientation, pose.translation), MyAvatar.position);
+        // add to the real position so the grab-point is out in front of the hand, a bit
         position = Vec3.sum(position, Vec3.multiplyQbyV(orientation, GRAB_POINT_SPHERE_OFFSET));
 
         return {position: position, orientation: orientation};
@@ -847,8 +848,8 @@ function MyController(hand) {
                 drawInFront: false,
                 parentID: MyAvatar.sessionUUID,
                 parentJointIndex: MyAvatar.getJointIndex(this.hand === RIGHT_HAND ?
-                                                         "Controller.Standard.RightHand" :
-                                                         "Controller.Standard.LeftHand")
+                                                         "_CONTROLLER_RIGHTHAND" :
+                                                         "_CONTROLLER_LEFTHAND")
             });
         }
     };
@@ -1866,6 +1867,7 @@ function MyController(hand) {
         if (this.ignoreIK) {
             var controllerLocation = this.getControllerLocation();
             handRotation = controllerLocation.orientation;
+            // subtract off the GRAB_POINT_SPHERE_OFFSET that was added in getControllerLocation
             handPosition = Vec3.subtract(controllerLocation.position,
                                          Vec3.multiplyQbyV(handRotation, GRAB_POINT_SPHERE_OFFSET));
         } else {

From 232917fcf8dc774b503310ab26561f5cf71ee4b8 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth.alves@gmail.com>
Date: Thu, 25 Aug 2016 16:02:58 -0700
Subject: [PATCH 07/44] when processing AvatarActionHold for another's avatar,
 use their hand-controller joint information

---
 interface/src/avatar/AvatarActionHold.cpp | 14 ++++++++++----
 1 file changed, 10 insertions(+), 4 deletions(-)

diff --git a/interface/src/avatar/AvatarActionHold.cpp b/interface/src/avatar/AvatarActionHold.cpp
index 5acee052f2..acd710bf41 100644
--- a/interface/src/avatar/AvatarActionHold.cpp
+++ b/interface/src/avatar/AvatarActionHold.cpp
@@ -159,11 +159,17 @@ bool AvatarActionHold::getTarget(float deltaTimeStep, glm::quat& rotation, glm::
             }
         } else { // regular avatar
             if (isRightHand) {
-                palmPosition = holdingAvatar->getRightPalmPosition();
-                palmRotation = holdingAvatar->getRightPalmRotation();
+                Transform controllerRightTransform = Transform(holdingAvatar->getControllerRightHandMatrix());
+                Transform avatarTransform = holdingAvatar->getTransform();
+                palmRotation = avatarTransform.getRotation() * controllerRightTransform.getRotation();
+                palmPosition = avatarTransform.getTranslation() +
+                    (avatarTransform.getRotation() * controllerRightTransform.getTranslation());
             } else {
-                palmPosition = holdingAvatar->getLeftPalmPosition();
-                palmRotation = holdingAvatar->getLeftPalmRotation();
+                Transform controllerLeftTransform = Transform(holdingAvatar->getControllerLeftHandMatrix());
+                Transform avatarTransform = holdingAvatar->getTransform();
+                palmRotation = avatarTransform.getRotation() * controllerLeftTransform.getRotation();
+                palmPosition = avatarTransform.getTranslation() +
+                    (avatarTransform.getRotation() * controllerLeftTransform.getTranslation());
             }
         }
 

From dfae5b64bd1c18811da26289cfdcac8dfd38ddee Mon Sep 17 00:00:00 2001
From: Seth Alves <seth.alves@gmail.com>
Date: Thu, 25 Aug 2016 16:42:40 -0700
Subject: [PATCH 08/44] attempt to avoid jitter of held entity while walking

---
 interface/src/avatar/MyAvatar.cpp | 38 +++++++++++++++++++++----------
 interface/src/avatar/MyAvatar.h   |  6 +++--
 2 files changed, 30 insertions(+), 14 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index b021b9d5b2..a899406dc2 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -532,21 +532,32 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) {
     _hmdSensorFacing = getFacingDir2D(_hmdSensorOrientation);
 }
 
-void MyAvatar::updateJointsFromControllers() {
+void MyAvatar::updateJointFromController(glm::mat4& previousSensorToWorldInverseMatrix, controller::Action poseKey,
+                                         ThreadSafeValueCache<glm::mat4>& matrixCache) {
     if (QThread::currentThread() != thread()) { abort(); } // XXX
     auto userInputMapper = DependencyManager::get<UserInputMapper>();
+    controller::Pose controllerPose = userInputMapper->getPoseState(poseKey);
+    Transform transform;
+    transform.setTranslation(controllerPose.getTranslation());
+    transform.setRotation(controllerPose.getRotation());
+    glm::mat4 avatarMatrix = getTransform().getMatrix();
+    glm::mat4 avatarInverseMatrix = glm::inverse(avatarMatrix);
 
-    controller::Pose leftControllerPose = userInputMapper->getPoseState(controller::Action::LEFT_HAND);
-    Transform controllerLeftHandTransform;
-    controllerLeftHandTransform.setTranslation(leftControllerPose.getTranslation());
-    controllerLeftHandTransform.setRotation(leftControllerPose.getRotation());
-    _controllerLeftHandMatrixCache.set(controllerLeftHandTransform.getMatrix());
+    // do some backflips to avoid jitter
 
-    controller::Pose rightControllerPose = userInputMapper->getPoseState(controller::Action::RIGHT_HAND);
-    Transform controllerRightHandTransform;
-    controllerRightHandTransform.setTranslation(rightControllerPose.getTranslation());
-    controllerRightHandTransform.setRotation(rightControllerPose.getRotation());
-    _controllerRightHandMatrixCache.set(controllerRightHandTransform.getMatrix());
+    // get the controller pose (avatar space)
+    glm::mat4 controllerMatrix = transform.getMatrix();
+    // transform the controller pose into world space.
+    glm::mat4 controllerWorldSpace = avatarMatrix * controllerMatrix;
+    // transform the controller pose from world space into sensor space. But use the inverse of the ORIGINAL sensorToWorld
+    // matrix before updateSensorToWorldMatrix() changes it.
+    glm::mat4 controllerPreviousSensor = previousSensorToWorldInverseMatrix * controllerWorldSpace;
+    // then transform the sensor space controller pose back into world space using the NEW sensorToWorld matrix.
+    glm::mat4 controllerNewWorldSpace = _sensorToWorldMatrix * controllerPreviousSensor;
+    // then transform that world pose back into avatar space.
+    glm::mat4 newControllerMatrix = avatarInverseMatrix * controllerNewWorldSpace;
+
+    matrixCache.set(newControllerMatrix);
 }
 
 // best called at end of main loop, after physics.
@@ -554,6 +565,8 @@ void MyAvatar::updateJointsFromControllers() {
 // This is so the correct camera can be used for rendering.
 void MyAvatar::updateSensorToWorldMatrix() {
 
+    glm::mat4 previousSensorToWorldInverse = glm::inverse(_sensorToWorldMatrix);
+
     // update the sensor mat so that the body position will end up in the desired
     // position when driven from the head.
     glm::mat4 desiredMat = createMatFromQuatAndPos(getOrientation(), getPosition());
@@ -568,7 +581,8 @@ void MyAvatar::updateSensorToWorldMatrix() {
 
     _sensorToWorldMatrixCache.set(_sensorToWorldMatrix);
 
-    updateJointsFromControllers();
+    updateJointFromController(previousSensorToWorldInverse, controller::Action::LEFT_HAND, _controllerLeftHandMatrixCache);
+    updateJointFromController(previousSensorToWorldInverse, controller::Action::RIGHT_HAND, _controllerRightHandMatrixCache);
 }
 
 //  Update avatar head rotation with sensor data
diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h
index eb37f9976c..cb733aa0ed 100644
--- a/interface/src/avatar/MyAvatar.h
+++ b/interface/src/avatar/MyAvatar.h
@@ -19,6 +19,7 @@
 #include <Sound.h>
 
 #include <controllers/Pose.h>
+#include <controllers/Actions.h>
 
 #include "Avatar.h"
 #include "AtRestDetector.h"
@@ -117,8 +118,9 @@ public:
     // as it moves through the world.
     void updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix);
 
-    // read the locations of hand controllers and save the values
-    void updateJointsFromControllers();
+    // read the location of a hand controller and save the transform
+    void updateJointFromController(glm::mat4& previousSensorToWorldInverseMatrix, controller::Action poseKey,
+                                   ThreadSafeValueCache<glm::mat4>& matrixCache);
 
     // best called at end of main loop, just before rendering.
     // update sensor to world matrix from current body position and hmd sensor.

From 174a95a0052bc69f9bda045992eac8772814dedb Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Thu, 25 Aug 2016 18:49:47 -0700
Subject: [PATCH 09/44] do some hokey pokey to avoid jitter of held entities
 while walking

---
 interface/src/avatar/AvatarActionHold.cpp | 37 ++++++++++++++++++++---
 interface/src/avatar/AvatarActionHold.h   |  3 ++
 2 files changed, 35 insertions(+), 5 deletions(-)

diff --git a/interface/src/avatar/AvatarActionHold.cpp b/interface/src/avatar/AvatarActionHold.cpp
index acd710bf41..5aa64a72cf 100644
--- a/interface/src/avatar/AvatarActionHold.cpp
+++ b/interface/src/avatar/AvatarActionHold.cpp
@@ -56,6 +56,12 @@ void AvatarActionHold::prepareForPhysicsSimulation() {
     }
 
     withWriteLock([&]{
+        glm::vec3 avatarRigidBodyPosition;
+        glm::quat avatarRigidBodyRotation;
+        getAvatarRigidBodyLocation(avatarRigidBodyPosition, avatarRigidBodyRotation);
+        _preStepAvatarPosition = avatarRigidBodyPosition;
+        _preStepAvatarRotation = avatarRigidBodyRotation;
+
         if (_ignoreIK) {
             return;
         }
@@ -70,9 +76,6 @@ void AvatarActionHold::prepareForPhysicsSimulation() {
             palmRotation = holdingAvatar->getUncachedLeftPalmRotation();
         }
 
-        glm::vec3 avatarRigidBodyPosition;
-        glm::quat avatarRigidBodyRotation;
-        getAvatarRigidBodyLocation(avatarRigidBodyPosition, avatarRigidBodyRotation);
 
         // determine the difference in translation and rotation between the avatar's
         // rigid body and the palm position.  The avatar's rigid body will be moved by bullet
@@ -129,8 +132,32 @@ bool AvatarActionHold::getTarget(float deltaTimeStep, glm::quat& rotation, glm::
             if (_ignoreIK && pose.isValid()) {
                 // We cannot ignore other avatars IK and this is not the point of this option
                 // This is meant to make the grabbing behavior more reactive.
-                palmPosition = pose.getTranslation();
-                palmRotation = pose.getRotation();
+
+                // The avatar moves between prepareForPhysicsSimulation and this, so do some stuff to avoid jitter:
+                // - transform the pose's world-position into the space relative to the old rigid-body
+                // - then transform this relative position back into world-space via the new rigid-body's transform
+
+                Transform poseTransform;
+                poseTransform.setTranslation(pose.getTranslation());
+                poseTransform.setRotation(pose.getRotation());
+
+                Transform preStepAvatarTransform;
+                preStepAvatarTransform.setTranslation(_preStepAvatarPosition);
+                preStepAvatarTransform.setRotation(_preStepAvatarRotation);
+                Transform inversePreStepAvatarTransform = Transform(preStepAvatarTransform.getInverseMatrix());
+
+                Transform avatarTransform;
+                glm::vec3 avatarRigidBodyPosition;
+                glm::quat avatarRigidBodyRotation;
+                getAvatarRigidBodyLocation(avatarRigidBodyPosition, avatarRigidBodyRotation);
+                avatarTransform.setTranslation(avatarRigidBodyPosition);
+                avatarTransform.setRotation(avatarRigidBodyRotation);
+
+                glm::mat4 adjustedMatrix = avatarTransform.getMatrix() *
+                    (inversePreStepAvatarTransform.getMatrix() * poseTransform.getMatrix());
+                Transform adjustedTransform = Transform(adjustedMatrix);
+                palmPosition = adjustedTransform.getTranslation();
+                palmRotation = adjustedTransform.getRotation();
             } else {
                 glm::vec3 avatarRigidBodyPosition;
                 glm::quat avatarRigidBodyRotation;
diff --git a/interface/src/avatar/AvatarActionHold.h b/interface/src/avatar/AvatarActionHold.h
index bfa392172d..b30360884f 100644
--- a/interface/src/avatar/AvatarActionHold.h
+++ b/interface/src/avatar/AvatarActionHold.h
@@ -68,6 +68,9 @@ private:
     static const int velocitySmoothFrames;
     QVector<glm::vec3> _measuredLinearVelocities;
     int _measuredLinearVelocitiesIndex { 0 };
+
+    glm::vec3 _preStepAvatarPosition;
+    glm::quat _preStepAvatarRotation;
 };
 
 #endif // hifi_AvatarActionHold_h

From acb04a0bc93cff6821ed822581dfc2a8f8d0d8a0 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Thu, 25 Aug 2016 19:09:10 -0700
Subject: [PATCH 10/44] don't add children of controller joints until we know
 our avatar ID

---
 scripts/system/controllers/handControllerGrab.js | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/scripts/system/controllers/handControllerGrab.js b/scripts/system/controllers/handControllerGrab.js
index 4834d533f5..bd04975545 100644
--- a/scripts/system/controllers/handControllerGrab.js
+++ b/scripts/system/controllers/handControllerGrab.js
@@ -829,16 +829,17 @@ function MyController(hand) {
         }
     };
 
-
     this.grabPointSphereOn = function() {
         if (!SHOW_GRAB_POINT_SPHERE) {
             return;
         }
+        if (!MyAvatar.sessionUUID) {
+            return;
+        }
         if (!this.grabPointSphere) {
-            var controllerLocation = this.getControllerLocation();
             this.grabPointSphere = Overlays.addOverlay("sphere", {
-                position: controllerLocation.position,
-                rotation: { x: 0, y: 0, z: 0, w: 1 },
+                localPosition: GRAB_POINT_SPHERE_OFFSET,
+                localRotation: { x: 0, y: 0, z: 0, w: 1 },
                 dimensions: GRAB_POINT_SPHERE_RADIUS,
                 color: GRAB_POINT_SPHERE_COLOR,
                 alpha: GRAB_POINT_SPHERE_ALPHA,

From 9884426ad83961a4c1eb89eeee610f6bb443e44b Mon Sep 17 00:00:00 2001
From: Seth Alves <seth.alves@gmail.com>
Date: Fri, 26 Aug 2016 06:13:33 -0700
Subject: [PATCH 11/44] replace debugging aborts with asserts

---
 interface/src/avatar/MyAvatar.cpp                         | 2 +-
 libraries/controllers/src/controllers/UserInputMapper.cpp | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index a899406dc2..1d543b4ed5 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -534,7 +534,7 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) {
 
 void MyAvatar::updateJointFromController(glm::mat4& previousSensorToWorldInverseMatrix, controller::Action poseKey,
                                          ThreadSafeValueCache<glm::mat4>& matrixCache) {
-    if (QThread::currentThread() != thread()) { abort(); } // XXX
+    assert(QThread::currentThread() == thread());
     auto userInputMapper = DependencyManager::get<UserInputMapper>();
     controller::Pose controllerPose = userInputMapper->getPoseState(poseKey);
     Transform transform;
diff --git a/libraries/controllers/src/controllers/UserInputMapper.cpp b/libraries/controllers/src/controllers/UserInputMapper.cpp
index ec32e6b8c9..ff44d5d13d 100755
--- a/libraries/controllers/src/controllers/UserInputMapper.cpp
+++ b/libraries/controllers/src/controllers/UserInputMapper.cpp
@@ -335,7 +335,7 @@ QVector<QString> UserInputMapper::getActionNames() const {
 }
 
 Pose UserInputMapper::getPoseState(Action action) const {
-    if (QThread::currentThread() != thread()) { abort(); } // XXX
+    assert(QThread::currentThread() == thread());
     return _poseStates[toInt(action)];
 }
 

From 9394a25a1c9674a2896c4d8979b6e26270e935c4 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Fri, 26 Aug 2016 12:47:19 -0700
Subject: [PATCH 12/44] remove some anti-jitter code that was doing more harm
 than good

---
 interface/src/avatar/MyAvatar.cpp | 18 +-----------------
 1 file changed, 1 insertion(+), 17 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 1d543b4ed5..50baabc844 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -540,24 +540,8 @@ void MyAvatar::updateJointFromController(glm::mat4& previousSensorToWorldInverse
     Transform transform;
     transform.setTranslation(controllerPose.getTranslation());
     transform.setRotation(controllerPose.getRotation());
-    glm::mat4 avatarMatrix = getTransform().getMatrix();
-    glm::mat4 avatarInverseMatrix = glm::inverse(avatarMatrix);
-
-    // do some backflips to avoid jitter
-
-    // get the controller pose (avatar space)
     glm::mat4 controllerMatrix = transform.getMatrix();
-    // transform the controller pose into world space.
-    glm::mat4 controllerWorldSpace = avatarMatrix * controllerMatrix;
-    // transform the controller pose from world space into sensor space. But use the inverse of the ORIGINAL sensorToWorld
-    // matrix before updateSensorToWorldMatrix() changes it.
-    glm::mat4 controllerPreviousSensor = previousSensorToWorldInverseMatrix * controllerWorldSpace;
-    // then transform the sensor space controller pose back into world space using the NEW sensorToWorld matrix.
-    glm::mat4 controllerNewWorldSpace = _sensorToWorldMatrix * controllerPreviousSensor;
-    // then transform that world pose back into avatar space.
-    glm::mat4 newControllerMatrix = avatarInverseMatrix * controllerNewWorldSpace;
-
-    matrixCache.set(newControllerMatrix);
+    matrixCache.set(controllerMatrix);
 }
 
 // best called at end of main loop, after physics.

From e4b2b7158fc338f6886c599dfb628ef2d13d01df Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Sat, 27 Aug 2016 09:30:26 -0700
Subject: [PATCH 13/44] hotspots with large radius work correctly, again

---
 scripts/system/controllers/handControllerGrab.js | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/scripts/system/controllers/handControllerGrab.js b/scripts/system/controllers/handControllerGrab.js
index bd04975545..11219266dc 100644
--- a/scripts/system/controllers/handControllerGrab.js
+++ b/scripts/system/controllers/handControllerGrab.js
@@ -98,10 +98,11 @@ var PICK_MAX_DISTANCE = 500; // max length of pick-ray
 // near grabbing
 //
 
-var EQUIP_RADIUS = 0.1; // radius used for palm vs equip-hotspot for equipping.
+var EQUIP_RADIUS = 0.2; // radius used for palm vs equip-hotspot for equipping.
 // if EQUIP_HOTSPOT_RENDER_RADIUS is greater than zero, the hotspot will appear before the hand
 // has reached the required position, and then grow larger once the hand is close enough to equip.
 var EQUIP_HOTSPOT_RENDER_RADIUS = 0.0; // radius used for palm vs equip-hotspot for rendering hot-spots
+var MAX_EQUIP_HOTSPOT_RADIUS = 1.0;
 
 var NEAR_GRABBING_ACTION_TIMEFRAME = 0.05; // how quickly objects move to their new position
 
@@ -1070,7 +1071,7 @@ function MyController(hand) {
 
         this.grabPointSphereOn();
 
-        var candidateEntities = Entities.findEntities(this.getHandPosition(), EQUIP_HOTSPOT_RENDER_RADIUS);
+        var candidateEntities = Entities.findEntities(this.getHandPosition(), MAX_EQUIP_HOTSPOT_RADIUS);
         entityPropertiesCache.addEntities(candidateEntities);
         var potentialEquipHotspot = this.chooseBestEquipHotspot(candidateEntities);
         if (!this.waitForTriggerRelease) {
@@ -1404,7 +1405,7 @@ function MyController(hand) {
             entityPropertiesCache.addEntity(rayPickInfo.entityID);
         }
 
-        var candidateEntities = Entities.findEntities(handPosition, NEAR_GRAB_RADIUS);
+        var candidateEntities = Entities.findEntities(handPosition, MAX_EQUIP_HOTSPOT_RADIUS);
         entityPropertiesCache.addEntities(candidateEntities);
 
         var potentialEquipHotspot = this.chooseBestEquipHotspot(candidateEntities);

From cfca32e0b484597b3fc2910c97f44e0804d053c3 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Sat, 27 Aug 2016 16:00:27 -0700
Subject: [PATCH 14/44] fix code that notices and fixes an abandoned grab

---
 scripts/system/controllers/handControllerGrab.js | 12 ++++++++----
 1 file changed, 8 insertions(+), 4 deletions(-)

diff --git a/scripts/system/controllers/handControllerGrab.js b/scripts/system/controllers/handControllerGrab.js
index 11219266dc..9d57e1d03f 100644
--- a/scripts/system/controllers/handControllerGrab.js
+++ b/scripts/system/controllers/handControllerGrab.js
@@ -718,7 +718,7 @@ function MyController(hand) {
         var standardControllerValue = (hand === RIGHT_HAND) ? Controller.Standard.RightHand : Controller.Standard.LeftHand;
         var pose = Controller.getPoseValue(standardControllerValue);
 
-        var orientation = Quat.multiply(MyAvatar.orientation, pose.rotation)
+        var orientation = Quat.multiply(MyAvatar.orientation, pose.rotation);
         var position = Vec3.sum(Vec3.multiplyQbyV(MyAvatar.orientation, pose.translation), MyAvatar.position);
         // add to the real position so the grab-point is out in front of the hand, a bit
         position = Vec3.sum(position, Vec3.multiplyQbyV(orientation, GRAB_POINT_SPHERE_OFFSET));
@@ -1102,6 +1102,11 @@ function MyController(hand) {
         this.prevPotentialEquipHotspot = potentialEquipHotspot;
     };
 
+    this.heartBeatIsStale = function(data) {
+        var now = Date.now();
+        return data.heartBeat === undefined || now - data.heartBeat > HEART_BEAT_TIMEOUT;
+    };
+
     // Performs ray pick test from the hand controller into the world
     // @param {number} which hand to use, RIGHT_HAND or LEFT_HAND
     // @returns {object} returns object with two keys entityID and distance
@@ -1229,7 +1234,7 @@ function MyController(hand) {
         var okToEquipFromOtherHand = ((this.getOtherHandController().state == STATE_NEAR_GRABBING ||
                 this.getOtherHandController().state == STATE_DISTANCE_HOLDING) &&
             this.getOtherHandController().grabbedEntity == hotspot.entityID);
-        if (refCount > 0 && !okToEquipFromOtherHand) {
+        if (refCount > 0 && !this.heartBeatIsStale(grabProps) && !okToEquipFromOtherHand) {
             if (debug) {
                 print("equip is skipping '" + props.name + "': grabbed by someone else");
             }
@@ -2407,8 +2412,7 @@ function MyController(hand) {
                 };
                 Entities.editEntity(entityID, whileHeldProperties);
             } else if (data.refCount > 1) {
-                if (data.heartBeat === undefined ||
-                    now - data.heartBeat > HEART_BEAT_TIMEOUT) {
+                if (this.heartBeatIsStale(data)) {
                     // this entity has userData suggesting it is grabbed, but nobody is updating the hearbeat.
                     // deactivate it before grabbing.
                     this.resetAbandonedGrab(entityID);

From 765cd72e0e5c79771c18a49a29435831ae2c258f Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Sat, 27 Aug 2016 19:13:32 -0700
Subject: [PATCH 15/44] fix code that prints relative offsets of an equipped
 entity after it's been adjusted by the other hand

---
 .../system/controllers/handControllerGrab.js  | 22 ++++++++++---------
 1 file changed, 12 insertions(+), 10 deletions(-)

diff --git a/scripts/system/controllers/handControllerGrab.js b/scripts/system/controllers/handControllerGrab.js
index 9d57e1d03f..c46c5b7688 100644
--- a/scripts/system/controllers/handControllerGrab.js
+++ b/scripts/system/controllers/handControllerGrab.js
@@ -2297,16 +2297,7 @@ function MyController(hand) {
             // If this looks like the release after adjusting something still held in the other hand, print the position
             // and rotation of the held thing to help content creators set the userData.
             var grabData = getEntityCustomData(GRAB_USER_DATA_KEY, this.grabbedEntity, {});
-            if (grabData.refCount > 1) {
-                var grabbedProperties = Entities.getEntityProperties(this.grabbedEntity, ["localPosition", "localRotation"]);
-                if (grabbedProperties && grabbedProperties.localPosition && grabbedProperties.localRotation) {
-                    print((this.hand === RIGHT_HAND ? '"LeftHand"' : '"RightHand"') + ":" +
-                        '[{"x":' + grabbedProperties.localPosition.x + ', "y":' + grabbedProperties.localPosition.y +
-                        ', "z":' + grabbedProperties.localPosition.z + '}, {"x":' + grabbedProperties.localRotation.x +
-                        ', "y":' + grabbedProperties.localRotation.y + ', "z":' + grabbedProperties.localRotation.z +
-                        ', "w":' + grabbedProperties.localRotation.w + '}]');
-                }
-            }
+            this.printNewOffsets = (grabData.refCount > 1);
 
             if (this.actionID !== null) {
                 Entities.deleteAction(this.grabbedEntity, this.actionID);
@@ -2555,6 +2546,17 @@ function MyController(hand) {
                     }
                 };
                 Entities.editEntity(entityID, deactiveProps);
+
+                if (this.printNewOffsets) {
+                    var grabbedProperties = Entities.getEntityProperties(this.grabbedEntity, ["localPosition", "localRotation"]);
+                    if (grabbedProperties && grabbedProperties.localPosition && grabbedProperties.localRotation) {
+                        print((this.hand === RIGHT_HAND ? '"LeftHand"' : '"RightHand"') + ":" +
+                              '[{"x":' + grabbedProperties.localPosition.x + ', "y":' + grabbedProperties.localPosition.y +
+                              ', "z":' + grabbedProperties.localPosition.z + '}, {"x":' + grabbedProperties.localRotation.x +
+                              ', "y":' + grabbedProperties.localRotation.y + ', "z":' + grabbedProperties.localRotation.z +
+                              ', "w":' + grabbedProperties.localRotation.w + '}]');
+                    }
+                }
             } else if (noVelocity) {
                 Entities.editEntity(entityID, {
                     velocity: {

From 7dad303ce72a38205c042575b7231853a2a9307c Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Sat, 27 Aug 2016 19:25:05 -0700
Subject: [PATCH 16/44] don't leave stray blue search sphere when near-grabbing

---
 scripts/system/controllers/handControllerGrab.js | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/scripts/system/controllers/handControllerGrab.js b/scripts/system/controllers/handControllerGrab.js
index c46c5b7688..5236db21c7 100644
--- a/scripts/system/controllers/handControllerGrab.js
+++ b/scripts/system/controllers/handControllerGrab.js
@@ -938,10 +938,14 @@ function MyController(hand) {
         var searchSphereLocation = Vec3.sum(distantPickRay.origin,
             Vec3.multiply(distantPickRay.direction, this.searchSphereDistance));
         this.searchSphereOn(searchSphereLocation, SEARCH_SPHERE_SIZE * this.searchSphereDistance,
-            (this.triggerSmoothedGrab() || this.secondarySqueezed()) ? COLORS_GRAB_SEARCHING_FULL_SQUEEZE : COLORS_GRAB_SEARCHING_HALF_SQUEEZE);
+                            (this.triggerSmoothedGrab() || this.secondarySqueezed()) ?
+                            COLORS_GRAB_SEARCHING_FULL_SQUEEZE :
+                            COLORS_GRAB_SEARCHING_HALF_SQUEEZE);
         if (PICK_WITH_HAND_RAY) {
             this.overlayLineOn(handPosition, searchSphereLocation,
-                (this.triggerSmoothedGrab() || this.secondarySqueezed()) ? COLORS_GRAB_SEARCHING_FULL_SQUEEZE : COLORS_GRAB_SEARCHING_HALF_SQUEEZE);
+                               (this.triggerSmoothedGrab() || this.secondarySqueezed()) ?
+                               COLORS_GRAB_SEARCHING_FULL_SQUEEZE :
+                               COLORS_GRAB_SEARCHING_HALF_SQUEEZE);
         }
     };
 
@@ -1841,6 +1845,7 @@ function MyController(hand) {
         this.grabPointSphereOff();
         this.lineOff();
         this.overlayLineOff();
+        this.searchSphereOff();
 
         this.dropGestureReset();
         this.clearEquipHaptics();

From 3355097bd04c4c74026e4e113f1966e6bc7907cb Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Sat, 27 Aug 2016 19:46:15 -0700
Subject: [PATCH 17/44] handle another type of abandoned grab

---
 scripts/system/controllers/handControllerGrab.js | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/scripts/system/controllers/handControllerGrab.js b/scripts/system/controllers/handControllerGrab.js
index 5236db21c7..c2708c1598 100644
--- a/scripts/system/controllers/handControllerGrab.js
+++ b/scripts/system/controllers/handControllerGrab.js
@@ -23,7 +23,7 @@ Script.include("/~/system/libraries/Xform.js");
 //
 var WANT_DEBUG = false;
 var WANT_DEBUG_STATE = false;
-var WANT_DEBUG_SEARCH_NAME = null;
+var WANT_DEBUG_SEARCH_NAME = "Hifi-Bow"; // null;
 
 var FORCE_IGNORE_IK = true;
 var SHOW_GRAB_POINT_SPHERE = true;
@@ -1255,20 +1255,21 @@ function MyController(hand) {
         var physical = propsArePhysical(props);
         var grabbable = false;
         var debug = (WANT_DEBUG_SEARCH_NAME && props.name === WANT_DEBUG_SEARCH_NAME);
+        var refCount = ("refCount" in grabProps) ? grabProps.refCount : 0;
 
         if (physical) {
             // physical things default to grabbable
             grabbable = true;
         } else {
             // non-physical things default to non-grabbable unless they are already grabbed
-            if ("refCount" in grabProps && grabProps.refCount > 0) {
+            if (refCount > 0) {
                 grabbable = true;
             } else {
                 grabbable = false;
             }
         }
 
-        if (grabbableProps.hasOwnProperty("grabbable")) {
+        if (grabbableProps.hasOwnProperty("grabbable") && refCount == 0) {
             grabbable = grabbableProps.grabbable;
         }
 

From 2a5d686a762fae7692207e5bac95574566d41a4c Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Sat, 27 Aug 2016 20:11:16 -0700
Subject: [PATCH 18/44] don't show grab-point sphere when near grabbing or
 equipping

---
 scripts/system/controllers/handControllerGrab.js | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/scripts/system/controllers/handControllerGrab.js b/scripts/system/controllers/handControllerGrab.js
index c2708c1598..c2d980edb8 100644
--- a/scripts/system/controllers/handControllerGrab.js
+++ b/scripts/system/controllers/handControllerGrab.js
@@ -1983,6 +1983,8 @@ function MyController(hand) {
 
     this.nearGrabbing = function(deltaTime, timestamp) {
 
+        this.grabPointSphereOff();
+
         if (this.state == STATE_NEAR_GRABBING && !this.triggerClicked) {
             this.callEntityMethodOnGrabbed("releaseGrab");
             this.setState(STATE_OFF, "trigger released");

From d69e712bef2debc7c8d03165c5c215f3e557d1b6 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Sat, 27 Aug 2016 20:23:19 -0700
Subject: [PATCH 19/44] keep near-grab radius small but still allow large equip
 hotspots

---
 scripts/system/controllers/handControllerGrab.js | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/scripts/system/controllers/handControllerGrab.js b/scripts/system/controllers/handControllerGrab.js
index c2d980edb8..8bbff61a41 100644
--- a/scripts/system/controllers/handControllerGrab.js
+++ b/scripts/system/controllers/handControllerGrab.js
@@ -1269,7 +1269,7 @@ function MyController(hand) {
             }
         }
 
-        if (grabbableProps.hasOwnProperty("grabbable") && refCount == 0) {
+        if (grabbableProps.hasOwnProperty("grabbable") && refCount === 0) {
             grabbable = grabbableProps.grabbable;
         }
 
@@ -1415,10 +1415,10 @@ function MyController(hand) {
             entityPropertiesCache.addEntity(rayPickInfo.entityID);
         }
 
-        var candidateEntities = Entities.findEntities(handPosition, MAX_EQUIP_HOTSPOT_RADIUS);
-        entityPropertiesCache.addEntities(candidateEntities);
+        var candidateHotSpotEntities = Entities.findEntities(handPosition, MAX_EQUIP_HOTSPOT_RADIUS);
+        entityPropertiesCache.addEntities(candidateHotSpotEntities);
 
-        var potentialEquipHotspot = this.chooseBestEquipHotspot(candidateEntities);
+        var potentialEquipHotspot = this.chooseBestEquipHotspot(candidateHotSpotEntities);
         if (potentialEquipHotspot) {
             if (this.triggerSmoothedGrab()) {
                 this.grabbedHotspot = potentialEquipHotspot;
@@ -1428,6 +1428,7 @@ function MyController(hand) {
             }
         }
 
+        var candidateEntities = Entities.findEntities(handPosition, NEAR_GRAB_RADIUS);
         var grabbableEntities = candidateEntities.filter(function(entity) {
             return _this.entityIsNearGrabbable(entity, handPosition, NEAR_GRAB_MAX_DISTANCE);
         });

From 5daf334a0e70a1b4a8bb383afd911b8754d740da Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Sat, 27 Aug 2016 20:42:03 -0700
Subject: [PATCH 20/44] avoid extra grab-point spheres when resetting scripts

---
 scripts/system/controllers/handControllerGrab.js | 1 +
 1 file changed, 1 insertion(+)

diff --git a/scripts/system/controllers/handControllerGrab.js b/scripts/system/controllers/handControllerGrab.js
index 8bbff61a41..889f63f9f2 100644
--- a/scripts/system/controllers/handControllerGrab.js
+++ b/scripts/system/controllers/handControllerGrab.js
@@ -2341,6 +2341,7 @@ function MyController(hand) {
 
     this.cleanup = function() {
         this.release();
+        this.grabPointSphereOff();
     };
 
     this.heartBeat = function(entityID) {

From 3fb10d0eea59a00ea530fb7095569ebbd89724c4 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth.alves@gmail.com>
Date: Sun, 28 Aug 2016 19:08:13 -0700
Subject: [PATCH 21/44] action for arrow flight, etc

---
 interface/src/InterfaceActionFactory.cpp      |   3 +
 .../entities/src/EntityActionInterface.cpp    |   5 +
 .../entities/src/EntityActionInterface.h      |   3 +-
 .../src/ObjectActionTravelOriented.cpp        | 203 ++++++++++++++++++
 .../physics/src/ObjectActionTravelOriented.h  |  39 ++++
 5 files changed, 252 insertions(+), 1 deletion(-)
 create mode 100644 libraries/physics/src/ObjectActionTravelOriented.cpp
 create mode 100644 libraries/physics/src/ObjectActionTravelOriented.h

diff --git a/interface/src/InterfaceActionFactory.cpp b/interface/src/InterfaceActionFactory.cpp
index 1869980270..2bc4608e86 100644
--- a/interface/src/InterfaceActionFactory.cpp
+++ b/interface/src/InterfaceActionFactory.cpp
@@ -14,6 +14,7 @@
 #include <avatar/AvatarActionHold.h>
 #include <ObjectActionOffset.h>
 #include <ObjectActionSpring.h>
+#include <ObjectActionTravelOriented.h>
 #include <LogHandler.h>
 
 #include "InterfaceActionFactory.h"
@@ -29,6 +30,8 @@ EntityActionPointer interfaceActionFactory(EntityActionType type, const QUuid& i
             return std::make_shared<ObjectActionSpring>(id, ownerEntity);
         case ACTION_TYPE_HOLD:
             return std::make_shared<AvatarActionHold>(id, ownerEntity);
+        case ACTION_TYPE_TRAVEL_ORIENTED:
+            return std::make_shared<ObjectActionTravelOriented>(id, ownerEntity);
     }
 
     Q_ASSERT_X(false, Q_FUNC_INFO, "Unknown entity action type");
diff --git a/libraries/entities/src/EntityActionInterface.cpp b/libraries/entities/src/EntityActionInterface.cpp
index ce9a93a6ac..2ce4ce5555 100644
--- a/libraries/entities/src/EntityActionInterface.cpp
+++ b/libraries/entities/src/EntityActionInterface.cpp
@@ -100,6 +100,9 @@ EntityActionType EntityActionInterface::actionTypeFromString(QString actionTypeS
     if (normalizedActionTypeString == "hold") {
         return ACTION_TYPE_HOLD;
     }
+    if (normalizedActionTypeString == "traveloriented") {
+        return ACTION_TYPE_TRAVEL_ORIENTED;
+    }
 
     qDebug() << "Warning -- EntityActionInterface::actionTypeFromString got unknown action-type name" << actionTypeString;
     return ACTION_TYPE_NONE;
@@ -115,6 +118,8 @@ QString EntityActionInterface::actionTypeToString(EntityActionType actionType) {
             return "spring";
         case ACTION_TYPE_HOLD:
             return "hold";
+        case ACTION_TYPE_TRAVEL_ORIENTED:
+            return "travel-oriented";
     }
     assert(false);
     return "none";
diff --git a/libraries/entities/src/EntityActionInterface.h b/libraries/entities/src/EntityActionInterface.h
index 9a881cf94c..d9a901f1f6 100644
--- a/libraries/entities/src/EntityActionInterface.h
+++ b/libraries/entities/src/EntityActionInterface.h
@@ -28,7 +28,8 @@ enum EntityActionType {
     ACTION_TYPE_NONE = 0,
     ACTION_TYPE_OFFSET = 1000,
     ACTION_TYPE_SPRING = 2000,
-    ACTION_TYPE_HOLD = 3000
+    ACTION_TYPE_HOLD = 3000,
+    ACTION_TYPE_TRAVEL_ORIENTED = 4000
 };
 
 
diff --git a/libraries/physics/src/ObjectActionTravelOriented.cpp b/libraries/physics/src/ObjectActionTravelOriented.cpp
new file mode 100644
index 0000000000..003dc4c8b5
--- /dev/null
+++ b/libraries/physics/src/ObjectActionTravelOriented.cpp
@@ -0,0 +1,203 @@
+//
+//  ObjectActionTravelOriented.cpp
+//  libraries/physics/src
+//
+//  Created by Seth Alves 2015-6-5
+//  Copyright 2015 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#include "QVariantGLM.h"
+#include "ObjectActionTravelOriented.h"
+
+const uint16_t ObjectActionTravelOriented::actionVersion = 1;
+
+
+ObjectActionTravelOriented::ObjectActionTravelOriented(const QUuid& id, EntityItemPointer ownerEntity) :
+    ObjectAction(ACTION_TYPE_TRAVEL_ORIENTED, id, ownerEntity) {
+    #if WANT_DEBUG
+    qDebug() << "ObjectActionTravelOriented::ObjectActionTravelOriented";
+    #endif
+}
+
+ObjectActionTravelOriented::~ObjectActionTravelOriented() {
+    #if WANT_DEBUG
+    qDebug() << "ObjectActionTravelOriented::~ObjectActionTravelOriented";
+    #endif
+}
+
+void ObjectActionTravelOriented::updateActionWorker(btScalar deltaTimeStep) {
+    withReadLock([&]{
+        auto ownerEntity = _ownerEntity.lock();
+        if (!ownerEntity) {
+            return;
+        }
+        void* physicsInfo = ownerEntity->getPhysicsInfo();
+        if (!physicsInfo) {
+            return;
+        }
+        ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
+        btRigidBody* rigidBody = motionState->getRigidBody();
+        if (!rigidBody) {
+            qDebug() << "ObjectActionTravelOriented::updateActionWorker no rigidBody";
+            return;
+        }
+        const float MAX_TIMESCALE = 600.0f; // 10 min is a long time
+        if (_angularTimeScale > MAX_TIMESCALE) {
+            return;
+        }
+
+        // find normalized velocity
+        glm::vec3 velocity = bulletToGLM(rigidBody->getLinearVelocity());
+        float speed = glm::length(velocity);
+        const float TRAVEL_ORIENTED_TOO_SLOW = 0.001; // meters / second
+        if (speed < TRAVEL_ORIENTED_TOO_SLOW) {
+            return;
+        }
+        velocity = glm::normalize(velocity);
+
+        // find current angle of "forward"
+        btQuaternion bodyRotation = rigidBody->getOrientation();
+        glm::quat orientation = bulletToGLM(bodyRotation);
+        glm::vec3 forwardInWorldFrame = glm::normalize(orientation * _forward);
+
+        // find the rotation that would line up velocity and forward
+        glm::quat rotationalTarget = glm::rotation(forwardInWorldFrame, velocity);
+        btVector3 targetVelocity(0.0f, 0.0f, 0.0f);
+
+        auto alignmentDot = bodyRotation.dot(glmToBullet(rotationalTarget));
+        const float ALMOST_ONE = 0.99999f;
+        if (glm::abs(alignmentDot) < ALMOST_ONE) {
+            btQuaternion target = glmToBullet(rotationalTarget);
+            if (alignmentDot < 0.0f) {
+                target = -target;
+            }
+            // if dQ is the incremental rotation that gets an object from Q0 to Q1 then:
+            //
+            //      Q1 = dQ * Q0
+            //
+            // solving for dQ gives:
+            //
+            //      dQ = Q1 * Q0^
+            btQuaternion deltaQ = target * bodyRotation.inverse();
+            float speed = deltaQ.getAngle() / _angularTimeScale;
+            targetVelocity = speed * deltaQ.getAxis();
+            if (speed > rigidBody->getAngularSleepingThreshold()) {
+                rigidBody->activate();
+            }
+        }
+        // this action is aggresively critically damped and defeats the current velocity
+        rigidBody->setAngularVelocity(targetVelocity);
+    });
+}
+
+const float MIN_TIMESCALE = 0.1f;
+
+
+bool ObjectActionTravelOriented::updateArguments(QVariantMap arguments) {
+    glm::vec3 forward;
+    float angularTimeScale;
+
+    bool needUpdate = false;
+    bool somethingChanged = ObjectAction::updateArguments(arguments);
+    withReadLock([&]{
+        // targets are required, spring-constants are optional
+        bool ok = true;
+        forward = EntityActionInterface::extractVec3Argument("travel oriented action", arguments, "forward", ok, false);
+        if (ok) {
+            forward = _forward;
+        }
+        ok = true;
+        angularTimeScale =
+            EntityActionInterface::extractFloatArgument("spring action", arguments, "angularTimeScale", ok, false);
+        if (!ok) {
+            angularTimeScale = _angularTimeScale;
+        }
+
+        if (somethingChanged ||
+            forward != _forward ||
+            angularTimeScale != _angularTimeScale) {
+            // something changed
+            needUpdate = true;
+        }
+    });
+
+    if (needUpdate) {
+        withWriteLock([&] {
+            _forward = forward;
+            _angularTimeScale = glm::max(MIN_TIMESCALE, glm::abs(angularTimeScale));
+            _active = (_forward != glm::vec3());
+
+            auto ownerEntity = _ownerEntity.lock();
+            if (ownerEntity) {
+                ownerEntity->setActionDataDirty(true);
+                ownerEntity->setActionDataNeedsTransmit(true);
+            }
+        });
+        activateBody();
+    }
+
+    return true;
+}
+
+QVariantMap ObjectActionTravelOriented::getArguments() {
+    QVariantMap arguments = ObjectAction::getArguments();
+    withReadLock([&] {
+        arguments["forward"] = glmToQMap(_forward);
+        arguments["angularTimeScale"] = _angularTimeScale;
+    });
+    return arguments;
+}
+
+QByteArray ObjectActionTravelOriented::serialize() const {
+    QByteArray serializedActionArguments;
+    QDataStream dataStream(&serializedActionArguments, QIODevice::WriteOnly);
+
+    dataStream << ACTION_TYPE_SPRING;
+    dataStream << getID();
+    dataStream << ObjectActionTravelOriented::actionVersion;
+
+    withReadLock([&] {
+        dataStream << _forward;
+        dataStream << _angularTimeScale;
+
+        dataStream << localTimeToServerTime(_expires);
+        dataStream << _tag;
+    });
+
+    return serializedActionArguments;
+}
+
+void ObjectActionTravelOriented::deserialize(QByteArray serializedArguments) {
+    QDataStream dataStream(serializedArguments);
+
+    EntityActionType type;
+    dataStream >> type;
+    assert(type == getType());
+
+    QUuid id;
+    dataStream >> id;
+    assert(id == getID());
+
+    uint16_t serializationVersion;
+    dataStream >> serializationVersion;
+    if (serializationVersion != ObjectActionTravelOriented::actionVersion) {
+        assert(false);
+        return;
+    }
+
+    withWriteLock([&] {
+        dataStream >> _forward;
+        dataStream >> _angularTimeScale;
+
+        quint64 serverExpires;
+        dataStream >> serverExpires;
+        _expires = serverTimeToLocalTime(serverExpires);
+
+        dataStream >> _tag;
+
+        _active = (_forward != glm::vec3());
+    });
+}
diff --git a/libraries/physics/src/ObjectActionTravelOriented.h b/libraries/physics/src/ObjectActionTravelOriented.h
new file mode 100644
index 0000000000..66a425b409
--- /dev/null
+++ b/libraries/physics/src/ObjectActionTravelOriented.h
@@ -0,0 +1,39 @@
+//
+//  ObjectActionTravelOriented.h
+//  libraries/physics/src
+//
+//  Created by Seth Alves 2016-8-28
+//  Copyright 2015 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#ifndef hifi_ObjectActionTravelOriented_h
+#define hifi_ObjectActionTravelOriented_h
+
+#include "ObjectAction.h"
+
+class ObjectActionTravelOriented : public ObjectAction {
+public:
+    ObjectActionTravelOriented(const QUuid& id, EntityItemPointer ownerEntity);
+    virtual ~ObjectActionTravelOriented();
+
+    virtual bool updateArguments(QVariantMap arguments) override;
+    virtual QVariantMap getArguments() override;
+
+    virtual void updateActionWorker(float deltaTimeStep) override;
+
+    virtual QByteArray serialize() const override;
+    virtual void deserialize(QByteArray serializedArguments) override;
+
+protected:
+    static const uint16_t actionVersion;
+
+    glm::vec3 _forward { glm::vec3() }; // the vector in object space that should point in the direction of travel
+    float _angularTimeScale { 0.1f };
+
+    glm::vec3 _angularVelocityTarget;
+};
+
+#endif // hifi_ObjectActionTravelOriented_h

From 670e85994d5d54c64e294cf88835a323d0f99ca2 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Sun, 28 Aug 2016 20:05:29 -0700
Subject: [PATCH 22/44] fix arrow action

---
 libraries/physics/src/ObjectActionTravelOriented.cpp | 11 +++++------
 1 file changed, 5 insertions(+), 6 deletions(-)

diff --git a/libraries/physics/src/ObjectActionTravelOriented.cpp b/libraries/physics/src/ObjectActionTravelOriented.cpp
index 003dc4c8b5..3deff31b47 100644
--- a/libraries/physics/src/ObjectActionTravelOriented.cpp
+++ b/libraries/physics/src/ObjectActionTravelOriented.cpp
@@ -29,7 +29,7 @@ ObjectActionTravelOriented::~ObjectActionTravelOriented() {
 }
 
 void ObjectActionTravelOriented::updateActionWorker(btScalar deltaTimeStep) {
-    withReadLock([&]{
+    withReadLock([&] {
         auto ownerEntity = _ownerEntity.lock();
         if (!ownerEntity) {
             return;
@@ -103,15 +103,14 @@ bool ObjectActionTravelOriented::updateArguments(QVariantMap arguments) {
     bool needUpdate = false;
     bool somethingChanged = ObjectAction::updateArguments(arguments);
     withReadLock([&]{
-        // targets are required, spring-constants are optional
         bool ok = true;
-        forward = EntityActionInterface::extractVec3Argument("travel oriented action", arguments, "forward", ok, false);
-        if (ok) {
+        forward = EntityActionInterface::extractVec3Argument("travel oriented action", arguments, "forward", ok, true);
+        if (!ok) {
             forward = _forward;
         }
         ok = true;
         angularTimeScale =
-            EntityActionInterface::extractFloatArgument("spring action", arguments, "angularTimeScale", ok, false);
+            EntityActionInterface::extractFloatArgument("travel oriented action", arguments, "angularTimeScale", ok, false);
         if (!ok) {
             angularTimeScale = _angularTimeScale;
         }
@@ -155,7 +154,7 @@ QByteArray ObjectActionTravelOriented::serialize() const {
     QByteArray serializedActionArguments;
     QDataStream dataStream(&serializedActionArguments, QIODevice::WriteOnly);
 
-    dataStream << ACTION_TYPE_SPRING;
+    dataStream << ACTION_TYPE_TRAVEL_ORIENTED;
     dataStream << getID();
     dataStream << ObjectActionTravelOriented::actionVersion;
 

From a729a953786e14832254cf763a00303e22e007b8 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Sun, 28 Aug 2016 21:03:32 -0700
Subject: [PATCH 23/44] fix math

---
 libraries/physics/src/ObjectActionTravelOriented.cpp | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/libraries/physics/src/ObjectActionTravelOriented.cpp b/libraries/physics/src/ObjectActionTravelOriented.cpp
index 3deff31b47..bf844bad5f 100644
--- a/libraries/physics/src/ObjectActionTravelOriented.cpp
+++ b/libraries/physics/src/ObjectActionTravelOriented.cpp
@@ -9,6 +9,8 @@
 //  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
 //
 
+#include <glm/gtc/quaternion.hpp>
+
 #include "QVariantGLM.h"
 #include "ObjectActionTravelOriented.h"
 
@@ -64,7 +66,8 @@ void ObjectActionTravelOriented::updateActionWorker(btScalar deltaTimeStep) {
         glm::vec3 forwardInWorldFrame = glm::normalize(orientation * _forward);
 
         // find the rotation that would line up velocity and forward
-        glm::quat rotationalTarget = glm::rotation(forwardInWorldFrame, velocity);
+        glm::quat neededRotation = ::rotationBetween(forwardInWorldFrame, velocity);
+        glm::quat rotationalTarget = orientation * neededRotation;
         btVector3 targetVelocity(0.0f, 0.0f, 0.0f);
 
         auto alignmentDot = bodyRotation.dot(glmToBullet(rotationalTarget));

From be1332532fd4d9585e594958e62c3caaa5892404 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Mon, 29 Aug 2016 03:39:52 -0700
Subject: [PATCH 24/44] disable debugging prints

---
 scripts/system/controllers/handControllerGrab.js | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/scripts/system/controllers/handControllerGrab.js b/scripts/system/controllers/handControllerGrab.js
index 889f63f9f2..25af96ba8a 100644
--- a/scripts/system/controllers/handControllerGrab.js
+++ b/scripts/system/controllers/handControllerGrab.js
@@ -23,7 +23,7 @@ Script.include("/~/system/libraries/Xform.js");
 //
 var WANT_DEBUG = false;
 var WANT_DEBUG_STATE = false;
-var WANT_DEBUG_SEARCH_NAME = "Hifi-Bow"; // null;
+var WANT_DEBUG_SEARCH_NAME = null;
 
 var FORCE_IGNORE_IK = true;
 var SHOW_GRAB_POINT_SPHERE = true;
@@ -995,7 +995,8 @@ function MyController(hand) {
     this.turnOffVisualizations = function() {
 
         this.overlayLineOff();
-
+        this.grabPointSphereOff();
+        this.lineOff();
         this.searchSphereOff();
         restore2DMode();
 

From 3c90413ebb26d271f839bdb2e03f6b9942553d46 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Mon, 29 Aug 2016 04:06:53 -0700
Subject: [PATCH 25/44] don't render stray/invalid hand-lasers in HMD mode

---
 .../src/display-plugins/hmd/HmdDisplayPlugin.cpp          | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/libraries/display-plugins/src/display-plugins/hmd/HmdDisplayPlugin.cpp b/libraries/display-plugins/src/display-plugins/hmd/HmdDisplayPlugin.cpp
index 6904700be5..01f607b0ed 100644
--- a/libraries/display-plugins/src/display-plugins/hmd/HmdDisplayPlugin.cpp
+++ b/libraries/display-plugins/src/display-plugins/hmd/HmdDisplayPlugin.cpp
@@ -527,9 +527,11 @@ void HmdDisplayPlugin::compositeExtra() {
             if (_presentHandPoses[index] == IDENTITY_MATRIX) {
                 return;
             }
-            const auto& points = _presentHandLaserPoints[index];
-            const auto& lasers = _presentHandLasers[index];
-            geometryCache->renderGlowLine(batch, points.first, points.second, lasers.color);
+            const auto& laser = _presentHandLasers[index];
+            if (laser.valid()) {
+                const auto& points = _presentHandLaserPoints[index];
+                geometryCache->renderGlowLine(batch, points.first, points.second, laser.color);
+            }
         });
     });
 }

From 55b68ad4b1555ec842ef83980831e3081ab3b5ca Mon Sep 17 00:00:00 2001
From: Seth Alves <seth.alves@gmail.com>
Date: Mon, 29 Aug 2016 10:21:58 -0700
Subject: [PATCH 26/44] fix warning

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

diff --git a/libraries/physics/src/ObjectActionTravelOriented.cpp b/libraries/physics/src/ObjectActionTravelOriented.cpp
index bf844bad5f..7cf1e6da84 100644
--- a/libraries/physics/src/ObjectActionTravelOriented.cpp
+++ b/libraries/physics/src/ObjectActionTravelOriented.cpp
@@ -54,7 +54,7 @@ void ObjectActionTravelOriented::updateActionWorker(btScalar deltaTimeStep) {
         // find normalized velocity
         glm::vec3 velocity = bulletToGLM(rigidBody->getLinearVelocity());
         float speed = glm::length(velocity);
-        const float TRAVEL_ORIENTED_TOO_SLOW = 0.001; // meters / second
+        const float TRAVEL_ORIENTED_TOO_SLOW = 0.001f; // meters / second
         if (speed < TRAVEL_ORIENTED_TOO_SLOW) {
             return;
         }

From 7d13f9220c7100aad33b32b1da1e20a9505f06ef Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Tue, 30 Aug 2016 09:55:42 -0700
Subject: [PATCH 27/44] fix math, renamed a couple variables

---
 .../physics/src/ObjectActionTravelOriented.cpp    | 15 ++++++++-------
 1 file changed, 8 insertions(+), 7 deletions(-)

diff --git a/libraries/physics/src/ObjectActionTravelOriented.cpp b/libraries/physics/src/ObjectActionTravelOriented.cpp
index 7cf1e6da84..18d09d21d9 100644
--- a/libraries/physics/src/ObjectActionTravelOriented.cpp
+++ b/libraries/physics/src/ObjectActionTravelOriented.cpp
@@ -58,17 +58,18 @@ void ObjectActionTravelOriented::updateActionWorker(btScalar deltaTimeStep) {
         if (speed < TRAVEL_ORIENTED_TOO_SLOW) {
             return;
         }
-        velocity = glm::normalize(velocity);
+        glm::vec3 direction = glm::normalize(velocity);
 
         // find current angle of "forward"
         btQuaternion bodyRotation = rigidBody->getOrientation();
         glm::quat orientation = bulletToGLM(bodyRotation);
         glm::vec3 forwardInWorldFrame = glm::normalize(orientation * _forward);
 
-        // find the rotation that would line up velocity and forward
-        glm::quat neededRotation = ::rotationBetween(forwardInWorldFrame, velocity);
-        glm::quat rotationalTarget = orientation * neededRotation;
-        btVector3 targetVelocity(0.0f, 0.0f, 0.0f);
+        // find the rotation that would line up direction and forward
+        glm::quat neededRotation = glm::rotation(forwardInWorldFrame, direction);
+        glm::quat rotationalTarget = neededRotation * orientation;
+
+        btVector3 targetAngularVelocity(0.0f, 0.0f, 0.0f);
 
         auto alignmentDot = bodyRotation.dot(glmToBullet(rotationalTarget));
         const float ALMOST_ONE = 0.99999f;
@@ -86,13 +87,13 @@ void ObjectActionTravelOriented::updateActionWorker(btScalar deltaTimeStep) {
             //      dQ = Q1 * Q0^
             btQuaternion deltaQ = target * bodyRotation.inverse();
             float speed = deltaQ.getAngle() / _angularTimeScale;
-            targetVelocity = speed * deltaQ.getAxis();
+            targetAngularVelocity = speed * deltaQ.getAxis();
             if (speed > rigidBody->getAngularSleepingThreshold()) {
                 rigidBody->activate();
             }
         }
         // this action is aggresively critically damped and defeats the current velocity
-        rigidBody->setAngularVelocity(targetVelocity);
+        rigidBody->setAngularVelocity(targetAngularVelocity);
     });
 }
 

From 3a5f92d7a2b3ba5f8b0d6a07889b75b4bcdbcdd8 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Tue, 30 Aug 2016 11:16:13 -0700
Subject: [PATCH 28/44] ignoreIK for equipping.  change how auto-unequip
 searches

---
 .../system/controllers/handControllerGrab.js  | 30 +++++++++++++++++--
 1 file changed, 27 insertions(+), 3 deletions(-)

diff --git a/scripts/system/controllers/handControllerGrab.js b/scripts/system/controllers/handControllerGrab.js
index 24a3794244..c6e0d691a6 100644
--- a/scripts/system/controllers/handControllerGrab.js
+++ b/scripts/system/controllers/handControllerGrab.js
@@ -1926,7 +1926,15 @@ function MyController(hand) {
         } else {
             // grab entity via parenting
             this.actionID = null;
-            var handJointIndex = MyAvatar.getJointIndex(this.hand === RIGHT_HAND ? "RightHand" : "LeftHand");
+            var handJointIndex;
+            if (this.ignoreIK) {
+                handJointIndex = MyAvatar.getJointIndex(this.hand === RIGHT_HAND ?
+                                                        "_CONTROLLER_RIGHTHAND" :
+                                                        "_CONTROLLER_LEFTHAND");
+            } else {
+                handJointIndex = MyAvatar.getJointIndex(this.hand === RIGHT_HAND ? "RightHand" : "LeftHand");
+            }
+
             var reparentProps = {
                 parentID: MyAvatar.sessionUUID,
                 parentJointIndex: handJointIndex,
@@ -2043,11 +2051,23 @@ function MyController(hand) {
             this.lastUnequipCheckTime = now;
 
             if (props.parentID == MyAvatar.sessionUUID) {
-                var handPosition = this.getHandPosition();
+                var heldItemPosition;
+                var heldItemRotation;
+                if (this.ignoreIK) {
+                    var heldItemLocation = this.getControllerLocation();
+                    heldItemPosition = heldItemLocation.position;
+                    heldItemRotation = heldItemLocation.orientation;
+                } else {
+                    heldItemPosition = this.getHandPosition();
+                    heldItemRotation = this.getHandRotation();
+                }
+
+                heldItemPosition = Vec3.sum(heldItemPosition, Vec3.multiplyQbyV(heldItemRotation, this.offsetPosition));
+
                 // the center of the equipped object being far from the hand isn't enough to auto-unequip -- we also
                 // need to fail the findEntities test.
                 var TEAR_AWAY_DISTANCE = 0.04;
-                var nearPickedCandidateEntities = Entities.findEntities(handPosition, NEAR_GRAB_RADIUS + TEAR_AWAY_DISTANCE);
+                var nearPickedCandidateEntities = Entities.findEntities(heldItemPosition, NEAR_GRAB_RADIUS + TEAR_AWAY_DISTANCE);
                 if (nearPickedCandidateEntities.indexOf(this.grabbedEntity) == -1) {
                     // for whatever reason, the held/equipped entity has been pulled away.  ungrab or unequip.
                     print("handControllerGrab -- autoreleasing held or equipped item because it is far from hand." +
@@ -2429,6 +2449,10 @@ function MyController(hand) {
         // unhook them.
         var handJointIndex = MyAvatar.getJointIndex(this.hand === RIGHT_HAND ? "RightHand" : "LeftHand");
         var children = Entities.getChildrenIDsOfJoint(MyAvatar.sessionUUID, handJointIndex);
+        var controllerJointIndex = MyAvatar.getJointIndex(this.hand === RIGHT_HAND ?
+                                                          "_CONTROLLER_RIGHTHAND" :
+                                                          "_CONTROLLER_LEFTHAND");
+        children.concat(Entities.getChildrenIDsOfJoint(MyAvatar.sessionUUID, controllerJointIndex));
         children.forEach(function(childID) {
             print("disconnecting stray child of hand: (" + _this.hand + ") " + childID);
             Entities.editEntity(childID, {

From 5bda94b2ba433b88032954fefafa55eb8f1bf3b3 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth.alves@gmail.com>
Date: Tue, 30 Aug 2016 12:39:30 -0700
Subject: [PATCH 29/44] use same data as used for drawing show-hand-targets for
 controller joints in MyAvatar

---
 interface/src/avatar/MyAvatar.cpp | 32 +++++++++++++++++++++++++++++++
 interface/src/avatar/MyAvatar.h   |  3 +++
 2 files changed, 35 insertions(+)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 50baabc844..17ee91bf42 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -2233,3 +2233,35 @@ bool MyAvatar::didTeleport() {
 bool MyAvatar::hasDriveInput() const {
     return fabsf(_driveKeys[TRANSLATE_X]) > 0.0f || fabsf(_driveKeys[TRANSLATE_Y]) > 0.0f || fabsf(_driveKeys[TRANSLATE_Z]) > 0.0f;
 }
+
+glm::quat MyAvatar::getAbsoluteJointRotationInObjectFrame(int index) const {
+    switch(index) {
+        case CONTROLLER_LEFTHAND_INDEX: {
+            auto leftHandPose = getLeftHandControllerPoseInWorldFrame();
+            return leftHandPose.getRotation();
+        }
+        case CONTROLLER_RIGHTHAND_INDEX: {
+            auto rightHandPose = getRightHandControllerPoseInWorldFrame();
+            return rightHandPose.getRotation();
+        }
+        default: {
+            return Avatar::getAbsoluteJointRotationInObjectFrame(index);
+        }
+    }
+}
+
+glm::vec3 MyAvatar::getAbsoluteJointTranslationInObjectFrame(int index) const {
+    switch(index) {
+        case CONTROLLER_LEFTHAND_INDEX: {
+            auto leftHandPose = getLeftHandControllerPoseInWorldFrame();
+            return leftHandPose.getTranslation();
+        }
+        case CONTROLLER_RIGHTHAND_INDEX: {
+            auto rightHandPose = getRightHandControllerPoseInWorldFrame();
+            return rightHandPose.getTranslation();
+        }
+        default: {
+            return Avatar::getAbsoluteJointTranslationInObjectFrame(index);
+        }
+    }
+}
diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h
index cb733aa0ed..696aaabd2f 100644
--- a/interface/src/avatar/MyAvatar.h
+++ b/interface/src/avatar/MyAvatar.h
@@ -275,6 +275,9 @@ public:
     Q_INVOKABLE void setCharacterControllerEnabled(bool enabled);
     Q_INVOKABLE bool getCharacterControllerEnabled();
 
+    virtual glm::quat getAbsoluteJointRotationInObjectFrame(int index) const override;
+    virtual glm::vec3 getAbsoluteJointTranslationInObjectFrame(int index) const override;
+
 public slots:
     void increaseSize();
     void decreaseSize();

From 78f54a7f3379e86bf6e31a625816020287f7010d Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Tue, 30 Aug 2016 14:39:11 -0700
Subject: [PATCH 30/44] get rid of some jitter in equipped items.  do a better
 job of deciding when something equipped has been torn-away

---
 interface/src/avatar/MyAvatar.cpp             | 12 +++----
 .../system/controllers/handControllerGrab.js  | 33 ++++++++++---------
 2 files changed, 21 insertions(+), 24 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index 17ee91bf42..e7a93fa928 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -2237,12 +2237,10 @@ bool MyAvatar::hasDriveInput() const {
 glm::quat MyAvatar::getAbsoluteJointRotationInObjectFrame(int index) const {
     switch(index) {
         case CONTROLLER_LEFTHAND_INDEX: {
-            auto leftHandPose = getLeftHandControllerPoseInWorldFrame();
-            return leftHandPose.getRotation();
+            return getLeftHandControllerPoseInAvatarFrame().getRotation();
         }
         case CONTROLLER_RIGHTHAND_INDEX: {
-            auto rightHandPose = getRightHandControllerPoseInWorldFrame();
-            return rightHandPose.getRotation();
+            return getRightHandControllerPoseInAvatarFrame().getRotation();
         }
         default: {
             return Avatar::getAbsoluteJointRotationInObjectFrame(index);
@@ -2253,12 +2251,10 @@ glm::quat MyAvatar::getAbsoluteJointRotationInObjectFrame(int index) const {
 glm::vec3 MyAvatar::getAbsoluteJointTranslationInObjectFrame(int index) const {
     switch(index) {
         case CONTROLLER_LEFTHAND_INDEX: {
-            auto leftHandPose = getLeftHandControllerPoseInWorldFrame();
-            return leftHandPose.getTranslation();
+            return getLeftHandControllerPoseInAvatarFrame().getTranslation();
         }
         case CONTROLLER_RIGHTHAND_INDEX: {
-            auto rightHandPose = getRightHandControllerPoseInWorldFrame();
-            return rightHandPose.getTranslation();
+            return getRightHandControllerPoseInAvatarFrame().getTranslation();
         }
         default: {
             return Avatar::getAbsoluteJointTranslationInObjectFrame(index);
diff --git a/scripts/system/controllers/handControllerGrab.js b/scripts/system/controllers/handControllerGrab.js
index bae788aa88..726e5e49ad 100644
--- a/scripts/system/controllers/handControllerGrab.js
+++ b/scripts/system/controllers/handControllerGrab.js
@@ -714,14 +714,16 @@ function MyController(hand) {
         }
     };
     // controllerLocation is where the controller would be, in-world.
-    this.getControllerLocation = function () {
+    this.getControllerLocation = function (doOffset) {
         var standardControllerValue = (hand === RIGHT_HAND) ? Controller.Standard.RightHand : Controller.Standard.LeftHand;
         var pose = Controller.getPoseValue(standardControllerValue);
 
         var orientation = Quat.multiply(MyAvatar.orientation, pose.rotation);
         var position = Vec3.sum(Vec3.multiplyQbyV(MyAvatar.orientation, pose.translation), MyAvatar.position);
         // add to the real position so the grab-point is out in front of the hand, a bit
-        position = Vec3.sum(position, Vec3.multiplyQbyV(orientation, GRAB_POINT_SPHERE_OFFSET));
+        if (doOffset) {
+            position = Vec3.sum(position, Vec3.multiplyQbyV(orientation, GRAB_POINT_SPHERE_OFFSET));
+        }
 
         return {position: position, orientation: orientation};
     };
@@ -1067,7 +1069,7 @@ function MyController(hand) {
         }
         if (!this.waitForTriggerRelease && this.triggerSmoothedSqueezed()) {
             this.lastPickTime = 0;
-            this.startingHandRotation = this.getControllerLocation().orientation;
+            this.startingHandRotation = this.getControllerLocation(true).orientation;
             if (this.triggerSmoothedSqueezed()) {
                 this.setState(STATE_SEARCHING, "trigger squeeze detected");
                 return;
@@ -1117,7 +1119,7 @@ function MyController(hand) {
     // @returns {object} returns object with two keys entityID and distance
     //
     this.calcRayPickInfo = function(hand) {
-        var controllerLocation = this.getControllerLocation();
+        var controllerLocation = this.getControllerLocation(true);
         var worldHandPosition = controllerLocation.position;
         var worldHandRotation = controllerLocation.orientation;
 
@@ -1413,7 +1415,7 @@ function MyController(hand) {
         }
 
         // var handPosition = this.getHandPosition();
-        var handPosition = this.getControllerLocation().position;
+        var handPosition = this.getControllerLocation(true).position;
 
         var rayPickInfo = this.calcRayPickInfo(this.hand);
 
@@ -1598,7 +1600,7 @@ function MyController(hand) {
         this.clearEquipHaptics();
         this.grabPointSphereOff();
 
-        var worldControllerPosition = this.getControllerLocation().position;
+        var worldControllerPosition = this.getControllerLocation(true).position;
 
         // transform the position into room space
         var worldToSensorMat = Mat4.inverse(MyAvatar.getSensorToWorldMatrix());
@@ -1664,7 +1666,7 @@ function MyController(hand) {
         this.heartBeat(this.grabbedEntity);
 
 
-        var controllerLocation = this.getControllerLocation();
+        var controllerLocation = this.getControllerLocation(true);
         var worldControllerPosition = controllerLocation.position;
         var worldControllerRotation = controllerLocation.orientation;
 
@@ -1801,7 +1803,7 @@ function MyController(hand) {
     };
 
     this.dropGestureProcess = function(deltaTime) {
-        var worldHandRotation = this.getControllerLocation().orientation;
+        var worldHandRotation = this.getControllerLocation(true).orientation;
         var localHandUpAxis = this.hand === RIGHT_HAND ? {
             x: 1,
             y: 0,
@@ -1879,11 +1881,9 @@ function MyController(hand) {
         var handRotation;
         var handPosition;
         if (this.ignoreIK) {
-            var controllerLocation = this.getControllerLocation();
+            var controllerLocation = this.getControllerLocation(false);
             handRotation = controllerLocation.orientation;
-            // subtract off the GRAB_POINT_SPHERE_OFFSET that was added in getControllerLocation
-            handPosition = Vec3.subtract(controllerLocation.position,
-                                         Vec3.multiplyQbyV(handRotation, GRAB_POINT_SPHERE_OFFSET));
+            handPosition = controllerLocation.position;
         } else {
             handRotation = this.getHandRotation();
             handPosition = this.getHandPosition();
@@ -2059,7 +2059,7 @@ function MyController(hand) {
                 var heldItemPosition;
                 var heldItemRotation;
                 if (this.ignoreIK) {
-                    var heldItemLocation = this.getControllerLocation();
+                    var heldItemLocation = this.getControllerLocation(false);
                     heldItemPosition = heldItemLocation.position;
                     heldItemRotation = heldItemLocation.orientation;
                 } else {
@@ -2067,6 +2067,7 @@ function MyController(hand) {
                     heldItemRotation = this.getHandRotation();
                 }
 
+                // figure out where the center of the held object should be
                 heldItemPosition = Vec3.sum(heldItemPosition, Vec3.multiplyQbyV(heldItemRotation, this.offsetPosition));
 
                 // the center of the equipped object being far from the hand isn't enough to auto-unequip -- we also
@@ -2207,7 +2208,7 @@ function MyController(hand) {
 
     this.entityTouchingEnter = function() {
         // test for intersection between controller laser and web entity plane.
-        var intersectInfo = handLaserIntersectEntity(this.grabbedEntity, this.getControllerLocation());
+        var intersectInfo = handLaserIntersectEntity(this.grabbedEntity, this.getControllerLocation(true));
         if (intersectInfo) {
             var pointerEvent = {
                 type: "Press",
@@ -2232,7 +2233,7 @@ function MyController(hand) {
 
     this.entityTouchingExit = function() {
         // test for intersection between controller laser and web entity plane.
-        var intersectInfo = handLaserIntersectEntity(this.grabbedEntity, this.getControllerLocation());
+        var intersectInfo = handLaserIntersectEntity(this.grabbedEntity, this.getControllerLocation(true));
         if (intersectInfo) {
             var pointerEvent;
             if (this.deadspotExpired) {
@@ -2270,7 +2271,7 @@ function MyController(hand) {
         }
 
         // test for intersection between controller laser and web entity plane.
-        var intersectInfo = handLaserIntersectEntity(this.grabbedEntity, this.getControllerLocation());
+        var intersectInfo = handLaserIntersectEntity(this.grabbedEntity, this.getControllerLocation(true));
         if (intersectInfo) {
 
             if (Entities.keyboardFocusEntity != this.grabbedEntity) {

From cdd08bd38e47496377137c5d9260f10563100a3f Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Tue, 30 Aug 2016 14:59:30 -0700
Subject: [PATCH 31/44] more anti-jitter changes

---
 interface/src/avatar/AvatarActionHold.cpp | 37 ++++++-----------------
 1 file changed, 10 insertions(+), 27 deletions(-)

diff --git a/interface/src/avatar/AvatarActionHold.cpp b/interface/src/avatar/AvatarActionHold.cpp
index 5aa64a72cf..559c34e249 100644
--- a/interface/src/avatar/AvatarActionHold.cpp
+++ b/interface/src/avatar/AvatarActionHold.cpp
@@ -127,37 +127,20 @@ bool AvatarActionHold::getTarget(float deltaTimeStep, glm::quat& rotation, glm::
             if (pose.isValid()) {
                 linearVelocity = pose.getVelocity();
                 angularVelocity = pose.getAngularVelocity();
+
+                if (isRightHand) {
+                    pose = avatarManager->getMyAvatar()->getRightHandControllerPoseInAvatarFrame();
+                } else {
+                    pose = avatarManager->getMyAvatar()->getLeftHandControllerPoseInAvatarFrame();
+                }
             }
 
             if (_ignoreIK && pose.isValid()) {
-                // We cannot ignore other avatars IK and this is not the point of this option
-                // This is meant to make the grabbing behavior more reactive.
-
-                // The avatar moves between prepareForPhysicsSimulation and this, so do some stuff to avoid jitter:
-                // - transform the pose's world-position into the space relative to the old rigid-body
-                // - then transform this relative position back into world-space via the new rigid-body's transform
-
-                Transform poseTransform;
-                poseTransform.setTranslation(pose.getTranslation());
-                poseTransform.setRotation(pose.getRotation());
-
-                Transform preStepAvatarTransform;
-                preStepAvatarTransform.setTranslation(_preStepAvatarPosition);
-                preStepAvatarTransform.setRotation(_preStepAvatarRotation);
-                Transform inversePreStepAvatarTransform = Transform(preStepAvatarTransform.getInverseMatrix());
-
                 Transform avatarTransform;
-                glm::vec3 avatarRigidBodyPosition;
-                glm::quat avatarRigidBodyRotation;
-                getAvatarRigidBodyLocation(avatarRigidBodyPosition, avatarRigidBodyRotation);
-                avatarTransform.setTranslation(avatarRigidBodyPosition);
-                avatarTransform.setRotation(avatarRigidBodyRotation);
-
-                glm::mat4 adjustedMatrix = avatarTransform.getMatrix() *
-                    (inversePreStepAvatarTransform.getMatrix() * poseTransform.getMatrix());
-                Transform adjustedTransform = Transform(adjustedMatrix);
-                palmPosition = adjustedTransform.getTranslation();
-                palmRotation = adjustedTransform.getRotation();
+                auto myAvatar = DependencyManager::get<AvatarManager>()->getMyAvatar();
+                avatarTransform = myAvatar->getTransform();
+                palmPosition = avatarTransform.transform(pose.getTranslation());
+                palmRotation = avatarTransform.getRotation() * pose.getRotation();
             } else {
                 glm::vec3 avatarRigidBodyPosition;
                 glm::quat avatarRigidBodyRotation;

From 3d436d4c2615037ce2a8a334fc65c87a7b452ac2 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Tue, 30 Aug 2016 16:23:11 -0700
Subject: [PATCH 32/44] bump entity protocol version

---
 libraries/networking/src/udt/PacketHeaders.cpp | 2 +-
 libraries/networking/src/udt/PacketHeaders.h   | 1 +
 2 files changed, 2 insertions(+), 1 deletion(-)

diff --git a/libraries/networking/src/udt/PacketHeaders.cpp b/libraries/networking/src/udt/PacketHeaders.cpp
index 0ef5dcfce3..c0272a644d 100644
--- a/libraries/networking/src/udt/PacketHeaders.cpp
+++ b/libraries/networking/src/udt/PacketHeaders.cpp
@@ -47,7 +47,7 @@ PacketVersion versionForPacketType(PacketType packetType) {
         case PacketType::EntityAdd:
         case PacketType::EntityEdit:
         case PacketType::EntityData:
-            return VERSION_WEB_ENTITIES_SUPPORT_DPI;
+            return VERSION_ENTITIES_ARROW_ACTION;
         case PacketType::AvatarIdentity:
         case PacketType::AvatarData:
         case PacketType::BulkAvatarData:
diff --git a/libraries/networking/src/udt/PacketHeaders.h b/libraries/networking/src/udt/PacketHeaders.h
index 2718f4c4a0..8d08585c2b 100644
--- a/libraries/networking/src/udt/PacketHeaders.h
+++ b/libraries/networking/src/udt/PacketHeaders.h
@@ -187,6 +187,7 @@ const PacketVersion VERSION_ENTITIES_PROPERLY_ENCODE_SHAPE_EDITS = 60;
 const PacketVersion VERSION_MODEL_ENTITIES_SUPPORT_STATIC_MESH = 61;
 const PacketVersion VERSION_MODEL_ENTITIES_SUPPORT_SIMPLE_HULLS = 62;
 const PacketVersion VERSION_WEB_ENTITIES_SUPPORT_DPI = 63;
+const PacketVersion VERSION_ENTITIES_ARROW_ACTION = 64;
 
 enum class AvatarMixerPacketVersion : PacketVersion {
     TranslationSupport = 17,

From 280b60374710f1411e9c89d6a447c0cc56890503 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth.alves@gmail.com>
Date: Wed, 31 Aug 2016 16:30:51 -0700
Subject: [PATCH 33/44] remove some leftover code

---
 interface/src/avatar/AvatarActionHold.cpp | 2 --
 interface/src/avatar/AvatarActionHold.h   | 3 ---
 2 files changed, 5 deletions(-)

diff --git a/interface/src/avatar/AvatarActionHold.cpp b/interface/src/avatar/AvatarActionHold.cpp
index 559c34e249..5e02eb6bf7 100644
--- a/interface/src/avatar/AvatarActionHold.cpp
+++ b/interface/src/avatar/AvatarActionHold.cpp
@@ -59,8 +59,6 @@ void AvatarActionHold::prepareForPhysicsSimulation() {
         glm::vec3 avatarRigidBodyPosition;
         glm::quat avatarRigidBodyRotation;
         getAvatarRigidBodyLocation(avatarRigidBodyPosition, avatarRigidBodyRotation);
-        _preStepAvatarPosition = avatarRigidBodyPosition;
-        _preStepAvatarRotation = avatarRigidBodyRotation;
 
         if (_ignoreIK) {
             return;
diff --git a/interface/src/avatar/AvatarActionHold.h b/interface/src/avatar/AvatarActionHold.h
index b30360884f..bfa392172d 100644
--- a/interface/src/avatar/AvatarActionHold.h
+++ b/interface/src/avatar/AvatarActionHold.h
@@ -68,9 +68,6 @@ private:
     static const int velocitySmoothFrames;
     QVector<glm::vec3> _measuredLinearVelocities;
     int _measuredLinearVelocitiesIndex { 0 };
-
-    glm::vec3 _preStepAvatarPosition;
-    glm::quat _preStepAvatarRotation;
 };
 
 #endif // hifi_AvatarActionHold_h

From dc31525794e611f37b4fa01b7d3a4edbb4f0b42c Mon Sep 17 00:00:00 2001
From: Seth Alves <seth.alves@gmail.com>
Date: Wed, 31 Aug 2016 16:39:57 -0700
Subject: [PATCH 34/44] remove some left-over code

---
 interface/src/avatar/MyAvatar.cpp | 10 +++-------
 interface/src/avatar/MyAvatar.h   |  3 +--
 2 files changed, 4 insertions(+), 9 deletions(-)

diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp
index e7a93fa928..24dbc62318 100644
--- a/interface/src/avatar/MyAvatar.cpp
+++ b/interface/src/avatar/MyAvatar.cpp
@@ -532,8 +532,7 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) {
     _hmdSensorFacing = getFacingDir2D(_hmdSensorOrientation);
 }
 
-void MyAvatar::updateJointFromController(glm::mat4& previousSensorToWorldInverseMatrix, controller::Action poseKey,
-                                         ThreadSafeValueCache<glm::mat4>& matrixCache) {
+void MyAvatar::updateJointFromController(controller::Action poseKey, ThreadSafeValueCache<glm::mat4>& matrixCache) {
     assert(QThread::currentThread() == thread());
     auto userInputMapper = DependencyManager::get<UserInputMapper>();
     controller::Pose controllerPose = userInputMapper->getPoseState(poseKey);
@@ -548,9 +547,6 @@ void MyAvatar::updateJointFromController(glm::mat4& previousSensorToWorldInverse
 // update sensor to world matrix from current body position and hmd sensor.
 // This is so the correct camera can be used for rendering.
 void MyAvatar::updateSensorToWorldMatrix() {
-
-    glm::mat4 previousSensorToWorldInverse = glm::inverse(_sensorToWorldMatrix);
-
     // update the sensor mat so that the body position will end up in the desired
     // position when driven from the head.
     glm::mat4 desiredMat = createMatFromQuatAndPos(getOrientation(), getPosition());
@@ -565,8 +561,8 @@ void MyAvatar::updateSensorToWorldMatrix() {
 
     _sensorToWorldMatrixCache.set(_sensorToWorldMatrix);
 
-    updateJointFromController(previousSensorToWorldInverse, controller::Action::LEFT_HAND, _controllerLeftHandMatrixCache);
-    updateJointFromController(previousSensorToWorldInverse, controller::Action::RIGHT_HAND, _controllerRightHandMatrixCache);
+    updateJointFromController(controller::Action::LEFT_HAND, _controllerLeftHandMatrixCache);
+    updateJointFromController(controller::Action::RIGHT_HAND, _controllerRightHandMatrixCache);
 }
 
 //  Update avatar head rotation with sensor data
diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h
index 696aaabd2f..c4ffc08cbc 100644
--- a/interface/src/avatar/MyAvatar.h
+++ b/interface/src/avatar/MyAvatar.h
@@ -119,8 +119,7 @@ public:
     void updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix);
 
     // read the location of a hand controller and save the transform
-    void updateJointFromController(glm::mat4& previousSensorToWorldInverseMatrix, controller::Action poseKey,
-                                   ThreadSafeValueCache<glm::mat4>& matrixCache);
+    void updateJointFromController(controller::Action poseKey, ThreadSafeValueCache<glm::mat4>& matrixCache);
 
     // best called at end of main loop, just before rendering.
     // update sensor to world matrix from current body position and hmd sensor.

From 47259ee0531e5031680731af956c08e5fbba5116 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Wed, 31 Aug 2016 16:52:46 -0700
Subject: [PATCH 35/44] find equip-points with controller position rather than
 hand

---
 scripts/system/controllers/handControllerGrab.js | 13 ++++++-------
 1 file changed, 6 insertions(+), 7 deletions(-)

diff --git a/scripts/system/controllers/handControllerGrab.js b/scripts/system/controllers/handControllerGrab.js
index 726e5e49ad..118025d13d 100644
--- a/scripts/system/controllers/handControllerGrab.js
+++ b/scripts/system/controllers/handControllerGrab.js
@@ -1078,11 +1078,11 @@ function MyController(hand) {
 
         this.grabPointSphereOn();
 
-        var candidateEntities = Entities.findEntities(this.getHandPosition(), MAX_EQUIP_HOTSPOT_RADIUS);
+        var candidateEntities = Entities.findEntities(this.getControllerLocation(true).position, MAX_EQUIP_HOTSPOT_RADIUS);
         entityPropertiesCache.addEntities(candidateEntities);
         var potentialEquipHotspot = this.chooseBestEquipHotspot(candidateEntities);
         if (!this.waitForTriggerRelease) {
-            this.updateEquipHaptics(potentialEquipHotspot, this.getHandPosition());
+            this.updateEquipHaptics(potentialEquipHotspot, this.getControllerLocation(true).position);
         }
 
         var nearEquipHotspots = this.chooseNearEquipHotspots(candidateEntities, EQUIP_HOTSPOT_RENDER_RADIUS);
@@ -1367,7 +1367,7 @@ function MyController(hand) {
             return _this.collectEquipHotspots(entityID);
         })).filter(function(hotspot) {
             return (_this.hotspotIsEquippable(hotspot) &&
-                Vec3.distance(hotspot.worldPosition, _this.getHandPosition()) < hotspot.radius + distance);
+                Vec3.distance(hotspot.worldPosition, _this.getControllerLocation(true).position) < hotspot.radius + distance);
         });
         return equippableHotspots;
     };
@@ -1378,8 +1378,8 @@ function MyController(hand) {
         if (equippableHotspots.length > 0) {
             // sort by distance
             equippableHotspots.sort(function(a, b) {
-                var aDistance = Vec3.distance(a.worldPosition, this.getHandPosition());
-                var bDistance = Vec3.distance(b.worldPosition, this.getHandPosition());
+                var aDistance = Vec3.distance(a.worldPosition, this.getControllerLocation(true).position);
+                var bDistance = Vec3.distance(b.worldPosition, this.getControllerLocation(true).position);
                 return aDistance - bDistance;
             });
             return equippableHotspots[0];
@@ -1414,7 +1414,6 @@ function MyController(hand) {
             return;
         }
 
-        // var handPosition = this.getHandPosition();
         var handPosition = this.getControllerLocation(true).position;
 
         var rayPickInfo = this.calcRayPickInfo(this.hand);
@@ -1721,7 +1720,7 @@ function MyController(hand) {
         var objectToAvatar = Vec3.subtract(this.currentObjectPosition, MyAvatar.position);
         if (handControllerData.disableMoveWithHead !== true) {
             // mix in head motion
-            if (MOVE_WITH_HEAD) {
+            if (MOVE_WITH_HEAD) {
                 var objDistance = Vec3.length(objectToAvatar);
                 var before = Vec3.multiplyQbyV(this.currentCameraOrientation, {
                     x: 0.0,

From 53ba190ca6469beabb8be95427e0ba6af4cc3715 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Thu, 1 Sep 2016 09:57:46 -0700
Subject: [PATCH 36/44] don't do auto-release test unless the hand is equipping

---
 libraries/entities-renderer/src/EntityTreeRenderer.cpp | 5 ++---
 libraries/entities-renderer/src/EntityTreeRenderer.h   | 2 +-
 scripts/system/controllers/handControllerGrab.js       | 2 +-
 3 files changed, 4 insertions(+), 5 deletions(-)

diff --git a/libraries/entities-renderer/src/EntityTreeRenderer.cpp b/libraries/entities-renderer/src/EntityTreeRenderer.cpp
index d9f1234674..24d22fee96 100644
--- a/libraries/entities-renderer/src/EntityTreeRenderer.cpp
+++ b/libraries/entities-renderer/src/EntityTreeRenderer.cpp
@@ -556,14 +556,13 @@ ModelPointer EntityTreeRenderer::allocateModel(const QString& url, float loading
     return model;
 }
 
-ModelPointer EntityTreeRenderer::updateModel(ModelPointer model, const QString& newUrl, const QString& collisionUrl) {
+ModelPointer EntityTreeRenderer::updateModel(ModelPointer model, const QString& newUrl) {
     // Only create and delete models on the thread that owns the EntityTreeRenderer
     if (QThread::currentThread() != thread()) {
         QMetaObject::invokeMethod(this, "updateModel", Qt::BlockingQueuedConnection,
             Q_RETURN_ARG(ModelPointer, model),
                 Q_ARG(ModelPointer, model),
-                Q_ARG(const QString&, newUrl),
-                Q_ARG(const QString&, collisionUrl));
+                Q_ARG(const QString&, newUrl));
 
         return model;
     }
diff --git a/libraries/entities-renderer/src/EntityTreeRenderer.h b/libraries/entities-renderer/src/EntityTreeRenderer.h
index 5c4bc04510..b1d875c2fb 100644
--- a/libraries/entities-renderer/src/EntityTreeRenderer.h
+++ b/libraries/entities-renderer/src/EntityTreeRenderer.h
@@ -76,7 +76,7 @@ public:
     Q_INVOKABLE ModelPointer allocateModel(const QString& url, float loadingPriority = 0.0f);
 
     /// if a renderable entity item needs to update the URL of a model, we will handle that for the entity
-    Q_INVOKABLE ModelPointer updateModel(ModelPointer original, const QString& newUrl, const QString& collisionUrl);
+    Q_INVOKABLE ModelPointer updateModel(ModelPointer original, const QString& newUrl);
 
     /// if a renderable entity item is done with a model, it should return it to us
     void releaseModel(ModelPointer model);
diff --git a/scripts/system/controllers/handControllerGrab.js b/scripts/system/controllers/handControllerGrab.js
index 118025d13d..532adf8de2 100644
--- a/scripts/system/controllers/handControllerGrab.js
+++ b/scripts/system/controllers/handControllerGrab.js
@@ -2051,7 +2051,7 @@ function MyController(hand) {
         }
 
         var now = Date.now();
-        if (now - this.lastUnequipCheckTime > MSECS_PER_SEC * CHECK_TOO_FAR_UNEQUIP_TIME) {
+        if (this.state == STATE_HOLD && now - this.lastUnequipCheckTime > MSECS_PER_SEC * CHECK_TOO_FAR_UNEQUIP_TIME) {
             this.lastUnequipCheckTime = now;
 
             if (props.parentID == MyAvatar.sessionUUID) {

From a35e527128e54c254ee53c963e9e0beef7876111 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Thu, 1 Sep 2016 11:46:08 -0700
Subject: [PATCH 37/44] try to fix far-trigger

---
 scripts/system/controllers/handControllerGrab.js | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

diff --git a/scripts/system/controllers/handControllerGrab.js b/scripts/system/controllers/handControllerGrab.js
index 532adf8de2..a35207ad11 100644
--- a/scripts/system/controllers/handControllerGrab.js
+++ b/scripts/system/controllers/handControllerGrab.js
@@ -2044,7 +2044,7 @@ function MyController(hand) {
         var props = Entities.getEntityProperties(this.grabbedEntity, ["localPosition", "parentID",
                                                                       "position", "rotation", "dimensions"]);
         if (!props.position) {
-            // server may have reset, taking our equipped entity with it.  move back to "off" stte
+            // server may have reset, taking our equipped entity with it.  move back to "off" state
             this.callEntityMethodOnGrabbed("releaseGrab");
             this.setState(STATE_OFF, "entity has no position property");
             return;
@@ -2175,10 +2175,9 @@ function MyController(hand) {
             return;
         }
 
-        var handPosition = this.getHandPosition();
         var pickRay = {
-            origin: handPosition,
-            direction: Quat.getUp(this.getControllerRotation())
+            origin: this.getControllerLocation().position,
+            direction: Quat.getUp(this.getControllerLocation().orientation)
         };
 
         var now = Date.now();

From 1a9f43cb67153f88b779aa05f4b18eb36d95515a Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Thu, 1 Sep 2016 12:17:34 -0700
Subject: [PATCH 38/44] fix AvatarActionHold's idea of where the controllers
 are when the avatar has been scaled

---
 interface/src/avatar/AvatarActionHold.cpp | 2 +-
 libraries/avatars/src/AvatarData.h        | 2 ++
 2 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/interface/src/avatar/AvatarActionHold.cpp b/interface/src/avatar/AvatarActionHold.cpp
index 5e02eb6bf7..51171b9c6b 100644
--- a/interface/src/avatar/AvatarActionHold.cpp
+++ b/interface/src/avatar/AvatarActionHold.cpp
@@ -137,7 +137,7 @@ bool AvatarActionHold::getTarget(float deltaTimeStep, glm::quat& rotation, glm::
                 Transform avatarTransform;
                 auto myAvatar = DependencyManager::get<AvatarManager>()->getMyAvatar();
                 avatarTransform = myAvatar->getTransform();
-                palmPosition = avatarTransform.transform(pose.getTranslation());
+                palmPosition = avatarTransform.transform(pose.getTranslation() / myAvatar->getTargetScale());
                 palmRotation = avatarTransform.getRotation() * pose.getRotation();
             } else {
                 glm::vec3 avatarRigidBodyPosition;
diff --git a/libraries/avatars/src/AvatarData.h b/libraries/avatars/src/AvatarData.h
index 9c49e11956..715c24b8ee 100644
--- a/libraries/avatars/src/AvatarData.h
+++ b/libraries/avatars/src/AvatarData.h
@@ -373,6 +373,8 @@ public slots:
     virtual bool setAbsoluteJointRotationInObjectFrame(int index, const glm::quat& rotation) override { return false; }
     virtual bool setAbsoluteJointTranslationInObjectFrame(int index, const glm::vec3& translation) override { return false; }
 
+    float getTargetScale() { return _targetScale; }
+
 protected:
     glm::vec3 _handPosition;
 

From 045042a9c6521da8b182bd9a60303d3f36ce4273 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Thu, 1 Sep 2016 14:15:49 -0700
Subject: [PATCH 39/44] don't disable grabbing when teleporting

---
 scripts/system/controllers/teleport.js | 13 -------------
 1 file changed, 13 deletions(-)

diff --git a/scripts/system/controllers/teleport.js b/scripts/system/controllers/teleport.js
index f3e94d23de..b4a8eefcd2 100644
--- a/scripts/system/controllers/teleport.js
+++ b/scripts/system/controllers/teleport.js
@@ -94,7 +94,6 @@ function Teleporter() {
 
     this.initialize = function() {
         this.createMappings();
-        this.disableGrab();
     };
 
     this.createMappings = function() {
@@ -218,10 +217,6 @@ function Teleporter() {
         this.updateConnected = null;
         this.inCoolIn = false;
         inTeleportMode = false;
-
-        Script.setTimeout(function() {
-            _this.enableGrab();
-        }, 200);
     };
 
     this.update = function() {
@@ -494,14 +489,6 @@ function Teleporter() {
         });
     };
 
-    this.disableGrab = function() {
-        Messages.sendLocalMessage('Hifi-Hand-Disabler', this.teleportHand);
-    };
-
-    this.enableGrab = function() {
-        Messages.sendLocalMessage('Hifi-Hand-Disabler', 'none');
-    };
-
     this.triggerHaptics = function() {
         var hand = this.teleportHand === 'left' ? 0 : 1;
         var haptic = Controller.triggerShortHapticPulse(0.2, hand);

From dd7e25441c39d7849057190d56177a131a55eca5 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth.alves@gmail.com>
Date: Thu, 1 Sep 2016 14:58:37 -0700
Subject: [PATCH 40/44] include which hand in grab messages

---
 scripts/system/controllers/handControllerGrab.js | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/scripts/system/controllers/handControllerGrab.js b/scripts/system/controllers/handControllerGrab.js
index a35207ad11..0f41a458ea 100644
--- a/scripts/system/controllers/handControllerGrab.js
+++ b/scripts/system/controllers/handControllerGrab.js
@@ -1720,7 +1720,7 @@ function MyController(hand) {
         var objectToAvatar = Vec3.subtract(this.currentObjectPosition, MyAvatar.position);
         if (handControllerData.disableMoveWithHead !== true) {
             // mix in head motion
-            if (MOVE_WITH_HEAD) {
+            if (MOVE_WITH_HEAD) {
                 var objDistance = Vec3.length(objectToAvatar);
                 var before = Vec3.multiplyQbyV(this.currentCameraOrientation, {
                     x: 0.0,
@@ -1925,7 +1925,8 @@ function MyController(hand) {
             }
             Messages.sendMessage('Hifi-Object-Manipulation', JSON.stringify({
                 action: 'grab',
-                grabbedEntity: this.grabbedEntity
+                grabbedEntity: this.grabbedEntity,
+                joint: this.hand === RIGHT_HAND ? "RightHand" : "LeftHand"
             }));
         } else {
             // grab entity via parenting
@@ -1953,7 +1954,8 @@ function MyController(hand) {
 
             Messages.sendMessage('Hifi-Object-Manipulation', JSON.stringify({
                 action: 'equip',
-                grabbedEntity: this.grabbedEntity
+                grabbedEntity: this.grabbedEntity,
+                joint: this.hand === RIGHT_HAND ? "RightHand" : "LeftHand"
             }));
         }
 

From b1eb4c361dab51dc9e0d8613cce5dc3762659076 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Thu, 1 Sep 2016 15:02:41 -0700
Subject: [PATCH 41/44] if a hand is disabled while holding something, release
 it

---
 .../system/controllers/handControllerGrab.js  | 24 +++++++++++--------
 1 file changed, 14 insertions(+), 10 deletions(-)

diff --git a/scripts/system/controllers/handControllerGrab.js b/scripts/system/controllers/handControllerGrab.js
index 0f41a458ea..9845418798 100644
--- a/scripts/system/controllers/handControllerGrab.js
+++ b/scripts/system/controllers/handControllerGrab.js
@@ -1595,7 +1595,7 @@ function MyController(hand) {
     };
 
     this.distanceHoldingEnter = function() {
-        Messages.sendLocalMessage('Hifi-Teleport-Disabler','both');
+        Messages.sendLocalMessage('Hifi-Teleport-Disabler', 'both');
         this.clearEquipHaptics();
         this.grabPointSphereOff();
 
@@ -2308,7 +2308,7 @@ function MyController(hand) {
     };
 
     this.release = function() {
-        Messages.sendLocalMessage('Hifi-Teleport-Disabler','none');
+        Messages.sendLocalMessage('Hifi-Teleport-Disabler', 'none');
         this.turnOffVisualizations();
 
         var noVelocity = false;
@@ -2334,17 +2334,17 @@ function MyController(hand) {
                     noVelocity = true;
                 }
             }
+
+            this.deactivateEntity(this.grabbedEntity, noVelocity);
+
+            Messages.sendMessage('Hifi-Object-Manipulation', JSON.stringify({
+                action: 'release',
+                grabbedEntity: this.grabbedEntity,
+                joint: this.hand === RIGHT_HAND ? "RightHand" : "LeftHand"
+            }));
         }
 
-        this.deactivateEntity(this.grabbedEntity, noVelocity);
         this.actionID = null;
-
-        Messages.sendMessage('Hifi-Object-Manipulation', JSON.stringify({
-            action: 'release',
-            grabbedEntity: this.grabbedEntity,
-            joint: this.hand === RIGHT_HAND ? "RightHand" : "LeftHand"
-        }));
-
         this.grabbedEntity = null;
         this.grabbedHotspot = null;
 
@@ -2644,9 +2644,13 @@ function update(deltaTime) {
 
     if (handToDisable !== LEFT_HAND && handToDisable !== 'both') {
         leftController.update(deltaTime, timestamp);
+    } else {
+        leftController.release();
     }
     if (handToDisable !== RIGHT_HAND && handToDisable !== 'both') {
         rightController.update(deltaTime, timestamp);
+    } else {
+        rightController.release();
     }
     equipHotspotBuddy.update(deltaTime, timestamp);
     entityPropertiesCache.update();

From da352819410543bb0a0b7a7e45ede4869555c167 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Thu, 1 Sep 2016 16:57:23 -0700
Subject: [PATCH 42/44] don't assume collides-with-dynamic during a multi-grab

---
 scripts/system/controllers/handControllerGrab.js | 15 +++++++++++++--
 1 file changed, 13 insertions(+), 2 deletions(-)

diff --git a/scripts/system/controllers/handControllerGrab.js b/scripts/system/controllers/handControllerGrab.js
index 9845418798..47aba44614 100644
--- a/scripts/system/controllers/handControllerGrab.js
+++ b/scripts/system/controllers/handControllerGrab.js
@@ -189,7 +189,6 @@ var STATE_ENTITY_TOUCHING = 7;
 // "collidesWith" is specified by comma-separated list of group names
 // the possible group names are:  static, dynamic, kinematic, myAvatar, otherAvatar
 var COLLIDES_WITH_WHILE_GRABBED = "dynamic,otherAvatar";
-var COLLIDES_WITH_WHILE_MULTI_GRABBED = "dynamic";
 
 var HEART_BEAT_INTERVAL = 5 * MSECS_PER_SEC;
 var HEART_BEAT_TIMEOUT = 15 * MSECS_PER_SEC;
@@ -418,6 +417,18 @@ function removeMyAvatarFromCollidesWith(origCollidesWith) {
     return collidesWithSplit.join();
 }
 
+function removeAvatarsFromCollidesWith(origCollidesWith) {
+    var collidesWithSplit = origCollidesWith.split(",");
+    // remove myAvatar from the array
+    for (var i = collidesWithSplit.length - 1; i >= 0; i--) {
+        if (collidesWithSplit[i] === "myAvatar" || collidesWithSplit[i] === "otherAvatar") {
+            collidesWithSplit.splice(i, 1);
+        }
+    }
+    return collidesWithSplit.join();
+}
+
+
 // If another script is managing the reticle (as is done by HandControllerPointer), we should not be setting it here,
 // and we should not be showing lasers when someone else is using the Reticle to indicate a 2D minor mode.
 var EXTERNALLY_MANAGED_2D_MINOR_MODE = true;
@@ -2442,7 +2453,7 @@ function MyController(hand) {
                 // bootstrap themselves with the held object.  This happens because the meaning of "otherAvatar" in
                 // the collision mask hinges on who the physics simulation owner is.
                 Entities.editEntity(entityID, {
-                    "collidesWith": COLLIDES_WITH_WHILE_MULTI_GRABBED
+                    "collidesWith": removeAvatarsFromCollidesWith(grabbedProperties.collidesWith)
                 });
             }
         }

From 8505d1eeed951e3e62d00e10fb4e2efe0ab1a60a Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Thu, 1 Sep 2016 17:51:02 -0700
Subject: [PATCH 43/44] avoid a bootstrap

---
 scripts/system/controllers/handControllerGrab.js | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/scripts/system/controllers/handControllerGrab.js b/scripts/system/controllers/handControllerGrab.js
index 47aba44614..5b35245e74 100644
--- a/scripts/system/controllers/handControllerGrab.js
+++ b/scripts/system/controllers/handControllerGrab.js
@@ -2453,7 +2453,8 @@ function MyController(hand) {
                 // bootstrap themselves with the held object.  This happens because the meaning of "otherAvatar" in
                 // the collision mask hinges on who the physics simulation owner is.
                 Entities.editEntity(entityID, {
-                    "collidesWith": removeAvatarsFromCollidesWith(grabbedProperties.collidesWith)
+                    // "collidesWith": removeAvatarsFromCollidesWith(grabbedProperties.collidesWith)
+                    collisionless: true
                 });
             }
         }
@@ -2712,7 +2713,7 @@ var handleHandMessages = function(channel, message, sender) {
                 selectedController.nearGrabbingEnter();
 
             } catch (e) {
-                print("WARNING: error parsing Hifi-Hand-Grab message");
+                print("WARNING: handControllerGrab.js -- error parsing Hifi-Hand-Grab message: " + message);
             }
 
         } else if (channel === 'Hifi-Hand-RayPick-Blacklist') {
@@ -2732,7 +2733,7 @@ var handleHandMessages = function(channel, message, sender) {
                 }
 
             } catch (e) {
-                print("WARNING: error parsing Hifi-Hand-RayPick-Blacklist message");
+                print("WARNING: handControllerGrab.js -- error parsing Hifi-Hand-RayPick-Blacklist message: " + message);
             }
         }
     }

From 9ef03b120572575a06ddb10aeecbfb5677f07443 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth@highfidelity.io>
Date: Thu, 1 Sep 2016 19:13:08 -0700
Subject: [PATCH 44/44] avoid having auto-unequip code trigger during a
 teleport

---
 scripts/system/controllers/handControllerGrab.js | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/scripts/system/controllers/handControllerGrab.js b/scripts/system/controllers/handControllerGrab.js
index 5b35245e74..ab144dcbd9 100644
--- a/scripts/system/controllers/handControllerGrab.js
+++ b/scripts/system/controllers/handControllerGrab.js
@@ -708,6 +708,7 @@ var equipHotspotBuddy = new EquipHotspotBuddy();
 
 function MyController(hand) {
     this.hand = hand;
+    this.autoUnequipCounter = 0;
 
     // handPosition is where the avatar's hand appears to be, in-world.
     this.getHandPosition = function () {
@@ -2087,6 +2088,12 @@ function MyController(hand) {
                 var TEAR_AWAY_DISTANCE = 0.04;
                 var nearPickedCandidateEntities = Entities.findEntities(heldItemPosition, NEAR_GRAB_RADIUS + TEAR_AWAY_DISTANCE);
                 if (nearPickedCandidateEntities.indexOf(this.grabbedEntity) == -1) {
+                    this.autoUnequipCounter += 1;
+                } else {
+                    this.autoUnequipCounter = 0;
+                }
+
+                if (this.autoUnequipCounter > 1) {
                     // for whatever reason, the held/equipped entity has been pulled away.  ungrab or unequip.
                     print("handControllerGrab -- autoreleasing held or equipped item because it is far from hand." +
                         props.parentID + " " + vec3toStr(props.position));
@@ -2388,6 +2395,8 @@ function MyController(hand) {
     };
 
     this.activateEntity = function(entityID, grabbedProperties, wasLoaded) {
+        this.autoUnequipCounter = 0;
+
         if (this.entityActivated) {
             return;
         }