Remove frame lag from near grabbed physical objects.

In the game loop, physics occurs before avatar update.
Before this PR, when the avatar is moved during avatar update, near grabbed objects will not pick up this move until one frame later, when
the physics is run on the next update.

After this PR, near grabbed objects are adjusted to reflect any position or rotation change that occurred during the avatar update.

interface/src/avatar/AvatarActionHold.cpp

@@ -25,14 +25,25 @@ AvatarActionHold::AvatarActionHold(const QUuid& id, EntityItemPointer ownerEntit
 {
     _type = ACTION_TYPE_HOLD;
     _measuredLinearVelocities.resize(AvatarActionHold::velocitySmoothFrames);
+
+    auto myAvatar = DependencyManager::get<AvatarManager>()->getMyAvatar();
+    if (myAvatar) {
+        myAvatar->addHoldAction(this);
+    }
+
 #if WANT_DEBUG
-    qDebug() << "AvatarActionHold::AvatarActionHold";
+    qDebug() << "AvatarActionHold::AvatarActionHold" << (void*)this;
 #endif
 }
 
 AvatarActionHold::~AvatarActionHold() {
+    auto myAvatar = DependencyManager::get<AvatarManager>()->getMyAvatar();
+    if (myAvatar) {
+        myAvatar->removeHoldAction(this);
+    }
+
 #if WANT_DEBUG
-    qDebug() << "AvatarActionHold::~AvatarActionHold";
+    qDebug() << "AvatarActionHold::~AvatarActionHold" << (void*)this;
 #endif
 }
 
@@ -460,3 +471,40 @@ void AvatarActionHold::deserialize(QByteArray serializedArguments) {
 
     forceBodyNonStatic();
 }
+
+void AvatarActionHold::lateAvatarUpdate(const AnimPose& prePhysicsRoomPose, const AnimPose& postAvatarUpdateRoomPose) {
+    auto ownerEntity = _ownerEntity.lock();
+    if (!ownerEntity) {
+        return;
+    }
+    void* physicsInfo = ownerEntity->getPhysicsInfo();
+    if (!physicsInfo) {
+        return;
+    }
+    ObjectMotionState* motionState = static_cast<ObjectMotionState*>(physicsInfo);
+    btRigidBody* rigidBody = motionState ? motionState->getRigidBody() : nullptr;
+    if (!rigidBody) {
+        return;
+    }
+    auto avatarManager = DependencyManager::get<AvatarManager>();
+    auto holdingAvatar = std::static_pointer_cast<Avatar>(avatarManager->getAvatarBySessionID(_holderID));
+    if (!holdingAvatar || !holdingAvatar->isMyAvatar()) {
+        return;
+    }
+
+    btTransform worldTrans = rigidBody->getWorldTransform();
+    AnimPose worldBodyPose(glm::vec3(1), bulletToGLM(worldTrans.getRotation()), bulletToGLM(worldTrans.getOrigin()));
+
+    // transform the body transform into sensor space with the prePhysics sensor-to-world matrix.
+    // then transform it back into world uisng the postAvatarUpdate sensor-to-world matrix.
+    AnimPose newWorldBodyPose = postAvatarUpdateRoomPose * prePhysicsRoomPose.inverse() * worldBodyPose;
+
+    worldTrans.setOrigin(glmToBullet(newWorldBodyPose.trans));
+    worldTrans.setRotation(glmToBullet(newWorldBodyPose.rot));
+    rigidBody->setWorldTransform(worldTrans);
+
+    bool positionSuccess;
+    ownerEntity->setPosition(bulletToGLM(worldTrans.getOrigin()) + ObjectMotionState::getWorldOffset(), positionSuccess, false);
+    bool orientationSuccess;
+    ownerEntity->setOrientation(bulletToGLM(worldTrans.getRotation()), orientationSuccess, false);
+}

interface/src/avatar/AvatarActionHold.h

@@ -15,6 +15,7 @@
 #include <QUuid>
 
 #include <EntityItem.h>
+#include <AnimPose.h>
 #include <ObjectActionSpring.h>
 
 #include "avatar/MyAvatar.h"
@@ -41,6 +42,8 @@ public:
 
     virtual void prepareForPhysicsSimulation() override;
 
+    void lateAvatarUpdate(const AnimPose& prePhysicsRoomPose, const AnimPose& postAvatarUpdateRoomPose);
+
 private:
     void doKinematicUpdate(float deltaTimeStep);
 

interface/src/avatar/MyAvatar.cpp

@@ -45,6 +45,7 @@
 #include "Application.h"
 #include "devices/Faceshift.h"
 #include "AvatarManager.h"
+#include "AvatarActionHold.h"
 #include "Menu.h"
 #include "MyAvatar.h"
 #include "Physics.h"
@@ -1309,6 +1310,8 @@ void MyAvatar::prepareForPhysicsSimulation() {
     } else {
         _follow.deactivate();
     }
+
+    _prePhysicsRoomPose = AnimPose(_sensorToWorldMatrix);
 }
 
 void MyAvatar::harvestResultsFromPhysicsSimulation(float deltaTime) {
@@ -1549,8 +1552,11 @@ void MyAvatar::postUpdate(float deltaTime) {
 
     DebugDraw::getInstance().updateMyAvatarPos(getPosition());
     DebugDraw::getInstance().updateMyAvatarRot(getOrientation());
-}
 
+    AnimPose postUpdateRoomPose(_sensorToWorldMatrix);
+
+    updateHoldActions(_prePhysicsRoomPose, postUpdateRoomPose);
+}
 
 void MyAvatar::preDisplaySide(RenderArgs* renderArgs) {
 
@@ -2257,3 +2263,25 @@ glm::vec3 MyAvatar::getAbsoluteJointTranslationInObjectFrame(int index) const {
         }
     }
 }
