diff --git a/src/xrt/drivers/steamvr_lh/device.cpp b/src/xrt/drivers/steamvr_lh/device.cpp index 766f71009..7ef065604 100644 --- a/src/xrt/drivers/steamvr_lh/device.cpp +++ b/src/xrt/drivers/steamvr_lh/device.cpp @@ -9,6 +9,7 @@ #include #include +#include #include #include @@ -18,6 +19,7 @@ #include "util/u_device.h" #include "util/u_logging.h" #include "util/u_json.hpp" +#include "xrt/xrt_defines.h" #include "xrt/xrt_device.h" #define DEV_ERR(...) U_LOG_IFL_E(ctx->log_level, __VA_ARGS__) @@ -36,9 +38,9 @@ struct InputClass namespace { const std::unordered_map hmd_classes{ - {"vive", InputClass{XRT_DEVICE_GENERIC_HMD, "Vive HMD", {XRT_INPUT_GENERIC_HEAD_POSE}, {}, {}}}, - {"indexhmd", InputClass{XRT_DEVICE_GENERIC_HMD, "Index HMD", {XRT_INPUT_GENERIC_HEAD_POSE}, {}, {}}}, - {"vive_pro", InputClass{XRT_DEVICE_GENERIC_HMD, "Vive Pro HMD", {XRT_INPUT_GENERIC_HEAD_POSE}, {}, {}}}, + {"vive", InputClass{XRT_DEVICE_GENERIC_HMD, "Vive HMD", {XRT_INPUT_GENERIC_HEAD_POSE}, {}}}, + {"indexhmd", InputClass{XRT_DEVICE_GENERIC_HMD, "Index HMD", {XRT_INPUT_GENERIC_HEAD_POSE}, {}}}, + {"vive_pro", InputClass{XRT_DEVICE_GENERIC_HMD, "Vive Pro HMD", {XRT_INPUT_GENERIC_HEAD_POSE}, {}}}, }; // Adding support for a new controller is a simple as adding it here. @@ -83,8 +85,6 @@ HmdDevice::HmdDevice(const DeviceBuilder &builder) : Device(builder) this->device_type = XRT_DEVICE_TYPE_HMD; this->container_handle = 0; - set_input_class(&hmd_class); - #define SETUP_MEMBER_FUNC(name) this->xrt_device::name = &device_bouncer SETUP_MEMBER_FUNC(get_view_poses); SETUP_MEMBER_FUNC(compute_distortion); @@ -191,6 +191,18 @@ Device::get_tracked_pose(xrt_input_name name, uint64_t at_timestamp_ns, xrt_spac *out_relation = this->relation; } +void +HmdDevice::get_tracked_pose(xrt_input_name name, uint64_t at_timestamp_ns, xrt_space_relation *out_relation) +{ + *out_relation = relation; +} + +void +ControllerDevice::get_tracked_pose(xrt_input_name name, uint64_t at_timestamp_ns, xrt_space_relation *out_relation) +{ + *out_relation = relation; +} + void ControllerDevice::set_output(xrt_output_name name, const xrt_output_value *value) @@ -210,6 +222,45 @@ ControllerDevice::set_output(xrt_output_name name, const xrt_output_value *value ctx->add_haptic_event(event); } +void +HmdDevice::SetDisplayEyeToHead(uint32_t unWhichDevice, + const vr::HmdMatrix34_t &eyeToHeadLeft, + const vr::HmdMatrix34_t &eyeToHeadRight) +{ + xrt_matrix_3x3 leftEye_prequat; + xrt_matrix_3x3 rightEye_prequat; + + xrt_pose leftEye_postquat; + xrt_pose rightEye_postquat; + + // This is a HmdMatrix34 to xrt_matrix_3x3 copy. + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + leftEye_prequat.v[i * 3 + j] = eyeToHeadLeft.m[i][j]; + rightEye_prequat.v[i * 3 + j] = eyeToHeadRight.m[i][j]; + } + } + + math_quat_from_matrix_3x3(&leftEye_prequat, &leftEye_postquat.orientation); + math_quat_from_matrix_3x3(&rightEye_prequat, &rightEye_postquat.orientation); + leftEye_postquat.position.x = eyeToHeadLeft.m[0][3]; + leftEye_postquat.position.y = eyeToHeadLeft.m[1][3]; + leftEye_postquat.position.z = eyeToHeadLeft.m[2][3]; + + rightEye_postquat.position.x = eyeToHeadRight.m[0][3]; + rightEye_postquat.position.y = eyeToHeadRight.m[1][3]; + rightEye_postquat.position.z = eyeToHeadRight.m[2][3]; + + this->eye[0].orientation = leftEye_postquat.orientation; + this->eye[0].position.x += leftEye_postquat.position.x; + this->eye[0].position.y += leftEye_postquat.position.y; + this->eye[0].position.z += leftEye_postquat.position.z; + this->eye[1].orientation = rightEye_postquat.orientation; + this->eye[1].position.x += rightEye_postquat.position.x; + this->eye[1].position.y += rightEye_postquat.position.y; + this->eye[1].position.z += rightEye_postquat.position.z; +} + void HmdDevice::get_view_poses(const xrt_vec3 *default_eye_relation, uint64_t at_timestamp_ns, @@ -218,8 +269,14 @@ HmdDevice::get_view_poses(const xrt_vec3 *default_eye_relation, xrt_fov *out_fovs, xrt_pose *out_poses) { - u_device_get_view_poses(this, default_eye_relation, at_timestamp_ns, view_count, out_head_relation, out_fovs, + struct xrt_vec3 eye_relation = *default_eye_relation; + eye_relation.x = ipd; + + u_device_get_view_poses(this, &eye_relation, at_timestamp_ns, view_count, out_head_relation, out_fovs, out_poses); + + out_poses[0].orientation = this->eye[0].orientation; + out_poses[1].orientation = this->eye[1].orientation; } bool @@ -421,7 +478,15 @@ HmdDevice::handle_property_write(const vr::PropertyWrite_t &prop) } break; } - default: break; + case vr::Prop_UserIpdMeters_Float: { + ipd = *static_cast(prop.pvBuffer); + break; + } + case vr::Prop_SecondsFromVsyncToPhotons_Float: { + vsync_to_photon_ns = *static_cast(prop.pvBuffer) * 1e9f; + break; + } + default: DEV_DEBUG("Unassigned HMD property: %i", prop.prop); break; } } diff --git a/src/xrt/drivers/steamvr_lh/device.hpp b/src/xrt/drivers/steamvr_lh/device.hpp index 1aa56b5da..ab36c3bcc 100644 --- a/src/xrt/drivers/steamvr_lh/device.hpp +++ b/src/xrt/drivers/steamvr_lh/device.hpp @@ -51,16 +51,18 @@ public: void update_pose(const vr::DriverPose_t &newPose); - void - get_tracked_pose(xrt_input_name name, uint64_t at_timestamp_ns, xrt_space_relation *out_relation); - void handle_properties(const vr::PropertyWrite_t *batch, uint32_t count); + //! Maps to @ref xrt_device::get_track_pose. + virtual void + get_tracked_pose(xrt_input_name name, uint64_t at_timestamp_ns, xrt_space_relation *out_relation) = 0; + protected: Device(const DeviceBuilder &builder); std::shared_ptr ctx; vr::PropertyContainerHandle_t container_handle{0}; + float vsync_to_photon_ns{0.f}; virtual void handle_property_write(const vr::PropertyWrite_t &prop) = 0; @@ -85,6 +87,8 @@ private: class HmdDevice : public Device { public: + xrt_pose eye[2]; + float ipd{0.063}; // meters struct Parts { xrt_hmd_parts base; @@ -93,6 +97,14 @@ public: HmdDevice(const DeviceBuilder &builder); + void + get_tracked_pose(xrt_input_name name, uint64_t at_timestamp_ns, xrt_space_relation *out_relation) override; + + void + SetDisplayEyeToHead(uint32_t unWhichDevice, + const vr::HmdMatrix34_t &eyeToHeadLeft, + const vr::HmdMatrix34_t &eyeToHeadRight); + void get_view_poses(const xrt_vec3 *default_eye_relation, uint64_t at_timestamp_ns, @@ -107,6 +119,12 @@ public: void set_hmd_parts(std::unique_ptr parts); + inline float + get_ipd() const + { + return ipd; + } + private: std::unique_ptr hmd_parts{nullptr}; @@ -131,6 +149,9 @@ public: void set_haptic_handle(vr::VRInputComponentHandle_t handle); + void + get_tracked_pose(xrt_input_name name, uint64_t at_timestamp_ns, xrt_space_relation *out_relation) override; + private: vr::VRInputComponentHandle_t haptic_handle{0}; std::unique_ptr output{nullptr}; diff --git a/src/xrt/drivers/steamvr_lh/steamvr_lh.cpp b/src/xrt/drivers/steamvr_lh/steamvr_lh.cpp index a3d531504..3bf653eb9 100644 --- a/src/xrt/drivers/steamvr_lh/steamvr_lh.cpp +++ b/src/xrt/drivers/steamvr_lh/steamvr_lh.cpp @@ -320,7 +320,9 @@ void Context::SetDisplayEyeToHead(uint32_t unWhichDevice, const vr::HmdMatrix34_t &eyeToHeadLeft, const vr::HmdMatrix34_t &eyeToHeadRight) -{} +{ + hmd->SetDisplayEyeToHead(unWhichDevice, eyeToHeadLeft, eyeToHeadRight); +} void Context::SetDisplayProjectionRaw(uint32_t unWhichDevice, const vr::HmdRect2_t &eyeLeft, const vr::HmdRect2_t &eyeRight)