d/steamvr_lh: Simplify coordinate space conversion · matrixfurry.com/monado@4e1a3e1

+44 -49

2 changed files

expand all

src

xrt

drivers

steamvr_lh

device.cpp

device.hpp

+42 -46

src/xrt/drivers/steamvr_lh/device.cpp

··· 572 572 { 573 573 return xrt_vec3{(float)vec[0], (float)vec[1], (float)vec[2]}; 574 574 } 575 + 576 + xrt_pose 577 + copy_pose(const vr::HmdQuaternion_t &orientation, const double (&position)[3]) 578 + { 579 + return xrt_pose{copy_quat(orientation), copy_vec3(position)}; 580 + } 575 581 } // namespace 576 582 577 583 void ··· 599 605 } 600 606 601 607 // XXX: This may be broken if there are multiple known universes - how do we determine which to use then? 602 - JSONNode universe = lighthousedb["known_universes"][0]; 603 - std::string id = universe["id"].asString(); 604 - JSONNode info; 608 + const JSONNode universe = lighthousedb["known_universes"][0]; 609 + const std::string id = universe["id"].asString(); 610 + JSONNode info = {}; 605 611 for (const JSONNode &u : chap_info["universes"].asArray()) { 606 612 if (u["universeID"].asString() == id) { 607 613 DEV_INFO("Found info for universe %s", id.c_str()); ··· 616 622 } 617 623 618 624 std::vector<JSONNode> translation_arr = info["standing"]["translation"].asArray(); 619 - double yaw = info["standing"]["yaw"].asDouble(); 620 - chaperone_center = {static_cast<float>(translation_arr[0].asDouble()), 621 - static_cast<float>(translation_arr[1].asDouble()), 622 - static_cast<float>(translation_arr[2].asDouble())}; 625 + 626 + // If the array is missing elements, add zero. 627 + for (size_t i = translation_arr.size(); i < 3; i++) { 628 + translation_arr.push_back(JSONNode("0.0")); 629 + } 630 + 631 + const double yaw = info["standing"]["yaw"].asDouble(); 623 632 const xrt_vec3 yaw_axis{0.0, -1.0, 0.0}; 624 - math_quat_from_angle_vector(yaw, &yaw_axis, &chaperone_yaw); 633 + math_quat_from_angle_vector(static_cast<float>(yaw), &yaw_axis, &chaperone.orientation); 634 + chaperone.position = copy_vec3({ 635 + translation_arr[0].asDouble(), 636 + translation_arr[1].asDouble(), 637 + translation_arr[2].asDouble(), 638 + }); 639 + math_quat_rotate_vec3(&chaperone.orientation, &chaperone.position, &chaperone.position); 625 640 DEV_INFO("Initialized chaperone data."); 626 641 } 627 642 628 643 void 629 - Device::update_pose(const vr::DriverPose_t &newPose) 644 + Device::update_pose(const vr::DriverPose_t &newPose) const 630 645 { 631 - xrt_space_relation relation; 646 + xrt_space_relation relation = {}; 632 647 if (newPose.poseIsValid && newPose.deviceIsConnected) { 633 648 relation.relation_flags = XRT_SPACE_RELATION_BITMASK_ALL; 634 - 635 - const xrt_vec3 to_local_pos = copy_vec3(newPose.vecDriverFromHeadTranslation); 636 - const xrt_quat to_local_rot = copy_quat(newPose.qDriverFromHeadRotation); 637 - const xrt_vec3 to_world_pos = copy_vec3(newPose.vecWorldFromDriverTranslation); 638 - const xrt_quat to_world_rot = copy_quat(newPose.qWorldFromDriverRotation); 639 - 640 - xrt_pose &pose = relation.pose; 641 - pose.position = copy_vec3(newPose.vecPosition); 642 - pose.orientation = copy_quat(newPose.qRotation); 649 + relation.pose = copy_pose(newPose.qRotation, newPose.vecPosition); 643 650 relation.linear_velocity = copy_vec3(newPose.vecVelocity); 644 651 relation.angular_velocity = copy_vec3(newPose.vecAngularVelocity); 645 652 646 - // apply world transform 647 - auto world_transform = [&](xrt_vec3 &vec) { 648 - math_quat_rotate_vec3(&to_world_rot, &vec, &vec); 649 - math_vec3_accum(&to_world_pos, &vec); 650 - }; 651 - world_transform(pose.position); 652 - math_quat_rotate(&to_world_rot, &pose.orientation, &pose.orientation); 653 - math_quat_rotate_vec3(&to_world_rot, &relation.linear_velocity, &relation.linear_velocity); 654 - math_quat_rotate_vec3(&pose.orientation, &relation.angular_velocity, &relation.angular_velocity); 653 + math_quat_rotate_vec3(&relation.pose.orientation, &relation.angular_velocity, 654 + &relation.angular_velocity); 655 655 656 - // apply local transform 657 - xrt_vec3 local_rotated; 658 - math_quat_rotate_vec3(&pose.orientation, &to_local_pos, &local_rotated); 659 - math_vec3_accum(&local_rotated, &pose.position); 660 - math_quat_rotate(&pose.orientation, &to_local_rot, &pose.orientation); 656 + // apply over local transform 657 + const xrt_pose local = copy_pose(newPose.qDriverFromHeadRotation, newPose.vecDriverFromHeadTranslation); 658 + math_pose_transform(&relation.pose, &local, &relation.pose); 659 + 660 + // apply world transform 661 + const xrt_pose world = 662 + copy_pose(newPose.qWorldFromDriverRotation, newPose.vecWorldFromDriverTranslation); 663 + math_pose_transform(&world, &relation.pose, &relation.pose); 664 + math_quat_rotate_vec3(&world.orientation, &relation.linear_velocity, &relation.linear_velocity); 665 + math_quat_rotate_vec3(&world.orientation, &relation.angular_velocity, &relation.angular_velocity); 661 666 662 667 // apply chaperone transform 663 - auto chap_transform = [&](xrt_vec3 &vec) { 664 - math_vec3_accum(&chaperone_center, &vec); 665 - math_quat_rotate_vec3(&chaperone_yaw, &vec, &vec); 666 - }; 667 - chap_transform(pose.position); 668 - math_quat_rotate(&chaperone_yaw, &pose.orientation, &pose.orientation); 669 - math_quat_rotate_vec3(&chaperone_yaw, &relation.linear_velocity, &relation.linear_velocity); 670 - math_quat_rotate_vec3(&chaperone_yaw, &relation.angular_velocity, &relation.angular_velocity); 671 - } else { 672 - relation.relation_flags = XRT_SPACE_RELATION_BITMASK_NONE; 668 + math_pose_transform(&chaperone, &relation.pose, &relation.pose); 669 + math_quat_rotate_vec3(&chaperone.orientation, &relation.linear_velocity, &relation.linear_velocity); 670 + math_quat_rotate_vec3(&chaperone.orientation, &relation.angular_velocity, &relation.angular_velocity); 673 671 } 674 672 675 - uint64_t ts = chrono_timestamp_ns(); 676 - uint64_t ts_offset = static_cast<uint64_t>(newPose.poseTimeOffset * 1000000.0); 677 - ts += ts_offset; 673 + const uint64_t ts = chrono_timestamp_ns() + static_cast<uint64_t>(newPose.poseTimeOffset * 1000000.0); 678 674 679 675 m_relation_history_push(relation_hist, &relation, ts); 680 676 }

+2 -3

src/xrt/drivers/steamvr_lh/device.hpp

··· 51 51 update_inputs(); 52 52 53 53 void 54 - update_pose(const vr::DriverPose_t &newPose); 54 + update_pose(const vr::DriverPose_t &newPose) const; 55 55 56 56 //! Helper to use the @ref m_relation_history member. 57 57 void ··· 70 70 vr::PropertyContainerHandle_t container_handle{0}; 71 71 std::unordered_map<std::string_view, xrt_input *> inputs_map; 72 72 std::vector<xrt_input> inputs_vec; 73 - inline static xrt_vec3 chaperone_center{}; 74 - inline static xrt_quat chaperone_yaw = XRT_QUAT_IDENTITY; 73 + inline static xrt_pose chaperone = XRT_POSE_IDENTITY; 75 74 const InputClass *input_class; 76 75 77 76 float vsync_to_photon_ns{0.f};

Configure Feed

Configure Feed