Skip to content

Commit d47d139

Browse files
Sebane1Sebane1
committed
Fix SlimeVR data source for pulling offset data.
1 parent f62a6a7 commit d47d139

2 files changed

Lines changed: 61 additions & 58 deletions

File tree

‎src/VRDriver.cpp‎

Lines changed: 54 additions & 56 deletions
Original file line numberDiff line numberDiff line change
@@ -503,6 +503,17 @@ void SlimeVRDriver::VRDriver::DriverToTrackingPosition(
503503
tracking_pos[2] -= trans.translation.v[2];
504504
}
505505

506+
void SlimeVRDriver::VRDriver::TrackingToRoomPosition(
507+
const SlimeVRDriver::UniverseTranslation &trans,
508+
const double tracking_pos[3], float room_pos[3]) {
509+
float tp[3] = {static_cast<float>(tracking_pos[0]) + trans.translation.v[0],
510+
static_cast<float>(tracking_pos[1]) + trans.translation.v[1],
511+
static_cast<float>(tracking_pos[2]) + trans.translation.v[2]};
512+
double qw = std::cos(trans.yaw / 2.0);
513+
double qy = std::sin(trans.yaw / 2.0);
514+
RotateVectorByQuaternion(qw, 0.0, -qy, 0.0, tp, room_pos);
515+
}
516+
506517
vr::HmdQuaternion_t SlimeVRDriver::VRDriver::QuatConjugate(
507518
const vr::HmdQuaternion_t &q) {
508519
return vr::HmdQuaternion_t{q.w, -q.x, -q.y, -q.z};
@@ -862,83 +873,70 @@ bool SlimeVRDriver::VRDriver::ExternalHandNearEdgeOfFov(
862873
void SlimeVRDriver::VRDriver::UpdateHandCorrectionFromPoses(
863874
bool left_hand, const vr::DriverPose_t &vd_pose,
864875
const vr::DriverPose_t &slimevr_pose) {
876+
// VD pose is in room space (from GetRawTrackedDevicePoses). SlimeVR pose is
877+
// from protobuf (GetPose()); in tracking space when universe set, else room.
878+
// Express both in room, then offset_room = VD - SlimeVR_room.
879+
float slimevr_room[3];
880+
if (current_universe_.has_value()) {
881+
TrackingToRoomPosition(current_universe_.value().second,
882+
slimevr_pose.vecPosition, slimevr_room);
883+
} else {
884+
for (int i = 0; i < 3; i++)
885+
slimevr_room[i] = static_cast<float>(slimevr_pose.vecPosition[i]);
886+
}
887+
float offset_room[3];
888+
for (int i = 0; i < 3; i++) {
889+
offset_room[i] =
890+
static_cast<float>(vd_pose.vecPosition[i]) - slimevr_room[i];
891+
if (i == 1) offset_room[i] *= config_hand_offset_height_scale_;
892+
}
865893
float alpha = config_hand_offset_ema_alpha_;
866894
if (left_hand) {
867895
if (!hand_offset_left_valid_) {
868896
alpha = 0.f;
869897
hand_offset_left_valid_ = true;
870898
}
871899
if (current_universe_.has_value()) {
872-
// VD pose is in OpenVR driver/room space; SlimeVR pose is in tracking space (we set worldFromDriver on the device). Convert VD to tracking so both are in the same space.
873-
float vd_tracking[3];
874-
DriverToTrackingPosition(current_universe_.value().second,
875-
vd_pose.vecPosition, vd_tracking);
876-
for (int i = 0; i < 3; i++) {
877-
float delta = vd_tracking[i] - static_cast<float>(slimevr_pose.vecPosition[i]);
878-
if (i == 1) delta *= config_hand_offset_height_scale_;
900+
const auto &trans = current_universe_.value().second;
901+
double qw = std::cos(trans.yaw / 2.0);
902+
double qy = std::sin(trans.yaw / 2.0);
903+
float delta[3];
904+
RotateVectorByQuaternion(qw, 0.0, qy, 0.0, offset_room, delta);
905+
for (int i = 0; i < 3; i++)
879906
hand_offset_left_tracking_[i] =
880-
alpha * hand_offset_left_tracking_[i] + (1.f - alpha) * delta;
881-
}
882-
vr::HmdQuaternion_t w_rot{
883-
std::cos(current_universe_.value().second.yaw / 2.0), 0.0,
884-
std::sin(current_universe_.value().second.yaw / 2.0), 0.0};
885-
vr::HmdQuaternion_t target_tracking =
886-
QuatMultiply(QuatConjugate(w_rot), vd_pose.qRotation);
887-
vr::HmdQuaternion_t new_correction =
888-
QuatMultiply(QuatConjugate(slimevr_pose.qRotation), target_tracking);
889-
hand_correction_left_rot_ =
890-
QuatSlerp(hand_correction_left_rot_, new_correction, 1.f - alpha);
907+
alpha * hand_offset_left_tracking_[i] + (1.f - alpha) * delta[i];
891908
} else {
892-
for (int i = 0; i < 3; i++) {
893-
float delta = static_cast<float>(vd_pose.vecPosition[i]) -
894-
static_cast<float>(slimevr_pose.vecPosition[i]);
895-
if (i == 1) delta *= config_hand_offset_height_scale_;
909+
for (int i = 0; i < 3; i++)
896910
hand_offset_left_tracking_[i] =
897-
alpha * hand_offset_left_tracking_[i] + (1.f - alpha) * delta;
898-
}
899-
vr::HmdQuaternion_t new_correction =
900-
QuatMultiply(QuatConjugate(slimevr_pose.qRotation), vd_pose.qRotation);
901-
hand_correction_left_rot_ =
902-
QuatSlerp(hand_correction_left_rot_, new_correction, 1.f - alpha);
911+
alpha * hand_offset_left_tracking_[i] + (1.f - alpha) * offset_room[i];
903912
}
913+
vr::HmdQuaternion_t new_correction =
914+
QuatMultiply(QuatConjugate(slimevr_pose.qRotation), vd_pose.qRotation);
915+
hand_correction_left_rot_ =
916+
QuatSlerp(hand_correction_left_rot_, new_correction, 1.f - alpha);
904917
} else {
905918
if (!hand_offset_right_valid_) {
906919
alpha = 0.f;
907920
hand_offset_right_valid_ = true;
908921
}
909922
if (current_universe_.has_value()) {
910-
// VD pose is in OpenVR driver/room space; SlimeVR pose is in tracking space. Convert VD to tracking so both are in the same space.
911-
float vd_tracking[3];
912-
DriverToTrackingPosition(current_universe_.value().second,
913-
vd_pose.vecPosition, vd_tracking);
914-
for (int i = 0; i < 3; i++) {
915-
float delta = vd_tracking[i] - static_cast<float>(slimevr_pose.vecPosition[i]);
916-
if (i == 1) delta *= config_hand_offset_height_scale_;
923+
const auto &trans = current_universe_.value().second;
924+
double qw = std::cos(trans.yaw / 2.0);
925+
double qy = std::sin(trans.yaw / 2.0);
926+
float delta[3];
927+
RotateVectorByQuaternion(qw, 0.0, qy, 0.0, offset_room, delta);
928+
for (int i = 0; i < 3; i++)
917929
hand_offset_right_tracking_[i] =
918-
alpha * hand_offset_right_tracking_[i] + (1.f - alpha) * delta;
919-
}
920-
vr::HmdQuaternion_t w_rot{
921-
std::cos(current_universe_.value().second.yaw / 2.0), 0.0,
922-
std::sin(current_universe_.value().second.yaw / 2.0), 0.0};
923-
vr::HmdQuaternion_t target_tracking =
924-
QuatMultiply(QuatConjugate(w_rot), vd_pose.qRotation);
925-
vr::HmdQuaternion_t new_correction =
926-
QuatMultiply(QuatConjugate(slimevr_pose.qRotation), target_tracking);
927-
hand_correction_right_rot_ =
928-
QuatSlerp(hand_correction_right_rot_, new_correction, 1.f - alpha);
930+
alpha * hand_offset_right_tracking_[i] + (1.f - alpha) * delta[i];
929931
} else {
930-
for (int i = 0; i < 3; i++) {
931-
float delta = static_cast<float>(vd_pose.vecPosition[i]) -
932-
static_cast<float>(slimevr_pose.vecPosition[i]);
933-
if (i == 1) delta *= config_hand_offset_height_scale_;
932+
for (int i = 0; i < 3; i++)
934933
hand_offset_right_tracking_[i] =
935-
alpha * hand_offset_right_tracking_[i] + (1.f - alpha) * delta;
936-
}
937-
vr::HmdQuaternion_t new_correction =
938-
QuatMultiply(QuatConjugate(slimevr_pose.qRotation), vd_pose.qRotation);
939-
hand_correction_right_rot_ =
940-
QuatSlerp(hand_correction_right_rot_, new_correction, 1.f - alpha);
934+
alpha * hand_offset_right_tracking_[i] + (1.f - alpha) * offset_room[i];
941935
}
936+
vr::HmdQuaternion_t new_correction =
937+
QuatMultiply(QuatConjugate(slimevr_pose.qRotation), vd_pose.qRotation);
938+
hand_correction_right_rot_ =
939+
QuatSlerp(hand_correction_right_rot_, new_correction, 1.f - alpha);
942940
}
943941
}
944942

‎src/VRDriver.hpp‎

Lines changed: 7 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -139,7 +139,8 @@ class VRDriver : public IVRDriver {
139139
QuatSlerpTowardIdentity(const vr::HmdQuaternion_t &q, float t);
140140
static vr::HmdQuaternion_t QuatSlerp(const vr::HmdQuaternion_t &a,
141141
const vr::HmdQuaternion_t &b, float t);
142-
/** Update hand correction (position + rotation) from VD and SlimeVR poses. */
142+
/** Update hand correction from VD pose (runtime room space) and SlimeVR pose
143+
* (from protobuf / device; tracking space when universe set). */
143144
void UpdateHandCorrectionFromPoses(bool left_hand,
144145
const vr::DriverPose_t &vd_pose,
145146
const vr::DriverPose_t &slimevr_pose);
@@ -148,10 +149,14 @@ class VRDriver : public IVRDriver {
148149
bool ExternalHandMotionIsErraticOrFlailing(
149150
const vr::DriverPose_t &current,
150151
const std::optional<vr::DriverPose_t> &last) const;
151-
/** Transform position from driver to tracking space using universe (q, t). */
152+
/** Transform position from driver/room to tracking space using universe. */
152153
static void
153154
DriverToTrackingPosition(const SlimeVRDriver::UniverseTranslation &trans,
154155
const double driver_pos[3], float tracking_pos[3]);
156+
/** Transform position from tracking to room space (inverse of DriverToTracking). */
157+
static void
158+
TrackingToRoomPosition(const SlimeVRDriver::UniverseTranslation &trans,
159+
const double tracking_pos[3], float room_pos[3]);
155160
/** Rotate a vector by quaternion (w,x,y,z). Inverse = use conjugate
156161
* (w,-x,-y,-z). */
157162
static void RotateVectorByQuaternion(double qw, double qx, double qy,

0 commit comments

Comments
 (0)