diff --git a/interface/resources/controllers/standard.json b/interface/resources/controllers/standard.json index 53285ea974..62eec9bc3c 100644 --- a/interface/resources/controllers/standard.json +++ b/interface/resources/controllers/standard.json @@ -61,6 +61,11 @@ { "from": "Standard.RightHand", "to": "Actions.RightHand" }, { "from": "Standard.LeftFoot", "to": "Actions.LeftFoot" }, - { "from": "Standard.RightFoot", "to": "Actions.RightFoot" } + { "from": "Standard.RightFoot", "to": "Actions.RightFoot" }, + + { "from": "Standard.Hips", "to": "Actions.Hips" }, + { "from": "Standard.Spine2", "to": "Actions.Spine2" }, + + { "from": "Standard.Head", "to": "Actions.Head" } ] } diff --git a/interface/src/Application.cpp b/interface/src/Application.cpp index c6cd185034..5b014f7009 100644 --- a/interface/src/Application.cpp +++ b/interface/src/Application.cpp @@ -4395,6 +4395,13 @@ void Application::update(float deltaTime) { controller::Pose rightFootPose = userInputMapper->getPoseState(controller::Action::RIGHT_FOOT); myAvatar->setFootControllerPosesInSensorFrame(leftFootPose.transform(avatarToSensorMatrix), rightFootPose.transform(avatarToSensorMatrix)); + controller::Pose hipsPose = userInputMapper->getPoseState(controller::Action::HIPS); + controller::Pose spine2Pose = userInputMapper->getPoseState(controller::Action::SPINE2); + myAvatar->setSpineControllerPosesInSensorFrame(hipsPose.transform(avatarToSensorMatrix), spine2Pose.transform(avatarToSensorMatrix)); + + controller::Pose headPose = userInputMapper->getPoseState(controller::Action::HEAD); + myAvatar->setHeadControllerPoseInSensorFrame(headPose.transform(avatarToSensorMatrix)); + updateThreads(deltaTime); // If running non-threaded, then give the threads some time to process... updateDialogs(deltaTime); // update various stats dialogs if present diff --git a/interface/src/avatar/MyAvatar.cpp b/interface/src/avatar/MyAvatar.cpp index da09bfba7e..0906dc7f7f 100644 --- a/interface/src/avatar/MyAvatar.cpp +++ b/interface/src/avatar/MyAvatar.cpp @@ -642,6 +642,7 @@ void MyAvatar::updateSensorToWorldMatrix() { updateJointFromController(controller::Action::RIGHT_HAND, _controllerRightHandMatrixCache); } + // Update avatar head rotation with sensor data void MyAvatar::updateFromTrackers(float deltaTime) { glm::vec3 estimatedPosition, estimatedRotation; @@ -660,8 +661,9 @@ void MyAvatar::updateFromTrackers(float deltaTime) { estimatedPosition.x *= -1.0f; _trackedHeadPosition = estimatedPosition; - const float OCULUS_LEAN_SCALE = 0.05f; - estimatedPosition /= OCULUS_LEAN_SCALE; + // wut + // const float OCULUS_LEAN_SCALE = 0.05f; + // estimatedPosition /= OCULUS_LEAN_SCALE; } else if (inFacetracker) { estimatedPosition = tracker->getHeadTranslation(); _trackedHeadPosition = estimatedPosition; @@ -1384,6 +1386,68 @@ controller::Pose MyAvatar::getRightFootControllerPoseInAvatarFrame() const { return getRightFootControllerPoseInWorldFrame().transform(invAvatarMatrix); } +void MyAvatar::setSpineControllerPosesInSensorFrame(const controller::Pose& hips, const controller::Pose& spine2) { + if (controller::InputDevice::getLowVelocityFilter()) { + auto oldHipsPose = getHipsControllerPoseInSensorFrame(); + auto oldSpine2Pose = getSpine2ControllerPoseInSensorFrame(); + _hipsControllerPoseInSensorFrameCache.set(applyLowVelocityFilter(oldHipsPose, hips)); + _spine2ControllerPoseInSensorFrameCache.set(applyLowVelocityFilter(oldSpine2Pose, spine2)); + } else { + _hipsControllerPoseInSensorFrameCache.set(hips); + _spine2ControllerPoseInSensorFrameCache.set(spine2); + } +} + +controller::Pose MyAvatar::getHipsControllerPoseInSensorFrame() const { + return _hipsControllerPoseInSensorFrameCache.get(); +} + +controller::Pose MyAvatar::getSpine2ControllerPoseInSensorFrame() const { + return _spine2ControllerPoseInSensorFrameCache.get(); +} + +controller::Pose MyAvatar::getHipsControllerPoseInWorldFrame() const { + return _hipsControllerPoseInSensorFrameCache.get().transform(getSensorToWorldMatrix()); +} + +controller::Pose MyAvatar::getSpine2ControllerPoseInWorldFrame() const { + return _spine2ControllerPoseInSensorFrameCache.get().transform(getSensorToWorldMatrix()); +} + +controller::Pose MyAvatar::getHipsControllerPoseInAvatarFrame() const { + glm::mat4 invAvatarMatrix = glm::inverse(createMatFromQuatAndPos(getOrientation(), getPosition())); + return getHipsControllerPoseInWorldFrame().transform(invAvatarMatrix); +} + +controller::Pose MyAvatar::getSpine2ControllerPoseInAvatarFrame() const { + glm::mat4 invAvatarMatrix = glm::inverse(createMatFromQuatAndPos(getOrientation(), getPosition())); + return getSpine2ControllerPoseInWorldFrame().transform(invAvatarMatrix); +} + +void MyAvatar::setHeadControllerPoseInSensorFrame(const controller::Pose& headPose) { + bool inHmd = qApp->isHMDMode(); + Head* head = getHead(); + if (inHmd) { + _headControllerPoseInSensorFrameCache.set(headPose); + _trackedHeadPosition = headPose.translation; + head->setDeltaPitch(headPose.rotation.x); + head->setDeltaYaw(headPose.rotation.y); + head->setDeltaRoll(headPose.rotation.z); + } +} + +controller::Pose MyAvatar::getHeadControllerPoseInSensorFrame() const { + return _headControllerPoseInSensorFrameCache.get(); +} + +controller::Pose MyAvatar::getHeadControllerPoseInWorldFrame() const { + return _headControllerPoseInSensorFrameCache.get().transform(getSensorToWorldMatrix()); +} + +controller::Pose MyAvatar::getHeadControllerPoseInAvatarFrame() const { + glm::mat4 invAvatarMatrix = glm::inverse(createMatFromQuatAndPos(getOrientation(), getPosition())); + return getHeadControllerPoseInWorldFrame().transform(invAvatarMatrix); +} void MyAvatar::updateMotors() { _characterController.clearMotors(); diff --git a/interface/src/avatar/MyAvatar.h b/interface/src/avatar/MyAvatar.h index 0146bd11a4..01b4dc824b 100644 --- a/interface/src/avatar/MyAvatar.h +++ b/interface/src/avatar/MyAvatar.h @@ -453,6 +453,19 @@ public: controller::Pose getLeftFootControllerPoseInAvatarFrame() const; controller::Pose getRightFootControllerPoseInAvatarFrame() const; + void setSpineControllerPosesInSensorFrame(const controller::Pose& hips, const controller::Pose& spine2); + controller::Pose getHipsControllerPoseInSensorFrame() const; + controller::Pose getSpine2ControllerPoseInSensorFrame() const; + controller::Pose getHipsControllerPoseInWorldFrame() const; + controller::Pose getSpine2ControllerPoseInWorldFrame() const; + controller::Pose getHipsControllerPoseInAvatarFrame() const; + controller::Pose getSpine2ControllerPoseInAvatarFrame() const; + + void setHeadControllerPoseInSensorFrame(const controller::Pose& headPose); + controller::Pose getHeadControllerPoseInSensorFrame() const; + controller::Pose getHeadControllerPoseInWorldFrame() const; + controller::Pose getHeadControllerPoseInAvatarFrame() const; + bool hasDriveInput() const; Q_INVOKABLE void setCharacterControllerEnabled(bool enabled); @@ -693,9 +706,11 @@ private: // These are stored in SENSOR frame ThreadSafeValueCache _leftHandControllerPoseInSensorFrameCache { controller::Pose() }; ThreadSafeValueCache _rightHandControllerPoseInSensorFrameCache { controller::Pose() }; - ThreadSafeValueCache _leftFootControllerPoseInSensorFrameCache{ controller::Pose() }; ThreadSafeValueCache _rightFootControllerPoseInSensorFrameCache{ controller::Pose() }; + ThreadSafeValueCache _hipsControllerPoseInSensorFrameCache{ controller::Pose() }; + ThreadSafeValueCache _spine2ControllerPoseInSensorFrameCache{ controller::Pose() }; + ThreadSafeValueCache _headControllerPoseInSensorFrameCache{ controller::Pose() }; bool _hmdLeanRecenterEnabled = true; diff --git a/libraries/controllers/src/controllers/Actions.cpp b/libraries/controllers/src/controllers/Actions.cpp index 300fa684d9..62a10c851f 100644 --- a/libraries/controllers/src/controllers/Actions.cpp +++ b/libraries/controllers/src/controllers/Actions.cpp @@ -53,6 +53,9 @@ namespace controller { makePosePair(Action::RIGHT_HAND, "RightHand"), makePosePair(Action::LEFT_FOOT, "LeftFoot"), makePosePair(Action::RIGHT_FOOT, "RightFoot"), + makePosePair(Action::HIPS, "Hips"), + makePosePair(Action::SPINE2, "Spine2"), + makePosePair(Action::HEAD, "Head"), makeButtonPair(Action::LEFT_HAND_CLICK, "LeftHandClick"), makeButtonPair(Action::RIGHT_HAND_CLICK, "RightHandClick"), diff --git a/libraries/controllers/src/controllers/Actions.h b/libraries/controllers/src/controllers/Actions.h index edf3dee07a..534f01d865 100644 --- a/libraries/controllers/src/controllers/Actions.h +++ b/libraries/controllers/src/controllers/Actions.h @@ -44,6 +44,9 @@ enum class Action { RIGHT_HAND, LEFT_FOOT, RIGHT_FOOT, + HIPS, + SPINE2, + HEAD, LEFT_HAND_CLICK, RIGHT_HAND_CLICK,