call a method on actions before each physics simulation step. use this to attempt to dejitter held objects

interface/src/Application.cpp

@@ -2989,6 +2989,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,57 @@ AvatarActionHold::~AvatarActionHold() {
 #endif
 }
 
+void AvatarActionHold::prepareForPhysicsSimulation() {
+    auto avatarManager = DependencyManager::get<AvatarManager>();
+    auto holdingAvatar = std::static_pointer_cast<Avatar>(avatarManager->getAvatarBySessionID(_holderID));
+
+    if (!holdingAvatar) {
+        return;
+    }
+
+    withTryReadLock([&]{
+        bool isRightHand = (_hand == "right");
+
+        if (_ignoreIK && holdingAvatar->isMyAvatar()) {
+            return;
+        }
+
+        if (holdingAvatar->isMyAvatar()) {
+            glm::vec3 palmPosition { Vectors::ZERO };
+            glm::quat palmRotation { Quaternions::IDENTITY };
+            if (isRightHand) {
+                palmPosition = holdingAvatar->getHand()->getCopyOfPalmData(HandData::RightHand).getPosition();
+                palmRotation = holdingAvatar->getHand()->getCopyOfPalmData(HandData::RightHand).getRotation();
+            } else {
+                palmPosition = holdingAvatar->getHand()->getCopyOfPalmData(HandData::LeftHand).getPosition();
+                palmRotation = holdingAvatar->getHand()->getCopyOfPalmData(HandData::LeftHand).getRotation();
+            }
+
+            // XXX dup code
+            MyAvatar* myAvatar = DependencyManager::get<AvatarManager>()->getMyAvatar();
+            MyCharacterController* controller = myAvatar ? myAvatar->getCharacterController() : nullptr;
+            if (!controller) {
+                qDebug() << "AvatarActionHold::prepareForPhysicsSimulation failed to get character controller";
+                return;
+            }
+            glm::vec3 avatarRigidBodyPosition;
+            glm::quat avatarRigidBodyRotation;
+            controller->getRigidBodyLocation(avatarRigidBodyPosition, avatarRigidBodyRotation);
+
+
+            if (isRightHand) {
+                palmPosition = holdingAvatar->getRightPalmPosition();
+                palmRotation = holdingAvatar->getRightPalmRotation();
+            } else {
+                palmPosition = holdingAvatar->getLeftPalmPosition();
+                palmRotation = holdingAvatar->getLeftPalmRotation();
+            }
+
+            _palmOffsetFromRigidBody = palmPosition - avatarRigidBodyPosition;
+        }
+    });
+}
+
 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));
@@ -44,7 +96,7 @@ std::shared_ptr<Avatar> AvatarActionHold::getTarget(glm::quat& rotation, glm::ve
         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 +107,22 @@ 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()) {
+            // XXX dup code
+            MyAvatar* myAvatar = DependencyManager::get<AvatarManager>()->getMyAvatar();
+            MyCharacterController* controller = myAvatar ? myAvatar->getCharacterController() : nullptr;
+            if (!controller) {
+                qDebug() << "AvatarActionHold::prepareForPhysicsSimulation failed to get character controller";
+                return;
+            }
+            glm::vec3 avatarRigidBodyPosition;
+            glm::quat avatarRigidBodyRotation;
+            controller->getRigidBodyLocation(avatarRigidBodyPosition, avatarRigidBodyRotation);
+
+
+
+            palmPosition = avatarRigidBodyPosition + _palmOffsetFromRigidBody;
+            palmRotation = holdingAvatar->getRightPalmRotation(); // XXX
         } else {
             if (isRightHand) {
                 palmPosition = holdingAvatar->getRightPalmPosition();

interface/src/avatar/AvatarActionHold.h

@@ -25,18 +25,20 @@ 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(); }
 
     std::shared_ptr<Avatar> getTarget(glm::quat& rotation, glm::vec3& position);
 
+    virtual void prepareForPhysicsSimulation() override;
+
 private:
     void doKinematicUpdate(float deltaTimeStep);
 
@@ -56,6 +58,8 @@ private:
 
     float _previousDeltaTimeStep = 0.0f;
     glm::vec3 _previousPositionalDelta;
+
+    glm::vec3 _palmOffsetFromRigidBody;
 };
 
 #endif // hifi_AvatarActionHold_h

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);