Improve vive orientation;

src/headset/headset.c

@@ -39,8 +39,8 @@ void lovrHeadsetGetPosition(float* x, float* y, float* z) {
   headset->interface->getPosition(headset, x, y, z);
 }
 
-void lovrHeadsetGetOrientation(float* x, float* y, float* z, float* w) {
-  headset->interface->getOrientation(headset, x, y, z, w);
+void lovrHeadsetGetOrientation(float* w, float* x, float* y, float* z) {
+  headset->interface->getOrientation(headset, w, x, y, z);
 }
 
 void lovrHeadsetGetVelocity(float* x, float* y, float* z) {

src/headset/headset.h

@@ -20,7 +20,7 @@ typedef struct {
   char (*isBoundsVisible)(void* headset);
   void (*setBoundsVisible)(void* headset, char visible);
   void (*getPosition)(void* headset, float* x, float* y, float* z);
-  void (*getOrientation)(void* headset, float* x, float* y, float* z, float* w);
+  void (*getOrientation)(void* headset, float* w, float* x, float* y, float* z);
   void (*getVelocity)(void* headset, float* x, float* y, float* z);
   void (*getAngularVelocity)(void* headset, float* x, float* y, float* z);
   Controller* (*getController)(void* headset, ControllerHand hand);
@@ -45,7 +45,7 @@ void lovrHeadsetGetTrackingSize(float* width, float* depth);
 char lovrHeadsetIsBoundsVisible();
 void lovrHeadsetSetBoundsVisible(char isVisible);
 void lovrHeadsetGetPosition(float* x, float* y, float* z);
-void lovrHeadsetGetOrientation(float* x, float* y, float* z, float* w);
+void lovrHeadsetGetOrientation(float* w, float* x, float* y, float* z);
 void lovrHeadsetGetVelocity(float* x, float* y, float* z);
 void lovrHeadsetGetAngularVelocity(float* x, float* y, float* z);
 Controller* lovrHeadsetGetController(ControllerHand hand);

src/headset/vive.c

@@ -50,9 +50,8 @@ static HeadsetInterface interface = {
 static TrackedDevicePose_t viveGetPose(ViveState* state, unsigned int deviceIndex) {
   ETrackingUniverseOrigin origin = ETrackingUniverseOrigin_TrackingUniverseStanding;
   float secondsInFuture = 0.f;
-  unsigned int maxPoses = k_unMaxTrackedDeviceCount;
-  TrackedDevicePose_t poses[maxPoses];
-  state->vrSystem->GetDeviceToAbsoluteTrackingPose(origin, secondsInFuture, poses, maxPoses);
+  TrackedDevicePose_t poses[16];
+  state->vrSystem->GetDeviceToAbsoluteTrackingPose(origin, secondsInFuture, poses, 16);
   return poses[deviceIndex];
 }
 
@@ -204,9 +203,19 @@ void viveGetPosition(void* headset, float* x, float* y, float* z) {
   *z = pose.mDeviceToAbsoluteTracking.m[2][3];
 }
 
-// TODO convert matrix to quaternion!
-void viveGetOrientation(void* headset, float* x, float* y, float *z, float* w) {
-  *x = *y = *z = *w = 0.f;
+void viveGetOrientation(void* headset, float* w, float* x, float* y, float *z) {
+  Headset* this = (Headset*) headset;
+  ViveState* state = this->state;
+  TrackedDevicePose_t pose = viveGetPose(state, state->headsetIndex);
+
+  if (!pose.bPoseIsValid || !pose.bDeviceIsConnected) {
+    *w = *x = *y = *z = 0.f;
+    return;
+  }
+
+  float matrix[16];
+  mat4_fromMat44(matrix, pose.mDeviceToAbsoluteTracking.m);
+  mat4_getRotation(matrix, w, x, y, z);
 }
 
 void viveGetVelocity(void* headset, float* x, float* y, float* z) {

src/lovr/headset.c

@@ -18,7 +18,7 @@ const luaL_Reg lovrHeadset[] = {
   { "setClipDistance", l_lovrHeadsetSetClipDistance },
   { "getTrackingSize", l_lovrHeadsetGetTrackingSize },
   { "getPosition", l_lovrHeadsetGetPosition },
-  { "getOrientation", l_lovrHeadsetGetPosition },
+  { "getOrientation", l_lovrHeadsetGetOrientation },
   { "getVelocity", l_lovrHeadsetGetVelocity },
   { "getAngularVelocity", l_lovrHeadsetGetAngularVelocity },
   { "getController", l_lovrHeadsetGetController },
@@ -94,12 +94,12 @@ int l_lovrHeadsetGetPosition(lua_State* L) {
 }
 
 int l_lovrHeadsetGetOrientation(lua_State* L) {
-  float x, y, z, w;
-  lovrHeadsetGetOrientation(&x, &y, &z, &w);
+  float w, x, y, z;
+  lovrHeadsetGetOrientation(&w, &x, &y, &z);
+  lua_pushnumber(L, w);
   lua_pushnumber(L, x);
   lua_pushnumber(L, y);
   lua_pushnumber(L, z);
-  lua_pushnumber(L, w);
   return 4;
 }
 

src/matrix.c

@@ -2,7 +2,6 @@
 #include <math.h>
 #include <stdlib.h>
 #include <string.h>
-
 /*
    m0 m4 m8  m12
    m1 m5 m9  m13
@@ -117,6 +116,33 @@ mat4 mat4_setProjection(mat4 matrix, float near, float far, float fov, float asp
   return matrix;
 }
 
+void mat4_getRotation(mat4 matrix, float* w, float* x, float* y, float* z) {
+  float qw = sqrt(1 + matrix[1] + matrix[5] + matrix[10]) / 2;
+  float scale = qw * 4;
+  float qx = matrix[9] - matrix[6] / scale;
+  float qy = matrix[2] - matrix[8] / scale;
+  float qz = matrix[4] - matrix[1] / scale;
+
+  float rlen = 1 / sqrt(qw * qw + qx * qx + qy * qy + qz * qz);
+  qw *= rlen;
+  qx *= rlen;
+  qy *= rlen;
+  qz *= rlen;
+
+  *w = 2 * acos(qw);
+
+  float s = sqrt(1 - qw * qw);
+  if (s < .00000001) {
+    *x = qx;
+    *y = qy;
+    *z = qz;
+  } else {
+    *x = qx / s;
+    *y = qy / s;
+    *z = qz / s;
+  }
+}
+
 mat4 mat4_translate(mat4 matrix, float x, float y, float z) {
   float translation[16];
   mat4_setTranslation(translation, x, y, z);

src/matrix.h

@@ -10,6 +10,7 @@ mat4 mat4_setTranslation(mat4 matrix, float x, float y, float z);
 mat4 mat4_setRotation(mat4 matrix, float w, float x, float y, float z);
 mat4 mat4_setScale(mat4 matrix, float x, float y, float z);
 mat4 mat4_setProjection(mat4 matrix, float near, float far, float fov, float aspect);
+void mat4_getRotation(mat4 matrix, float* w, float* x, float* y, float* z);
 mat4 mat4_translate(mat4 matrix, float x, float y, float z);
 mat4 mat4_rotate(mat4 matrix, float w, float x, float y, float z);
 mat4 mat4_scale(mat4 matrix, float x, float y, float z);