diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index 7e88ea28dc..47d008aa69 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -3228,7 +3228,9 @@ void Application::update(float deltaTime) { controller::Pose leftHandPose = userInputMapper->getPoseState(controller::Action::LEFT_HAND); controller::Pose rightHandPose = userInputMapper->getPoseState(controller::Action::RIGHT_HAND); auto myAvatarMatrix = createMatFromQuatAndPos(myAvatar->getOrientation(), myAvatar->getPosition()); - myAvatar->setHandControllerPosesInWorldFrame(leftHandPose.transform(myAvatarMatrix), rightHandPose.transform(myAvatarMatrix)); + auto worldToSensorMatrix = glm::inverse(myAvatar->getSensorToWorldMatrix()); + auto avatarToSensorMatrix = worldToSensorMatrix * myAvatarMatrix; + myAvatar->setHandControllerPosesInSensorFrame(leftHandPose.transform(avatarToSensorMatrix), rightHandPose.transform(avatarToSensorMatrix)); updateThreads(deltaTime); // If running non-threaded, then give the threads some time to process... updateDialogs(deltaTime); // update various stats dialogs if present @@ -3315,6 +3317,9 @@ void Application::update(float deltaTime) { qApp->updateMyAvatarLookAtPosition(); + // update sensorToWorldMatrix for camera and hand controllers + myAvatar->updateSensorToWorldMatrix(); + avatarManager->updateMyAvatar(deltaTime); } @@ -3374,9 +3379,6 @@ void Application::update(float deltaTime) { QMetaObject::invokeMethod(DependencyManager::get().data(), "sendDownstreamAudioStatsPacket", Qt::QueuedConnection); } } - - // update sensorToWorldMatrix for rendering camera. - myAvatar->updateSensorToWorldMatrix(); } diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index 4405204b47..8f11c635e9 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -418,7 +418,7 @@ void MyAvatar::updateFromHMDSensorMatrix(const glm::mat4& hmdSensorMatrix) { _hmdSensorFacing = getFacingDir2D(_hmdSensorOrientation); } -// best called at end of main loop, just before rendering. +// best called at end of main loop, after physics. // update sensor to world matrix from current body position and hmd sensor. // This is so the correct camera can be used for rendering. void MyAvatar::updateSensorToWorldMatrix() { @@ -1087,24 +1087,32 @@ static controller::Pose applyLowVelocityFilter(const controller::Pose& oldPose, return finalPose; } -void MyAvatar::setHandControllerPosesInWorldFrame(const controller::Pose& left, const controller::Pose& right) { +void MyAvatar::setHandControllerPosesInSensorFrame(const controller::Pose& left, const controller::Pose& right) { if (controller::InputDevice::getLowVelocityFilter()) { - auto oldLeftPose = getLeftHandControllerPoseInWorldFrame(); - auto oldRightPose = getRightHandControllerPoseInWorldFrame(); - _leftHandControllerPoseInWorldFrameCache.set(applyLowVelocityFilter(oldLeftPose, left)); - _rightHandControllerPoseInWorldFrameCache.set(applyLowVelocityFilter(oldRightPose, right)); + auto oldLeftPose = getLeftHandControllerPoseInSensorFrame(); + auto oldRightPose = getRightHandControllerPoseInSensorFrame(); + _leftHandControllerPoseInSensorFrameCache.set(applyLowVelocityFilter(oldLeftPose, left)); + _rightHandControllerPoseInSensorFrameCache.set(applyLowVelocityFilter(oldRightPose, right)); } else { - _leftHandControllerPoseInWorldFrameCache.set(left); - _rightHandControllerPoseInWorldFrameCache.set(right); + _leftHandControllerPoseInSensorFrameCache.set(left); + _rightHandControllerPoseInSensorFrameCache.set(right); } } +controller::Pose MyAvatar::getLeftHandControllerPoseInSensorFrame() const { + return _leftHandControllerPoseInSensorFrameCache.get(); +} + +controller::Pose MyAvatar::getRightHandControllerPoseInSensorFrame() const { + return _rightHandControllerPoseInSensorFrameCache.get(); +} + controller::Pose MyAvatar::getLeftHandControllerPoseInWorldFrame() const { - return _leftHandControllerPoseInWorldFrameCache.get(); + return _leftHandControllerPoseInSensorFrameCache.get().transform(getSensorToWorldMatrix()); } controller::Pose MyAvatar::getRightHandControllerPoseInWorldFrame() const { - return _rightHandControllerPoseInWorldFrameCache.get(); + return _rightHandControllerPoseInSensorFrameCache.get().transform(getSensorToWorldMatrix()); } controller::Pose MyAvatar::getLeftHandControllerPoseInAvatarFrame() const { diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 3554d9b8bc..92bf9e7614 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -247,7 +247,9 @@ public: virtual void rebuildCollisionShape() override; - void setHandControllerPosesInWorldFrame(const controller::Pose& left, const controller::Pose& right); + void setHandControllerPosesInSensorFrame(const controller::Pose& left, const controller::Pose& right); + controller::Pose getLeftHandControllerPoseInSensorFrame() const; + controller::Pose getRightHandControllerPoseInSensorFrame() const; controller::Pose getLeftHandControllerPoseInWorldFrame() const; controller::Pose getRightHandControllerPoseInWorldFrame() const; controller::Pose getLeftHandControllerPoseInAvatarFrame() const; @@ -451,9 +453,9 @@ private: bool _hoverReferenceCameraFacingIsCaptured { false }; glm::vec3 _hoverReferenceCameraFacing { 0.0f, 0.0f, -1.0f }; // hmd sensor space - // These are stored in WORLD frame - ThreadSafeValueCache _leftHandControllerPoseInWorldFrameCache { controller::Pose() }; - ThreadSafeValueCache _rightHandControllerPoseInWorldFrameCache { controller::Pose() }; + // These are stored in SENSOR frame + ThreadSafeValueCache _leftHandControllerPoseInSensorFrameCache { controller::Pose() }; + ThreadSafeValueCache _rightHandControllerPoseInSensorFrameCache { controller::Pose() }; float AVATAR_MOVEMENT_ENERGY_CONSTANT { 0.001f }; float AUDIO_ENERGY_CONSTANT { 0.000001f };