Store matrices for all tracked OpenVR devices, not just the HMD

[libs/vr.git] / source / openvr / openvrsystem.cpp

diff --git a/source/openvr/openvrsystem.cpp b/source/openvr/openvrsystem.cpp
index 50d2ecfec54b05380c41bcfa24084aca5e14738a..8c3188c370006edf0e5f38c3da713afe37d0a938 100644 (file)
--- a/source/openvr/openvrsystem.cpp
+++ b/source/openvr/openvrsystem.cpp
@@ -23,7 +23,8 @@ namespace VR {
 
 unsigned OpenVRSystem::n_instances = 0;
 
-OpenVRSystem::OpenVRSystem()
+OpenVRSystem::OpenVRSystem():
+       n_tracked_devices(0)
 {
        if(!n_instances)
        {
@@ -39,6 +40,12 @@ OpenVRSystem::OpenVRSystem()
                throw runtime_error("OpenVR compositor initialization failed");
 
        vr::VRCompositor()->SetTrackingSpace(vr::TrackingUniverseSeated);
+
+       for(unsigned i=0; i<vr::k_unMaxTrackedDeviceCount; ++i)
+               if(vr_sys->IsTrackedDeviceConnected(i))
+                       n_tracked_devices = i+1;
+
+       tracking_matrices.resize(n_tracked_devices);
 }
 
 OpenVRSystem::~OpenVRSystem()
@@ -97,12 +104,25 @@ OpenVRCombiner *OpenVRSystem::create_combiner(GL::View &v)
 void OpenVRSystem::update_pose_matrices()
 {
        vector<vr::TrackedDevicePose_t> poses;
-       poses.resize(vr::k_unTrackedDeviceIndex_Hmd+1);
+       poses.resize(n_tracked_devices);
        vr::VRCompositor()->WaitGetPoses(&poses[0], poses.size(), 0, 0);
 
-       vr::TrackedDevicePose_t &hmd_pose = poses[vr::k_unTrackedDeviceIndex_Hmd];
-       if(hmd_pose.bPoseIsValid)
-               hmd_matrix = convert_matrix(hmd_pose.mDeviceToAbsoluteTracking);
+       for(unsigned i=0; i<n_tracked_devices; ++i)
+               if(poses[i].bPoseIsValid)
+                       tracking_matrices[i] = convert_matrix(poses[i].mDeviceToAbsoluteTracking);
+}
+
+const GL::Matrix &OpenVRSystem::get_tracking_matrix(unsigned index) const
+{
+       if(index>=tracking_matrices.size())
+               throw out_of_range("OpenVRSystem::get_tracking_matrix");
+
+       return tracking_matrices[index];
+}
+
+const GL::Matrix &OpenVRSystem::get_hmd_matrix() const
+{
+       return get_tracking_matrix(vr::k_unTrackedDeviceIndex_Hmd);
 }
 
 } // namespace VR