From d7fc516cfbc19a555f479b34eaa3fa0559300366 Mon Sep 17 00:00:00 2001
From: Moses Turner <moses@collabora.com>
Date: Thu, 2 Sep 2021 01:31:46 -0500
Subject: [PATCH] d/vive: Use hand tracking if VIVE_USE_HANDTRACKING is set

---
 src/xrt/drivers/CMakeLists.txt     |  4 ++++
 src/xrt/drivers/vive/vive_prober.c | 37 +++++++++++++++++++++++++++---
 2 files changed, 38 insertions(+), 3 deletions(-)

diff --git a/src/xrt/drivers/CMakeLists.txt b/src/xrt/drivers/CMakeLists.txt
index 61138f705..862e5c56e 100644
--- a/src/xrt/drivers/CMakeLists.txt
+++ b/src/xrt/drivers/CMakeLists.txt
@@ -221,6 +221,10 @@ if(XRT_BUILD_DRIVER_VIVE)
 	target_link_libraries(drv_vive PRIVATE ${ZLIB_LIBRARIES})
 	target_include_directories(drv_vive PRIVATE ${ZLIB_INCLUDE_DIRS})
 	list(APPEND ENABLED_HEADSET_DRIVERS vive)
+
+	if (XRT_BUILD_DRIVER_HANDTRACKING)
+		target_link_libraries(drv_vive PRIVATE drv_ht)
+	endif()
 endif()
 
 if(XRT_HAVE_V4L2)
diff --git a/src/xrt/drivers/vive/vive_prober.c b/src/xrt/drivers/vive/vive_prober.c
index a01919b05..392f0803c 100644
--- a/src/xrt/drivers/vive/vive_prober.c
+++ b/src/xrt/drivers/vive/vive_prober.c
@@ -16,6 +16,10 @@
 #include "vive_controller.h"
 #include "vive_prober.h"
 
+#include "../ht/ht_interface.h"
+#include "../multi_wrapper/multi.h"
+#include "xrt/xrt_config_drivers.h"
+
 static const char VIVE_PRODUCT_STRING[] = "HTC Vive";
 static const char VIVE_PRO_PRODUCT_STRING[] = "VIVE Pro";
 static const char VALVE_INDEX_PRODUCT_STRING[] = "Index HMD";
@@ -24,6 +28,10 @@ static const char VIVE_MANUFACTURER_STRING[] = "HTC";
 
 DEBUG_GET_ONCE_LOG_OPTION(vive_log, "VIVE_LOG", U_LOGGING_WARN)
 
+#ifdef XRT_BUILD_DRIVER_HANDTRACKING
+DEBUG_GET_ONCE_BOOL_OPTION(vive_use_handtracking, "VIVE_USE_HANDTRACKING", false)
+#endif
+
 static int
 log_vive_string(struct xrt_prober *xp, struct xrt_prober_device *dev, enum xrt_prober_string type)
 {
@@ -199,7 +207,7 @@ init_valve_index(struct xrt_prober *xp,
                  struct xrt_prober_device **devices,
                  size_t num_devices,
                  enum u_logging_level ll,
-                 struct xrt_device **out_xdev)
+                 struct xrt_device **out_xdevs)
 {
 	log_vive_device(ll, xp, dev);
 
@@ -239,9 +247,32 @@ init_valve_index(struct xrt_prober *xp,
 		return -1;
 	}
 
-	*out_xdev = &d->base;
+	int out_idx = 0;
 
-	return 1;
+	out_xdevs[out_idx++] = &d->base;
+
+#ifdef XRT_BUILD_DRIVER_HANDTRACKING
+	if (debug_get_bool_option_vive_use_handtracking()) {
+		struct t_stereo_camera_calibration *cal = NULL;
+
+		struct xrt_pose head_in_left_cam;
+		// vive_get_stereo_camera_calibration(&ss->hmd->hmd.config, &cal, &head_in_left_cam);
+		vive_get_stereo_camera_calibration(&d->config, &cal, &head_in_left_cam);
+
+		struct xrt_device *ht = ht_device_create(xp, cal);
+		if (ht != NULL) { // Returns NULL if there's a problem and the hand tracker can't start. By no means a
+			          // fatal error.
+			struct xrt_device *wrap =
+			    multi_create_tracking_override(XRT_TRACKING_OVERRIDE_ATTACHED, ht, &d->base,
+			                                   XRT_INPUT_GENERIC_HEAD_POSE, &head_in_left_cam);
+			out_xdevs[out_idx++] = wrap;
+		}
+		// Don't need it anymore. And it's not even created unless we hit this codepath, which is somewhat hard.
+		t_stereo_camera_calibration_reference(&cal, NULL);
+	}
+#endif
+
+	return out_idx;
 }
 
 int