From d448bdc95c7bba13adba65e9adbe7a58309c998e Mon Sep 17 00:00:00 2001
From: bjorn <tie.372@gmail.com>
Date: Wed, 22 Jan 2020 19:00:51 -0800
Subject: [PATCH 1/9] Finger devices/axes; OpenVR finger poses;

---
 src/api/l_headset.c           |  12 ++++
 src/modules/headset/headset.h |  12 ++++
 src/modules/headset/openvr.c  | 102 ++++++++++++++++++++++++++++++----
 3 files changed, 116 insertions(+), 10 deletions(-)

diff --git a/src/api/l_headset.c b/src/api/l_headset.c
index a9711d1f..7e6c337a 100644
--- a/src/api/l_headset.c
+++ b/src/api/l_headset.c
@@ -35,6 +35,16 @@ const char* Devices[] = {
   [DEVICE_HAND_RIGHT] = "hand/right",
   [DEVICE_EYE_LEFT] = "eye/left",
   [DEVICE_EYE_RIGHT] = "eye/right",
+  [DEVICE_HAND_LEFT_FINGER_THUMB] = "hand/left/finger/thumb",
+  [DEVICE_HAND_LEFT_FINGER_INDEX] = "hand/left/finger/index",
+  [DEVICE_HAND_LEFT_FINGER_MIDDLE] = "hand/left/finger/middle",
+  [DEVICE_HAND_LEFT_FINGER_RING] = "hand/left/finger/ring",
+  [DEVICE_HAND_LEFT_FINGER_PINKY] = "hand/left/finger/pinky",
+  [DEVICE_HAND_RIGHT_FINGER_THUMB] = "hand/right/finger/thumb",
+  [DEVICE_HAND_RIGHT_FINGER_INDEX] = "hand/right/finger/index",
+  [DEVICE_HAND_RIGHT_FINGER_MIDDLE] = "hand/right/finger/middle",
+  [DEVICE_HAND_RIGHT_FINGER_RING] = "hand/right/finger/ring",
+  [DEVICE_HAND_RIGHT_FINGER_PINKY] = "hand/right/finger/pinky",
   NULL
 };
 
@@ -57,6 +67,8 @@ const char* DeviceAxes[] = {
   [AXIS_THUMBSTICK] = "thumbstick",
   [AXIS_TOUCHPAD] = "touchpad",
   [AXIS_GRIP] = "grip",
+  [AXIS_CURL] = "curl",
+  [AXIS_SPLAY] = "splay",
   NULL
 };
 
diff --git a/src/modules/headset/headset.h b/src/modules/headset/headset.h
index 9fcb275c..7b1f2e80 100644
--- a/src/modules/headset/headset.h
+++ b/src/modules/headset/headset.h
@@ -28,6 +28,16 @@ typedef enum {
   DEVICE_HAND_RIGHT,
   DEVICE_EYE_LEFT,
   DEVICE_EYE_RIGHT,
+  DEVICE_HAND_LEFT_FINGER_THUMB,
+  DEVICE_HAND_LEFT_FINGER_INDEX,
+  DEVICE_HAND_LEFT_FINGER_MIDDLE,
+  DEVICE_HAND_LEFT_FINGER_RING,
+  DEVICE_HAND_LEFT_FINGER_PINKY,
+  DEVICE_HAND_RIGHT_FINGER_THUMB,
+  DEVICE_HAND_RIGHT_FINGER_INDEX,
+  DEVICE_HAND_RIGHT_FINGER_MIDDLE,
+  DEVICE_HAND_RIGHT_FINGER_RING,
+  DEVICE_HAND_RIGHT_FINGER_PINKY,
   MAX_DEVICES
 } Device;
 
@@ -50,6 +60,8 @@ typedef enum {
   AXIS_THUMBSTICK,
   AXIS_TOUCHPAD,
   AXIS_GRIP,
+  AXIS_CURL,
+  AXIS_SPLAY,
   MAX_AXES
 } DeviceAxis;
 
diff --git a/src/modules/headset/openvr.c b/src/modules/headset/openvr.c
index f38b1ace..d46819fa 100644
--- a/src/modules/headset/openvr.c
+++ b/src/modules/headset/openvr.c
@@ -27,6 +27,41 @@ extern bool VR_IsRuntimeInstalled();
 #define HEADSET k_unTrackedDeviceIndex_Hmd
 #define INVALID_DEVICE k_unTrackedDeviceIndexInvalid
 
