From 8973730d8acaa1bf41d5f1593de446e0d946bba5 Mon Sep 17 00:00:00 2001 From: bjorn Date: Sat, 22 Aug 2020 13:32:56 -0600 Subject: [PATCH] openvr_getSkeleton; Untested --- src/api/l_math_vectors.c | 4 +- src/core/maf.h | 4 +- src/modules/headset/headset_openvr.c | 83 +++++++++++++++++----------- src/modules/headset/headset_vrapi.c | 2 +- 4 files changed, 57 insertions(+), 36 deletions(-) diff --git a/src/api/l_math_vectors.c b/src/api/l_math_vectors.c index 7578acaf..1a267214 100644 --- a/src/api/l_math_vectors.c +++ b/src/api/l_math_vectors.c @@ -1287,7 +1287,7 @@ static int l_lovrQuatMul(lua_State* L) { quat_rotate(q, r); lua_settop(L, 2); } else { - quat_mul(q, r); + quat_mul(q, q, r); lua_settop(L, 1); } return 1; @@ -1339,7 +1339,7 @@ static int l_lovrQuat__mul(lua_State* L) { quat_rotate(q, vec3_init(out, r)); } else { quat out = luax_newtempvector(L, V_QUAT); - quat_mul(quat_init(out, q), r); + quat_mul(out, q, r); } return 1; } diff --git a/src/core/maf.h b/src/core/maf.h index a4d94c5f..a6402866 100644 --- a/src/core/maf.h +++ b/src/core/maf.h @@ -157,8 +157,8 @@ MAF quat quat_fromMat4(quat q, mat4 m) { return quat_set(q, x, y, z, w); } -MAF quat quat_mul(quat q, quat r) { - return quat_set(q, +MAF quat quat_mul(quat out, quat q, quat r) { + return quat_set(out, q[0] * r[3] + q[3] * r[0] + q[1] * r[2] - q[2] * r[1], q[1] * r[3] + q[3] * r[1] + q[2] * r[0] - q[0] * r[2], q[2] * r[3] + q[3] * r[2] + q[0] * r[1] - q[1] * r[0], diff --git a/src/modules/headset/headset_openvr.c b/src/modules/headset/headset_openvr.c index 73bf77be..1813ac5c 100755 --- a/src/modules/headset/headset_openvr.c +++ b/src/modules/headset/headset_openvr.c @@ -410,43 +410,64 @@ static bool openvr_isTouched(Device device, DeviceButton button, bool* touched) } static bool openvr_getAxis(Device device, DeviceAxis axis, vec3 value) { - if (device == DEVICE_HAND_LEFT || device == DEVICE_HAND_RIGHT) { - InputAnalogActionData_t actionData; - state.input->GetAnalogActionData(state.axisActions[device - DEVICE_HAND_LEFT][axis], &actionData, sizeof(actionData), 0); - vec3_set(value, actionData.x, actionData.y, actionData.z); - return actionData.bActive; - } - - uint32_t finger; - VRActionHandle_t skeletonAction; - if (device >= DEVICE_HAND_LEFT_FINGER_THUMB && device <= DEVICE_HAND_LEFT_FINGER_PINKY) { - finger = device - DEVICE_HAND_LEFT_FINGER_THUMB; - skeletonAction = state.skeletonActions[0]; - } else if (device >= DEVICE_HAND_RIGHT_FINGER_THUMB && device <= DEVICE_HAND_RIGHT_FINGER_PINKY) { - finger = device - DEVICE_HAND_RIGHT_FINGER_THUMB; - skeletonAction = state.skeletonActions[1]; - } else { + if (device != DEVICE_HAND_LEFT && device != DEVICE_HAND_RIGHT) { return false; } - VRSkeletalSummaryData_t summary; - if (state.input->GetSkeletalSummaryData(skeletonAction, &summary)) { - return false; - } - - if (axis == AXIS_CURL) { - value[0] = summary.flFingerCurl[finger]; - return true; - } else if (axis == AXIS_SPLAY && finger < 4) { - value[0] = summary.flFingerSplay[finger]; - return true; - } - - return false; + InputAnalogActionData_t actionData; + state.input->GetAnalogActionData(state.axisActions[device - DEVICE_HAND_LEFT][axis], &actionData, sizeof(actionData), 0); + vec3_set(value, actionData.x, actionData.y, actionData.z); + return actionData.bActive; } static bool openvr_getSkeleton(Device device, float* poses) { - return false; + if (device != DEVICE_HAND_LEFT && device != DEVICE_HAND_RIGHT) { + return false; + } + + // Bone transforms are relative to the hand instead of the origin, so get the hand pose first + InputPoseActionData_t handPose; + state.input->GetPoseActionData(state.poseActions[device], state.compositor->GetTrackingSpace(), 0.f, &handPose, sizeof(handPose), 0); + if (!handPose.pose.bPoseIsValid) { + return false; + } + float transform[16], position[4], orientation[4]; + mat4_fromMat34(transform, handPose.pose.mDeviceToAbsoluteTracking.m); + transform[13] += state.offset; + mat4_getPosition(transform, position); + mat4_getOrientation(transform, orientation); + + InputSkeletalActionData_t info; + VRActionHandle_t skeleton = state.skeletonActions[device - DEVICE_HAND_LEFT]; + EVRInputError error = state.input->GetSkeletalActionData(skeleton, &info, sizeof(info)); + if (error || !info.bActive) { + return false; + } + + VRBoneTransform_t bones[32]; + EVRSkeletalTransformSpace space = EVRSkeletalTransformSpace_VRSkeletalTransformSpace_Model; + EVRSkeletalMotionRange motionRange = EVRSkeletalMotionRange_VRSkeletalMotionRange_WithController; + error = state.input->GetSkeletalBoneData(skeleton, space, motionRange, bones, 32); + if (error) { + return false; + } + + // Copy SteamVR bone transform to output (indices match up) + // Swap x/w (HmdQuaternionf_t has w first) + // Premultiply by hand pose + float* pose = poses; + for (uint32_t i = 0; i < HAND_JOINT_COUNT; i++) { + memcpy(pose, &bones[i].position, 8 * sizeof(float)); + float w = pose[4]; + pose[4] = pose[7]; + pose[7] = w; + quat_rotate(orientation, pose); + vec3_add(pose, position); + quat_mul(pose + 4, orientation, pose + 4); + pose += 8; + } + + return true; } static bool openvr_vibrate(Device device, float strength, float duration, float frequency) { diff --git a/src/modules/headset/headset_vrapi.c b/src/modules/headset/headset_vrapi.c index b9eeb37c..df00d9e5 100644 --- a/src/modules/headset/headset_vrapi.c +++ b/src/modules/headset/headset_vrapi.c @@ -351,7 +351,7 @@ static bool vrapi_getSkeleton(Device device, float* poses) { memcpy(translation, &skeleton->BonePoses[i].Position.x, 3 * sizeof(float)); quat_rotate(pose + 4, translation); vec3_add(pose + 0, translation); - quat_mul(pose + 4, &handPose->BoneRotations[i].x); + quat_mul(pose + 4, pose + 4, &handPose->BoneRotations[i].x); } // We try our best, okay?