implement XR_MSFT_controller_model

src/modules/headset/headset_openxr.c

@@ -89,10 +89,14 @@ uintptr_t gpu_vk_get_queue(uint32_t* queueFamilyIndex, uint32_t* queueIndex);
   X(xrDestroyHandTrackerEXT)\
   X(xrLocateHandJointsEXT)\
   X(xrGetHandMeshFB)\
-  X(xrGetDisplayRefreshRateFB) \
-  X(xrEnumerateDisplayRefreshRatesFB) \
-  X(xrRequestDisplayRefreshRateFB) \
-  X(xrQuerySystemTrackedKeyboardFB) \
+  X(xrGetControllerModelKeyMSFT)\
+  X(xrLoadControllerModelMSFT)\
+  X(xrGetControllerModelPropertiesMSFT)\
+  X(xrGetControllerModelStateMSFT)\
+  X(xrGetDisplayRefreshRateFB)\
+  X(xrEnumerateDisplayRefreshRatesFB)\
+  X(xrRequestDisplayRefreshRateFB)\
+  X(xrQuerySystemTrackedKeyboardFB)\
   X(xrCreateKeyboardSpaceFB)
 
 #define XR_DECLARE(fn) static PFN_##fn fn;
@@ -168,12 +172,14 @@ static struct {
   XrAction actions[MAX_ACTIONS];
   XrPath actionFilters[MAX_DEVICES];
   XrHandTrackerEXT handTrackers[2];
+  XrControllerModelKeyMSFT controllerModelKeys[2];
   struct {
     bool depth;
     bool gaze;
     bool handTracking;
     bool handTrackingAim;
     bool handTrackingMesh;
+    bool controllerModel;
     bool keyboardTracking;
     bool overlay;
     bool refreshRate;
@@ -253,6 +259,30 @@ static XrHandTrackerEXT getHandTracker(Device device) {
   return *tracker;
 }
 
+// Controller model keys are created lazily because the runtime is allowed to
+// return XR_NULL_CONTROLLER_MODEL_KEY_MSFT until it is ready.
+static XrControllerModelKeyMSFT getControllerModelKey(Device device) {
+  if (!state.features.controllerModel || (device != DEVICE_HAND_LEFT && device != DEVICE_HAND_RIGHT)) {
+    return XR_NULL_CONTROLLER_MODEL_KEY_MSFT;
+  }
+
+  XrControllerModelKeyMSFT* modelKey = &state.controllerModelKeys[device == DEVICE_HAND_RIGHT];
+
+  if (!*modelKey) {
+    XrControllerModelKeyStateMSFT modelKeyState = {
+      .type = XR_TYPE_CONTROLLER_MODEL_KEY_STATE_MSFT,
+    };
+
+    if (XR_FAILED(xrGetControllerModelKeyMSFT(state.session, state.actionFilters[device], &modelKeyState))) {
+      return XR_NULL_CONTROLLER_MODEL_KEY_MSFT;
+    }
+
+    *modelKey = modelKeyState.modelKey;
+  }
+
+  return *modelKey;
+}
+
 static void openxr_getVulkanPhysicalDevice(void* instance, uintptr_t physicalDevice) {
   XrVulkanGraphicsDeviceGetInfoKHR info = {
     .type = XR_TYPE_VULKAN_GRAPHICS_DEVICE_GET_INFO_KHR,
@@ -336,6 +366,7 @@ static bool openxr_init(HeadsetConfig* config) {
       { "XR_FB_display_refresh_rate", &state.features.refreshRate, true },
       { "XR_FB_hand_tracking_aim", &state.features.handTrackingAim, true },
       { "XR_FB_hand_tracking_mesh", &state.features.handTrackingMesh, true },
+      { "XR_MSFT_controller_model", &state.features.controllerModel, true },
       { "XR_EXTX_overlay", &state.features.overlay, config->overlay },
       { "XR_HTCX_vive_tracker_interaction", &state.features.viveTrackers, true },
       { "XR_FB_keyboard_tracking", &state.features.keyboardTracking, true }
@@ -1481,17 +1512,7 @@ static bool openxr_vibrate(Device device, float power, float duration, float fre
   return true;
 }
 
-static ModelData* openxr_newModelData(Device device, bool animated) {
-  if (!state.features.handTrackingMesh) {
-    return NULL;
-  }
-
-  XrHandTrackerEXT tracker = getHandTracker(device);
-
-  if (!tracker) {
-    return NULL;
-  }
-
+static ModelData* openxr_newModelDataFB(XrHandTrackerEXT tracker, bool animated) {
   // First, figure out how much data there is
   XrHandTrackingMeshFB mesh = { .type = XR_TYPE_HAND_TRACKING_MESH_FB };
   XrResult result = xrGetHandMeshFB(tracker, &mesh);
@@ -1685,13 +1706,42 @@ static ModelData* openxr_newModelData(Device device, bool animated) {
   return model;
 }
 
-static bool openxr_animate(Device device, Model* model) {
-  XrHandTrackerEXT tracker = getHandTracker(device);
-
-  if (!tracker) {
-    return false;
+static ModelData* openxr_newModelDataMSFT(XrControllerModelKeyMSFT modelKey, bool animated) {
+  uint32_t size;
+  if (XR_FAILED(xrLoadControllerModelMSFT(state.session, modelKey, 0, &size, NULL))) {
+    return NULL;
   }
 
+  unsigned char* modelData = malloc(size);
+  if (!modelData) return NULL;
+
+  if (XR_FAILED(xrLoadControllerModelMSFT(state.session, modelKey, size, &size, modelData))) {
+    free(modelData);
+    return NULL;
+  }
+
+  Blob* blob = lovrBlobCreate(modelData, size, "Controller Model Data");
+  ModelData* model = lovrModelDataCreate(blob, NULL);
+  lovrRelease(blob, lovrBlobDestroy);
+
+  return model;
+}
+
+static ModelData* openxr_newModelData(Device device, bool animated) {
+  XrHandTrackerEXT tracker;
+  if ((tracker = getHandTracker(device))) {
+    return openxr_newModelDataFB(tracker, animated);
+  }
+
+  XrControllerModelKeyMSFT modelKey;
+  if ((modelKey = getControllerModelKey(device))) {
+    return openxr_newModelDataMSFT(modelKey, animated);
+  }
+
+  return NULL;
+}
+
+static bool openxr_animateFB(XrHandTrackerEXT tracker, Model* model) {
   // TODO might be nice to cache joints so getSkeleton/animate only locate joints once (profile)
   XrHandJointsLocateInfoEXT info = {
     .type = XR_TYPE_HAND_JOINTS_LOCATE_INFO_EXT,
@@ -1773,6 +1823,66 @@ static bool openxr_animate(Device device, Model* model) {
   return true;
 }
 
+static bool openxr_animateMSFT(XrControllerModelKeyMSFT modelKey, Model* model) {
+  XrControllerModelNodePropertiesMSFT nodeProperties[16];
+  for (uint32_t i = 0; i < COUNTOF(nodeProperties); i++) {
+    nodeProperties[i].type = XR_TYPE_CONTROLLER_MODEL_NODE_PROPERTIES_MSFT;
+    nodeProperties[i].next = 0;
+  }
+  XrControllerModelPropertiesMSFT properties = {
+    .type = XR_TYPE_CONTROLLER_MODEL_PROPERTIES_MSFT,
+    .nodeCapacityInput = COUNTOF(nodeProperties),
+    .nodeProperties = nodeProperties,
+  };
+
+  if (XR_FAILED(xrGetControllerModelPropertiesMSFT(state.session, modelKey, &properties))) {
+    return false;
+  }
+
+  XrControllerModelNodeStateMSFT nodeStates[16];
+  for (uint32_t i = 0; i < COUNTOF(nodeStates); i++) {
+    nodeStates[i].type = XR_TYPE_CONTROLLER_MODEL_NODE_STATE_MSFT;
+    nodeStates[i].next = 0;
+  }
+  XrControllerModelStateMSFT modelState = {
+    .type = XR_TYPE_CONTROLLER_MODEL_STATE_MSFT,
+    .nodeCapacityInput = COUNTOF(nodeStates),
+    .nodeStates = nodeStates,
+  };
+
+  if (XR_FAILED(xrGetControllerModelStateMSFT(state.session, modelKey, &modelState))) {
+    return false;
+  }
+
+  for (uint32_t i = 0; i < modelState.nodeCountOutput; i++) {
+    const char* name = nodeProperties[i].nodeName;
+    ModelData* data = lovrModelGetInfo(model)->data;
+    uint64_t nodeIndex = map_get(&data->nodeMap, hash64(name, strlen(name)));
+    lovrCheck(nodeIndex != MAP_NIL, "ModelData has no node named '%s'", name);
+
+    float position[4], rotation[4];
+    vec3_init(position, (vec3)&nodeStates[i].nodePose.position);
+    quat_init(rotation, (quat)&nodeStates[i].nodePose.orientation);
+    lovrModelSetNodeTransform(model, nodeIndex, position, NULL, rotation, 1);
+  }
+
+  return false;
+}
+
+static bool openxr_animate(Device device, Model* model) {
+  XrHandTrackerEXT tracker;
+  if ((tracker = getHandTracker(device))) {
+    return openxr_animateFB(tracker, model);
+  }
+
+  XrControllerModelKeyMSFT modelKey;
+  if ((modelKey = getControllerModelKey(device))) {
+    return openxr_animateMSFT(modelKey, model);
+  }
+
+  return false;
+}
+
 static Texture* openxr_getTexture(void) {
   if (!SESSION_ACTIVE(state.sessionState)) {
     return NULL;