double GetYawFromOrientation(HmdQuaternion_t orientation) { // don't really understand math well enough to know whether the quaternion from GetOrientation(HmdMatrix34_t) is normalized or not. // --> let's assume it's not. --> some additional calculation (although, in case of Yaw, only seems to affect the singularities (rare, if ever)) // Conversion formula from: // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm double x = orientation.x; double y = orientation.y; double z = orientation.z; double w = orientation.w; double sqx = x * x; double sqy = y * y; double sqz = z * z; double sqw = w * w; double unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor // check singularity: double test = x * y + z * w; if (test > 0.499 * unit) { // singularity at north pole return(2 * Math.Atan2(x, w)); } if (test < -0.499 * unit) { // singularity at south pole return(-2 * Math.Atan2(x, w)); } return(Math.Atan2(2 * y * w - 2 * x * z, 1 - 2 * y * y - 2 * z * z)); }
public static void UpdateTrackableDevicePosition() { VRControllerState_t controllerState = new VRControllerState_t(); var size = (uint)Marshal.SizeOf(typeof(VRControllerState_t)); HmdVector3_t position = new HmdVector3_t(); HmdQuaternion_t quaternion = new HmdQuaternion_t(); uint LeftControllerIndex = OpenVR.System.GetTrackedDeviceIndexForControllerRole(ETrackedControllerRole.LeftHand), RightControllerIndex = OpenVR.System.GetTrackedDeviceIndexForControllerRole(ETrackedControllerRole.RightHand); OpenVR.System.GetDeviceToAbsoluteTrackingPose(ETrackingUniverseOrigin.TrackingUniverseStanding, 1 / Program.DataFrameRate, TrackedDevicePose_t); if (TrackedDevicePose_t[0].bPoseIsValid) { GetPosition(TrackedDevicePose_t[0].mDeviceToAbsoluteTracking, ref position); GetRotation(TrackedDevicePose_t[0].mDeviceToAbsoluteTracking, ref quaternion); VREventCallback.NewPoseEvent(VREventCallback.DeviceType.HMD, position, quaternion); } OpenVR.System.GetControllerStateWithPose(ETrackingUniverseOrigin.TrackingUniverseStanding, LeftControllerIndex, ref controllerState, size, ref LeftControllerPose); if (LeftControllerPose.bPoseIsValid) { GetPosition(LeftControllerPose.mDeviceToAbsoluteTracking, ref position); GetRotation(LeftControllerPose.mDeviceToAbsoluteTracking, ref quaternion); VREventCallback.NewPoseEvent(VREventCallback.DeviceType.LeftController, position, quaternion); } OpenVR.System.GetControllerStateWithPose(ETrackingUniverseOrigin.TrackingUniverseStanding, RightControllerIndex, ref controllerState, size, ref RightControllerPose); if (RightControllerPose.bPoseIsValid) { GetPosition(RightControllerPose.mDeviceToAbsoluteTracking, ref position); GetRotation(RightControllerPose.mDeviceToAbsoluteTracking, ref quaternion); VREventCallback.NewPoseEvent(VREventCallback.DeviceType.RightController, position, quaternion); } }
public InputPoseActionData_t PoseFetchEventResult() { var size = (uint)Marshal.SizeOf(typeof(InputPoseActionData_t)); OpenVR.Input.GetPoseActionData(ActionHandle, ETrackingUniverseOrigin.TrackingUniverseStanding, 1 / Program.DataFrameRate, ref Pose, size, controller.ControllerHandle); if (Pose.pose.bDeviceIsConnected == true && Pose.pose.bPoseIsValid == true) { HmdVector3_t position = new HmdVector3_t(); HmdQuaternion_t quaternion = new HmdQuaternion_t(); TrackableDeviceInfo.GetPosition(Pose.pose.mDeviceToAbsoluteTracking, ref position); TrackableDeviceInfo.GetRotation(Pose.pose.mDeviceToAbsoluteTracking, ref quaternion); VREventCallback.NewPoseEvent(controller.ControllerType, (PoseControllerEvent)this, position, quaternion); } return(Pose); }
HmdQuaternion_t GetOrientation(HmdMatrix34_t matrix) { // conversion formula from: // https://www.codeproject.com/Articles/1171122/How-to-Get-Raw-Positional-Data-from-HTC-Vive HmdQuaternion_t q = new HmdQuaternion_t(); q.w = Math.Sqrt(Math.Max(0, 1 + matrix.m0 + matrix.m5 + matrix.m10)) / 2; q.x = Math.Sqrt(Math.Max(0, 1 + matrix.m0 - matrix.m5 - matrix.m10)) / 2; q.y = Math.Sqrt(Math.Max(0, 1 - matrix.m0 + matrix.m5 - matrix.m10)) / 2; q.z = Math.Sqrt(Math.Max(0, 1 - matrix.m0 - matrix.m5 + matrix.m10)) / 2; q.x = CopySign(q.x, matrix.m9 - matrix.m6); q.y = CopySign(q.y, matrix.m2 - matrix.m8); q.z = CopySign(q.z, matrix.m4 - matrix.m1); return(q); }