From 5fe4e511b9eb2f093070270e8f5694bcdf3a4f96 Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Thu, 6 Oct 2016 02:41:47 +0300
Subject: [PATCH] Store matrices for all tracked OpenVR devices, not just the
 HMD

---
 source/openvr/openvrsystem.cpp | 30 +++++++++++++++++++++++++-----
 source/openvr/openvrsystem.h   |  6 ++++--
 2 files changed, 29 insertions(+), 7 deletions(-)

diff --git a/source/openvr/openvrsystem.cpp b/source/openvr/openvrsystem.cpp
index 50d2ecf..8c3188c 100644
--- 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
diff --git a/source/openvr/openvrsystem.h b/source/openvr/openvrsystem.h
index 06881f8..ca5411e 100644
--- a/source/openvr/openvrsystem.h
+++ b/source/openvr/openvrsystem.h
@@ -12,7 +12,8 @@ namespace VR {
 class OpenVRSystem: public System
 {
 private:
-	GL::Matrix hmd_matrix;
+	unsigned n_tracked_devices;
+	std::vector<GL::Matrix> tracking_matrices;
 
 	static unsigned n_instances;
 
@@ -31,7 +32,8 @@ public:
 	virtual OpenVRCombiner *create_combiner(GL::View &);
 
 	void update_pose_matrices();
-	const GL::Matrix &get_hmd_matrix() const { return hmd_matrix; }
+	const GL::Matrix &get_tracking_matrix(unsigned) const;
+	const GL::Matrix &get_hmd_matrix() const;
 };
 
 } // namespace VR
-- 
2.43.0