Avatar: Fix for one frame lag in hold action

AvatarActionHold now uses the most recent un-cached palm positions/orientations
instead of the cached version from the previous frame.

interface/src/avatar/Avatar.cpp

@@ -1160,25 +1160,59 @@ void Avatar::rebuildCollisionShape() {
 }
 
 // thread-safe
-glm::vec3 Avatar::getLeftPalmPosition() {
+glm::vec3 Avatar::getLeftPalmPosition() const {
     return _leftPalmPositionCache.get();
 }
 
 // thread-safe
-glm::quat Avatar::getLeftPalmRotation() {
+glm::quat Avatar::getLeftPalmRotation() const {
     return _leftPalmRotationCache.get();
 }
 
 // thread-safe
-glm::vec3 Avatar::getRightPalmPosition() {
+glm::vec3 Avatar::getRightPalmPosition() const {
     return _rightPalmPositionCache.get();
 }
 
 // thread-safe
-glm::quat Avatar::getRightPalmRotation() {
+glm::quat Avatar::getRightPalmRotation() const {
     return _rightPalmRotationCache.get();
 }
 
+glm::vec3 Avatar::getUncachedLeftPalmPosition() const {
+    assert(QThread::currentThread() == thread());  // main thread access only
+    glm::quat leftPalmRotation;
+    getSkeletonModel().getJointRotationInWorldFrame(getSkeletonModel().getLeftHandJointIndex(), leftPalmRotation);
+    glm::vec3 leftPalmPosition;
+    getSkeletonModel().getLeftHandPosition(leftPalmPosition);
+    leftPalmPosition += HAND_TO_PALM_OFFSET * glm::inverse(leftPalmRotation);
+    return leftPalmPosition;
+}
+
+glm::quat Avatar::getUncachedLeftPalmRotation() const {
+    assert(QThread::currentThread() == thread());  // main thread access only
+    glm::quat leftPalmRotation;
+    getSkeletonModel().getJointRotationInWorldFrame(getSkeletonModel().getLeftHandJointIndex(), leftPalmRotation);
+    return leftPalmRotation;
+}
+
+glm::vec3 Avatar::getUncachedRightPalmPosition() const {
+    assert(QThread::currentThread() == thread());  // main thread access only
+    glm::quat rightPalmRotation;
+    getSkeletonModel().getJointRotationInWorldFrame(getSkeletonModel().getRightHandJointIndex(), rightPalmRotation);
+    glm::vec3 rightPalmPosition;
+    getSkeletonModel().getRightHandPosition(rightPalmPosition);
+    rightPalmPosition += HAND_TO_PALM_OFFSET * glm::inverse(rightPalmRotation);
+    return rightPalmPosition;
+}
+
+glm::quat Avatar::getUncachedRightPalmRotation() const {
+    assert(QThread::currentThread() == thread());  // main thread access only
+    glm::quat rightPalmRotation;
+    getSkeletonModel().getJointRotationInWorldFrame(getSkeletonModel().getRightHandJointIndex(), rightPalmRotation);
+    return rightPalmRotation;
+}
+
 void Avatar::setPosition(const glm::vec3& position) {
     AvatarData::setPosition(position);
     updateAttitude();

interface/src/avatar/Avatar.h

@@ -167,13 +167,20 @@ public:
     using SpatiallyNestable::setOrientation;
     virtual void setOrientation(const glm::quat& orientation) override;
 
+    // NOT thread safe, must be called on main thread.
+    glm::vec3 getUncachedLeftPalmPosition() const;
+    glm::quat getUncachedLeftPalmRotation() const;
+    glm::vec3 getUncachedRightPalmPosition() const;
+    glm::quat getUncachedRightPalmRotation() const;
+
 public slots:
 
     // FIXME - these should be migrated to use Pose data instead
-    glm::vec3 getLeftPalmPosition();
+    // thread safe, will return last valid palm from cache
-    glm::quat getLeftPalmRotation();
+    glm::vec3 getLeftPalmPosition() const;
-    glm::vec3 getRightPalmPosition();
+    glm::quat getLeftPalmRotation() const;
-    glm::quat getRightPalmRotation();
+    glm::vec3 getRightPalmPosition() const;
+    glm::quat getRightPalmRotation() const;
 
 protected:
     friend class AvatarManager;

interface/src/avatar/AvatarActionHold.cpp

@@ -60,11 +60,11 @@ void AvatarActionHold::prepareForPhysicsSimulation() {
         glm::vec3 palmPosition;
         glm::quat palmRotation;
         if (_hand == "right") {
-            palmPosition = holdingAvatar->getRightPalmPosition();
+            palmPosition = holdingAvatar->getUncachedRightPalmPosition();
-            palmRotation = holdingAvatar->getRightPalmRotation();
+            palmRotation = holdingAvatar->getUncachedRightPalmRotation();
         } else {
-            palmPosition = holdingAvatar->getLeftPalmPosition();
+            palmPosition = holdingAvatar->getUncachedLeftPalmPosition();
-            palmRotation = holdingAvatar->getLeftPalmRotation();
+            palmRotation = holdingAvatar->getUncachedLeftPalmRotation();
         }
 
         glm::vec3 avatarRigidBodyPosition;