d/qwerty: Implement controller pose reset

src/xrt/drivers/qwerty/qwerty_device.c

@@ -360,6 +360,7 @@ qwerty_setup_var_tracking(struct qwerty_system *qs)
 	u_var_add_ro_text(qs, "Hold for movement speed", "LSHIFT");
 	u_var_add_ro_text(qs, "Modify FD movement speed", "Mouse wheel");
 	u_var_add_ro_text(qs, "Modify FD movement speed", "Numpad +/-");
+	u_var_add_ro_text(qs, "Reset both or FC pose", "R");
 	u_var_add_ro_text(qs, "Toggle both or FC parenting to HMD", "F");
 	u_var_add_ro_text(qs, "FC Select click", "Left Click");
 	u_var_add_ro_text(qs, "FC Menu click", "Middle Click");
@@ -520,3 +521,21 @@ qwerty_follow_hmd(struct qwerty_controller *qc, bool follow)
 	qd->pose = rel.pose;
 	qc->follow_hmd = follow;
 }
+
+void
+qwerty_reset_controller_pose(struct qwerty_controller *qc)
+{
+	struct qwerty_device *qd = &qc->base;
+
+	bool no_qhmd = !qd->sys->hmd;
+	if (no_qhmd) {
+		return;
+	}
+
+	struct xrt_quat quat_identity = {0, 0, 0, 1};
+	bool is_left = qc == qd->sys->lctrl;
+
+	qwerty_follow_hmd(qc, true);
+	struct xrt_pose pose = {quat_identity, QWERTY_CONTROLLER_INITIAL_POS(is_left)};
+	qd->pose = pose;
+}

src/xrt/drivers/qwerty/qwerty_device.h

@@ -215,6 +215,10 @@ qwerty_menu_click(struct qwerty_controller *qc);
 void
 qwerty_follow_hmd(struct qwerty_controller *qc, bool follow);
 
+//! Reset controller to initial pose and makes it follow the HMD
+void
+qwerty_reset_controller_pose(struct qwerty_controller *qc);
+
 /*!
  * @}
  */

src/xrt/drivers/qwerty/qwerty_sdl.c

@@ -217,4 +217,14 @@ qwerty_process_event(struct xrt_device **xdevs, size_t num_xdevs, SDL_Event even
 			qwerty_follow_hmd(qright, both_not_following);
 		}
 	}
+
+	// Reset controller poses
+	if (event.type == SDL_KEYDOWN && event.key.keysym.sym == SDLK_r && event.key.repeat == 0) {
+		if (qdev != qd_hmd) {
+			qwerty_reset_controller_pose(qctrl);
+		} else { // If no controller is focused, reset both
+			qwerty_reset_controller_pose(qleft);
+			qwerty_reset_controller_pose(qright);
+		}
+	}
 }