From d3de3f9f0176091ac23704371b24bc4ec722bfec Mon Sep 17 00:00:00 2001
From: Glen Fraser <holaglen@gmail.com>
Date: Wed, 23 Dec 2015 12:02:21 +0100
Subject: [PATCH] Add method to get full Oculus pose

- also get Oculus monitor id in a more reliable way.
---
 src/ofxOculusDK2.cpp | 46 +++++++++++++++++++++++++++++++++++++-------
 src/ofxOculusDK2.h   |  1 +
 2 files changed, 40 insertions(+), 7 deletions(-)

diff --git a/src/ofxOculusDK2.cpp b/src/ofxOculusDK2.cpp
index fd0c2a8..c27fb96 100644
--- a/src/ofxOculusDK2.cpp
+++ b/src/ofxOculusDK2.cpp
@@ -468,16 +468,23 @@ void ofxOculusDK2::fullscreenOnRift() {
     
     // Grab the active displays
     CGGetActiveDisplayList(32, displays, &displayCount);
-    int numDisplays= displayCount;
     
-    // If two displays present, use the 2nd one. If one, use the first.
-    int whichDisplay = hmd->DisplayId;
+    // Figure out which display the Oculus is on
+    int whichDisplay = 0;
+    for (int i = 0; i < displayCount; ++i) {
+        CGRect displayBounds = CGDisplayBounds ( displays[i] );
+        if (hmd->WindowsPos.x == displayBounds.origin.x &&
+            hmd->WindowsPos.y == displayBounds.origin.y &&
+            hmd->Resolution.w == displayBounds.size.width &&
+            hmd->Resolution.h == displayBounds.size.height) {
+            whichDisplay = i;
+            break;
+        }
+    }
     
-    int displayHeight= CGDisplayPixelsHigh ( displays[whichDisplay] );
-    int displayWidth= CGDisplayPixelsWide ( displays[whichDisplay] );
-    CGRect displayBounds= CGDisplayBounds ( displays[whichDisplay] );
+    CGRect displayBounds = CGDisplayBounds ( displays[whichDisplay] );
     
-    ofRectangle riftDisplay = ofRectangle(displayBounds.origin.x, displayBounds.origin.y, displayWidth, displayHeight);
+    ofRectangle riftDisplay = ofRectangle(displayBounds.origin.x, displayBounds.origin.y, displayBounds.size.width, displayBounds.size.height);
     
     ofSetWindowShape(riftDisplay.width, riftDisplay.height);
     ofSetWindowPosition(riftDisplay.x+1, riftDisplay.y+1);
@@ -673,6 +680,31 @@ ofQuaternion ofxOculusDK2::getOrientationQuat(){
 	 return ofQuaternion();
 }
 
+ofVec3f ofxOculusDK2::getTranslation(){
+
+    ovrTrackingState ts = ovrHmd_GetTrackingState(hmd, ovr_GetTimeInSeconds());
+    if (ts.StatusFlags & (ovrStatus_OrientationTracked | ovrStatus_PositionTracked)){
+        return toOf(ts.HeadPose.ThePose.Position);
+    }
+    return ofVec3f();
+}
+
+// This returns the pose of the head (the inverse of its viewing transform).
+// This is useful if you want to draw it in another view.
+ofMatrix4x4 ofxOculusDK2::getFullHeadPose(){
+    ofMatrix4x4 baseCameraPose = baseCamera->getModelViewMatrix().getInverse();
+    ofMatrix4x4 hmdPose;
+    
+    // head orientation and position
+    ovrTrackingState ts = ovrHmd_GetTrackingState(hmd, ovr_GetTimeInSeconds());
+    if (ts.StatusFlags & (ovrStatus_OrientationTracked | ovrStatus_PositionTracked)){
+        hmdPose = ofMatrix4x4::newRotationMatrix( toOf(ts.HeadPose.ThePose.Orientation)) * ofMatrix4x4::newTranslationMatrix( toOf(ts.HeadPose.ThePose.Position));
+    }
+
+    // return the composed result
+    return hmdPose * baseCameraPose;
+}
+
 ofMatrix4x4 ofxOculusDK2::getProjectionMatrix(ovrEyeType eye) {
     return toOf(ovrMatrix4f_Projection(eyeRenderDesc[eye].Fov, .01f, 10000.0f, true) );
 }
diff --git a/src/ofxOculusDK2.h b/src/ofxOculusDK2.h
index 95952bb..06067e1 100644
--- a/src/ofxOculusDK2.h
+++ b/src/ofxOculusDK2.h
@@ -108,6 +108,7 @@ class ofxOculusDK2
 	ofQuaternion getOrientationQuat();
 	ofMatrix4x4 getOrientationMat();
     ofVec3f getTranslation();
+    ofMatrix4x4 getFullHeadPose();
 	
 	//default 1 has more constrained mouse movement,
 	//while turning it up increases the reach of the mouse