Merge pull request #6676 from sethalves/dejitter-hold-action

Dejitter hold action

interface/src/Application.cpp

@@ -2988,6 +2988,9 @@ void Application::update(float deltaTime) {
         _physicsEngine->changeObjects(motionStates);
 
         myAvatar->prepareForPhysicsSimulation();
+        _physicsEngine->forEachAction([&](EntityActionPointer action) {
+            action->prepareForPhysicsSimulation();
+        });
 
         getEntities()->getTree()->withWriteLock([&] {
             _physicsEngine->stepSimulation();

interface/src/avatar/AvatarActionHold.cpp

@@ -14,6 +14,7 @@
 #include <QVariantGLM.h>
 
 #include "avatar/AvatarManager.h"
+#include "CharacterController.h"
 
 const uint16_t AvatarActionHold::holdVersion = 1;
 
@@ -32,6 +33,64 @@ AvatarActionHold::~AvatarActionHold() {
 #endif
 }
 
+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::getAvatarRigidBodyLocation failed to get character controller";
+        return false;
+    }
+    controller->getRigidBodyLocation(avatarRigidBodyPosition, avatarRigidBodyRotation);
+    return true;
+}
+
+void AvatarActionHold::prepareForPhysicsSimulation() {
+    auto avatarManager = DependencyManager::get<AvatarManager>();
+    auto holdingAvatar = std::static_pointer_cast<Avatar>(avatarManager->getAvatarBySessionID(_holderID));
+
+    if (!holdingAvatar || !holdingAvatar->isMyAvatar()) {
+        return;
+    }
+
+    withWriteLock([&]{
+        if (_ignoreIK) {
+            return;
+        }
+
+        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.
+        glm::quat avatarRotationInverse = glm::inverse(avatarRigidBodyRotation);
+
+        // the offset should be in the frame of the avatar, but something about the order
+        // things are updated makes this wrong:
+        //   _palmOffsetFromRigidBody = avatarRotationInverse * (palmPosition - avatarRigidBodyPosition);
+        // I'll leave it here as a comment in case avatar handling changes.
+        _palmOffsetFromRigidBody = palmPosition - avatarRigidBodyPosition;
+
+        // rotation should also be needed, but again, the order of updates makes this unneeded.  leaving
+        // code here for future reference.
+        // _palmRotationFromRigidBody = avatarRotationInverse * palmRotation;
+    });
+}
+
 std::shared_ptr<Avatar> AvatarActionHold::getTarget(glm::quat& rotation, glm::vec3& position) {
     auto avatarManager = DependencyManager::get<AvatarManager>();
     auto holdingAvatar = std::static_pointer_cast<Avatar>(avatarManager->getAvatarBySessionID(_holderID));
@@ -40,11 +99,11 @@ std::shared_ptr<Avatar> AvatarActionHold::getTarget(glm::quat& rotation, glm::ve
         return holdingAvatar;
     }
 
-    withTryReadLock([&]{
+    withReadLock([&]{
         bool isRightHand = (_hand == "right");
         glm::vec3 palmPosition { Vectors::ZERO };
         glm::quat palmRotation { Quaternions::IDENTITY };
-            
+
         if (_ignoreIK && holdingAvatar->isMyAvatar()) {
             // 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.
@@ -55,6 +114,31 @@ std::shared_ptr<Avatar> AvatarActionHold::getTarget(glm::quat& rotation, glm::ve
                 palmPosition = holdingAvatar->getHand()->getCopyOfPalmData(HandData::LeftHand).getPosition();
                 palmRotation = holdingAvatar->getHand()->getCopyOfPalmData(HandData::LeftHand).getRotation();
             }
+        } else if (holdingAvatar->isMyAvatar()) {
+            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.
+
+            // this line is more correct but breaks for the current way avatar data is updated.
+            // palmPosition = avatarRigidBodyPosition + avatarRigidBodyRotation * _palmOffsetFromRigidBody;
+            // instead, use this for now:
+            palmPosition = avatarRigidBodyPosition + _palmOffsetFromRigidBody;
+
+            // the item jitters the least by getting the rotation based on the opinion of Avatar.h rather
+            // than that of the rigid body.  leaving this next line here for future reference:
+            // palmRotation = avatarRigidBodyRotation * _palmRotationFromRigidBody;
+
+            if (isRightHand) {
+                palmRotation = holdingAvatar->getRightPalmRotation();
+            } else {
+                palmRotation = holdingAvatar->getLeftPalmRotation();
+            }
         } else {
             if (isRightHand) {
                 palmPosition = holdingAvatar->getRightPalmPosition();
@@ -103,21 +187,19 @@ void AvatarActionHold::updateActionWorker(float deltaTimeStep) {
     if (valid && holdCount > 0) {
         position /= holdCount;
 
-        bool gotLock = withTryWriteLock([&]{
+        withWriteLock([&]{
             _positionalTarget = position;
             _rotationalTarget = rotation;
             _positionalTargetSet = true;
             _rotationalTargetSet = true;
             _active = true;
         });
-        if (gotLock) {
-            if (_kinematic) {
-                doKinematicUpdate(deltaTimeStep);
-            } else {
-                activateBody();
-                forceBodyNonStatic();
-                ObjectActionSpring::updateActionWorker(deltaTimeStep);
-            }
+        if (_kinematic) {
+            doKinematicUpdate(deltaTimeStep);
+        } else {
+            activateBody();
+            forceBodyNonStatic();
+            ObjectActionSpring::updateActionWorker(deltaTimeStep);
         }
     }
 }

interface/src/avatar/AvatarActionHold.h

@@ -25,18 +25,21 @@ public:
     AvatarActionHold(const QUuid& id, EntityItemPointer ownerEntity);
     virtual ~AvatarActionHold();
 
-    virtual bool updateArguments(QVariantMap arguments);
-    virtual QVariantMap getArguments();
+    virtual bool updateArguments(QVariantMap arguments) override;
+    virtual QVariantMap getArguments() override;
 
-    virtual void updateActionWorker(float deltaTimeStep);
+    virtual void updateActionWorker(float deltaTimeStep) override;
 
     QByteArray serialize() const;
-    virtual void deserialize(QByteArray serializedArguments);
+    virtual void deserialize(QByteArray serializedArguments) override;
 
-    virtual bool shouldSuppressLocationEdits() { return _active && !_ownerEntity.expired(); }
+    virtual bool shouldSuppressLocationEdits() override { return _active && !_ownerEntity.expired(); }
 
+    bool getAvatarRigidBodyLocation(glm::vec3& avatarRigidBodyPosition, glm::quat& avatarRigidBodyRotation);
     std::shared_ptr<Avatar> getTarget(glm::quat& rotation, glm::vec3& position);
 
+    virtual void prepareForPhysicsSimulation() override;
+
 private:
     void doKinematicUpdate(float deltaTimeStep);
 
@@ -56,6 +59,10 @@ private:
 
     float _previousDeltaTimeStep = 0.0f;
     glm::vec3 _previousPositionalDelta;
+
+    glm::vec3 _palmOffsetFromRigidBody;
+    // leaving this here for future refernece.
+    // glm::quat _palmRotationFromRigidBody;
 };
 
 #endif // hifi_AvatarActionHold_h

libraries/entities-renderer/src/RenderableModelEntityItem.cpp

@@ -625,3 +625,11 @@ glm::vec3 RenderableModelEntityItem::getAbsoluteJointTranslationInObjectFrame(in
     }
     return glm::vec3(0.0f);
 }
+
+void RenderableModelEntityItem::locationChanged() {
+    EntityItem::locationChanged();
+    if (_model && _model->isActive()) {
+        _model->setRotation(getRotation());
+        _model->setTranslation(getPosition());
+    }
+}

libraries/entities-renderer/src/RenderableModelEntityItem.h

@@ -73,6 +73,7 @@ public:
     virtual glm::vec3 getAbsoluteJointTranslationInObjectFrame(int index) const override;
 
     virtual void loader() override;
+    virtual void locationChanged() override;
 
 private:
     void remapTextures();

libraries/entities/src/EntityActionInterface.h

@@ -58,6 +58,8 @@ public:
 
     virtual bool shouldSuppressLocationEdits() { return false; }
 
+    virtual void prepareForPhysicsSimulation() { }
+
     // these look in the arguments map for a named argument.  if it's not found or isn't well formed,
     // ok will be set to false (note that it's never set to true -- set it to true before calling these).
     // if required is true, failure to extract an argument will cause a warning to be printed.

libraries/physics/src/CharacterController.cpp

@@ -15,6 +15,7 @@
 
 #include "BulletUtil.h"
 #include "PhysicsCollisionGroups.h"
+#include "ObjectMotionState.h"
 
 const btVector3 LOCAL_UP_AXIS(0.0f, 1.0f, 0.0f);
 const float JUMP_SPEED = 3.5f;
@@ -379,3 +380,15 @@ void CharacterController::preSimulation() {
 void CharacterController::postSimulation() {
     // postSimulation() exists for symmetry and just in case we need to do something here later
 }
+
+
+bool CharacterController::getRigidBodyLocation(glm::vec3& avatarRigidBodyPosition, glm::quat& avatarRigidBodyRotation) {
+    if (!_rigidBody) {
+        return false;
+    }
+
+    const btTransform& worldTrans = _rigidBody->getCenterOfMassTransform();
+    avatarRigidBodyPosition = bulletToGLM(worldTrans.getOrigin()) + ObjectMotionState::getWorldOffset();
+    avatarRigidBodyRotation = bulletToGLM(worldTrans.getRotation());
+    return true;
+}

libraries/physics/src/CharacterController.h

@@ -79,6 +79,8 @@ public:
     void setEnabled(bool enabled);
     bool isEnabled() const { return _enabled && _dynamicsWorld; }
 
+    bool getRigidBodyLocation(glm::vec3& avatarRigidBodyPosition, glm::quat& avatarRigidBodyRotation);
+
 protected:
     void updateUpAxis(const glm::quat& rotation);
 

libraries/physics/src/ObjectAction.h

@@ -29,12 +29,12 @@ public:
     ObjectAction(EntityActionType type, const QUuid& id, EntityItemPointer ownerEntity);
     virtual ~ObjectAction();
 
-    virtual void removeFromSimulation(EntitySimulation* simulation) const;
-    virtual EntityItemWeakPointer getOwnerEntity() const { return _ownerEntity; }
-    virtual void setOwnerEntity(const EntityItemPointer ownerEntity) { _ownerEntity = ownerEntity; }
+    virtual void removeFromSimulation(EntitySimulation* simulation) const override;
+    virtual EntityItemWeakPointer getOwnerEntity() const override { return _ownerEntity; }
+    virtual void setOwnerEntity(const EntityItemPointer ownerEntity) override { _ownerEntity = ownerEntity; }
 
-    virtual bool updateArguments(QVariantMap arguments);
-    virtual QVariantMap getArguments();
+    virtual bool updateArguments(QVariantMap arguments) override;
+    virtual QVariantMap getArguments() override;
 
     // this is called from updateAction and should be overridden by subclasses
     virtual void updateActionWorker(float deltaTimeStep) = 0;
@@ -43,25 +43,25 @@ public:
     virtual void updateAction(btCollisionWorld* collisionWorld, btScalar deltaTimeStep);
     virtual void debugDraw(btIDebugDraw* debugDrawer);
 
-    virtual QByteArray serialize() const = 0;
-    virtual void deserialize(QByteArray serializedArguments) = 0;
+    virtual QByteArray serialize() const override = 0;
+    virtual void deserialize(QByteArray serializedArguments) override = 0;
 
-    virtual bool lifetimeIsOver();
-    virtual quint64 getExpires() { return _expires; }
+    virtual bool lifetimeIsOver() override;
+    virtual quint64 getExpires() override { return _expires; }
 
 protected:
     quint64 localTimeToServerTime(quint64 timeValue) const;
     quint64 serverTimeToLocalTime(quint64 timeValue) const;
 
     virtual btRigidBody* getRigidBody();
-    virtual glm::vec3 getPosition();
-    virtual void setPosition(glm::vec3 position);
-    virtual glm::quat getRotation();
-    virtual void setRotation(glm::quat rotation);
-    virtual glm::vec3 getLinearVelocity();
-    virtual void setLinearVelocity(glm::vec3 linearVelocity);
-    virtual glm::vec3 getAngularVelocity();
-    virtual void setAngularVelocity(glm::vec3 angularVelocity);
+    virtual glm::vec3 getPosition() override;
+    virtual void setPosition(glm::vec3 position) override;
+    virtual glm::quat getRotation() override;
+    virtual void setRotation(glm::quat rotation) override;
+    virtual glm::vec3 getLinearVelocity() override;
+    virtual void setLinearVelocity(glm::vec3 linearVelocity) override;
+    virtual glm::vec3 getAngularVelocity() override;
+    virtual void setAngularVelocity(glm::vec3 angularVelocity) override;
     virtual void activateBody();
     virtual void forceBodyNonStatic();
 

libraries/physics/src/ObjectActionOffset.h

@@ -22,13 +22,13 @@ public:
     ObjectActionOffset(const QUuid& id, EntityItemPointer ownerEntity);
     virtual ~ObjectActionOffset();
 
-    virtual bool updateArguments(QVariantMap arguments);
-    virtual QVariantMap getArguments();
+    virtual bool updateArguments(QVariantMap arguments) override;
+    virtual QVariantMap getArguments() override;
 
-    virtual void updateActionWorker(float deltaTimeStep);
+    virtual void updateActionWorker(float deltaTimeStep) override;
 
-    virtual QByteArray serialize() const;
-    virtual void deserialize(QByteArray serializedArguments);
+    virtual QByteArray serialize() const override;
+    virtual void deserialize(QByteArray serializedArguments) override;
 
  private:
     static const uint16_t offsetVersion;

libraries/physics/src/ObjectActionSpring.h

@@ -19,13 +19,13 @@ public:
     ObjectActionSpring(const QUuid& id, EntityItemPointer ownerEntity);
     virtual ~ObjectActionSpring();
 
-    virtual bool updateArguments(QVariantMap arguments);
-    virtual QVariantMap getArguments();
+    virtual bool updateArguments(QVariantMap arguments) override;
+    virtual QVariantMap getArguments() override;
 
-    virtual void updateActionWorker(float deltaTimeStep);
+    virtual void updateActionWorker(float deltaTimeStep) override;
 
-    virtual QByteArray serialize() const;
-    virtual void deserialize(QByteArray serializedArguments);
+    virtual QByteArray serialize() const override;
+    virtual void deserialize(QByteArray serializedArguments) override;
 
 protected:
     static const uint16_t springVersion;

libraries/physics/src/PhysicsEngine.cpp

@@ -497,3 +497,11 @@ void PhysicsEngine::removeAction(const QUuid actionID) {
         _objectActions.remove(actionID);
     }
 }
+
+void PhysicsEngine::forEachAction(std::function<void(EntityActionPointer)> actor) {
+    QHashIterator<QUuid, EntityActionPointer> iter(_objectActions);
+    while (iter.hasNext()) {
+        iter.next();
+        actor(iter.value());
+    }
+}

libraries/physics/src/PhysicsEngine.h

@@ -97,6 +97,7 @@ public:
     EntityActionPointer getActionByID(const QUuid& actionID) const;
     void addAction(EntityActionPointer action);
     void removeAction(const QUuid actionID);
+    void forEachAction(std::function<void(EntityActionPointer)> actor);
 
 private:
     void removeContacts(ObjectMotionState* motionState);