summaryrefslogtreecommitdiffhomepage
path: root/tracker-steamvr
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2016-12-28 06:31:05 +0100
committerStanislaw Halik <sthalik@misaki.pl>2016-12-28 06:31:05 +0100
commit4f390f6c8f5c58babd8bc9db13b12cc0b1949cae (patch)
treed64c8376209f254acedd5f09a807af3592af9b4a /tracker-steamvr
parent9f163b82da3ddb08d312d5e935d9792225ac2c69 (diff)
tracker/steamvr: add untested geometry code
Diffstat (limited to 'tracker-steamvr')
-rw-r--r--tracker-steamvr/CMakeLists.txt7
-rw-r--r--tracker-steamvr/steamvr.cpp58
-rw-r--r--tracker-steamvr/steamvr.hpp7
3 files changed, 47 insertions, 25 deletions
diff --git a/tracker-steamvr/CMakeLists.txt b/tracker-steamvr/CMakeLists.txt
index db058e31..48848fa6 100644
--- a/tracker-steamvr/CMakeLists.txt
+++ b/tracker-steamvr/CMakeLists.txt
@@ -27,11 +27,8 @@ endif()
if(steamvr-dll)
SET(SDK_VALVE_STEAMVR "" CACHE PATH "Valve's SteamVR")
if(SDK_VALVE_STEAMVR)
- set(porked NO-INSTALL)
- opentrack_boilerplate(opentrack-tracker-steamvr ${porked})
- if(NOT porked)
- install(FILES "${SDK_VALVE_STEAMVR}/bin/${steamvr-dir}/${steamvr-dll}" DESTINATION "${opentrack-hier-pfx}")
- endif()
+ opentrack_boilerplate(opentrack-tracker-steamvr)
+ install(FILES "${SDK_VALVE_STEAMVR}/bin/${steamvr-dir}/${steamvr-dll}" DESTINATION "${opentrack-hier-pfx}")
target_include_directories(opentrack-tracker-steamvr SYSTEM PUBLIC "${SDK_VALVE_STEAMVR}/headers")
target_link_libraries(opentrack-tracker-steamvr "${SDK_VALVE_STEAMVR}/lib/${steamvr-dir}/${steamvr-lib}")
diff --git a/tracker-steamvr/steamvr.cpp b/tracker-steamvr/steamvr.cpp
index bab98f0d..9ceb5db2 100644
--- a/tracker-steamvr/steamvr.cpp
+++ b/tracker-steamvr/steamvr.cpp
@@ -2,10 +2,10 @@
#include "steamvr.hpp"
#include "api/plugin-api.hpp"
#include "compat/util.hpp"
-#include "compat/euler.hpp"
#include <cstdlib>
#include <cmath>
+#include <algorithm>
#include <QMessageBox>
#include <QDebug>
@@ -79,16 +79,39 @@ void steamvr::start_tracker(QFrame*)
}
}
+quat steamvr::get_quaternion(const vr::HmdMatrix34_t& r)
+{
+ using std::max;
+ using std::sqrt;
+ using std::acos;
+
+ const auto& m = r.m;
+
+ float qw = sqrt(max(0.f, 1 + m[0][0] + m[1][1] + m[2][2])) / 2;
+ float qx = sqrt(max(0.f, 1 + m[0][0] - m[1][1] - m[2][2])) / 2;
+ float qy = sqrt(max(0.f, 1 - m[0][0] + m[1][1] - m[2][2])) / 2;
+ float qz = sqrt(max(0.f, 1 - m[0][0] - m[1][1] + m[2][2])) / 2;
+ qx = (m[1][2] - m[2][1]) < 0 ? -qx : qx;
+ qy = (m[2][0] - m[0][2]) < 0 ? -qy : qy;
+ qz = (m[0][1] - m[1][0]) < 0 ? -qz : qz;
+
+ float s = sqrt(1 - qw * qw);
+ s = s < .001 ? 1 : s;
+
+ return quat(2 * acos(qw), qx/s, qy/s, qz/s);
+}
+
void steamvr::data(double* data)
{
if (vr)
{
- using m = float[3][4];
vr::TrackedDevicePose_t devices[vr::k_unMaxTrackedDeviceCount] = {};
vr->GetDeviceToAbsoluteTrackingPose(vr::ETrackingUniverseOrigin::TrackingUniverseStanding, 0,
devices, vr::k_unMaxTrackedDeviceCount);
+ bool done = false;
+
for (unsigned k = 0; k < vr::k_unMaxTrackedDeviceCount; k++)
{
using namespace euler;
@@ -99,34 +122,29 @@ void steamvr::data(double* data)
if (vr->GetTrackedDeviceClass(k) != vr::ETrackedDeviceClass::TrackedDeviceClass_HMD)
continue;
- const m& M = devices[k].mDeviceToAbsoluteTracking.m;
+ const auto& result = devices[k].mDeviceToAbsoluteTracking;
static constexpr double c[3] { -1, 1, -1 };
for (unsigned i = 0; i < 3; i++)
- data[i] = double(M[i][3]) * c[i] * 100;
-
- static constexpr unsigned indices[3] = {Roll, Yaw, Pitch};
+ data[i] = double(result.m[i][3]) * c[i] * 100;
- rmat r(M[0][0], M[1][0], M[2][0],
- M[0][1], M[1][1], M[2][1],
- M[0][2], M[1][2], M[2][2]);
+ const quat q = get_quaternion(result);
+ static constexpr double r2d = 180/M_PI;
- euler_t ypr(rmat_to_euler(r));
+ using std::atan2;
+ using std::asin;
- for (unsigned i = 0; i < 3; i++)
- data[indices[i]] = 180/M_PI * ypr(i);
+ data[Roll] = r2d * atan2(2*(q(0)*q(1) + q(2)*q(3)), 1 - 2*(q(1)*q(1) + q(2)*q(2)));
+ data[Pitch] = r2d * asin(2*(q(0)*q(2) - q(3)*q(1)));
+ data[Yaw] = r2d * atan2(2*(q(0)*q(3) + q(1)*q(2)), 1 - 2*(q(2)*q(2) + q(3)*q(3)));
- for (unsigned i = 0; i < 3; i++)
- data[i+3] *= c[i];
-
- goto done;
+ done = true;
+ break;
}
- qDebug() << "steamvr: no device with pose found";
-
-done:
- (void)0; // expects statement after label
+ if (!done)
+ qDebug() << "steamvr: no device with pose found";
}
}
diff --git a/tracker-steamvr/steamvr.hpp b/tracker-steamvr/steamvr.hpp
index 9b53979e..1adb40b1 100644
--- a/tracker-steamvr/steamvr.hpp
+++ b/tracker-steamvr/steamvr.hpp
@@ -4,6 +4,8 @@
#include "compat/util.hpp"
#include "options/options.hpp"
+#include "compat/euler.hpp"
+
#include <openvr.h>
#include <atomic>
@@ -16,6 +18,8 @@
using namespace options;
+using quat = Mat<float, 4, 1>;
+
struct settings : opts
{
settings() : opts("valve-steamvr") {}
@@ -29,12 +33,15 @@ public:
void start_tracker(QFrame *) override;
void data(double *data) override;
private:
+
using error_t = vr::EVRInitError;
using vr_t = vr::IVRSystem*;
vr_t vr;
settings s;
+
+ static quat get_quaternion(const vr::HmdMatrix34_t& r);
static void vr_deleter();
static vr_t vr_init(error_t& error);
static QString strerror(error_t error);