+
+// thread-safe
+void MyAvatar::addHoldAction(AvatarActionHold* holdAction) {
+    std::lock_guard<std::mutex> guard(_holdActionsMutex);
+    _holdActions.push_back(holdAction);
+}
+
+// thread-safe
+void MyAvatar::removeHoldAction(AvatarActionHold* holdAction) {
+    std::lock_guard<std::mutex> guard(_holdActionsMutex);
+    auto iter = std::find(std::begin(_holdActions), std::end(_holdActions), holdAction);
+    if (iter != std::end(_holdActions)) {
+        _holdActions.erase(iter);
+    }
+}
+
+void MyAvatar::updateHoldActions(const AnimPose& prePhysicsPose, const AnimPose& postUpdatePose) {
+    std::lock_guard<std::mutex> guard(_holdActionsMutex);
+    for (auto& holdAction : _holdActions) {
+        holdAction->lateAvatarUpdate(prePhysicsPose, postUpdatePose);
+    }
+}

interface/src/avatar/MyAvatar.h

@@ -26,6 +26,7 @@
 #include "MyCharacterController.h"
 #include <ThreadSafeValueCache.h>
 
+class AvatarActionHold;
 class ModelItemID;
 
 enum DriveKeys {
@@ -277,6 +278,10 @@ public:
     virtual glm::quat getAbsoluteJointRotationInObjectFrame(int index) const override;
     virtual glm::vec3 getAbsoluteJointTranslationInObjectFrame(int index) const override;
 
+    void addHoldAction(AvatarActionHold* holdAction);  // thread-safe
+    void removeHoldAction(AvatarActionHold* holdAction);  // thread-safe
+    void updateHoldActions(const AnimPose& prePhysicsPose, const AnimPose& postUpdatePose);
+
 public slots:
     void increaseSize();
     void decreaseSize();
@@ -488,6 +493,10 @@ private:
 
     bool _hmdLeanRecenterEnabled = true;
 
+    AnimPose _prePhysicsRoomPose;
+    std::mutex _holdActionsMutex;
+    std::vector<AvatarActionHold*> _holdActions;
+
     float AVATAR_MOVEMENT_ENERGY_CONSTANT { 0.001f };
     float AUDIO_ENERGY_CONSTANT { 0.000001f };
     float MAX_AVATAR_MOVEMENT_PER_FRAME { 30.0f };