Merge pull request #8717 from hyperlogic/bug-fix/near-grab-lag-fix

Remove frame lag and jitter from near grabbed physical objects.

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,35 @@ 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) {
+    EntityTreeRenderer* entityTreeRenderer = qApp->getEntities();
+    EntityTreePointer entityTree = entityTreeRenderer ? entityTreeRenderer->getTree() : nullptr;
+    if (entityTree) {
+        // to prevent actions from adding or removing themselves from the _holdActions vector
+        // while we are iterating, we need to enter a critical section.
+        std::lock_guard<std::mutex> guard(_holdActionsMutex);
+
+        // lateAvatarUpdate will modify entity position & orientation, so we need an entity write lock
+        entityTree->withWriteLock([&] {
+            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 };