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] 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