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);
}