+enum {
+  eBone_Root = 0,
+  eBone_Wrist,
+  eBone_Thumb0,
+  eBone_Thumb1,
+  eBone_Thumb2,
+  eBone_Thumb3,
+  eBone_IndexFinger0,
+  eBone_IndexFinger1,
+  eBone_IndexFinger2,
+  eBone_IndexFinger3,
+  eBone_IndexFinger4,
+  eBone_MiddleFinger0,
+  eBone_MiddleFinger1,
+  eBone_MiddleFinger2,
+  eBone_MiddleFinger3,
+  eBone_MiddleFinger4,
+  eBone_RingFinger0,
+  eBone_RingFinger1,
+  eBone_RingFinger2,
+  eBone_RingFinger3,
+  eBone_RingFinger4,
+  eBone_PinkyFinger0,
+  eBone_PinkyFinger1,
+  eBone_PinkyFinger2,
+  eBone_PinkyFinger3,
+  eBone_PinkyFinger4,
+  eBone_Aux_Thumb,
+  eBone_Aux_IndexFinger,
+  eBone_Aux_MiddleFinger,
+  eBone_Aux_RingFinger,
+  eBone_Aux_PinkyFinger,
+  eBone_Count
+};
+
 static struct {
   struct VR_IVRSystem_FnTable* system;
   struct VR_IVRCompositor_FnTable* compositor;
@@ -295,24 +330,71 @@ static const float* openvr_getBoundsGeometry(uint32_t* count) {
 }
 
 static bool openvr_getPose(Device device, vec3 position, quat orientation) {
-  InputPoseActionData_t actionData;
-  TrackedDevicePose_t* pose;
+  float transform[16];
 
+  // Early exit for head pose
   if (device == DEVICE_HEAD) {
-    pose = &state.headPose;
-  } else if (device == DEVICE_HAND_LEFT || device == DEVICE_HAND_RIGHT) {
-    state.input->GetPoseActionData(state.poseActions[device], state.compositor->GetTrackingSpace(), 0.f, &actionData, sizeof(actionData), 0);
-    pose = &actionData.pose;
+    mat4_fromMat34(transform, state.headPose.mDeviceToAbsoluteTracking.m);
+    transform[13] += state.offset;
+    mat4_getPosition(transform, position);
+    mat4_getOrientation(transform, orientation);
+    return state.headPose.bPoseIsValid;
+  }
+
+  Device hand;
+  if (device == DEVICE_HAND_LEFT || device == DEVICE_HAND_RIGHT) {
+    hand = device;
+  } else if (device >= DEVICE_HAND_LEFT_FINGER_THUMB && device <= DEVICE_HAND_LEFT_FINGER_PINKY) {
+    hand = DEVICE_HAND_LEFT;
+  } else if (device >= DEVICE_HAND_RIGHT_FINGER_THUMB && device <= DEVICE_HAND_RIGHT_FINGER_PINKY) {
+    hand = DEVICE_HAND_RIGHT;
   } else {
     return false;
   }
 
-  float transform[16];
-  mat4_fromMat34(transform, pose->mDeviceToAbsoluteTracking.m);
+  InputPoseActionData_t actionData;
+  state.input->GetPoseActionData(state.poseActions[hand], state.compositor->GetTrackingSpace(), 0.f, &actionData, sizeof(actionData), 0);
+  mat4_fromMat34(transform, actionData.pose.mDeviceToAbsoluteTracking.m);
+  transform[13] += state.offset;
+
+  // Early exit for hand pose
+  if (device == hand) {
+    mat4_getPosition(transform, position);
+    mat4_getOrientation(transform, orientation);
+    return actionData.pose.bPoseIsValid;
+  }
+
+  InputSkeletalActionData_t info;
+  EVRInputError error = state.input->GetSkeletalActionData(state.skeletonActions[hand - DEVICE_HAND_LEFT], &info, sizeof(info));
+  if (error || !info.bActive) {
+    return false;
+  }
+
+  VRBoneTransform_t bones[31];
+  EVRSkeletalTransformSpace space = EVRSkeletalTransformSpace_VRSkeletalTransformSpace_Model;
+  EVRSkeletalMotionRange motionRange = EVRSkeletalMotionRange_VRSkeletalMotionRange_WithoutController;
+  error = state.input->GetSkeletalBoneData(state.skeletonActions[hand - DEVICE_HAND_LEFT], space, motionRange, bones, sizeof(bones) / sizeof(bones[0]));
+  if (error) {
+    return false;
+  }
+
+  uint32_t finger = (hand == DEVICE_HAND_LEFT) ? (device - DEVICE_HAND_LEFT_FINGER_THUMB) : (device - DEVICE_HAND_RIGHT_FINGER_THUMB);
+  uint32_t boneIndex;
+  switch (finger) {
+    case 0: boneIndex = eBone_Thumb3; break;
+    case 1: boneIndex = eBone_IndexFinger4; break;
+    case 2: boneIndex = eBone_MiddleFinger4; break;
+    case 3: boneIndex = eBone_RingFinger4; break;
+    case 4: boneIndex = eBone_PinkyFinger4; break;
+    default: return false;
+  }
+
+  VRBoneTransform_t* bone = &bones[boneIndex];
+  mat4_translate(transform, bone->position.v[0], bone->position.v[1], bone->position.v[2]);
+  mat4_rotateQuat(transform, (float[4]) { bone->orientation.x, bone->orientation.y, bone->orientation.z, bone->orientation.w });
   mat4_getPosition(transform, position);
   mat4_getOrientation(transform, orientation);
-  transform[13] += state.offset;
-  return pose->bPoseIsValid;
+  return true;
 }
 
 static bool openvr_getVelocity(Device device, vec3 velocity, vec3 angularVelocity) {

From 71e63ddac2e66b3eaf4708f3aba3b9376df95308 Mon Sep 17 00:00:00 2001
From: bjorn <tie.372@gmail.com>
Date: Fri, 24 Jan 2020 18:14:44 -0800
Subject: [PATCH 2/9] openvr: add curl/splay axes for finger devices;

---
 src/api/l_headset.c          |  4 +++-
 src/modules/headset/openvr.c | 38 ++++++++++++++++++++++++++++++------
 2 files changed, 35 insertions(+), 7 deletions(-)

diff --git a/src/api/l_headset.c b/src/api/l_headset.c
index 7e6c337a..f822aeb2 100644
--- a/src/api/l_headset.c
+++ b/src/api/l_headset.c
@@ -466,7 +466,9 @@ static const int axisCounts[MAX_AXES] = {
   [AXIS_TRIGGER] = 1,
   [AXIS_THUMBSTICK] = 2,
   [AXIS_TOUCHPAD] = 2,
-  [AXIS_GRIP] = 1
+  [AXIS_GRIP] = 1,
+  [AXIS_CURL] = 1,
+  [AXIS_SPLAY] = 2
 };
 
 static int l_lovrHeadsetGetAxis(lua_State* L) {
diff --git a/src/modules/headset/openvr.c b/src/modules/headset/openvr.c
index d46819fa..1a15e65b 100644
--- a/src/modules/headset/openvr.c
+++ b/src/modules/headset/openvr.c
@@ -69,7 +69,7 @@ static struct {
   struct VR_IVRRenderModels_FnTable* renderModels;
   struct VR_IVRInput_FnTable* input;
   VRActionSetHandle_t actionSet;
-  VRActionHandle_t poseActions[MAX_DEVICES];
+  VRActionHandle_t poseActions[3];
   VRActionHandle_t buttonActions[2][MAX_BUTTONS];
   VRActionHandle_t touchActions[2][MAX_BUTTONS];
   VRActionHandle_t axisActions[2][MAX_AXES];
@@ -439,14 +439,40 @@ static bool openvr_isTouched(Device device, DeviceButton button, bool* touched)
 }
 
 static bool openvr_getAxis(Device device, DeviceAxis axis, vec3 value) {
-  if (device != DEVICE_HAND_LEFT && device != DEVICE_HAND_RIGHT) {
+  if (device == DEVICE_HAND_LEFT || device == DEVICE_HAND_RIGHT) {
+    InputAnalogActionData_t actionData;
+    state.input->GetAnalogActionData(state.axisActions[device - DEVICE_HAND_LEFT][axis], &actionData, sizeof(actionData), 0);
+    vec3_set(value, actionData.x, actionData.y, actionData.z);
+    return actionData.bActive;
+  }
+
+  uint32_t finger;
+  VRActionHandle_t skeletonAction;
+  if (device >= DEVICE_HAND_LEFT_FINGER_THUMB && device <= DEVICE_HAND_LEFT_FINGER_PINKY) {
+    finger = device - DEVICE_HAND_LEFT_FINGER_THUMB;
+    skeletonAction = state.skeletonActions[0];
+  } else if (device >= DEVICE_HAND_RIGHT_FINGER_THUMB && device <= DEVICE_HAND_RIGHT_FINGER_PINKY) {
+    finger = device - DEVICE_HAND_RIGHT_FINGER_THUMB;
+    skeletonAction = state.skeletonActions[1];
+  } else {
     return false;
   }
 
-  InputAnalogActionData_t actionData;
-  state.input->GetAnalogActionData(state.axisActions[device - DEVICE_HAND_LEFT][axis], &actionData, sizeof(actionData), 0);
-  vec3_set(value, actionData.x, actionData.y, actionData.z);
-  return actionData.bActive;
+  VRSkeletalSummaryData_t summary;
+  if (state.input->GetSkeletalSummaryData(skeletonAction, &summary)) {
+    return false;
+  }
+
+  if (axis == AXIS_CURL) {
+    value[0] = summary.flFingerCurl[finger];
+    return true;
+  } else if (axis == AXIS_SPLAY) {
+    value[0] = finger > 0 ? summary.flFingerSplay[finger - 1] : 0.f;
+    value[1] = finger < 4 ? summary.flFingerSplay[finger - 0] : 0.f;
+    return true;
+  }
+
+  return false;
 }
 
 static bool openvr_vibrate(Device device, float strength, float duration, float frequency) {

From 7e34c7adcc86df148275019dc7a1245c01aca004 Mon Sep 17 00:00:00 2001
From: bjorn <tie.372@gmail.com>
Date: Fri, 24 Jan 2020 18:50:24 -0800
Subject: [PATCH 3/9] leap: getPose for finger devices;

---
 src/modules/headset/leap.c | 27 +++++++++++++++++++++++----
 1 file changed, 23 insertions(+), 4 deletions(-)

diff --git a/src/modules/headset/leap.c b/src/modules/headset/leap.c
index 21b772d0..f248a0f9 100644
--- a/src/modules/headset/leap.c
+++ b/src/modules/headset/leap.c
@@ -83,14 +83,33 @@ static void adjustPose(vec3 position, vec3 direction) {
 }
 
 static bool leap_getPose(Device device, vec3 position, quat orientation) {
-  if ((device != DEVICE_HAND_LEFT && device != DEVICE_HAND_RIGHT) || !state.hands[device - DEVICE_HAND_LEFT]) {
+  if (device == DEVICE_HAND_LEFT || device == DEVICE_HAND_RIGHT) {
+    LEAP_HAND* hand = state.hands[device - DEVICE_HAND_LEFT];
+    if (hand) {
+      float direction[4];
+      vec3_init(position, hand->palm.position.v);
+      vec3_init(direction, hand->palm.normal.v);
+      adjustPose(position, direction);
+      quat_between(orientation, (float[4]) { 0.f, 0.f, -1.f }, direction);
+      return true;
+    } else {
+      return false;
+    }
+  }
+
+  LEAP_BONE* distal;
+  if (state.hands[0] && device >= DEVICE_HAND_LEFT_FINGER_THUMB && device <= DEVICE_HAND_LEFT_FINGER_PINKY) {
+    distal = &state.hands[0]->digits[device - DEVICE_HAND_LEFT_FINGER_THUMB].distal;
+  } else if (state.hands[1] && device >= DEVICE_HAND_RIGHT_FINGER_THUMB && device <= DEVICE_HAND_RIGHT_FINGER_PINKY) {
+    distal = &state.hands[1]->digits[device - DEVICE_HAND_RIGHT_FINGER_THUMB].distal;
+  } else {
     return false;
   }
 
   float direction[4];
-  LEAP_HAND* hand = state.hands[device - DEVICE_HAND_LEFT];
-  vec3_init(position, hand->palm.position.v);
-  vec3_init(direction, hand->palm.normal.v);
+  vec3_init(position, distal->next_joint.v);
+  vec3_init(direction, distal->next_joint.v);
+  vec3_sub(direction, distal->prev_joint.v);
   adjustPose(position, direction);
   quat_between(orientation, (float[4]) { 0.f, 0.f, -1.f }, direction);
   return true;

From 81e31193212705f7160267cf1a6281fdff5f7e8e Mon Sep 17 00:00:00 2001
From: bjorn <tie.372@gmail.com>
Date: Fri, 24 Jan 2020 21:47:52 -0800
Subject: [PATCH 4/9] leap: curl/splay emulation; openvr fixes;

---
 src/modules/headset/leap.c   | 106 ++++++++++++++++++++++++++++++-----
 src/modules/headset/openvr.c |   4 +-
 2 files changed, 94 insertions(+), 16 deletions(-)

diff --git a/src/modules/headset/leap.c b/src/modules/headset/leap.c
index f248a0f9..4678b139 100644
--- a/src/modules/headset/leap.c
+++ b/src/modules/headset/leap.c
@@ -85,16 +85,17 @@ static void adjustPose(vec3 position, vec3 direction) {
 static bool leap_getPose(Device device, vec3 position, quat orientation) {
   if (device == DEVICE_HAND_LEFT || device == DEVICE_HAND_RIGHT) {
     LEAP_HAND* hand = state.hands[device - DEVICE_HAND_LEFT];
-    if (hand) {
-      float direction[4];
-      vec3_init(position, hand->palm.position.v);
-      vec3_init(direction, hand->palm.normal.v);
-      adjustPose(position, direction);
-      quat_between(orientation, (float[4]) { 0.f, 0.f, -1.f }, direction);
-      return true;
-    } else {
+
+    if (!hand) {
       return false;
     }
+
+    float direction[4];
+    vec3_init(position, hand->palm.position.v);
+    vec3_init(direction, hand->palm.normal.v);
+    adjustPose(position, direction);
+    quat_between(orientation, (float[4]) { 0.f, 0.f, -1.f }, direction);
+    return true;
   }
 
   LEAP_BONE* distal;
@@ -149,17 +150,94 @@ static bool leap_isTouched(Device device, DeviceButton button, bool* touched) {
 }
 
 static bool leap_getAxis(Device device, DeviceAxis axis, float* value) {
-  if ((device != DEVICE_HAND_LEFT && device != DEVICE_HAND_RIGHT) || !state.hands[device - DEVICE_HAND_LEFT]) {
+  if (device == DEVICE_HAND_LEFT || device == DEVICE_HAND_RIGHT) {
+    LEAP_HAND* hand = state.hands[device - DEVICE_HAND_LEFT];
+
+    if (!hand) {
+      return false;
+    }
+
+    switch (axis) {
+      case AXIS_TRIGGER: value[0] = hand->pinch_strength; return true;
+      case AXIS_GRIP: value[0] = hand->grab_strength; return true;
+      default: return false;
+    }
+  }
+
+  uint32_t finger;
+  LEAP_HAND* hand;
+  if (state.hands[0] && device >= DEVICE_HAND_LEFT_FINGER_THUMB && device <= DEVICE_HAND_LEFT_FINGER_PINKY) {
+    hand = state.hands[0];
+    finger = device - DEVICE_HAND_LEFT_FINGER_THUMB;
+  } else if (state.hands[1] && device >= DEVICE_HAND_RIGHT_FINGER_THUMB && device <= DEVICE_HAND_RIGHT_FINGER_PINKY) {
+    hand = state.hands[1];
+    finger = device - DEVICE_HAND_RIGHT_FINGER_THUMB;
+  } else {
     return false;
   }
 
-  LEAP_HAND* hand = state.hands[device - DEVICE_HAND_LEFT];
+  if (axis == AXIS_CURL) {
+    float curl = 1.f;
+    float direction[4];
+    float lastDirection[4];
 
-  switch (axis) {
-    case AXIS_TRIGGER: *value = hand->pinch_strength; return true;
-    case AXIS_GRIP: *value = hand->grab_strength; return true;
-    default: return false;
+    bool thumb = (finger == 0);
+    LEAP_DIGIT* digit = &hand->digits[finger];
+    vec3_init(lastDirection, digit->bones[0 + thumb].next_joint.v);
+    vec3_sub(lastDirection, digit->bones[0 + thumb].prev_joint.v);
+    vec3_normalize(lastDirection);
+
+    // Multiply the dot products of all successive finger bone directions
+    for (uint32_t i = 1 + thumb; i < 4; i++) {
+      vec3_init(direction, digit->bones[i].next_joint.v);
+      vec3_sub(direction, digit->bones[i].prev_joint.v);
+      vec3_normalize(direction);
+      curl *= vec3_dot(direction, lastDirection);
+      vec3_init(lastDirection, direction);
+    }
+
+    // Exaggerate thumb curliness, it has fewer bones
+    if (thumb) {
+      curl = curl * curl * curl;
+    }
+
+    value[0] = 1.f - curl;
+    return true;
+  } else if (axis == AXIS_SPLAY) {
+    float direction[4];
+    float otherDirection[4];
+
+    // Get the direction of the first knuckle, comparing it to the knuckles of any adjacent fingers
+    vec3_init(direction, hand->digits[finger].bones[1 + (finger == 0)].next_joint.v);
+    vec3_sub(direction, hand->digits[finger].bones[1 + (finger == 0)].prev_joint.v);
+    vec3_normalize(direction);
+
+    if (finger > 0) {
+      LEAP_BONE* proximal = &hand->digits[finger - 1].bones[1 + ((finger - 1) == 0)];
+      vec3_init(otherDirection, proximal->next_joint.v);
+      vec3_sub(otherDirection, proximal->prev_joint.v);
+      vec3_normalize(otherDirection);
+      float divisor = ((finger - 1) == 0) ? .9f : .12f;
+      value[0] = MIN((1.f - vec3_dot(direction, otherDirection)) / divisor, 1.f);
+    } else {
+      value[0] = 0.f;
+    }
+
+    if (finger < 4) {
+      LEAP_BONE* proximal = &hand->digits[finger + 1].bones[1];
+      vec3_init(otherDirection, proximal->next_joint.v);
+      vec3_sub(otherDirection, proximal->prev_joint.v);
+      vec3_normalize(otherDirection);
+      float divisor = (finger == 0) ? .9f : .12f;
+      value[1] = MIN((1.f - vec3_dot(direction, otherDirection)) / divisor, 1.f);
+    } else {
+      value[1] = 0.f;
+    }
+
+    return true;
   }
+
+  return false;
 }
 
 static bool leap_vibrate(Device device, float strength, float duration, float frequency) {
diff --git a/src/modules/headset/openvr.c b/src/modules/headset/openvr.c
index 1a15e65b..316a5f3f 100644
--- a/src/modules/headset/openvr.c
+++ b/src/modules/headset/openvr.c
@@ -372,7 +372,7 @@ static bool openvr_getPose(Device device, vec3 position, quat orientation) {
 
   VRBoneTransform_t bones[31];
   EVRSkeletalTransformSpace space = EVRSkeletalTransformSpace_VRSkeletalTransformSpace_Model;
-  EVRSkeletalMotionRange motionRange = EVRSkeletalMotionRange_VRSkeletalMotionRange_WithoutController;
+  EVRSkeletalMotionRange motionRange = EVRSkeletalMotionRange_VRSkeletalMotionRange_WithController;
   error = state.input->GetSkeletalBoneData(state.skeletonActions[hand - DEVICE_HAND_LEFT], space, motionRange, bones, sizeof(bones) / sizeof(bones[0]));
   if (error) {
     return false;
@@ -391,7 +391,7 @@ static bool openvr_getPose(Device device, vec3 position, quat orientation) {
 
   VRBoneTransform_t* bone = &bones[boneIndex];
   mat4_translate(transform, bone->position.v[0], bone->position.v[1], bone->position.v[2]);
-  mat4_rotateQuat(transform, (float[4]) { bone->orientation.x, bone->orientation.y, bone->orientation.z, bone->orientation.w });
+  mat4_rotateQuat(transform, (float[4]) { bone->orientation.w, bone->orientation.x, bone->orientation.y, bone->orientation.z });
   mat4_getPosition(transform, position);
   mat4_getOrientation(transform, orientation);
   return true;

From 0e4c42c8b0c9e56e96a86b9c81c8d6405ae43b26 Mon Sep 17 00:00:00 2001
From: bjorn <tie.372@gmail.com>
Date: Fri, 24 Jan 2020 22:18:10 -0800
Subject: [PATCH 5/9] Add pinch axis;

---
 src/api/l_headset.c           | 4 +++-
 src/modules/headset/headset.h | 1 +
 src/modules/headset/leap.c    | 3 +--
 3 files changed, 5 insertions(+), 3 deletions(-)

diff --git a/src/api/l_headset.c b/src/api/l_headset.c
index f822aeb2..df0277f4 100644
--- a/src/api/l_headset.c
+++ b/src/api/l_headset.c
@@ -69,6 +69,7 @@ const char* DeviceAxes[] = {
   [AXIS_GRIP] = "grip",
   [AXIS_CURL] = "curl",
   [AXIS_SPLAY] = "splay",
+  [AXIS_PINCH] = "pinch",
   NULL
 };
 
@@ -468,7 +469,8 @@ static const int axisCounts[MAX_AXES] = {
   [AXIS_TOUCHPAD] = 2,
   [AXIS_GRIP] = 1,
   [AXIS_CURL] = 1,
-  [AXIS_SPLAY] = 2
+  [AXIS_SPLAY] = 2,
+  [AXIS_PINCH] = 1
 };
 
 static int l_lovrHeadsetGetAxis(lua_State* L) {
diff --git a/src/modules/headset/headset.h b/src/modules/headset/headset.h
index 7b1f2e80..70d7dd5d 100644
--- a/src/modules/headset/headset.h
+++ b/src/modules/headset/headset.h
@@ -62,6 +62,7 @@ typedef enum {
   AXIS_GRIP,
   AXIS_CURL,
   AXIS_SPLAY,
+  AXIS_PINCH,
   MAX_AXES
 } DeviceAxis;
 
diff --git a/src/modules/headset/leap.c b/src/modules/headset/leap.c
index 4678b139..e0cef2e0 100644
--- a/src/modules/headset/leap.c
+++ b/src/modules/headset/leap.c
@@ -139,7 +139,6 @@ static bool leap_isDown(Device device, DeviceButton button, bool* down, bool* ch
   *changed = false; // TODO
 
   switch (button) {
-    case BUTTON_TRIGGER: *down = hand->pinch_strength > .5f; return true;
     case BUTTON_GRIP: *down = hand->grab_strength > .5f; return true;
     default: return false;
   }
@@ -158,7 +157,7 @@ static bool leap_getAxis(Device device, DeviceAxis axis, float* value) {
     }
 
     switch (axis) {
-      case AXIS_TRIGGER: value[0] = hand->pinch_strength; return true;
+      case AXIS_PINCH: value[0] = hand->pinch_strength; return true;
       case AXIS_GRIP: value[0] = hand->grab_strength; return true;
       default: return false;
     }

From 5197d30b94a947d8cd8e6755433e650405096515 Mon Sep 17 00:00:00 2001
From: bjorn <tie.372@gmail.com>
Date: Wed, 12 Feb 2020 21:37:52 -0800
Subject: [PATCH 6/9] Add getSkeleton;

---
 src/api/l_headset.c           | 44 +++++++++++++++++++++++++++++++++++
 src/modules/headset/headset.h | 13 +++++++----
 src/modules/headset/leap.c    |  5 ++++
 src/modules/headset/openvr.c  | 41 ++++++++++++++++++++++++++++++++
 4 files changed, 98 insertions(+), 5 deletions(-)

diff --git a/src/api/l_headset.c b/src/api/l_headset.c
index df0277f4..027dc32f 100644
--- a/src/api/l_headset.c
+++ b/src/api/l_headset.c
@@ -492,6 +492,49 @@ static int l_lovrHeadsetGetAxis(lua_State* L) {
   return count;
 }
 
+static int l_lovrHeadsetGetSkeleton(lua_State* L) {
+  Device device = luax_optdevice(L, 1);
+  float poses[MAX_HEADSET_BONES * 8];
+  uint32_t poseCount = MAX_HEADSET_BONES;
+  FOREACH_TRACKING_DRIVER(driver) {
+    if (driver->getSkeleton(device, poses, &poseCount)) {
+      if (!lua_istable(L, 2)) {
+        lua_createtable(L, poseCount, 0);
+      } else {
+        lua_settop(L, 2);
+      }
+
+      for (uint32_t i = 0; i < poseCount; i++) {
+        lua_createtable(L, 7, 0);
+
+        float angle, ax, ay, az;
+        float* pose = poses + i * 8;
+        quat_getAngleAxis(pose + 4, &angle, &ax, &ay, &az);
+        lua_pushnumber(L, pose[0]);
+        lua_pushnumber(L, pose[1]);
+        lua_pushnumber(L, pose[2]);
+        lua_pushnumber(L, angle);
+        lua_pushnumber(L, ax);
+        lua_pushnumber(L, ay);
+        lua_pushnumber(L, az);
+        lua_rawseti(L, -8, 7);
+        lua_rawseti(L, -7, 6);
+        lua_rawseti(L, -6, 5);
+        lua_rawseti(L, -5, 4);
+        lua_rawseti(L, -4, 3);
+        lua_rawseti(L, -3, 2);
+        lua_rawseti(L, -2, 1);
+
+        lua_rawseti(L, -2, i + 1);
+      }
+
+      return 1;
+    }
+  }
+  lua_pushnil(L);
+  return 1;
+}
+
 static int l_lovrHeadsetVibrate(lua_State* L) {
   Device device = luax_optdevice(L, 1);
   float strength = luax_optfloat(L, 2, 1.f);
@@ -632,6 +675,7 @@ static const luaL_Reg lovrHeadset[] = {
   { "getAxis", l_lovrHeadsetGetAxis },
   { "vibrate", l_lovrHeadsetVibrate },
   { "newModel", l_lovrHeadsetNewModel },
+  { "getSkeleton", l_lovrHeadsetGetSkeleton },
   { "renderTo", l_lovrHeadsetRenderTo },
   { "update", l_lovrHeadsetUpdate },
   { "getTime", l_lovrHeadsetGetTime },
diff --git a/src/modules/headset/headset.h b/src/modules/headset/headset.h
index 70d7dd5d..e7e059a4 100644
--- a/src/modules/headset/headset.h
+++ b/src/modules/headset/headset.h
@@ -4,14 +4,11 @@
 
 #pragma once
 
+#define MAX_HEADSET_BONES 32
+
 struct ModelData;
 struct Texture;
 
-typedef enum {
-  ORIGIN_HEAD,
-  ORIGIN_FLOOR
-} HeadsetOrigin;
-
 typedef enum {
   DRIVER_DESKTOP,
   DRIVER_LEAP_MOTION,
@@ -22,6 +19,11 @@ typedef enum {
   DRIVER_WEBVR
 } HeadsetDriver;
 
+typedef enum {
+  ORIGIN_HEAD,
+  ORIGIN_FLOOR
+} HeadsetOrigin;
+
 typedef enum {
   DEVICE_HEAD,
   DEVICE_HAND_LEFT,
@@ -95,6 +97,7 @@ typedef struct HeadsetInterface {
   bool (*isDown)(Device device, DeviceButton button, bool* down, bool* changed);
   bool (*isTouched)(Device device, DeviceButton button, bool* touched);
   bool (*getAxis)(Device device, DeviceAxis axis, float* value);
+  bool (*getSkeleton)(Device device, float* poses, uint32_t* poseCount);
   bool (*vibrate)(Device device, float strength, float duration, float frequency);
   struct ModelData* (*newModelData)(Device device);
   void (*renderTo)(void (*callback)(void*), void* userdata);
diff --git a/src/modules/headset/leap.c b/src/modules/headset/leap.c
index e0cef2e0..22fea95f 100644
--- a/src/modules/headset/leap.c
+++ b/src/modules/headset/leap.c
@@ -239,6 +239,10 @@ static bool leap_getAxis(Device device, DeviceAxis axis, float* value) {
   return false;
 }
 
+static bool leap_getSkeleton(Device device, float* poses, uint32_t* poseCount) {
+  return false;
+}
+
 static bool leap_vibrate(Device device, float strength, float duration, float frequency) {
   return false;
 }
@@ -296,6 +300,7 @@ HeadsetInterface lovrHeadsetLeapMotionDriver = {
   .isDown = leap_isDown,
   .isTouched = leap_isTouched,
   .getAxis = leap_getAxis,
+  .getSkeleton = leap_getSkeleton,
   .vibrate = leap_vibrate,
   .newModelData = leap_newModelData,
   .update = leap_update
diff --git a/src/modules/headset/openvr.c b/src/modules/headset/openvr.c
index 316a5f3f..d79677d6 100644
--- a/src/modules/headset/openvr.c
+++ b/src/modules/headset/openvr.c
@@ -475,6 +475,46 @@ static bool openvr_getAxis(Device device, DeviceAxis axis, vec3 value) {
   return false;
 }
 
+static bool openvr_getSkeleton(Device device, float* poses, uint32_t* poseCount) {
+  if (device != DEVICE_HAND_LEFT && device != DEVICE_HAND_RIGHT) {
+    return false;
+  }
+
+  InputSkeletalActionData_t info;
+  VRActionHandle_t action = state.skeletonActions[device - DEVICE_HAND_LEFT];
+  EVRInputError error = state.input->GetSkeletalActionData(action, &info, sizeof(info));
+  if (error || !info.bActive) {
+    return false;
+  }
+
+  uint32_t boneCount;
+  error = state.input->GetBoneCount(action, &boneCount);
+  if (error || boneCount > MAX_HEADSET_BONES || boneCount > *poseCount) {
+    return false;
+  }
+
+  VRBoneTransform_t bones[MAX_HEADSET_BONES];
+  EVRSkeletalTransformSpace space = EVRSkeletalTransformSpace_VRSkeletalTransformSpace_Parent;
+  EVRSkeletalMotionRange motionRange = EVRSkeletalMotionRange_VRSkeletalMotionRange_WithController;
+  error = state.input->GetSkeletalBoneData(action, space, motionRange, bones, boneCount);
+  if (error) {
+    return false;
+  }
+
+  float* p = poses;
+  for (uint32_t i = 0; i < boneCount; i++) {
+    memcpy(p, bones[i].position.v, 4 * sizeof(float));
+    p[4] = bones[i].orientation.x;
+    p[5] = bones[i].orientation.y;
+    p[6] = bones[i].orientation.z;
+    p[7] = bones[i].orientation.w;
+    p += 8;
+  }
+
+  *poseCount = boneCount;
+  return true;
+}
+
 static bool openvr_vibrate(Device device, float strength, float duration, float frequency) {
   if (duration <= 0.f || (device != DEVICE_HAND_LEFT && device != DEVICE_HAND_RIGHT)) return false;
 
@@ -684,6 +724,7 @@ HeadsetInterface lovrHeadsetOpenVRDriver = {
   .isDown = openvr_isDown,
   .isTouched = openvr_isTouched,
   .getAxis = openvr_getAxis,
+  .getSkeleton = openvr_getSkeleton,
   .vibrate = openvr_vibrate,
   .newModelData = openvr_newModelData,
   .renderTo = openvr_renderTo,

From a0c7d0235f54668d35213aabda916c04d635dcfc Mon Sep 17 00:00:00 2001
From: bjorn <tie.372@gmail.com>
Date: Thu, 13 Feb 2020 15:13:08 -0800
Subject: [PATCH 7/9] leap: rm curl/splay emulation;

---
 src/modules/headset/leap.c | 77 ++++----------------------------------
 1 file changed, 8 insertions(+), 69 deletions(-)

diff --git a/src/modules/headset/leap.c b/src/modules/headset/leap.c
index 22fea95f..ec0a8bc3 100644
--- a/src/modules/headset/leap.c
+++ b/src/modules/headset/leap.c
@@ -163,77 +163,16 @@ static bool leap_getAxis(Device device, DeviceAxis axis, float* value) {
     }
   }
 
-  uint32_t finger;
-  LEAP_HAND* hand;
   if (state.hands[0] && device >= DEVICE_HAND_LEFT_FINGER_THUMB && device <= DEVICE_HAND_LEFT_FINGER_PINKY) {
-    hand = state.hands[0];
-    finger = device - DEVICE_HAND_LEFT_FINGER_THUMB;
+    LEAP_HAND* hand = state.hands[0];
+    LEAP_DIGIT* digit = &hand->digits[device - DEVICE_HAND_LEFT_FINGER_THUMB];
+    *value = digit->is_extended ? 0.f : 1.f;
+    return axis == AXIS_CURL;
   } else if (state.hands[1] && device >= DEVICE_HAND_RIGHT_FINGER_THUMB && device <= DEVICE_HAND_RIGHT_FINGER_PINKY) {
-    hand = state.hands[1];
-    finger = device - DEVICE_HAND_RIGHT_FINGER_THUMB;
-  } else {
-    return false;
-  }
-
-  if (axis == AXIS_CURL) {
-    float curl = 1.f;
-    float direction[4];
-    float lastDirection[4];
-
-    bool thumb = (finger == 0);
-    LEAP_DIGIT* digit = &hand->digits[finger];
-    vec3_init(lastDirection, digit->bones[0 + thumb].next_joint.v);
-    vec3_sub(lastDirection, digit->bones[0 + thumb].prev_joint.v);
-    vec3_normalize(lastDirection);
-
-    // Multiply the dot products of all successive finger bone directions
-    for (uint32_t i = 1 + thumb; i < 4; i++) {
-      vec3_init(direction, digit->bones[i].next_joint.v);
-      vec3_sub(direction, digit->bones[i].prev_joint.v);
-      vec3_normalize(direction);
-      curl *= vec3_dot(direction, lastDirection);
-      vec3_init(lastDirection, direction);
-    }
-
-    // Exaggerate thumb curliness, it has fewer bones
-    if (thumb) {
-      curl = curl * curl * curl;
-    }
-
-    value[0] = 1.f - curl;
-    return true;
-  } else if (axis == AXIS_SPLAY) {
-    float direction[4];
-    float otherDirection[4];
-
-    // Get the direction of the first knuckle, comparing it to the knuckles of any adjacent fingers
-    vec3_init(direction, hand->digits[finger].bones[1 + (finger == 0)].next_joint.v);
-    vec3_sub(direction, hand->digits[finger].bones[1 + (finger == 0)].prev_joint.v);
-    vec3_normalize(direction);
-
-    if (finger > 0) {
-      LEAP_BONE* proximal = &hand->digits[finger - 1].bones[1 + ((finger - 1) == 0)];
-      vec3_init(otherDirection, proximal->next_joint.v);
-      vec3_sub(otherDirection, proximal->prev_joint.v);
-      vec3_normalize(otherDirection);
-      float divisor = ((finger - 1) == 0) ? .9f : .12f;
-      value[0] = MIN((1.f - vec3_dot(direction, otherDirection)) / divisor, 1.f);
-    } else {
-      value[0] = 0.f;
-    }
-
-    if (finger < 4) {
-      LEAP_BONE* proximal = &hand->digits[finger + 1].bones[1];
-      vec3_init(otherDirection, proximal->next_joint.v);
-      vec3_sub(otherDirection, proximal->prev_joint.v);
-      vec3_normalize(otherDirection);
-      float divisor = (finger == 0) ? .9f : .12f;
-      value[1] = MIN((1.f - vec3_dot(direction, otherDirection)) / divisor, 1.f);
-    } else {
-      value[1] = 0.f;
-    }
-
-    return true;
+    LEAP_HAND* hand = state.hands[1];
+    LEAP_DIGIT* digit = &hand->digits[device - DEVICE_HAND_RIGHT_FINGER_THUMB];
+    *value = digit->is_extended ? 0.f : 1.f;
+    return axis == AXIS_CURL;
   }
 
   return false;

From e218733ee2832b5d18ba90c260d18b500966e9f6 Mon Sep 17 00:00:00 2001
From: bjorn <tie.372@gmail.com>
Date: Thu, 13 Feb 2020 15:18:12 -0800
Subject: [PATCH 8/9] Make splay axis 1D;

---
 src/api/l_headset.c          | 2 +-
 src/modules/headset/openvr.c | 5 ++---
 2 files changed, 3 insertions(+), 4 deletions(-)

diff --git a/src/api/l_headset.c b/src/api/l_headset.c
index 027dc32f..1ba09d2c 100644
--- a/src/api/l_headset.c
+++ b/src/api/l_headset.c
@@ -469,7 +469,7 @@ static const int axisCounts[MAX_AXES] = {
   [AXIS_TOUCHPAD] = 2,
   [AXIS_GRIP] = 1,
   [AXIS_CURL] = 1,
-  [AXIS_SPLAY] = 2,
+  [AXIS_SPLAY] = 1,
   [AXIS_PINCH] = 1
 };
 
diff --git a/src/modules/headset/openvr.c b/src/modules/headset/openvr.c
index d79677d6..9c713ae2 100644
--- a/src/modules/headset/openvr.c
+++ b/src/modules/headset/openvr.c
@@ -466,9 +466,8 @@ static bool openvr_getAxis(Device device, DeviceAxis axis, vec3 value) {
   if (axis == AXIS_CURL) {
     value[0] = summary.flFingerCurl[finger];
     return true;
-  } else if (axis == AXIS_SPLAY) {
-    value[0] = finger > 0 ? summary.flFingerSplay[finger - 1] : 0.f;
-    value[1] = finger < 4 ? summary.flFingerSplay[finger - 0] : 0.f;
+  } else if (axis == AXIS_SPLAY && finger < 4) {
+    value[0] = summary.flFingerSplay[finger];
     return true;
   }
 

From 409c2ad0c61eb4ad83b3b086da75b8e3d4c18004 Mon Sep 17 00:00:00 2001
From: bjorn <tie.372@gmail.com>
Date: Thu, 13 Feb 2020 16:33:17 -0800
Subject: [PATCH 9/9] Fix projection matrices;

---
 src/core/maf.h                | 4 ----
 src/modules/headset/desktop.c | 2 +-
 src/modules/headset/openvr.c  | 9 ++++++---
 src/modules/headset/openxr.c  | 6 +++++-
 4 files changed, 12 insertions(+), 9 deletions(-)

diff --git a/src/core/maf.h b/src/core/maf.h
index 4ef6f168..6601b4c6 100644
--- a/src/core/maf.h
+++ b/src/core/maf.h
@@ -561,10 +561,6 @@ MAF mat4 mat4_perspective(mat4 m, float clipNear, float clipFar, float fovy, flo
 
 // This is currently specific to OpenGL
 MAF mat4 mat4_fov(mat4 m, float left, float right, float up, float down, float clipNear, float clipFar) {
-  left = tanf(left);
-  right = tanf(right);
-  up = tanf(up);
-  down = tanf(down);
   float idx = 1.f / (right - left);
   float idy = 1.f / (up - down);
   float idz = 1.f / (clipFar - clipNear);
diff --git a/src/modules/headset/desktop.c b/src/modules/headset/desktop.c
index 4ce060ba..ca81207e 100644
--- a/src/modules/headset/desktop.c
+++ b/src/modules/headset/desktop.c
@@ -176,7 +176,7 @@ static void desktop_renderTo(void (*callback)(void*), void* userdata) {
   float left, right, up, down;
   desktop_getViewAngles(0, &left, &right, &up, &down);
   Camera camera = { .canvas = NULL, .viewMatrix = { MAT4_IDENTITY }, .stereo = true };
-  mat4_fov(camera.projection[0], left, right, up, down, state.clipNear, state.clipFar);
+  mat4_fov(camera.projection[0], tanf(left), tanf(right), tanf(up), tanf(down), state.clipNear, state.clipFar);
   mat4_multiply(camera.viewMatrix[0], state.headTransform);
   mat4_invert(camera.viewMatrix[0]);
   mat4_set(camera.projection[1], camera.projection[0]);
diff --git a/src/modules/headset/openvr.c b/src/modules/headset/openvr.c
index 9c713ae2..d8934da4 100644
--- a/src/modules/headset/openvr.c
+++ b/src/modules/headset/openvr.c
@@ -296,6 +296,10 @@ static bool openvr_getViewPose(uint32_t view, float* position, float* orientatio
 static bool openvr_getViewAngles(uint32_t view, float* left, float* right, float* up, float* down) {
   EVREye eye = view ? EVREye_Eye_Right : EVREye_Eye_Left;
   state.system->GetProjectionRaw(eye, left, right, up, down);
+  *left = atanf(*left);
+  *right = atanf(*right);
+  *up = atanf(*up);
+  *down = atanf(*down);
   return view < 2;
 }
 
@@ -653,10 +657,9 @@ static void openvr_renderTo(void (*callback)(void*), void* userdata) {
   mat4_fromMat34(head, state.headPose.mDeviceToAbsoluteTracking.m);
 
   for (int i = 0; i < 2; i++) {
-    float left, right, up, down, eye[16];
+    float eye[16];
     EVREye vrEye = (i == 0) ? EVREye_Eye_Left : EVREye_Eye_Right;
-    openvr_getViewAngles(i, &left, &right, &up, &down);
-    mat4_fov(camera.projection[i], left, right, up, down, state.clipNear, state.clipFar);
+    mat4_fromMat44(camera.projection[i], state.system->GetProjectionMatrix(vrEye, state.clipNear, state.clipFar).m);
     mat4_init(camera.viewMatrix[i], head);
     mat4_multiply(camera.viewMatrix[i], mat4_fromMat34(eye, state.system->GetEyeToHeadTransform(vrEye).m));
     mat4_invert(camera.viewMatrix[i]);
diff --git a/src/modules/headset/openxr.c b/src/modules/headset/openxr.c
index 8532e4bb..209f3aeb 100644
--- a/src/modules/headset/openxr.c
+++ b/src/modules/headset/openxr.c
@@ -710,7 +710,11 @@ static void openxr_renderTo(void (*callback)(void*), void* userdata) {
         XrVector3f* v = &view->pose.position;
         XrQuaternionf* q = &view->pose.orientation;
         XrFovf* fov = &view->fov;
-        mat4_fov(camera.projection[eye], fov->angleLeft, fov->angleRight, fov->angleUp, fov->angleDown, state.clipNear, state.clipFar);
+        float left = tanf(fov->angleLeft);
+        float right = tanf(fov->angleRight);
+        float up = tanf(fov->angleUp);
+        float down = tanf(fov->angleDown);
+        mat4_fov(camera.projection[eye], left, right, up, down, state.clipNear, state.clipFar);
         mat4_identity(camera.viewMatrix[eye]);
         mat4_translate(camera.viewMatrix[eye], v->x, v->y, v->z);
         mat4_rotateQuat(camera.viewMatrix[eye], (float[4]) { q->x, q->y, q->z, q->w });