From 0ae87feb96bd333c73c2cbb293609ae51a0f8209 Mon Sep 17 00:00:00 2001
From: Stefano Dafarra <stefano.dafarra@gmail.com>
Date: Wed, 15 Nov 2023 16:17:39 +0100
Subject: [PATCH] Added possibility to have the GUI following the eyes (#42)

* Added possibility to have the GUI following the eyes

* Bump version for release
---
 CMakeLists.txt                              |  2 +-
 src/devices/openxrheadset/OpenXrHeadset.cpp | 21 +++++++++++++++++++++
 src/devices/openxrheadset/OpenXrHeadset.h   |  3 ++-
 3 files changed, 24 insertions(+), 2 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 60b2856..d501183 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -8,7 +8,7 @@ cmake_minimum_required(VERSION 3.12)
 
 project(yarp-device-openxrheadset
         LANGUAGES C CXX
-        VERSION 0.0.2)
+        VERSION 0.0.3)
 
 # Defines the CMAKE_INSTALL_LIBDIR, CMAKE_INSTALL_BINDIR and many other useful macros.
 # See https://cmake.org/cmake/help/latest/module/GNUInstallDirs.html
diff --git a/src/devices/openxrheadset/OpenXrHeadset.cpp b/src/devices/openxrheadset/OpenXrHeadset.cpp
index ddfa21d..972143d 100644
--- a/src/devices/openxrheadset/OpenXrHeadset.cpp
+++ b/src/devices/openxrheadset/OpenXrHeadset.cpp
@@ -140,6 +140,7 @@ bool yarp::dev::OpenXrHeadset::open(yarp::os::Searchable &cfg)
                 m_huds.back().z       = -std::max(0.01, std::abs(guip.find("z").asFloat64())); //make sure that z is negative and that is at least 0.01 in modulus
                 std::transform(groupName.begin(), groupName.end(), groupName.begin(), ::tolower);
                 m_huds.back().portName = m_prefix + "/" + guip.check("port_id", yarp::os::Value(groupName)).toString();
+                m_huds.back().followEyes = guip.check("follow_eyes") && (guip.find("follow_eyes").isNull() || guip.find("follow_eyes").asBool());
                 m_huds.back().visibility = visibility;
             }
         }
@@ -523,6 +524,26 @@ void yarp::dev::OpenXrHeadset::run()
 
         for (GuiParam& gui : m_huds)
         {
+            if (gui.followEyes)
+            {
+                Eigen::Quaternionf newRotation;
+                if (gui.visibility == IOpenXrQuadLayer::Visibility::LEFT_EYE)
+                {
+                    newRotation = m_eyesManager.getLeftEyeDesiredRotation();
+                }
+                else if (gui.visibility == IOpenXrQuadLayer::Visibility::RIGHT_EYE)
+                {
+                    newRotation = m_eyesManager.getRightEyeDesiredRotation();
+                }
+                else
+                {
+                    newRotation = averageRotation;
+                }
+
+                Eigen::Vector3f guiPosition = { gui.x, gui.y, gui.z };
+                gui.layer.setPose(newRotation * guiPosition, newRotation);
+            }
+
             if (!gui.layer.updateTexture()) {
                 yCError(OPENXRHEADSET) << "Failed to update" << gui.portName << "display texture.";
             }
diff --git a/src/devices/openxrheadset/OpenXrHeadset.h b/src/devices/openxrheadset/OpenXrHeadset.h
index e5a352e..4e73a98 100644
--- a/src/devices/openxrheadset/OpenXrHeadset.h
+++ b/src/devices/openxrheadset/OpenXrHeadset.h
@@ -264,7 +264,8 @@ class yarp::dev::OpenXrHeadset : public yarp::dev::DeviceDriver,
         float         x;
         float         y;
         float         z;
-        std::string    portName;
+        std::string   portName;
+        bool          followEyes;
         IOpenXrQuadLayer::Visibility visibility;
         ImagePortToQuadLayer<yarp::sig::ImageOf<yarp::sig::PixelRgba>> layer;
     };