From d7affcf811a8e5a46d5f201336effbeeec1584e3 Mon Sep 17 00:00:00 2001
From: Seth Alves <seth.alves@gmail.com>
Date: Wed, 16 Dec 2015 15:41:27 -0800
Subject: [PATCH] keep track of palm rotation vs avatar as well as palm
 translation

---
 interface/src/avatar/AvatarActionHold.cpp | 49 +++++++++++++++++------
 interface/src/avatar/AvatarActionHold.h   |  3 +-
 2 files changed, 39 insertions(+), 13 deletions(-)

diff --git a/interface/src/avatar/AvatarActionHold.cpp b/interface/src/avatar/AvatarActionHold.cpp
index 160a437f44..5a4dbfa4d1 100644
--- a/interface/src/avatar/AvatarActionHold.cpp
+++ b/interface/src/avatar/AvatarActionHold.cpp
@@ -33,17 +33,15 @@ AvatarActionHold::~AvatarActionHold() {
 #endif
 }
 
-glm::vec3 AvatarActionHold::getAvatarRigidBodyPosition() {
+bool AvatarActionHold::getAvatarRigidBodyLocation(glm::vec3& avatarRigidBodyPosition, glm::quat& avatarRigidBodyRotation) {
     MyAvatar* myAvatar = DependencyManager::get<AvatarManager>()->getMyAvatar();
     MyCharacterController* controller = myAvatar ? myAvatar->getCharacterController() : nullptr;
     if (!controller) {
-        qDebug() << "AvatarActionHold::getAvatarRigidBodyPosition failed to get character controller";
-        return glm::vec3(0.0f);
+        qDebug() << "AvatarActionHold::getAvatarRigidBodyLocation failed to get character controller";
+        return false;
     }
-    glm::vec3 avatarRigidBodyPosition;
-    glm::quat avatarRigidBodyRotation;
     controller->getRigidBodyLocation(avatarRigidBodyPosition, avatarRigidBodyRotation);
-    return avatarRigidBodyPosition;
+    return true;
 }
 
 void AvatarActionHold::prepareForPhysicsSimulation() {
@@ -59,9 +57,28 @@ void AvatarActionHold::prepareForPhysicsSimulation() {
             return;
         }
         if (holdingAvatar->isMyAvatar()) {
-            glm::vec3 palmPosition = (_hand == "right") ?
-                holdingAvatar->getRightPalmPosition() : holdingAvatar->getLeftPalmPosition();
-            _palmOffsetFromRigidBody = palmPosition - getAvatarRigidBodyPosition();
+            glm::vec3 palmPosition;
+            glm::quat palmRotation;
+            if (_hand == "right") {
+                palmPosition = holdingAvatar->getRightPalmPosition();
+                palmRotation = holdingAvatar->getRightPalmRotation();
+            } else {
+                palmPosition = holdingAvatar->getLeftPalmPosition();
+                palmRotation = holdingAvatar->getLeftPalmRotation();
+            }
+
+            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
+            // between this call and the call to getTarget, below.  A call to get*PalmPosition in
+            // getTarget would get the palm position of the previous location of the avatar (because
+            // bullet has moved the av's rigid body but the rigid body's location has not yet been
+            // copied out into the Avatar class.
+            _palmOffsetFromRigidBody = palmPosition - avatarRigidBodyPosition;
+            _palmRotationFromRigidBody = glm::inverse(avatarRigidBodyRotation) * palmRotation;
         }
     });
 }
@@ -90,7 +107,17 @@ std::shared_ptr<Avatar> AvatarActionHold::getTarget(glm::quat& rotation, glm::ve
                 palmRotation = holdingAvatar->getHand()->getCopyOfPalmData(HandData::LeftHand).getRotation();
             }
         } else if (holdingAvatar->isMyAvatar()) {
-            palmPosition = getAvatarRigidBodyPosition() + _palmOffsetFromRigidBody;
+            glm::vec3 avatarRigidBodyPosition;
+            glm::quat avatarRigidBodyRotation;
+            getAvatarRigidBodyLocation(avatarRigidBodyPosition, avatarRigidBodyRotation);
+
+            // the offset and rotation between the avatar's rigid body and the palm were determined earlier
+            // in prepareForPhysicsSimulation.  At this point, the avatar's rigid body has been moved by bullet
+            // and the data in the Avatar class is stale.  This means that the result of get*PalmPosition will
+            // be stale.  Instead, determine the current palm position with the current avatar's rigid body
+            // location and the saved offsets.
+            palmPosition = avatarRigidBodyPosition + _palmOffsetFromRigidBody;
+            palmRotation = avatarRigidBodyRotation * _palmRotationFromRigidBody;
             if (isRightHand) {
                 palmRotation = holdingAvatar->getRightPalmRotation();
             } else {
@@ -199,8 +226,6 @@ void AvatarActionHold::doKinematicUpdate(float deltaTimeStep) {
 
         motionState->dirtyInternalKinematicChanges();
 
-        ownerEntity->setPosition(_positionalTarget);
-
         _previousPositionalTarget = _positionalTarget;
         _previousRotationalTarget = _rotationalTarget;
         _previousSet = true;
diff --git a/interface/src/avatar/AvatarActionHold.h b/interface/src/avatar/AvatarActionHold.h
index 36a9aa3049..fc8baf6dcc 100644
--- a/interface/src/avatar/AvatarActionHold.h
+++ b/interface/src/avatar/AvatarActionHold.h
@@ -35,7 +35,7 @@ public:
 
     virtual bool shouldSuppressLocationEdits() override { return _active && !_ownerEntity.expired(); }
 
-    glm::vec3 getAvatarRigidBodyPosition();
+    bool getAvatarRigidBodyLocation(glm::vec3& avatarRigidBodyPosition, glm::quat& avatarRigidBodyRotation);
     std::shared_ptr<Avatar> getTarget(glm::quat& rotation, glm::vec3& position);
 
     virtual void prepareForPhysicsSimulation() override;
@@ -61,6 +61,7 @@ private:
     glm::vec3 _previousPositionalDelta;
 
     glm::vec3 _palmOffsetFromRigidBody;
+    glm::quat _palmRotationFromRigidBody;
 };
 
 #endif // hifi_AvatarActionHold_h