From cd2d0aafbd0e006f2d5e5b129294d5f1119d3ec2 Mon Sep 17 00:00:00 2001
From: Colin Sherratt <colin.sherratt@gmail.com>
Date: Tue, 16 Jun 2015 20:05:46 -0400
Subject: [PATCH] added poses

---
 examples/test.rs |  3 +--
 src/lib.rs       | 49 ++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 50 insertions(+), 2 deletions(-)

diff --git a/examples/test.rs b/examples/test.rs
index 3dd7521..98039ed 100644
--- a/examples/test.rs
+++ b/examples/test.rs
@@ -22,7 +22,6 @@ fn main() {
 
     println!("eye_to_head: {:?}", ivr.eye_to_head_transform(vr::Eye::Left));
     println!("vsync: {:?}", ivr.time_since_last_vsync());
-
-
+    println!("poses {:?}", ivr.tracked_devices(0.).as_slice());
     println!("Done! \\o/");
 }
\ No newline at end of file
diff --git a/src/lib.rs b/src/lib.rs
index af9bd7f..4345b25 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -34,6 +34,28 @@ pub enum Eye {
     Left, Right
 }
 
+#[derive(Debug, Copy, Clone)]
+pub struct TrackedDevicePose {
+    pub to_device: [[f32; 4]; 3],
+    pub velocity: [f32; 3],
+    pub angular_velocity: [f32; 3],
+    pub is_valid: bool,
+    pub is_connected: bool,
+}
+
+#[derive(Debug, Copy, Clone)]
+pub struct TrackedDevicePoses {
+    pub count: usize,
+    pub poses: [TrackedDevicePose; 16],
+}
+
+impl TrackedDevicePoses {
+    pub fn as_slice(&self) -> &[TrackedDevicePose] {
+        &self.poses[0..self.count]
+    }
+}
+
+
 impl Eye {
     fn to_raw(&self) -> openvr_sys::Hmd_Eye {
         match self {
@@ -172,6 +194,33 @@ impl IVRSystem {
             }
         }  
     }
+
+    pub fn tracked_devices(&self, time: f32) -> TrackedDevicePoses {
+        unsafe {
+            let mut data: [openvr_sys::TrackedDevicePose_t; 16] = std::mem::zeroed();
+            openvr_sys::VR_IVRSystem_GetDeviceToAbsoluteTrackingPose(
+                self.0,
+                openvr_sys::TrackingUniverseOrigin::TrackingUniverseSeated,
+                time,
+                &mut data[0],
+                16
+            );
+
+            let mut out: TrackedDevicePoses = std::mem::zeroed();
+            for (i, d) in data.iter().enumerate() {
+                if d.bDeviceIsConnected {
+                    out.count = i + 1;
+                }
+                out.poses[i].is_connected = d.bDeviceIsConnected;
+                out.poses[i].is_valid = d.bPoseIsValid;
+                out.poses[i].to_device = d.mDeviceToAbsoluteTracking.m;
+                out.poses[i].velocity = d.vVelocity.v;
+                out.poses[i].angular_velocity = d.vAngularVelocity.v;
+            }
+
+            out
+        }
+    }
 }
 
 impl Drop for IVRSystem {