diff --git a/interface/src/avatar/AvatarActionHold.cpp b/interface/src/avatar/AvatarActionHold.cpp index 8b8f8e8c2e..5f41bf40d4 100644 --- a/interface/src/avatar/AvatarActionHold.cpp +++ b/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()->getMyAvatar(); + if (myAvatar) { + myAvatar->addHoldAction(this); + } + #if WANT_DEBUG - qDebug() << "AvatarActionHold::AvatarActionHold"; + qDebug() << "AvatarActionHold::AvatarActionHold" << (void*)this; #endif } AvatarActionHold::~AvatarActionHold() { + auto myAvatar = DependencyManager::get()->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(physicsInfo); + btRigidBody* rigidBody = motionState ? motionState->getRigidBody() : nullptr; + if (!rigidBody) { + return; + } + auto avatarManager = DependencyManager::get(); + auto holdingAvatar = std::static_pointer_cast(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); +} diff --git a/interface/src/avatar/AvatarActionHold.h b/interface/src/avatar/AvatarActionHold.h index bfa392172d..f0b42111ed 100644 --- a/interface/src/avatar/AvatarActionHold.h +++ b/interface/src/avatar/AvatarActionHold.h @@ -15,6 +15,7 @@ #include #include +#include #include #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); diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 24dbc62318..20c4f41568 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/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 guard(_holdActionsMutex); + _holdActions.push_back(holdAction); +} + +// thread-safe +void MyAvatar::removeHoldAction(AvatarActionHold* holdAction) { + std::lock_guard 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 guard(_holdActionsMutex); + for (auto& holdAction : _holdActions) { + holdAction->lateAvatarUpdate(prePhysicsPose, postUpdatePose); + } +} diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index c4ffc08cbc..71f185c6ed 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -26,6 +26,7 @@ #include "MyCharacterController.h" #include +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 _holdActions; + float AVATAR_MOVEMENT_ENERGY_CONSTANT { 0.001f }; float AUDIO_ENERGY_CONSTANT { 0.000001f }; float MAX_AVATAR_MOVEMENT_PER_FRAME { 30.0f };