1 files changed, 116 insertions, 0 deletions
diff --git a/tracker-eyeware-beam/eyeware_beam.cpp b/tracker-eyeware-beam/eyeware_beam.cpp
new file mode 100644
index 00000000..f48b5d4d
--- /dev/null
+++ b/tracker-eyeware-beam/eyeware_beam.cpp
@@ -0,0 +1,116 @@
+/* Copyright (c) 2023 Eyeware Tech SA https://www.eyeware.tech
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ */
+
+#include "eyeware_beam.h"
+
+#include <QMutexLocker>
+
+static constexpr double rad_to_deg = 180.0 / M_PI;
+static constexpr double m_to_cm = 100.0;
+static constexpr double epsilon = 0.000001;
+
+eyeware_beam_tracker::eyeware_beam_tracker()
+{
+}
+
+eyeware_beam_tracker::~eyeware_beam_tracker()
+{
+    QMutexLocker lck(&mtx);
+    release_tracker_instance(tracker_client);
+    tracker_client = nullptr;
+}
+
+module_status eyeware_beam_tracker::start_tracker(QFrame* videoframe)
+{
+    QMutexLocker lck(&mtx);
+    try
+    {
+        tracker_client = create_tracker_instance("127.0.0.1", 12010);
+    }
+    catch (...)
+    {
+        return error("Eyeware Beam initialization has failed");
+    }
+
+    return status_ok();
+}
+
+void eyeware_beam_tracker::extract_translation(const eyeware::Vector3D& t,
+                                               double& translation_x_cm,
+                                               double& translation_y_cm,
+                                               double& translation_z_cm)
+{
+    translation_x_cm = +t.x * m_to_cm;
+    translation_y_cm = -t.y * m_to_cm;
+    translation_z_cm = +t.z * m_to_cm;
+}
+
+void eyeware_beam_tracker::extract_rotation_angles(const eyeware::Matrix3x3& R,
+                                                   double& pitch_deg,
+                                                   double& roll_deg,
+                                                   double& yaw_deg)
+{
+    double r00 = static_cast<double>(R[0][0]);
+    double r01 = static_cast<double>(R[0][1]);
+    double r02 = static_cast<double>(R[0][2]);
+    double r10 = static_cast<double>(R[1][0]);
+    double r11 = static_cast<double>(R[1][1]);
+    double r12 = static_cast<double>(R[1][2]);
+    double r20 = static_cast<double>(R[2][0]);
+    double r21 = static_cast<double>(R[2][1]);
+    double r22 = static_cast<double>(R[2][2]);
+
+    double dy = std::sqrt(r00 * r00 + r10 * r10);
+    last_yaw_deg = -std::atan2(-r20, dy) * rad_to_deg;
+    last_roll_deg = 0.0;
+    if (dy > epsilon)
+    {
+        last_pitch_deg = -std::atan2(r21, r22) * rad_to_deg;
+        last_roll_deg = +std::atan2(r10, r00) * rad_to_deg;
+    }
+    else
+    {
+        last_pitch_deg = -std::atan2(-r12, r11) * rad_to_deg;
+    }
+}
+
+void eyeware_beam_tracker::data(double *data)
+{
+     QMutexLocker lck(&mtx);
+
+     if (connected(tracker_client))
+     {
+         eyeware::HeadPoseInfo head_pose_info = get_head_pose_info(tracker_client);
+         if (!head_pose_info.is_lost)
+         {
+             extract_translation(head_pose_info.transform.translation, last_translation_x_cm,
+                                 last_translation_y_cm, last_translation_z_cm);
+             extract_rotation_angles(head_pose_info.transform.rotation, last_pitch_deg, last_roll_deg, last_yaw_deg);
+         }
+     }
+
+     data[TX] = last_translation_x_cm;
+     data[TY] = last_translation_y_cm;
+     data[TZ] = last_translation_z_cm;
+     data[Yaw] = last_yaw_deg;
+     data[Pitch] = last_pitch_deg;
+     data[Roll] = last_roll_deg;
+}
+
+eyeware_beam_dialog::eyeware_beam_dialog()
+{
+    ui.setupUi(this);
+
+    connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK()));
+}
+
+void eyeware_beam_dialog::doOK()
+{
+    close();
+}
+
+OPENTRACK_DECLARE_TRACKER(eyeware_beam_tracker, eyeware_beam_dialog, eyeware_beam_metadata)